/**
 * @file bpt_update.c
 * @copyright Copyright (c) 2022  Semidrive Semiconductor.
 *            All rights reserved.
 */

#include <board.h>
#include <bpt_v2.h>
#include <config.h>
#include <ctype.h>
#include <debug.h>
#include <flash_wrapper.h>
#include <param.h>
#include <sd_boot_img/sd_boot_img.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

sfs_t *sfs = NULL;
static uint8_t sfs_buffer[FL_SFS_SIZE] __ALIGNED(CONFIG_ARCH_CACHE_LINE);
uint8_t bpt_buffer[FL_BPT_HEDAER_LEN] __ALIGNED(CONFIG_ARCH_CACHE_LINE);
bpt_iib_t iib_info_temp __ALIGNED(CONFIG_ARCH_CACHE_LINE) = {0};

/* dump data */
void hexdump(const void *ptr, size_t len)
{
    addr_t address = (addr_t)ptr;
    size_t count;

    for (count = 0; count < len; count += 16) {
        union {
            uint32_t buf[4];
            uint8_t cbuf[16];
        } u;
        size_t s = ROUNDUP(MIN(len - count, 16), 4);
        size_t i;

        printf("0x%08x: ", address);
        for (i = 0; i < s / 4; i++) {
            u.buf[i] = ((const uint32_t *)address)[i];
            printf("%08x ", u.buf[i]);
        }
        for (; i < 4; i++) {
            printf("         ");
        }
        printf("|");

        for (i = 0; i < 16; i++) {
            unsigned char c = u.cbuf[i];
            if (i < s && isprint(c)) {
                printf("%c", c);
            } else {
                printf(".");
            }
        }
        printf("|\r\n");
        address += 16;
    }
}

/* dump data */
static void BptInit(bpt_v2_t *bpt)
{
    bpt->tag = FL_BPT_TAG_VAL;
    bpt->reserved1 = 0;
    bpt->size = FL_BPT_HEDAER_LEN;
    bpt->sec_ver = 0;
    bpt->digest_alg = 3;
    memset(bpt->reserved2, 0, 19);
    bpt->rcp.tag = FL_BPT_RCP_TAG;
    bpt->rcp.size = FL_BPT_RCP_SIZE;
    memset(&bpt->rcp.rot_id, 0, 1040);
    memset(bpt->signature, 0, 512);
    memset(bpt->key_wraps, 0, 512);
    memset(bpt->reserved3, 0, 984);
    bpt->sn = FL_BPT_PAGE_SN;
    bpt->sn_inversion = ~(FL_BPT_PAGE_SN);
    memset(bpt->reserved4, 0, 8);
}

/* add iib data */
static int BptAddIIB(bpt_iib_t *iib_info, uint32_t img_base, uint32_t bpt_base,
                     uint32_t link_base, uint32_t img_size, uint8_t core)
{
    uint32_t img_offset;

    img_offset = FLASH_ADDR_MASK((uint32_t)img_base);

    if (img_offset >= (bpt_base + FL_BPT_HEDAER_LEN)) {
        img_offset -= bpt_base;
    } else {
        ssdk_printf(SSDK_ALERT, "img_base 0x%x < bpt base 0x%x\r\n", img_base,
                    bpt_base);
        return -1;
    }

    iib_info->tag = FL_BPT_IIB_TAG_VAL;
    iib_info->size = FL_BPT_IIB_SIZE;
    iib_info->reserved1 = 0;
    memset(iib_info->dbg_ctrl, 0, 8);
    memset(iib_info->did, 0, 8);
    iib_info->img_type = 0;
    iib_info->target_core = core;
    iib_info->dec_ctrl = 0;
    iib_info->reserved2 = 0;
    memset(iib_info->iv, 0, 8);
    iib_info->img_page = img_offset / FL_BPT_PAGE_SIZE;
    iib_info->img_size = img_size;

    if (link_base == 0) {
        iib_info->load_addr = (uint32_t)img_base;
    } else {
        iib_info->load_addr = link_base;
    }

    iib_info->reserved3 = 0;
    iib_info->entry = iib_info->load_addr;
    iib_info->reserved4 = 0;
    memset(iib_info->hash, 0x00, 64);
    return 0;
}

/* get the iib image base */
uint32_t BptGetIIBImgbase(bpt_v2_t *bpt, uint8_t core)
{
    bpt_iib_t *iib_info;
    uint8_t idx;

    for (idx = 0; idx < FL_BPT_IIB_NUM; idx++) {
        iib_info = (bpt_iib_t *)(bpt->iib[idx]);

        if (iib_info->tag == FL_BPT_IIB_TAG_VAL &&
            iib_info->target_core == core) {
            return (iib_info->img_page) * FL_BPT_PAGE_SIZE;
        }
    }

    return 0;
}

/* mask an iib */
static void BptMaskIIB(bpt_v2_t *bpt, uint8_t core)
{
    bpt_iib_t *iib_info;
    uint8_t idx;

    for (idx = 0; idx < FL_BPT_IIB_NUM; idx++) {
        iib_info = (bpt_iib_t *)(bpt->iib[idx]);

        if (iib_info->tag == FL_BPT_IIB_TAG_VAL &&
            iib_info->target_core == core) {
            memset(iib_info, 0, FL_BPT_IIB_SIZE);
            break;
        }
    }
}

/* remove an empty iib*/
static void BptRemoveEmptyIIB(bpt_v2_t *bpt, uint8_t index)
{
    bpt_iib_t *iib_info;
    bpt_iib_t *iib_info_next;
    uint8_t idx = index;

    for (idx = index; idx < FL_BPT_IIB_NUM - 1; idx++) {
        iib_info = (bpt_iib_t *)(bpt->iib[idx]);
        iib_info_next = (bpt_iib_t *)(bpt->iib[idx + 1]);

        if (iib_info->tag != FL_BPT_IIB_TAG_VAL) {
            memcpy(iib_info, iib_info_next, FL_BPT_IIB_SIZE);
        }
    }

    if (idx == (FL_BPT_IIB_NUM - 1)) {
        iib_info = (bpt_iib_t *)(bpt->iib[idx]);

        if (iib_info->tag != FL_BPT_IIB_TAG_VAL) {
            memset(iib_info, 0, FL_BPT_IIB_SIZE);
        }
    }
}

/* exchange two iib's position */
static void BptExchangeIIB(bpt_v2_t *bpt, uint8_t index_des, uint8_t index_src)
{
    bpt_iib_t *iib_info_des;
    bpt_iib_t *iib_info_src;

    iib_info_des = (bpt_iib_t *)(bpt->iib[index_des]);
    iib_info_src = (bpt_iib_t *)(bpt->iib[index_src]);

    memcpy(&iib_info_temp, iib_info_des, FL_BPT_IIB_SIZE);
    memcpy(iib_info_des, iib_info_src, FL_BPT_IIB_SIZE);
    memcpy(iib_info_src, &iib_info_temp, FL_BPT_IIB_SIZE);
}

/* search iib for one core */
static bpt_iib_t *BptSearchIIB(bpt_v2_t *bpt, uint8_t core)
{
    bpt_iib_t *iib_info;
    uint8_t idx;
    uint8_t core_temp = core;
    uint8_t core_remove = 0;

    if (core == FL_BPT_CORE_CR5_SX) {
        core = FL_BPT_CORE_CR5_SX0;
        core_remove = FL_BPT_CORE_CR5_SX1;
    }

    if (core == FL_BPT_CORE_CR5_SP) {
        core = FL_BPT_CORE_CR5_SP0;
        core_remove = FL_BPT_CORE_CR5_SP1;
    }

    iib_info = (bpt_iib_t *)(bpt->iib[0]);

    if ((iib_info->tag != FL_BPT_IIB_TAG_VAL) || (iib_info->target_core != 0)) {
        ssdk_printf(SSDK_ALERT, "sf core not in bpt, download sf first\r\n");
        return NULL;
    }

    for (idx = 1; idx < FL_BPT_IIB_NUM; idx++) {
        iib_info = (bpt_iib_t *)(bpt->iib[idx]);

        /* find same core */
        if (iib_info->tag == FL_BPT_IIB_TAG_VAL &&
            ((iib_info->target_core == core) ||
             (iib_info->target_core == core_temp))) {

            /* if the core is lockstep, remove the split core sp1 or sx1*/
            if (core_temp != core) {
                BptMaskIIB(bpt, core_remove);
            }

            iib_info->target_core = core_temp;
            return iib_info;
        }
    }

    for (idx = 1; idx < FL_BPT_IIB_NUM; idx++) {
        iib_info = (bpt_iib_t *)(bpt->iib[idx]);

        /* find the first invalid tag core */
        if (iib_info->tag != FL_BPT_IIB_TAG_VAL) {
            return iib_info;
        }
    }

    return NULL;
}

/* sort iib */
static void BptSortIIB(bpt_v2_t *bpt)
{
    bpt_iib_t *iib_info;
    bpt_iib_t *iib_info_next;
    uint8_t idx;
    uint8_t idy;
    uint8_t min_idy;

    for (idx = 1; idx < FL_BPT_IIB_NUM; idx++) {
        BptRemoveEmptyIIB(bpt, idx);
    }

    for (idx = 1; idx < FL_BPT_IIB_NUM - 1; idx++) {

        iib_info = (bpt_iib_t *)(bpt->iib[idx]);

        if (iib_info->tag != FL_BPT_IIB_TAG_VAL)
            break;

        min_idy = idx;

        for (idy = idx + 1; idy < FL_BPT_IIB_NUM; idy++) {
            iib_info_next = (bpt_iib_t *)(bpt->iib[idy]);

            if (iib_info_next->tag != FL_BPT_IIB_TAG_VAL)
                break;

            if (iib_info_next->target_core < iib_info->target_core) {
                min_idy = idy;
            } else if (iib_info_next->target_core == iib_info->target_core) {
                memset(iib_info_next, 0, FL_BPT_IIB_SIZE);
                BptRemoveEmptyIIB(bpt, idy);
            }
        }

        if (min_idy != idx)
            BptExchangeIIB(bpt, idx, min_idy);
    }

    return;
}

/* dump bpt information */
static int32_t dump_bpt_info(bpt_v2_t *bpt)
{
    int i = 0;
    bpt_iib_t *iib;

    ssdk_printf(SSDK_ALERT,
                "=================  FL_BPT 0  ================= \r\n");
    ssdk_printf(SSDK_ALERT, "bpt->tag                  = 0x%08x\r\n", bpt->tag);
    ssdk_printf(SSDK_ALERT, "bpt->size                 = 0x%x\r\n", bpt->size);
    ssdk_printf(SSDK_ALERT, "bpt->sec_version          = 0x%08x\r\n",
                bpt->sec_ver);
    ssdk_printf(SSDK_ALERT, "bpt->hash_alg             = 0x%x\r\n",
                bpt->digest_alg);
    ssdk_printf(SSDK_ALERT, "bpt->crc32                = 0x%08x\r\n", bpt->crc);
    ssdk_printf(SSDK_ALERT, "bpt->pac_serial_num       = 0x%08x\r\n", bpt->sn);
    ssdk_printf(SSDK_ALERT, "bpt->inver_pac_serial_num = 0x%08x\r\n",
                bpt->sn_inversion);

    for (i = 0; i < FL_BPT_IIB_NUM; i++) {
        iib = (bpt_iib_t *)(bpt->iib[i]);

        if ((iib->tag != FL_BPT_IIB_TAG_VAL)) {
            ssdk_printf(SSDK_ALERT, "iib[%d] is none\r\n", i);
            break;
        }

        ssdk_printf(SSDK_ALERT, "--------\r\n");
        ssdk_printf(SSDK_ALERT, "iib[%d]->tag            = 0x%x\r\n", i,
                    iib->tag);
        ssdk_printf(SSDK_ALERT, "iib[%d]->size           = 0x%x\r\n", i,
                    iib->size);
        ssdk_printf(SSDK_ALERT, "iib[%d]->image_type     = 0x%x\r\n", i,
                    iib->img_type);
        ssdk_printf(SSDK_ALERT, "iib[%d]->target_core    = 0x%x\r\n", i,
                    iib->target_core);
        ssdk_printf(SSDK_ALERT, "iib[%d]->decryp_ctl     = 0x%x\r\n", i,
                    iib->dec_ctrl);
        ssdk_printf(SSDK_ALERT, "iib[%d]->dev_logic_page = 0x%08x\r\n", i,
                    iib->img_page);
        ssdk_printf(SSDK_ALERT, "iib[%d]->image_size     = 0x%08x\r\n", i,
                    iib->img_size);
        ssdk_printf(SSDK_ALERT, "iib[%d]->load_base      = 0x%08x\r\n", i,
                    iib->load_addr);
        ssdk_printf(SSDK_ALERT, "iib[%d]->entry_point    = 0x%08x\r\n", i,
                    iib->entry);
    }

    return 0;
}

/* update bpt */
int BptUpdate(flash_wrapper_t *flash_wrapper, void *img_base,
              uint32_t link_base, uint32_t img_size, uint8_t core)
{
    bpt_v2_t *bpt;
    bpt_iib_t *iib_info;
    uint32_t bpt_base0, bpt_base1, bpt_base2;
    int crc;

    // read sfs
    if (sfs == NULL) {
        ssdk_printf(SSDK_ALERT, "read sfs error\r\n");
        return -1;
    }

    bpt_base0 = sfs->normal_img_base;
    bpt_base1 = sfs->backup_img_base;
    bpt_base2 = sfs->third_img_base;

    ssdk_printf(SSDK_ALERT, "bpt0: 0x%x, bpt1: 0x%x, bpt2: 0x%x\r\n", bpt_base0,
                bpt_base1, bpt_base2);

    flash_wrapper_read(flash_wrapper, (flash_addr_t)bpt_base0, bpt_buffer,
                       FL_BPT_HEDAER_LEN);

    bpt = (bpt_v2_t *)bpt_buffer;

    if (core == 0) {
        BptInit(bpt);
        iib_info = (bpt_iib_t *)(bpt->iib[0]);
    } else {
        iib_info = BptSearchIIB(bpt, core);
    }

    if (iib_info == NULL) {
        ssdk_printf(SSDK_ALERT, "Find IIB error\r\n");
        return -1;
    }

    if (BptAddIIB(iib_info, (uint32_t)img_base, bpt_base0, link_base, img_size,
                  core) < 0) {
        return -1;
    }

    BptSortIIB(bpt);

    crc = sfs_crc32(0, (uint8_t *)bpt, FL_BPT_CRC_CHECKSUM_OFFSET);
    memcpy(&bpt->crc, &crc, 4);

    if (flash_wrapper_erase(flash_wrapper, (flash_addr_t)bpt_base0,
                            FL_BPT_HEDAER_LEN)) {
        ssdk_printf(SSDK_ALERT, " erase 0x%08x, length 0x%08x error",
                    (uint32_t)bpt_base0, FL_BPT_HEDAER_LEN);
        return -1;
    }

    if (flash_wrapper_write(flash_wrapper, (flash_addr_t)bpt_base0, bpt_buffer,
                            FL_BPT_HEDAER_LEN)) {
        ssdk_printf(SSDK_ALERT, " write 0x%08x, length 0x%08x error",
                    (uint32_t)bpt_base0, FL_BPT_HEDAER_LEN);
        return -1;
    }

    if (flash_wrapper_read(flash_wrapper, (flash_addr_t)bpt_base0, bpt_buffer,
                           FL_BPT_HEDAER_LEN)) {
        ssdk_printf(SSDK_ALERT, " read 0x%08x, length 0x%08x error",
                    (uint32_t)bpt_base0, FL_BPT_HEDAER_LEN);
        return -1;
    }
    bpt = (bpt_v2_t *)bpt_buffer;
    dump_bpt_info(bpt);

    return 0;
}

/* read sfs in flash */
int read_sfs(flash_wrapper_t *flash_wrapper)
{
    uint32_t crc_val = 0;
    uint32_t crc_orig = 0;

    flash_wrapper_read(flash_wrapper, FL_SFS_BASE, sfs_buffer, FL_SFS_SIZE);

    ssdk_printf(SSDK_ALERT, "sfs dump\r\n");
    hexdump(sfs_buffer, FL_SFS_SIZE);
    sfs = (sfs_t *)sfs_buffer;

    crc_orig = sfs->crc32;
    crc_val = sfs_crc32(0, sfs_buffer, FL_SFS_SIZE - 4);

    if (crc_val != crc_orig) {
        ssdk_printf(SSDK_ALERT, "sfs crc_orig:0x%0x crc_val:0x%0x!\r\n",
                    crc_orig, crc_val);
        sfs = NULL;
        return -1;
    }

    ssdk_printf(SSDK_ALERT, "sfs tag = 0x%08x\r\n", sfs->tag);
    ssdk_printf(SSDK_ALERT, "sfs freq = 0x%02x\r\n", sfs->freq);
    ssdk_printf(SSDK_ALERT, "sfs sw_reset_info = 0x%02x\r\n",
                sfs->sw_reset_info);
    ssdk_printf(SSDK_ALERT, "sfs normal_img_base = 0x%08x\r\n",
                sfs->normal_img_base);
    ssdk_printf(SSDK_ALERT, "sfs backup_img_base = 0x%08x\r\n",
                sfs->backup_img_base);
    ssdk_printf(SSDK_ALERT, "sfs third_img_base = 0x%08x\r\n",
                sfs->third_img_base);
    ssdk_printf(SSDK_ALERT, "sfs crc32 = 0x%08x\r\n", sfs->crc32);

    return 0;
}