#include "tusb322.h"

extern I2C i2c;
InterruptIn IO_Usb(PB_4);

void UsbInterrupt(void) {
    // No real uses for this at the moment
}

void TUSB322_Init(void) {
    char buffer[sizeof(TUSB322_REGS)] = {0};
    
    IO_Usb.fall(&UsbInterrupt);

    // Disable UFP accessory support (otherwise IC stays in DRP mode)
    buffer[0] = FIELD_OFFSET(TUSB322_REGS, Status2);
    CONN_STATUS2 *stat = (CONN_STATUS2*)&buffer[1];
    stat->UFP_ACCESSORY = 0x1;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);

    // Disable CC termination to change to UFP mode
    buffer[0] = FIELD_OFFSET(TUSB322_REGS, Control);
    CTRL *ctrl = (CTRL*)&buffer[1];
    ctrl->DISABLE_TERM = 0x1;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);

    // For operation in UFP mode
    ctrl->MODE_SELECT = 0x01;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);

    // Reenable CC termination
    ctrl->DISABLE_TERM = 0x0;
    i2c.write(TUSB322_ADDR << 1, buffer, 2);
}