#include <cstdint>
#include "hardware/5400tp194.h"
#include "i2c_test.h"

void i2c_test() {
	//clocking on
	uint32_t clken = CMM_APB0CLKEN_I2C0 | CMM_APB0CLKEN_I2C1;
	CMM->apb0clken |= clken;

	//enabling alternative functions GPIOA
	uint32_t altfen = GPIOA->altf0 | GPIOA_ALTF0_P16_I2C0_SCL | GPIOA_ALTF0_P17_I2C0_SDA
				| GPIOA_ALTF0_P18_I2C1_SCL | GPIOA_ALTF0_P19_I2C1_SDA;
	GPIOA->altf0 |= altfen;

	I2C0->prsc = 0xA;
	I2C1->prsc = 0xA;

	I2C0->rxThreshold = 0x7;
	I2C1->rxThreshold = 0x7;

	I2C1->addr = 0xF;
	I2C0->txFifo = (0xF << 1);

	I2C0->txFifo = 0x70;
	I2C0->txFifo = 0xFF;
	I2C0->txFifo = 0xAA;
	I2C0->txFifo = 0xBB;
	I2C0->txFifo = 0x33;
	I2C0->txFifo = 0x33;
	I2C0->txFifo = 0x80;
	I2C0->txFifo = 0xFF;

	I2C0->cfg = 0;
	I2C1->cfg = 0;

	I2C1->ctrl = 0b1001;
	I2C0->ctrl = 0b1011;

	while((I2C0->st & 1) != 1);
	while((I2C1->st & 1) != 1);
	while((I2C0->st & 0x400) != 0x400);

	I2C0->ctrl = (I2C1->ctrl) | 0b100;

	while((I2C0->st & 0x2) != 0x2);
	while((I2C1->st & 0x2) != 0x2);
	while((I2C1->st & 0x8000) != 0x8000);

	GPIOA->altf0 = 0b0;
	int rx_data;

	rx_data = (I2C1->rxFifo);
	if (rx_data != 0x70) while(1);
	rx_data = (I2C1->rxFifo);
	if (rx_data != 0xFF) while(1);
	rx_data = (I2C1->rxFifo);
	if (rx_data != 0xAA) while(1);
	rx_data = (I2C1->rxFifo);
	if (rx_data != 0xBB) while(1);
	rx_data = (I2C1->rxFifo);
	if (rx_data != 0x33) while(1);
	rx_data = (I2C1->rxFifo);
	if (rx_data != 0x33) while(1);
	rx_data = (I2C1->rxFifo);
	if (rx_data != 0x80) while(1);

	I2C0->ctrl = 0;
	I2C1->ctrl = 0;

	//clocking off
	CMM->apb0clken &= ~clken;

	//disabling alternative GPIOA functions
	GPIOA->altf0 &= ~altfen;
}
