/**
 * i2c.c
 *
 * Initialization and communication for the Inter-Integrated Circuit (I2C) hardware.
 * 
 * Copyright (c) 2022 Bigscreen, Inc.
 */

#include <asf.h>
#include "i2c.h"
#include "rtt_util.h"

static uint32_t flexcom_id_lookup(Flexcom* flxcm) {
	if(flxcm == FLEXCOM0) {
		return ID_FLEXCOM0;
		} else if(flxcm == FLEXCOM1){
		return ID_FLEXCOM1;
		} else if(flxcm == FLEXCOM2){
		return ID_FLEXCOM2;
		} else if(flxcm == FLEXCOM3){
		return ID_FLEXCOM3;
		} else if(flxcm == FLEXCOM4){
		return ID_FLEXCOM4;
		} else if(flxcm == FLEXCOM5){
		return ID_FLEXCOM5;
		} else if(flxcm == FLEXCOM6){
		return ID_FLEXCOM6;
#if defined(__SAMG55J19__)
		} else if(flxcm == FLEXCOM7){
		return ID_FLEXCOM7;
#endif
	}
	return ID_PERIPH_COUNT; // Invalid ID
}

static uint32_t calculate_clock_period_register(uint32_t periph_clock, uint32_t desired_i2c_clock) {
	// Setting t_high = t_low = 1/2 t_i2c ... so f_high = f_low = 2*f_i2c
	// (CHDIV * (2^CKDIV) + 3)*t_periph = t_i2c
	// t_i2c / t_periph = (CHDIV * (2^CKDIV) + 3)
	// where f_periph = 1/t_periph, and f_i2c = 1/t_i2c
	// f_periph / f_i2c = (CHDIV * (2^CKDIV) + 3)
	// CHDIV * (2^CKDIV) = f_periph / f_i2c - 3
	// Then... find the smallest CKDIV that allows for the division factor
	// to fit into CHDIV / CLDIV, which are 8 bit numbers

	uint32_t f_pulse = 2*desired_i2c_clock;
	uint32_t chdiv = periph_clock / f_pulse - 3;
	uint8_t ckdiv = 0;
	while(chdiv > 255) {
		chdiv = chdiv >> 1UL;
		ckdiv++;
	}

	return TWI_CWGR_CHDIV((uint8_t)chdiv) + TWI_CWGR_CLDIV((uint8_t)chdiv) + TWI_CWGR_CKDIV(ckdiv);

}


//static uint8_t i2c_write_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
//static uint8_t i2c_read_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
//static uint8_t i2c_write_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
//static uint8_t i2c_read_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn);
static uint8_t i2c_write_blocking(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn, uint32_t timeout);
static uint8_t i2c_read_blocking(I2C_Handle *hi2c, I2C_Transaction_Request *hi2c_txn, uint32_t timeout);

void init_i2c(I2C_Handle* hi2c, uint32_t desired_clock_speed) {
	Flexcom* flxcm = hi2c->flexcom;
	Twi* twi = (Twi*)(((uint32_t)flxcm) + TWI_OFFSET_FROM_FLEXCOM);

	// Sanity check
	uint32_t flexcom_id = flexcom_id_lookup(flxcm);
	if(flexcom_id >= ID_PERIPH_COUNT){
		return;
	}

	// Enable clocks to necessary peripherals
	sysclk_enable_peripheral_clock(flexcom_id);
	
	// Put the Flexcom into I2C mode
	flxcm->FLEXCOM_MR = FLEXCOM_MR_OPMODE_TWI;
	
	// Setup for initiator mode
	twi->TWI_CR = TWI_CR_MSEN + TWI_CR_SVDIS;
	
	// TODO: change fixed clock speed to grab from device settings
	twi->TWI_CWGR = calculate_clock_period_register(98304000, desired_clock_speed);
	// For 400kHz on 98.304MHz clock:
	// twi->TWI_CWGR = TWI_CWGR_CHDIV(15) + TWI_CWGR_CLDIV(15) + TWI_CWGR_CKDIV(3);
	
	hi2c->busy = pdFALSE;
	hi2c->status = I2C_IDLE;
	
	// Enable interrupt vector
	NVIC_EnableIRQ(flexcom_id);
	// ID is shared between clock control and IRQ number. So we can use the same ID
	NVIC_SetPriority(flexcom_id, I2C_INTERRUPT_PRIO);
#if defined(__FREERTOS__)
	hi2c->i2c_mutex = xSemaphoreCreateMutex();
	hi2c->i2c_busywait = xSemaphoreCreateBinary();
#endif
}

void disable_i2c(I2C_Handle* hi2c) {
	Flexcom* flxcm = hi2c->flexcom;
	uint32_t flexcom_id = flexcom_id_lookup(flxcm);
	if(flexcom_id >= ID_PERIPH_COUNT){
		return;
	}
	NVIC_DisableIRQ(flexcom_id);
	sysclk_disable_peripheral_clock(flexcom_id);
	
}

#if defined (__FREERTOS__)
uint8_t i2c_lock(I2C_Handle* hi2c) {
	return xSemaphoreTake(hi2c->i2c_mutex, portMAX_DELAY);
}

uint8_t i2c_unlock(I2C_Handle* hi2c) {
	return xSemaphoreGive(hi2c->i2c_mutex);
}

#endif
/*
uint8_t i2c_transact(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	if(hi2c->status == I2C_NOT_INIT) return pdFAIL;
	switch(hi2c_txn->ttype) {
		case I2C_SIMPLE_WRITE:
		return i2c_write_simple(hi2c, hi2c_txn);
		case I2C_SIMPLE_READ:
		return i2c_read_simple(hi2c, hi2c_txn);
		case I2C_INT_ADDR_WRITE:
		return i2c_write_extended(hi2c, hi2c_txn);
		case I2C_INT_ADDR_READ:
		return i2c_read_extended(hi2c, hi2c_txn);
		default:
		return pdFAIL;
	}
}
*/

uint8_t i2c_transact_blocking(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn, uint32_t timeout) {
	uint8_t retval;
	
	if(hi2c->status == I2C_NOT_INIT) return pdFAIL;
	
	if((hi2c->status != I2C_SUCCESS) && (hi2c->status != I2C_IDLE)) {
		// try to reset the I2C peripheral and clear any stuck buffers
		uint32_t clock_reg;
		Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
		
		clock_reg = twi->TWI_CWGR;
		twi->TWI_CR = TWI_CR_SWRST;
		for(uint16_t j = 0; j < 700; j++); // another delay
		twi->TWI_CWGR = clock_reg;
		twi->TWI_CR = TWI_CR_MSEN + TWI_CR_SVDIS;
			
		// Reset status and busy
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_IDLE;
	}

	switch(hi2c_txn->ttype) {
		case I2C_SIMPLE_WRITE:
		case I2C_INT_ADDR_WRITE:
			retval = i2c_write_blocking(hi2c, hi2c_txn, timeout);
			break;
		case I2C_SIMPLE_READ:
		case I2C_INT_ADDR_READ:
			retval = i2c_read_blocking(hi2c, hi2c_txn,timeout);
			break;
		default:
			retval = pdFAIL;
			break;
	}

	return retval;
}

/*
static uint8_t i2c_write_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);

	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	twi->TWI_THR = hi2c_txn->data[hi2c->sent_bytes++];
	
	// Enable interrupts to continue transaction in IRQ
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_TXRDY;
	
	return pdPASS;
}

static uint8_t i2c_read_simple(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);

	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr))
	+ TWI_MMR_MREAD;
	
	// Special case for single byte read. Must set START and STOP simultaneously
	if(hi2c_txn->len == 1) {
		twi->TWI_CR = TWI_CR_START + TWI_CR_STOP;
		} else {
		// multiple byte transfers set the STOP bit just after the 2nd-to-last byte was read
		twi->TWI_CR = TWI_CR_START;
	}
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_RXRDY;
	
	return pdPASS;
}

static uint8_t i2c_write_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	
	// Two ways to do this:
	// 1) use the SAMG55 TWI module's internal address capabilities
	//		this involves setting the "address size" to non-zero
	//		before starting a transaction. The MCU will send the device
	//		address then the bytes in internal address register IADR
	//		then start transmitting data from the transmit holding reg
	// 2) do it manually. the internal address bytes can simply be sent
	//		before the data as if they were normal data bytes
	// I chose option 2.
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	twi->TWI_THR = hi2c_txn->internal_addr;
	
	// Enable interrupts to continue transaction in IRQ
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_TXRDY;
	
	return pdPASS;
}

static uint8_t i2c_read_extended(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if(hi2c->busy == pdTRUE) {
		return pdFAIL;
	}
	
	// Again, 2 ways to do this:
	// 1) use SAMG55 TWI's internal address capabilites
	//		set IADRSZ to non-zero, set address in IADR
	//		after setting start bit, MCU will send device
	//		address, the internal address, then generate
	//		a repeated start, send the device address in
	//		read mode, and then finally start reading bytes
	// 2) do it manually. have to send the internal address
	//		as if it's a normal data byte to write, then
	//		send a repeated start by setting START in the
	//		control register CR. Then proceed with a read
	//		transaction
	// I chose option 2
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	twi->TWI_THR = hi2c_txn->internal_addr;
	
	// Enable interrupts to continue transaction in IRQ
	twi->TWI_IER = TWI_IER_ARBLST + TWI_IER_NACK + TWI_IER_TXRDY;
	// Note this looks a lot like the write command...in fact, the first
	// half of this transaction is pretty much exactly a write with one
	// data byte, the internal address.
	
	
	return pdPASS;
}
*/

static void i2c_irq_handle_txrdy(I2C_Handle* hi2c) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	twi->TWI_THR = hi2c->current_txn->data[hi2c->sent_bytes++];
	if(hi2c->current_txn->len == hi2c->sent_bytes) {
		// just loaded final byte
		// issue STOP and wait for txcomp
		// no longer wait for txrdy since we have no more bytes to feed
		twi->TWI_CR = TWI_CR_STOP;
		twi->TWI_IER = TWI_IER_TXCOMP;
		twi->TWI_IDR = TWI_IDR_TXRDY;
	}
}

static void i2c_irq_handle_txcomp(I2C_Handle* hi2c) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	hi2c->status = I2C_SUCCESS;
	twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
}

static void i2c_irq_handle_rxrdy(I2C_Handle* hi2c) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	hi2c->current_txn->data[hi2c->sent_bytes++] = twi->TWI_RHR;
	if(((hi2c->current_txn->len) - (hi2c->sent_bytes)) == 1) {
		// just received the 2nd to last byte
		// now is the time to set STOP
		twi->TWI_CR = TWI_CR_STOP;
	}
	else if((hi2c->current_txn->len) == (hi2c->sent_bytes)) {
		// and now we wait for TXCOMP
		twi->TWI_IDR = TWI_IDR_RXRDY;
		twi->TWI_IER = TWI_IER_TXCOMP;
	}
}
	
	

static uint8_t i2c_write_blocking(I2C_Handle* hi2c, I2C_Transaction_Request* hi2c_txn, uint32_t timeout) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if((hi2c->busy == pdTRUE) || ((hi2c_txn->len) == 0)) {
		return pdFAIL;
	}
	
	hi2c->busy = pdTRUE;
	hi2c->status = I2C_IDLE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	

	// set the responder's address		
	// if we are doing an internal address write, send that address first (IADR field and IADRSZ size)
	if(hi2c_txn->ttype == I2C_INT_ADDR_WRITE) {
		twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr))
						+ TWI_MMR_IADRSZ_1_BYTE; // one byte internal address
		twi->TWI_IADR = hi2c_txn->internal_addr;
	} else {
		twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr));
	}
	
	// Begin by sending the first byte
	twi->TWI_THR = hi2c->current_txn->data[hi2c->sent_bytes++];
	
	// Set the possible interrupt flags we want to watch for
	if(hi2c->current_txn->len == 1) {
		// Special case for one byte sent
		// Load the tx register and immediately set STOP
		// This will ensure only one byte is sent
		twi->TWI_CR = TWI_CR_STOP; 
		twi->TWI_IER = TWI_IER_NACK + TWI_IER_ARBLST + TWI_IER_OVRE + TWI_IER_UNRE + TWI_IER_TXCOMP;
	} 
	else {
		// Wait for txrdy to send next byte
		twi->TWI_IER = TWI_IER_NACK + TWI_IER_ARBLST + TWI_IER_OVRE + TWI_IER_UNRE + TWI_IER_TXRDY;
		// interrupt handler will figure out when to set STOP
	}

	if(pdFALSE == xSemaphoreTake(hi2c->i2c_busywait, timeout))
	{
		// Timeout occurred.
		hi2c->status = I2C_TIMED_OUT;
		hi2c->busy = pdFALSE;
		// attempt to clear the lockup, whatever it was
		twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
		twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
	}
	
	
	if(I2C_SUCCESS == hi2c->status)
		return pdPASS;
	else
		return pdFAIL;
}

static uint8_t i2c_read_blocking(I2C_Handle *hi2c, I2C_Transaction_Request *hi2c_txn, uint32_t timeout) {
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if((hi2c->busy == pdTRUE) || ((hi2c_txn->len) == 0)) {
		return pdFAIL;
	}
	
	hi2c->busy = pdTRUE;
	hi2c->status = I2C_IDLE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	
	if(hi2c_txn->ttype == I2C_INT_ADDR_READ) {
		// combine the internal address thingy again here
		twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr)) 
						+ TWI_MMR_MREAD + TWI_MMR_IADRSZ_1_BYTE; // with MREAD for reading, and internal address size
		twi->TWI_IADR = hi2c_txn->internal_addr;
	} else {
		twi->TWI_MMR = TWI_MMR_DADR(hi2c_txn->shift == I2C_Unshifted ? (hi2c_txn->dev_addr >> 1) : (hi2c_txn->dev_addr))
						+ TWI_MMR_MREAD; 
	}
	
	if(hi2c_txn->len == 1) {
		// special case for 1 byte, need to send START and STOP simultaneously
		twi->TWI_CR = TWI_CR_START + TWI_CR_STOP;
		twi->TWI_IER = TWI_IER_NACK + TWI_IER_ARBLST + TWI_IER_OVRE + TWI_IER_UNRE + TWI_IER_RXRDY;
		
	} else {
		twi->TWI_CR = TWI_CR_START;
		twi->TWI_IER = TWI_IER_NACK + TWI_IER_ARBLST + TWI_IER_OVRE + TWI_IER_UNRE + TWI_IER_RXRDY;
	}

	if(pdFALSE == xSemaphoreTake(hi2c->i2c_busywait, timeout))
	{
		// Timeout occurred. 
		hi2c->status = I2C_TIMED_OUT;
		hi2c->busy = pdFALSE;
		// attempt to clear the lockup, whatever it was
		twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
		twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
	}
	
	if(I2C_SUCCESS == hi2c->status)
		return pdPASS;
	else
		return pdFAIL;
}

uint8_t i2c_busy(I2C_Handle* hi2c) {
	return hi2c->busy;
}

I2C_Status_Type i2c_status(I2C_Handle* hi2c)
{
	return hi2c->status;
}

void i2c_irq_handler(I2C_Handle* hi2c) {
	// Get status to figure out what happened
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	uint32_t status = twi->TWI_SR;
	BaseType_t xHigherPriorityTaskWoken = pdFALSE;

	I2C_Transaction_Request* hi2c_txn = hi2c->current_txn;
	
	// TXRDY - available to push next byte to write
	if((status & TWI_SR_TXRDY) != 0 && (twi->TWI_IMR & TWI_IMR_TXRDY) != 0) {
		i2c_irq_handle_txrdy(hi2c);
	}
	// RXRDY - responder sent a byte in and it's ready to pull out of the holding register
	if((status & TWI_SR_RXRDY) != 0 && (twi->TWI_IMR & TWI_IMR_RXRDY) != 0) {
		i2c_irq_handle_rxrdy(hi2c);
	}
	
	// NACK - no reply from responder device
	if((status & TWI_SR_NACK) != 0 && (twi->TWI_IMR & TWI_IMR_NACK) != 0) {
		// Cancel transaction
		twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_NO_REPLY;
		if(hi2c_txn->cb != NULL) {
			hi2c_txn->cb();
		}
		xSemaphoreGiveFromISR( hi2c->i2c_busywait, &xHigherPriorityTaskWoken );
		
	}
	// ARBLST - arbitration lost...some other initiator device on the bus?!
	if((status & TWI_SR_ARBLST) != 0 && (twi->TWI_IMR & TWI_IMR_ARBLST) != 0) {
		// Cancel transaction
		twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_ARB_LOST;
		if(hi2c_txn->cb != NULL) {
			hi2c_txn->cb();
		}
		xSemaphoreGiveFromISR( hi2c->i2c_busywait, &xHigherPriorityTaskWoken );
	}
	// OVRE - overrun. receive holding reg loaded while rxrdy was set. missed reading a byte
	if((status & TWI_SR_OVRE) != 0 && (twi->TWI_IMR & TWI_IMR_OVRE) != 0) {
		// Cancel transaction
		twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_OVERRUN;
		if(hi2c_txn->cb != NULL) {
			hi2c_txn->cb();
		}
		xSemaphoreGiveFromISR( hi2c->i2c_busywait, &xHigherPriorityTaskWoken );
	}
	// UNRE - underrun. transmit holding reg was not filled in time. missed writing a byte
	if((status & TWI_SR_UNRE) != 0 && (twi->TWI_IMR & TWI_IMR_UNRE) != 0) {
		// Cancel transaction
		twi->TWI_IDR = TWI_IDR_TXCOMP + TWI_IDR_RXRDY + TWI_IDR_TXRDY + TWI_IDR_OVRE + TWI_IDR_UNRE + TWI_IDR_NACK + TWI_IDR_ARBLST;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_UNDERRUN;
		if(hi2c_txn->cb != NULL) {
			hi2c_txn->cb();
		}
		xSemaphoreGiveFromISR( hi2c->i2c_busywait, &xHigherPriorityTaskWoken );
	}
	// TXCOMP - data has been sent, stop has been sent
	if((status & TWI_SR_TXCOMP) != 0 && (twi->TWI_IMR & TWI_IMR_TXCOMP) != 0) {
		i2c_irq_handle_txcomp(hi2c);
		hi2c->busy = pdFALSE;
		if(hi2c_txn->cb != NULL) {
			hi2c_txn->cb();
		}
		xSemaphoreGiveFromISR( hi2c->i2c_busywait, &xHigherPriorityTaskWoken );
	}
	
	portYIELD_FROM_ISR( xHigherPriorityTaskWoken );
}



void i2c_lockup_clear_sequence(I2C_Handle* hi2c) {
	Pio* port;
	uint8_t pin;
	uint32_t clock_reg;
	Twi* twi = (Twi*)((uint32_t)(hi2c->flexcom) + TWI_OFFSET_FROM_FLEXCOM);
	
	if(hi2c->SCL_Pin > 32) { // port B is 32 and up, port A is 0 to 31
		port = PIOB;
		pin = hi2c->SCL_Pin - 32;
		} else {
		port = PIOA;
		pin = hi2c->SCL_Pin;
	}
	// Do a lockup clear sequence on SCL
	
	port->PIO_PER = (1u << pin); // remove peripheral control
	port->PIO_MDER = (1u << pin); // open collector
	port->PIO_OER = (1u << pin); // output
	port->PIO_SODR = (1u << pin); // high level
	
	// Toggle clock pin 10x without caring for data pin
	for(uint8_t i = 0; i < 10; i++) {
		for(uint16_t j = 0; j < 700; j++); // 50us ish delay
		port->PIO_CODR = (1u << pin); // toggle low
		for(uint16_t j = 0; j < 700; j++); // another delay
		port->PIO_SODR = (1u << pin); // toggle high
	}
	
	// Release clock pin
	port->PIO_ODR = (1u << pin); // back to input mode
	port->PIO_PDR = (1u << pin); // and peripheral control
	
	// Reset I2C peripheral
	clock_reg = twi->TWI_CWGR;
	twi->TWI_CR = TWI_CR_SWRST;
	for(uint16_t j = 0; j < 700; j++); // another delay
	twi->TWI_CWGR = clock_reg;
	twi->TWI_CR = TWI_CR_MSEN + TWI_CR_SVDIS;
	
	// Reset status and busy
	hi2c->busy = pdFALSE;
	hi2c->status = I2C_IDLE;
	
}

uint8_t i2c_wait_to_complete(I2C_Handle* hi2c, uint32_t timeout) {
	uint32_t start_time = rtt_read_timer_value();
	while(i2c_busy(hi2c)) {
		if(rtt_timeout_check(start_time, timeout)) {
			return pdFAIL;
		}
	}
	
	return pdPASS;
}