
#include "core.h"
#include "stm32f446xx.h"

typedef uint32_t u32;
typedef uint8_t u8;


#define FCPU        16000000

/* Memory MACRO */
#define APP_START_ADDRESS   0x08004000

#define SRAM_SIZE   (128*1024)
#define SRAM_END    (SRAM_BASE + SRAM_SIZE)


#define ENABLE_BOOTLOADER_PROTECTION  0






/* Startup and interrupt handler functions declaration */
void _mcu_start (void);
int  main (void);
void SysTick_Handler (void);
void USART2_IRQHandler (void);

/* Minimal vector table needed by the bootloader */

u32 *vector_table [] __attribute__((section(".isr_vector"))) = {
    (u32*) SRAM_END,        // MSP, initial stack pointer
    (u32*) _mcu_start,      // main function as reset handler
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  0,
    (u32*)  SysTick_Handler,

    0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,
    (u32*) USART2_IRQHandler
};



u32 gTick = 0;

/* Systich Handler Routine placed in CMM memory*/
void SysTick_Handler (void) {
  
  gTick ++;
}

u32 getTick (void) {
  return gTick;
}



extern u32 _sidata;
extern u32 _sdata;
extern u32 _edata;

extern u32 _sbss;
extern u32 _ebss;

void __initialize_data (u32 * flash_begin, u32 *data_begin, u32 * data_end) {
    u32 * ptr = data_begin;
    while (ptr < data_end) {
        *ptr++ = *flash_begin++;
    }
}

void __initialize_bss (u32* bss_begin, u32* bss_end) {
    u32* ptr = bss_begin;
    while (ptr < bss_end) {
        *ptr++ = 0;
    }
}

void __attribute__ ((noreturn, weak)) _mcu_start (void) {
    __initialize_data (&_sidata, &_sdata, &_edata);
    __initialize_bss (&_sbss, &_ebss);
    main ();

    for (;;);
}





#include "usart2.h"
#include "TI_aes_128.h"
#include <string.h>



#define ACK         0x79
#define NACK        0x1f
#define CMD_ERASE   0x43
#define CMD_GETID   0x02
#define CMD_WRITE   0x2b 



#define LED_PIN   5
#define LED_PORT  GPIOA
#define BTN_PIN   13
#define BTN_PORT  GPIOC


#define GPIO_PIN_VALUE(port, pin)    ((port->IDR &= (1<<pin)) != 0) 
#define GPIO_PIN_TOGGLE(port, pin)   (port->ODR ^= (1<<pin))
#define GPIO_PIN_SET_HIGH(port, pin) (port->BSRR |= (1<<pin))
#define GPIO_PIN_SET_LOW(port, pin)  (port->BSRR |= (1<<(pin+16)))


#define PIN_VALUE(x) GPIO_PIN_VALUE(x##_PORT, x##_PIN)
#define PIN_TOGGLE(x) GPIO_PIN_TOGGLE(x##_PORT, x##_PIN)
#define PIN_SET_HIGH(x) GPIO_PIN_SET_HIGH(x##_PORT, x##_PIN)
#define PIN_SET_LOW(x)  GPIO_PIN_SET_LOW(x##_PORT, x##_PIN)



#define FLASH_KEY1                0x45670123U
#define FLASH_KEY2                0xCDEF89ABU

#define FLASH_PSIZE_BYTE          0x00000000U
#define FLASH_PSIZE_HALF_WORD     0x00000100U
#define FLASH_PSIZE_WORD          0x00000200U
#define FLASH_PSIZE_DOUBLE_WORD   0x00000300U
#define CR_PSIZE_MASK             0xFFFFFCFFU

#define FLASH_VOLTAGE_RANGE_1     0x00000000U  /*!< Device operating range: 1.8V to 2.1V                */
#define FLASH_VOLTAGE_RANGE_2     0x00000001U  /*!< Device operating range: 2.1V to 2.7V                */
#define FLASH_VOLTAGE_RANGE_3     0x00000002U  /*!< Device operating range: 2.7V to 3.6V                */
#define FLASH_VOLTAGE_RANGE_4     0x00000003U  /*!< Device operating range: 2.7V to 3.6V + External Vpp */

#define FLASH_SECTOR_0     0U /*!< Sector Number 0   */
#define FLASH_SECTOR_1     1U /*!< Sector Number 1   */
#define FLASH_SECTOR_2     2U /*!< Sector Number 2   */
#define FLASH_SECTOR_3     3U /*!< Sector Number 3   */
#define FLASH_SECTOR_4     4U /*!< Sector Number 4   */
#define FLASH_SECTOR_5     5U /*!< Sector Number 5   */
#define FLASH_SECTOR_6     6U /*!< Sector Number 6   */
#define FLASH_SECTOR_7     7U /*!< Sector Number 7   */

#define FLASH_SECTOR_TOTAL 8U

#define FLASH_TIMEOUT_VALUE       50000U /* 50 s */

u32 calculateCRC (u32 * buffer, u32 len);
void FLASH_Unlock (void);
void FLASH_Lock (void);
void FLASH_Erase (u8 firstSector, u8 nbrSector, u8 voltageLevel);
void FLASH_EraseSector (uint32_t Sector, uint8_t VoltageRange);
void FLASH_WaitForLastOperation ();
void FLASH_ProgramByte (uint32_t Address, u8 Data);



void cmdGetId (u8* buffer);
void cmdErase (u8* buffer);
void cmdWrite(u8* buffer);

uint8_t AES_KEY[] = { 0x4D, 0x61, 0x73, 0x74, 0x65, 0x72, 0x69, 0x6E, 
                      0x67, 0x20, 0x20, 0x53, 0x54, 0x4D, 0x33, 0x32 };



#define NVIC_PRIORITYGROUP_0         0x00000007U /*!< 0 bits for pre-emption priority
                                                      4 bits for subpriority */
#define NVIC_PRIORITYGROUP_1         0x00000006U /*!< 1 bits for pre-emption priority
                                                      3 bits for subpriority */
#define NVIC_PRIORITYGROUP_2         0x00000005U /*!< 2 bits for pre-emption priority
                                                      2 bits for subpriority */
#define NVIC_PRIORITYGROUP_3         0x00000004U /*!< 3 bits for pre-emption priority
                                                      1 bits for subpriority */
#define NVIC_PRIORITYGROUP_4         0x00000003U /*!< 4 bits for pre-emption priority
                                                      0 bits for subpriority */




int main(void) {

  NVIC_SetPriorityGrouping (NVIC_PRIORITYGROUP_4);
  
  // enable clock for GPIO and GPIOC
  RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN_Msk | RCC_AHB1ENR_GPIOCEN_Msk;


  char buffer [20];

  // configure systick for firing IR every 1ms
  SysTick_Config(FCPU/1000); 

  // configure GPIOA<5> as output
  GPIOA->MODER |= (0b01 << (2*LED_PIN));
  // GPIOC<13> is already in input mode


  // wait for 2 seconds for the user to press the User Button
  u32 tick = getTick (), blinkTick = tick;
  u8 upload = 0;

  while (getTick () - tick < 2000) {
    if (PIN_VALUE (BTN) == 0) {
      upload = 1;
      break;
    }
    // blink led every 250ms
    if (getTick() - blinkTick > 250) {
      PIN_TOGGLE (LED);
      blinkTick = getTick();
    }
  }
  


  if (upload) {
    // wait for program flash

    blinkTick = getTick ();
    u8 usartBuffer [20];
    usart2_init ();
    usart2_puts ("UPLOAD\n\n");
    // enable CRC module clock
    RCC->AHB1ENR |= RCC_AHB1ENR_CRCEN;

    PIN_SET_LOW (LED);
    while(1) {
      // blink led every 250ms
      if (getTick() - blinkTick > 500) {
        //PIN_TOGGLE (LED);
        blinkTick = getTick();
      }

      // implement command reception from usart

      if (usart2_data_available () >= 20) {

        // read data
        usart2_read_buff (usartBuffer, 20);
        u8 cmd = usartBuffer[0];
        switch (cmd) {
        case CMD_GETID:
          cmdGetId (usartBuffer);
          break;

        case CMD_ERASE:
          cmdErase (usartBuffer);
          break;

        case CMD_WRITE:
          cmdWrite(usartBuffer);
          break;
        };
      }
    }
  } 

  else {

    // if there is no application to run, blink led fast
    if (*((u32*)APP_START_ADDRESS) != SRAM_END) {
      blinkTick = getTick ();
      while(1) {
        if (getTick() - blinkTick > 80) {
          PIN_TOGGLE (LED);
          blinkTick = getTick();
        }
      }
    }

    //Load the application
    
    /* Deinit GPIO and clocks */
    GPIOA->MODER = 0x00000000;
    GPIOA->ODR = 0x00000000;
    RCC->AHB1ENR &= ~(RCC_AHB1ENR_GPIOAEN_Msk | RCC_AHB1ENR_GPIOCEN_Msk);

    /* Deinit systick and its interrupt */
    SysTick->CTRL = 0x0;

    RCC->CIR = 0x00000000;
    __set_MSP (* ((volatile u32*) APP_START_ADDRESS));
    __DMB ();
    // relocate vector table to sector 1 (start of user app)
    SCB->VTOR = APP_START_ADDRESS;
    __DSB ();

    // get the memory address of the user application's reset handler
    u32 jumpAddress = *((volatile u32*) (APP_START_ADDRESS + 4));
    void (*resetHandler) (void) = (void *) jumpAddress;

    // launch the user application
    resetHandler ();

    // never reached
    while(1);
  }
}




void cmdGetId (u8* data) {
  u16 devID;
  u32 crc = 0;
  u32 cmd = data[0];

  // copy the crc 
  memcpy (&crc, data+1, sizeof(u32));

  u32 calculatedCrc = calculateCRC (&cmd, 1);

  if (crc == calculatedCrc) {

    devID = (u16) (DBGMCU->IDCODE & 0x0fff);

    // Send acknoledge and device ID
    data [0] = ACK;
    usart2_write_buff (data, 1);
    usart2_write_buff ((u8*)&devID, 2);
  }

  else {
    // Send negative acknoledge
    data [0] = NACK;
    usart2_write_buff (data, 1);
  }
}



void cmdErase (u8* data) {
  u32 crc = 0;
  u32 cmd[] = {data [0], data[1]};

  memcpy (&crc, data +2, sizeof (u32));

  if (crc == calculateCRC (cmd, 2)) {

    /*Erase data [1] flash memory sectors starting from sector 1*/
    FLASH_Unlock ();
    FLASH_Erase (FLASH_SECTOR_1, data[1] == 0xff ? FLASH_SECTOR_TOTAL : data[1], FLASH_VOLTAGE_RANGE_3);
    FLASH_Lock ();  
    data [0] = ACK;
    usart2_write_buff (data, 1);
  }

  else {
    // Send negative acknoledge
    data [0] = NACK;
    usart2_write_buff (data, 1);
  }
}

void cmdWrite(u8* data) {

  u32 sAddr = 0, crc = 0;

  memcpy (&sAddr, data+1, sizeof (u32));
  memcpy (&crc, data+5, sizeof (u32));

  u32 cmdData [5];
  for (u8 i=0; i<5; ++i)
    cmdData[i] = data[i];

  u32 calculatedCrc = calculateCRC(cmdData, 5);

  if (crc == calculatedCrc && sAddr >= APP_START_ADDRESS) {

    data[0] = ACK;
    usart2_write_buff (data, 1);

    // wait for data
    while (usart2_data_available() < 20);
    u8 dataBuffer [20];
    usart2_read_buff (dataBuffer, 20);
    memcpy (&crc, dataBuffer+16, sizeof (u32));

    calculatedCrc = calculateCRC ((u32*)dataBuffer, 4);
    if (crc == calculatedCrc) {

      FLASH_Unlock ();
      //aes_enc_dec((uint8_t*) data, AES_KEY, 1);
      for (u8 i=0; i<16; ++i) {
        FLASH_ProgramByte (sAddr, dataBuffer[i]);
        sAddr += 1;
      }
      FLASH_Lock ();

      data[0] = ACK;
      usart2_write_buff (data, 1);
    }else {
      data[0] = NACK;
      usart2_write_buff (data, 1);
    }
  }else {
      data[0] = NACK;
      usart2_write_buff (data, 1);
  }
}







void FLASH_Unlock (void) {

  if((FLASH->CR & FLASH_CR_LOCK) != 0) {
    FLASH->KEYR = FLASH_KEY1;
    FLASH->KEYR = FLASH_KEY2;
  }
}

void FLASH_Lock (void) {
  FLASH->CR |= FLASH_CR_LOCK;
}





void FLASH_Erase (u8 firstSector, u8 nbrSector, u8 voltageLevel) {

  /* Wait for last operation to be completed */
  FLASH_WaitForLastOperation((uint32_t)FLASH_TIMEOUT_VALUE);

  for (u32 sector = firstSector; sector < (firstSector+nbrSector); sector++) {
    FLASH_EraseSector(sector, (uint8_t) voltageLevel);

    /* Wait for last operation to be completed */
    FLASH_WaitForLastOperation((uint32_t)FLASH_TIMEOUT_VALUE);

    /* If the erase operation is completed, disable the SER and SNB Bits */
    FLASH->CR &= ~(FLASH_CR_SER | FLASH_CR_SNB);
  }
}


void FLASH_EraseSector (uint32_t Sector, uint8_t VoltageRange) {

  uint32_t tmp_psize = 0U;

  switch (VoltageRange) {
  case FLASH_VOLTAGE_RANGE_1:
    tmp_psize = FLASH_PSIZE_BYTE;
    break;
  
  case FLASH_VOLTAGE_RANGE_2:
    tmp_psize = FLASH_PSIZE_HALF_WORD;
    break;

  case FLASH_VOLTAGE_RANGE_3:
    tmp_psize = FLASH_PSIZE_WORD;
    break;
  default:
    tmp_psize = FLASH_PSIZE_DOUBLE_WORD;
  }

  // reset parallelism and sector number bit
  FLASH->CR &= FLASH_CR_PSIZE;
  FLASH->CR &= FLASH_CR_SNB;

  // set program parallelism
  FLASH->CR |= tmp_psize;
  // select sector erase mode and the desired sector number
  FLASH->CR |= FLASH_CR_SER | (Sector << FLASH_CR_SNB_Pos);
  // start erase operation
  FLASH->CR |= FLASH_CR_STRT;

  FLASH_WaitForLastOperation ();
}

void FLASH_WaitForLastOperation () {
  // wait for operation to complete
  while((FLASH->SR & FLASH_SR_BSY_Msk) != 0);

  // clear end of operation flag if it was set
  if ((FLASH->SR & FLASH_SR_EOP_Msk) != 0)
    FLASH->SR |= FLASH_SR_EOP_Msk;
}


void FLASH_ProgramByte (uint32_t Address, u8 Data) {
  FLASH_WaitForLastOperation ();

  FLASH->CR &= ~(FLASH_CR_PSIZE);
  FLASH->CR |= FLASH_PSIZE_BYTE;
  FLASH->CR |= FLASH_CR_PG;

  // write the byte into FLASH as if it was with a normal memory address
  *(volatile uint8_t*)Address = Data;

  FLASH_WaitForLastOperation ();
  FLASH->CR &= (~FLASH_CR_PG); 
}



u32 calculateCRC (u32 * buffer, u32 len) {


  CRC->CR |= CRC_CR_RESET;

  for (u32 i=0; i<len; ++i) {
    CRC->DR = buffer[i];
  }
  return CRC->DR;
}
