#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/CEM)";
const char MFR_MDL[] = "DI/OT MoniMod (bl)";
const char MFR_REV[] = "0.3.0";
const char MFR_LOC[] = "Geneva";
const char MFR_DAT[] = "190801";
const char MFR_SER[] = "123456789";
const char IC_DEVICE_REV[] = __GIT_VER__;

void query_prp();
uint8_t query_r;

void written_fw_block();
void written_fw_checksum();
void uc_reset();
void get_fw_type(void);
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 reset_byte;
static uint8_t fw_type = FW_BOOTLOADER;

enum {
        cmd_0x1A = 0,
        cmd_0x99,
        cmd_0x9A,
        cmd_0x9B,
        cmd_0x9C,
        cmd_0x9D,
        cmd_0x9E,
        cmd_0xAE,
        /* MFR specific commands */
        cmd_0xD1,
        cmd_0xD2,
        cmd_0xD3,
        cmd_0xD4,
        cmd_0xD5,
        cmd_0xD6
};

/* "- 1" to avoid adding null char at the end of a string */
static const int8_t cmd_data_lengths[] = {
        [cmd_0x1A] = -1,
        [cmd_0x99] = -((int8_t)sizeof(MFR_ID) - 1),
        [cmd_0x9A] = -((int8_t)sizeof(MFR_MDL) - 1),
        [cmd_0x9B] = -((int8_t)sizeof(MFR_REV) - 1),
        [cmd_0x9C] = -((int8_t)sizeof(MFR_LOC) - 1),
        [cmd_0x9D] = -((int8_t)sizeof(MFR_DAT) - 1),
        [cmd_0x9E] = -((int8_t)sizeof(MFR_SER) - 1),
        [cmd_0xAE] = -((int8_t)sizeof(IC_DEVICE_REV) - 1),
        /* MFR specific commands */
        [cmd_0xD1] = 2,
        [cmd_0xD2] = 8,
        [cmd_0xD3] = 2,
        [cmd_0xD4] = 2,
        [cmd_0xD5] = 1,
        [cmd_0xD6] = 1
};

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

static cmd_t cmds_cmds[] = (cmd_t[]){
        {0x1A, (int8_t *)&cmd_data_length_query,      (uint8_t *)&query_r,           (fp_t)NULL, &query_prp,                 (fp_t)NULL, QUERY_WR,                 1},
        {0x99, (int8_t *)&cmd_data_lengths[cmd_0x99], (uint8_t *)&MFR_ID,            (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x9A, (int8_t *)&cmd_data_lengths[cmd_0x9A], (uint8_t *)&MFR_MDL,           (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x9B, (int8_t *)&cmd_data_lengths[cmd_0x9B], (uint8_t *)&MFR_REV,           (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x9C, (int8_t *)&cmd_data_lengths[cmd_0x9C], (uint8_t *)&MFR_LOC,           (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x9D, (int8_t *)&cmd_data_lengths[cmd_0x9D], (uint8_t *)&MFR_DAT,           (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0x9E, (int8_t *)&cmd_data_lengths[cmd_0x9E], (uint8_t *)&MFR_SER,           (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0xAE, (int8_t *)&cmd_data_lengths[cmd_0xAE], (uint8_t *)&IC_DEVICE_REV,     (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD | QUERY_FMT_NAN, 0},
        {0xD1, (int8_t *)&cmd_data_lengths[cmd_0xD1], (uint8_t *)&fw_len,            (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_WR | QUERY_RD,      0},
        {0xD2, (int8_t *)&cmd_data_lengths[cmd_0xD2], (uint8_t *)fw_block,           (fp_t)NULL, (fp_t)&written_fw_block,    (fp_t)NULL, QUERY_WR | QUERY_RD,      1},
        {0xD3, (int8_t *)&cmd_data_lengths[cmd_0xD3], (uint8_t *)&fw_checksum,       (fp_t)NULL, (fp_t)&written_fw_checksum, (fp_t)NULL, QUERY_WR | QUERY_RD,      0},
        {0xD4, (int8_t *)&cmd_data_lengths[cmd_0xD4], (uint8_t *)&fw_local_checksum, (fp_t)NULL, (fp_t)NULL,                 (fp_t)NULL, QUERY_RD,                 0},
        {0xD5, (int8_t *)&cmd_data_lengths[cmd_0xD5], (uint8_t *)&fw_type,           &get_fw_type, (fp_t)&boot_new_fw,       (fp_t)NULL, QUERY_WR | QUERY_RD,      0},
        {0xD6, (int8_t *)&cmd_data_lengths[cmd_0xD6], (uint8_t *)&reset_byte,        (fp_t)NULL, (fp_t)&uc_reset,            (fp_t)NULL, QUERY_WR,                 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()
{
        fw_local_checksum = 0;

        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()
{
        /* Check magic */
        if (reset_byte != UC_RESET_MAGIC) {
                /* Wrong magic, abort */
                return;
        }

        NVIC_SystemReset();
}

void get_fw_type(void)
{
        fw_type = FW_BOOTLOADER;
}

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;

        /* Check magic */
        if (fw_type != BOOT_NEW_FW_MAGIC) {
                /* Wrong magic, abort */
                return;
        }

        /* 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);

        /* Set a proper Magic for uC reset */
        reset_byte = UC_RESET_MAGIC;
        /* Reset the uC */
        uc_reset();
}
