Chromecast Marvell Bootloader

/*
 * (C) Copyright Marvell Semiconductors, Inc 2008 All rightes reserved
 */
#include "common.h"
#include "chipctrl_const.h"
#ifdef CONFIG_SM
#include "sm_const.h"
#include "SysMgr.h"
#endif // CONFIG_SM
#include "figo.h"
#include "config.h"
#include "nand_priv.h"
#include "util.h"
#include "cold_linuxparam.h"
#include "SystemManager.h"
#include "SysMgr.h"
#include "system_config.h"
#include "global.h"
#include "gpio.h"
#include "led_pwm.h"
#include "apb_perf_base.h"
#include "apb_gpio.h"
#include "galois_speed.h"
#include "pin_settings.h"
#include "wol.h"
#include "berlin_bootinfo.h"
#include "mv_nand.h"
#include "drmdmx.h"
#include "basic_inc.h"
#include "timer.h"
#if 0 // BOOTLOADER_FASTLOGO
#include "thinvpp_api.h"
#include "avpll.h"
#include "api_avio_dhub.h"
#endif
#include "fastlogo.h"
#include "nand_block0_layout_A0.h"
#include "image2_header_A0.h"

#include "image3_header.h"
#include "version_table.h"
#if BG2CDP
#include "MctrlSS.h"
#include "cpu_ca7.h"
#include "block1_layout.h"
#include "bcmREG.h"
#include "bcm_mailbox.h"
#define BCM_MAILBOX          MEMMAP_BCM_REG_BASE //0xF7930000
#define BCM_PI_IMAGE_VERIFY  0x004E
#define BCM_IMG_KERNEL_TYPE  4
#define BCM_IMG_USBIMG_TYPE  5

#define SYSPLL_CTRL_BASE        (MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_sysPll)
#define CPUPLL_CTRL_BASE        (MEMMAP_CA7_REG_BASE + RA_CA7PllReg_PllReg)
#define MEMPLL_CTRL_BASE        (MEMMAP_MCTRLSS_REG_BASE + RA_MctrlSS_memPll)

#define VCORE_MASTER_ID         (0x00)
#define LED_MASTER_ID (0x00)

#define VCORE_VOLT_INDEX        (0x0A) // set to 1.15v
#endif

#if ANDROID_BOOT
#include "android/bootimg.h"
#include "android/flash_ts.h"
#include "board_config.h"
#endif

#include "nflash_drv.h"

#include "linux_param.h"

#include "bootloader_customize.h"

#ifdef CONFIG_USB
#include "usb_storage.h"
#include "usb_key.h"
#endif
#include "cec.h"

// disable crash counter. b/10478589.
#define DISABLE_CRASH_COUNTER 1

//#define CONFIG_FORCE_ENCRYPTION

#define BOOT_SRC_NAND     1
#define BOOT_SRC_USB      2

/* on a USB storage device, leave first 4KB for MBR/part table
 * and start boot image at a 4KB offset.
 */
#define USB_BOOTIMG_START      (4 * 1024)
#define USB_BLOCK_SIZ          512

#define CPU_IMG_OFFS_MAGIC       4
#define CPU_IMG_OFFS_USRDATA     10
#define CPU_IMG_OFFS_IMGSIZ      40
#define CPU_IMG_OFFS_IMGSTA      332

#define CPU_IMG_USB_USRDATA      0xA33A
#define CPU_IMG_NAND_USRDATA     0x0
#define CPU_IMG_CODE_MAGIC       0xC0DE

#define MAX_ANDROID_IMG_SIZE (32 * 1024 * 1024)
#define MIN_ANDROID_IMG_SIZE (1 * 1024 * 1024)
#define MAX_LOGO_FRM_SIZE (4 * 1024 * 1024)

// The bootloader needs some temporary memory for loading the android image
// (during verification) and for the fast logo frame buffer.  This memory is
// is not needed once we boot the kernel, but it should not conflict with the
// bootloader or where we relocate the kernel and ramdisk.  Furthermore,
// the very top of memory is off limits (protected by the trust zone).
//
// Memory maps, starting at address 0:
//
// At kernel load time:
//   [ TZ resident memory ] - size 17MB
//   [ Bootloader + data (including bcm console) ] - starting at 22MB
//   [ Unused memory ]
//     Fast logo frame buffer - max size 4MB
//     Android image buffer - max size 32MB
//   [ secure memory ] - starts at MEMTOP - 8MB, size 8MB
//
// At kernel launch time:
//   [ TZ resident memory ] - size 17MB
//   [ Linux memory ] - total size ~302MB
//       Kernel copied to 17MB, (note: *must* be < 5MB)
//       Bootloader image is at 22MB
//       Ramdisk copied to 96MB
//   [ cache memory ] - size 168MB
//   [ secure memory ] - starts at (BOARD_MEM_SIZE - 8MB), size 8MB

// Define an arbitrary maximum secure memory size which should be
// more than enough to avoid problems in case of expansion.  The
// actual size is determined by the TZ and the kernel.
#define MAX_SECURE_MEMORY (32 * 1024 * 1024)

// Define the start of bootloader temporary memory.
#define BOOTLOADER_TEMP_MEMORY (BOARD_MEM_SIZE - MAX_SECURE_MEMORY - \
    (MAX_LOGO_FRM_SIZE + MAX_ANDROID_IMG_SIZE))

// Logo is at the start of temporary memory.
#define LOGO_FRM_BUF (BOOTLOADER_TEMP_MEMORY)

// Android image buffer begins right after the frame buffer.
#define ANDROID_IMG_BUF (BOOTLOADER_TEMP_MEMORY + MAX_LOGO_FRM_SIZE)

#define CHIP_VERSION   "BG2CDP"
#if !CDP_A0
#define MV88DE3100_CHIPID_REG1  (0xF7CFF030)
#define MV88DE3100_CHIPID_REG2  (0xF7CFF034)
#else
#define MV88DE3100_CHIPID_REG1  (0xF7CFF060)
#define MV88DE3100_CHIPID_REG2  (0xF7CFF064)
#endif

#define PRODUCT_CHIP_EXT_ID     (MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_ProductId_ext)
#define MV88DE3XXX_OTP_STS_REG 0xF7CC08C8
#define MV88DE3XXX_OTP_LKG_ID 0XF7CC07DD
#define MV88DE3XXX_SOC_REV_REG 0XF7CC07D4
#define MV88DE3XXX_SOC_REV_MASK 0x3
#define MV88DE3XXX_SOC_REV_SHIFT 29

#define MAX_RAMDISK_IMPLICT_SIZE    (0x2000000)

#define PRNG_DATA_SIZE 64

#define BERLIN_BOOTINFO_ADDR    0x01010000

/*
 * Macro to remove unused variable warning for function args not used for
 * specific platform.
 */
#define UNUSED(var) do { (void)(var); } while(0)

#define ARRAY_SIZE(a) (sizeof(a) / sizeof(a[0]))

enum sm_ldo_ctrl_vout_sel_e {

    SM_LDO_CTRL_VOUT_0_900V = 0,
    SM_LDO_CTRL_VOUT_0_789V,
    SM_LDO_CTRL_VOUT_0_826V,
    SM_LDO_CTRL_VOUT_0_863V,
    SM_LDO_CTRL_VOUT_0_937V,
    SM_LDO_CTRL_VOUT_0_974V,
    SM_LDO_CTRL_VOUT_1_011V,
    SM_LDO_CTRL_VOUT_1_048V,
    SM_LDO_CTRL_VOUT_MAX
};

extern int __bss_start;
extern int __bss_end;

#define BSS_START  ((unsigned int)(&__bss_start))
#define BSS_END    ((unsigned int)(&__bss_end))

#if ANDROID_BOOT
#define FTS_NAME        "fts"
#define KERNEL_NAME     "kernel"
#define RECOVERY_NAME   "recovery"

#define FTS_KEY_MACADDR         "macaddr"
#define FTS_WIFI_MFG_MODE "wifi_mfg_mode"
#define FTS_DEVICE_CFGED  "device_configured"

#define BOOTLOADER_COMMAND_KEY      "bootloader.command"
#define ANDROID_CRASH_COUNTER_KEY   "crashcounter.android"
#define MAX_CRASH_COUNT                 5
#define BOOTMODE_NORMAL                 0
#define BOOTMODE_RECOVERY               1
#define BOOTMODE_BOOTUSB                2
#define BOOTMODE_BACKUPSYS              3
const char kBootcmdRecovery[] = "recovery";
const char kFtsBootcmdRecovery[] = "boot-recovery";
const char kFtsBootcmdBackupsys[] = "boot-backupsys";

#endif /* #if ANDROID_BOOT */

#define STR(x) #x
#define XSTR(x) STR(x)

//#if defined(DEBUG)
//#define debug_printf(a...) lgpl_printf(a)
//#else
//#define debug_printf(a...)
//#endif

//#define debug_printf(a...) lgpl_printf(a)
extern struct mv_nand_data nand_data ;
block0_t block0;

ver_table_entry_t vt_img3;
//ver_table_entry_t vt_sysconf;
//ver_table_entry_t vt_flashless;
#if ANDROID_BOOT
static const ver_table_entry_t vt_fts = {
    .part1_start_blkind = FTS_BLK_START,
    .part1_blks = FTS_BLK_CNT,
};
static const ver_table_entry_t vt_recovery = {
    .part1_start_blkind = RECOVERY_BLK_START,
    .part1_blks = RECOVERY_BLK_CNT,
};
static const ver_table_entry_t vt_kernel = {
    .part1_start_blkind = KERNEL_BLK_START,
    .part1_blks = KERNEL_BLK_CNT,
};
static const ver_table_entry_t vt_backupsys = {
    .part1_start_blkind = BACKUPSYS_BLK_START,
    .part1_blks = BACKUPSYS_BLK_CNT,
};

static unsigned int fts_dev_id = 0;

static boot_img_hdr Mkbootimg_hdr;
void (*TheKernel)(int zero, int arch, unsigned int params);
static int Bootmode = BOOTMODE_NORMAL;
#endif /* #if ANDROID_BOOT */

Image3_Attr img3_hdr;
sm_image_attr *sm_hdr;
cpu0_image_attr *cpu0_hdr;
cpu1_image_attr *cpu1_hdr;
recovery_ou_attr *recou_hdr;
Encryption_Header en_hdr;
gen_header_t sysconf_hdr;

Mem_Layout *sMem_Layout ;

#define REBOOT_MSG_EXTRA_BUF_ADDR   0xa00000    // TODO: need a buffer address
#define REBOOT_MSG_EXTRA_BUF_SIZE   512

char reset_msg_extra_buf[REBOOT_MSG_EXTRA_BUF_SIZE];

unsigned int uiWarmDown_2_Linux_Addr = 0;

void setup_linux_bootparam(u32 mem_start, u32 mem_size, char *cmdline,
                           u8 chip_rev, u8 board_rev);
extern void Jump(unsigned int, unsigned int, unsigned int, unsigned int);
extern void Copy_Code_To_Figo_Sram();
extern int MV_SM_Send_Msg(int, void *, int);
extern void APB_UART_init(unsigned int , unsigned int , unsigned int ) ;

extern char gp_cust_figo_image[];
extern int MV_DRMLIB_Load_Customer_Key( unsigned char *, int);
extern int lgpl_printf(const char *format, ...);
extern void AVPLL_Enable(void);

inline char itoc(char i);

HRESULT MV_SM_Dev_Init(UCHAR * pucData_Buffer,UINT32 uiData_Size);

void abort(){printf("Abort\n"); while(1);}

int IS_Z1() { return 0; return (readl(PRODUCT_CHIP_EXT_ID) & 0xFF) == 0; }
int IS_Z2() { return 0; return (readl(PRODUCT_CHIP_EXT_ID) & 0xFF) == 2; }
int IS_B0() { return 1; return (readl(PRODUCT_CHIP_EXT_ID) & 0xFF) >= 0xB0; }
#ifdef ENABLE_CONNECT_BTN
extern int is_EnterRecovery;
extern void Check_connectbtnGPIO();
#endif

#if BG2CDP

int bcm_image_verify(unsigned int type, unsigned int src, unsigned int dst);

#define VT_OFFSET_CDP (nand_data.szofblk - 4096) // CDP: version table at last 4KiB of the block

#define SZ_MV_BOOT_IMAGE_HEAD     1024
#define SZW_MV_BOOT_IMAGE_HEAD    (SZ_MV_BOOT_IMAGE_HEAD/sizeof(int))
#define SZ_MV_KEY_STORE_TYPE_AESK 64
#define SZ_MV_KEY_STORE_TYPE_RSAK (1024 - 2*SZ_MV_KEY_STORE_TYPE_AESK)
#define MV_KEY_STORE_TYPE_MASK    0x0000ffff
#define MV_KEY_STORE_TYPE_RSAK    0x0000a2e1
#define MV_KEY_STORE_TYPE_AESK    0x0000c237
#define MV_KEY_STORE_TYPE_ENDK    0x0f01c0de

typedef struct _MV_KEY_STORE_HEAD
{
    unsigned int version;
    unsigned int type;
} MV_KEY_STORE_HEAD;

typedef struct _MV_LASTK_STORE
{
    union
    {
        MV_KEY_STORE_HEAD custk;
        unsigned char custk_bytes[SZ_MV_KEY_STORE_TYPE_AESK];
    };
    union
    {
        MV_KEY_STORE_HEAD custk_kernel;
        unsigned char custk_kernel_bytes[SZ_MV_KEY_STORE_TYPE_AESK];
    };
    union
    {
        MV_KEY_STORE_HEAD extrsak;
        unsigned char extrsak_bytes[SZ_MV_KEY_STORE_TYPE_RSAK];
    };
} MV_LASTK_STORE;

typedef struct _MV_BOOT_IMAGE_HEAD
{
    union
    {
        MV_KEY_STORE_HEAD head;
        unsigned char bytes[SZ_MV_BOOT_IMAGE_HEAD];
        unsigned int words[SZW_MV_BOOT_IMAGE_HEAD];
    };
} MV_BOOT_IMAGE_HEAD;

typedef struct _MV_LASTK_IMAGE
{
    union
    {
        struct
        {
            MV_BOOT_IMAGE_HEAD image;
        } h1;
        struct
        {
            MV_BOOT_IMAGE_HEAD custk;
            MV_BOOT_IMAGE_HEAD image;
        } h2;
        struct
        {
            MV_BOOT_IMAGE_HEAD extrsak;
            MV_BOOT_IMAGE_HEAD image;
        } h2a;
        struct
        {
            MV_BOOT_IMAGE_HEAD custk;
            MV_BOOT_IMAGE_HEAD extrsak;
            MV_BOOT_IMAGE_HEAD image;
        } h3;
    };
} MV_LASTK_IMAGE;

int load_lastk(void *key_store)
{
    MV_LASTK_STORE *lastk = (MV_LASTK_STORE *) key_store;
    int err = 0;

    if ((lastk->custk_kernel.type& MV_KEY_STORE_TYPE_MASK) == MV_KEY_STORE_TYPE_AESK)
    {
        // there is an custk for kernel image; let's load it
        MV_LASTK_IMAGE *header = (MV_LASTK_IMAGE *) &lastk->custk_kernel;
        MV_KEY_STORE_HEAD *bi;

        if (lastk->extrsak.type != MV_KEY_STORE_TYPE_RSAK)
        {
            // there is no extrsak for kernel image; use h2 format
            bi = &header->h2.image.head;
        }
        else
        {
            // there is also a extrsak for kernel image; use h3 format
            UtilMemCpy(header->h3.extrsak.bytes, lastk->extrsak_bytes, sizeof(lastk->extrsak_bytes));
            bi = &header->h3.image.head;
        }
        bi->version = 1; // only version 1 is supported
        bi->type = MV_KEY_STORE_TYPE_ENDK;
        err = bcm_image_verify(0, (unsigned int) header, 0);
        if (err == (int)0xe0000301) // we are expecting this return code
            err = 0;
    }
    else
    {
        lgpl_printf("\tno need to load kernel keys\n");
    }

    return err;
}

#endif

void pad_init(void)
{
#if !BG2CD
    int val;
    T32Gbl_PadSelect reg;
    GA_REG_WORD32_READ(MEMMAP_CHIP_CTRL_REG_BASE+ RA_Gbl_PadSelect, &(reg.u32));
    reg.uPadSelect_DVIO_OEN = Gbl_PadSelect_DVIO_OEN_Enable;
#if 0 //def CYPRESS_EMI // default always disable
    reg.uPadSelect_DVIO0_V18EN = 1;     //BG_FIX for EMI issue
#else
    reg.uPadSelect_DVIO0_V18EN = 0;
#endif
    reg.uPadSelect_CLK0_ZN = 7;
    reg.uPadSelect_CLK0_ZP = 7;
    reg.uPadSelect_DVIO0_ZN = 7;
#if (BERLIN_CHIP_VERSION < BERLIN_BG2)
    reg.uPadSelect_DVIO0_ZP = 7;
#endif
    GA_REG_WORD32_WRITE(MEMMAP_CHIP_CTRL_REG_BASE+ RA_Gbl_PadSelect, (reg.u32));

#if (BERLIN_CHIP_VERSION >= BERLIN_BG2)
    T32Gbl_PadSelect1 reg1;
    GA_REG_WORD32_READ(MEMMAP_CHIP_CTRL_REG_BASE+ RA_Gbl_PadSelect1, &(reg1.u32));
    reg1.uPadSelect_DVIO0_ZP = 7;
    GA_REG_WORD32_WRITE(MEMMAP_CHIP_CTRL_REG_BASE+ RA_Gbl_PadSelect1, (reg1.u32));
#endif
#endif
}

#if CDP_A0==1
#define SOC_SM_CEC_BASE            0xF7FE1000
static int detect_reset_state(void)
{
    unsigned int value, warm_boot = 0;
    GA_REG_WORD32_READ(SOC_SM_CEC_BASE + RA_Cec_CHIP_RESET_TRACKER, &value);

    if (value == 0) {
        lgpl_printf("chip is cold boot.\n");
    } else {
        lgpl_printf("chip is warm boot.\n");
        warm_boot = 1;
    }

    return warm_boot;
}

// Call this to make subsequent calls to detect_reset_state "warm"
// Lasts until power cycle.
static void set_reset_state_warm(void)
{
    GA_REG_WORD32_WRITE(SOC_SM_CEC_BASE + RA_Cec_CHIP_RESET_TRACKER, 0x01);
}

#endif //CDP_A0==1

/* Initiailize GPIOs used by bootloader.
 */
static int board_gpio_init(void)
{
    int ret = 0;
    if ((ret = GPIO_PortSetInOut(BUTTON_GPIO, 1)) != 0) {
        lgpl_printf("ERROR: failed to set button GPIO: %d \n",
                    BUTTON_GPIO);
        return -1;
    }
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
    if ((ret = GPIO_PortSetInOut(TOPBOARD_INTR_GPIO, 1)) != 0) {
        lgpl_printf("ERROR: failed to set top board intr GPIO: %d \n",
                    TOPBOARD_INTR_GPIO);
        return -1;
    }
#endif // BOARD_CHIRP_B{1,3,4} BOARD_JOPLIN_B{1,3,4}
    return 0;
}

static void prng_init(char *kernel_param, size_t kernel_param_size)
{
    UINT8 rand_buf[PRNG_DATA_SIZE];
    UINT32 rand_size = sizeof(rand_buf);
        int ret = bcm_generate_random_number(rand_buf,rand_size);
        if (ret != 0) {
        debug_printf("ERROR: failed to generate random data. ret = %d\n",ret);
        return;
    }

    // Append 64 bytes (128-bytes in hex) to kernel command line "prng_data=".
    strlcat(kernel_param, " prng_data=", kernel_param_size);
    size_t i;
    for (i = 0; i < sizeof(rand_buf); ++i) {
        char tmp[3] = {0};
        snprintf(tmp, sizeof(tmp), "%02x", rand_buf[i]);
        strlcat(kernel_param, tmp, kernel_param_size);
    }

    // Display random bits.
    debug_printf("PRNG bytes\n");
    for (i = 0; i < sizeof(rand_buf); i += 16) {
        int j = 0;
        debug_printf("\t");
        for (j = 0; j < 16; ++j) {
            debug_printf("%02x", rand_buf[i + j]);
        }
        debug_printf("\n");
    }
}

/* get button status
 * !0 - pressed
 *  0 - not pressed
 */
static int get_button_status(void)
{
    /* For CDp, read the status of bootstrap bit at start of boot process
     * rather than reading current status of button. This will capture
     * user's intent to boot in to USB early in boot process, even if
     * the button was released in between IROM boot -> USB timeout
     * -> sys_init -> bootloader transition.
     */
    int button = readl(CHIP_CTRL_REG_BASE + RA_Gbl_bootStrap) & 0x1;
    return (button == 0);
}

/* Get SOC revision information */
static unsigned char get_soc_rev(void)
{
    /* Bits 30:29 are marked by Marvell to flag SoC revision.
     * 0x0 - A0
     * 0x1 - A1 (LVT + Metal fix)
     * 0x2 - A2 (Metal fix only)
     */
    /*unsigned int rev = readl(MV88DE3XXX_SOC_REV_REG);
    rev &= (MV88DE3XXX_SOC_REV_MASK << MV88DE3XXX_SOC_REV_SHIFT);
    rev >>= MV88DE3XXX_SOC_REV_SHIFT;
    return (unsigned char)rev;*/
    return 0;
}

#if ANDROID_BOOT
//typedef unsigned int u32;
void setup_linux_bootparam_with_ramdisk(u32 mem_start, u32 mem_size, char *cmdline,
                                        u32 initrd_start, u32 initrd_size,
                                        u8 chip_rev, u8 board_rev);
static long long nand_read_generic(unsigned int start, unsigned int end, unsigned char* data_buff,
                    unsigned int data_size);
inline unsigned get_aligned(unsigned address, unsigned page_size) {
    return (address + page_size - 1) / page_size * page_size;
}

int bootimg_print_image_hdr (boot_img_hdr *hdr)
{
    int i;
    UNUSED(hdr);
    debug_printf ("   Image magic:   %s\n", hdr->magic);

    debug_printf ("   kernel_size:   0x%x\n", hdr->kernel_size);
    debug_printf ("   kernel_addr:   0x%x\n", hdr->kernel_addr);

    debug_printf ("   rdisk_size:   0x%x\n", hdr->ramdisk_size);
    debug_printf ("   rdisk_addr:   0x%x\n", hdr->ramdisk_addr);

    debug_printf ("   second_size:   0x%x\n", hdr->second_size);
    debug_printf ("   second_addr:   0x%x\n", hdr->second_addr);

    debug_printf ("   tags_addr:   0x%x\n", hdr->tags_addr);
    debug_printf ("   page_size:   0x%x\n", hdr->page_size);

    debug_printf ("   name:      %s\n", hdr->name);
    debug_printf ("   cmdline:   %s\n", hdr->cmdline);

    for (i=0;i<8;i++)
        debug_printf ("   id[%d]:   0x%x\n", i, hdr->id[i]);

    return 0;
}

static int write_android_bootloader_message(const char *command,
                    const char *status,
                    const char *recovery)
{
    int res = flash_ts_set("bootloader.command", command);
    if (!res)
        res = flash_ts_set("bootloader.status", status);
    if (!res)
        res = flash_ts_set("bootloader.recovery", recovery);
    else {
        debug_printf("Failed to set bootloader command\n");
    }

    return res;
}

static unsigned long checked_usb_read(int device, unsigned long blknr,
    unsigned long blkcnt, void *buffer) {
    block_dev_desc_t desc;
    if (get_usb_storage_desc(device, &desc) != 0) {
        debug_printf("Failed to get device descriptor for usb storage "
            "device %d\n", device);
        return 0;
    }

    if (desc.blksz != USB_BLOCK_SIZ) {
        debug_printf("Invalid USB block size. Expected :%zu. Got: %zu",
            (size_t)USB_BLOCK_SIZ, (size_t)desc.blksz);
        return 0;
    }

    return usb_stor_read(device, blknr, blkcnt, buffer);
}

#ifndef DISABLE_CRASH_COUNTER
void set_android_crash_counter(int cnt)
{
    char value[2];
    value[0] = '0' + cnt;
    value[1] = '\0';
    flash_ts_set(ANDROID_CRASH_COUNTER_KEY, value);
}

int increase_android_crash_counter(void)
{
    int crash_counter = flash_ts_get_int(ANDROID_CRASH_COUNTER_KEY, 0);
    set_android_crash_counter(crash_counter + 1);
}
#endif  // !DISABLE_CRASH_COUNTER

int check_android_recovery_mode(void)
{
    char boot_command[256] = {0};
    int button_status;

#ifndef DISABLE_CRASH_COUNTER
    int crash_counter = flash_ts_get_int(ANDROID_CRASH_COUNTER_KEY, 0);
    debug_printf ("fts: %s: %d\n", ANDROID_CRASH_COUNTER_KEY, crash_counter);
#endif  // !DISABLE_CRASH_COUNTER

    flash_ts_get(BOOTLOADER_COMMAND_KEY, boot_command, sizeof(boot_command));
    debug_printf ("fts: %s: %s\n", BOOTLOADER_COMMAND_KEY, boot_command);

    if (strncmp(boot_command, kFtsBootcmdRecovery, sizeof(kFtsBootcmdRecovery)) == 0) {
        return BOOTMODE_RECOVERY;
    }
    if (strncmp(boot_command, kFtsBootcmdBackupsys,
                sizeof(kFtsBootcmdBackupsys)) == 0) {
        return BOOTMODE_BACKUPSYS;
    }
    /* Boot in to USB on button press only if no other
     * conditions for booting in to recovery are being met. Since
     * button press is also used for factory reset, request to
     * boot in to recovery in fts overrides button press.
     */
    button_status = get_button_status();
    if (button_status){
        lgpl_printf("Detected button press -- booting from USB\n");
        return BOOTMODE_BOOTUSB;
    }
#ifndef DISABLE_CRASH_COUNTER
    if (crash_counter > MAX_CRASH_COUNT) {
        char dev_cfged[64] = {0,};
        flash_ts_get(FTS_DEVICE_CFGED, dev_cfged, sizeof(dev_cfged));
        /* Attempt to boot in to recovery after consecutive crashes
         * only if the device is configured at least once.
         */
        if (!flash_ts_get_int("crashcounter-disable", 0) &&
            !strcmp("true", dev_cfged)) {
            /* TODO(kolla): determine if we need to send a special message
             * to recovery to recover from crash.
             */
            return BOOTMODE_RECOVERY;
        }
    }
#endif  // !DISABLE_CRASH_COUNTER

    return BOOTMODE_NORMAL;
}


#include "sha1.h"

static unsigned int swap32(unsigned int x)
{
    unsigned y;
    y = ((x << 8) & 0xFF00FF00) | ((x >> 8) & 0x00FF00FF);
    y = (y >> 16) | (y << 16);
    return y;
}

int verify_android_image(const unsigned char *img_start, unsigned int len)
{
    int i;
    SHA1Context sha;        /* SHA-1 context                 */
    const boot_img_hdr *hdr = (const boot_img_hdr *)img_start;
    const unsigned char *kernel_data = img_start + hdr->page_size;
    const unsigned char *ramdisk_data = kernel_data + get_aligned(hdr->kernel_size, hdr->page_size);
    const unsigned char *second_data = ramdisk_data + get_aligned(hdr->ramdisk_size, hdr->page_size);
    SHA1Reset(&sha);
    SHA1Input(&sha, kernel_data, hdr->kernel_size);
    SHA1Input(&sha, (const unsigned char *)&hdr->kernel_size, sizeof(hdr->kernel_size));
    SHA1Input(&sha, ramdisk_data, hdr->ramdisk_size);
    SHA1Input(&sha, (const unsigned char *)&hdr->ramdisk_size, sizeof(hdr->ramdisk_size));
    SHA1Input(&sha, second_data, hdr->second_size);
    SHA1Input(&sha, (const unsigned char *)&hdr->second_size, sizeof(hdr->second_size));

    if (!SHA1Result(&sha))
        printf("bootimg SHA: could not compute message digest\n");
    else {
        /* convert it into mkbootimg header's byte sequence */
        for (i = 0; i < 5; i++)
            sha.Message_Digest[i] = swap32(sha.Message_Digest[i]);

        for (i = 0; i < 5; i++)
            if (sha.Message_Digest[i] != hdr->id[i]) {
                printf("bootimg SHA1 verify fail: start 0x%08x, size %d\n"
                        "\tOrig %08X %08X %08X %08X %08X\n"
                        "\tCalc %08X %08X %08X %08X %08X\n",
                        (unsigned int)img_start, len,
                        hdr->id[0],
                        hdr->id[1],
                        hdr->id[2],
                        hdr->id[3],
                        hdr->id[4],
                        sha.Message_Digest[0],
                        sha.Message_Digest[1],
                        sha.Message_Digest[2],
                        sha.Message_Digest[3],
                        sha.Message_Digest[4]);

                return -1;
            }
        return 0;
    }

    return -1;
}

static int bootimg_hdr_verify(unsigned char *img, int boot_src)
{
    UNUSED(img);
    UNUSED(boot_src);
#if (CFG_BOARD_NAME == BOARD_LEXX_B4) \
    || (CFG_BOARD_NAME == BOARD_EARTH_B4) \
    || (CFG_BOARD_NAME == BOARD_HENDRIX_B4) \
    || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
    || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
    unsigned short img_magic = *(short *)(img + CPU_IMG_OFFS_MAGIC);
    unsigned char code_type = img[7];
    unsigned short img_udata = *(short *)(img + CPU_IMG_OFFS_USRDATA);

    if (boot_src == BOOT_SRC_USB){
        /* TODO(kolla): Enable checks for ver, mkt_id etc.*/
        if (img_magic != CPU_IMG_CODE_MAGIC)
            return -1;
        if (code_type != BCM_IMG_USBIMG_TYPE)
            return -1;
        if (img_udata != CPU_IMG_USB_USRDATA)
            return -1;
    } else {
        /* TODO(kolla): Enable robust header checks for NAND image.*/
        if (img_magic != CPU_IMG_CODE_MAGIC)
            return -1;
        if (code_type != BCM_IMG_KERNEL_TYPE)
            return -1;
        if (img_udata != CPU_IMG_NAND_USRDATA)
            return -1;
    }
#endif // BOARD_{LEXX,EARTH,HENDRIX,CHIRP,JOPLIN}_B4
    return 0;
}

static int load_android_image(int bootmode, int boot_src)
{
    ver_table_entry_t *vt_android;
    int iPageSize;
    int iBlockSize;
    unsigned int android_start, android_end;
    int ret;
    unsigned mkbootimg_page;
    unsigned char* k_buff;
    unsigned int k_buff_size;
    unsigned int k_buff_offset;
    unsigned int cpu_img_siz = 0;
    int usb_stor_cnt;
    unsigned char* k_buff_img;
    unsigned int bcm_img_type = BCM_IMG_KERNEL_TYPE;
    unsigned long hdr_blk_cnt = get_aligned(CPU_IMG_OFFS_IMGSTA,
                                  USB_BLOCK_SIZ)/USB_BLOCK_SIZ;

    UNUSED(iPageSize);
    /* Use memory near the end of DDR for loading
     * Android image for verification. Bootloader has
     * configured MMU for flat memory map, i.e. VA == PA.
     */
    k_buff = (unsigned char *)ANDROID_IMG_BUF;
    /* Actual image may start with an offset in k_buff */
    k_buff_img = k_buff;

    /* Wipe the scratch memory first to make sure nothing */
    /* is present from the last boot attempt. */
    memset(k_buff, 0, MAX_ANDROID_IMG_SIZE);

    if (boot_src == BOOT_SRC_USB){
#ifdef CONFIG_USB
        android_start = USB_BOOTIMG_START/USB_BLOCK_SIZ;
        ret = usb_init();
        if (ret < 0){
            lgpl_printf("ERROR: Failed to usb_init ret %d\n", ret);
            return -1;
        }
        /* recognize storage devices */
        usb_stor_cnt = usb_stor_scan();
        if (!usb_stor_cnt){
            lgpl_printf("ERROR: No USB storage device detected\n");
            return -1;
        }
#if (CFG_BOARD_NAME == BOARD_LEXX_B4) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B4) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B4) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
        /* Prepend keystores used for sign verification and decryption
         * to the image being read.
         */
        /* TODO(kolla): Fix CUSTK, encryption of RSAK */
        memcpy(k_buff_img, usb_extrsak_store, sizeof(usb_extrsak_store));
        k_buff_img += USB_EXT_RSAKSTORE_LEN;
#endif  // BOARD_{LEXX,EARTH,HENDRIX,CHIRP,JOPLIN}_B4
        lgpl_printf("Reading %d blocks from USB device \n", hdr_blk_cnt);
        /* Read from first USB device */
        if(checked_usb_read(0, android_start, hdr_blk_cnt, k_buff_img) != hdr_blk_cnt) {
            lgpl_printf("ERROR: Failed to read CPU image header from USB \n");
            return -1;
        }
        lgpl_printf("Read %d blocks from USB device \n", hdr_blk_cnt);
#else
        debug_printf("ERROR: USB boot is not supported\n");
        return -1;
#endif  // CONFIG_USB
    }else{

        if (bootmode == BOOTMODE_RECOVERY)
            vt_android = (ver_table_entry_t *)&vt_recovery;
        else if (bootmode == BOOTMODE_BACKUPSYS)
            vt_android = (ver_table_entry_t *)&vt_backupsys;
        else
            vt_android = (ver_table_entry_t *)&vt_kernel;

        iPageSize = nand_data.szofpg ;
        iBlockSize = nand_data.szofblk;

        android_start = vt_android->part1_start_blkind;
        android_end = android_start + vt_android->part1_blks;
        android_start *= iBlockSize;
        android_end *= iBlockSize;
        debug_printf("NAND: Start to read CPU image header!\n");

        /* Read CPU image header to determing length of the image */
        ret = nand_read_generic(android_start ,
                    android_end,
                    k_buff_img, USB_BLOCK_SIZ);
        if (ret < 0){
            debug_printf("NAND: Failed to read CPU image header!\n");
            return -1;
        }
    }

    cpu_img_siz = *((unsigned int *)(k_buff_img + CPU_IMG_OFFS_IMGSIZ));
    cpu_img_siz = get_aligned(cpu_img_siz, 16);
    cpu_img_siz += CPU_IMG_OFFS_IMGSTA;

    /* Add min size check to be safe */
    int reqd_buf_siz = cpu_img_siz + k_buff_img - k_buff;
    if (reqd_buf_siz > MAX_ANDROID_IMG_SIZE || reqd_buf_siz < MIN_ANDROID_IMG_SIZE) {
        debug_printf("ERROR: aligned CPU img_siz %u is invalid\n",
                     reqd_buf_siz);
        return -1;
    }

    if (boot_src == BOOT_SRC_USB){
#ifdef CONFIG_USB
        int blk_cnt;
        int ret;

        blk_cnt = get_aligned(cpu_img_siz, USB_BLOCK_SIZ)/USB_BLOCK_SIZ;
        /* Always read from first USB storage device */
        lgpl_printf("Reading %d blocks from USB device \n", blk_cnt);

        ret = checked_usb_read(0,
                            (android_start + hdr_blk_cnt),
                            (blk_cnt - hdr_blk_cnt),
                            (k_buff_img + hdr_blk_cnt * USB_BLOCK_SIZ));
        if (ret != (int)(blk_cnt - hdr_blk_cnt)){
            lgpl_printf("ERROR: Failed to read CPU image blk_cnt %d ret %d\n",
                        (blk_cnt - hdr_blk_cnt), ret);
            return -1;
        }
        lgpl_printf("Read %d blocks from USB device \n", blk_cnt);
        /* There can be trailing bytes beyond the end of the image */
        /* due to rounding up to USB_BLOCK_SIZ (and which would */
        /* otherwise be unverified).  Might as well zero all remaining */
        /* memory to be safer. */
        int trailing_bytes = MAX_ANDROID_IMG_SIZE - reqd_buf_siz;
        if (trailing_bytes > 0) {
            lgpl_printf("NOTE: %x trailing bytes zeroed\n", trailing_bytes);
            memset(k_buff_img + cpu_img_siz, 0, trailing_bytes);
        }
        bcm_img_type = BCM_IMG_USBIMG_TYPE;
#endif  // CONFIG_USB
    }else{
        ret = nand_read_generic(android_start,
                    android_end,
                    k_buff_img,
                    cpu_img_siz);
        if (ret < 0){
            lgpl_printf("ERROR: Failed to read CPU image ret %d\n",
                        ret);
            return -1;
        }
    }
    /* Verify image header */
    ret = bootimg_hdr_verify(k_buff_img, boot_src);
    if (ret) {
      lgpl_printf("ERROR: Boot image verify header failed!ret=0x%x\n", ret);
      return -1;
    }
    ret = bcm_image_verify(bcm_img_type, (unsigned) k_buff, (unsigned) k_buff);
    if (ret) {
        lgpl_printf("ERROR: Verify k_buff image failed!ret=0x%x\n", ret);
        return -1;
    }
    memcpy(&Mkbootimg_hdr, k_buff, sizeof(Mkbootimg_hdr));

    if (strncmp((const char *)Mkbootimg_hdr.magic, BOOT_MAGIC, BOOT_MAGIC_SIZE)) {
        printf("ERROR: Invalid mkbootimg header!\n");
        return -1;
    }

    /* force to use predefined kernel addr (CPU0_START_ADDR) in config.
     * We don't use the kernel_addr from kernel partition for scurity
     * consideration.
     */
    Mkbootimg_hdr.kernel_addr = CPU0_START_ADDR;

    TheKernel = (void (*)(int, int, unsigned int))(Mkbootimg_hdr.kernel_addr);

    mkbootimg_page = Mkbootimg_hdr.page_size;
    // align by page_size and add one page for skipping header.
    k_buff_size = mkbootimg_page +
        get_aligned(Mkbootimg_hdr.kernel_size, mkbootimg_page) +
        get_aligned(Mkbootimg_hdr.ramdisk_size, mkbootimg_page) +
        get_aligned(Mkbootimg_hdr.second_size, mkbootimg_page);

    /* Check bootloader has enough memory to be able to
     * read the firmware image for verification.
     */
    if (k_buff_size > MAX_ANDROID_IMG_SIZE){
        debug_printf("BOOTLOADER: Firmware image is too big %d for "
                     "verification\n",
                     k_buff_size);
        return -1;
    }

    UtilMemCpy((void *)Mkbootimg_hdr.kernel_addr, k_buff + mkbootimg_page, Mkbootimg_hdr.kernel_size);
    if (Mkbootimg_hdr.ramdisk_size) {
        k_buff_offset = get_aligned(Mkbootimg_hdr.kernel_size + mkbootimg_page, mkbootimg_page);
        UtilMemCpy((void *)Mkbootimg_hdr.ramdisk_addr, k_buff + k_buff_offset, Mkbootimg_hdr.ramdisk_size);
    }

        bootimg_print_image_hdr(&Mkbootimg_hdr);
    return 0;
}

// Set mac address for 8801.
static void make_wifi_macaddr(char *wifi_mac_addr_buf)
{
    long chipid_reg1;
    char *p;
    int i;

    // Use lowest 24 bit from chip ID.
    chipid_reg1 = readl(MV88DE3100_CHIPID_REG1) ;
    strcpy(wifi_mac_addr_buf, "F8:8F:CA");
    p = wifi_mac_addr_buf + strlen(wifi_mac_addr_buf);

    for(i = 0;i < 3; ++i) {
        char hex;
        hex = chipid_reg1 >> (16 - 8 * i);
        *p++ = ':';
        *p++ = itoc(hex>>4);
        *p++ = itoc(hex);
    }
    *p = '\0';
}

void setup_android_kernel_param(int boot_mode)
{
    char kernel_param[1024];
    char wifi_mac_addr[64] = {0};
    char tmp_buf[256];

    //add flash_ts dev_id info
    snprintf(tmp_buf, sizeof(tmp_buf),
        " flash_ts.dev_id=%d flash_ts.size=%d flash_ts.erasesize=%d flash_ts.writesize=%d",
            fts_dev_id, vt_fts.part1_blks * nand_data.szofblk, nand_data.szofblk, nand_data.szofpg);

    lgpl_printf("mkbootimg bootargs: %s\n", Mkbootimg_hdr.cmdline);

    strlcpy(kernel_param, (CHAR *)Mkbootimg_hdr.cmdline,
        sizeof(kernel_param));
    /* Append mac address and fts command line params to kernel
     * command line.
     */
    strlcat(kernel_param, " ", sizeof(kernel_param));
    strlcat(kernel_param, tmp_buf, sizeof(kernel_param));

    if (Mkbootimg_hdr.ramdisk_size) {
      memset(tmp_buf, 0, sizeof(tmp_buf));
      strlcat(kernel_param, tmp_buf, sizeof(kernel_param));
    }
    lgpl_printf("Generated bootargs: %s\n", kernel_param);

    /* Setup androidboot.hardware and androidboot.bootmode
     * params needed by kernel. Allow boot from USB to gain all
     * permissions a recovery image would have.
     */
    strlcat(kernel_param, " androidboot.hardware="XSTR(BOARD_NAME),
        sizeof(kernel_param));
    if (boot_mode == BOOTMODE_NORMAL)
        strlcat(kernel_param, " androidboot.mode=normal",
            sizeof(kernel_param));
    else if ((boot_mode == BOOTMODE_RECOVERY) ||
            (boot_mode == BOOTMODE_BOOTUSB))
        strlcat(kernel_param, " androidboot.mode=recovery",
            sizeof(kernel_param));
    else if (boot_mode == BOOTMODE_BACKUPSYS)
        strlcat(kernel_param, " androidboot.mode=backupsys",
            sizeof(kernel_param));
    else
        lgpl_printf("ERROR: unknown boot mode %d \n", boot_mode);

    /* Pass bootloader version info to kernel */
    strlcat(kernel_param, " androidboot.bootloader="BOOTLOADER_VERSION,
        sizeof(kernel_param));

    /* Pass NAND boot path source info to kernel */
    {
        const volatile BERLIN_BOOTINFO *pinfo = (BERLIN_BOOTINFO *)BERLIN_BOOTINFO_ADDR;
        snprintf(tmp_buf, sizeof(tmp_buf),
            " nandboot.src_block_num=0x%08x%08x",
            pinfo->nand_src_blk_num[1], pinfo->nand_src_blk_num[0]);
        strlcat(kernel_param, tmp_buf, sizeof(kernel_param));
    }

    /* If the fts param for wifi mfg mode is set, pass module
     * params to enable wifi mfg mode.
     */
    flash_ts_get(FTS_WIFI_MFG_MODE, tmp_buf, sizeof(tmp_buf));
    if ((boot_mode == BOOTMODE_NORMAL) && !strcmp("1", tmp_buf)) {
#if (CFG_BOARD_NAME == BOARD_EARTH_B1) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B2) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B3) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B4) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B1) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B3) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B4) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
        strlcat(kernel_param, " sd8887.mfg_mode=1 sd8887.drv_mode=1",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.fw_name=mrvl/sd8887_mfg_wlan.bin",
            sizeof(kernel_param));
        strlcat(kernel_param, " bt8xxx.fw_name=mrvl/sd8887_mfg_bt.bin",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.cal_data_cfg=mrvl/WlanCalData_sd8887-b1.conf",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.cfg80211_drcs=0",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.ps_mode=2",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.uap_oper_ctrl=0x20001",
            sizeof(kernel_param));
#else
        strlcat(kernel_param, " sd8801.mfg_mode=1 sd8801.drv_mode=1",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.fw_name=mrvl/sd8801_mfg.bin",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.cfg80211_drcs=0",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.ps_mode=2",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.hw_test=1",
            sizeof(kernel_param));
#endif
    } else {
        // Set MAC address only in non-MFG mode.
        make_wifi_macaddr(wifi_mac_addr);

        // Set calibration config file.
#if (CFG_BOARD_NAME == BOARD_LEXX_B3) \
        || (CFG_BOARD_NAME == BOARD_LEXX_B4)
        strlcat(kernel_param, " sd8801.cal_data_cfg=mrvl/WlanCalData_sd8801-b2.conf",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.mac_addr=", sizeof(kernel_param));
        strlcat(kernel_param, wifi_mac_addr, sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.txpwrlimit_cfg=mrvl/txpwr",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.cfg80211_drcs=0",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.ps_mode=2", sizeof(kernel_param));
        strlcat(kernel_param, " sd8801.hw_test=1", sizeof(kernel_param));
#elif (CFG_BOARD_NAME == BOARD_EARTH_B1) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B2) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B3) \
        || (CFG_BOARD_NAME == BOARD_EARTH_B4) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B1) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B3) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B4) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
        strlcat(kernel_param, " sd8887.cal_data_cfg=mrvl/WlanCalData_sd8887-b1.conf", sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.fw_name=mrvl/sd8887_wlan_a2.bin",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.auto_ds=2",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.mac_addr=",
            sizeof(kernel_param));
        strlcat(kernel_param, wifi_mac_addr, sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.txpwrlimit_cfg=mrvl/txpwr",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.cfg80211_drcs=0",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.ps_mode=2",
            sizeof(kernel_param));
        strlcat(kernel_param, " sd8887.uap_oper_ctrl=0x20001",
            sizeof(kernel_param));
        strlcat(kernel_param, " bt8xxx.fw_name=mrvl/sd8887_bt_a2.bin",
            sizeof(kernel_param));
        strlcat(kernel_param, " bt8xxx.bt_fw_serial=0",
            sizeof(kernel_param));
#endif
    }

#if CDP_A0==1
    int rv = detect_reset_state();
    snprintf(tmp_buf, sizeof(tmp_buf), " warm_boot=%d", rv);
    strlcat(kernel_param, tmp_buf, sizeof(kernel_param));
#endif  // CDP_A0==1

    prng_init(kernel_param, sizeof(kernel_param));

    setup_linux_bootparam_with_ramdisk(Mkbootimg_hdr.kernel_addr - 0x8000,
                                       MEMORY_SIZE - Mkbootimg_hdr.kernel_addr,
                                       kernel_param,
                                       Mkbootimg_hdr.ramdisk_addr,
                                       Mkbootimg_hdr.ramdisk_size,
                                       get_soc_rev(), BOARD_REV);
}

// TODO(yichunko): Joplin uses only led_frame_15
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
static void set_topboard_leds(unsigned char* led_frame_15,
                              size_t led_frame_15_size,
                              unsigned char* led_frame_17,
                              size_t led_frame_17_size) {
    if (detect_reset_state() == 1) {
        // This is a warm boot, do not show LEDs
        return;
    }

    // Boot is cold. Show LEDs.
    // See mapping at
    // https://docs.google.com/document/d/1L_DVMgrjZeU3MMt8VjrJeZIAtCuu-HxV1DrQcBH0fTg
    diag_i2c_led_set_frame(LED_MASTER_ID, 0x15, led_frame_15,
                           led_frame_15_size);
    diag_i2c_led_set_frame(LED_MASTER_ID, 0x17, led_frame_17,
                           led_frame_17_size);
}
#endif  // BOARD_CHIRP_B{1,3,4} BOARD_JOPLIN_B{1,3,4}

void start_android_kernel(void)
{
    // TODO: Set appropriate arch parameter.
    unsigned arch = 0;
    unsigned params = Mkbootimg_hdr.kernel_addr - 0x8000 + 0x100;

    if (Bootmode == BOOTMODE_NORMAL) {
        lgpl_printf("Boot normal GTV image\n");
        // crash counter will be cleared after completing GTV boot (by CrashCounter app)
#ifndef DISABLE_CRASH_COUNTER
        increase_android_crash_counter();
#endif  // !DISABLE_CRASH_COUNTER
    } else {
        lgpl_printf("Boot recovery image\n");
#ifndef DISABLE_CRASH_COUNTER
        set_android_crash_counter(0);
#endif  // !DISABLE_CRASH_COUNTER
    }
// TODO(yichunko): Joplin uses only led_frame_15, P1 has 21 leds and P2 has 12
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3)  \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4)  \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
    unsigned char led_frame_15[18] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                                      0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00,
                                      0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF};
    unsigned char led_frame_17[18] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                                      0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00,
                                      0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00};
    set_topboard_leds(led_frame_15, ARRAY_SIZE(led_frame_15),
                          led_frame_17, ARRAY_SIZE(led_frame_17));
#endif  // BOARD_CHIRP_B{1,3,4} BOARD_JOPLIN_B{1,3,4}
#if CDP_A0==1
    set_reset_state_warm();
#endif  //CDP_A0==1
    lgpl_printf("Start kernel at 0x%08x\n", Mkbootimg_hdr.kernel_addr);
    __reset_cpu((int)TheKernel , 1330, params) ;
    TheKernel (0, arch, params);
}

#endif  // ANDROID_BOOT

/*
 * return: the block address after the last block of the image
 */
static long long nand_read_generic(unsigned int start, unsigned int end, unsigned char* data_buff,
                    unsigned int data_size)
{
    unsigned int i,j,k;
    unsigned int block_size, page_size;
    int page_buff[8448/4];
    unsigned int residual, br;

    block_size = nand_data.szofblk;
    page_size = nand_data.szofpg;

    if((start & (block_size - 1)) || (end & (block_size - 1)))
        return -1;
    if( (end < start) || (end - start < data_size))
        return -1;
    j=0;
    for(i=start;i<end;i+=block_size) {
        residual = data_size - j;
        br = residual < block_size ? residual : block_size;
        if(j >= data_size)
            break;
        if(is_block_bad((loff_t)i)) {
            debug_printf("Bad block found @0x%08x.\n", i);
            continue;
        }
        for(k=0;k<br;k+=page_size) {
            unsigned int resd = br - k;
            unsigned int cp_size = resd < page_size ? resd : page_size;
            //get_nand_partition_type((loff_t)i + k);
            //if(NFlash_PageRead((loff_t)i + k, page_buff))
            //int mv_nand_read_large_page(loff_t srcx , char *buf, int oob)
            if (page_size != (UINT32)mv_nand_read_block( (loff_t)i+k, (CHAR *)page_buff, page_size))
            {
                lgpl_printf("Read failed @ 0x%08x\n", i + k);
                return -1;
            }
            UtilMemCpy(data_buff + j + k, page_buff, cp_size);
        }
        j+=block_size;
    }
    if(j < data_size)
        return -1;

    return i;
}

#define NW_START 0x01000000
/*
 * ECC strength parameter is for A0
 * fix addr_cycle to 5
 */
int init_nfc(int block_size, int page_size, int ecc_strength)
{
    // int ret=0;
    // configure device
    unsigned int nand_param_buff[4];
    nand_data.szofpg = page_size;
    nand_data.szofblk = block_size;
    nand_data.t = ecc_strength;

    if((nand_data.szofpg != 512) && (nand_data.szofpg != 2048) &&
        (nand_data.szofpg != 4096) && (nand_data.szofpg != 8192)) {

        UtilMemCpy((char*)nand_param_buff, (char *)(NW_START + 16), 16);
        nand_data.szofpg = nand_param_buff[0];
        nand_data.t = nand_param_buff[1];
        nand_data.szofblk = nand_param_buff[2];
        printf("[%s:%d] get nand parameters from tz_loder (block_size=%d, page_size=%d, ecc=%d)\n",
            __func__, __LINE__, nand_data.szofblk, nand_data.szofpg, nand_data.t);
    }

    if((nand_data.szofpg != 512) && (nand_data.szofpg != 2048) &&
        (nand_data.szofpg != 4096) && (nand_data.szofpg != 8192)) {
    #if 1
        nand_data.szofpg = 0x1000;
        nand_data.szofblk = 0x100000;
        nand_data.t = 48;
        printf("[%s:%d] force nand parameters(block_size=%d, page_size=%d, ecc=%d)\n",
            __func__, __LINE__, nand_data.szofblk, nand_data.szofpg, nand_data.t);
    #else
        printf("[%s:%d] error nand parameters(block_size=%d, page_size=%d, ecc=%d)\n",
            __func__, __LINE__, block_size, page_size, ecc_strength);
        return -1;
        #endif
    }

    debug_printf("NAND block size %d, page size %d, ecc_strength %d\n",
        nand_data.szofblk, nand_data.szofpg, nand_data.t);

    NFlash_Init();
#if 0
    if(NFlash_ResetDevice()){
        lgpl_printf("Reset NAND error!\n");;
//      while(1);
    }
    if( (ret = NAND_ReadData( (unsigned char *)0,
                (unsigned char *)(&block0), sizeof(block0_t) )) != 0 ) {
        lgpl_printf("Read NAND block0 error, ret = %u!\n", ret);;
//      while(1);
        return ret;
    }

    nand_data.ndtr0cs0 = block0.ndtr0cs0;
    nand_data.ndtr1cs0 = block0.ndtr1cs0;
    nand_data.ndredel = block0.ndredel;

    NFlash_Init();
    if(NFlash_ResetDevice()){
        lgpl_printf("reinit NAND error!\n");;
        while(1);
    }
#endif
    return 0;
}



void DisableInterrupt(void)
{
    // int my_cpsr ;

    __asm__ __volatile__(
            "MRS            r0, CPSR"   "\n\t"      //get CRPR
            "BIC            r0, #0xC0"  "\n\t"      // disable IRQ and FIQ
            "MSR            CPSR, r0"  "\n\t"
            :
            :
            :"r0"
        );
}

inline int ctoi(char c)
{
    if((c>='0')&&(c<='9'))
        return c-'0';
    else if((c>='a')&&(c<='f'))
        return c-'a' + 10;
    else if((c>='A')&&(c<='F'))
        return c-'A' + 10;
    else
        return -1;
}

inline char itoc(char i)
{
    int j = i&0xf;
    if(j<10)
        return j+'0';
    else
        return j-10+'A';
}

#define SOC_WOL_BASE    0xf7fe0000
void set_wol_macaddr(char* mac)
{
    int i=0;
    int value[2];
    char *p=(char*)value;

    printf("WOL MAC address: %s\n", mac);
    for(i=0;i<6;i++){
        p[i]=(ctoi(mac[3*i])<<4) + ctoi(mac[3*i+1]);
    }
#if !CDP_A0
    writel(value[0], (SOC_WOL_BASE + RA_WOL_MAC_ADDR_LO));
    writel(value[1], (SOC_WOL_BASE + RA_WOL_MAC_ADDR_HI));
#endif /* FIXME: A0 bring up */
}

#if !ANDROID_BOOT
/*
 * Get MAC address from NAND
 */
static int get_mac_addr(char* macaddr)
{
    int i;

#define NAND_UBOOT_ENV_BLOCK_START  (12)
    int uboot_env_start = NAND_UBOOT_ENV_BLOCK_START * nand_data.szofblk;
    int uboot_env_end = 0x4000000;
    int uboot_env_size = 0x1000;
    char *env_buff = UtilMemAlloc(uboot_env_size);
    int blk_size = nand_data.szofblk;
    char *tmp_buf;

    for(i=0;i<4;i++) {
        if(nand_read_generic(uboot_env_start + i*blk_size,
                             uboot_env_end, (unsigned char *)env_buff,
                             uboot_env_size) >= 0) {
            break;
        }
    }

    if(i==4) {
        UtilMemCpy(macaddr, "macaddr=00:11:22:33:44:55", 25);
        macaddr[25]='\0';
        return -1;
    }

    i=0;
    while(i<uboot_env_size) {
        tmp_buf = strstr(env_buff, "ethaddr=");
        if(tmp_buf)
            break;
        i += strlen(env_buff);
        env_buff += strlen(env_buff);
        i++;
        env_buff++;
    }
    // lgpl_printf("uboot env: %s\n", env_buff);
    if(i>=uboot_env_size) {
        UtilMemCpy(macaddr, "macaddr=00:11:22:33:44:55", 25);
        macaddr[25]='\0';
        return -1;
    }
    strcpy(macaddr, "macaddr=");
    UtilMemCpy(macaddr+8, tmp_buf+8, 17);
    macaddr[25]='\0';
    return 0;
}
#endif  // !ANDROID_BOOT

#if !ANDROID_BOOT
static void init_mac_address(long long chipid, const char* old_param, char* new_param)
{
    int len;
    char *new = new_param;

    debug_printf("cpu id:%08x %08x\n", (int)(chipid>>32), (int)(chipid));
    len = strlen(old_param);
    UtilMemCpy(new, old_param, len);
    new+=len;
    *new = ' ';
    new++;
    make_mac_from_chipid(chipid, new);
    set_wol_macaddr(new+8);
/*
    UtilMemCpy(new, " macaddr=",sizeof(" macaddr=")-1);
    new+=sizeof(" macaddr=")-1;
    *new++='0';
    *new++='0';
    for(i=0;i<5;i++) {
        char hex;
        hex = chipid >> (32-8*i);
        *new++=':';
        *new++=itoc(hex>>4);
        *new++=itoc(hex);
    }
    *new='\0';
*/
}

static long long get_next_img_addr(unsigned int start, unsigned int end, unsigned int data_size)
{
    unsigned int i,j;
    unsigned int block_size;

    block_size = nand_data.szofblk;
    j = (data_size + block_size - 1) & ~(block_size - 1);
    for(i=start;i<end;i+=block_size) {
        if(j==0)
            break;
        if(is_block_bad((loff_t)i)) {
            debug_printf("Bad block found @0x%08x.\n", i);
            continue;
        }
        j-=block_size;
    }
    if(i >= end)
        return -1;

    return i;
}
#endif  // !ANDROID_BOOT

#if BG2CDP
typedef struct BTLD_CTX {
    //unsigned char * gp_vt_buf; // VT_BUF_ADDR
    //unsigned char *gp_figo_buf; // FIGO_BUF_ADDR;
    //unsigned char *gp_key_buf; // KEY_BUF_ADDR;
    //unsigned char * gp_loader_buf; // Loader_BUF_ADDR;
    //int iVT_OFFSET;
    //int iLoader_OFFSET;
    unsigned blk_addr;

    //unsigned has_lastk;
    unsigned char lastk[1024*3];
    char partition_info_buff[4096];
} BTLD_CTX;
BTLD_CTX g;



unsigned berlin_delay(unsigned int tick)
{
    volatile unsigned int itick = tick * 250;
    while (itick--);
    return itick;
}

#define BCM_STATUS_BCM_FAULT               (1<<10)
#define BCM_STATUS_BOOTSTRAP_IN_PROGRESS   (1<<9)
#define BCM_STATUS_BCM_READY               (1<<8)
#define BCM_STATUS_BCM_CMD_FLIP            (1<<7)

int bcm_image_verify(unsigned int type, unsigned int src, unsigned int dst)
{
    unsigned status;
    unsigned waitCount;
    unsigned i, *p32;
    volatile NOT_MAILBOX volatile *mb = (NOT_MAILBOX *) BCM_MAILBOX;

    p32 = (unsigned *) src;
    lgpl_printf("\tverify 0k:0x%x, 1k:0x%x, 2k:0x%x\n", p32[1], p32[1+1024/4], p32[1+2048/4]);

    status = mb->command_fifo_status;
    // lgpl_printf("bcm fifo staus = 0x%x\n", status);

    // older erom will have BCM_STATUS_BOOTSTRAP_IN_PROGRESS set
    if (0 == (status & BCM_STATUS_BOOTSTRAP_IN_PROGRESS))
    {
        // wait for bcm ready
        // lgpl_printf("wait for bcm ready...\n");
        // for (waitCount=~255; waitCount; waitCount--)
        while (0 == (status & BCM_STATUS_BCM_READY))
        {
            if (status != mb->command_fifo_status)
            {
                status = mb->command_fifo_status;
                // lgpl_printf("bcm fifo staus = 0x%x\n", status);
            }
        }
    }

    //lgpl_printf("send BCM cmd: 0x%x 0x%x 0x%x 0x%x: 0x%x\n", BCM_PI_IMAGE_VERIFY, type, src, dst, mb-> host_interrupt_register);

    mb->primitive_command_parameter0 = type;
    mb->primitive_command_parameter1 = src;
    mb->primitive_command_parameter2 = dst;
    mb->secure_processor_command = BCM_PI_IMAGE_VERIFY;

    //for (waitCount=0; waitCount<~255; waitCount++) // Wait_For_WTM_Complete( 0x10000, pCtrl );
    for (waitCount=0; ; waitCount++) // Wait_For_WTM_Complete( 0x10000, pCtrl );
    {
        //if ((mb->command_fifo_status & BCM_STATUS_BCM_CMD_FLIP) != status)
            //break;
        // wait for "command complete" or timeout
        if( mb-> host_interrupt_register & 0x1 )
            break;
        berlin_delay(100);
    }

    mb-> host_interrupt_register = 0x70001; // Clear_WTM_Interrupts( 0xffffffff, pCtrl );
    status = mb->command_return_status;
    i = mb->command_status_0;

    lgpl_printf("\tverify image %x, size=%d, waitcount=%d\n", status, i, waitCount);

    return status;
}
#endif  // BG2CDP

#if ANDROID_BOOT
/*
 * For Android not need to load SM from bootimgs.
 * Currently, we use this solution. Because it can save the space of flash
 * memory of bootimgs & bootimgs-B. And currently, there is a bug in recoery
 * scripts, it doesn't update bootimgs-B.
 */

int Image_Load_And_Start(void)
{
    int ret= 0;
#ifdef ENABLE_CONNECT_BTN
        /* the second time to check connect btn */
#ifndef BG2CDP
        Check_connectbtnGPIO();
#endif
#endif

#ifdef ENABLE_CONNECT_BTN
        /* the third time to check connect btn */
#ifndef BG2CDP
        Check_connectbtnGPIO();
#endif
#endif
    lgpl_printf("**************************************************************** \n");
    lgpl_printf("                  Read and setup kernel Image                    \n");
    lgpl_printf("**************************************************************** \n");
    Bootmode = check_android_recovery_mode();
    if (Bootmode == BOOTMODE_BOOTUSB)
        ret = load_android_image(Bootmode, BOOT_SRC_USB);
    else
        ret = load_android_image(Bootmode, BOOT_SRC_NAND);
    if (ret != 0 && Bootmode == BOOTMODE_NORMAL) {
        lgpl_printf("Invalid normal boot image! Boot recovery image...\n");
        write_android_bootloader_message("boot-recovery", "", "recovery\n--show_text\n");
        Bootmode = BOOTMODE_RECOVERY;
        ret = load_android_image(Bootmode, BOOT_SRC_NAND);
    } else if (ret != 0 && Bootmode == BOOTMODE_RECOVERY) {
        lgpl_printf("Invalid recovery boot image! Boot normal image...\n");
        /* Note: we don't clear the recovery flags, we just let device
         *       to enter recovery mode if recovery mode is avaliable.
         * write_android_bootloader_message("", "", "");
         */

        Bootmode = BOOTMODE_NORMAL;
        ret = load_android_image(BOOTMODE_NORMAL, BOOT_SRC_NAND);
    }
    if (ret != 0) {
        /* Booting from NAND failed, try boot from USB */
        lgpl_printf("Booting from NAND failed, booting from USB....!\n");
        Bootmode = BOOTMODE_BOOTUSB;
        ret = load_android_image(Bootmode, BOOT_SRC_USB);
    }
    if (ret != 0){
        lgpl_printf("FATAL ERROR! There is no bootable image on this machine!\n");
        return -1;
    }

    /* Turn on Red LED indicating booting */
    set_led_backlight(LED_RED_PWM, 100);

    /*
     * setup linux boot parameter
     */
    setup_android_kernel_param(Bootmode);

    return 0;
}

#else /* !#if ANDROID_BOOT */
/* For
 * - Linux
 * - Android needs to load SM from bootimgs
 */
int Image_Load_And_Start()
{
    int iPageSize = 0;
    int iBlockSize  ;

    int ret= 0;

    unsigned int img3_start, img3_end;
    unsigned int en_size;

    char *sm_buff, *cpu0_buff, *cpu1_buff, *en_buff;
    int sm_addr, cpu0_addr, cpu1_addr, recou_addr, en_addr;

    long long chipid;

    iPageSize = nand_data.szofpg ;
    iBlockSize = nand_data.szofblk;

#ifdef ENABLE_CONNECT_BTN
        /* the second time to check connect btn */
        Check_connectbtnGPIO();
#endif

    if((vt_img3.part1_version.major_version > vt_img3.part2_version.major_version)
        || ((vt_img3.part1_version.major_version == vt_img3.part2_version.major_version)
         &&(vt_img3.part1_version.minor_version > vt_img3.part2_version.minor_version)))
    {
        img3_start = vt_img3.part1_start_blkind;
        img3_end = img3_start + vt_img3.part1_blks;
        img3_start *= iBlockSize;
        img3_end *= iBlockSize;
    }
    else
    {
        img3_start = vt_img3.part2_start_blkind;
        img3_end = img3_start + vt_img3.part2_blks;
        img3_start *= iBlockSize;
        img3_end *= iBlockSize;
    }

    /****************************************************************
     * Read Image 3 Header
     ****************************************************************/
    debug_printf("Image3 start:%08x, end: %08x\n", img3_start, img3_end);

    debug_printf("Reading image3 attribute ...\n");

    sm_addr = nand_read_generic(img3_start, img3_end, &img3_hdr, sizeof(Image3_Attr));
    if(sm_addr < 0)
    {
        lgpl_printf("Read image3 header error!\n");
        return -1;
    }
    //strcpy(img3_hdr.linux_bootargs, LINUX_PARAM_COMMON_PART);
    UtilMemCpy(img3_hdr.linux_bootargs, linux_bootargs, ((sizeof(linux_bootargs)+3)&~3));

    sm_hdr = &img3_hdr.sm_param;
    cpu0_hdr = &img3_hdr.cpu0_param;
    cpu1_hdr = &img3_hdr.cpu1_param;
    recou_hdr = &img3_hdr.recou_param;
    sMem_Layout = &img3_hdr.mem_layout;

#ifdef CONFIG_FORCE_ENCRYPTION
    sm_hdr->bsm_image_encrypt = 1;
    cpu0_hdr->bcpu0_image_encrypt = 1;
    cpu1_hdr->bcpu1_image_encrypt = 1;
#endif  // CONFIG_FORCE_ENCRYPTION
    en_size = sm_hdr->bsm_image_encrypt | cpu0_hdr->bcpu0_image_encrypt | cpu1_hdr->bcpu1_image_encrypt;

    cpu0_addr = get_next_img_addr(sm_addr, img3_end, sm_hdr->uism_image_final_size);
    cpu1_addr = get_next_img_addr(cpu0_addr, img3_end, cpu0_hdr->uicpu0_image_final_size);
    recou_addr = get_next_img_addr(cpu1_addr, img3_end, cpu1_hdr->uicpu1_image_final_size);
    en_addr = get_next_img_addr(recou_addr, img3_end, recou_hdr->size);

    debug_printf("sm_addr: %08x, cpu0_addr: %08x, cpu1_addr: %08x, recou_addr: %08x, en_addr: %08x\n",
            sm_addr, cpu0_addr, cpu1_addr, recou_addr, en_addr);
    debug_printf("Memory map: CPU0 %uM, CPU1 %uM, ZSP: %uM, SHR %uM\n",
            sMem_Layout->uicpu0_mem_size>>20, sMem_Layout->uicpu1_mem_size>>20,
            sMem_Layout->uizsp_mem_size>>20, sMem_Layout->uishm_mem_size>>20);
    if((cpu0_addr < 0) || (cpu1_addr < 0) || (recou_addr < 0) || (en_addr < 0))
    {
        lgpl_printf("Scan image3 error.\n");
        lgpl_printf("sm_addr: %08x, cpu0_addr: %08x, cpu1_addr: %08x, recou_addr: %08x, en_addr: %08x\n",
            sm_addr, cpu0_addr, cpu1_addr, recou_addr, en_addr);
        return -1;
    }

    if (!en_size)
    {
      UtilMemSet((void *)&en_hdr, 0, sizeof(en_hdr));
    }
    else
    {

      if(nand_read_generic(en_addr, img3_end, &en_hdr, sizeof(en_hdr)) < 0)
      {
        lgpl_printf("Read image3 header error!\n");
        return -1;
      }


      //    debug_printf("Encryption atrribute:\n");
      //    for(k=0;k<sizeof(en_hdr)/4;k++) debug_printf("0x%08x.\n", ((int *)&en_hdr)[k]);
      // currently, we don't support CPU1 image
      en_size = en_hdr.uiCPU0_Encrypt_Image_Header_Offset + en_hdr.uiCPU0_Encrypt_Image_Header_Size;
      debug_printf("Encryption image size= %d\n", en_size);
      en_buff = UtilMemAlloc(en_size);
      if(!en_buff) {
        lgpl_printf("Malloc failed.\n");
        return -1;
      }
      if(nand_read_generic(en_addr, img3_end, (unsigned char *)en_buff, en_size) < 0)
      {
        lgpl_printf("Read image3 header error!\n");
        return -1;
      }
    }

    debug_printf("encryption header copied\n");
#ifdef BG2CDP
#else
    /****************************************************************
     * Load and start SM
     ****************************************************************/
    {
          // make Beetle "DDR_Off" case like the other warmups: there's no need to reload
          // SM. Even though DDR is gone, it doesn't affect SM.

                /*  ( MV_SoC_STATE_SYSRESET != uiBoot ) */ ))
    {
        //  for cold boot up, Image-2 load SM's image and kick off Sm


        //  when box product release is defined, SM will shut down SoC after power on.
        //  this can be done by write the cold boot flag to SM's register
#ifdef  BOX_PRODUCT_RELEASE
        {
#else
        if(false) {
#endif
            debug_printf("Set cold boot flag: %08x@%08x\n", MV_SM_RSTFLAG_COLD, SM_BOOT_FLAG_REG);
            writel(MV_SM_RSTFLAG_COLD, SM_BOOT_FLAG_REG);
        }

        // copy SM image from NAND
        debug_printf("Loading SM image... final size %u\n", sm_hdr->uism_image_final_size);
        if(!sm_hdr->bsm_image_encrypt)
            en_hdr.uiSM_Encrypt_Image_Header_Size = 0;

        sm_buff = UtilMemAlloc(sm_hdr->uism_image_final_size + en_hdr.uiSM_Encrypt_Image_Header_Size);
        if(!sm_buff) {
            lgpl_printf("Malloc failed.\n");
            return -1;
        }

        UtilMemCpy(sm_buff, en_buff + en_hdr.uiSM_Encrypt_Image_Header_Offset, en_hdr.uiSM_Encrypt_Image_Header_Size);

        if(nand_read_generic(sm_addr, cpu0_addr, (sm_buff+en_hdr.uiSM_Encrypt_Image_Header_Size), sm_hdr->uism_image_final_size) < 0)
        {
            lgpl_printf("Read sm image error!\n");
            return -1;
        }

        if(sm_hdr->bsm_image_encrypt) {
            debug_printf("Encryption key index: %u\n", sm_buff[0]);
            ret = VerifyImage((unsigned int)sm_buff, sm_hdr->uism_encrypt_image_size, (unsigned int)sm_buff);
            if(ret) {
                lgpl_printf("Verify SM image failed! ret=%d\n", ret);
                return -1;
            }
            debug_printf("verify passed.\n");
        }

        if(sm_hdr->bsm_image_bss_init)
            UtilMemSet(sm_buff + sm_hdr->uism_bss_start_addr - sm_hdr->uism_image_load_addr, 0, sm_hdr->uism_bss_length);

#ifdef ENABLE_CONNECT_BTN
        /* the third time to check connect btn */
        Check_connectbtnGPIO();
#endif

        debug_printf("Done.\n");
        //  download SM image from DDR to ITCM and kick off SM
        MV_SM_Dev_Init((unsigned char*)sm_buff, sm_hdr->uism_image_ori_size ) ;
        debug_printf("SM CPU is kicked off.\n");

        //  send msg to SM to power off SoC
        //  FIGO_SRAM_ENTRY() ;
    }
#endif  // CONFIG_SM

    /****************************************************************
     * Read and setup kernel Image
     ****************************************************************/

#define mem_buff cpu0_buff
    cpu0_buff = (char *)cpu0_hdr->uicpu0_image_load_addr - sizeof(linux_hdr_t);
    {

        debug_printf("Loading CPU0 image... load address: 0x%x, final size %u\n", (int)cpu0_buff,cpu0_hdr->uicpu0_image_final_size);
        if(cpu0_hdr->bcpu0_image_encrypt)
            UtilMemCpy(cpu0_buff, en_buff + en_hdr.uiCPU0_Encrypt_Image_Header_Offset, en_hdr.uiCPU0_Encrypt_Image_Header_Size);
        else
            en_hdr.uiCPU0_Encrypt_Image_Header_Size = 0;

        if(nand_read_generic(cpu0_addr, cpu1_addr, (cpu0_buff+en_hdr.uiCPU0_Encrypt_Image_Header_Size), cpu0_hdr->uicpu0_image_final_size) < 0)
        {
            lgpl_printf("Read CPU0 image error!\n");
            return -1;
        }

        if(cpu0_hdr->bcpu0_image_encrypt) {
            #if BG2CDP
            ret = load_lastk(g.lastk);
            if (ret)
            {
                lgpl_printf("load last key failed!ret=0x%x\n", ret);
                return -1;
            }
            ret = bcm_image_verify(BCM_IMG_KERNEL_TYPE, (unsigned) mem_buff, (unsigned) cpu0_buff);
            if (ret)
            {
                lgpl_printf("Verify cpu0 image failed!ret=0x%x\n", ret);
                return -1;
            }
            #else
            debug_printf("Encryption key index: %u\n", cpu0_buff[0]);
            ret = VerifyImage((unsigned int)cpu0_buff, cpu0_hdr->uicpu0_encrypt_image_size, (unsigned int)cpu0_buff);
            if(ret) {
                lgpl_printf("Verify cpu0 image failed!ret=%d\n", ret);
                return -1;
            }
            #endif
            debug_printf("verify passed.\n");
        }

    //  Xiufeng:    do not use ramdisk for BG2
#if 1
    {
        linux_hdr_t *lhdr = cpu0_buff;
        int len;

        debug_printf("kernel_size=%d, ramdisk_size=%d, ramdisk_addr=%x\n",
                lhdr->kernel_size, lhdr->ramdisk_size, lhdr->ramdisk_addr);
        debug_printf("reloading ramdisk...");

        UtilMemCpy(lhdr->ramdisk_addr,
                cpu0_hdr->uicpu0_image_load_addr+lhdr->kernel_size,
                lhdr->ramdisk_size);


        debug_printf("DONE!\n");

        debug_printf("append initrd to linux bootargs\n");
        len = strlen((char *)img3_hdr.linux_bootargs);
        snprintf(img3_hdr.linux_bootargs + len,
            sizeof(img3_hdr.linux_bootargs) - len, " initrd=0x%x,0x%x",
                lhdr->ramdisk_addr, lhdr->ramdisk_size);
#if defined(ROOTFS_SQUASHFS)
        debug_printf("append rootfs type to linux bootargs\n");
        len = strlen((char *)img3_hdr.linux_bootargs);
        snprintf(img3_hdr.linux_bootargs + len,
            sizeof(img3_hdr.linux_bootargs) - len),
            " rootfstype=squashfs");
#endif
        debug_printf("%s\n", img3_hdr.linux_bootargs);
    }
#endif
    if(cpu0_hdr->bcpu0_image_bss_init)
        UtilMemSet((void*)cpu0_hdr->uicpu0_bss_start_addr, 0, cpu0_hdr->uicpu0_bss_length);

    }
    debug_printf("Done.\n");

        /*
         * setup linux boot parameter
         */
        //  if MV88DE3010, try to use CPUID as MAC address
#if !CDP_A0
        chipid = readl(MV88DE3100_CHIPID_REG1) ;
        chipid += (long long)(readl(MV88DE3100_CHIPID_REG2)) << 32;
#else
        chipid = 0x12345678abcdef0;
#endif /* FIXME: A0 bring up */

        if(chipid && block0.first_burn_flag){
            char tmp_param[1024];
            char tmp_buf[64];
            init_mac_address(chipid, (char *)(&img3_hdr.linux_bootargs), tmp_param);
            debug_printf("Real bootargs: %s\n", tmp_param);
            setup_linux_bootparam(cpu0_hdr->uicpu0_image_load_addr-0x8000, sMem_Layout->uicpu0_mem_size, tmp_param);
        } else {
            char tmp_param1[1024];
            char mac_addr[32];

            get_mac_addr(mac_addr);
            set_wol_macaddr(&mac_addr[8]);

            strlcpy(tmp_param1, img3_hdr.linux_bootargs, sizeof(tmp_param1));
            strcat(tmp_param1, " ");
            strncat(tmp_param1, mac_addr, sizeof(mac_addr));
            {
                char tmp_buf[64];
                //  Xiufeng:    Fix me
                //  The No.2 parameter uicpu0_mem_size, is not used by Linux.

                debug_printf("REal bootargs: %s\n", tmp_param1);
                setup_linux_bootparam(cpu0_hdr->uicpu0_image_load_addr-0x8000,
                    sMem_Layout->uicpu0_mem_size, tmp_param1);

            }
            debug_printf("%d: Done!\n", __LINE__);
        }
#if 0
            setup_linux_bootparam(cpu0_hdr->uicpu0_image_load_addr-0x8000,
                    sMem_Layout->uicpu0_mem_size, LINUX_BOOTARGS);

#endif
        //  Xiufeng: Here we set CHIP_CTRL_SW_GENERIC3_REG to 1 to keep consistent with Berlin
        //       In ideal case, BG2 Linux does not need NFC sync, but bootflow does not
        //       rely on this
        reg(CHIP_CTRL_SW_GENERIC3_REG) = 1;//allow Linux to use NFC

    return 0;
}
#endif  // ANDROID_BOOT

#include "bcm_primitive.h"
#include "bcm_status.h"
#include "bcm_mailbox.h"

// if here, Image-3 is copied into DDR
int Image_3_Verify_Jump(void)
{
    // write 0xBF0A0001 to CHIP_CTRL_SW_GENERIC0_REG
    // This is the flag to eCos passed by Bootflow. This value means the AVPLL
    // has been set by bootflow, eCos shall not reset AVPLL
    GA_REG_WORD32_WRITE(CHIP_CTRL_SW_GENERIC0_REG, 0xBF0A0001) ;

    // any state reach here should start/resume kernel
#if ANDROID_BOOT
    {
        /* flush the console output */
        putchar('\n');
        bcm_usb_console_func(EROM_TURN_OFF_USB_DEVICE);
        start_android_kernel();
    }
#else /* #if ANDROID_BOOT */

    reg(CHIP_CTRL_SW_GENERIC3_REG) = 0x1 ;
    {
        lgpl_printf("JUMp to kernel entrance 0x%08x\n", cpu0_hdr->uicpu0_image_load_addr);
        //parameter0: Linux load address
        //parameter1: 1330
        //parameter2: Linux Command line address

        /* flush the console output */
        putchar('\n');
        bcm_usb_console_func(EROM_TURN_OFF_USB_DEVICE);
        __reset_cpu(cpu0_hdr->uicpu0_image_load_addr, 1330, cpu0_hdr->uicpu0_image_load_addr-0x8000) ;
    }
#endif  // ANDROID_BOOT

    return 0;
}

void dump_version_entry(ver_table_entry_t * vte)
{
    printf("%s: part1(start=%d, blks=%d, version=%08u%04u), part2(start=%d, blks=%d, version=%08u%04u)\n", vte->name,
            vte->part1_start_blkind, vte->part1_blks, vte->part1_version.major_version, vte->part1_version.minor_version,
            vte->part2_start_blkind, vte->part2_blks, vte->part2_version.major_version, vte->part2_version.minor_version);
}

unsigned int crc32(unsigned int crc, unsigned char *buf, unsigned int len);
#define CRC32_SIZE      (4)
int get_version_table()
{
    unsigned int i,j,ret=0;
    int img3_found;
#if ANDROID_BOOT
    int android_kernel_found=0;
    int android_recovery_found=0;
#endif
    version_table_t *vt = (version_table_t *)g.partition_info_buff;
    ver_table_entry_t * vt_entry;
    unsigned vt_size = 0;
    unsigned int dev_id = 0;
    unsigned iVT_OFFSET;

    #if BG2CDP
    iVT_OFFSET = nand_data.szofblk - 4096;
    #else
    iVT_OFFSET = VT_OFFSET;
    #endif


    for(i=1; i<9;i++) {
        if(is_block_bad((loff_t)i*nand_data.szofblk)) {
            debug_printf("Bad block found @0x%08x.\n", i*nand_data.szofblk);
            continue;
        }

        ret = mv_nand_read_block( (long long)i*nand_data.szofblk + iVT_OFFSET,
            g.partition_info_buff, sizeof(g.partition_info_buff) );
        if( ret != sizeof(g.partition_info_buff) ) {
            debug_printf("Read version table failed, i=%u, ret=%u.\n", i, ret);
            continue;
        }
        vt_size = sizeof(version_table_t) + vt->num_entries * sizeof(ver_table_entry_t);
        /* verify the version table data */
        if(vt->magic != MAGIC_NUMBER || 0xffffffff != crc32(0, (UCHAR *)g.partition_info_buff, vt_size + CRC32_SIZE)) {
            lgpl_printf("error magic number! %x\n", vt->magic);
            continue;
        }

        UtilMemCpy(g.lastk, &g.partition_info_buff[3*1024], 1024);

        dev_id = 0;
        img3_found = 0;
        for(j=0;j<vt->num_entries;j++) {
            vt_entry = &vt->table[j];
            printf("[%02d,d%02d] ", j, dev_id);
            dump_version_entry(vt_entry);
            if(UtilMemCmp(IMG3_NAME, vt_entry->name, sizeof(IMG3_NAME)) == 0) {
                UtilMemCpy(&vt_img3, vt_entry, sizeof(ver_table_entry_t));
                img3_found = 1;
            }
            #if 0
            if(UtilMemCmp(SYSCONF_NAME, vt_entry->name, sizeof(SYSCONF_NAME)) == 0) {
                UtilMemCpy(&vt_sysconf, vt_entry, sizeof(ver_table_entry_t));
            }
            if(UtilMemCmp(FLASHLESS_NAME, vt_entry->name, sizeof(FLASHLESS_NAME)) == 0) {
                UtilMemCpy(&vt_flashless, vt_entry, sizeof(ver_table_entry_t));
            }
            #endif
#if ANDROID_BOOT
            if(UtilMemCmp(FTS_NAME, vt_entry->name, sizeof(FTS_NAME)) == 0) {
                fts_dev_id = dev_id;
                UtilMemCpy((void *)&vt_fts, vt_entry, sizeof(ver_table_entry_t));
            }
            if(UtilMemCmp(KERNEL_NAME, vt_entry->name, sizeof(KERNEL_NAME)) == 0) {
                UtilMemCpy((void *)&vt_kernel, vt_entry, sizeof(ver_table_entry_t));
                android_kernel_found = 1;
            }
            if(UtilMemCmp(RECOVERY_NAME, vt_entry->name, sizeof(RECOVERY_NAME)) == 0) {
                UtilMemCpy((void *)&vt_recovery, vt_entry, sizeof(ver_table_entry_t));
                android_recovery_found = 1;
            }
#endif
            if (vt_entry->part1_start_blkind != vt_entry->part2_start_blkind)
                /* double copy of the partition */
                dev_id += 2;
            else
                dev_id += 1;
        }
#if ANDROID_BOOT
        if(img3_found && android_kernel_found && android_recovery_found)
            break;
#else
        if(img3_found)
            break;
#endif
    }

    if(i==9) {
        lgpl_printf("Scan version table error!\n");
        return -1;
    }
    return 0;
}
#if 0 //BG2CDP
MV_Block1_Item layout_item;
MV_Block1_Item *loader_item;

int get_block1_layout(void *buf)
{
    int i, type;
    int iVT_OFFSET = VT_OFFSET;
    MV_Block1 *Layout = (MV_Block1 *) buf;

    if (Layout->head.MagicNumber == 0xda7a)
    {
        for (i=0; i<8; i++)
        {
            type = Layout->item[i].param&0xF;
            if (type == BLOCK1_IMAGE_TYPE_VT)
            {
                iVT_OFFSET = Layout->item[i].start;
                //debug_printf("VT block @0x%08x.\n", iVT_OFFSET);
                break;
            }
        }
    }
    return iVT_OFFSET;
}

unsigned status_32[17];
int bcm_image_verify(unsigned int type, unsigned int src, unsigned int dst)
{
    unsigned waitCount;
    unsigned i, *p32;
    NOT_MAILBOX volatile *mb = (NOT_MAILBOX *) BCM_MAILBOX;

    p32 = &mb->primitive_command_parameter0;
    p32[0] = type;
    p32[1] = src;
    p32[2] = dst;
    for (i=3; i<16; i++)
        p32[i] = 0;
    p32[i] = BCM_PI_IMAGE_VERIFY;

    lgpl_printf("Verifying cpu0 image @0x%x ... ", src);

    //for (waitCount=0x10000000; waitCount; waitCount--) // Wait_For_WTM_Complete( 0x10000, pCtrl );
    //for (waitCount=0; waitCount<0x1000000; waitCount++) // Wait_For_WTM_Complete( 0x10000, pCtrl );
    for (waitCount=0; ; waitCount++) // Wait_For_WTM_Complete( 0x10000, pCtrl );
    {
        // wait for "command complete" or timeout
        if( mb-> host_interrupt_register & 0x1 )
            break;
        //if (waitCount>~7)
            //break;
    }
    lgpl_printf("countup = 0x%08x.\n", waitCount);

    status_32[0] = ~0;
    status_32[1] = 0;
    //if( waitCount != 0 )
    {
        p32 = &mb-> command_return_status;
        for (i=0; i<17; ++i)
            status_32[i] = p32[i];
        //mb-> host_interrupt_register = ~0; // Clear Interrupts
    }
    mb-> host_interrupt_register = 0x70001; // Clear Interrupts
    lgpl_printf("Verify status=0x%x, size=%d.\n", status_32[0], status_32[1]);

    return status_32[0];
}
#endif

//char partition_info_buff[4096 + 32];
unsigned char partition_type_s[16];
#if defined(CONFIG_SLC)
extern unsigned int slc_partition_start;
#endif
int gen_nand_partition_info()
{
    unsigned int i, ret=0;

    version_table_t *vt = (version_table_t *)g.partition_info_buff;
    unsigned vt_size = 0;
    unsigned iVT_OFFSET;

    #if BG2CDP
    iVT_OFFSET = nand_data.szofblk - 4096;
    #else
    iVT_OFFSET = VT_OFFSET;
    #endif

    strncpy((CHAR *)partition_type_s, "eslc", 4);

    for(i=1; i<9;i++) {
        if(is_block_bad((loff_t)i*nand_data.szofblk)) {
            lgpl_printf("Bad block found @0x%08x.\n", i*nand_data.szofblk);
            continue;
        }

        // go get vt
        ret = mv_nand_read_block( (long long)i*nand_data.szofblk + iVT_OFFSET,
            (CHAR *)g.partition_info_buff, sizeof(g.partition_info_buff) );
        if( ret != sizeof(g.partition_info_buff) ) {
            lgpl_printf("get partition info failed, i=%u, ret=%u.\n", i, ret);
            continue;
        }
        vt_size = sizeof(version_table_t) + vt->num_entries * sizeof(ver_table_entry_t);
        /* verify the version table data */
        if(vt->magic != MAGIC_NUMBER || 0xffffffff != crc32(0, (UCHAR *)g.partition_info_buff, vt_size + CRC32_SIZE)){
            lgpl_printf("error magic number! %x\n", vt->magic);
        }else
            break;
    }
    if(i == 9) {
        lgpl_printf("Scan partition info error!\n");
        return -1;
    }
    return 0;
}

void dump_mem(unsigned int addr, int count)
{
    int i;
    for (i = 0; i < count; i++, addr += 4) {
        if (i % 4 == 0) {
            if (i)
                printf("\n");
            printf("0x%08x: ", addr);
        }
        printf("%08x ", readl(addr));
    }
    printf("\n");
}

#if defined(DEBUG)
static unsigned int read_otp_status(void)
{
    return readl(MV88DE3XXX_OTP_STS_REG);
}

static unsigned int read_otp_lkg_curr(void)
{
    return readb(MV88DE3XXX_OTP_LKG_ID) * 4;
}
#endif

void reset_usb_console();

void StartBootLoader(int block_size, int page_size, int ecc_strength)
{
    int ret = 0;
    uiWarmDown_2_Linux_Addr = ~0; //readl((SM_WARMDOWN_2_LINUX_ADDR)) ;

    UtilMemSet((unsigned char *)BSS_START, 0, BSS_END - BSS_START);

    pin_init();

    reset_usb_console();

#if BG2CDP
    lgpl_printf("%s BG2CD+: %x\n", BOARD_NAME, BERLIN_CHIP_VERSION);
    /* unmask WDT reset signal */
    writel(0, 0xF7FCD03C);
    /* enable CA7 CPU Timer*/
    writel(1, 0xF7920034);

    /* set up APB Timer*/
    writel(1000*1000, 0xF7E82c00);  /* reload value */
    writel(7, 0xF7E82C08);      /* ENABLE | PERIODIC | IRQ_MASK*/

#if !CDP_A0
    /* pinmux */
    writel(0x09004800, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x00);
    writel(0x00000001, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x04);
    writel(0x09000000, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x08);
    writel(0x00001249, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x0c);

    writel(0x00000009, SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus);

    /* invert the polarity of PMU IRQ */
    writel(0x01800000, 0xF7CA00B8);
#else /* BG2-CDP A0 */
    writel(0x08000000, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x04);
    /* invert the polarity of PMU IRQ */
    writel(0x01800000, MEMMAP_SOC_REG_BASE + 0x04);

#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
    GPIO_PortWrite(TOUCH_RESET_GPIO, 1);
    writel(0x00000252, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x00);
    // TODO may be for uniformity, find an absoute value to write to this pinmux register instead of having to read it first
    unsigned int reg_val = readl(MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x04);
    reg_val = reg_val | 0x00C0000;
    reg_val = reg_val & 0xFFEFFFFF;
    writel(reg_val, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x04);
    writel(0x0838A249, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x08);
    writel(0x00009B64, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x0c);
    writel(0x000000B6, SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus);
    udelay(100); // keep TOUCH_RESET_GPIO HIGH for 100 usec
    GPIO_PortWrite(TOUCH_RESET_GPIO, 0);
    udelay(1000); // add 1 ms delay to allow ATMEL MCU to go to booloader mode TODO: test that delay is really 1 ms
    GPIO_PortWrite(TOUCH_RESET_GPIO, 1); //now toggle the reset pin back to 1
#elif (CFG_BOARD_NAME == BOARD_HENDRIX_B1) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B3) \
        || (CFG_BOARD_NAME == BOARD_HENDRIX_B4)
    GPIO_PortWrite(AUD_MUTE_GPIO, 0); // set AUD_MUTE# pin to output 0
    writel(0x00000252, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x00);
    writel(0x08388249, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x08);
    writel(0x00009B64, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x0c);
    writel(0x000001B6, SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus);
#else
    writel(0x00000000, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x00);
    writel(0x08208249, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x08);
    writel(0x00009264, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x0c);
    writel(0x00000064, SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus);
#endif

#endif /* CDP_A0 */
#else
    lgpl_printf("\n%s %s\n", BOARD_NAME, CHIP_VERSION);
#endif
    board_gpio_init();
    /* Turn on red LED */
    set_led_backlight(LED_RED_PWM, 100);
// TODO(yichunko): Joplin uses only led_frame_15 and has different # of LEDs
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
        || (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
        || (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
    diag_i2c_led_init(LED_MASTER_ID, 0x15);
    diag_i2c_led_init(LED_MASTER_ID, 0x17);
    // Bootloader fast logo
    unsigned char led_frame_15[18] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                                      0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00,
                                      0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
    unsigned char led_frame_17[18] = {0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                                      0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
                                      0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00};
    set_topboard_leds(led_frame_15, ARRAY_SIZE(led_frame_15),
                          led_frame_17, ARRAY_SIZE(led_frame_17));
#endif // BOARD_CHIRP_B{1,3,4} BOARD_JOPLIN_B{1,3,4}

    power_tuning();

    /* show public OTP fields */
    {
        const volatile BERLIN_BOOTINFO *pinfo = (BERLIN_BOOTINFO *)BERLIN_BOOTINFO_ADDR;
        lgpl_printf("OTP/RKEK ID: ");
        lgpl_printf("%08X", pinfo->otp_rkek_id[0]);
        lgpl_printf("%08X", pinfo->otp_rkek_id[1]);
        lgpl_printf("\n");

        lgpl_printf("OTP/VERSION: ");
        lgpl_printf("%08X", pinfo->otp_version);
        lgpl_printf("\n");

        lgpl_printf("EROM ID      :%08X\n", pinfo->erom_id);

        lgpl_printf("RSA KEY INDEX:%1X\n", pinfo->rsa_key_index);
        lgpl_printf("USB BOOT TO  :%1X\n", pinfo->usb_boot_timeout);
        lgpl_printf("ROMK DISABLE :%1x\n", pinfo->romkey_disable);
        lgpl_printf("DIS USB BOOT :%1x\n", pinfo->disable_usb_boot);
        lgpl_printf("DIS BOOTSTRAP:%1x\n", pinfo->usb_disable_bootstrap);
        lgpl_printf("DIS BLANK MED:%1x\n", pinfo->usb_disable_blank_media);

        lgpl_printf("OTP/MID:     ");
        lgpl_printf("%08X", pinfo->otp_market_id);
        lgpl_printf("\n");

        lgpl_printf("OTP/ULT:     ");
        lgpl_printf("%08X", pinfo->otp_ult[0]);
        lgpl_printf("%08X", pinfo->otp_ult[1]);
        lgpl_printf("\n");

        lgpl_printf("OTP LOCK    :%1X\n", pinfo->otp_locked);

        lgpl_printf("RKEK CRC    :%08X\n", pinfo->rkek_crc);
        lgpl_printf("AESK0 CRC   :%08X\n", pinfo->aesk0_crc);
        lgpl_printf("SIGNK7 CRC  :%08X\n", pinfo->signk7_crc);
        lgpl_printf("MRVL SIGN R :%04X\n", pinfo->mrvl_sign_right);
        lgpl_printf("CUST SIGN R :%04X\n", pinfo->cust_sign_right);
    }

    #if BG2CDP
    dump_mem(SYSPLL_CTRL_BASE + RA_pll_ctrl, 4);    // SYSPLL Registers
    dump_mem(MEMPLL_CTRL_BASE + RA_pll_ctrl, 4);    // MEMPLL Registers
    dump_mem(CPUPLL_CTRL_BASE + RA_pll_ctrl, 4);    // CPUPLL Registers
    #else
    dump_mem(0xF7EA0014, 4);    // SYSPLL Registers
    dump_mem(0xF7EA0028, 4);    // MEMPLL Registers
    dump_mem(0xF7EA003C, 4);    // CPUPLL Registers
    #endif

    lgpl_printf("Clock configuration:\n");
#if BG2CDP
    list_speed(2);

#if !CDP_A0
    /* BG2-CDP Z1/Z2 */
    /* adjust vcore */
    diag_i2c_vcore_control(VCORE_MASTER_ID, VCORE_VOLT_INDEX);
#endif /* CDP_A0 */

#else
    show_speed();
#endif /* BG2CDP */

    // initialize nand flash
    init_nfc(block_size, page_size, ecc_strength);
    if(gen_nand_partition_info()){
        printf("Cannot get NAND partition info");
        while(1);
    }

    UtilMemReset();

#ifndef ANDROID_BOOT
    // always get version table for all the boot state
    if (get_version_table()) {
        lgpl_printf("get version table error!\n");
        while(1);
    }
#endif //! ANDROID_BOOT
#if ANDROID_BOOT
    ret = flash_ts_init(vt_fts.part1_start_blkind, vt_fts.part1_blks);
    if (ret){
        lgpl_printf("flash_ts_init() init fail.\n");
        while(1);
    }
#endif

    {
        // note: in the case of STATE_WARMUP_0/1/2, we expect RAM is OK. In the case of
        // Beetle DDR_OFF, it's not. So make sure Image_Load_And_Start is called for the Beetle case.
        // note: MV_SoC_STATE_BOOT_RECOVERY_KERNEL takes this branch.
        //TODO FOR BG2CDPbootloader_customize_load_image_preprocess(uiBoot);

#ifdef ENABLE_CONNECT_BTN
#ifndef BG2CDP
    Check_connectbtnGPIO();
#endif
#endif
        if(Image_Load_And_Start())
        {
            lgpl_printf("tbdzz---- Img_Ld_And_Start error! Spinning now!\n");
#if defined(CDP_A0) && defined(BOOTLOADER_SHOWLOGO)
            fastlogo_init();
            showlogo_start(1);
#endif
            while(1);
        }
#ifdef BG2CDP
        //TODO FOR BG2CDPbootloader_customize_load_image_preprocess(uiBoot);
#else
    bootloader_customize_load_image_preprocess(uiBoot);
#endif

    }

#ifdef BG2CDP
        //TODO FOR BG2CDPbootloader_customize_load_image_preprocess(uiBoot);
#else
    bootloader_customize_load_image_preprocess(uiBoot);
#endif

#if BOOTLOADER_SHOWLOGO
#if CDP_A0
    // Still needed. Otherwise kernel fast logo doesn't display.
    fastlogo_init();
    showlogo_start(0);
    showlogo_stop();
#endif
#else
#if CDP_A0
    AVPLL_Enable();
#endif
#endif  // BOOTLOADER_SHOWLOGO
    Image_3_Verify_Jump() ;
    while(1);
}

/* TODO(kolla): This seem to be a result of statically linking libgcc.a,
 * one of the u-boot forums recommended locally defining this function.
 * Find cleaner fix.
 */
void __aeabi_unwind_cpp_pr0(void)
{
}