#include <atmel_start.h>
#include <stdlib.h>

#include <i2c_slave.h>
#include <flash_utils.h>
#include <jump_func.h>
#include <checksums.h>


const char MFR_ID[] = "CERN (be/co)";
const char MFR_MDL[] = "DIOT MoniMod (bl)";
const char MFR_REV[] = "0.1.0";
const char MFR_LOC[] = "Geneva";
const char MFR_DAT[] = "190801";
const char MFR_SER[] = "123456789";

void query_prp();
uint8_t query_r;

void written_fw_block();
void written_fw_checksum();
void uc_reset();
void boot_new_fw();

static uint16_t fw_len;
static uint16_t fw_checksum;
static uint16_t fw_local_checksum;
static uint8_t fw_block[8];

static uint8_t dummy_byte;

static const int8_t cmd_data_lengths[] = {
        -1,                        // 0x1A
        -(int8_t)sizeof(MFR_ID),   // 0x99
        -(int8_t)sizeof(MFR_MDL),  // 0x9A
        -(int8_t)sizeof(MFR_REV),  // 0x9B
        -(int8_t)sizeof(MFR_LOC),  // 0x9C
        -(int8_t)sizeof(MFR_DAT),  // 0x9D
        -(int8_t)sizeof(MFR_SER)}; // 0x9E

// for block write-block read process calls, which modify the length
static int8_t cmd_data_length_query = -1;

static const int8_t ext_cmd_data_lengths[] = {
        2,  // 0x05
        8,  // 0x06
        2,  // 0xA0
        1,  // 0xB0
        1,  // 0xC0
        1}; // 0xC4

static cmd_t cmds_cmds[] = (cmd_t[]){
        {0x001A, (int8_t *)&cmd_data_length_query, (uint8_t *)&query_r,  (fp_t)NULL, &query_prp, (fp_t)NULL, QUERY_SUP, 1},
        {0x0099, (int8_t *)&cmd_data_lengths[1], (uint8_t *)&MFR_ID,   (fp_t)NULL, (fp_t)NULL, (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x009A, (int8_t *)&cmd_data_lengths[2], (uint8_t *)&MFR_MDL,  (fp_t)NULL, (fp_t)NULL, (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x009B, (int8_t *)&cmd_data_lengths[3], (uint8_t *)&MFR_REV,  (fp_t)NULL, (fp_t)NULL, (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x009C, (int8_t *)&cmd_data_lengths[4], (uint8_t *)&MFR_LOC,  (fp_t)NULL, (fp_t)NULL, (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x009D, (int8_t *)&cmd_data_lengths[5], (uint8_t *)&MFR_DAT,  (fp_t)NULL, (fp_t)NULL, (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x009E, (int8_t *)&cmd_data_lengths[6], (uint8_t *)&MFR_SER,  (fp_t)NULL, (fp_t)NULL, (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0xFF01, (int8_t *)&ext_cmd_data_lengths[0], (uint8_t *)&fw_len,      (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, 0, 0},
        {0xFF02, (int8_t *)&ext_cmd_data_lengths[1], (uint8_t *)fw_block,     (fp_t)NULL, (fp_t)&written_fw_block,    (fp_t)NULL, 0, 1},
        {0xFF03, (int8_t *)&ext_cmd_data_lengths[2], (uint8_t *)&fw_checksum, (fp_t)NULL, (fp_t)&written_fw_checksum, (fp_t)NULL, 0, 0},
        {0xFF04, (int8_t *)&ext_cmd_data_lengths[3], (uint8_t *)&fw_local_checksum, (fp_t)NULL, (fp_t)NULL,           (fp_t)NULL, 0, 0},
        {0xFF05, (int8_t *)&ext_cmd_data_lengths[4], (uint8_t *)&dummy_byte,  (fp_t)NULL, (fp_t)&boot_new_fw,         (fp_t)NULL, 0, 0},
        {0xFF06, (int8_t *)&ext_cmd_data_lengths[5], (uint8_t *)&dummy_byte,  (fp_t)NULL, (fp_t)&uc_reset,            (fp_t)NULL, 0, 0}};

cmd_space_t cmds = {
        sizeof(cmds_cmds)/sizeof(cmd_t),
        cmds_cmds
};

static uint8_t fw_write_buf[FLASH_ROW_SIZE] __attribute__((__aligned__(4)));
static uint8_t fw_write_cnt_octobyte;
static uint16_t fw_write_cnt_page;
static uint32_t *fw_write_addr = (uint32_t *)0x2000;
static uint32_t *fw_write_pntr;


void query_prp()
{
        int16_t cmd_idx = in_addr_space(&cmds, query_r);
        if (cmd_idx != -1)
                query_r = QUERY_SUP | cmds.cmds[cmd_idx].query_byte;
        else
                query_r = 0;
}


void written_fw_block()
{
        for (uint8_t i = 0; i < 8; ++i)
                fw_write_buf[i + 8*fw_write_cnt_octobyte] = fw_block[i];
        if (++fw_write_cnt_octobyte == FLASH_ROW_SIZE/8) { // = 256/8 = 32
                fw_write_cnt_octobyte = 0;
                fw_write_pntr = fw_write_addr + (fw_write_cnt_page++)*FLASH_ROW_SIZE/4;
                flash_write_row(fw_write_pntr, (uint32_t *)fw_write_buf);
        }
}


void write_last_fw_block()
{
        if (fw_write_cnt_octobyte) {
                while (fw_write_cnt_octobyte < FLASH_ROW_SIZE/8) {
                        for (uint8_t i = 0; i < 8; ++i)
                                fw_write_buf[i + 8*fw_write_cnt_octobyte] = 0;
                        ++fw_write_cnt_octobyte;
                }
                fw_write_pntr = fw_write_addr + (fw_write_cnt_page++)*FLASH_ROW_SIZE/4;
                flash_write_row(fw_write_pntr, (uint32_t *)fw_write_buf);
        }
}


void written_fw_checksum()
{
        write_last_fw_block();

        fw_local_checksum = sysv_checksum((uint8_t *)fw_write_addr, (uint8_t *)((uint32_t)fw_write_addr + 8*fw_len));

        fw_write_cnt_page = 0;
        fw_write_cnt_octobyte = 0;
}


void uc_reset()
{
        NVIC_SystemReset();
}


void boot_new_fw()
{
        uint8_t *char_user_flash = (uint8_t *)&user_flash;
        user_flash_t *tmp_user_flash = (user_flash_t *)fw_write_buf;

        /* backup existing user data */
        for (uint16_t i = 0; i < FLASH_ROW_SIZE; ++i)
                fw_write_buf[i] = char_user_flash[i];

        /* reset copy FW code */
        tmp_user_flash->copy_fw = 0;

        /* copy modified block back to the flash */
        flash_write_row((uint32_t *)&user_flash, (uint32_t *)fw_write_buf);

        /* reset the uC */
        uc_reset();
}
