#include "flash_dev_sst26.h"

#define SST26_INVALID_BIT_NUM 0xFF

typedef enum {
    /* Config */
    SST26_CMD_NOP       = 0,
    SST26_CMD_RSTEN     = 0x66, /* Reset Enable */
    SST26_CMD_RST       = 0x99, /* Reset Memory */
    SST26_CMD_EQUI      = 0x38, /* Enable Quad IO */
    SST26_CMD_RSTQIO    = 0xFF, /* Reset Quad IO */
    SST26_CMD_RDSR      = 0x05, /* Read status register */
    SST26_CMD_WRSR      = 0x01, /* Write status register */
    SST26_CMD_RDCR      = 0x35, /* Read configuration register */

    /* Read */
    SST26_CMD_READ              = 0x03, /* Read memory 40 MHz */
    SST26_CMD_FAST_READ         = 0x0B, /* Read memory 104/80 MHz */
    SST26_CMD_SQOR              = 0x6B, /* SPI Quad Output Read */
    SST26_CMD_SQIOR             = 0xEB, /* SPI Quad IO Read */
    SST26_CMD_SDOR              = 0x3B, /* SPI Dual Output Read */
    SST26_CMD_SDIOR             = 0xBB, /* SPI Dual IO Read */
    SST26_CMD_SB                = 0xC0, /* Set Burst Length */
    SST26_CMD_RBSQI             = 0x0C, /* SQI Read Burst with Wrap */
    SST26_CMD_RBSPI             = 0xEC, /* SPI Read Burst with Wrap */
    /* ID */
    SST26_CMD_JEDEC_ID          = 0x9F,
    SST26_CMD_QUAD_J_ID         = 0xAF,
    SST26_SFDP                  = 0x5A, /* Serial Flash Discoverable Parameters */
    /* Write */
    SST26_CMD_WREN              = 0x06, /* Write Enable */
    SST26_CMD_WRDI              = 0x04, /* Write Disable */
    SST26_CMD_SE                = 0x20, /* Erase 4K Array */
    SST26_CMD_BE                = 0xD8, /* Erase 64,32 or 8K block */
    SST26_CMD_CE                = 0xC7, /* Erase Chip */
    SST26_CMD_PP                = 0x02, /* Page Program */
    SST26_CMD_SQPP              = 0x32, /* SQI Quad Page Program */
    SST26_CMD_WRSU              = 0xB0, /* Suspends Program/Erase */
    SST26_CMD_WRRE              = 0x30, /* Resumes Program/Erase */
    /* Protection */
    SST26_CMD_RBPR              = 0x72, /* Read Block-Protection Register*/
    SST26_CMD_WBPR              = 0x42, /* Write Block-Protection Register*/
    SST26_CMD_LBPR              = 0x8D, /* Lock Down Block-Protection Register*/
    SST26_CMD_NVWLDR            = 0xE8, /* non-Volatile Write Lock-Down Register*/
    SST26_CMD_ULBPR             = 0x98, /* Global Block Protection Unlock */
    SST26_CMD_RSID              = 0x88, /* Read Security ID */
    SST26_CMD_PSID              = 0xA5, /* Program User Security ID area */
    SST26_CMD_LSID              = 0x85, /* Lockout Security ID Proamming */
    /* Power Saving */
    SST26_CMD_DPD               = 0xB9, /* Deep Power-down Mode */
    SST26_CMD_RDPD              = 0xAB, /* Release from Deep Power-down Mode */
} t_sst26_cmds;


typedef struct {
    unsigned start_addr;
    unsigned char bpr_wr_bit;
    unsigned char bpr_rd_bit;
    const t_flash_erase_cmd *erase_cmd;
} t_sst26_page;



static const t_flash_erase_cmd f_erase_block8_cmd =  {
    SST26_CMD_BE,              SST26_BLOCKL_SIZE,  25, 0
};
static const t_flash_erase_cmd f_erase_block32_cmd =  {
    SST26_CMD_BE,              SST26_BLOCKM_SIZE,  25, 0
};

static const t_flash_erase_cmd f_erase_block64_cmd =  {
    SST26_CMD_BE,    SST26_BLOCKH_SIZE,  25, 0
};

static const t_flash_erase_cmd f_erase_sector_cmd =  {
    SST26_CMD_SE,    SST26_SECTOR_SIZE,  25, 0
};

static const t_flash_erase_cmd f_erase_chip_cmd =  {
    SST26_CMD_CE,     SST26_FLASH_SIZE,  50, FLASH_ERASE_CMD_FLAG_NOADDR
};


const t_flash_info flash_info_sst26 =  {
    {SST26_ID_MANUFACTURER, SST26_ID_DEVICE, SST26_ID_CAPACITY, FLASH_ID_EDI_LEN_NOT_SUPPORTED},
    SST26_FLASH_SIZE,
    {SST26_CMD_RDSR, SST26_STATUS_BUSY, 0x0,0x0},
    {SST26_CMD_FAST_READ, 1},
    {NULL, SST26_CMD_WREN},
    {SST26_PAGE_SIZE, SST26_CMD_PP, 1500},
    {0},
    {0,0,0},
    {   NULL,
        flash_sst26_get_status,
        flash_sst26_read,
        flash_sst26_erase,
        flash_sst26_get_erase_cmd,
        flash_sst26_write,
        flash_sst26_write_enable,
        flash_sst26_lock_enable,
        flash_sst26_lock,
        flash_sst26_unlock,
        flash_sst26_lock_check,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL,
        NULL
    },
    NULL
};

static void sst26_get_page(unsigned addr, t_sst26_page *page) {
    if ((addr >= SST26_BLOCKH_SIZE) && (addr < (SST26_FLASH_SIZE - SST26_BLOCKH_SIZE))) {        
        page->erase_cmd = &f_erase_block64_cmd;
        page->bpr_wr_bit = addr / SST26_BLOCKH_SIZE - 1;
        page->bpr_rd_bit = SST26_INVALID_BIT_NUM;
    } else if ((addr >= SST26_BLOCKM_SIZE) && (addr < (SST26_FLASH_SIZE - SST26_BLOCKM_SIZE))) {
        page->erase_cmd = &f_erase_block32_cmd;
        page->bpr_wr_bit = addr < SST26_BLOCKH_SIZE ? 30 : 31;
        page->bpr_rd_bit = SST26_INVALID_BIT_NUM;
    } else if (addr < SST26_FLASH_SIZE) {
        int num = addr < SST26_BLOCKM_SIZE ? addr / SST26_BLOCKL_SIZE :
                                             (addr - (SST26_FLASH_SIZE - SST26_BLOCKM_SIZE))
                                             / SST26_BLOCKL_SIZE + 4;
        page->erase_cmd = &f_erase_block8_cmd;
        page->bpr_wr_bit = 32 + 2*num;
        page->bpr_rd_bit = 33 + 2*num;
    } else {
        page->start_addr = 0;
        page->erase_cmd = NULL;
        page->bpr_wr_bit = SST26_INVALID_BIT_NUM;
        page->bpr_rd_bit = SST26_INVALID_BIT_NUM;
    }

    if (page->erase_cmd) {
        page->start_addr = addr & ~(page->erase_cmd->size-1);
    }
}

t_flash_errs flash_sst26_set(t_flash_iface *iface) {
    return flash_set(iface, &flash_info_sst26, NULL);
}

const t_flash_erase_cmd *flash_sst26_get_erase_cmd(t_flash_iface *iface, unsigned addr, size_t size) {
    const t_flash_erase_cmd *ret = 0;
    if (flash_check_erase_cmd(&f_erase_chip_cmd, addr, size)) {
        ret = &f_erase_chip_cmd;
    } else {
        t_sst26_page page;
        sst26_get_page(addr, &page);
        if (page.erase_cmd && flash_check_erase_cmd(page.erase_cmd, addr, size)) {
            ret = page.erase_cmd;
        } else if (flash_check_erase_cmd(&f_erase_sector_cmd, addr, size)) {
            ret = &f_erase_sector_cmd;
        }
    }
    return ret;
}

t_flash_errs flash_sst26_get_status_ex(t_flash_iface *iface, unsigned short *status) {

    unsigned char status1, status2;
    t_flash_errs err = 0;
    if (iface->flash_info != &flash_info_sst26)
        err = FLASH_ERR_INVALID_DEVICE;

    if (!err)
        err = flash_get_status(iface, &status1);
    if (!err) {
        unsigned char cmd = SST26_CMD_RDCR;
        err = flash_exec_cmd(iface, &cmd, 1, NULL,0, &status2, 1, FLASH_FLAGS_FLUSH);
    }

    if (!err && (status != NULL))  {
        *status = (status2 << 8) | status1;
    }
    return err;
}

t_flash_errs flash_sst26_set_status_ex(t_flash_iface *iface, unsigned short status) {
    t_flash_errs err = 0;
    if (iface->flash_info != &flash_info_sst26)
        err = FLASH_ERR_INVALID_DEVICE;

    if (!err)
        err = flash_write_enable(iface);

    if (!err) {
        unsigned char cmd = SST26_CMD_WRSR;
        unsigned char status_bytes[2] = {status & 0xFF, (status >> 8) & 0xFF};
        err = flash_exec_cmd(iface, &cmd, 1, status_bytes, 2, NULL, 0, FLASH_FLAGS_FLUSH);
    }
    return err;
}

t_flash_errs flash_sst26_get_bpr(t_flash_iface *iface, t_sst26_bpr_value *bpr) {
    t_flash_errs err = 0;
    if (iface->flash_info != &flash_info_sst26)
        err = FLASH_ERR_INVALID_DEVICE;

    if (!err) {
        unsigned char cmd = SST26_CMD_RBPR;
        unsigned char bpr_bytes[6];
        err = flash_exec_cmd(iface, &cmd, 1, NULL, 0, bpr_bytes, sizeof(bpr_bytes), FLASH_FLAGS_FLUSH);
        if (!err) {
            size_t i;
            unsigned long long res = 0;
            for (i = 0; i < sizeof(bpr_bytes); i++) {
                res = (res << 8) | bpr_bytes[i];
            }
            if (bpr) {
                *bpr = res;
            }
        }
    }
    return err;

}

t_flash_errs flash_sst26_set_bpr(t_flash_iface *iface, t_sst26_bpr_value bpr) {
    t_flash_errs err = 0;
    if (iface->flash_info != &flash_info_sst26)
        err = FLASH_ERR_INVALID_DEVICE;

    if (!err)
        err = flash_write_enable(iface);
    if (!err) {
        unsigned char cmd = SST26_CMD_WBPR;
        unsigned char bpr_bytes[6];
        size_t i;
        for (i = 0; i < sizeof(bpr_bytes); i++) {
            bpr_bytes[sizeof(bpr_bytes) - i - 1] = bpr & 0xFF;
            bpr >>= 8;
        }

        err = flash_exec_cmd(iface, &cmd, 1, bpr_bytes, sizeof(bpr_bytes), NULL, 0,  FLASH_FLAGS_FLUSH);
    }
    return err;
}


t_sst26_bpr_value flash_sst26_bpr_bpr_diff_msk(unsigned addr, unsigned size, t_sst26_bpr_value bpr, int lock_en) {
    t_sst26_bpr_value change_msk = 0;
    unsigned check_addr = addr;
    while (check_addr < (addr + size)) {
        t_sst26_page page;
        sst26_get_page(check_addr, &page);
        t_sst26_bpr_value bit_msk = (t_sst26_bpr_value)1 << page.bpr_wr_bit;
        int locked = (bpr & bit_msk) != 0;
        if (locked != lock_en) {
            change_msk |= bit_msk;
        }
        check_addr = page.start_addr + page.erase_cmd->size;
    }
    return change_msk;
}

static t_flash_errs flash_sst26_change_lock(t_flash_iface *iface, unsigned addr, unsigned size, int locked) {
    t_flash_errs err;
    t_sst26_bpr_value bpr;
    err = flash_sst26_get_bpr(iface, &bpr);
    if (!err) {
        err = flash_sst26_lock_change_bpr(iface, addr, size, bpr, locked);
    }
    return err;
}


t_flash_errs flash_sst26_lock_enable(t_flash_iface *iface) {
    return FLASH_ERR_OK;
}

t_flash_errs flash_sst26_lock(t_flash_iface *iface, unsigned addr, unsigned size) {
    return flash_sst26_change_lock(iface, addr, size, 1);
}

t_flash_errs flash_sst26_unlock(t_flash_iface *iface, unsigned addr, unsigned size) {
    return flash_sst26_change_lock(iface, addr, size, 0);
}

int flash_sst26_check_locked_bpr(unsigned addr, unsigned size, t_sst26_bpr_value bpr, int locked) {
    return flash_sst26_bpr_bpr_diff_msk(addr, size, bpr, locked) == 0;
}

t_flash_errs flash_sst26_lock_check(t_flash_iface *iface, unsigned addr, unsigned size, int locked, int *check_ok) {
    t_flash_errs err;
    t_sst26_bpr_value bpr;
    err = flash_sst26_get_bpr(iface, &bpr);
    if (!err) {
        *check_ok = flash_sst26_check_locked_bpr(addr, size, bpr, locked);
    }
    return err;
}

t_flash_errs flash_sst26_lock_change_bpr(t_flash_iface *iface, unsigned addr, unsigned size, t_sst26_bpr_value bpr, int locked) {
    t_flash_errs err = FLASH_ERR_OK;
    t_sst26_bpr_value change_msk = flash_sst26_bpr_bpr_diff_msk(addr, size, bpr, locked);
    if (change_msk != 0) {
        bpr ^= change_msk;
        err = flash_sst26_set_bpr(iface, bpr);
        if (!err) {
            t_sst26_bpr_value rd_val;
            err = flash_sst26_get_bpr(iface, &rd_val);
            if (!err) {
                if (rd_val != bpr)
                    err = FLASH_ERR_OP_FAILED;
            }
        }
    }
    return err;
}
