/** 
 * @file    nvm_ext.c
 * @brief   Non-volatile memory external module : W25Q16JV
 * @author  Kenny Tran
 * @date    Feb 29 2024
 * @version 0.0.1
 *
 * @section LICENSES
 * Copyright (C) Morgan Advanced Programmable Systems, Inc.
 */


/*--------------------------------------------------------------------------------------------------
 * INCLUDE FILES
 *------------------------------------------------------------------------------------------------*/
#include <stdint.h>
#include <stdlib.h>
#include <string.h>

#include <zephyr/kernel.h>

#include "../lib/errno.h"
#include "../lib/print.h"

#include "../wrappers/spi.h"

#include "nvm_ext.h"


/*--------------------------------------------------------------------------------------------------
 * MACROS & CONSTANTS
 *------------------------------------------------------------------------------------------------*/
#define _NVM_EXT_MFG_ID                         (0xC8)          /* C8 for GD25Q1C           */
#define _NVM_EXT_DEV_ID                         (0x4015)
// #define _NVM_EXT_DEV_ID                         (0x4015)    /* 0x4015 for W25Q16JV-IQ/JQ
//                                                                0x7015 for W25Q16JV-IM/JM */
#define _NVM_EXT_JEDEC_ID                       ((_NVM_EXT_MFG_ID << 16) | _NVM_EXT_DEV_ID)

/**
 * @brief For use with reading NVM external status bit.
 * */
#define _NVM_EXT_STATUS_BUSY_MASK               (0x01)
#define _NVM_EXT_STATUS_BUSY                    (0x01)

/**
 * @brief Value after byte is erased.  Nature of a NOR NVM external chip.
 */
#define _NVM_EXT_ERASED_BYTE_VAL                (0xFF)

/**
 * @brief Macro used for splitting larger values in to byte-size pieces.
 */
#define _NVM_EXT_SPLIT_BYTE(x, offset)          (0xFF & (x >> (8 * offset)))


/*--------------------------------------------------------------------------------------------------
 * TYPE DEFINITIONS & ENUMERATIONS
 *------------------------------------------------------------------------------------------------*/
typedef struct _nvm_ext_reg
{
    uint32_t opcode;
    uint8_t  tx_len;        /* Note: Includes dummy bytes */
    uint8_t  rx_len;
} _nvm_ext_reg_s;

typedef struct _nvm_ext_regs
{
    _nvm_ext_reg_s wr_enable;
    _nvm_ext_reg_s sr_wr_enable;
    _nvm_ext_reg_s wr_disable;
    _nvm_ext_reg_s pwr_down_release;
    _nvm_ext_reg_s mfg_id;
    _nvm_ext_reg_s jedec_id;
    _nvm_ext_reg_s unique_id;
    _nvm_ext_reg_s rd_data;
    _nvm_ext_reg_s fast_rd_data;
    _nvm_ext_reg_s page_program;
    _nvm_ext_reg_s sector_erase_4k;
    _nvm_ext_reg_s block_erase_32k;
    _nvm_ext_reg_s block_erase_64k;
    _nvm_ext_reg_s chip_erase;
    _nvm_ext_reg_s rd_status_1;
    _nvm_ext_reg_s wr_status_1;
    _nvm_ext_reg_s rd_status_2;
    _nvm_ext_reg_s wr_status_2;
    _nvm_ext_reg_s rd_status_3;
    _nvm_ext_reg_s wr_status_3;
    _nvm_ext_reg_s sfdp;
    _nvm_ext_reg_s erase_security;
    _nvm_ext_reg_s program_security;
    _nvm_ext_reg_s rd_security;
    _nvm_ext_reg_s gbl_block_lock;
    _nvm_ext_reg_s gbl_block_unlock;
    _nvm_ext_reg_s rd_block_lock;
    _nvm_ext_reg_s individual_block_lock;
    _nvm_ext_reg_s individual_block_unlock;
    _nvm_ext_reg_s erase_suspend;
    _nvm_ext_reg_s erase_resume;
    _nvm_ext_reg_s pwr_down;
    _nvm_ext_reg_s rst_enable;
    _nvm_ext_reg_s rst_device;
} _nvm_ext_regs_s;


/*--------------------------------------------------------------------------------------------------
 * PRIVATE CONSTANTS & VARIABLES
 *------------------------------------------------------------------------------------------------*/
static       bool            _nvm_ext_init      = false;
static const _nvm_ext_regs_s _nvm_ext_reg_table = {
    .wr_enable               = (_nvm_ext_reg_s) { .opcode = 0x06, .tx_len = 1, .rx_len = 0 },
    .sr_wr_enable            = (_nvm_ext_reg_s) { .opcode = 0x50, .tx_len = 1, .rx_len = 0 },
    .wr_disable              = (_nvm_ext_reg_s) { .opcode = 0x04, .tx_len = 1, .rx_len = 0 },
    .pwr_down_release        = (_nvm_ext_reg_s) { .opcode = 0xAB, .tx_len = 4, .rx_len = 1 },
    .mfg_id                  = (_nvm_ext_reg_s) { .opcode = 0x90, .tx_len = 4, .rx_len = 2 },
    .jedec_id                = (_nvm_ext_reg_s) { .opcode = 0x9F, .tx_len = 1, .rx_len = 3 },
    .unique_id               = (_nvm_ext_reg_s) { .opcode = 0x4B, .tx_len = 5, .rx_len = 1 },
    .rd_data                 = (_nvm_ext_reg_s) { .opcode = 0x03, .tx_len = 4, .rx_len = 1 },
    .fast_rd_data            = (_nvm_ext_reg_s) { .opcode = 0x0B, .tx_len = 5, .rx_len = 1 },
    .page_program            = (_nvm_ext_reg_s) { .opcode = 0x02, .tx_len = 4, .rx_len = 0 },
    .sector_erase_4k         = (_nvm_ext_reg_s) { .opcode = 0x20, .tx_len = 4, .rx_len = 0 },
    .block_erase_32k         = (_nvm_ext_reg_s) { .opcode = 0x52, .tx_len = 4, .rx_len = 0 },
    .block_erase_64k         = (_nvm_ext_reg_s) { .opcode = 0xD8, .tx_len = 4, .rx_len = 0 },
    .chip_erase              = (_nvm_ext_reg_s) { .opcode = 0x60, .tx_len = 1, .rx_len = 0 },
    .rd_status_1             = (_nvm_ext_reg_s) { .opcode = 0x05, .tx_len = 1, .rx_len = 1 },
    .wr_status_1             = (_nvm_ext_reg_s) { .opcode = 0x01, .tx_len = 2, .rx_len = 0 },
    .rd_status_2             = (_nvm_ext_reg_s) { .opcode = 0x35, .tx_len = 1, .rx_len = 1 },
    .wr_status_2             = (_nvm_ext_reg_s) { .opcode = 0x31, .tx_len = 2, .rx_len = 0 },
    .rd_status_3             = (_nvm_ext_reg_s) { .opcode = 0x15, .tx_len = 1, .rx_len = 1 },
    .wr_status_3             = (_nvm_ext_reg_s) { .opcode = 0x11, .tx_len = 2, .rx_len = 0 },
    .sfdp                    = (_nvm_ext_reg_s) { .opcode = 0x5A, .tx_len = 5, .rx_len = 1 },
    .erase_security          = (_nvm_ext_reg_s) { .opcode = 0x44, .tx_len = 4, .rx_len = 0 },
    .program_security        = (_nvm_ext_reg_s) { .opcode = 0x42, .tx_len = 6, .rx_len = 0 },
    .rd_security             = (_nvm_ext_reg_s) { .opcode = 0x48, .tx_len = 5, .rx_len = 1 },
    .gbl_block_lock          = (_nvm_ext_reg_s) { .opcode = 0x7E, .tx_len = 1, .rx_len = 0 },
    .gbl_block_unlock        = (_nvm_ext_reg_s) { .opcode = 0x98, .tx_len = 1, .rx_len = 0 },
    .rd_block_lock           = (_nvm_ext_reg_s) { .opcode = 0x3D, .tx_len = 4, .rx_len = 1 },
    .individual_block_lock   = (_nvm_ext_reg_s) { .opcode = 0x36, .tx_len = 4, .rx_len = 0 },
    .individual_block_unlock = (_nvm_ext_reg_s) { .opcode = 0x39, .tx_len = 4, .rx_len = 0 },    
    .erase_suspend           = (_nvm_ext_reg_s) { .opcode = 0x75, .tx_len = 1, .rx_len = 0 },
    .erase_resume            = (_nvm_ext_reg_s) { .opcode = 0x7A, .tx_len = 1, .rx_len = 0 },
    .pwr_down                = (_nvm_ext_reg_s) { .opcode = 0xB9, .tx_len = 1, .rx_len = 0 },
    .rst_enable              = (_nvm_ext_reg_s) { .opcode = 0x66, .tx_len = 1, .rx_len = 0 },
    .rst_device              = (_nvm_ext_reg_s) { .opcode = 0x99, .tx_len = 1, .rx_len = 0 }
};


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION PROTOTYPES
 *------------------------------------------------------------------------------------------------*/
static uint32_t _nvm_ext_jedec_id_get (void);
static void _nvm_ext_write_enable (void);
static void _nvm_ext_write_disable (void);
static void _nvm_wait_til_ready (void);


/*--------------------------------------------------------------------------------------------------
 * PUBLIC FUNCTION DEFINITIOPNS
 *------------------------------------------------------------------------------------------------*/
ERRNO_e NVM_EXT_init (void)
{
    ERRNO_e err = ERRNO_FAILURE;

    if (_nvm_ext_init)
    {
        return ERRNO_ALREADY_INITIALIZED;
    }

    if (SPI_init () != ERRNO_SUCCESS)
    {
        return ERRNO_SPI_FAILURE;
    }

    /* SPI can sometimes read back all 0xFFs if transferring right after init.
       Delay to work around. */
    k_busy_wait(10000);

    /* Set init flag so we only execute once. */
    /* NOTE : should be set before calling any NVM_EXT public functions since they will check for this flag. */
    _nvm_ext_init = true;

    /* Release power down. */
    err = NVM_EXT_release_power_down ();
    if (err != ERRNO_SUCCESS)
    {
        return err;
    }
    
    /* Check JEDEC ID to verify flash chip. */
    if (_nvm_ext_jedec_id_get () != (uint32_t) _NVM_EXT_JEDEC_ID)
    {
        return ERRNO_ID_FAILURE;
    }

    /* Disable write until we are ready to use. */
    _nvm_ext_write_disable ();

    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_power_down (void)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.pwr_down;
    uint8_t        tx_buf[reg.tx_len];

    if (!_nvm_ext_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    tx_buf[0] = reg.opcode;

    /* Write power down register. */
    if (SPI_xfer (tx_buf, sizeof (tx_buf), NULL, 0) != ERRNO_SUCCESS)
    {
        return ERRNO_SPI_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_release_power_down (void)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.pwr_down_release;
    uint8_t        tx_buf[reg.tx_len];
    uint8_t        rx_buf[reg.rx_len];

    if (!_nvm_ext_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    tx_buf[0] = reg.opcode;

    /* Write release power down register.  ID returned, check optional. */
    if (SPI_xfer (tx_buf, sizeof (tx_buf), rx_buf, sizeof (rx_buf)) != ERRNO_SUCCESS)
    {
        return ERRNO_SPI_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_erase_all (void)
{
    _nvm_ext_reg_s  reg = _nvm_ext_reg_table.chip_erase;
    uint8_t         tx_buf[reg.tx_len];

    if (!_nvm_ext_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }

    /* Write enable command must set before performing an erase. */
    _nvm_ext_write_enable ();

    tx_buf[0] = reg.opcode;

    /* Write chip erase register. */
    if (SPI_xfer (tx_buf, sizeof (tx_buf), NULL, 0) != ERRNO_SUCCESS)
    {
        return ERRNO_SPI_FAILURE;
    }

    /* Wait for erase to finish. */
    _nvm_wait_til_ready ();

    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_erase_block (uint32_t addr, NVM_EXT_block_size_e block_size)
{
    _nvm_ext_reg_s  reg = _nvm_ext_reg_table.sector_erase_4k;
    uint8_t         tx_buf[reg.tx_len];

    if (!_nvm_ext_init)
    {
        return ERRNO_NOT_INITIALIZED;
    }
    /* Perform checks on input parameters. */
    else if (addr >= NVM_EXT_SIZE)
    {
        return ERRNO_INVALID_PARAM;
    }

    /* Write enable command must set before performing an erase. */
    _nvm_ext_write_enable ();
    
    /* Split up address into bytes. */
    tx_buf[1] = _NVM_EXT_SPLIT_BYTE (addr, 2);      /* A23 - A16 */
    tx_buf[2] = _NVM_EXT_SPLIT_BYTE (addr, 1);      /* A15 -  A8 */
    tx_buf[3] = _NVM_EXT_SPLIT_BYTE (addr, 0);      /*  A7 -  A0 */

    /* Perform a parameter check and set correct opcode. */
    switch (block_size)
    {
        case NVM_EXT_BLOCK_SIZE_4KB:
            tx_buf[0] = _nvm_ext_reg_table.sector_erase_4k.opcode;
            break;
        
        case NVM_EXT_BLOCK_SIZE_32KB:
            tx_buf[0] = _nvm_ext_reg_table.block_erase_32k.opcode;
            break;
        
        case NVM_EXT_BLOCK_SIZE_64KB:
            tx_buf[0] = _nvm_ext_reg_table.block_erase_64k.opcode;
            break;
        
        default:
            return ERRNO_INVALID_PARAM;
    }

    /* Write block / sector erase register. */
    if (SPI_xfer (tx_buf, sizeof (tx_buf), NULL, 0) != ERRNO_SUCCESS)
    {
        return ERRNO_SPI_FAILURE;
    }

    /* Wait for erase to finish and measure elapsed time. */
    #ifdef CONFIG_DBG_STATS 
    uint32_t t_start = k_uptime_get_32();
    #endif
    _nvm_wait_til_ready ();
    #ifdef CONFIG_DBG_STATS
    uint32_t elapsed = k_uptime_get_32() - t_start;
    PRINT(INFO, "NVM_EXT_erase_block: addr=0x%06X size=%uKB took %u ms\r\n",
          addr, (block_size == NVM_EXT_BLOCK_SIZE_4KB) ? 4 : (block_size == NVM_EXT_BLOCK_SIZE_32KB) ? 32 : 64, elapsed);
    #endif
    
    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_erase_range (uint32_t start_addr, uint32_t end_addr)
{
    /* To keep things simple, range erasing will use always use a block size of 4 Kb. */
    ERRNO_e  err;
    uint32_t i;
    uint8_t  data_start_buf[NVM_EXT_BLOCK_SIZE_4KB - 1];
    uint8_t  data_end_buf  [NVM_EXT_BLOCK_SIZE_4KB - 1];
    uint16_t block_start     =  start_addr / NVM_EXT_BLOCK_SIZE_4KB;
    uint16_t block_end       =    end_addr / NVM_EXT_BLOCK_SIZE_4KB;
    uint16_t data_start_addr = block_start * NVM_EXT_BLOCK_SIZE_4KB;
    uint16_t data_end_addr   =    end_addr + 1;
    uint16_t data_start_len  =  start_addr & (NVM_EXT_BLOCK_SIZE_4KB - 1);
    uint16_t data_end_len    = (~(end_addr % NVM_EXT_BLOCK_SIZE_4KB)) & (NVM_EXT_BLOCK_SIZE_4KB - 1);
    
    /* Perform checks on input parameters. */
    if ((start_addr >= NVM_EXT_SIZE) || (end_addr >= NVM_EXT_SIZE) || (start_addr > end_addr))
    {
        return ERRNO_INVALID_PARAM;
    }
    
    if (data_start_len != 0)
    {
        /* Read and save starting data in block section that is out of erase range. */
        err = NVM_EXT_read (data_start_addr, data_start_buf, data_start_len);
        if (err != ERRNO_SUCCESS)
        {
            return err;
        }
    }
    
    if (data_end_len != 0)
    {
        /* Read and save ending data in block section that is out of erase range. */
        err = NVM_EXT_read (data_end_addr, data_end_buf, data_end_len);
        if (err != ERRNO_SUCCESS)
        {
            return err;
        }
    }
    
    /* Loop through blocks and perform erases. */
    for (i = block_start; i <= block_end; i++)
    {
        err = NVM_EXT_erase_block (i * NVM_EXT_BLOCK_SIZE_4KB, NVM_EXT_BLOCK_SIZE_4KB);
        if (err != ERRNO_SUCCESS)
        {
            return err;
        }
    }
    
    /* Rewrite back starting data our of the desired erase range that got erased. */
    if (data_start_len != 0)
    {
        err = NVM_EXT_write (data_start_addr, data_start_buf, data_start_len);
        if (err != ERRNO_SUCCESS)
        {
            return err;
        }
    }
    
    /* Rewrite back ending data our of the desired erase range that got erased. */
    if (data_end_len != 0)
    {
        err = NVM_EXT_write (data_end_addr, data_end_buf, data_end_len);
        if (err != ERRNO_SUCCESS)
        {
            return err;
        }
    }
    
    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_read (uint32_t addr, uint8_t *p_dest, uint32_t len)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.rd_data;
    uint8_t        tx_buf[reg.tx_len];

    /* Perform checks on input parameter. */
    if ((addr >= NVM_EXT_SIZE) || (len == 0) || (addr + len) > NVM_EXT_SIZE)
    {
        return ERRNO_INVALID_PARAM;
    }
    else if (p_dest == NULL)
    {
        return ERRNO_BAD_POINTER;
    }
    
    /* Split up address into len. */
    tx_buf[0] = reg.opcode;
    tx_buf[1] = _NVM_EXT_SPLIT_BYTE (addr, 2);      /* A23 - A16 */
    tx_buf[2] = _NVM_EXT_SPLIT_BYTE (addr, 1);      /* A15 -  A8 */
    tx_buf[3] = _NVM_EXT_SPLIT_BYTE (addr, 0);      /*  A7 -  A0 */

    /* Perform read. */
    if (SPI_xfer (tx_buf, sizeof (tx_buf), p_dest, len) != ERRNO_SUCCESS)
    {
        return ERRNO_SPI_FAILURE;
    }

    return ERRNO_SUCCESS;
}

ERRNO_e NVM_EXT_write (uint32_t addr, uint8_t *p_src, uint32_t len)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.page_program;
    uint8_t        tx_buf[reg.tx_len + len];
    
    /* The calculated number of len to write */
    uint32_t cnt;

    /* Perform checks on input parameter. */
    if (addr >= NVM_EXT_SIZE || len == 0 || (addr + len) > NVM_EXT_SIZE)
    {
        return ERRNO_INVALID_PARAM;
    }
    
    if (p_src == NULL)
    {
        return ERRNO_BAD_POINTER;
    }

    while (len > 0)
    {
        /* Calculate the number of bytes to write (bytes until end of page) */
        cnt = NVM_EXT_PAGE_SIZE - (addr % NVM_EXT_PAGE_SIZE);
        
        /* Do not span pages if the data will fit in the remaining page space. */
        if (cnt >= len)
        {
            cnt = len;
        }
        
        /* Write enable command must set before performing a write. */
        _nvm_ext_write_enable ();

        tx_buf[0] = reg.opcode;
        tx_buf[1] = _NVM_EXT_SPLIT_BYTE (addr, 2);      /* A23 - A16 */
        tx_buf[2] = _NVM_EXT_SPLIT_BYTE (addr, 1);      /* A15 -  A8 */
        tx_buf[3] = _NVM_EXT_SPLIT_BYTE (addr, 0);      /*  A7 -  A0 */
        
        /* Copy data buffer into tx_buffer. */
        memcpy (&tx_buf[reg.tx_len], p_src, cnt);
        
        /* Perform write. */
        if (SPI_xfer (tx_buf, (reg.tx_len + cnt), NULL, 0) != ERRNO_SUCCESS)
        {
            return ERRNO_SPI_FAILURE;
        }
        
        /* Decrement byte count, increment address, and increment source pointer. */
        len   -= cnt;
        addr  += cnt;
        p_src += cnt;
        
        /* Wait for write to complete. */
        _nvm_wait_til_ready ();
    }

    return ERRNO_SUCCESS;
}

void NVM_EXT_cli_cmd (int32_t argc, char **argv)
{
	uint16_t i;
	uint8_t  buf[4096];
	uint32_t start_addr;
	uint8_t  data;  
	uint16_t n_bytes;
    ERRNO_e  err;
	
    if (argc == 0)
    {
        PRINT (INFO, "NVM EXTERNAL\r\n");
        PRINT (INFO, "-------------------------------------------\r\n");
        PRINT (INFO, "JEDEC ID     : 0x%06X\r\n", _nvm_ext_jedec_id_get ());
        PRINT (INFO, "Size (bytes) : %d\r\n", NVM_EXT_SIZE);
    }
	else if (argc == 1)
	{
        if (!strcmp (argv[0], "--help") || !strcmp (argv[0], "?"))
        {
            goto USAGE;
        }
        else
        {
            start_addr = (uint32_t) strtoul (argv[0], NULL, 0);
            
            err = NVM_EXT_read (start_addr, buf, 1);
            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err == ERRNO_SUCCESS)
            {
                PRINT (INFO, "Addr: 0x%06X = %d\r\n", start_addr, buf[0]);
            }
        }
	}
    else if (argc == 2)
    {
        if (!strcmp (argv[1], "--erase") || !strcmp (argv[1], "-e"))
        {
            if (!strcmp (argv[0], "all"))
            {
                err = NVM_EXT_erase_all ();
                PRINT (INFO, "Ret code : %d\r\n", err);
            }
            else
            {
                goto USAGE;
            }
        }
        else
        {
            goto USAGE;
        }
    }
	else if (argc == 3)
	{
        if (!strcmp (argv[1], "--read") || !strcmp (argv[1], "-r"))
        {
            start_addr = (uint32_t) strtoul (argv[0], NULL, 0);
            n_bytes    = (uint16_t) strtoul (argv[2], NULL, 0);
            err        = NVM_EXT_read (start_addr, buf, n_bytes);

            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err == ERRNO_SUCCESS)
            {
                for (i = 0; i < n_bytes; i++)
                {
                    PRINT (INFO, "Addr: 0x%06X = %d\r\n", (start_addr + i), buf[i]);
                }
            }
        }
        else if (!strcmp (argv[1], "--set") || !strcmp (argv[1], "-s"))
        {
            start_addr = (uint32_t) strtoul (argv[0], NULL, 0);
            data       = (uint8_t)  strtoul (argv[2], NULL, 0);
            err        = NVM_EXT_write (start_addr, &data, 1);

            PRINT (INFO, "Ret code : %d\r\n", err);
        }
        else if (!strcmp (argv[1], "--erase") || !strcmp (argv[1], "-e"))
        {
            uint32_t end_addr;
		
            start_addr = (uint32_t) strtoul (argv[0], NULL, 0);
            end_addr   = (uint32_t) strtoul (argv[2], NULL, 0) + start_addr;
            err        = NVM_EXT_erase_range (start_addr, end_addr);

            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err == ERRNO_SUCCESS)
            {
                PRINT (INFO, "Addr: 0x%06X - 0x%06X erased\r\n", start_addr, end_addr);
            }
        }
        else if (!strcmp (argv[1], "-type") || !strcmp (argv[1], "-t"))
        {
            start_addr = (uint32_t) strtoul (argv[0], NULL, 0);
            err        = NVM_EXT_read (start_addr, buf, 4);

            PRINT (INFO, "Ret code : %d\r\n", err);

            if (err == ERRNO_SUCCESS)
            {
                if (!strcmp (argv[2], "u8"))
                {
                    PRINT (INFO, "%u\r\n", *(uint8_t *) buf);
                }
                else if (!strcmp (argv[2], "u16"))
                {
                    PRINT (INFO, "%u\r\n", *(uint16_t *) buf);
                }
                else if (!strcmp (argv[2], "u32"))
                {
                    PRINT (INFO, "%u\r\n", *(uint32_t *) buf);
                }
                else if (!strcmp (argv[2], "s8"))
                {
                    PRINT (INFO, "%d\r\n", *(int8_t *) buf);
                }
                else if (!strcmp (argv[2], "s16"))
                {
                    PRINT (INFO, "%d\r\n", *(int16_t *) buf);
                }
                else if (!strcmp (argv[2], "s32"))
                {
                    PRINT (INFO, "%d\r\n", *(int32_t *) buf);
                }
                else if (!strcmp (argv[2], "float"))
                {
                    PRINT (INFO, "%f\r\n", *(float *) buf);
                }
                else
                {
                    PRINT (INFO, "Invalid argument\r\n");
                }
            }
        }
        else
        {
            goto USAGE;
        }
	}
	else if (argc == 4)
	{
		if (!strcmp (argv[1], "--set") || !strcmp (argv[1], "-s"))
        {
            start_addr = (uint32_t) strtoul (argv[0], NULL, 0);
            data       = (uint8_t)  strtoul (argv[2], NULL, 0);
            n_bytes    = (uint32_t) strtoul (argv[3], NULL, 0);
            
            memset (buf, data, n_bytes);

            err = NVM_EXT_write (start_addr, buf, n_bytes);
            PRINT (INFO, "Ret code : %d\r\n", err);
        }
        else
        {
            goto USAGE;
        }
	}
	else
	{
USAGE:
		PRINT (INFO, "Usage: nvm <addr | ""all""> [OPTION...]\r\n");
		PRINT (INFO, "  -r | --read      reads from flash\r\n");
		PRINT (INFO, "    <%%d>           number of bytes [1-255]\r\n");
		PRINT (INFO, "  -s |--set        write value to flash\r\n");
		PRINT (INFO, "    <%%d>           value to set\r\n");
		PRINT (INFO, "  -e | --erase     erase flash contents\r\n");
		PRINT (INFO, "    <%%d>           bytes to erase\r\n");
		PRINT (INFO, "  -t | --type      typecast flash content\r\n");
		PRINT (INFO, "    <%%s>           [u8, u16, u32, s8, s16, s32, float]\r\n");
	}
}


/*--------------------------------------------------------------------------------------------------
 * PRIVATE FUNCTION DEFINITIONS
 *------------------------------------------------------------------------------------------------*/
static uint32_t _nvm_ext_jedec_id_get (void)
{
    uint32_t       jedec_id = 0;
    _nvm_ext_reg_s reg      = _nvm_ext_reg_table.jedec_id;
    uint8_t        tx_buf[reg.tx_len];
    uint8_t        rx_buf[reg.rx_len];

    tx_buf[0] = reg.opcode;

    /* Read JEDEC ID register. */
    if (SPI_xfer (tx_buf, sizeof (tx_buf), rx_buf, sizeof (rx_buf)) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "SPI failure reading JEDEC ID.\r\n");
    }

    jedec_id = (((uint32_t) rx_buf[0] << 16) | ((uint32_t) rx_buf[1] << 8) | (uint32_t) rx_buf[2]);

    return jedec_id;
}

static void _nvm_ext_write_enable (void)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.wr_enable;
    uint8_t        tx_buf[reg.tx_len];

    tx_buf[0] = reg.opcode;

    if (SPI_xfer (tx_buf, sizeof (tx_buf), NULL, 0) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "SPI failure transferring write enable.\r\n");
    }
}

static void _nvm_ext_write_disable (void)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.wr_disable;
    uint8_t        tx_buf[reg.tx_len];

    tx_buf[0] = reg.opcode;

    if (SPI_xfer (tx_buf, sizeof (tx_buf), NULL, 0) != ERRNO_SUCCESS)
    {
        PRINT (ERROR, "SPI failure transferring write enable.\r\n");
    }
}

static void _nvm_wait_til_ready (void)
{
    _nvm_ext_reg_s reg = _nvm_ext_reg_table.rd_status_1;
    uint8_t        tx_buf[reg.tx_len];
    uint8_t        rx_buf[reg.rx_len];

    tx_buf[0] = reg.opcode;

    /* Wait for erase to complete by polling READYn/BUSY bit. */
    do
    {
        if (SPI_xfer (tx_buf, sizeof (tx_buf), rx_buf, sizeof (rx_buf)) != ERRNO_SUCCESS)
        {
            PRINT (ERROR, "SPI failure polling ready bit.\r\n");
        }
    } while ((rx_buf[0] & _NVM_EXT_STATUS_BUSY_MASK) == _NVM_EXT_STATUS_BUSY);
}
