#include "stm32g4xx.h"

#include <string.h>

#include "gpio.h"

#include "i2c.h"

static void Init(I2C_TypeDef* I2C)
{
  I2C->TIMINGR = 0x00303D5BUL; // 100kHz
  
  //I2C->TIMINGR = 	0x30108DFF;
  
  I2C->CR1 = I2C_CR1_PE;

  while(I2C->ISR & I2C_ISR_BUSY) { }
}
//------------------------------------------------------------------------------

void I2C2_Init()
{
  if (RCC->APB1ENR1 & RCC_APB1ENR1_I2C2EN) return;

  RCC->APB1ENR1 |= RCC_APB1ENR1_I2C2EN;

  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOAEN;

  GPIO_InitPin(GPIO_PIN_8 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
  GPIO_InitPin(GPIO_PIN_9 | GPIO_PORT_A | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);

  Init(I2C2);
}
//------------------------------------------------------------------------------

void I2C1_Init()
{
  if (RCC->APB1ENR1 & RCC_APB1ENR1_I2C1EN) return;

  RCC->APB1ENR1 |= RCC_APB1ENR1_I2C1EN;

  RCC->AHB2ENR |= RCC_AHB2ENR_GPIOBEN;

  GPIO_InitPin(GPIO_PIN_8 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);
  GPIO_InitPin(GPIO_PIN_9 | GPIO_PORT_B | GPIO_ALTER | GPIO_AF4 | GPIO_OSPEED_HIGH | GPIO_OPENDRAIN | GPIO_PULLUP);

  Init(I2C1);
}
//------------------------------------------------------------------------------

static void Stop(I2C_TypeDef* I2C)
{
  I2C->CR2 |= I2C_CR2_STOP;
  while (!(I2C->ISR & I2C_ISR_STOPF)) { asm volatile("NOP"); }
  I2C->ICR = I2C_ICR_STOPCF;
}
//------------------------------------------------------------------------------

static void Read(I2C_TypeDef* I2C, unsigned char Address, unsigned char* Data, unsigned char Size)
{
  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_NBYTES_Msk);
  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | I2C_CR2_RD_WRN | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
  I2C->CR2 |= I2C_CR2_START;
  
  while (Size--)
  {
    while (!(I2C->ISR & I2C_ISR_RXNE)) { }
    *Data++ = I2C->RXDR;
  }


    
    
}
//------------------------------------------------------------------------------

static void Write(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data, unsigned char Size)
{
  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size)<<I2C_CR2_NBYTES_Pos);
  I2C->CR2 |= I2C_CR2_START;

  while(I2C->CR2 & I2C_CR2_START) { asm volatile("NOP"); }
  
  while (Size--)
  {
    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
    I2C->TXDR = *Data++;
  }
  
  while(!(I2C->ISR & I2C_ISR_TC))  { asm volatile("NOP"); }//???



   

}
//------------------------------------------------------------------------------

static void Write2(I2C_TypeDef* I2C, unsigned char Address, const unsigned char* Data1, unsigned char Size1, const unsigned char* Data2, unsigned char Size2)
{
  I2C->CR2 &= ~(I2C_CR2_SADD_Msk | I2C_CR2_RD_WRN | I2C_CR2_NBYTES_Msk);
  I2C->CR2 |= (Address << (I2C_CR2_SADD_Pos + 1)) | (((unsigned long)Size1+Size2)<<I2C_CR2_NBYTES_Pos);
  I2C->CR2 |= I2C_CR2_START;
  
  while (Size1--)       
  {
    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
    I2C->TXDR = *Data1++;
  }
  
  while (Size2--)
  {
    while (!(I2C->ISR & I2C_ISR_TXE)) { asm volatile("NOP"); }
    I2C->TXDR = *Data2++;
  }
  
  while(!(I2C->ISR & I2C_ISR_TC))  { asm volatile("NOP"); }

           
}
//------------------------------------------------------------------------------

void I2C2_Write(unsigned char Address, unsigned char Data)
{
  Write(I2C2, Address, &Data, 1);
}
//------------------------------------------------------------------------------

void I2C2_Write(unsigned char Address, const void* Data, unsigned char Size)
{
  Write(I2C2, Address, (unsigned char*)Data, Size);
}
//------------------------------------------------------------------------------

void I2C2_Read(unsigned char Address, void* Data, unsigned char Size)
{
  Read(I2C2, Address, (unsigned char*)Data, Size);
}
//------------------------------------------------------------------------------

void I2C2_Stop()
{
  Stop(I2C2);
}
//------------------------------------------------------------------------------



void I2C1_Write(unsigned char Address, unsigned char Data)
{
  Write(I2C1, Address, &Data, 1);
}
//------------------------------------------------------------------------------

void I2C1_Write(unsigned char Address, const void* Data, unsigned char Size)
{
  Write(I2C1, Address, (unsigned char*)Data, Size);
}
//------------------------------------------------------------------------------

void I2C1_Write2(unsigned char Address, const void* Data1, unsigned char Size1, const void* Data2, unsigned char Size2)
{
  Write2(I2C1, Address, (unsigned char*)Data1, Size1, (unsigned char*)Data2, Size2);
}
//------------------------------------------------------------------------------

void I2C1_Read(unsigned char Address, void* Data, unsigned char Size)
{
  Read(I2C1, Address, (unsigned char*)Data, Size);
}
//------------------------------------------------------------------------------

void I2C1_Stop()
{
  Stop(I2C1);
}
//------------------------------------------------------------------------------



extern "C" {

int8_t I2C1_WriteBytes(uint8_t addr, const uint8_t *data, uint8_t size)
{
    Write(I2C1, addr, (unsigned char*)data, size);
 
    return 0;                 
}

int8_t I2C1_ReadBytes(uint8_t addr, uint8_t *data, uint8_t size)
{
    Read(I2C1, addr,(unsigned char*)data, size);

    return 0;
}

} // extern "C"