#include "stm32f030x6.h"
#include <i2c.h>
#include <stdint.h>

void i2c_init(void)
{
	RCC->APB1ENR |= RCC_APB1ENR_I2C1EN;

	/* Use SYSCLK for I2C, because HSI (default) is disabled */
	RCC->CFGR3 |= RCC_CFGR3_I2C1SW;

	i2c_reset();

	/* Setup clocks */
	I2C1->TIMINGR = ((6 << 28) & I2C_TIMINGR_PRESC) | ((8 << 20) & I2C_TIMINGR_SCLDEL) |
			((8 << 16) & I2C_TIMINGR_SDADEL) | ((0x20 << 8) & I2C_TIMINGR_SCLH) |
			((0x20 << 0) & I2C_TIMINGR_SCLL);


	/* Peripheral enable */
	I2C1->CR1 = I2C_CR1_PE;
}

int i2c_write(uint8_t i2c_addr, const uint8_t *data, uint8_t len)
{
	uint32_t isr;

	/* Clear stop flag */
	I2C1->ICR |= I2C_ICR_STOPCF;

	/* Setup len bytes for write transfer */
	I2C1->CR2 = I2C_CR2_AUTOEND | ((len << 16 ) & I2C_CR2_NBYTES) | (i2c_addr & 0xFE) | I2C_CR2_START;
	
	do {
		isr = I2C1->ISR;
		if (isr & I2C_ISR_NACKF) {
			/* NACK received */
			return -1;
		} else if (isr & I2C_ISR_TXIS) {
			/* I2C ready to get data to send */
			I2C1->TXDR = (uint32_t)*data;
			data++;
			len--;
		}
	} while (len > 0);

	/* All data transferred. Wait for automatically generated stop */
	while (!(I2C1->ISR & I2C_ISR_STOPF)) {
		if (I2C1->ISR & I2C_ISR_NACKF) {
			return -1;
		}
	}

	/* Clear stop flag */
	I2C1->ICR |= I2C_ICR_STOPCF;

	return 0;
}

int i2c_read(uint8_t i2c_addr, uint8_t command, uint8_t *data, uint8_t len)
{
	int run;
	int err;
	uint32_t isr;

	if (!data || !len) {
		return -1000;
	}

	/* Clear stop flag */
	I2C1->ICR |= I2C_ICR_STOPCF;

	/* Setup len bytes for write transfer  of command */
	I2C1->CR2 = ((1 << 16 ) & I2C_CR2_NBYTES) | (i2c_addr & 0xFE) | I2C_CR2_START;

	err = 0;
	run = 1;
	do {
		isr = I2C1->ISR;

		if (isr & I2C_ISR_NACKF) {
			err = 1;
			run = 0;
		} else if (isr & I2C_ISR_TXIS) {
			I2C1->TXDR = (uint32_t)command;
			run = 0;
		}
	} while (run);

	if (err) {
		return -1;
	}

	/* Wait for command to be transmitted */
	run = 1;
	err = 0;
	do {
		isr = I2C1->ISR;

		if (isr & I2C_ISR_NACKF) {
			err = 1;
			run = 0;
		} else if (isr & I2C_ISR_TC) {
			run = 0;
		}
	} while (run);

	if (err) {
		return -1;
	}

	/* Setup read request */
	I2C1->CR2 = I2C_CR2_AUTOEND | I2C_CR2_RD_WRN | ((len << 16 ) & I2C_CR2_NBYTES) | (i2c_addr & 0xFE) | I2C_CR2_START;
	while (len) {
		if (I2C1->ISR & I2C_ISR_RXNE) {
			*data = I2C1->RXDR;
			data++;
			len--;
		}
	}

	while (!(I2C1->ISR & I2C_ISR_STOPF)) {
		if (I2C1->ISR & I2C_ISR_NACKF) {
			return -1;
		}
	}

	/* Clear stop flag */
	I2C1->ICR |= I2C_ICR_STOPCF;

	return 0;
}

void i2c_reset(void)
{
	I2C1->CR1 = 0;
	I2C1->CR2 = 0;
}