#include <FlashOS.h>
#include <compiler.h>
#include <debug.h>
#include <part.h>
#include <stdint.h>

#if CONFIG_E3L == 1
#define CMD_ARGS_ADDRESS 0x4FDFE0
#else
#define CMD_ARGS_ADDRESS 0x3FDFE0
#endif
#define CMD_BUF_ADDRESS (CMD_ARGS_ADDRESS + 0x20)

#define CMD_PROGRAM (1)
#define CMD_BLOCK_ERASE (2)
#define CMD_CHIP_ERASE (5)

#define NO_ERROR (0x0)
#define PROGRAM_ERROR (0x100)
#define SECTOR_REASE_ERROR (0x101)
#define BULK_ERASE_ERROR (0x102)
#define LOCK_ERROR (0x103)
#define UNLOCK_ERROR (0x104)
#define NOT_IMPLEMENTED_ERROR (0x141)
#define FLASH_INIT_ERROR (0x142)

typedef struct trace32_cmd_args {
    uint32_t flash_base_address;
    uint32_t reserved;
    uint32_t data_bus_width;
    uint32_t offset;
    uint32_t address;
    uint32_t size;
    uint32_t timing;
    uint32_t command;
} __PACKED trace32_cmd_args_t;

typedef struct trace32_return_args {
    uint32_t not_valid_1;
    uint32_t reserved_1;
    uint32_t not_valid_2;
    uint32_t reserved_2;
    uint32_t address;
    uint32_t not_valid_3;
    uint32_t reserved_3;
    uint32_t status;
} __PACKED trace32_return_args_t;

void t32_flashloader_end(void);
void t32_flashloader_begin(void);

__ARM __USED __SECTION(".T32CODEBEGIN") void t32_flashloader_begin(void)
{
    t32_flashloader_end();
}

__USED __SECTION(".T32CODEEND") void t32_flashloader_end(void)
{
    struct trace32_cmd_args *pCmdArgs =
        (struct trace32_cmd_args *)CMD_ARGS_ADDRESS;
    struct trace32_return_args *pRetArgs =
        (struct trace32_return_args *)CMD_ARGS_ADDRESS;
    int ret;

    ret = Init(NULL, NULL, NULL);
    if (ret != 0) {
        pRetArgs->status = FLASH_INIT_ERROR;
        return;
    }

    if (pCmdArgs->command == CMD_PROGRAM) {
        ret = SEGGER_OPEN_Program(pCmdArgs->address, pCmdArgs->size,
                                  (unsigned char *)CMD_BUF_ADDRESS);
        if (ret != 0) {
            pRetArgs->status = PROGRAM_ERROR;
            return;
        }
    } else if (pCmdArgs->command == CMD_BLOCK_ERASE) {
        if (pCmdArgs->address % jflash_wrapper->sector_size != 0) {
            ssdk_printf(SSDK_NOTICE,
                        "Erase address must be aligned to the sector size\r\n");
            pRetArgs->status = SECTOR_REASE_ERROR;
            return;
        } else if (pCmdArgs->size != jflash_wrapper->sector_size) {
            ssdk_printf(SSDK_NOTICE,
                        "Erase size must be aligned to the sector size\r\n");
            pRetArgs->status = SECTOR_REASE_ERROR;
            return;
        } else {
            ret = SEGGER_OPEN_Erase(pCmdArgs->address, NULL, 1);
            if (ret != 0) {
                pRetArgs->status = SECTOR_REASE_ERROR;
                return;
            }
        }
    } else if (pCmdArgs->command == CMD_CHIP_ERASE) {
        ret = EraseChip();
        if (ret != 0) {
            pRetArgs->status = BULK_ERASE_ERROR;
            return;
        }
    } else {
        pRetArgs->status = NOT_IMPLEMENTED_ERROR;
        ssdk_printf(SSDK_NOTICE, "Not implemented command\r\n");
        return;
    }
    pRetArgs->status = NO_ERROR;
    return;
}