/**
 * 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();
#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) {
	if(hi2c->status == I2C_NOT_INIT) return pdFAIL;
	switch(hi2c_txn->ttype) {
		case I2C_SIMPLE_WRITE:
		case I2C_INT_ADDR_WRITE:
		return i2c_write_blocking(hi2c, hi2c_txn, timeout);
		case I2C_SIMPLE_READ:
		case I2C_INT_ADDR_READ:
		return i2c_read_blocking(hi2c, hi2c_txn,timeout);
		default:
		return pdFAIL;
	}
}


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 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);
	uint32_t twistatus;
	uint32_t start_time; 
	
	if((hi2c->busy == pdTRUE) || ((hi2c_txn->len) == 0)) {
		return pdFAIL;
	}
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	// timestamp of when we start
	start_time = rtt_read_timer_value();
	
	// 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));
	}
	
	// send all bytes
	while( ( (hi2c_txn->len) - (hi2c->sent_bytes) ) > 0) {
		twistatus = twi->TWI_SR;
		// Check for nack
		if((twistatus & TWI_SR_NACK)!=0) {
			hi2c->status = I2C_NO_REPLY;
			hi2c->busy = pdFALSE;
			break;
		}
		// check for arbitration lost
		if((twistatus & TWI_SR_ARBLST)!=0) {
			hi2c->status = I2C_ARB_LOST;
			hi2c->busy = pdFALSE;
			break;
		}
		// overrun
		if((twistatus & TWI_SR_OVRE)!=0) {
			hi2c->status = I2C_OVERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		// underrun
		if((twistatus & TWI_SR_UNRE)!=0) {
			hi2c->status = I2C_UNDERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		// check if the Tx buffer is ready (data can be sent)
		if((twistatus & TWI_SR_TXRDY)!=0) {
			twi->TWI_THR = hi2c_txn->data[hi2c->sent_bytes++];
		}
		// check for timeout
		if(rtt_timeout_check(start_time, timeout)) {
			hi2c->status = I2C_TIMED_OUT;
			hi2c->busy = pdFALSE;
			// try to clear the lockup, whatever it was
			twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
			break;
		}
	}
	
	if(!(hi2c->busy)) {
		// this means it failed up in the loop above, time to return with a failure
		return pdFAIL;
	}
	
	while(pdTRUE) {
		twistatus = twi->TWI_SR;
		if((twistatus & TWI_SR_NACK)!=0) {
			hi2c->status = I2C_NO_REPLY;
			hi2c->busy = pdFALSE;
			break;
		}
		// check for arbitration lost
		if((twistatus & TWI_SR_ARBLST)!=0) {
			hi2c->status = I2C_ARB_LOST;
			hi2c->busy = pdFALSE;
			break;
		}
		// overrun
		if((twistatus & TWI_SR_OVRE)!=0) {
			hi2c->status = I2C_OVERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		// underrun
		if((twistatus & TWI_SR_UNRE)!=0) {
			hi2c->status = I2C_UNDERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		
		// Success case, Transmit is ready
		// Break from the while loop without setting busy to false
		if((twistatus & TWI_SR_TXRDY)!=0) {
			break;
		}
		
		if(rtt_timeout_check(start_time, timeout)) {
			hi2c->status = I2C_TIMED_OUT;
			hi2c->busy = pdFALSE;
			// try to clear the lockup, whatever it was
			twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
			break;
		}
	}
	
	if(!(hi2c->busy)) {
		// this means it failed up in the loop above, time to return with a failure
		return pdFAIL;
	}
	
	// last data byte is now in the shift register, we can issue the stop command
	twi->TWI_CR = TWI_CR_STOP; 
	
	while(pdTRUE) {
		twistatus = twi->TWI_SR;
		if((twistatus & TWI_SR_TXCOMP)!=0) {
			hi2c->busy = pdFALSE;
			hi2c->status = I2C_SUCCESS;
			return pdPASS;
		}
		if((twistatus & TWI_SR_NACK)!=0) {
			hi2c->status = I2C_NO_REPLY;
			hi2c->busy = pdFALSE;
			break;
		}
		// check for arbitration lost
		if((twistatus & TWI_SR_ARBLST)!=0) {
			hi2c->status = I2C_ARB_LOST;
			hi2c->busy = pdFALSE;
			break;
		}
		// overrun
		if((twistatus & TWI_SR_OVRE)!=0) {
			hi2c->status = I2C_OVERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		// underrun
		if((twistatus & TWI_SR_UNRE)!=0) {
			hi2c->status = I2C_UNDERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		if(rtt_timeout_check(start_time, timeout)) {
			hi2c->status = I2C_TIMED_OUT;
			hi2c->busy = pdFALSE;
			// try to clear the lockup, whatever it was
			twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
			break;
		}
	}
	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);
	uint32_t twistatus;
	uint32_t start_time;
	uint8_t stop_sent = pdFALSE;
	
	if((hi2c->busy == pdTRUE) || ((hi2c_txn->len) == 0)) {
		return pdFAIL;
	}
	
	hi2c->busy = pdTRUE;
	hi2c->sent_bytes = 0;
	hi2c->current_txn = hi2c_txn;
	
	// timestamp of when we start
	start_time = rtt_read_timer_value();
	
	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;
		stop_sent = pdTRUE;
	} else {
		twi->TWI_CR = TWI_CR_START;
	}
	
	while((hi2c->sent_bytes) < (hi2c_txn->len)) {
		twistatus = twi->TWI_SR;
		if((twistatus & TWI_SR_NACK)!=0) {
			hi2c->status = I2C_NO_REPLY;
			hi2c->busy = pdFALSE;
			break;
		}
		if((twistatus & TWI_SR_ARBLST)!=0) {
			hi2c->status = I2C_ARB_LOST;
			hi2c->busy = pdFALSE;
			break;
		}
		// overrun
		if((twistatus & TWI_SR_OVRE)!=0) {
			hi2c->status = I2C_OVERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		// underrun
		if((twistatus & TWI_SR_UNRE)!=0) {
			hi2c->status = I2C_UNDERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		if(rtt_timeout_check(start_time, timeout)) {
			hi2c->status = I2C_TIMED_OUT;
			hi2c->busy = pdFALSE;
			twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
			break;
		}
		if((stop_sent == pdFALSE)  && (((hi2c_txn->len) - (hi2c->sent_bytes)) == 1)) {
			// second to last byte read
			// send stop
			twi->TWI_CR = TWI_CR_STOP;
			stop_sent = pdTRUE;
		}
		
		if((twistatus & TWI_SR_RXRDY) != 0) {
			// new byte ready to read
			hi2c_txn->data[hi2c->sent_bytes++] = twi->TWI_RHR;
		}
	}
	
	if(!(hi2c->busy)) {
		// there was a failure in the while loop above, return with failure status
		return pdFAIL;
	}
	
	// now wait for the TXCOMP
	while(pdTRUE) {
		twistatus = twi->TWI_SR;
		
		if((twistatus & TWI_SR_TXCOMP) !=0 ) {
			hi2c->busy = pdFALSE;
			hi2c->status = I2C_SUCCESS;
			return pdPASS;
		}
		if((twistatus & TWI_SR_NACK)!=0) {
			hi2c->status = I2C_NO_REPLY;
			hi2c->busy = pdFALSE;
			break;
		}
		if((twistatus & TWI_SR_ARBLST)!=0) {
			hi2c->status = I2C_ARB_LOST;
			hi2c->busy = pdFALSE;
			break;
		}
		// overrun
		if((twistatus & TWI_SR_OVRE)!=0) {
			hi2c->status = I2C_OVERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		// underrun
		if((twistatus & TWI_SR_UNRE)!=0) {
			hi2c->status = I2C_UNDERRUN;
			hi2c->busy = pdFALSE;
			break;
		}
		
		if(rtt_timeout_check(start_time, timeout)) {
			hi2c->status = I2C_TIMED_OUT;
			hi2c->busy = pdFALSE;
			twi->TWI_CR = TWI_CR_LOCKCLR + TWI_CR_STOP + TWI_CR_CLEAR + TWI_CR_THRCLR;
			break;
		}
	}
	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;

	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) {
		// In simple writing mode
		if(hi2c_txn->ttype == I2C_SIMPLE_WRITE || hi2c_txn->ttype == I2C_INT_ADDR_WRITE) {
			if(hi2c->sent_bytes < hi2c_txn->len) {
				// push next byte
				twi->TWI_THR = hi2c_txn->data[hi2c->sent_bytes++];
				} else {
				// final byte was already pushed - set STOP bit to finish transaction
				twi->TWI_CR = TWI_CR_STOP;
				// Now wait for Txcomp
				twi->TWI_IER = TWI_IER_TXCOMP;
				twi->TWI_IDR = TWI_IDR_TXRDY;
			}
		}
		if(hi2c_txn->ttype == I2C_INT_ADDR_READ) {
			// If we are here, that means the internal address has been sent to the
			// holding register. We need to trigger the repeated start command
			
			// Change to reading mode first
			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) {
				// If only one byte to transmit, START and STOP should be set simultaneously
				twi->TWI_CR = TWI_CR_START + TWI_CR_STOP;
				} else {
				twi->TWI_CR = TWI_CR_START;
			}
			// And disable the TXRDY interrupt, enable the RXRDY interrupt
			twi->TWI_IDR = TWI_IDR_TXRDY;
			twi->TWI_IER = TWI_IER_RXRDY;
		}
	}
	// 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) {
		// In simple reading mode
		if(hi2c_txn->ttype == I2C_SIMPLE_READ || hi2c_txn->ttype == I2C_INT_ADDR_READ) {
			// pull the byte into our buffer
			hi2c_txn->data[hi2c->sent_bytes++] = twi->TWI_RHR;
			// Get ready to send a nack if this was 2nd-to-last byte
			if(hi2c->sent_bytes == (hi2c_txn->len - 1)) {
				twi->TWI_CR = TWI_CR_STOP;
				// we should get one more RXRDY with the final byte, then the TXCOMP after STOP bit sent
			}
			if(hi2c->sent_bytes == hi2c_txn->len) {
				// Now we are waiting on TXCOMP to finish this transaction
				twi->TWI_IER = TWI_IER_TXCOMP;
				twi->TWI_IDR = TWI_IDR_RXRDY;
			}
		}
	}
	
	// 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_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_TXRDY + TWI_IDR_RXRDY;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_NO_REPLY;
		if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
	}
	// 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_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_TXRDY + TWI_IDR_RXRDY;
		hi2c->busy = pdFALSE;
		hi2c->status = I2C_ARB_LOST;
		if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
	}
	// TXCOMP - data has been sent, stop has been sent
	if((status & TWI_SR_TXCOMP) != 0 && (twi->TWI_IMR & TWI_IMR_TXCOMP) != 0) {
		// In simple writing mode
		if(hi2c_txn->ttype == I2C_SIMPLE_WRITE || hi2c_txn->ttype == I2C_INT_ADDR_WRITE) {
			// Transaction complete
			twi->TWI_IDR = TWI_IDR_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_TXRDY;
			hi2c->busy = pdFALSE;
			hi2c->status = I2C_SUCCESS;
			if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
		}
		// In simple reading mode
		if(hi2c_txn->ttype == I2C_SIMPLE_READ || hi2c_txn->ttype == I2C_INT_ADDR_READ) {
			// Transaction complete
			twi->TWI_IDR = TWI_IDR_ARBLST + TWI_IDR_NACK + TWI_IDR_TXCOMP + TWI_IDR_RXRDY;
			hi2c->busy = pdFALSE;
			hi2c->status = I2C_SUCCESS;
			if(hi2c_txn->cb != NULL) {hi2c_txn->cb();}
		}
	}

}



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;
}
