/*
 * (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 "init_clock.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

#define PMIC_TYPE_UNKNOWN  0
#define PMIC_TYPE_PG86X    1
#define PMIC_TYPE_SY20212B 2
#define PMIC_TYPE_MAX      3

static int PMIC_type = PMIC_TYPE_UNKNOWN;

#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"

#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)
#undef DISABLE_CRASH_COUNTER
#else
// disable crash counter. b/10478589.
#define DISABLE_CRASH_COUNTER 1
#endif

//#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_KEY_COLOR	  "color"
#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                 10
#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);
}
#else
static int detect_reset_state(void) {
	return 0; /* cold boot */
}
#endif //CDP_A0==1

/* Initiailize GPIOs used by bootloader.
 */
static void 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);
	}
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
		|| (CFG_BOARD_NAME == BOARD_COCO_P1) \
		|| (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);
	}
#elif (CFG_BOARD_NAME == BOARD_COCO_P2)
	if ((ret = GPIO_PortSetInOut(LAMP_BUTTON_GPIO, 1)) != 0) {
		lgpl_printf("ERROR: failed to set lamp button GPIO: %d \n",
		            LAMP_BUTTON_GPIO);
	}
	if ((ret = GPIO_PortSetInOut(VOL_UP_BUTTON_GPIO, 1)) != 0) {
		lgpl_printf("ERROR: failed to set volume up button GPIO: %d \n",
		            VOL_UP_BUTTON_GPIO);
	}
	if ((ret = GPIO_PortSetInOut(VOL_DOWN_BUTTON_GPIO, 1)) != 0) {
		lgpl_printf("ERROR: failed to set volume down button GPIO: %d \n",
		            VOL_DOWN_BUTTON_GPIO);
	}
#endif
}

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[4] = {0};
	if (cnt < 0) cnt = 0;
	if (cnt > 99) cnt = 99;
	if (cnt >= 10) {
		value[0] = '0' + (cnt / 10);
		value[1] = '0' + (cnt % 10);
	} else {
		value[0] = '0' + cnt;
	}
	flash_ts_set(ANDROID_CRASH_COUNTER_KEY, value);
	lgpl_printf("set crashcounter to %d\n", cnt);
}

void increase_android_crash_counter(void)
{
	int crash_counter = flash_ts_get_int(ANDROID_CRASH_COUNTER_KEY, 0);
	if (crash_counter <= MAX_CRASH_COUNT) {
		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) {
		if (!flash_ts_get_int("crashcounter-disable", 0)) {
			lgpl_printf("force FDR due to crashcounter: %d\n", crash_counter);
			write_android_bootloader_message("boot-recovery",
				"", "recovery\n--wipe_data\n");
			/* set_android_crash_counter(0) is called at start_android_kernel */
			return BOOTMODE_RECOVERY;
		}
	}
	lgpl_printf("boot normally (crashcounter: %d)\n", crash_counter);
#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) \
	|| (CFG_BOARD_NAME == BOARD_CAPRICA_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,CAPRICA}_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) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_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,CAPRICA}_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 CDP_A0==1
	if (PMIC_type > PMIC_TYPE_UNKNOWN && PMIC_type < PMIC_TYPE_MAX) {
		int pmic_pg86x = 0, sy20212b = 0;
		if (PMIC_type == PMIC_TYPE_PG86X) {
			pmic_pg86x = 1;
		} else if (PMIC_type == PMIC_TYPE_SY20212B) {
			sy20212b = 1;
		}
		// Append PMIC kernel params
		snprintf(tmp_buf, sizeof(tmp_buf), " sy8824x.enable=%d 88pg86x.enable=%d", sy20212b, pmic_pg86x);
		strlcat(kernel_param, tmp_buf, sizeof(kernel_param));
	} else { /* for debugging purpose. export the i2cget value to kernel command line */
		extern int pg86x_val, sy21202_val;
		snprintf(tmp_buf, sizeof(tmp_buf), " sy21202_val=0x%x pg86x_val=0x%x", sy21202_val, pg86x_val);
		strlcat(kernel_param, tmp_buf, sizeof(kernel_param));
	}
#endif

	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_CHIRP_B1) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
		|| (CFG_BOARD_NAME == BOARD_COCO_P1) \
		|| (CFG_BOARD_NAME == BOARD_COCO_P2) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B4) \
		|| (CFG_BOARD_NAME == BOARD_HENDRIX_B1) \
		|| (CFG_BOARD_NAME == BOARD_HENDRIX_B3) \
		|| (CFG_BOARD_NAME == BOARD_HENDRIX_B4) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_B1) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_B3) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_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_COCO_P1) \
		|| (CFG_BOARD_NAME == BOARD_COCO_P2) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B4) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_B1) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_B3) \
		|| (CFG_BOARD_NAME == BOARD_CAPRICA_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);
}

#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B4) \
		|| (CFG_BOARD_NAME == BOARD_COCO_P1) \
		|| (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_slave_addr,
			      unsigned char* led_frame,
			      size_t led_frame_size,
			      enum led_chip_model_t model) {
	if (detect_reset_state() == 1) {
		// This is a warm boot, do not show LEDs
		return;
	}

	// Boot is cold. Show LEDs.
	// See mapping at
	// Chirp:
	// https://docs.google.com/document/d/1L_DVMgrjZeU3MMt8VjrJeZIAtCuu-HxV1DrQcBH0fTg
	// Joplin:
	// https://docs.google.com/document/d/1DpCafQJHLSdMjOAKXNlkX9_osCplYxzsBgVyqlK8_yo
	diag_i2c_led_set_frame(LED_MASTER_ID, led_slave_addr, led_frame,
			       led_frame_size, model);
}
#endif  // BOARD_CHIRP_B{1,3,4} BOARD_JOPLIN_B{1,3,4}

#if (CFG_BOARD_NAME == BOARD_COCO_P1) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
static void get_device_color(char color[], size_t n) {
	flash_ts_get(FTS_KEY_COLOR, color, n);
}
#endif  // BOARD_COCO_P1, 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
		if (!detect_reset_state()) {
			lgpl_printf("cold boot\n");
			increase_android_crash_counter(); /* cold boot only */
		} else {
			lgpl_printf("warm boot\n");
		}
#endif  // !DISABLE_CRASH_COUNTER
	} else {
		lgpl_printf("Boot recovery image\n");
#ifndef DISABLE_CRASH_COUNTER
		set_android_crash_counter(0);
#endif  // !DISABLE_CRASH_COUNTER
	}
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_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(0x15, led_frame_15, ARRAY_SIZE(led_frame_15),
			  LED_MODEL_PCA9956B);
	set_topboard_leds(0x17, led_frame_17, ARRAY_SIZE(led_frame_17),
			  LED_MODEL_PCA9956B);
#elif (CFG_BOARD_NAME == BOARD_COCO_P1) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B1) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B3) \
		|| (CFG_BOARD_NAME == BOARD_JOPLIN_B4)
	char color[64] = {0};
	get_device_color(color, sizeof(color));
	// Reduce Red by 50% on campari color devices
	if (!strcmp("campari", color)) {
		unsigned char led_frame_17[16] = {0x00, 0xFF, 0x80, 0x64,
						  0x00, 0xFF, 0x80, 0x64,
						  0x00, 0x64, 0x80, 0xFF,
						  0x00, 0x64, 0x80, 0xFF};
		set_topboard_leds(0x17, led_frame_17, ARRAY_SIZE(led_frame_17),
				  LED_MODEL_PCA9955B);
	} else {
		unsigned char led_frame_17[16] = {0x00, 0xFF, 0xFF, 0x64,
						  0x00, 0xFF, 0xFF, 0x64,
						  0x00, 0x64, 0xFF, 0xFF,
						  0x00, 0x64, 0xFF, 0xFF};
		set_topboard_leds(0x17, led_frame_17, ARRAY_SIZE(led_frame_17),
				  LED_MODEL_PCA9955B);
	}
#endif  // BOARD_CHIRP_B{1,3,4} BOARD_COCO_P1 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;
	}

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

#if CDP_A0==1
	if (!diag_i2c_check_pg86x(VCORE_MASTER_ID)) {
		lgpl_printf("Detected pg86x regulator\n");
		PMIC_type = PMIC_TYPE_PG86X;
	} else if (!diag_i2c_check_sy21202(VCORE_MASTER_ID)) {
		lgpl_printf("Detected sy21202 regulator\n");
		PMIC_type = PMIC_TYPE_SY20212B;
	} else {
		lgpl_printf("Did NOT detect PMIC\n");
	}
#endif

	/*
	 * 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_COCO_P1) \
		|| (CFG_BOARD_NAME == BOARD_COCO_P2)
	// This sets these four pins as GPIO12-GPIO15
	unsigned pinmux0 = ((Gbl_pinMuxCntlBus_TW1_SCL_MODE_2 << 0) |
			    (Gbl_pinMuxCntlBus_TW1_SDA_MODE_2 << 3) |
			    (Gbl_pinMuxCntlBus_HDMI_CEC_MODE_1 << 6) |
			    (Gbl_pinMuxCntlBus_HDMI_HPD_MODE_1 << 9));
	writel(pinmux0, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x00);
	// Sets NAND_WEn to GPIO28 (touch reset), sets SD0_CLK as SD0_CLK
	unsigned pinmux1 = ((Gbl_pinMuxCntlBus_SD0_CLK_MODE_1 << 27) |
			    (Gbl_pinMuxCntlBus_NAND_RDY_MODE_0 << 24) |
			    (Gbl_pinMuxCntlBus_NAND_CEn_MODE_0 << 21) |
			    (Gbl_pinMuxCntlBus_NAND_WEn_MODE_3 << 18));
	writel(pinmux1, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x04);
	// Enable both Rx and Tx UART pins vs wiring Rx to a GPIO
	// Sets SD0_DATx, SD0_CMD pins as SD0 pins
	// Sets SD0_CDn as PDM_DI and SD0_WP as I2S2_DO
	// Sets SS0n to GPIO5
	unsigned pinmux2 = ((Gbl_pinMuxCntlBus_SD0_DAT0_MODE_1 << 0) |
			    (Gbl_pinMuxCntlBus_SD0_DAT1_MODE_1 << 3) |
			    (Gbl_pinMuxCntlBus_SD0_DAT2_MODE_1 << 6) |
			    (Gbl_pinMuxCntlBus_SD0_DAT3_MODE_1 << 9) |
			    (Gbl_pinMuxCntlBus_SD0_CDn_MODE_2 << 12) |
			    (Gbl_pinMuxCntlBus_SD0_CMD_MODE_1 << 15) |
			    (Gbl_pinMuxCntlBus_SD0_WP_MODE_6 << 18)  |
			    (Gbl_pinMuxCntlBus_URT0_RXD_MODE_0 << 21) |
			    (Gbl_pinMuxCntlBus_URT0_TXD_MODE_0 << 24) |
			    (Gbl_pinMuxCntlBus_SPI1_SS0n_MODE_1 << 27));
	writel(pinmux2, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x08);
	// Sets SPI1_SD1 to GPIO10, SPI1_SDO to PWM3, SPI1_SCLK to PWM2
	// SPI1_SS2n to TWO_SDA and SPI1_SS1n to TWO_SCL
        // COCO_P2 SPI1_SDO is GPIO9 SP1_SCLK is GPO8
	// Sets USB0_DRV to GPIO11
	unsigned pinmux3 = ((Gbl_pinMuxCntlBus_USB0_DRV_VBUS_MODE_1 << 15) |
			    (Gbl_pinMuxCntlBus_SPI1_SDI_MODE_1 << 12) |
#if (CFG_BOARD_NAME == BOARD_COCO_P1)
			    (Gbl_pinMuxCntlBus_SPI1_SDO_MODE_5 << 9) |
			    (Gbl_pinMuxCntlBus_SPI1_SCLK_MODE_5 << 6) |
#else
			    (Gbl_pinMuxCntlBus_SPI1_SDO_MODE_1 << 9) |
			    (Gbl_pinMuxCntlBus_SPI1_SCLK_MODE_1 << 6) |
#endif
			    (Gbl_pinMuxCntlBus_SPI1_SS2n_MODE_4 << 3) |
			    (Gbl_pinMuxCntlBus_SPI1_SS1n_MODE_4 << 0));
	writel(pinmux3, MEMMAP_CHIP_CTRL_REG_BASE + RA_Gbl_pinMuxCntlBus + 0x0C);
	// Sets SM_TMS to I2S1_BCLK, SM_TDI to I2S1_LRCLK, SM_TDO to PDM_CLK
	unsigned pinmuxSM = ((smSysCtl_smPinMuxCntlBus_SM_TMS_MODE_6 << 0) |
			     (smSysCtl_smPinMuxCntlBus_SM_TDI_MODE_6 << 3) |
			     (smSysCtl_smPinMuxCntlBus_SM_TDO_MODE_2 << 6));
	writel(pinmuxSM, SM_SYS_CTRL_REG_BASE + RA_smSysCtl_smPinMuxCntlBus);
#if (CFG_BOARD_NAME == BOARD_COCO_P2)
	// Set the MUX to switch from JTAG pinout to serial pinout
	GPIO_PortWrite(JTAG_MUX_GPIO, 1);
#endif
	// Reset peripherals (Touch controller on Joplin, MCU on Lilo)
	// Delay so the peripherals fully reset
	GPIO_PortWrite(PERIPHERAL_RESET_GPIO, 1);
	udelay(100);
	GPIO_PortWrite(PERIPHERAL_RESET_GPIO, 0);
	udelay(1000);
	GPIO_PortWrite(PERIPHERAL_RESET_GPIO, 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

	// 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
	board_gpio_init();
#ifdef LED_RED_PWM
	/* Turn on red LED */
	set_led_backlight(LED_RED_PWM, 100);
#endif
#if (CFG_BOARD_NAME == BOARD_CHIRP_B1) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B3) \
		|| (CFG_BOARD_NAME == BOARD_CHIRP_B4)
	diag_i2c_led_init(LED_MASTER_ID, 0x15, LED_MODEL_PCA9956B);
	diag_i2c_led_init(LED_MASTER_ID, 0x17, LED_MODEL_PCA9956B);
	// 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(0x15, led_frame_15, ARRAY_SIZE(led_frame_15),
			  LED_MODEL_PCA9956B);
	set_topboard_leds(0x17, led_frame_17, ARRAY_SIZE(led_frame_17),
			  LED_MODEL_PCA9956B);
#elif (CFG_BOARD_NAME == BOARD_COCO_P1) \
		|| (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, 0x17, LED_MODEL_PCA9955B);
	char color[64] = {0};
	get_device_color(color, sizeof(color));
	// Reduce Red by 50% on campari color devices
	if (!strcmp("campari", color)) {
		unsigned char led_frame_17[16] = {0x00, 0x00, 0x00, 0x00,
						  0x00, 0xFF, 0x80, 0x64,
						  0x00, 0x64, 0x80, 0xFF,
						  0x00, 0x00, 0x00, 0x00};
		set_topboard_leds(0x17, led_frame_17, ARRAY_SIZE(led_frame_17),
				  LED_MODEL_PCA9955B);
	} else {
		unsigned char led_frame_17[16] = {0x00, 0x00, 0x00, 0x00,
						  0x00, 0xFF, 0xFF, 0x64,
						  0x00, 0x64, 0xFF, 0xFF,
						  0x00, 0x00, 0x00, 0x00};
		set_topboard_leds(0x17, led_frame_17, ARRAY_SIZE(led_frame_17),
				  LED_MODEL_PCA9955B);
	}
#endif  // BOARD_CHIRP_B{1,3,4} BOARD_COCO_P1 BOARD_JOPLIN_B{1,3,4}

	init_clock();
	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 */

	{
		// 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)
{
}