/* With non GPL files, use following license */
/*
 * Copyright 2008 Sony Corporation.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions, and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 *
 * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 */
/* Otherwise with GPL files, use following license */
/*
 * Copyright 2008 Sony Corporation.
 *
 * This program 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 of the  License.
 *
 * THIS  SOFTWARE  IS PROVIDED   ``AS  IS'' AND   ANY  EXPRESS OR IMPLIED
 * WARRANTIES,   INCLUDING, BUT NOT  LIMITED  TO, THE IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.  IN
 * NO  EVENT  SHALL   THE AUTHOR  BE    LIABLE FOR ANY   DIRECT, INDIRECT,
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
 * NOT LIMITED   TO, PROCUREMENT OF  SUBSTITUTE GOODS  OR SERVICES; LOSS OF
 * USE, DATA,  OR PROFITS; OR  BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
 * ANY THEORY OF LIABILITY, WHETHER IN  CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
 * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 * You should have received a copy of the  GNU General Public License along
 * with this program; if not, write  to the Free Software Foundation, Inc.,
 * 675 Mass Ave, Cambridge, MA 02139, USA.
 */
 /*
 *  Copyright (C) 2007 - 2008 TOSHIBA Corporation, All Rights Reserved.
 *
 *! \file lba_nand.c
 *  \brief <b> This source file contains the Block Driver OS 
 *             specific interface implementation for the LBA-NAND device
 *             </b>
 ** $Name: ALPHA_1_0_2_LBA_Linux_IMX31 $
 *  $Id: lba_nand.c,v 1.3 2008/04/03 15:03:14 rowen Exp $
 *
 */
//==========================================================================
//
//      mxc_nfc_lba.c
//
//      Flash programming to support LBA-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):
// Contributors:
// Date:         2008-01-30
// Purpose:
// Description:
// Copyright:    2008 Kinpo Electronics, Inc.
//
//####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>

#define MXC_NFC_LBA_C /* avoid unused warning */
#include <cyg/io/mxc_nfc.h>

#include "tinyfat.h"

/* #define DEBUG_LBA */
/* #define PROGRAMER_TEST */

/* LBA-NAND definitions */
#define u8  unsigned char
#define u16 unsigned short
#define u32 unsigned int
#define bool int

#ifndef false
#define false 0
#endif /* !false */

#ifndef true
#define true  (!false)
#endif /* !true */

#ifndef NULL
#define NULL 0
#endif /* !NULL */

#define NFC_DATA_RAMBUF_0               0
#define NFC_DATA_RAMBUF_1               1
#define NFC_DATA_RAMBUF_2               2
#define NFC_DATA_RAMBUF_3               3

#define FLUSH_CMD                       (0xF9)
#define TERMINATE_CMD                   (0xFB)
#define READRETRY_CMD                   (0x31)
#define RELOAD_CMD                      (0xFD)
#define RESET_CMD                       (0xFF)
#define UPDATERST_CMD                   (0xFA)
#define MODECHG_CMD                     (0xFC)
#define END_CMD                         (0xFE)
#define READNEXT_CMD                    (0xF8)
#define NULL_CMD                        (0x00)
#define READID_CMD0                     (0x90)
#define READID_CMD1                     (0x92)
#define READSTS_CMD0                    (0x70)
#define READSTS_CMD1                    (0x71)
#define READ_1CYL_CMD                   (0x00)
#define READ_2CYL_CMD                   (0x30)

#define WRITE_1CYL_CMD                  (0x80)
#define WRITE_2CYL_CMD0                 (0x10)
#define WRITE_2CYL_CMD1                 (0x15)

#define READID_ADDR                     (0x00)

#define HIGH_SPEED_ENABLE               (true)
#define HIGH_SPEED_DISABLE              (false)

#define LBA_OK                          1
#define LBA_NG                          0

#define LBA_ID_1                        0
#define LBA_ID_2                        1

/* For LBA-NAND MAL */
#define TRANS_PROTOCOL_1                0
#define TRANS_PROTOCOL_2                1
#define PT_1_512	0x21
#define PT_1_4096	0x24
#define PT_2		0x06
u8 cur_pt1 = 0;

#define ECC_DIS                         0
#define ECC_EN                          0x80
#define ECC_CORRECT                     0xc0
#define SEC_SIZE_512                    0
#define SEC_SIZE_528                    0x20

#define MULTI_SEC_1                     1
#define MULTI_SEC_4                     2
#define MULTI_SEC_8                     4

#define PNR_MODE                        0  /* Pure NAND Read Mode       */
#define MDA_MODE                        1  /* Multimedia Data Partition */
#define VFA_MODE                        2  /* Vendor Firmware Partition */
#define PNA_MODE                        3  /* Pure NAND Partition       */

/* status after &= LBA_MODE_STS_MASK */
#define PNR_MODE_STS                    0x00
#define MDA_MODE_STS                    0x06
#define VFA_MODE_STS                    0x04
#define PNA_MODE_STS                    0x02
#define LBA_MODE_STS_MASK               0x06

#define PNAND_SIZE                      2112

#define SB_SEC_SIZE                     528
#define LB_SEC_SIZE                     2112
#define LBA_READ_TYPE_A                 0
#define LBA_READ_TYPE_B                 2
#define LBA_READ_TYPE_C                 3
#define LBA_WRITE_TYPE_A                0
#define LBA_WRITE_TYPE_B                4

#define LBA_ID_MODE0                    0
#define LBA_ID_MODE1                    1
#define LBA_STS_MODE0                   0
#define LBA_STS_MODE1                   1


#define KERNEL_SECTOR_SIZE              512


typedef struct cmd_data {
    u32 cnt;
    u8  data[5];
} cmd_data_t;

typedef struct mal_cfg {
    u32 curmode;   /* current mode(PNA/MDA/VFA/PNR) */
    u32 rwmode;    /* read mode/write mode          */
    u32 secsize;   /* sector size(512/528byte)      */
    u32 multisec;  /* 1,4,8                         */
    u32 eccmode;   /* no check/check/correct        */
} mal_cfg_t;

typedef struct {
    union {
        u16 word;
        u8 byte[2];
    };
} u16_buf_t;

#define _LBA_ID_NUM		5

#define NFC_66_5MHZ	1
#define NFC_44_3MHZ	2
#define NFC_33_2MHZ	3
#define NFC_26_6MHZ	4
#define NFC_22_1MHZ	5
#define NFC_19_0MHZ	6


#define TOSHIBA_LBA_ID1		{0x98,0xdc,0x00,0x15,0x00}
#define TOSHIBA_LBA_ID2_1G	{0x98,0x21,0x00,0x55,0xaa}
#define TOSHIBA_LBA_ID2_2G	{0x98,0x21,0x01,0x55,0xaa}
#define TOSHIBA_LBA_ID2_4G	{0x98,0x21,0x02,0x55,0xaa}


u8 _lba_id[_LBA_ID_NUM][5] = {
		TOSHIBA_LBA_ID1,
		TOSHIBA_LBA_ID2_1G,
		TOSHIBA_LBA_ID2_2G,
		TOSHIBA_LBA_ID2_4G,		
};
bool _lba_init_ok = false;
static mal_cfg_t _mal_mode;

/* multi boot */
enum
{
	normal_boot = 1,
	recover_boot,
	diagnostic_boot,
};

/* multi boot config */
int boot_config = recover_boot;

/* external functions */
extern void nand_usage(char *why);

/* external variables */
extern void *NAND_cmds;

/* LBA-NAND PAL function definitions */
static u16  lba_pal_read_data(u8 cmd1, u8 cmd2, cmd_data_t *cmd_data,
                              u8 *buf, u32 data_count, u16 sector_size);
static u16  lba_pal_set_cfg(u8 cmd1, u8 cmd2, cmd_data_t *cmd_data,
                            u8 *buf, u16 data_count);
static u16  lba_pal_write_data(u8 cmd1, u8 cmd2, cmd_data_t *cmd_data,
                               u8 *buf, u32 data_count, u16 sector_size);
static u16  lba_pal_set_ctrl(u8 cmd, u8 *buf, u16 data_count);
static u16  lba_pal_read_info(u8 cmd, cmd_data_t *cmd_data,
                              u8 *buf, u16 data_count);
static void lba_pal_lba_is_ready(int usec);

/* LBA-NAND MAL function definitions */
static void lba_mal_write_boot_code(u8 *data_buf, u32 page_addr, bool ecc_on);
static void lba_mal_read_boot_code(u8 *data_buf, u32 page_addr, bool ecc_on);
static void lba_mal_flush_cache(void);
static void lba_mal_status_read(u8 mode, u8 *data);
static void lba_mal_status_init(void);
static void lba_mal_read_id(u8 mode, u8 *data);
static int  lba_mal_mode_change(u8 mode, u8 *password);
static void lba_mal_speed_mode(bool is_high);
static int  lba_mal_set_transfer_protocol(u8 mode, u8 data);
static int  lba_mal_get_transfer_protocol(u8 mode, u8 *data);
static void lba_mal_fw_reload(void);
static void lba_mal_set_vfa_unit(u32 sector_unit, u16 size_type);
static void lba_mal_get_vfa_unit(u32 *num_secotr);
static void lba_mal_get_mda_unit(u32 *low_byte, u8 *hi_byte);
static void lba_mal_into_bootcodemaintenance(void);
static void lba_mal_bulk_write(u32 sector, u16 len, u8 *mem);
void lba_mal_write_page(u8 *buf, u32 page_addr);
void lba_mal_read_page(u8 *buf, u32 page_addr);

/* utility functions */
static void get_read_page(u16 addr, u8 *buf, u16 len);
static void set_write_page(u16 addr, u8 *buf, u16 len);

#if 1 /* For furture use, comment out prevent compile warning. */
static void lba_nfc_clock(u32 div);
#endif

/* public id tools */
bool lba_id_compare(u8 *id);
bool lba_get_bat_cal_value(u8 *valueStr,u8 len);

/* public boot config tools */
void lba_config_move(u32 ori_size,u32 ori_sector,bool write2ori,int cfg_num);

/* public product level */
bool read_level(u8 *level,u8 len);

u16 lba_pal_read_data(u8 cmd1, u8 cmd2, cmd_data_t *cmd_data,
                      u8 *buf, u32 data_count, u16 sector_size)
{
    u16 i;
    u16_buf_t u16_buf;
    u8 *ptr_data = buf;
    volatile u16 *sur_data = (volatile u16 *)(NAND_MAIN_BUF0);

    /* Send command */
    NFC_CMD_INPUT(cmd1);

    /* Send address */
    for (i = 0; i < cmd_data->cnt; ++i) {
        NFC_ADDR_INPUT(cmd_data->data[i]);
    }

    if (cmd2 != 0) {
        NFC_CMD_INPUT(cmd2);
    }

    /* Start HW transfer */
    NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_0, FDO_PAGE_SPARE, 0);

    /* Start read from buffer */
    u16_buf.word = *sur_data;
    for (i = 0; i < data_count; i += 2) {
        *ptr_data = u16_buf.byte[0];
        ++ptr_data;
        *ptr_data = u16_buf.byte[1];
        ++ptr_data;

        ++sur_data;
        u16_buf.word = *sur_data;
    }

    return LBA_OK;
}

u16 lba_pal_set_cfg(u8 cmd1, u8 cmd2, cmd_data_t *cmd_data,
                    u8 *buf, u16 data_count)
{
    u16 i;
    u16_buf_t u16_buf;
    u8 *ptr_data = buf;
    volatile u16 *sur_data = (volatile u16 *)(NAND_MAIN_BUF0);

    /* Send command */
    NFC_CMD_INPUT(cmd1);

    /* Send address */
    for (i = 0; i < cmd_data->cnt; ++i) {
        NFC_ADDR_INPUT(cmd_data->data[i]);
    }

    NFC_CMD_INPUT(cmd2);

    if (buf && data_count > 0) {
        /* Start HW transfer */
         NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_0, FDO_PAGE_SPARE, 0);

        /* Start read from buffer */
        u16_buf.word = *sur_data;

        if (data_count && 0x1) {
            for (i = 0; i < data_count - 1; i += 2) {
                *ptr_data = u16_buf.byte[0];
                ++ptr_data;
                *ptr_data = u16_buf.byte[1];
                ++ptr_data;

                ++sur_data;
                u16_buf.word = *sur_data;
            }

            *ptr_data = u16_buf.byte[0];
        } else {
            for (i = 0; i < data_count; i += 2) {
                *ptr_data = u16_buf.byte[0];
                ++ptr_data;
                *ptr_data = u16_buf.byte[1];
                ++ptr_data;

                ++sur_data;
                u16_buf.word = *sur_data;
            }
        }
    }

    return LBA_OK;
}

u16 lba_pal_write_data(u8 cmd1, u8 cmd2, cmd_data_t *cmd_data,
                       u8 *buf, u32 data_count, u16 sector_size)
{
    u16 i;
    u16_buf_t u16_buf;
    u8 *sur_data = buf;
    volatile u16 *ptr_data = (volatile u16 *)(NAND_MAIN_BUF0);

    /* Start write to buffer */
    for (i = 0; i < data_count; i += 2) {
        u16_buf.byte[0] = *sur_data;
        ++sur_data;
        u16_buf.byte[1] = *sur_data;
        ++sur_data;

        *ptr_data = u16_buf.word;
        ++ptr_data;
    }

    /* Send command */
    NFC_CMD_INPUT(cmd1);

    /* Send address */
    for (i = 0; i < cmd_data->cnt; ++i) {
        NFC_ADDR_INPUT(cmd_data->data[i]);
    }

    /* Start HW transfer */
    NFC_DATA_INPUT(NFC_DATA_RAMBUF_0, NFC_MAIN_ONLY, 0);

    /* Send command */
    NFC_CMD_INPUT(cmd2);

    return LBA_OK;
}

u16 lba_pal_set_ctrl(u8 cmd, u8 *buf, u16 data_count)
{
    NFC_CMD_INPUT(cmd);

    return LBA_OK;
}

u16 lba_pal_read_info(u8 cmd, cmd_data_t *cmd_data,
                      u8 *buf, u16 data_count)
{
    u16 i;
    u16_buf_t u16_buf;
    u8 *ptr_data = buf;
    volatile u16 *sur_data = (volatile u16 *)(NAND_MAIN_BUF0);

    /* Send command */
    NFC_CMD_INPUT(cmd);

    /* Send address */
    for (i = 0; i <cmd_data->cnt; ++i) {
        NFC_ADDR_INPUT(cmd_data->data[i]);
    }

    if (buf && data_count > 0) {
        /* Start HW transfer */
        NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_0, FDO_PAGE_SPARE, 0);

        /* Start read from buffer */
        u16_buf.word = *sur_data;

        if (data_count && 0x1) {
            for (i = 0; i < data_count - 1; i += 2) {
                *ptr_data = u16_buf.byte[0];
                ++ptr_data;
                *ptr_data = u16_buf.byte[1];
                ++ptr_data;

                ++sur_data;
                u16_buf.word = *sur_data;
            }

            *ptr_data = u16_buf.byte[0];
        } else {
            for (i = 0; i < data_count;i += 2) {
                *ptr_data = u16_buf.byte[0];
                ++ptr_data;
                *ptr_data = u16_buf.byte[1];
                ++ptr_data;

                ++sur_data;
                u16_buf.word = *sur_data;
            }
        }
    }

    return LBA_OK;
}

void lba_pal_lba_is_ready(int usec)
{
    u8 data;
    cmd_data_t cmd_data;

    cmd_data.cnt = 0;

    while (--usec > 0) {
        lba_pal_read_info(READSTS_CMD0, &cmd_data, &data, 1);
        if (data & 0x40)
            break;
    }

    if (usec <= 0) {
        diag_printf("lba-nand not ready, into for loop\n");
        for(;;);
    }
}

void lba_mal_write_boot_code(u8 *data_buf, u32 page_addr, bool ecc_on)
{
    u16 i;
    u16_buf_t u16_buf;
    u8 *sur_data = data_buf;
    volatile u16 *ptr_data = (volatile u16 *)(NAND_MAIN_BUF0);

#ifdef PROGRAMER_TEST
	volatile u16 *s = (volatile u16 *)(NAND_SPAR_BUF0);

    for (i = 0; i < 64; i += 2) {
        *s = 0x0;
		s++;
    }
#endif

    for (i = 0; i < 2048; i += 2) {
        u16_buf.byte[0] = *sur_data;
        ++sur_data;
        u16_buf.byte[1] = *sur_data;
        ++sur_data;

        *ptr_data = u16_buf.word;
        ++ptr_data;
    }

    NFC_CMD_INPUT(0x80);

    NFC_ADDR_INPUT(0x00);
    NFC_ADDR_INPUT(0x00);
    NFC_ADDR_INPUT(page_addr);
    NFC_ADDR_INPUT(0x00);
    NFC_ADDR_INPUT(0x00);

    NFC_DATA_INPUT(NFC_DATA_RAMBUF_0, NFC_MAIN_ONLY, ecc_on);
	NFC_DATA_INPUT(NFC_DATA_RAMBUF_1, NFC_MAIN_ONLY, ecc_on);
	NFC_DATA_INPUT(NFC_DATA_RAMBUF_2, NFC_MAIN_ONLY, ecc_on);
	NFC_DATA_INPUT(NFC_DATA_RAMBUF_3, NFC_MAIN_ONLY, ecc_on);
	
    //NFC_DATA_INPUT_2k(NFC_DATA_RAMBUF_1);
    //NFC_DATA_INPUT_2k(NFC_DATA_RAMBUF_2);
    //NFC_DATA_INPUT_2k(NFC_DATA_RAMBUF_3);

    NFC_CMD_INPUT(0x10);
}

void lba_mal_read_boot_code(u8 *data_buf, u32 page_addr, bool ecc_on)
{
    u16 i;
    u16_buf_t u16_buf;
    u8 *ptr_data = data_buf;
    volatile u16 *sur_data = (volatile u16 *)(NAND_MAIN_BUF0);
#ifdef PROGRAMER_TEST
	volatile u16 *s = (volatile u16 *)(NAND_SPAR_BUF0);
#endif

    NFC_CMD_INPUT(0x00);

    NFC_ADDR_INPUT(0x00);
    NFC_ADDR_INPUT(0x00);
    NFC_ADDR_INPUT(page_addr);
    NFC_ADDR_INPUT(0x00);
    NFC_ADDR_INPUT(0x00);

    NFC_CMD_INPUT(0x30);

    NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_0, FDO_PAGE_SPARE, ecc_on);
    NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_1, FDO_PAGE_SPARE, ecc_on);
    NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_2, FDO_PAGE_SPARE, ecc_on);
    NFC_DATA_OUTPUT(NFC_DATA_RAMBUF_3, FDO_PAGE_SPARE, ecc_on);

    u16_buf.word = *sur_data;
    for (i = 0; i < 2048; i += 2) {
        *ptr_data = u16_buf.byte[0];
        ++ptr_data;
        *ptr_data = u16_buf.byte[1];
        ++ptr_data;

        ++sur_data;
        u16_buf.word = *sur_data;
    }

#ifdef PROGRAMER_TEST
	if (ecc_on) {
		diag_printf("%04X-%04X-%04X-%04X= %04X-%04X-%04X-%04X= %04X-%04X-%04X-%04X= %04X-%04X-%04X-%04X= ",
			*(s+0),*(s+1),*(s+2),*(s+3),*(s+4),*(s+5),*(s+6),*(s+7),
			*(s+8),*(s+9),*(s+10),*(s+11),*(s+12),*(s+13),*(s+14),*(s+15));
		s+=16;
		diag_printf("\n");
		diag_printf("%04X-%04X-%04X-%04X= %04X-%04X-%04X-%04X= %04X-%04X-%04X-%04X= %04X-%04X-%04X-%04X= ",
			*(s+0),*(s+1),*(s+2),*(s+3),*(s+4),*(s+5),*(s+6),*(s+7),
			*(s+8),*(s+9),*(s+10),*(s+11),*(s+12),*(s+13),*(s+14),*(s+15));
		s+=16;
		diag_printf("\n");

		diag_printf("\n\n\n");
	}
#endif
}

void lba_mal_flush_cache(void)
{
    lba_pal_lba_is_ready(200);
    lba_pal_set_ctrl(FLUSH_CMD, NULL, 0);
}

void lba_mal_status_read(u8 mode, u8 *data)
{
    u8 cmd;
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);
    cmd_data.cnt = 0;
    cmd = (mode == LBA_STS_MODE0) ? READSTS_CMD0 : READSTS_CMD1;
    lba_pal_read_info(cmd, &cmd_data, data, 1);
}

void lba_mal_status_init(void)
{
    u8 status;

    lba_pal_lba_is_ready(200);

    /* After power on, LBA-NAND is in PNR mode */
    lba_mal_status_read(READSTS_CMD1, &status);

    status &= LBA_MODE_STS_MASK;
    if (MDA_MODE_STS == status) {
        diag_printf("INFO: LBA_INIT mode=MDA_MODE\n");
        _mal_mode.curmode = MDA_MODE;
    } else if (VFA_MODE_STS == status) {
        diag_printf("INFO: LBA_INIT mode=VFA_MODE\n");
        _mal_mode.curmode = VFA_MODE;
    } else if (PNA_MODE_STS == status) {
        diag_printf("INFO: LBA_INIT mode=PNA_MODE\n");
        _mal_mode.curmode = PNA_MODE;
    } else {
        diag_printf("INFO: LBA_INIT mode=PNR_MODE\n");
        _mal_mode.curmode = PNR_MODE;
    }

    /* Not used in PNR mode */
    _mal_mode.rwmode   = LBA_READ_TYPE_A;
    _mal_mode.secsize  = 0;
    _mal_mode.multisec = 0;
    _mal_mode.eccmode  = 0;
}

void lba_mal_read_id(u8 mode, u8 *data)
{
    u8 cmd;
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    cmd_data.cnt=1;
    cmd_data.data[0] = READID_ADDR;

    cmd = (mode == LBA_ID_MODE0) ? READID_CMD0 : READID_CMD1;

    lba_pal_read_info(cmd, &cmd_data, data, 5);
}

int lba_mal_mode_change(u8 mode, u8 *password)
{
    u8 status;
    u16 ret = LBA_OK;
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    if (mode == _mal_mode.curmode) {
        return LBA_OK;
    }

    switch (_mal_mode.curmode) {
    case PNR_MODE:
        if (MDA_MODE == mode) {
            ret = lba_pal_set_ctrl(MODECHG_CMD, NULL, 0);
        } else {
            diag_printf("LBA_MODE_CHANGE-2-- error\n");
            return LBA_NG;
        }
        break;

    case MDA_MODE:
    case VFA_MODE:
    case PNA_MODE:
        if (PNR_MODE == mode)
            return LBA_NG;

        memset(&cmd_data, 0, sizeof(cmd_data));
        if (MDA_MODE == mode) {
            ret = lba_pal_set_ctrl(MODECHG_CMD, NULL, 0);
        } else if(mode==VFA_MODE) {
            cmd_data.data[0] = 0xBE;
            cmd_data.data[1] = password[0];
            cmd_data.data[2] = password[1];
            cmd_data.cnt = 5;
            ret = lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
        } else {
            cmd_data.data[0] = 0xBF;
            cmd_data.cnt = 3;
            ret = lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
        }
        break;

    default:
        return LBA_NG;
    }

    if (LBA_OK != ret) {
        diag_printf("LBA_MODE_CHANGE-3-- error\n");
        return ret;
    }

    lba_pal_lba_is_ready(200);

    /* Read status to check whether change is successful. */
    lba_mal_status_read(READSTS_CMD1, &status);

    status &= LBA_MODE_STS_MASK;
    switch (mode) {
    case MDA_MODE:
        if (MDA_MODE_STS != status) {
            diag_printf("LBA_MODE_CHANGE-4---- error =0x%X\n",status);
            return LBA_NG;
        }
        break;

    case VFA_MODE:
        if (VFA_MODE_STS != status) {
            diag_printf("LBA_MODE_CHANGE-5---- error =0x%X\n",status);
            return LBA_NG;
        }
        break;

    case PNA_MODE:
        if (PNA_MODE_STS != status) {
            diag_printf("LBA_MODE_CHANGE-6---- error =0x%X\n",status);
            return LBA_NG;
        }
        break;

    default:
        return LBA_NG;
    }

    _mal_mode.curmode = mode;
    return LBA_OK;
}

void lba_mal_speed_mode(bool is_high)
{
    u8 status;
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    memset(&cmd_data, 0, sizeof(cmd_data));
    if (HIGH_SPEED_DISABLE == is_high) {
        /* High speed write mode disable */
        cmd_data.data[0] = 0xBD;
    } else {
        /* High speed write mode enable */
        cmd_data.data[0] = 0xBC;
    }
    cmd_data.cnt = 5;

    lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    lba_pal_lba_is_ready(200);
    lba_mal_status_read(READSTS_CMD1, &status);
}

void lba_mal_write_page(u8 *buf, u32 page_addr)
{
    cmd_data_t cmd_data;
    u8 cmd1 = WRITE_1CYL_CMD;
    u8 cmd2 = WRITE_2CYL_CMD0;

    cmd_data.data[0] = 1;                         /* count 0   */
    cmd_data.data[1] = 0;                         /* count 1   */
    cmd_data.data[2] = (page_addr >>  0) & 0xFF;  /* address 0 */
    cmd_data.data[3] = (page_addr >>  8) & 0xFF;  /* address 1 */
    cmd_data.data[4] = (page_addr >> 16) & 0xFF;  /* address 2 */
    cmd_data.cnt = 5;

    lba_pal_write_data(cmd1, cmd2, &cmd_data, buf, 512, 0);
}

void lba_mal_read_page(u8 *buf, u32 page_addr)
{
    cmd_data_t cmd_data;
    u8 cmd1 = READ_1CYL_CMD;
    u8 cmd2 = READ_2CYL_CMD;

    cmd_data.data[0] = 1;                         /* count 0   */
    cmd_data.data[1] = 0;                         /* count 1   */
    cmd_data.data[2] = (page_addr >>  0) & 0xFF;  /* address 0 */
    cmd_data.data[3] = (page_addr >>  8) & 0xFF;  /* address 1 */
    cmd_data.data[4] = (page_addr >> 16) & 0xFF;  /* address 2 */
    cmd_data.cnt = 5;

    lba_pal_read_data(cmd1, cmd2, &cmd_data, buf, 512, 0);
}

void lba_mal_bulk_read(u32 sector, u16 len, u8 *mem)
{
    u16 dlen;
	u8 jmp;

	if((len&0x7) > 0) {
		lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1,PT_1_512);
		dlen=512;
		jmp=1;
	} else {
		lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1,PT_1_4096);
		dlen=2048;
		jmp=8;	
	}

	NFC_CMD_INPUT(0x00);
	NFC_ADDR_INPUT(len & 0xFF);
	NFC_ADDR_INPUT((len >> 8) & 0xFF);
	NFC_ADDR_INPUT(sector & 0xFF);
	NFC_ADDR_INPUT((sector >>  8) & 0xFF);
	NFC_ADDR_INPUT((sector >> 16) & 0xFF);
	NFC_CMD_INPUT(0x30);

	if((len&0x7) > 0) {
		while(len) {
			NFC_DATA_OUTPUT(0, FDO_PAGE_SPARE, 0);
		    get_read_page(0, mem, dlen);
			NFC_CMD_INPUT(0xF8);
			mem+=dlen;
			len-=jmp;
		}
	} else {
		while(len) {
			NFC_DATA_OUTPUT(0, FDO_PAGE_SPARE, 0);
			NFC_DATA_OUTPUT(1, FDO_PAGE_SPARE, 0);
			NFC_DATA_OUTPUT(2, FDO_PAGE_SPARE, 0);
			NFC_DATA_OUTPUT(3, FDO_PAGE_SPARE, 0);
		    get_read_page(0, mem, dlen);
			mem+=dlen;
        	NFC_DATA_OUTPUT(0, FDO_PAGE_SPARE, 0);
			NFC_DATA_OUTPUT(1, FDO_PAGE_SPARE, 0);
			NFC_DATA_OUTPUT(2, FDO_PAGE_SPARE, 0);
			NFC_DATA_OUTPUT(3, FDO_PAGE_SPARE, 0);
        	get_read_page(0, mem, dlen);
			NFC_CMD_INPUT(0xF8);
			mem+=dlen;
			len-=jmp;
		}
	}
}

void lba_mal_bulk_write(u32 sector, u16 len, u8 *mem)
{
    u16 dlen;
	u8 jmp;

	if((len&0x7) > 0) {
		lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1,PT_1_512);
		dlen=512;
		jmp=1;
	} else {
		lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1,PT_1_4096);
		dlen=2048;
		jmp=8;	
	}

    NFC_CMD_INPUT(0x80);
    NFC_ADDR_INPUT(len & 0xFF);
    NFC_ADDR_INPUT((len >> 8) & 0xFF);
    NFC_ADDR_INPUT(sector & 0xFF);
    NFC_ADDR_INPUT((sector >>  8) & 0xFF);
    NFC_ADDR_INPUT((sector >> 16) & 0xFF);

	if((len&0x7) > 0) {
		
		len-=jmp;
		
		while(len) {
			set_write_page(0, mem, dlen);
			NFC_DATA_INPUT(0, NFC_MAIN_ONLY, 0);
			NFC_CMD_INPUT(0x15);
			NFC_CMD_INPUT(0x80);
			mem+=dlen;
			len-=jmp;
		}

		set_write_page(0, mem, dlen);
		NFC_DATA_INPUT(0, NFC_MAIN_ONLY, 0);
		NFC_CMD_INPUT(0x10);
	} else {

		len-=jmp;
		
		while(len) {
			set_write_page(0, mem, dlen);
			NFC_DATA_INPUT(0, NFC_MAIN_ONLY, 0);
			NFC_DATA_INPUT(1, NFC_MAIN_ONLY, 0);
			NFC_DATA_INPUT(2, NFC_MAIN_ONLY, 0);
			NFC_DATA_INPUT(3, NFC_MAIN_ONLY, 0);
			mem+=dlen;
			set_write_page(0, mem, dlen);
			NFC_DATA_INPUT(0, NFC_MAIN_ONLY, 0);
			NFC_DATA_INPUT(1, NFC_MAIN_ONLY, 0);
			NFC_DATA_INPUT(2, NFC_MAIN_ONLY, 0);
			NFC_DATA_INPUT(3, NFC_MAIN_ONLY, 0);	
			mem+=dlen;
			NFC_CMD_INPUT(0x15);
			NFC_CMD_INPUT(0x80);
			len-=jmp;
		}
		
		set_write_page(0, mem, dlen);
		NFC_DATA_INPUT(0, NFC_MAIN_ONLY, 0);
		NFC_DATA_INPUT(1, NFC_MAIN_ONLY, 0);
		NFC_DATA_INPUT(2, NFC_MAIN_ONLY, 0);
		NFC_DATA_INPUT(3, NFC_MAIN_ONLY, 0);
		mem+=dlen;
		set_write_page(0, mem, dlen);
		NFC_DATA_INPUT(0, NFC_MAIN_ONLY, 0);
		NFC_DATA_INPUT(1, NFC_MAIN_ONLY, 0);
		NFC_DATA_INPUT(2, NFC_MAIN_ONLY, 0);
		NFC_DATA_INPUT(3, NFC_MAIN_ONLY, 0);	
		mem+=dlen;
		NFC_CMD_INPUT(0x10);
	}
}

int lba_mal_set_transfer_protocol(u8 mode, u8 data)
{
    u16 ret;
    cmd_data_t cmd_data;

	if(cur_pt1==data)
		return LBA_OK;
	else
		cur_pt1 = data;
	
    lba_pal_lba_is_ready(200);

    memset(&cmd_data, 0, sizeof(cmd_data));
    if (TRANS_PROTOCOL_1 == mode) {
        cmd_data.data[0] = 0xA2;
    } else if(TRANS_PROTOCOL_2 == mode) {
        cmd_data.data[0] = 0xA3;
    } else {
        return LBA_NG;
    }

    cmd_data.data[1] = data;
    cmd_data.cnt = 5;

    ret = lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    if (LBA_OK == ret) {
        if(TRANS_PROTOCOL_1 == mode) {
            _mal_mode.secsize = (data & 0x20) ? 528 : 512;
            _mal_mode.multisec = (data & 1) > 0 ? 1 : (data & 0x02) ? 4 : 8;
            _mal_mode.eccmode = data >> 6;
        } else {
            _mal_mode.rwmode = data;
        }
    }

    return ret;
}

int lba_mal_get_transfer_protocol(u8 mode, u8 *data)
{
    u16 ret;
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    memset(&cmd_data, 0, sizeof(cmd_data));
    cmd_data.data[0] = TRANS_PROTOCOL_1 == mode ? 0xB2 : 0xB3;
    cmd_data.cnt = 5;

    ret = lba_pal_set_cfg(0x00, 0x57, &cmd_data, data, 1);

    return ret;
}

void lba_mal_fw_reload(void)
{
    lba_pal_lba_is_ready(200);
    lba_pal_set_ctrl(RELOAD_CMD, NULL, 0);
    _mal_mode.curmode = PNR_MODE;
}

void lba_mal_set_vfa_unit(u32 sector_unit, u16 size_type)
{
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    if (8 == size_type) {
        cmd_data.data[0] = 0x22;
        cmd_data.data[1] = 0x00;
        cmd_data.data[2] = 0x40;
        cmd_data.data[3] = 0xFF;
        cmd_data.data[4] = 0xBF;
        cmd_data.cnt = 5;
        lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    } else if (16 == size_type) {
        cmd_data.data[0] = 0x22;
        cmd_data.data[1] = 0x00;
        cmd_data.data[2] = 0x80;
        cmd_data.data[3] = 0xFF;
        cmd_data.data[4] = 0x7F;
        cmd_data.cnt = 5;
        lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    } else if (32 == size_type) {
        cmd_data.data[0] = 0x22;
        cmd_data.data[1] = 0x00;
        cmd_data.data[2] = 0x00;
        cmd_data.data[3] = 0xFF;
        cmd_data.data[4] = 0xFF;
        cmd_data.cnt = 5;
        lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    } else if (64 == size_type) {
        cmd_data.data[0] = 0x24;
        cmd_data.data[1] = 0x11;
        cmd_data.data[2] = 0x04;
        cmd_data.data[3] = 0xEE;
        cmd_data.data[4] = 0xFB;
        cmd_data.cnt = 5;
        lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    } else if (128 == size_type) {
        cmd_data.data[0] = 0x24;
        cmd_data.data[1] = 0x11;
        cmd_data.data[2] = 0x08;
        cmd_data.data[3] = 0xEE;
        cmd_data.data[4] = 0xF7;
        cmd_data.cnt = 5;
        lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    } else if (256 == size_type) {
        cmd_data.data[0] = 0x24;
        cmd_data.data[1] = 0x11;
        cmd_data.data[2] = 0x10;
        cmd_data.data[3] = 0xEE;
        cmd_data.data[4] = 0xEF;
        cmd_data.cnt = 5;
        lba_pal_set_cfg(0x00, 0x57, &cmd_data, NULL, 0);
    }
}

void lba_mal_get_vfa_unit(u32 *num_sector)
{
    u16 tmp;
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    memset(&cmd_data, 0, sizeof(cmd_data));
    cmd_data.data[0] = 0xB5;
    cmd_data.cnt = 5;
    lba_pal_set_cfg(0x00, 0x57, &cmd_data, (u8*)(&tmp), 2);
    *num_sector = (u32)tmp;
}

void lba_mal_get_mda_unit(u32 *low_byte, u8 *hi_byte)
{
    u8 tmp[5];
    cmd_data_t cmd_data;

    lba_pal_lba_is_ready(200);

    memset(&cmd_data, 0, sizeof(cmd_data));
    cmd_data.data[0] = 0xB0;
    cmd_data.cnt = 5;
    lba_pal_set_cfg(0x00, 0x57, &cmd_data, tmp, 5);

    *low_byte =  (u32)tmp[0]       |
                 (u32)tmp[1] <<  8 |
                 (u32)tmp[2] << 16 |
                 (u32)tmp[3] << 24 ;
    *hi_byte = tmp[4];
}

void lba_mal_into_bootcodemaintenance(void)
{
    cmd_data_t cmd_data;

    cmd_data.data[0] = 0x55;
    cmd_data.data[1] = 0xAA;
    cmd_data.data[2] = 0x55;
    cmd_data.data[3] = 0xFC;
    cmd_data.data[4] = 0xAA;
    cmd_data.cnt = 5;

    lba_pal_set_cfg(0x00, 0x30, &cmd_data, NULL, 0);
}

int lba_hwr_init(void)
{
    u8 status;
    u8 buf[6];
    u8 password[2] = {0xFF, 0xFF};
    u32 num_sector;
    u32 lo_byte;
    u8 hi_byte;
    int ret;
	
    lba_pal_lba_is_ready(200);
    lba_mal_status_init();

    ret = lba_mal_mode_change(MDA_MODE, password);
    if (LBA_OK != ret) {
        return FLASH_ERR_HWR;
    }
#ifdef DEBUG_LBA
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (MDA_MODE_STS != (status & LBA_MODE_STS_MASK)) {
        diag_printf("error1 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }
#endif /* DEBUG_LBA */

    /* ID_1_Read */
    memset(buf, 0xBB, sizeof(buf));
    lba_mal_read_id(LBA_ID_MODE0, buf);
    if (lba_id_compare(buf) == false) {
        diag_printf("LBA-NAND id-1 FAIL\n");
        return FLASH_ERR_HWR;
    } else {
        diag_printf("LBA-NAND ID1=0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n",
            buf[0], buf[1], buf[2], buf[3], buf[4]);
    }

    /* ID_2_Read */
    memset(buf, 0xBB, sizeof(buf));
    lba_mal_read_id(LBA_ID_MODE1, buf);
    if (lba_id_compare(buf) == false) {
        diag_printf("LBA-NAND id-2 FAIL\n");
        return FLASH_ERR_HWR;
    } else {
        diag_printf("LBA-NAND ID2=0x%02X 0x%02X 0x%02X 0x%02X 0x%02X\n",
            buf[0], buf[1], buf[2], buf[3], buf[4]);
    }

    ret = lba_mal_mode_change(VFA_MODE, password);
    if (LBA_OK != ret) {
        return FLASH_ERR_HWR;
    }
		
#ifdef DEBUG_LBA
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (VFA_MODE_STS != (status & LBA_MODE_STS_MASK)) {
        diag_printf("error1 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }
#endif /* DEBUG_LBA */

    lba_mal_speed_mode(HIGH_SPEED_ENABLE);
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (!(status & 0x08)) {
        diag_printf("error2 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }

    lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1, PT_1_4096);
    lba_mal_get_transfer_protocol(TRANS_PROTOCOL_1, &status);
    if (PT_1_4096 != status) {
        diag_printf("error3 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }

    lba_mal_set_transfer_protocol(TRANS_PROTOCOL_2, PT_2);
    lba_mal_get_transfer_protocol(TRANS_PROTOCOL_2, &status);
    if (PT_2 != status) {
        diag_printf("error4 status=0x%X\n", status);
        return false;
    }

    lba_mal_get_vfa_unit(&num_sector);
    diag_printf("vfa size = %d\n", num_sector);

    lba_mal_get_mda_unit(&lo_byte, &hi_byte);
    diag_printf("mda size lo_byte=0x%X, hi_byte=0x%X\n", lo_byte, hi_byte);

    ret = lba_mal_mode_change(MDA_MODE, password);
    if (LBA_OK != ret) {
        return FLASH_ERR_HWR;
    }
#ifdef DEBUG_LBA
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (MDA_MODE_STS != (status & LBA_MODE_STS_MASK)) {
        diag_printf("error5 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }
#endif /* DEBUG_LBA */

    _lba_init_ok = true;

    return FLASH_ERR_OK;
}

// Read data into buffer
int lba_nfc_read_region(u32 addr, u8* data, int len)
{
#ifdef DEBUG_LBA
    u8 status;
#endif /* DEBUG_LBA */
    int ret;
    u8 password[2] = {0xFF, 0xFF};
    u32 sector;

    ret = lba_mal_mode_change(VFA_MODE, password);
    if (LBA_OK != ret) {
        return FLASH_ERR_HWR;
    }
#ifdef DEBUG_LBA
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (VFA_MODE_STS != (status & LBA_MODE_STS_MASK)) {
        diag_printf("error-1 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }
#endif /* DEBUG_LBA */

    sector = addr / 512;
    lba_mal_bulk_read(sector, len / KERNEL_SECTOR_SIZE, data);

    ret = lba_mal_mode_change(MDA_MODE, password);
    if (LBA_OK != ret) {
        return FLASH_ERR_HWR;
    }
#ifdef DEBUG_LBA
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (MDA_MODE_STS != (status & LBA_MODE_STS_MASK)) {
        diag_printf("error5 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }
#endif /* DEBUG_LBA */

    return FLASH_ERR_OK;
}

/*!
 * Program data from memory to LBA-NAND flash
 * @param addr          linear NAND flash address. it has to be block size aligned
 * @param data          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 lba_nfc_program_region(u32 addr, u8* data, int len)
{
    u8  status;
    u8  password[2] = {0xFF, 0xFF};
    u16 len_write;
    u32 lba_sector;

    /* change to VFA mode */
    lba_mal_mode_change(VFA_MODE, password);
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (!(status & 0x06)) {
        diag_printf("error-1 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }

    lba_sector = addr / 512;
    len_write = len / KERNEL_SECTOR_SIZE;

    lba_mal_bulk_write(lba_sector, len_write, data);
    lba_mal_flush_cache();

    /* change to MDA mode */
    lba_mal_mode_change(MDA_MODE, password);
    lba_mal_status_read(LBA_STS_MODE1, &status);
    if (!(status & 0x06)) {
        diag_printf("error-2 status=0x%X\n", status);
        return FLASH_ERR_HWR;
    }

    return FLASH_ERR_OK;
}

void get_read_page(u16 addr, u8 *buf, u16 len)
{
	volatile u16 *sur_data;
	u16 *ptr_data;
	
	sur_data = (volatile u16 *)(NAND_MAIN_BUF0);
	ptr_data = (u16*)buf;

	memcpy((void*)buf,(void*)sur_data,len);
}

void set_write_page(u16 addr, u8 *buf, u16 len)
{
	volatile u16 *ptr_data;
	u16 *sur_data;
	
	ptr_data = (volatile u16 *)(NAND_MAIN_BUF0);
	sur_data = (u16*)buf;
	
	memcpy((void*)ptr_data,(void*)sur_data,len);
}

bool lba_id_compare(u8 *id)
{
	#define LBA4G	0x3447
	#define LBAXG	0x5847
	
    bool ans = false;
    int i;
	u8 lba_id1[] = TOSHIBA_LBA_ID1;
	u8 lba_4g[] = TOSHIBA_LBA_ID2_4G;

	// Because the SG device not include PCMCIA interface,
	// so we use PCMCIA register and record LBA type for Kernal-DVFS.
	// You can see other source code in Kernel(g23.c).
	volatile u32 *checkforlba = (u32*)(0xB8004028);
	
    for (i = 0; i < _LBA_ID_NUM; ++i) {
        if (0 == memcmp(id, &_lba_id[i], 5)) {
            ans = true;
        }
    }

	if (0 == memcmp(id, lba_id1, 5)) {
		// if id = lba_id1 , we don't change NFC clock.
		//diag_printf("id1 don't change NFC clock.\n");
		return ans;
	}

	if (0 == memcmp(id, lba_4g, 5)) {
		// if id = lba_4g , we will use high freq in NFC.
		lba_nfc_clock(NFC_33_2MHZ);
		*checkforlba = LBA4G;
		diag_printf("NFC clock = 33.2MHz.\n");
	}else{
		// if id != lba_4g , we will use normal freq in NFC.
		lba_nfc_clock(NFC_26_6MHZ);
		*checkforlba = LBAXG;
		diag_printf("NFC clock = 26.6MHz.\n");
	}
	
    return ans;
}

bool lba_get_bat_cal_value(u8 *valueStr,u8 len)
{
    u8 rBuf[512];
	int b1=0,b2=0,b3=0,b4=0,b5=0;
	
	if (len!=32) {
		return false;
	}

	lba_mal_bulk_read(128, 1, rBuf);

	memcpy(&b1,&rBuf[288],4);
	memcpy(&b2,&rBuf[292],4);
	memcpy(&b3,&rBuf[296],4);
	memcpy(&b4,&rBuf[300],4);
	memcpy(&b5,&rBuf[304],4);

    diag_sprintf(valueStr,"%d,%d,%d,%d,%d",b1,b2,b3,b4,b5);

	//diag_printf("str=%s\n",valueStr);

	return true;
}

/*
 * ori_size:   original boot config size
 * ori_sector: original boot config start sector
 * 
 * write2ori & cfg_num: 
 *     if write2ori=true, copy config_data[cfg_num] to original config area
 *     if write2ori=false, copy original config area to config_data[cfg_num]
 *
 *  ori_sector->_________________________  _         
 *	     	    |                       |  |
 *              |   redboot config      | ori_size
 *              |_______________________|  |
 *              |                       |  -
 *              |   config-1(cfg_num=1) |
 *              |_______________________|
 *              |                       |
 *              |   config-1(cfg_num=2) |
 *              |_______________________|
 *              |                       |
 *              |   config-1(cfg_num=3) |
 *              |_______________________|
 *
 *                         .
 *                         .
 *
 *
 */
void lba_config_move(u32 ori_size,u32 ori_sector,bool write2ori,int cfg_num)
{
	u32 count= ori_size/KERNEL_SECTOR_SIZE;
	u32 start= ori_sector/KERNEL_SECTOR_SIZE;
	u8 buf[512];
	u32 det;
	u32 sur;
	int ret;
	u8 password[2] = {0xFF, 0xFF};

    ret = lba_mal_mode_change(VFA_MODE, password);
    if (LBA_OK != ret) {
		diag_printf("lba_config_move: mode change fail\n");
        return;
    }
	
	if (write2ori) {
		sur = start + cfg_num * count;
		det = start;
	} else {
		sur = start;
		det = start + cfg_num * count;
	}

	while (count--) {
		lba_mal_bulk_read(sur, 1, buf);
		lba_mal_bulk_write(det, 1, buf);
		sur++;
		det++;
	}

	lba_mal_flush_cache();

    ret = lba_mal_mode_change(MDA_MODE, password);
    if (LBA_OK != ret) {
		diag_printf("lba_config_move: mode change fail\n");
        return;
    }
}

//////////////////////// "lba-nand" commands support ///////////////////////
static void lba_read(int argc, char *argv[])
{
    int mode;
    u32 len, len_read;
    u32 mem_addr, start_sector;
    bool mem_addr_set = false;
    bool start_sector_set = false;
    bool length_set = false;
    bool mode_set = false;
    struct option_info opts[4];
    u8 password[2] = {0xFF, 0xFF};
    u8 status;

    init_opts(&opts[0], 's', true, OPTION_ARG_TYPE_NUM,
              (void *)&start_sector, (bool *)&start_sector_set,
              "FLASH memory base address");
    init_opts(&opts[1], 'a', true, OPTION_ARG_TYPE_NUM,
              (void *)&mem_addr, (bool *)&mem_addr_set, "memory base address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&length_set, "image length");
    init_opts(&opts[3], 'm', true, OPTION_ARG_TYPE_NUM,
              (void *)&mode, (bool *)&mode_set, "LBA mode");

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

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

    if (1 == mode) {
        lba_mal_mode_change(MDA_MODE, password);
    } else if (2 == mode) {
        lba_mal_mode_change(VFA_MODE, password);
    } else {
        diag_printf("lbaread: mode parameter fail\n");
    }

    lba_mal_status_read(LBA_STS_MODE1, &status);
    status &= LBA_MODE_STS_MASK;
    if (1 == mode && MDA_MODE_STS != status) {
        diag_printf("lbaread: change mode to MDA fail\n");
        return;
    }
    if (2 == mode && VFA_MODE_STS != status) {
        diag_printf("lbaread: change mode to VFA fail\n");
        return;
    }

	lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1, 0x24);

    if (len % KERNEL_SECTOR_SIZE) {
        len_read = len / 512;
        ++len_read;
    } else {
        len_read = len / 512;
    }

    if (len_read <= 0xffff) {
        diag_printf("count=0x%X\n", len_read);
        lba_mal_bulk_read(start_sector, (u16)len_read, (u8*)mem_addr);
    } else {
        diag_printf("count > 0xffff\n");
    }

    lba_mal_mode_change(MDA_MODE, password);

	lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1, 0x21);
}

#if 1 /* For furture use, comment out prevent compile warning. */
static void lba_nfc_clock(u32 div)
{
	if ((div>8)||(div<0)) {
		diag_printf("lba_nfc_clock: div set error\n");
		return;	
	}
	
	insl((div<<8),0x53F80004,(0x7<<8));
}
#endif

static void lba_write(int argc, char *argv[])
{
    int mode;
    u32 len, len_write;
    u32 mem_addr, start_sector;
    bool mem_addr_set = false;
    bool start_sector_set = false;
    bool length_set = false;
    bool mode_set = false;
    struct option_info opts[4];
    u8 password[2] = {0xFF,0xFF};
    u8 status;

    init_opts(&opts[0], 's', true, OPTION_ARG_TYPE_NUM,
              (void *)&start_sector, (bool *)&start_sector_set,
              "FLASH memory base address");
    init_opts(&opts[1], 'a', true, OPTION_ARG_TYPE_NUM,
              (void *)&mem_addr, (bool *)&mem_addr_set, "memory base address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&length_set, "image length");
    init_opts(&opts[3], 'm', true, OPTION_ARG_TYPE_NUM,
              (void *)&mode, (bool *)&mode_set, "LBA mode");

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

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

    if (1 == mode) {
        lba_mal_mode_change(MDA_MODE, password);
    } else if (2 == mode) {
        lba_mal_mode_change(VFA_MODE, password);
    } else {
        diag_printf("lbawrite: mode parameter fail\n");
    }

    lba_mal_status_read(LBA_STS_MODE1, &status);
    status &= LBA_MODE_STS_MASK;
    if (1 == mode && MDA_MODE_STS != status) {
        diag_printf("lbawrite: change mode to MDA fail\n");
        return;
    }
    if (2 == mode && VFA_MODE_STS != status) {
        diag_printf("lbawrite: change mode to VFA fail\n");
        return;
    }

	lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1, 0x24);
	
    if (len % KERNEL_SECTOR_SIZE) {
        len_write = len / 512;
        ++len_write;
    } else {
        len_write = len / 512;
    }

    if (len_write <= 0xffff) {
        diag_printf("count=0x%X\n", len_write);
        lba_mal_bulk_write(start_sector, (u16)len_write, (u8*)mem_addr);
    } else {
        diag_printf("count > 0xffff\n");
    }

    lba_mal_flush_cache();
    lba_mal_mode_change(MDA_MODE, password);

	lba_mal_set_transfer_protocol(TRANS_PROTOCOL_1, 0x21);
}

static void lba_format(int argc, char *argv[])
{
    u32 len;
    bool len_set = false;
    u32 pw;
    bool pw_set = false;
    struct option_info opts[4];

    init_opts(&opts[0], 'p', true, OPTION_ARG_TYPE_NUM,
              (void *)&pw, (bool *)&pw_set,"password");
    init_opts(&opts[1], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&len_set, "8,16,32,64,128,256");

    if (!scan_opts(argc, argv, 2, opts, 2, 0, 0, 0)) {
        nand_usage("invalid arguments");
        return;
    }
	
    if (!pw_set || !len_set) {
        nand_usage("required parameter missing");
        return;
    }

    if ((len!=8)&&(len!=16)&&(len!=32)&&(len!=64)&&(len!=128)&&(len!=256)) {
        diag_printf("lbaformat: parameter error , must 8,16,32,64,128,256\n");
        return;
    }

    diag_printf("Low level format start!! Don't turn off power!! ");
    diag_printf("wait 10sec\n");

    if (lba_mal_mode_change(VFA_MODE, (char*)&pw) != LBA_OK) {
        diag_printf("lbaformat: mode change fail , password fail\n");
        return;
    }

    lba_mal_set_vfa_unit(0, len);

#ifdef DEBUG_LBA
    while (1) {
        lba_mal_status_read(LBA_STS_MODE0, &status);
        if (0xF0 == status)
            break;
    }
	
    lba_mal_get_mda_unit(&lo_byte, &hi_byte);
    diag_printf("mda size 0x%X\n", lo_byte);
#endif
}

static void lba_mem_cmp(int argc, char *argv[])
{
    u32 s_mem, d_mem, len_cmp;
    bool s_mem_set = false;
    bool d_mem_set = false;
    bool len_cmp_set = false;
    struct option_info opts[4];

    init_opts(&opts[0], 's', true, OPTION_ARG_TYPE_NUM,
              (void *)&s_mem, (bool *)&s_mem_set, "memory source address");
    init_opts(&opts[1], 'd', true, OPTION_ARG_TYPE_NUM,
              (void *)&d_mem, (bool *)&d_mem_set, "memory destination address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len_cmp, (bool *)&len_cmp_set, "compare length");

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

    if (!s_mem_set || !d_mem_set || !len_cmp_set) {
        nand_usage("required parameter missing");
        return;
    }

    if (memcmp((u8*)s_mem, (u8*)d_mem, len_cmp) != 0) {
        diag_printf("\nmemory different----- X \n\n");
    } else {
        diag_printf("\nmemory equal--------- O \n\n");
    }
}

static void lba_program_redboot(int argc, char *argv[])
{
    u32 len, len_write;
    u32 mem_addr;
    bool mem_addr_set = false;
    bool length_set = false;
    struct option_info opts[4];
    int i;
    u8 status;
    volatile u32 cc;
    u8 buf[2048];
    u32 s_mem;
    bool s_mem_set = false;
#ifdef PROGRAMER_TEST
    u32 s_ecc;
    bool s_ecc_set = false;
#endif

    init_opts(&opts[0], 's', true, OPTION_ARG_TYPE_NUM,
              (void *)&s_mem, (bool *)&s_mem_set, "memory source address");
    init_opts(&opts[1], 'a', true, OPTION_ARG_TYPE_NUM,
              (void *)&mem_addr, (bool *)&mem_addr_set, "memory base address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&length_set, "image length");
#ifdef PROGRAMER_TEST
    init_opts(&opts[3], 'e', true, OPTION_ARG_TYPE_NUM,
              (void *)&s_ecc, (bool *)&s_ecc_set, "ecc on/off");
#endif

#ifdef PROGRAMER_TEST
    if (!scan_opts(argc, argv, 2, opts, 4, 0, 0, 0)) {
#else
	if (!scan_opts(argc, argv, 2, opts, 3, 0, 0, 0)) {
#endif
        nand_usage("invalid arguments");
        return;
    }

#ifdef PROGRAMER_TEST
    if (!mem_addr_set || !length_set || !s_mem_set || !s_ecc_set) {
#else
    if (!mem_addr_set || !length_set || !s_mem_set) {
#endif
        nand_usage("required parameter missing");
        return;
    }

    diag_printf("redboot: start\n");

    lba_mal_fw_reload();

    for (cc = 0; cc < 10000; ++cc);

    lba_mal_into_bootcodemaintenance();

    lba_mal_status_read(LBA_STS_MODE0, &status);
    diag_printf("redboot: mode0=0x%X\n", status);
    lba_mal_status_read(LBA_STS_MODE1, &status);
    diag_printf("redboot: mode1=0x%X\n", status);

    if ((status & LBA_MODE_STS_MASK) != PNA_MODE_STS) {
        diag_printf("redboot: change mode to PNR fail\n");
        return;
    }

    if (len % 2048) {
        len_write = len / 2048;
        ++len_write;
    } else {
        len_write = len / 2048;
    }

#ifdef PROGRAMER_TEST
	if (0 == s_ecc) {
	    if (2048 == s_mem) {
	        /* program ipl 2kB, ecc off */
	        lba_mal_write_boot_code((u8*)mem_addr, 0, false);
			lba_mal_read_boot_code(buf, 0, false);
	        //diag_printf("lba_redboot_program: ecc_on = true\n");
	    }
	} else {
	    if (2048 == s_mem) {
	        /* program ipl 2kB, ecc on */
	        lba_mal_write_boot_code((u8*)mem_addr, 0, true);
			lba_mal_read_boot_code(buf, 0, true);
	    }
	}
#else
    if (2048 == s_mem) {
        /* program ipl 2kB, ecc on */
        lba_mal_write_boot_code((u8*)mem_addr, 0, true);
    }
#endif

    mem_addr += 2048;

    for (i = 1; i < len_write; ++i) {
        lba_mal_write_boot_code((u8*)mem_addr, i, false);
        lba_mal_read_boot_code(buf, i, false);

        if (memcmp((void*)buf, (void*)mem_addr, 2048) != 0) {
            diag_printf("redboot: compare fail at sector[%d]\n", i);
            return;
        } else {
            mem_addr += 2048;
        }
    }

    if (len % 2048) {
        lba_mal_write_boot_code((u8*)mem_addr, i, false);
        lba_mal_read_boot_code(buf, i, false);

        if (memcmp((void*)buf, (void*)mem_addr, len % 2048) != 0) {
            diag_printf("redboot: compare fail at sector[%d]\n", i);
            return;
        }
    }
		
    lba_mal_flush_cache();

    diag_printf("redboot: exit\n");
}

static void lba_read_redboot(int argc, char *argv[])
{
    u32 len, len_read;
    u32 mem_addr;
    bool mem_addr_set = false;
    bool length_set = false;
    struct option_info opts[4];
    int i;
    u8 status;
    volatile u32 cc;
    u8 buf[2048];
    u8 *data;
    u32 s_mem;
    bool s_mem_set = false;
#ifdef PROGRAMER_TEST
    u32 s_ecc;
    bool s_ecc_set = false;
#endif

    init_opts(&opts[0], 's', true, OPTION_ARG_TYPE_NUM,
              (void *)&s_mem, (bool *)&s_mem_set, "memory source address");
    init_opts(&opts[1], 'a', true, OPTION_ARG_TYPE_NUM,
              (void *)&mem_addr, (bool *)&mem_addr_set, "memory base address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&len, (bool *)&length_set, "image length");
#ifdef PROGRAMER_TEST
    init_opts(&opts[3], 'e', true, OPTION_ARG_TYPE_NUM,
              (void *)&s_ecc, (bool *)&s_ecc_set, "ecc on/off");
#endif

#ifdef PROGRAMER_TEST
    if (!scan_opts(argc, argv, 2, opts, 4, 0, 0, 0)) {
#else
    if (!scan_opts(argc, argv, 2, opts, 3, 0, 0, 0)) {
#endif
        nand_usage("invalid arguments");
        return;
    }

#ifdef PROGRAMER_TEST
    if (!mem_addr_set || !length_set || !s_mem_set || !s_ecc_set) {
#else
    if (!mem_addr_set || !length_set || !s_mem_set) {
#endif
        nand_usage("required parameter missing");
        return;
    }

    diag_printf("redboot: start\n");

    lba_mal_fw_reload();

    for (cc = 0; cc < 10000; ++cc);

    lba_mal_into_bootcodemaintenance();

    lba_mal_status_read(LBA_STS_MODE0, &status);
    diag_printf("redboot: mode0=0x%X\n", status);
    lba_mal_status_read(LBA_STS_MODE1, &status);
    diag_printf("redboot: mode1=0x%X\n", status);

    if ((status & LBA_MODE_STS_MASK) != PNA_MODE_STS) {
        diag_printf("redboot: change mode to PNR fail\n");
        return;
    }

    if (len % 2048) {
        len_read = len / 2048;
        ++len_read;
    } else {
        len_read = len / 2048;
    }

    data = (u8*)mem_addr;

#ifdef PROGRAMER_TEST
	if (0 == s_ecc) {
	    if (2048 == s_mem) {
	        /* read ipl 2kB, ecc off */
	        lba_mal_read_boot_code(data, 0, false);
	    }
	} else {
	    if (2048 == s_mem) {
	        /* read ipl 2kB, ecc off */
	        lba_mal_read_boot_code(data, 0, true);
	    }
	}
#else
    if (2048 == s_mem) {
        /* read ipl 2kB, ecc off */
        lba_mal_read_boot_code(data, 0, true);
    }
#endif

	
    data += 2048;

    for (i = 1; i < len_read; ++i) {
        lba_mal_read_boot_code(data, i, false);
        data += 2048;
    }

    if (len % 2048) {
        lba_mal_read_boot_code(buf, i, false);

        for (i = 0; i < len % 2048; ++i) {
            *data = buf[i];
            ++data;
        }
    }
	
    diag_printf("redboot: exit\n");
}

bool read_level(u8 *level,u8 len)
{
#define SECTOR_LENGTH	512
#define DATA_SHIFT		128
#define DATA_LEN		33

    u8 password[2] = {0xFF, 0xFF};
    u8 rBuf[SECTOR_LENGTH];
    int ret;

	if (len!=DATA_LEN) {
		return false;
	}
	
    ret = lba_mal_mode_change(VFA_MODE, password);

    if (LBA_OK != ret) {
        diag_printf("redboot: lba_test mode change fail\n");
        return false;
    }

	lba_mal_bulk_read(128, 1, rBuf);

	rBuf[DATA_LEN-1] = 0;
	
    memcpy(level,&rBuf[DATA_SHIFT],DATA_LEN);

	return true;
}

int run_lba_test(void)
{
	#define TEST_TIME	500
	
    u32 i;
    u8 password[2] = {0xFF, 0xFF};
    u8 buf[6];
	int ret;
	
	for (i = 0; i < TEST_TIME; i++) {
	    ret = lba_mal_mode_change(VFA_MODE, password);

	    if (LBA_OK != ret) {
	        diag_printf("redboot: lba_test mode change fail\n");
	        return 0;
	    }

	    ret = lba_mal_mode_change(MDA_MODE, password);

	    if (LBA_OK != ret) {
	        diag_printf("redboot: lba_test mode change fail\n");
	        return 0;
	    }
	}

    for (i = 0; i < TEST_TIME; ++i) {
	    memset(buf, 0xBB, sizeof(buf));
	    lba_mal_read_id(LBA_ID_MODE1, buf);
	    if (lba_id_compare(buf) == false) {
	        diag_printf("LBA-NAND id-2 FAIL\n");
	        return 0;
	    }
    }

	
	return 1;	
}

int run_ram_test(void)
{
	//#define TEST_START	0x00010d00
	//#define TEST_END	0x03eed000
	#define TEST_START	(volatile u32 *)0x01000000
	#define TEST_END	(volatile u32 *)0x02000000
	#define TEST_VALUE	0

	volatile u32 *addr = (volatile u32 *)0x100000;
	u32 dif;
	u32 pattern;
	bool test_success = true;
	u32 count = 30;
	
	while (count--) {
		for (pattern = 1; pattern != 0; pattern <<=1) {
			*addr = pattern;

			if (*addr != pattern) {
				test_success = false;
				goto test_exit;
			}
		}

		dif = (TEST_END-TEST_START);
		addr = TEST_START;
		while (dif--) {
			*(addr++) = TEST_VALUE;
		}

		dif = (TEST_END-TEST_START);
		addr = TEST_START;
		*addr = ~TEST_VALUE;
		while (--dif) {
			if (*(++addr) != TEST_VALUE) {
				test_success = false;
				goto test_exit;
			}
		}
	}
	
	test_exit:
		
	if (test_success) {
		diag_printf("ram_test: SUCCESS\n");
		return 1;
	}

	return 0;
}

static int lba_read_emulation(uint32_t lba_start, int lba_blocks, uint8_t* buf)
{
#if 0
    diag_printf("lba_start=0x%08x\n", lba_start);
    diag_printf("lba_blocks=0x%08x\n", lba_blocks);
#endif

    /* always skip the partition table and read first partition */
    lba_start += 63;
    lba_mal_bulk_read(lba_start, (u16)lba_blocks, (u8*)buf);
    return 1;
}

static void do_fatread(int argc, char *argv[])
{
    struct option_info opts[3];
    u8 password[2] = {0xFF, 0xFF};
    bool filename_set = false;
    bool memaddr_set = false;
    bool length_set = false;
    u32 memaddr;
    char *filename;
    int ret, size, length;
    static bool tinyfat_init = false;

    init_opts(&opts[0], 'f', true, OPTION_ARG_TYPE_STR,
              (void *)&filename, (bool *)&filename_set, "file name");
    init_opts(&opts[1], 'b', true, OPTION_ARG_TYPE_NUM,
              (void *)&memaddr, (bool *)&memaddr_set, "memory address");
    init_opts(&opts[2], 'l', true, OPTION_ARG_TYPE_NUM,
              (void *)&length, (bool *)&length_set, "read length");

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

    if (!filename_set || !memaddr_set) {
        nand_usage("required parameter missing");
        return;
    }

    diag_printf("filename=%s\n", filename);
    diag_printf("address=0x%08x\n", memaddr);
    if (length_set)
        diag_printf("length=%d\n", length);

    if (!tinyfat_init) {
        diag_printf("tinyfat init\n");
        ret = fat_init(&lba_read_emulation, 0);
        if (ret < 0) {
            diag_printf("tinyfat init fail!\n");
            return;
        }
        tinyfat_init = true;
    }

    /* change mode to MDP */
    ret = lba_mal_mode_change(MDA_MODE, password);
    if (LBA_OK != ret) {
        diag_printf("change to MDP mode fail\n");
        return;
    }

    if (length_set) {
        size = length;
    } else {
        size = fat_getsize(filename);
    }
    diag_printf("read size=%d\n", size);

    fat_loadfile(filename, (void*)memaddr, size);
}

local_cmd_entry("lbaread",
    "Read data from LBA-NAND into RAM",
    "-s <start sector number> -a <memory address> -l <read length> -m <1:MDA 2:VFA>",
    lba_read,
    NAND_cmds
);

local_cmd_entry("lbawrite",
    "Write data from RAM into LBA-NAND",
    "-s <start sector number> -a <memory address> -l <read length> -m <1:MDA 2:VFA>",
    lba_write,
    NAND_cmds
);

local_cmd_entry("lbaformat",
    "Format LBA-NAND",
    "-p <password> -l <length>",
    lba_format,
    NAND_cmds
);

local_cmd_entry("lbacmp",
    "Compare data at RAM",
    "-s <memory source address> -d <memory destination address> -l <compare length>",
    lba_mem_cmp,
    NAND_cmds
);

#ifdef PROGRAMER_TEST
local_cmd_entry("lbaprb",
    "Program RedBoot",
    "-s <start> -a <memory address> -l <read length> -e <ecc on=!0/off=0>",
    lba_program_redboot,
    NAND_cmds
);

local_cmd_entry("lbarrb",
    "Read RedBoot",
    "-s <start> -a <memory address> -l <read length> -e <ecc on=!0/off=0>",
    lba_read_redboot,
    NAND_cmds
);
#else
local_cmd_entry("lbaprb",
    "Program RedBoot",
    "-s <start> -a <memory address> -l <read length>",
    lba_program_redboot,
    NAND_cmds
);

local_cmd_entry("lbarrb",
    "Read RedBoot",
    "-s <start> -a <memory address> -l <read length>",
    lba_read_redboot,
    NAND_cmds
);
#endif

local_cmd_entry("fatread",
    "Read a file from fat partition",
    "-f <file name> -b <memory address> [-l <read length>]",
    do_fatread,
    NAND_cmds
);
