//==========================================================================
//
//      mxc_nfc.c
//
//      Flash programming to support NAND flash on Freescale MXC platforms
//
//==========================================================================
//####ECOSGPLCOPYRIGHTBEGIN####
// -------------------------------------------
// This file is part of eCos, the Embedded Configurable Operating System.
// Copyright (C) 1998, 1999, 2000, 2001, 2002 Red Hat, Inc.
//
// eCos is free software; you can redistribute it and/or modify it under
// the terms of the GNU General Public License as published by the Free
// Software Foundation; either version 2 or (at your option) any later version.
//
// eCos is distributed in the hope that it will be useful, but WITHOUT ANY
// WARRANTY; without even the implied warranty of MERCHANTABILITY or
// FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
// for more details.
//
// You should have received a copy of the GNU General Public License along
// with eCos; if not, write to the Free Software Foundation, Inc.,
// 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
//
// As a special exception, if other files instantiate templates or use macros
// or inline functions from this file, or you compile this file and link it
// with other works to produce a work based on this file, this file does not
// by itself cause the resulting work to be covered by the GNU General Public
// License. However the source code for this file must still be made available
// in accordance with section (3) of the GNU General Public License.
//
// This exception does not invalidate any other reasons why a work based on
// this file might be covered by the GNU General Public License.
//
// Alternative licenses for eCos may be arranged by contacting Red Hat, Inc.
// at http://sources.redhat.com/ecos/ecos-license/
// -------------------------------------------
//####ECOSGPLCOPYRIGHTEND####
//==========================================================================
//#####DESCRIPTIONBEGIN####
//
// Author(s):    Kevin Zhang <k.zhang@freescale.com>
// Contributors: Kevin Zhang <k.zhang@freescale.com>
// Date:         2006-01-23
// Purpose:
// Description:
//
//####DESCRIPTIONEND####
//
//==========================================================================

#include <pkgconf/hal.h>
#include <cyg/hal/hal_arch.h>
#include <cyg/hal/hal_cache.h>
#include <redboot.h>
#include <stdlib.h>

#include CYGHWR_MEMORY_LAYOUT_H

#include <cyg/hal/hal_io.h>
#define  _FLASH_PRIVATE_
#include <cyg/io/flash.h>

#ifdef CYGPKG_HAL_ARM_MXC30031ADS
#include <cyg/io/mxc_nfc_v2.h>
#else
#include <cyg/io/mxc_nfc.h>
#endif

#ifdef MXCFLASH_SELECT_NAND_LBA
#include <cyg/io/mxc_nfc_lba.h>
#endif /* MXCFLASH_SELECT_NAND_LBA */

#define MXC_UNLOCK_BLK_END      0xFFFF

#ifndef MXCFLASH_SELECT_NAND_LBA
static void nfc_flash_reset(void);
static void read_nflash_id(void* id);
#endif /* MXCFLASH_SELECT_NAND_LBA */
static void print_pkt_16(u16* pkt, u32 len);
static void print_page (u32 addr, bool spare_only);
static u32 nfc_l_to_p(u32 la);
static int nfc_read_page(u32 addr);
static int nfc_read_page_sp(u32 addr);
static int nfc_program_page(u32 flash_addr, u32 mem_addr, enum nfc_page_area area);
static void nfc_update_blk_table(u32 bad_blk, bool is_bad);
static bool nfc_is_badblock(u32 ra);
static int mxc_nfc_scan(bool verbose);
static int nand_flash_index = -1;
static int g_ecc_enable = true;
static int g_spare_only_read_ok = true;
static int g_nfc_debug_level = NFC_DEBUG_MIN;
static bool g_nfc_debug_measure = false;
static bool g_nfc_scan_done = false;
static bool g_is_2k_page = false;
static unsigned int g_nfc_version = MXC_NFC_V1;
static unsigned int is_bad_blk = false;

#define NFC_2K_BI_SWAP

extern unsigned int hal_timer_count(void);

#define nfc_printf(level, args...)          \
    do {                                \
        if (g_nfc_debug_level >= level)     \
            diag_printf(args);          \
    } while(0)

const static char *nand_vendor_info[] = {
    "Samsung K9F5608x0C 8-bit 512B page 32MB",
    "Samsung K9F1208R0B 8-bit 512B page 128MB",
    "Samsung K9K1G08x0B 8-bit 512B page 128MB",
    "Samsung K9F1G08U0A 8-bit 2K page 128MB",
    "Samsung K9F1G08R0A 8-bit 2K page 128MB",
    "Samsung K9LAG08U0M 8-bit 2K page 2GB MLC",
};

#ifndef MXCFLASH_SELECT_MULTI
void flash_query(void* data)
#else
void nandflash_query(void* data)
#endif
{
#ifdef MXCFLASH_SELECT_NAND_LBA
    extern u8 _lba_id[2][5];
    extern bool _lba_init_ok;
    if (_lba_init_ok) {
        memcpy(data, _lba_id[1], 4);
        return;
    }
#else /* !MXCFLASH_SELECT_NAND_LBA */
    read_nflash_id(data);
    nfc_printf(NFC_DEBUG_MAX, "%s(ID=0x%x: 0x%x, 0x%x, 0x%x)\n",
               __FUNCTION__, *(u8*)(data), *(u8*)((u32)data+1),
	       *(u8*)((u32)data+2), *(u8*)((u32)data+3));
#endif /* MXCFLASH_SELECT_NAND_LBA */
}

#ifndef MXCFLASH_SELECT_MULTI
int flash_program_buf(void* addr, void* data, int len)
#else
int nandflash_program_buf(void* addr, void* data, int len)
#endif
{
    nfc_printf(NFC_DEBUG_MAX, "%s(addr=%p, data=%p, len=0x%x)\n",
               __FUNCTION__, addr, data, len);
#ifdef MXCFLASH_SELECT_NAND_LBA
    return lba_nfc_program_region((u32)addr, (u8*)data, len);
#else /* !MXCFLASH_SELECT_NAND_LBA */
    return nfc_program_region((u32) addr, (u32) data, (u32) len);
#endif /* MXCFLASH_SELECT_NAND_LBA */
}

#ifndef MXCFLASH_SELECT_MULTI
int flash_erase_block(void* block, unsigned int size)
#else
int nandflash_erase_block(void* block, unsigned int size)
#endif
{
    nfc_printf(NFC_DEBUG_MAX, "%s(block=%p, size=0x%x)\n",
               __FUNCTION__, block, size);
    return nfc_erase_region((u32) block, size);
}

#ifndef MXCFLASH_SELECT_MULTI
bool flash_code_overlaps(void *start, void *end)
#else
bool nandflash_code_overlaps(void *start, void *end)
#endif
{
    extern unsigned char _stext[], _etext[];

    return ((((unsigned long)&_stext >= (unsigned long)start) &&
             ((unsigned long)&_stext < (unsigned long)end)) ||
            (((unsigned long)&_etext >= (unsigned long)start) &&
             ((unsigned long)&_etext < (unsigned long)end)));
}

#ifndef MXCFLASH_SELECT_MULTI
int flash_hwr_map_error(int e)
#else
int nandflash_hwr_map_error(int e)
#endif
{
    return e;
}

#ifndef MXCFLASH_SELECT_MULTI
int flash_lock_block(void* block)
#else
int nandflash_lock_block(void* block)
#endif
{
    // Not supported yet
    return 0;
}

#ifndef MXCFLASH_SELECT_MULTI
int flash_unlock_block(void* block, int block_size, int blocks)
#else
int nandflash_unlock_block(void* block, int block_size, int blocks)
#endif
{
    // Not supported yet
    return 0;
}

//----------------------------------------------------------------------------
// Now that device properties are defined, include magic for defining
// accessor type and constants.
#include <cyg/io/flash_dev.h>

// Information about supported devices
typedef struct flash_dev_info {
    cyg_uint16   device_id;
    cyg_uint16   device_id2;
    cyg_uint16   device_id3;
    cyg_uint16   device_id4;
    cyg_uint16   page_size;
    cyg_uint32   spare_size;
    cyg_uint32   pages_per_block;
    cyg_uint32   block_size;
    cyg_int32    block_count;
    cyg_uint32   base_mask;
    cyg_uint32   device_size;
    cyg_uint32   port_size;     // x8 or x16 IO
    cyg_uint32   type;          //SLC vs MLC
} flash_dev_info_t;

static const flash_dev_info_t* flash_dev_info;
static const flash_dev_info_t supported_devices[] = {
#include <cyg/io/mxc_nand_parts.inl>
};
#define NUM_DEVICES (sizeof(supported_devices)/sizeof(flash_dev_info_t))

#define NF_PG_SZ                    flash_dev_info->page_size
#define NF_PG_PER_BLK               flash_dev_info->pages_per_block
#define NF_DEV_SZ                   flash_dev_info->device_size
#define NF_BLK_SZ                   flash_dev_info->block_size
#define NF_BLK_CNT                  flash_dev_info->block_count

#define NAND_PG_SHIFT               ((g_is_2k_page)? 12: 9)

// Mask off the higher bits representing linear address of the nand flash
#define MXC_NAND_LA_MASK            (NF_DEV_SZ - 1)

#define NFC_DEVICE_ALIGN(a)         ((a) & MXC_NAND_LA_MASK & (~(NF_DEV_SZ - 1)))
#define NFC_BLOCK_ALIGN(a)          ((a) & MXC_NAND_LA_MASK & (~(NF_BLK_SZ - 1)))
#define NFC_PAGE_ALIGN(a)           ((a) & MXC_NAND_LA_MASK & (~(NF_PG_SZ - 1)))

#define BLOCK_TO_OFFSET(blk)            (blk * NF_PG_PER_BLK * NF_PG_SZ)
#define BLOCK_TO_PAGE(blk)              (blk * NF_PG_PER_BLK)
#define BLOCK_PAGE_TO_OFFSET(blk, pge)  ((blk * NF_PG_PER_BLK + pge) * NF_PG_SZ)
#define OFFSET_TO_BLOCK(offset)         ((offset / NF_PG_SZ) / NF_PG_PER_BLK)
#define OFFSET_TO_PAGE(offset)          ((offset / NF_PG_SZ) % NF_PG_PER_BLK)

static u8 *bad_block_table;
static u32 *l_to_p_table;
static bool mxcnfc_init_ok = false;

//mxc_nand_fixup_t nand_page_sz_2k_fixup;
//mxc_nand_fixup_t nand_port_sz_x16_fixup;

nfc_setup_func_t *nfc_setup = NULL;

int
#ifndef MXCFLASH_SELECT_MULTI
flash_hwr_init(void)
#else
nandflash_hwr_init(void)
#endif
{
#ifdef MXCFLASH_SELECT_NAND_LBA
    int ret;

    NFC_PRESET(MXC_UNLOCK_BLK_END);
    ret = lba_hwr_init();
    if (FLASH_ERR_OK == ret)
        mxcnfc_init_ok = true;

    return ret;
#else /* !MXCFLASH_SELECT_NAND_LBA */
    cyg_uint16 id[4];
    int i, bad_block_num;
    nfc_printf(NFC_DEBUG_MAX, "%s()\n", __FUNCTION__);
#ifdef CYGPKG_HAL_ARM_MXC91131
extern u32 system_rev;
    // take care of the NFC spare-only read bug on MXC91131 TO 2.0
    if (system_rev == CHIP_REV_2_0) {
        g_spare_only_read_ok = false;
    }
#endif

    // Look through table for device data
    flash_dev_info = supported_devices;
    for (i = 0; i < NUM_DEVICES; i++) {
        flash_dev_query(id);
        if ((flash_dev_info->device_id == id[0]) &&
            (flash_dev_info->device_id2 == 0xFFFF || flash_dev_info->device_id2 == id[1]))
            break;
        flash_dev_info++;
    }

    nfc_printf(NFC_DEBUG_MED, "%s(): %d out of NUM_DEVICES=%d, id=0x%x\n",
               __FUNCTION__, i, (u32)NUM_DEVICES, flash_dev_info->device_id);

    // Did we find the device? If not, return error.
    if (NUM_DEVICES == i) {
        diag_printf("Unrecognized NAND part: 0x%04x, 0x%04x, 0x%04x, 0x%04x\n",
                    id[0], id[1], id[2], id[3]);
        return FLASH_ERR_DRV_WRONG_PART;
    }

    if (NF_PG_SZ == 2048) {
        g_is_2k_page = true;
        g_spare_only_read_ok = false;
    }
    if (nfc_setup) {
        g_nfc_version = nfc_setup(NF_PG_SZ, flash_dev_info->port_size,
                                  flash_dev_info->type);
    }

    nand_flash_index = i;
    nfc_flash_reset();
    mxcnfc_init_ok = true;

    bad_block_table = (u8*)malloc(NF_BLK_CNT);
    memset(bad_block_table, -1, NF_BLK_CNT);
    l_to_p_table = (u32*)malloc(NF_BLK_CNT * 4);
    if (bad_block_table == 0 || l_to_p_table == 0) {
        diag_printf("%s(): malloc failed (%p, %p)\n",
                    __FUNCTION__, bad_block_table, l_to_p_table);
        return FLASH_ERR_PROTOCOL;
    }
    nfc_printf(NFC_DEBUG_MAX, "%s(bad_block_table=%p)\n",
               __FUNCTION__, bad_block_table);
    bad_block_num = mxc_nfc_scan(false);
    diag_printf("\nFound %d bad blocks\n\n", bad_block_num);

    // Hard wired for now
    flash_info.block_size = NF_BLK_SZ;
    flash_info.blocks = NF_BLK_CNT - bad_block_num;
    flash_info.start = (void *)MXC_NAND_BASE_DUMMY;
    flash_info.end = (void *)(MXC_NAND_BASE_DUMMY + (NF_DEV_SZ) -
                              (bad_block_num * NF_BLK_SZ));

    nfc_printf(NFC_DEBUG_MED, "%s(): block_size=0x%x, blocks=0x%x, start=%p, end=%p\n",
               __FUNCTION__, flash_info.block_size, flash_info.blocks,
               flash_info.start, flash_info.end);

    return FLASH_ERR_OK;
#endif /* MXCFLASH_SELECT_NAND_LBA */
}

/*!
 * Starts the address input cycles for different operations as defined in ops.
 *
 * @param ops           operations as defined in enum nfc_addr_ops
 * @param addr          starting address
 * @param mask          mask for the full address range of the nand flash
 *                      For 64MB flash, the mask should be 0x03FFFFFF (64MB-1)
 */
static void start_nfc_addr_ops(enum nfc_addr_ops ops, u32 addr, u32 mask)
{
    u32 m = mask, a = addr;

    switch (ops) {
    case ADDRESS_INPUT_READ_ID:
        NFC_ADDR_INPUT(0);
        return;
    case ADDRESS_INPUT_READ_PAGE:
    case ADDRESS_INPUT_PROGRAM_PAGE:
        if (g_is_2k_page) {
            NFC_ADDR_INPUT(a & 0xFF);
            NFC_ADDR_INPUT((a >> 8) & 0xF);
        } else {
            NFC_ADDR_INPUT(a & 0xFF);
        }
        // don't break on purpose
    case ADDRESS_INPUT_ERASE_BLOCK:
        if (g_is_2k_page) {
            a >>= 12;
            m >>= 12;
        } else {
            a >>= 9;
            m >>= 9;
        }
        break;
    default:
        diag_printf("!!!!!! %s(): wrong ops: %d !!!!!\n", __FUNCTION__, ops);
        return;
    }

    do {
        NFC_ADDR_INPUT(a);
        m >>= ADDR_INPUT_SIZE;
        a >>= ADDR_INPUT_SIZE;
    } while (m != 0);
}

#ifndef MXCFLASH_SELECT_NAND_LBA
// Doesn't seem to need it as when reaching here means past the query
// function. So reset isn't necessary.
static void nfc_flash_reset(void)
{
#ifdef MXC_NFC_RESET
    nfc_printf(NFC_DEBUG_MAX, "%s()\n", __FUNCTION__);
    NFC_PRESET(MXC_UNLOCK_BLK_END);
    NFC_CMD_INPUT(FLASH_Reset);
#endif
}
#endif /* !MXCFLASH_SELECT_NAND_LBA */

static void nfc_buf_mem_cpy(void *dst, void *src, u32 len)
{
    u16 *d = (u16*) dst, *s = (u16*) src;
    if ((len & 1) != 0) {
        diag_printf("len has to be even number\n");
        return;
    }
    if (len == 0) {
        return;
    }
    do {
        *d++ = *s++;
        len -= 2;
    } while (len > 0);
}

#ifndef MXCFLASH_SELECT_NAND_LBA
static void read_nflash_id(void* id)
{
    volatile u32 *ptr = (u32*) NAND_MAIN_BUF0;
    volatile u32 *id_32 = (u32*)id;

    nfc_printf(NFC_DEBUG_MAX, "%s()\n", __FUNCTION__);
//    NFC_PRESET(NF_BLK_CNT -1); -- doesn't work for 2k flash, why?
    NFC_PRESET(MXC_UNLOCK_BLK_END);
    NFC_CMD_INPUT(FLASH_Read_ID);
    start_nfc_addr_ops(ADDRESS_INPUT_READ_ID, 0, 0);
    NFC_DATA_OUTPUT(RAM_BUF_0, FDO_FLASH_ID, g_ecc_enable);

    *id_32++ = *ptr++;
    *id_32++ = *ptr++;
}
#endif /* !MXCFLASH_SELECT_NAND_LBA */

/*!
 * Checks to see if a block is bad by looking at the 6th byte of the spare area
 * inside a page.
 * @param ra        starting address in the raw address space (offset)
 *                  (No error checking). It doesn't have to be block-aligned.
 * @return          true if bad block; false otherwise
 */
static bool nfc_is_badblock(u32 ra)
{
    u32 block = OFFSET_TO_BLOCK(ra), ecc_val = g_ecc_enable;
    bool res = false;
    u16 temp, i;

    if (g_nfc_scan_done) {
        if (block >= NF_BLK_CNT) {
            diag_printf("Error %d: Block count out of range: %d\n", __LINE__, block);
            return true;
        }
        if (bad_block_table[block] != 0) {
            return true;
        } else{
            return false;
        }
    }

    // turn off ecc when scanning for bad blocks
//    g_ecc_enable = false; // TODO: we should turn OFF ecc
    g_ecc_enable = true;
    // check for the 1st and 2nd pages
    for (i = 0, ra = NFC_BLOCK_ALIGN(ra); i < 2; i++, ra += NF_PG_SZ) {
        if (nfc_read_page(ra) != 0) {
            diag_printf("Warning %d: uncorrectable ECC at addr 0x%x\n", __LINE__, ra);
        }
        if (g_is_2k_page && is_bad_blk) {
            diag_printf("Bad block\n");
            res = true;
            break;
        } else {
            temp = readw(NAND_SPAR_BUF0 + 4);
            if ((temp >> 8) != 0xFF) {
                res = true;
                break;
            }
        }
    }
    g_ecc_enable = ecc_val;
    return res;
}

static void nfc_update_blk_table(u32 faddr, bool is_bad)
{
    u32 block = OFFSET_TO_BLOCK(faddr);
    if (block >= NF_BLK_CNT) {
        diag_printf("Block count out of range: %d\n", block);
        return;
    }
    if (is_bad) {
        nfc_printf(NFC_DEBUG_MAX, "Block %d is bad\n", block);
        bad_block_table[block] = 1;
    } else {
        nfc_printf(NFC_DEBUG_MAX, "Block %d is good\n", block);
        bad_block_table[block] = 0;
    }
}

/*!
 * Erase a block without checking the BI field. If the block is bad, mark it
 * in the global table. Note that there is NO error checking for passed-in ra.
 * @param ra        starting address in the raw address space (offset)
 *                  Must be block-aligned
 * @return          0 if successful; -1 otherwise
 */
static int nfc_erase_blk(u32 ra)
{
    u16 flash_status;
    u32 flash_addr;

    if (ra % NF_BLK_SZ) {
        diag_printf("Non block-aligned read not supported here: 0x%x\n", ra);
        return -1;
    }
    flash_addr = (ra / NF_PG_SZ) << NAND_PG_SHIFT;

    NFC_CMD_INPUT(FLASH_Block_Erase);

    start_nfc_addr_ops(ADDRESS_INPUT_ERASE_BLOCK, flash_addr, MXC_NAND_LA_MASK);
    NFC_CMD_INPUT(FLASH_Start_Erase);

    flash_status = NFC_STATUS_READ();

    // check I/O bit 0 to see if it is 0 for success
    if((flash_status & 0x1) != 0) {
        diag_printf("Error %d: %s(at 0x%x) status=0x%x\n",
                    __LINE__, __FUNCTION__, ra, flash_status);
        nfc_update_blk_table(ra, true);
        return -1;
    }
    return 0;
}

/*!
 * Program a block of data in the flash. This function doesn't do
 * bad block checking. But if program fails, it return error.
 * @param ra        destination raw flash address
 * @param maddr     source address in the RAM
 @ @return          0 if successful; -1 otherwise
 */
static int nfc_program_blk(u32 ra, u32 maddr)
{
    u32 i;

    for (i = 0; i < NF_PG_PER_BLK; i++) {
        if (nfc_program_page(ra, maddr, NFC_MAIN_ONLY) != 0) {
            nfc_update_blk_table(ra, true);
            return -1;
        }
        ra += NF_PG_SZ;
        maddr += NF_PG_SZ;
    }
    return 0;
}

/*
 * Convert a linear address to raw address. No address checking in this function.
 * @param la        linear address used by the upper flash driver
 * @return          raw address for NAND flash
 */
static u32 nfc_l_to_p(u32 la)
{
    u32 block, offset, ra;

    block = la / NF_BLK_SZ;
    offset = la % NF_BLK_SZ;
    ra = (l_to_p_table[block] * NF_BLK_SZ) + offset;
    nfc_printf(NFC_DEBUG_MAX, "\n%s(): l_to_p_table[0x%x]=0x%x, offset=0x%x\n",
               __FUNCTION__, block, l_to_p_table[block], offset);
    nfc_printf(NFC_DEBUG_MAX, "%s(la=0x%x, ra=0x%x)\n",
               __FUNCTION__, la, ra);
    return ra;

}
/*!
 * Erase a range of NAND flash
 * @param la            linear NAND flash address. it has to be block size aligned
 * @param len           number of bytes
 * @return              FLASH_ERR_OK (0) if successful; non-zero otherwise
 */
int nfc_erase_region(u32 la, int len)
{
/* LBA-NAND doesn't have to erase. */
#ifndef MXCFLASH_SELECT_NAND_LBA
    u32 ra;

    nfc_printf(NFC_DEBUG_MED, "%s(la=0x%x, len=0x%x)\n",
               __FUNCTION__, la, len);

    la &=  MXC_NAND_LA_MASK;
    if ((la % NF_BLK_SZ) != 0 || len <= 0) {
        diag_printf("%s(): invalid value or not aligned with block boundry\n", __FUNCTION__);
        diag_printf("la=0x%x, len=%d\n", la, len);
        return FLASH_ERR_INVALID;
    }

    // now la has to be block aligned
    do {
        ra = nfc_l_to_p(la);
        la += NF_BLK_SZ;

        if (ra > (NF_DEV_SZ - NF_BLK_SZ)) {
            diag_printf("\n%s1(): la=0x%x (ra=0x%x) is out of valid range\n",
                        __FUNCTION__, la, ra);
            return FLASH_ERR_ERASE;
        }
        if (nfc_is_badblock(ra)) {
            diag_printf("\n%s(ra=0x%x): bad block: %d\n",
                       __FUNCTION__, ra, OFFSET_TO_BLOCK(ra));
            return FLASH_ERR_ERASE;
        } else {
            if (nfc_erase_blk(ra) == 0) {
                // erase ok
                len -= NF_BLK_SZ;
            } else {
                diag_printf("\n%s2(): ra=0x%x bad block: %d\n",
                           __FUNCTION__, ra, OFFSET_TO_BLOCK(ra));
                return FLASH_ERR_ERASE;
            }
        }
    } while (len > 0);
#endif /* !MXCFLASH_SELECT_NAND_LBA */

    return FLASH_ERR_OK;
}

/*!
 * Program data from memory to flash
 * @param la            linear NAND flash address. it has to be block size aligned
 * @param maddr         memory buf address where data will be copied from
 * @param len           number of bytes
 * @return              FLASH_ERR_OK (0) if successful; non-zero otherwise
 */
int nfc_program_region(u32 la, u32 maddr, int len)
{
    u32 ra;

    nfc_printf(NFC_DEBUG_MED, "%s(la=0x%x, maddr=0x%x, len=0x%x)\n",
               __FUNCTION__, la, maddr, len);

    la &=  MXC_NAND_LA_MASK;

    if ((la % NF_BLK_SZ) != 0 || len <= 0) {
        diag_printf("%s(): invalid or not block aligned\n", __FUNCTION__);
        diag_printf("la=0x%x, len=%d\n", la, len);
        return FLASH_ERR_INVALID;
    }

    do {
        ra = nfc_l_to_p(la);
        la += NF_BLK_SZ;

        if (ra > (NF_DEV_SZ - NF_BLK_SZ)) {
            diag_printf("%s()1: la=0x%x (ra=0x%x) is out of valid range\n",
                        __FUNCTION__, la, ra);
            return FLASH_ERR_PROGRAM;
        }

        if (nfc_is_badblock(ra)) {
            diag_printf("\n%s(ra=0x%x): bad block: %d\n",
                        __FUNCTION__, ra, OFFSET_TO_BLOCK(ra));
            return FLASH_ERR_PROGRAM;
        } else {
            if (nfc_program_blk(ra, maddr) == 0) {
                len -= NF_BLK_SZ;
                maddr += NF_BLK_SZ;
            } else {
                diag_printf("\n%s2(ra=0x%x): bad block: %d\n",
                            __FUNCTION__, ra, OFFSET_TO_BLOCK(ra));
                return FLASH_ERR_PROGRAM;
            }
        }
    } while (len > 0);

    return FLASH_ERR_OK;
}

/*!
 * Read data from linear NAND flash address to memory. The MSB of the passed-
 * in flash address will be masked off inside the function.
 *
 * @param la            linear NAND flash address. it has to be page aligned
 * @param mem_addr      memory buf address where data will be copied to
 * @param len           number of bytes
 * @return              FLASH_ERR_OK (0) if successful; non-zero otherwise
 */
int nfc_read_region(u32 la, u32 mem_addr, int len)
{
    u32 ra;
    u32 dst = mem_addr;
    int i;

    // make sure 32-bit aligned
    len = (len + 3) & (~0x3);

    nfc_printf(NFC_DEBUG_MED, "\n%s(la=0x%x, mem_addr=0x%x, len=0x%x)\n",
               __FUNCTION__, la, mem_addr, len);

    if (la < (u32)(flash_info.start) || (la + len) > (u32)(flash_info.end)) {
        diag_printf("\n%s(): Error: invalide address=0x%x, len=%d\n",
                    __FUNCTION__, la, len);
        return FLASH_ERR_INVALID;
    }
    la &=  MXC_NAND_LA_MASK;

    if ((la % NF_PG_SZ) != 0 || len <= 0) {
        diag_printf("\n%s(): invalid value or not aligned with page boundry\n", __FUNCTION__);
        diag_printf("la=0x%x, len=%d\n", la, len);
        return FLASH_ERR_INVALID;
    }

    do {
        ra = nfc_l_to_p(la);
        la += NF_PG_SZ;

        if (nfc_is_badblock(ra)) {
            diag_printf("\n%s(1): ra=0x%x bad block: %d\n",
                       __FUNCTION__, ra, OFFSET_TO_BLOCK(ra));
            return FLASH_ERR_INVALID;
        } else {
            if (nfc_read_page(ra) != 0) {
                return FLASH_ERR_INVALID;
            }
            i = (len < NF_PG_SZ) ? len: NF_PG_SZ;
//            diag_printf("\nlen=%d, i=%d\n", len, i);
            // now do the copying
            nfc_buf_mem_cpy((void*)dst, (void*)(NAND_MAIN_BUF0), i);
            len -= i;
            dst += i;
        }
    } while (len > 0);

    return FLASH_ERR_OK;
}

static void mxc_swap_2k_BI_main_sp(int check_bad_blk)
{
    u16 tmp1, tmp2, new_tmp1;
    tmp1 = readw(BAD_BLK_MARKER_464);
    tmp2 = readw(BAD_BLK_MARKER_SP_5);

    new_tmp1 = (tmp1 & 0xFF00) | (tmp2 >> 8);
    tmp2 = (tmp1 << 8) | (tmp2 & 0xFF);
    writew(new_tmp1, BAD_BLK_MARKER_464);
    writew(tmp2, BAD_BLK_MARKER_SP_5);
    if (check_bad_blk) {
        is_bad_blk = 0;
        if ((tmp1 & 0xFF) != 0xFF) {
            is_bad_blk = 1;
        }
    }
}

static int nfc_write_pg_random(u32 flash_addr, u32 mem_addr,
                               enum nfc_page_area area, int swap)
{
    u16 flash_status, i;

//    diag_printf("%s(0x%x, 0x%x, %d\n", __FUNCTION__, ra, mem_addr, area);

    switch (area) {
    case NFC_MAIN_ONLY:
        // Read back the spare area first
        for (i = 0; i < 16; i++) {
            // Make all spare area as 0xFF
            writel(0xFFFFFFFF, NAND_SPAR_BUF0 + i * 4);
        }

        nfc_buf_mem_cpy((void *)NAND_MAIN_BUF0, (void *) mem_addr, 512);
        if (g_is_2k_page) {
            nfc_buf_mem_cpy((void *)NAND_MAIN_BUF1, (void *) (mem_addr + 512),
                            512 * 3);
#ifdef MXC_NAND_BOOT_LOAD_AT_0x400
            // To replace the data at offset 0x400 with the address of the NFC base
            // This is needed for certain platforms 
            if ((flash_addr <= 0x400) && ((flash_addr + NF_PG_SZ - 1) > 0x400)) {
//                diag_printf("\nflash_addr = 0x%x\n", flash_addr);
                diag_printf("\n[INFO] 2K page: copy data at 0x400 to spare area and set it to 0x%x\n", NFC_BASE);
                writel(readl(NFC_BASE + 0x400), NAND_SPAR_BUF2);
                writel(NFC_BASE, NFC_BASE + 0x400);
            }
#endif
#ifdef NFC_2K_BI_SWAP
            if (swap)
                mxc_swap_2k_BI_main_sp(0);
#endif
        } else {
#ifdef MXC_NAND_BOOT_LOAD_AT_0x400
            // To replace the data at offset 0x400 with the address of the NFC base
            // This is needed for certain platforms 
            if ((flash_addr <= 0x400) && ((flash_addr + NF_PG_SZ - 1) > 0x400)) {
                diag_printf("\nflash_addr = 0x%x\n", flash_addr);
                diag_printf("\n[INFO] 512 page: copy data at 0x400 to spare area and set it to 0x%x\n", NFC_BASE);
                writel(readl(NFC_BASE), NAND_SPAR_BUF0);
                writel(NFC_BASE, NFC_BASE);
            }
#endif
        }
        break;
    case NFC_SPARE_ONLY:
        // This is used ONLY for testing when manually create "bad" blocks
        nfc_buf_mem_cpy((void *)(NAND_SPAR_BUF0), (void *)mem_addr, 16);
        if (!g_is_2k_page) {
            NFC_CMD_INPUT(FLASH_Read_Mode3);
        }
        break;
    default:
        diag_printf("NOT supported yet!\n");
        return -1;
    }

    NFC_CMD_INPUT(FLASH_Send_Data);
    start_nfc_addr_ops(ADDRESS_INPUT_PROGRAM_PAGE, flash_addr,
                       MXC_NAND_LA_MASK);

    NFC_DATA_INPUT(RAM_BUF_0, area, g_ecc_enable);
    if (g_is_2k_page && PG_2K_DATA_OP_MULTI_CYCLES()) {
        NFC_DATA_INPUT_2k(RAM_BUF_1);
        NFC_DATA_INPUT_2k(RAM_BUF_2);
        NFC_DATA_INPUT_2k(RAM_BUF_3);
    }
    NFC_CMD_INPUT(FLASH_Program);

    flash_status = NFC_STATUS_READ();
    // check I/O bit 0 to see if it is 0 for success
    if((flash_status & 0x1) != 0) {
        diag_printf("Error %d: %s(page=%d, col=%d) status=0x%x\n",
                    __LINE__, __FUNCTION__, flash_addr >> NAND_PG_SHIFT,
                    flash_addr & ((1 << NAND_PG_SHIFT) -1), flash_status);

        return -1;
    }

    return 0;
}

/*!
 * This function programs a page's main, spare, or both. For main area program,
 * It copies out the spare area of that page first and then write it along
 * with the main area back to the NAND flash (FIXME: can't just program main alone?
 * For spare area program, it will scratch out the main area data (FIXME).
 *
 * @param ra            starting address to be programmed inside the NAND flash.
 *                      Must be page-aligned
 * @param mem_addr      source address in the RAM.
 *                      For main area:  mem_addr -> starting of data for main area
 *                      For spare area: mem_addr -> starting of data for spare area
 *                      For both area: mem_addr -> starting of data for main area along with spare area
 * @return              0 if no error or 1-bit error; -1 otherwise
 */
// FIXME: Add programming of the spare area only
static int nfc_program_page(u32 ra, u32 mem_addr, enum nfc_page_area area)
{
    u32 flash_addr;

//    diag_printf("%s(0x%x, 0x%x, %d\n", __FUNCTION__, ra, mem_addr, area);

    if (ra % NF_PG_SZ) {
        diag_printf("%s(): Non page-aligned read not supported: 0x%x\n",
                    __FUNCTION__, ra);
        return -1;
    }
    flash_addr = (ra / NF_PG_SZ) << NAND_PG_SHIFT;

    return nfc_write_pg_random(flash_addr, mem_addr,area, 1);
}

/*!
 * Low level spare-only read. Only applies to 512 byte page NAND.
 *
 * @param addr          starting address to be read from the NAND flash
 * @param buf           one of the internal buffers
 * @return              0 if no error or 1-bit error; -1 otherwise
 */
static int nfc_sp_only_read_ll(u32 addr, enum nfc_internal_buf buf)
{
    volatile u16 temp;

    NFC_CMD_INPUT(FLASH_Read_Mode3);
    start_nfc_addr_ops(ADDRESS_INPUT_READ_PAGE, addr, MXC_NAND_LA_MASK);
    NFC_DATA_OUTPUT(buf, FDO_SPARE_ONLY, g_ecc_enable);
    temp = readw(ECC_STATUS_RESULT_REG);
    NFC_CMD_INPUT(FLASH_Read_Mode1);

    if (g_ecc_enable) {
        if ((temp & 0x2) != 0x0) {
            nfc_printf(NFC_DEBUG_MED, "\nError %d: %s(addr=0x%x): ECC status result reg=0x%x\n",
                        __LINE__, __FUNCTION__, addr, temp);
            return -1;
        }
    }

    return 0;
}

/*!
 * Read spare area from NAND flash to the 1st internal RAM buffer. 
 * Not supported for 2kB page NAND.
 * 
 * @param addr          starting address to be read from the NAND flash
 *
 * @return              0 if no error or 1-bit error; -1 otherwise
 */
static int nfc_read_page_sp(u32 addr)
{
    if (g_spare_only_read_ok) {
        if (g_is_2k_page) {
            diag_printf("spare-only read for 2k page is not supported\n");
            return -1;
        }
        return nfc_sp_only_read_ll(addr, RAM_BUF_0);
    }
    return -1;
}

static int nfc_read_pg_random(u32 flash_addr, int swap)
{
    volatile u16 t1, t2 = 0, t3 = 0, t4 = 0;
    int res = 0;
        
#if 0 //TODO: kevin revisit
    if (!g_is_2k_page && (t1 = (flash_addr & ((1 << (1 + NAND_PG_SHIFT)) -1))) >= 512) {
            NFC_CMD_INPUT(FLASH_Read_Mode3);
            flash_addr -= 512;
            diag_printf("kevin: 0x%x\n", flash_addr);
    } else {
            NFC_CMD_INPUT(FLASH_Read_Mode1);
    }
#endif
    NFC_CMD_INPUT(FLASH_Read_Mode1);

    start_nfc_addr_ops(ADDRESS_INPUT_READ_PAGE, flash_addr, MXC_NAND_LA_MASK);
    if (g_is_2k_page) {
        NFC_CMD_INPUT(FLASH_Read_Mode1_2K);
    }
    
    if (g_nfc_version == MXC_NFC_V1) {
        NFC_DATA_OUTPUT(RAM_BUF_0, FDO_PAGE_SPARE, g_ecc_enable);
        t1 = readw(ECC_STATUS_RESULT_REG);

        if (g_is_2k_page) {
            NFC_DATA_OUTPUT(RAM_BUF_1, FDO_PAGE_SPARE, g_ecc_enable);
            t2 = readw(ECC_STATUS_RESULT_REG);
            NFC_DATA_OUTPUT(RAM_BUF_2, FDO_PAGE_SPARE, g_ecc_enable);
            t3 = readw(ECC_STATUS_RESULT_REG);
            NFC_DATA_OUTPUT(RAM_BUF_3, FDO_PAGE_SPARE, g_ecc_enable);
            t4 = readw(ECC_STATUS_RESULT_REG);
        }
        
        if (g_ecc_enable && ((t1 & 0xA) != 0x0 || (t2 & 0xA) != 0x0 || (t3 & 0xA) != 0x0
            || (t4 & 0xA) != 0x0)) {
            diag_printf("\nError %d: %s(page=%d, col=%d): ECC status=0x%x:0x%x:0x%x:0x%x\n",
                        __LINE__, __FUNCTION__, flash_addr >> NAND_PG_SHIFT,
                        flash_addr & ((1 << NAND_PG_SHIFT) -1), t1, t2, t3, t4);
            res = -1;
            goto out;
        }
    } else if (g_nfc_version == MXC_NFC_V2) {
        NFC_DATA_OUTPUT(RAM_BUF_0, FDO_PAGE_SPARE, g_ecc_enable);
        if (g_is_2k_page) {
            if (PG_2K_DATA_OP_MULTI_CYCLES()) {
                NFC_DATA_OUTPUT(RAM_BUF_1, FDO_PAGE_SPARE, g_ecc_enable);
                NFC_DATA_OUTPUT(RAM_BUF_2, FDO_PAGE_SPARE, g_ecc_enable);
                NFC_DATA_OUTPUT(RAM_BUF_3, FDO_PAGE_SPARE, g_ecc_enable);
            }
            // To replace the data at offset 0x400 with the address of the NFC base
            // This is needed for certain platforms 
            if ((flash_addr <= 0x400) && ((flash_addr + NF_PG_SZ - 1) > 0x400)) {
//                diag_printf("\nRead: flash_addr = 0x%x\n", flash_addr);
                diag_printf("\n[INFO] 2K page: copy back data from spare to 0x400\n");
                writel(readl(NAND_SPAR_BUF2), NFC_BASE + 0x400);
            }
        } else {
            // To replace the data at offset 0x400 with the address of the NFC base
            // This is needed for certain platforms 
            if ((flash_addr <= 0x400) && ((flash_addr + NF_PG_SZ - 1) > 0x400)) {
                diag_printf("\nflash_addr = 0x%x\n", flash_addr);
                diag_printf("\n[INFO] 512 page: copy back data from spare to 0x400\n");
                writel(readl(NAND_SPAR_BUF0), NFC_BASE);
            }
        }
        if (g_ecc_enable) {
            t1 = readw(ECC_STATUS_RESULT_REG);
            if ((t1 & 0xF) > 4) {
                diag_printf("\nError %d: %s(page=%d, col=%d): ECC status=0x%x\n",
                            __LINE__, __FUNCTION__, flash_addr >> NAND_PG_SHIFT,
                            flash_addr & ((1 << NAND_PG_SHIFT) -1), t1 & 0xF);
                res = -1;
                goto out;
            }

            if (g_is_2k_page) {
                t2 = (t1 >> 4) & 0xF;
                t3 = (t1 >> 8) & 0xF;
                t4 = (t1 >> 12) & 0xF;
                if (t2 > 4 || t3 > 4 || t4 > 4) {
                    diag_printf("\nError %d: %s(page=%d, col=%d): ECC status=0x%x:0x%x:0x%x\n",
                                __LINE__, __FUNCTION__, flash_addr >> NAND_PG_SHIFT,
                                flash_addr & ((1 << NAND_PG_SHIFT) -1), t2, t3, t4);
                    res = -1;
                    goto out;
                }
            }
        }
    }
out:
    if (g_is_2k_page) {
#ifdef NFC_2K_BI_SWAP
        if (swap)
            mxc_swap_2k_BI_main_sp(1);
#endif
    }
    return res;
}

/*!
 * Read a page's both main and spare area from NAND flash to the internal RAM buffer.
 * It always reads data to the internal buffer 0.
 *
 * @param ra          starting address to be read from the NAND flash; must be page-aligned
 *
 * @return              0 if no error or 1-bit error; -1 otherwise
 */
static int nfc_read_page(u32 ra)
{
    u32 flash_addr;

    if (ra % NF_PG_SZ) {
        diag_printf("Non page-aligned read not supported here: 0x%x\n", ra);
        return -1;
    }
    flash_addr = (ra / NF_PG_SZ) << NAND_PG_SHIFT;

    return nfc_read_pg_random(flash_addr, 1);
}

// Read data into buffer
int flash_read_buf(void* addr, void* data, int len)
{
    if (IS_BOOTING_FROM_NOR() || IS_FIS_FROM_NOR()) {
        memcpy(data, addr, len);
        return 0;
    } else {
#ifdef MXCFLASH_SELECT_NAND_LBA
        return lba_nfc_read_region((u32)addr, (u8*)data, len);
#else /* !MXCFLASH_SELECT_NAND_LBA */
        return nfc_read_region((u32)addr, (u32)data, (u32)len);
#endif /* MXCFLASH_SELECT_NAND_LBA */
    }
}

void mxc_nfc_print_info(void)
{
    diag_printf("[0x%08x bytes]: %d blocks of %d pages of %d bytes each.\n",
                NF_DEV_SZ, NF_BLK_CNT,
                NF_PG_PER_BLK, NF_PG_SZ);
}

static int mxc_nfc_scan(bool verbose)
{
    int addr, bad = 0, j = 0, i = 0;
    u32 count1 = hal_timer_count(), count2;

    g_nfc_scan_done = false;

    nfc_printf(NFC_DEBUG_MAX, "%s()\n", __FUNCTION__);
    for (addr = 0; addr < NF_DEV_SZ;
         addr += NF_BLK_SZ) {
        if (nfc_is_badblock(addr)) {
            bad++;
            nfc_update_blk_table(addr, true);
            if (verbose)
                nfc_printf(NFC_DEBUG_DEF, "  block %d at 0x%x\n",
                           OFFSET_TO_BLOCK(addr), addr);
        } else {
            nfc_update_blk_table(addr, false);
            l_to_p_table[j] = i;
            j++;
        }
        i++;
    }

    if (g_nfc_debug_measure) {
        count2 = hal_timer_count();
        diag_printf("counter1=%d, counter2=%d, diff=%d\n",
                    count1, count2, count2 - count1);
        diag_printf("Using [diff * 1000000 / 32768] to get usec\n");
    }
    g_nfc_scan_done = true;
    return bad;
}

////////////////////////// "nand" commands support /////////////////////////
// Image management functions
local_cmd_entry("info",
        "Show nand flash info (number of good/bad blocks)",
        "[-f <raw address>] [-l <length>]",
        nand_info,
        NAND_cmds
           );

local_cmd_entry("show",
        "Show a page main/spare areas or spare area only (-s)",
        "-f <raw page address> [-s]",
        nand_show,
        NAND_cmds
           );

local_cmd_entry("read",
        "Read data from nand flash into RAM",
        "-f <raw address> -b <memory_load_address> -l <image_length> [-c <col_addr>]",
        nand_read,
        NAND_cmds
           );

local_cmd_entry("write",
        "Write data from RAM into nand flash",
        "-f <raw address> -b <memory_address> -l <image_length> [-c <col_addr>]",
        nand_write,
        NAND_cmds
           );

local_cmd_entry("erase",
        "Erase nand flash contents",
        "-f <raw address> -l <length> [-o] [-z] \n\
                -o: force erase (even for bad blocks) \n\
                -z: mark bad (testing only!)",
        nand_erase,
        NAND_cmds
           );

#if 0
local_cmd_entry("format",
        "Check ALL blocks with ECC disabled and Erase the entire NAND flash with ECC ",
        "-f <raw address> -l <length> [-o] [-z] \n\
                -o: force erase (even for bad blocks) \n\
                -z: mark bad (testing only!)",
        nand_format,
        NAND_cmds
           );
#endif
local_cmd_entry("debug",
        "Various NAND debug features ",
        "<0> min debug messages <default> \n\
             <1> med debug messages \n\
             <2> max debug messages \n\
             <3> enable(default)/disable h/w ECC for both r/w \n\
             <4> disable(default)/enalbe spare-only read \n\
             <9> enable/disable measurement \n\
             no parameter - display current debug setup",
        nand_debug_fun,
        NAND_cmds
           );

// Define table boundaries
CYG_HAL_TABLE_BEGIN( __NAND_cmds_TAB__, NAND_cmds);
CYG_HAL_TABLE_END( __NAND_cmds_TAB_END__, NAND_cmds);

extern struct cmd __NAND_cmds_TAB__[], __NAND_cmds_TAB_END__;

// CLI function
static cmd_fun do_nand_cmds;
RedBoot_nested_cmd("nand",
           "Utility function to NAND flash using raw address",
           "{cmds}",
           do_nand_cmds,
           __NAND_cmds_TAB__, &__NAND_cmds_TAB_END__
          );

#ifdef MXCFLASH_SELECT_NAND_LBA
void nand_usage(char *why)
#else /* !MXCFLASH_SELECT_NAND_LBA */
static void nand_usage(char *why)
#endif /* MXCFLASH_SELECT_NAND_LBA */
{
    diag_printf("*** invalid 'nand' command: %s\n", why);
    cmd_usage(__NAND_cmds_TAB__, &__NAND_cmds_TAB_END__, "nand ");
}

static u32 curr_addr;
static void nand_show(int argc, char *argv[])
{
    u32 ra;
    bool flash_addr_set = false;
    bool spar_only = false;
    struct option_info opts[2];

    init_opts(&opts[0], 'f', true, OPTION_ARG_TYPE_NUM,
          (void *)&ra, (bool *)&flash_addr_set, "NAND FLASH memory byte address");
    init_opts(&opts[1], 's', false, OPTION_ARG_TYPE_FLG,
          (void *)&spar_only, (bool *)0, "Spare only");

    if (!scan_opts(argc, argv, 2, opts, 2, 0, 0, "")) {
        return;
    }
    if (!flash_addr_set) {
        ra = curr_addr;
        curr_addr += NF_PG_SZ;
    } else {
        curr_addr = ra;
    }

    ra &= MXC_NAND_LA_MASK;

    if (ra % NF_PG_SZ) {
        diag_printf("error: non-page aligned\n");
        return;
    }

    if (nfc_is_badblock(NFC_BLOCK_ALIGN(ra))) {
        diag_printf("This is a bad block\n");
    }

    print_page(ra, spar_only);
}

/*!
 * For low level nand read command. It doesn't check for bad block or not
 */
static void nand_read(int argc, char *argv[])
{
    int len;
    u32 mem_addr, ra, col;
    bool mem_addr_set = false;
    bool flash_addr_set = false;
    bool length_set = false;
    bool col_set = false;
    struct option_info opts[4];
    int j = 0;
    bool ecc_status = g_ecc_enable;;

    init_opts(&opts[0], 'b', true, OPTION_ARG_TYPE_NUM,
              (void *)&mem_addr, (bool *)&mem_addr_set, "memory base address");
    init_opts(&opts[1], 'f', true, OPTION_ARG_TYPE_NUM,
              (void *)&ra, (bool *)&flash_addr_set, "FLASH memory base address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&length_set, "image length [in FLASH]");
    init_opts(&opts[3], 'c', true, OPTION_ARG_TYPE_NUM,
              (void *)&col, (bool *)&col_set, "column addr");
    
    if (!scan_opts(argc, argv, 2, opts, 4, 0, 0, 0)) {
        nand_usage("invalid arguments");
        return;
    }

    if (!mem_addr_set || !flash_addr_set || !length_set) {
        nand_usage("required parameter missing");
        return;
    }
    if ((mem_addr < (CYG_ADDRESS)ram_start) ||
        ((mem_addr+len) >= (CYG_ADDRESS)ram_end)) {
        diag_printf("** WARNING: RAM address: %p may be invalid\n", (void *)mem_addr);
        diag_printf("   valid range is %p-%p\n", (void *)ram_start, (void *)ram_end);
    }

    // Safety check - make sure the address range is not within the code we're running
    if (flash_code_overlaps((void *)ra, (void *)(ra+len-1))) {
        diag_printf("Can't program this region - contains code in use!\n");
        return;
    }
    
    if (col_set) {
        u32 flash_addr;

        diag_printf("Random read at page %d, column %d\n",
                    ra / NF_PG_SZ, col);
        flash_addr = ((ra / NF_PG_SZ) << NAND_PG_SHIFT) + col;

        if (g_is_2k_page) {
            g_ecc_enable = false;
        }
        nfc_read_pg_random(flash_addr, 0); // don't swap BI for 2k page
        if (g_is_2k_page) {
            g_ecc_enable = ecc_status;
        }
        
        nfc_buf_mem_cpy((void *)mem_addr, (void *)NAND_MAIN_BUF0, NF_PG_SZ);
        return;
    }
    
    // insure integer multiple of page size
    len = (len + NF_PG_SZ - 1) & ~(NF_PG_SZ - 1);

    ra &=  MXC_NAND_LA_MASK;

    do {
        if (OFFSET_TO_BLOCK(ra) > (NF_BLK_CNT - 1)) {
            diag_printf("Out of range: addr=0x%x\n", ra);
            return;
        }
        if (nfc_read_page(ra) != 0) {
            diag_printf("Error %d: uncorrectable ECC at addr 0x%x\n", __LINE__, ra);
            diag_printf("should invoke bad block management to replace this block \n");
            diag_printf("and then mark this block \"bad\". But Redboot doesn't do it yet.\n");
        }
        if ((j++ % 0x20) == 0)
            diag_printf("\n%s 0x%08x: ", __FUNCTION__, ra);
        diag_printf(".");

        nfc_buf_mem_cpy((void *)mem_addr, (void *)NAND_MAIN_BUF0, NF_PG_SZ);

        ra += NF_PG_SZ;
        mem_addr += NF_PG_SZ;
        len -= NF_PG_SZ;
    } while (len > 0);
    diag_printf("\n");
}

static void nand_write(int argc, char *argv[])
{
    int len, j = 0;
    u32 mem_addr, ra, col;
    bool mem_addr_set = false;
    bool flash_addr_set = false;
    bool length_set = false;
    bool col_set = false;
    struct option_info opts[4];
    bool ecc_status = g_ecc_enable;;

    init_opts(&opts[0], 'b', true, OPTION_ARG_TYPE_NUM,
              (void *)&mem_addr, (bool *)&mem_addr_set, "memory base address");
    init_opts(&opts[1], 'f', true, OPTION_ARG_TYPE_NUM,
              (void *)&ra, (bool *)&flash_addr_set, "FLASH memory base address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&length_set, "image length [in FLASH]");
    init_opts(&opts[3], 'c', true, OPTION_ARG_TYPE_NUM,
              (void *)&col, (bool *)&col_set, "column addr");
    if (!scan_opts(argc, argv, 2, opts, 4, 0, 0, 0))
    {
        nand_usage("invalid arguments");
        return;
    }

    if (!mem_addr_set || !flash_addr_set || !length_set) {
        nand_usage("required parameter missing");
        return;
    }

    if ((mem_addr < (CYG_ADDRESS)ram_start) ||
        ((mem_addr+len) >= (CYG_ADDRESS)ram_end)) {
        diag_printf("** WARNING: RAM address: %p may be invalid\n", (void *)mem_addr);
        diag_printf("   valid range is %p-%p\n", (void *)ram_start, (void *)ram_end);
    }

    if (col_set) {
        u32 flash_addr;

        diag_printf("Random write at page %d, column %d\n",
                    ra / NF_PG_SZ, col);
        flash_addr = ((ra / NF_PG_SZ) << NAND_PG_SHIFT) + col;
        
diag_printf("Kevin g_ecc_enable=%d\n", g_ecc_enable);        
        if (g_is_2k_page) {
            g_ecc_enable = false;
        }
        nfc_write_pg_random(flash_addr, mem_addr, NFC_MAIN_ONLY, 0);
        if (g_is_2k_page) {
            g_ecc_enable = ecc_status;
        }
        return;
    }

    ra &=  MXC_NAND_LA_MASK;

    if ((len % NF_PG_SZ) != 0) {
        diag_printf("Not a full page write?\n\n");
    }

    do {
        if (OFFSET_TO_BLOCK(ra) > (NF_BLK_CNT - 1)) {
            diag_printf("Out of range: addr=0x%x\n", ra);
            return;
        }
        if (nfc_is_badblock(ra)) {
            diag_printf("\nERROR: bad block at raw addr=0x%x(block=%d)\n",
                       ra, OFFSET_TO_BLOCK(ra));
            diag_printf("%s() failed\n", __FUNCTION__);
            return;
        }

        if (nfc_program_page(ra, mem_addr, NFC_MAIN_ONLY) != 0) {
            if (g_nfc_debug_level >= NFC_DEBUG_DEF) {
                diag_printf("Error %d: program error at addr 0x%x\n", __LINE__, ra);
                diag_printf("should invoke bad block management to replace this block \n");
                diag_printf("and then mark this block \"bad\". But Redboot doesn't do it yet.\n");
            }
            return;
        }
        if ((j++ % 0x20) == 0)
            diag_printf("\nProgramming 0x%08x: ", ra);
        diag_printf(".");
        
        len -= NF_PG_SZ;
        ra += NF_PG_SZ;
        mem_addr += NF_PG_SZ;
    } while (len > 0);
    diag_printf("\n");
}

void nand_debug_fun(int argc, char *argv[])
{
    int opt;

    if (argc == 3) {
        opt = argv[2][0] - '0';
        switch (opt) {
        case 0:
            g_nfc_debug_level = NFC_DEBUG_MIN;
            break;
        case 1:
            g_nfc_debug_level = NFC_DEBUG_MED;
            break;
        case 2:
            g_nfc_debug_level = NFC_DEBUG_MAX;
            break;
        case 3:
            g_ecc_enable = g_ecc_enable? false: true;
            break;
        case 4:
            // toggle g_spare_only_read_ok
            g_spare_only_read_ok = g_spare_only_read_ok? false: true;
            break;
        case 9:
            g_nfc_debug_measure = g_nfc_debug_measure? false: true;
            break;

        default:
            diag_printf("%s(%s) not supported\n", __FUNCTION__, argv[2]);
            break;

        }
    }
    diag_printf("Current debug options are: \n");
    diag_printf("    h/w ECC: %s\n", g_ecc_enable ? "on":"off");
    diag_printf("    sp-only read: %s\n", g_spare_only_read_ok ? "on":"off");
    diag_printf("    measurement: %s\n", g_nfc_debug_measure ? "on":"off");
    diag_printf("    message level: %s\n", (g_nfc_debug_level == NFC_DEBUG_MIN) ? "min" : \
        ((g_nfc_debug_level == NFC_DEBUG_MED) ? "med" : "max"));
}

static void nand_erase(int argc, char *argv[])
{
    u32 i, j = 0, len, ra;
    bool faddr_set = false;
    bool force_erase_set = false;
    bool force_bad_block_set = false;
    bool length_set = false;
    struct option_info opts[4];

    init_opts(&opts[0], 'f', true, OPTION_ARG_TYPE_NUM,
          (void **)&ra, (bool *)&faddr_set, "FLASH memory base address");
    init_opts(&opts[1], 'l', true, OPTION_ARG_TYPE_NUM,
          (void **)&len, (bool *)&length_set, "length in bytes");
    init_opts(&opts[2], 'o', false, OPTION_ARG_TYPE_FLG,
          (void **)&force_erase_set, (bool *)&force_erase_set, "force erases block");
    init_opts(&opts[3], 'z', false, OPTION_ARG_TYPE_FLG,
              (void **)&force_bad_block_set, (bool *)&force_bad_block_set, "erases blocks and mark bad");

    if (!scan_opts(argc, argv, 2, opts, 4, 0, 0, 0)) {
        nand_usage("invalid arguments");
        return;
    }

    if (!faddr_set || !length_set) {
        nand_usage("missing argument");
        return;
    }
    if ((ra % NF_BLK_SZ) != 0 ||
        (len % NF_BLK_SZ) != 0 || len == 0) {
        diag_printf("Address or length is not block aligned or length is zero!\n");
        diag_printf("Block size is 0x%x\n", NF_BLK_SZ);
        return;
    }

    if (!verify_action("About to erase 0x%x bytes from nand offset 0x%x\n", len, ra)) {
        diag_printf("** Aborted\n");
        return;
    }

    ra &=  MXC_NAND_LA_MASK;

    // now ra is block aligned
    if (force_erase_set == true) {
        diag_printf("Force erase ...");
        for (i = ra; i < (ra + len); i += NF_BLK_SZ) {
            if (nfc_erase_blk(i) != 0) { //error
                diag_printf("erase error0 in block at raw address: 0x%x\n", i);
                goto nand_erase_out;           //don't erase bad block
            } else {
                if ((j++ % 0x20) == 0)
                    diag_printf("\nErasing 0x%08x: ", i);
                diag_printf(".");
            }
        }
        diag_printf("\n");
    } else if (force_bad_block_set == true) {
        u16 temp_spare_buf[8] = {0, 0, 0, 0, 0, 0, 0, 0};
        
        for (i = ra; i < (ra + len); i += NF_BLK_SZ) {
            if (i == 0) {
                diag_printf("Can't change first block to bad. Skip\n");
                continue;
            }
            if (nfc_is_badblock(i)) {
                diag_printf("\nblock at 0x%x is already bad\n", i);
                continue;           //don't erase bad block
            }
            diag_printf("Erasing ... \n");

            if (nfc_erase_blk(i) != 0) { //error
                diag_printf("erase error1 in block at raw address: 0x%x\n", i);
                goto nand_erase_out;           //don't erase bad block
            }

            diag_printf("Marking bad block at: 0x%08x\n", i);

            if (nfc_program_page(i, (u32)temp_spare_buf, NFC_SPARE_ONLY) != 0) {
                diag_printf("Can't program page: 0x%x\n", i);
                continue;
            }
        }
    } else {
        for (i = ra; i < (ra + len); i += NF_BLK_SZ) {
            if (nfc_is_badblock(i)) {
                diag_printf("\nCan't erase bad block at offset: 0x%x. Skipped\n", i);
                continue;           //don't erase bad block
            }
            if (nfc_erase_blk(i) != 0) { //error
                diag_printf("erase error2 in block at raw address: 0x%x\n", i);
                continue;           //don't erase bad block
            }
            if ((j++ % 0x20) == 0)
                diag_printf("\nErasing 0x%08x: ", i);
            diag_printf(".");
        }
    }
nand_erase_out:
    diag_printf("\n");
    mxc_nfc_scan(false);
}

static void nand_info(int argc, char *argv[])
{
    u32 i, j = 0, len, ra;
    bool flash_addr_set = false;
    bool flash_len_set = false;
    struct option_info opts[2];

    init_opts(&opts[0], 'f', true, OPTION_ARG_TYPE_NUM,
          (void *)&ra, (bool *)&flash_addr_set, "NAND FLASH memory byte address");
    init_opts(&opts[1], 'l', true, OPTION_ARG_TYPE_NUM,
          (void *)&len, (bool *)&flash_len_set, "length");

    if (!scan_opts(argc, argv, 2, opts, 2, 0, 0, 0)) {
        nand_usage("invalid arguments");
        return;
    }

    if (nand_flash_index == -1) {
        diag_printf("Can't find valid NAND flash: %d\n", __LINE__);
        return;
    }

    i = mxc_nfc_scan(true);
    if (argc == 2) {
        diag_printf("\nType: %s\n", nand_vendor_info[nand_flash_index]);
        diag_printf("Total size:\t 0x%08x bytes (%d MB)\n", NF_DEV_SZ, NF_DEV_SZ/0x100000);
        diag_printf("Total blocks:\t 0x%x (%d)\n", NF_BLK_CNT, NF_BLK_CNT);
        diag_printf("Block size:\t 0x%x (%d)\n", NF_BLK_SZ, NF_BLK_SZ);
        diag_printf("Page size:\t 0x%x (%d)\n", NF_PG_SZ, NF_PG_SZ);
        diag_printf("Pages per block: 0x%x (%d)\n", NF_PG_PER_BLK, NF_PG_PER_BLK);

        diag_printf("Bad blocks: \n");

        if (i == 0) {
            diag_printf("  none\n");
        } else {
            diag_printf("\n  Total number of bad blocks: %d\n\n", i);
        }
        return;
    }

    if (!flash_addr_set) {
        diag_printf("Wrong argument\n");
        return;
    }
    if (!flash_len_set) {
        len = NF_BLK_SZ;
    }

    ra &= MXC_NAND_LA_MASK;

    if (ra % NF_BLK_SZ) {
        diag_printf("error: non-block aligned\n");
        return;
    }
    diag_printf("\n");
    for (i = 0;
         i < ((len + NF_BLK_SZ - 1) / NF_BLK_SZ);
         i++) {

        if (nfc_is_badblock(NFC_BLOCK_ALIGN(ra))) {
            diag_printf("block %d at offset 0x%x is bad\n",
                        OFFSET_TO_BLOCK(ra), ra);
            j++;
        }
        ra += NF_BLK_SZ;
    }
    diag_printf("==================================\n");
    diag_printf("Found %d bad block(s) out of %d\n\n", j, i);
}

static void do_nand_cmds(int argc, char *argv[])
{
    struct cmd *cmd;

    if (!mxcnfc_init_ok) {
        diag_printf("\nWarning:NAND flash hasn't been initialized. Try \"factive nand\" first\n\n");
        return;
    }

    if (argc < 2) {
        nand_usage("too few arguments");
        return;
    }
    if ((cmd = cmd_search(__NAND_cmds_TAB__, &__NAND_cmds_TAB_END__,
                          argv[1])) != (struct cmd *)0) {
        (cmd->fun)(argc, argv);
        return;
    }
    nand_usage("unrecognized command");
}

/*!
 * Display a memory region by 16-bit words
 * @param pkt   pointer to the starting address of the memory
 * @param len   byte length of the buffer to be displayed
 */
static void print_pkt_16(u16* pkt, u32 len)
{
    diag_printf("******************** %d bytes********************\n", len);
    u32 i = 0, tempLen = (len + 1) / 2;

    while(tempLen >= 0)
        {
        if(tempLen >= 8) {
            diag_printf("[%03x-%03x] ", i*2, ((i*2)+14));
            diag_printf("%04x %04x %04x %04x %04x %04x %04x %04x\n",
                        pkt[i], pkt[i+1], pkt[i+2], pkt[i+3],
                        pkt[i+4], pkt[i+5], pkt[i+6], pkt[i+7]);
        }
        else {
            if (tempLen == 0) {
                diag_printf("*************************************************\n");
                return;
            }
            diag_printf("[%03x-%03x] ", i*2, ((i*2)+14));
            switch(tempLen) {
                case 1:
                    diag_printf("%04x\n", pkt[i]);
                    break;
                case 2:
                    diag_printf("%04x %04x\n", pkt[i], pkt[i+1]);
                    break;
                case 3:
                    diag_printf("%04x %04x %04x\n", pkt[i], pkt[i+1], pkt[i+2]);
                    break;
                case 4:
                    diag_printf("%04x %04x %04x %04x\n", pkt[i],pkt[i+1], pkt[i+2],pkt[i+3]);
                    break;
                case 5:
                    diag_printf("%04x %04x %04x %04x %04x\n", pkt[i], pkt[i+1], pkt[i+2], pkt[i+3],pkt[i+4]);
                    break;
                case 6:
                    diag_printf("%04x %04x %04x %04x %04x %04x\n", pkt[i], pkt[i+1], pkt[i+2], pkt[i+3],pkt[i+4],
                             pkt[i+5]);
                    break;
                case 7:
                    diag_printf("%04x %04x %04x %04x %04x %04x %04x\n", pkt[i], pkt[i+1], pkt[i+2], pkt[i+3],pkt[i+4],
                             pkt[i+5], pkt[i+6]);
                    break;
            }
        }
        tempLen -= 8;
        i += 8;
    }
}

// addr = starting byte address within NAND flash
static void print_page (u32 addr, bool spare_only)
{
    u32 blk_num = OFFSET_TO_BLOCK(addr), pg_num = OFFSET_TO_PAGE(addr);

    if (spare_only) {
        if(nfc_read_page_sp(addr) != 0) {
            diag_printf("Error %d: uncorrectable. But still printing ...\n", __LINE__);
        }
    } else {
        if(nfc_read_page(addr) != 0) {
            diag_printf("Error %d: uncorrectable. But still printing ...\n", __LINE__);
        }
    }

    diag_printf("\n============ Printing block(%d) page(%d)  ==============\n",
                blk_num, pg_num);

    diag_printf("<<<<<<<<< spare area >>>>>>>>>\n");
    print_pkt_16((u16*)(NAND_SPAR_BUF0), 16);

    if (!spare_only) {
        diag_printf("<<<<<<<<< main area >>>>>>>>>\n");
        print_pkt_16((u16*)(NAND_MAIN_BUF0), NF_PG_SZ);
    }

    diag_printf("\n");
}
