#include "stm32f10x.h"
#include "utils.h"
#include "lsm6ds3_spi.h"

void lsm6ds3_init(void)
{
  GPIOA->CRL &= ~GPIO_CRL_CNF4; // clear all used
  GPIOA->CRL |= GPIO_CRL_MODE4_0; // PA4 (/CS, OUTPUT-PP), medium speed (10 MHz)
  GPIOA->BSRR = GPIO_BSRR_BS4; // CS high

  lsm6ds3_write_byte(LSM6DS3_CTRL4_C, 0x84); // accel bandwidth set individually, I2C disable
  lsm6ds3_write_byte(LSM6DS3_CTRL1_XL, 0x63); // accel HP ODR 416 Hz, BW 50 Hz, +/- 2g
  lsm6ds3_write_byte(LSM6DS3_CTRL2_G, 0x64); // gyro HP ODR 416, BW 139 Hz, +/- 500 °/s
  lsm6ds3_write_byte(LSM6DS3_CTRL3_C, 0x44); // block data update, increment register address
}

void lsm6ds3_read_data(void)
{
  uint8_t i;
  uint8_t *p_data = (uint8_t *) &lsm6ds3_data.gyro;

  GPIOA->BSRR = GPIO_BSRR_BR4; // CS low

  spi_send_receive(0x80 | LSM6DS3_OUTX_L_G); // read
  for(i = 0; i < sizeof(lsm6ds3_data); i++)
    *p_data++ = spi_send_receive(0xFF);

  GPIOA->BSRR = GPIO_BSRR_BS4; // CS high
}

void lsm6ds3_write_byte(uint8_t address, uint8_t data)
{
  GPIOA->BSRR = GPIO_BSRR_BR4; // CS low

  spi_send_receive(address); // write
  spi_send_receive(data);

  GPIOA->BSRR = GPIO_BSRR_BS4; // CS high
}

uint8_t lsm6ds3_read_byte(uint8_t address)
{
  uint8_t data;

  GPIOA->BSRR = GPIO_BSRR_BR4; // CS low

  spi_send_receive(0x80 | address); // read
  data = spi_send_receive(0xFF);

  GPIOA->BSRR = GPIO_BSRR_BS4; // CS high

  return data;
}

void spi_init(void)
{
  GPIOA->CRL &= ~(GPIO_CRL_CNF7 | GPIO_CRL_CNF6 | GPIO_CRL_CNF5); // clear all used
  // PA7 (SPI1 MOSI, AF-PP), PA6 (SPI1 MISO, INPUT), PA5 (SPI1 SCK, AF-PP), medium speed (10 MHz)
  GPIOA->CRL |= GPIO_CRL_CNF7_1 | GPIO_CRL_MODE7_0 | GPIO_CRL_CNF6_0 | GPIO_CRL_CNF5_1 | GPIO_CRL_MODE5_0;

  SPI1->CR1 |= SPI_CR1_BR_1 | SPI_CR1_MSTR | SPI_CR1_CPOL | SPI_CR1_CPHA; // PCLK / 8 (4 MHz), master mode (8-bit data format), idle clock high, second edge sampling
  SPI1->CR2 |= SPI_CR2_SSOE; // slave select output enable (but it's not connected to IO pin)
  SPI1->CR1 |= SPI_CR1_SPE; // SPI1 enable
}

uint8_t spi_send_receive(uint8_t data)
{
  SPI1->DR = data;
  while(!(SPI1->SR & SPI_SR_TXE)) // send data, wait for complete transmission
    ;
  while(!(SPI1->SR & SPI_SR_RXNE)) // receive data, wait for complete reception
    ;
  while((SPI1->SR & SPI_SR_BSY)) // wait if the bus is busy
    ;

  return (uint8_t) SPI1->DR;
}
