berlin_tools/bootloader/source/bootloader.c

/*
 * Copyright (C) 2018 Synaptics Incorporated. All rights reserved.
 * (C) Copyright Marvell Semiconductors, Inc 2017 All rightes reserved
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License version 2 as
 * published by the Free Software Foundation.
 *
 * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND
 * SYNAPTICS EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES,
 * INCLUDING ANY IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE, AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY
 * INTELLECTUAL PROPERTY RIGHTS. IN NO EVENT SHALL SYNAPTICS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, PUNITIVE, OR
 * CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION WITH THE USE
 * OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED AND
 * BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF
 * COMPETENT JURISDICTION DOES NOT PERMIT THE DISCLAIMER OF DIRECT
 * DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS' TOTAL CUMULATIVE LIABILITY
 * TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S. DOLLARS.
 */

#include <stdint.h>

#include "common.h"
#include "config.h"
#include "util.h"
#include "global.h"
#include "chip_init.h"
#include "chip_id.h"
#include "board_init.h"
#include "gpio.h"
#include "led_pwm.h"
#include "apb_perif_base.h"
#include "apb_gpio.h"
#include "berlin_bootinfo.h"
#ifdef ENABLE_NAND
#include "nand_drv.h"
#include "nflash_drv.h"
#elif defined(ENABLE_EMMC)
#include "flash_adaptor.h"
#include "load_gpt.h"
#endif
#include "i2c_led_driver.h"

#include "timer.h"
#include "power_tuning.h"

#include "libfdt.h"
#include "libfdt_mrvl.h"
#include "lz4dec.h"
#include "xzdec.h"
#include "ra_gbl.h"
#include "memmap.h"

#include "cache.h"

#include "tz_nw_boot.h"

#if 0 // BOOTLOADER_FASTLOGO
#include "thinvpp_api.h"
#include "avpll.h"
#include "api_avio_dhub.h"
#endif
#include "fastlogo.h"

#if BOOTLOADER_INIT_AVPLL
#include "avpll_api.h"
#endif

#include "version_table.h"

#if defined(PV_COMP)
#include "chip_voltage_info.h"
#endif

#include "mem_ctrl.h"
#if 0 /* HACK: FIXME */
#include "cpu_ca7.h"
#endif
#define BCM_IMG_KERNEL_TYPE  5
#define BCM_IMG_USBIMG_TYPE  5

#if 0 /* HACK: FIXME */
#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)
#endif

#define VCORE_MASTER_ID         (0x00)

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

#include "android/bootimg.h"
#include "flash_ts.h"
#include "board_config.h"

#include "led.h"
#include "bootloader_customize.h"

#ifdef CONFIG_USB
#include "usb_interface.h"
#include "usb_key.h"
#endif

#ifdef CONFIG_AB_BOOTFLOW
#include "boot_mode.h"
#include "apb_watchdog.h"
#endif

#if defined(PV_COMP)
#define OPP_V2
#endif

#undef DISABLE_CRASH_COUNTER

//#define CONFIG_FORCE_ENCRYPTION

#define BOOT_SRC_FLASH     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      44
#define CPU_IMG_OFFS_IMGSTA      1136

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

#define MAX_ANDROID_IMG_SIZE (64 * 1024 * 1024)
#define MIN_ANDROID_IMG_SIZE (1 * 1024 * 1024)
#define MAX_LOGO_FRM_SIZE (4 * 1024 * 1024)
#define MAX_KERNEL_DTB_SIZE (2 * 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 ] - starting at 22MB
//   [ Unused memory ]
//     Fast logo frame buffer - max size 4MB
//     Kernel DTB buffer - max size 2MB
//     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 minimum board memory size to allow using generic memory layout.
#define MIN_BOARD_MEM_SIZE (256*1024*1024)

// 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 (MIN_BOARD_MEM_SIZE - MAX_SECURE_MEMORY - \
    (MAX_LOGO_FRM_SIZE + MAX_KERNEL_DTB_SIZE + MAX_ANDROID_IMG_SIZE))

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

// Kernel DTB buffer begins right after the frame buffer
#define KERNEL_DTB_BUF (BOOTLOADER_TEMP_MEMORY + MAX_LOGO_FRM_SIZE)

// Android image buffer begins right after the kernel DTB buffer.
#define ANDROID_IMG_BUF (KERNEL_DTB_BUF + MAX_KERNEL_DTB_SIZE)

#define MAX_RAMDISK_IMPLICT_SIZE	(0x2000000)

#define PRNG_DATA_SIZE 64


/*
 * 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]))

#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
#define BOOTMODE_FACTORY                4
const char kBootcmdRecovery[] = "recovery";
const char kFtsBootcmdRecovery[] = "boot-recovery";
const char kFtsBootcmdBackupsys[] = "boot-backupsys";

#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)
struct mv_nand_data nand_data ;

ver_table_entry_t vt_img3;
//ver_table_entry_t vt_sysconf;
//ver_table_entry_t vt_flashless;
static const ver_table_entry_t vt_fts = {
	.part1_start_blkind = FTS_BLK_START,
	.part1_blks = FTS_BLK_CNT,
};
#ifndef CONFIG_AB_BOOTFLOW
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 const ver_table_entry_t vt_factory = {
	.part1_start_blkind = CACHE_BLK_START,
	.part1_blks = CACHE_BLK_CNT,
};
#else
static const ver_table_entry_t vt_kernel_a = {
	.part1_start_blkind = KERNEL_A_BLK_START,
	.part1_blks = KERNEL_A_BLK_CNT,
};

static const ver_table_entry_t vt_kernel_b = {
	.part1_start_blkind = KERNEL_B_BLK_START,
	.part1_blks = KERNEL_B_BLK_CNT,
};

static const ver_table_entry_t vt_factory = {
	.part1_start_blkind = USERDATA_BLK_START,
	.part1_blks = USERDATA_BLK_CNT,
};
#endif

static unsigned int fts_dev_id = 0;

static int warm_boot = 0;
static int led_driver_type = LED_DRIVER_TYPE_UNKNOWN;

static boot_img_hdr Mkbootimg_hdr;
void (*TheKernel)(int zero, int arch, unsigned int params);
static int Bootmode = BOOTMODE_NORMAL;

extern void Jump(void *dest, unsigned long param[8]);

extern int lgpl_printf(const char *format, ...);

extern int is_AS370_A0(void);

inline char itoc(char i);

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

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

#define KDTB_MAGIC "KDTB"
#define KDTB_MAGIC_SZ (sizeof(KDTB_MAGIC) - 1)

// The Kernel is packed along with its dtb file
// The format is [header][xz compressed kernel][dtb file]
// This is the header
typedef struct __attribute__((__packed__)) KDTB_HEADER {
	char magic[KDTB_MAGIC_SZ];
	uint32_t kernel_size;
	uint32_t dtb_size;
} KDTB_HEADER;

typedef struct KDTB_PARSED {
	unsigned char *kernel_addr;
	uint32_t kernel_size;
	unsigned char *dtb_addr;
	uint32_t dtb_size;
} KDTB_PARSED;

static int parse_kern_dtb(unsigned char *kern_dtb, unsigned kern_dtb_size,
	KDTB_PARSED *parsed) {

	KDTB_HEADER header;
	memcpy(&header, kern_dtb, sizeof(header));
	if (memcmp(KDTB_MAGIC, header.magic, KDTB_MAGIC_SZ) != 0) {
		lgpl_printf("Invalid KDTB header magic\n");
		return -1;
	}
	if (header.kernel_size + header.dtb_size + sizeof(header) !=
		kern_dtb_size) {
		lgpl_printf("Invalid KDTB file\n");
		return -1;
	}
	parsed->kernel_addr = kern_dtb + sizeof(header);
	if ((uintptr_t)parsed->kernel_addr & 0x3) {
		lgpl_printf("Kernel must be 4 byte aligned\n");
		return -1;
	}
	parsed->kernel_size = header.kernel_size;
	parsed->dtb_addr = parsed->kernel_addr + header.kernel_size;
	parsed->dtb_size = header.dtb_size;
	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 = tz_nw_generate_rand(rand_buf, rand_size);
	if (ret != 0) {
		lgpl_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)
{
	int value;

	GPIO_PortRead(BUTTON_GPIO, &value);

	return (value == 0);
}

static long long flash_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;
}

#if !defined(DISABLE_CRASH_COUNTER)
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 {
		lgpl_printf("Failed to set bootloader command\n");
	}

	return res;
}
#endif

#ifdef CONFIG_USB
static unsigned long checked_usb_read(unsigned long blknr,
	unsigned long blkcnt, void *buffer) {
	return usb_stor_read(blknr, blkcnt, buffer);
}
#endif //CONFIG_USB

#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


#ifndef CONFIG_AB_BOOTFLOW
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

	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

#ifdef FCT_BOOTLOADER
	return BOOTMODE_FACTORY;
#else
	return BOOTMODE_NORMAL;
#endif
}
//#define ENABLE_CLEAR_LINUX
#else  //CONFIG_AB_BOOTFLOW
int check_android_boot_mode(void)
{
	int button_status;
	int ab_mode;

	button_status = get_button_status();
	if (button_status){
		lgpl_printf("Detected button press -- booting from USB\n");
		return BOOTMODE_BOOTUSB;
	}

#ifdef FCT_BOOTLOADER
	UNUSED(ab_mode);
	return BOOTMODE_FACTORY;
#else
	ab_mode = check_abmode();

	return ab_mode;
#endif
}
#endif

#ifndef ENABLE_CLEAR_LINUX
static int bootimg_hdr_verify(unsigned char *img, int boot_src)
{
	UNUSED(img);
	UNUSED(boot_src);

#if ENABLE_USB_BOOTIMG_HDR_VERIFY == 1
	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

	return 0;
}
#endif

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, decompressed_size = 0;
	unsigned char* k_buff_img;
#ifndef ENABLE_CLEAR_LINUX
	unsigned int bcm_img_type = BCM_IMG_KERNEL_TYPE;
#endif
#ifdef CONFIG_USB
	unsigned long hdr_blk_cnt = get_aligned(CPU_IMG_OFFS_IMGSTA,
								  USB_BLOCK_SIZ)/USB_BLOCK_SIZ;
#endif // CONFIG_USB

	/* Initialize android_start/android_end */
	android_start = 1; android_end = 0;

	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;
		}
#if ENABLE_USB_BOOTIMG_HDR_VERIFY == 1
		/* 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
		lgpl_printf("Reading %d blocks from USB device \n", hdr_blk_cnt);
		flush_dcache_range((void*)(uintptr_t)k_buff_img,
				(void*)(uintptr_t)(k_buff_img + hdr_blk_cnt * USB_BLOCK_SIZ));
		/* Read from first USB device */
		if(checked_usb_read(android_start, hdr_blk_cnt, k_buff_img) != 0) {
			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
		lgpl_printf("ERROR: USB boot is not supported\n");
		return -1;
#endif //CONFIG_USB
	}else{

#ifndef CONFIG_AB_BOOTFLOW
		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 if (bootmode == BOOTMODE_FACTORY)
			vt_android = (ver_table_entry_t *)&vt_factory;
		else
			vt_android = (ver_table_entry_t *)&vt_kernel;
#else
		if (bootmode == BOOTMODE_FACTORY)
			vt_android = (ver_table_entry_t *)&vt_factory;
		else if (BOOTSEL_A == get_abmode())
			vt_android = (ver_table_entry_t *)&vt_kernel_a;
		else
			vt_android = (ver_table_entry_t *)&vt_kernel_b;
#endif

#ifdef ENABLE_NAND
		iPageSize = nand_data.szofpg;
		iBlockSize = nand_data.szofblk;

#elif defined(ENABLE_EMMC)
		iPageSize = get_page_size();
		iBlockSize = get_block_size();
#endif

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

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

#ifndef ENABLE_CLEAR_LINUX
		/* HACK: FIXME - not go ahead if image is empty */
		int i;
		for(i=0; i<16; i++)
		{
			lgpl_printf("DBGDBG k_buff_img[%d]=0x%x\n", i, k_buff_img[i]);
			if(k_buff_img[i] != 0)
				break;
		}

		if(i >= 16)
		{
			lgpl_printf("Image seems to be blank!\n");
			return -1;
		}
#endif
	}

#ifndef ENABLE_CLEAR_LINUX
	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;
#else
	memcpy(&Mkbootimg_hdr, k_buff_img, sizeof(Mkbootimg_hdr));
	if (strncmp((const char *)Mkbootimg_hdr.magic, BOOT_MAGIC, BOOT_MAGIC_SIZE)) {
		lgpl_printf("ERROR: Invalid mkbootimg header!\n");
		return -1;
	}

	mkbootimg_page = Mkbootimg_hdr.page_size;
	// align by page_size and add one page for skipping header.
	cpu_img_siz = 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);
	lgpl_printf("bootimg size = %x\n", cpu_img_siz);
#endif

	/* 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) {
		lgpl_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);

		flush_dcache_range((void*)(uintptr_t)(k_buff_img + hdr_blk_cnt * USB_BLOCK_SIZ),
		    (void*)(uintptr_t)(k_buff_img + blk_cnt * USB_BLOCK_SIZ));
		ret = checked_usb_read((android_start + hdr_blk_cnt),
							(blk_cnt - hdr_blk_cnt),
							(k_buff_img + hdr_blk_cnt * USB_BLOCK_SIZ));
		if (ret != 0){
			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 = flash_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);
#if 0 /* HACK: FIXME */
			return -1;
#endif
		}
	}

#ifndef ENABLE_CLEAR_LINUX
	/* 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;
	}
	flush_dcache_range((void*)(uintptr_t) k_buff, (void*)(uintptr_t) (k_buff + MAX_ANDROID_IMG_SIZE));
	k_buff_size = MAX_ANDROID_IMG_SIZE;
	ret = tz_nw_verify_image(5, (unsigned long)k_buff,
		MAX_ANDROID_IMG_SIZE, (unsigned long)k_buff, (unsigned long)&k_buff_size, bcm_img_type);
	if (ret) {
		lgpl_printf("ERROR: Verify k_buff image failed!ret=0x%x\n", ret);
		return -1;
	}
	if (k_buff_size > MAX_ANDROID_IMG_SIZE){
		lgpl_printf("ERROR: Image is too large!\n");
		return -1;
	}
#else
	k_buff = k_buff_img;
#endif
	memcpy(&Mkbootimg_hdr, k_buff, sizeof(Mkbootimg_hdr));

	if (strncmp((const char *)Mkbootimg_hdr.magic, BOOT_MAGIC, BOOT_MAGIC_SIZE)) {
		lgpl_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;
	Mkbootimg_hdr.second_addr = KERNEL_DTB_BUF;

	TheKernel = (void (*)(int, int, unsigned int))((uintmax_t)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){
		lgpl_printf("BOOTLOADER: Firmware image is too big %d for "
					 "verification\n",
					 k_buff_size);
		return -1;
	}

        lgpl_printf("decompressing kernel image size %d...\n", Mkbootimg_hdr.kernel_size);
        decompressed_size = MAX_ANDROID_IMG_SIZE; //set default decompressed_size to maxium buffer size.

	KDTB_PARSED parsed;
	if (parse_kern_dtb(k_buff + mkbootimg_page, Mkbootimg_hdr.kernel_size,
		&parsed) != 0)
		return -1;  // Error logged

	if (*(uint32_t *)parsed.kernel_addr == LZ4IO_MAGICNUMBER) {
		ret = lz4_decompress(
			(const unsigned char *)parsed.kernel_addr,
			(const unsigned int)parsed.kernel_size,
			(unsigned char *)((uintmax_t)Mkbootimg_hdr.kernel_addr),
			&decompressed_size);
	} else if (memcmp(parsed.kernel_addr, XZ_MAGIC, XZ_MAGIC_SIZE) == 0) {
		ret = xz_decompress(
			(const unsigned char *)parsed.kernel_addr,
			(const unsigned int)parsed.kernel_size,
			(unsigned char *)((uintmax_t)Mkbootimg_hdr.kernel_addr),
			&decompressed_size);
	} else {
		lgpl_printf("Unknown image format. Magic: ");
		int i;
		for (i = 0; i < 6; ++i) {
			lgpl_printf("0x%x, ",
				*((const unsigned char *)parsed.kernel_addr +
				    i));
		}
		lgpl_printf("\n");
		return -1;
	}
        if (ret != 0){
                lgpl_printf("kernel image decompress failed.\n");
		return -1;
        }
        lgpl_printf("kernel img addr 0x%x, kern img len %d.\n", Mkbootimg_hdr.kernel_addr, decompressed_size);

	if (Mkbootimg_hdr.ramdisk_size) {
		k_buff_offset = get_aligned(Mkbootimg_hdr.kernel_size + mkbootimg_page, mkbootimg_page);
		Mkbootimg_hdr.ramdisk_addr = (uintmax_t)(k_buff + k_buff_offset);
	}

	if (parsed.dtb_size > 0) {
		UtilMemCpy((void *)((uintmax_t)Mkbootimg_hdr.second_addr),
			parsed.dtb_addr, parsed.dtb_size);
		Mkbootimg_hdr.second_size = parsed.dtb_size;
	}

	bootimg_print_image_hdr(&Mkbootimg_hdr);
	return 0;
}

// Set mac address for 8801.
static void make_wifi_macaddr(char *wifi_mac_addr_buf,
	size_t wifi_mac_addr_size)
{
	uint64_t chipid_64;
	uint32_t chipid_reg1;
	char *p;
	int i;

	chipid_64 = get_chip_id();
	// Use lowest 24 bit from chip ID.
	chipid_reg1 = (uint32_t) chipid_64;
	strlcpy(wifi_mac_addr_buf, "F8:8F:CA", wifi_mac_addr_size);
	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';
}


static void get_bootinfo(system_info_t *p)
{
	const volatile BERLIN_BOOTINFO *pinfo = (BERLIN_BOOTINFO *)BERLIN_BOOTINFO_ADDR;
	unsigned int temp_id;
	unsigned int unique_id_0;
	unsigned int feature0_0;
	unsigned int feature2_0;
	unsigned int feature4_0, feature4_1;

	p->system_serial_low  = (unsigned int)pinfo->otp_rkek_id[0];
	p->system_serial_high = (unsigned int)pinfo->otp_rkek_id[1];

	p->leakage_current = get_leakage_info();

	temp_id = get_temp_id();
	p->temp_id = (temp_id & 0x7F);

	unique_id_0 = get_unique_id(0);
	p->chip_timestamp = ((unique_id_0 & 0xFF) << 24) | ((unique_id_0 & 0xFF00) << 8) |
		((unique_id_0  & 0xFF0000) >> 8) | ((unique_id_0 & 0xFF000000) >> 24);

	feature0_0 = get_feature0(0);
	p->pcie_disable = (feature0_0 >> 6) & 0x1;

	p->pvcomp_rev = (unsigned int)pinfo->pvcomp_rev;

	feature4_0 = get_feature4_swapped(0);
	feature4_1 = get_feature4_swapped(1);
	p->leakage_current_v1 = feature4_0 & 0x3FFF;
	p->temp_id_v1 = (((feature4_0 >> 28) & 0xF) | ((feature4_1 & 0x2FF) << 4)) & 0x7F;

	feature2_0 = get_feature2_swapped(0);
	p->leakage_current_v2 = feature2_0 & 0x3FFF;
	p->temp_id_v2 = (feature2_0 >> 14) & 0x7F;
}

static void get_chip_version(unsigned int *chip_ver)
{
	if (is_AS370_A0()) {
		*chip_ver = AS370_CHIP_VERSION_A0;
	} else {
		*chip_ver = AS370_CHIP_VERSION_A1;
	}
}

void set_ramdisk_to_dts(void)
{
	int rv;
	unsigned char* dtb_start;
	unsigned int dtb_size;

	/* update FDT with kernel cmdline and ramdisk parameters */
	dtb_start = (unsigned char*)((uintmax_t) Mkbootimg_hdr.second_addr);
	dtb_size  = MAX_KERNEL_DTB_SIZE;
	set_fdt_addr((uintmax_t)dtb_start, dtb_size);

	lgpl_printf("Set dtb param for linux: ramdisk start 0x%X, size 0x%X\n", Mkbootimg_hdr.ramdisk_addr, Mkbootimg_hdr.ramdisk_size);
	if((rv=set_fdt((void *)dtb_start, dtb_size, Mkbootimg_hdr.ramdisk_addr, Mkbootimg_hdr.ramdisk_size, NULL)))
	{
		lgpl_printf("set fdt error %d!\n", rv);
	}
}

void setup_android_kernel_param(int boot_mode)
{
	char kernel_param[1024];
	char wifi_mac_addr[64] = {0};
	char tmp_buf[256];
#if defined(PV_COMP)
	unsigned int opp[64];
#endif

	system_info_t system_info;
	unsigned char* dtb_start;
	unsigned int dtb_size;
	int rv;

	//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="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) ||
			(boot_mode == BOOTMODE_FACTORY))
		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 console info to kernel, which is used by cast init */
	strlcat(kernel_param, " androidboot.console=ttyS0",
		sizeof(kernel_param));

	/* Pass NAND boot path source info to kernel */
#ifdef NAND_BOOT /* for eMMC boot, need kernel side confirmation */
	{
		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));
	}
#endif

	/* 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 || boot_mode == BOOTMODE_FACTORY)
		&& !strcmp("1", tmp_buf)) {
		strlcat(kernel_param, " sd8xxx.mfg_mode=1",
			sizeof(kernel_param));
		strlcat(kernel_param,
			" sd8xxx.cal_data_cfg=mrvl/WlanCalData_sd8987.conf",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.cfg80211_wext=0xf",
			sizeof(kernel_param));
		strlcat(kernel_param,
			" sd8xxx.fw_name=mrvl/sd8987_mfg_wlan.bin",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.fw_serial=0",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.auto_ds=2",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.ps_mode=2",
			sizeof(kernel_param));
		strlcat(kernel_param, " bt8xxx.fw_name=mrvl/sd8987_mfg_bt.bin",
			sizeof(kernel_param));
	} else {
		// Set MAC address only in non-MFG mode.
		make_wifi_macaddr(wifi_mac_addr, sizeof(wifi_mac_addr));

		// Wifi and BT settings
		strlcat(kernel_param,
			" sd8xxx.cal_data_cfg=mrvl/WlanCalData_sd8987.conf",
			sizeof(kernel_param));
#if 0
		strlcat(kernel_param, " sd8987.antcfg=0x13",
			sizeof(kernel_param));
#endif
		strlcat(kernel_param, " sd8xxx.cfg80211_wext=0xc",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.mac_addr=",
			sizeof(kernel_param));
		strlcat(kernel_param, wifi_mac_addr, sizeof(kernel_param));
		strlcat(kernel_param,
			" sd8xxx.fw_name=mrvl/sd8987_wlan.bin",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.fw_serial=0",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.auto_ds=2",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.ps_mode=2",
			sizeof(kernel_param));
		strlcat(kernel_param,
			" bt8xxx.fw_name=mrvl/sd8987_bt.bin",
			sizeof(kernel_param));
		if (boot_mode == BOOTMODE_RECOVERY)
			strlcat(kernel_param, " bt8xxx.boot_mode=recovery",
				sizeof(kernel_param));
		else if (boot_mode == BOOTMODE_NORMAL)
			strlcat(kernel_param, " bt8xxx.boot_mode=normal",
				sizeof(kernel_param));
	}

	snprintf(tmp_buf, sizeof(tmp_buf), " warm_boot=%d", warm_boot);
	strlcat(kernel_param, tmp_buf, sizeof(kernel_param));

	prng_init(kernel_param, sizeof(kernel_param));

	/* update FDT with kernel cmdline and ramdisk parameters */
	dtb_start = (unsigned char*)((uintmax_t) Mkbootimg_hdr.second_addr);
	dtb_size  = MAX_KERNEL_DTB_SIZE;
	set_fdt_addr((uintmax_t)dtb_start, dtb_size);

#if defined(PV_COMP)
	lgpl_printf("add opp to dtb\n");
#if defined(OPP_V1)
extern int get_vcpu_opp(unsigned int * opp);
	int count = 0;
	count = get_vcpu_opp(opp);
	rv = fdt_add_opp((void *)dtb_start, dtb_size, opp, count);
	if(rv < 0) {
		lgpl_printf("fdt add opp rror!\n");
	}
#else
extern void get_vcpu_vh_vl(unsigned int *opp);
	get_vcpu_vh_vl(opp);
	fdt_set_vcpu_opp((void *)(uintptr_t)dtb_start, opp[1], opp[0]);
#endif
#endif

	lgpl_printf("Set dtb param for linux: ramdisk start 0x%X, size 0x%X\n", Mkbootimg_hdr.ramdisk_addr, Mkbootimg_hdr.ramdisk_size);
	if((rv=set_fdt((void *)dtb_start, dtb_size, Mkbootimg_hdr.ramdisk_addr, Mkbootimg_hdr.ramdisk_size, kernel_param)))
	{
		lgpl_printf("set fdt error %d!\n", rv);
	}

	get_bootinfo(&system_info);
	get_chip_version(&system_info.chip_ver);
	rv = fdt_add_system_info((void *)dtb_start, dtb_size, "/chosen", (const system_info_t *)&system_info);
	if(rv) {
		lgpl_printf("fdt add prop rror!\n");
	}

}

void setup_android_kernel_param_new(int boot_mode)
{
	char kernel_param[1024];
	char wifi_mac_addr[64] = {0};
	char tmp_buf[256] = {0};
	int skip_led_lp5018 = 0;
	int skip_led_aw210xx = 0;
#if defined(PV_COMP)
	unsigned int opp[64];
	UNUSED(opp);
#endif

	system_info_t system_info;
	unsigned char* dtb_start;
	unsigned int dtb_size;
	int rv;

	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 0
	if (Mkbootimg_hdr.ramdisk_size) {
	  memset(tmp_buf, 0, sizeof(tmp_buf));
	  strlcat(kernel_param, tmp_buf, sizeof(kernel_param));
	}
#endif
	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="BOARD_NAME,
		sizeof(kernel_param));
#ifdef CONFIG_AB_BOOTFLOW
	if (boot_mode == BOOTMODE_NORMAL || boot_mode == BOOTMODE_FACTORY)
#else
	if (boot_mode == BOOTMODE_NORMAL)
#endif
		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);

#ifdef CONFIG_AB_BOOTFLOW
	if (BOOTSEL_A == get_abmode())
		strlcat(kernel_param, " androidboot.slot_suffix=_a",
			sizeof(kernel_param));
	else
		strlcat(kernel_param, " androidboot.slot_suffix=_b",
			sizeof(kernel_param));
#endif

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

	/* Pass console info to kernel, which is used by cast init */
	strlcat(kernel_param, " androidboot.console=ttyS0",
		sizeof(kernel_param));
#if 0
	/* 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));
	}
#endif
	/* 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 || boot_mode == BOOTMODE_FACTORY)
		&& !strcmp("1", tmp_buf)) {
		strlcat(kernel_param, " sd8xxx.mfg_mode=1",
			sizeof(kernel_param));
		strlcat(kernel_param,
			" sd8xxx.cal_data_cfg=mrvl/WlanCalData_sd8987.conf",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.cfg80211_wext=0xf",
			sizeof(kernel_param));
		strlcat(kernel_param,
			" sd8xxx.fw_name=mrvl/sd8987_mfg_wlan.bin",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.fw_serial=0",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.auto_ds=2",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.ps_mode=2",
			sizeof(kernel_param));
		strlcat(kernel_param, " bt8xxx.fw_name=mrvl/sd8987_mfg_bt.bin",
			sizeof(kernel_param));
	} else {
		// Set MAC address only in non-MFG mode.
		make_wifi_macaddr(wifi_mac_addr, sizeof(wifi_mac_addr));

		// Wifi and BT settings
		strlcat(kernel_param,
			" sd8xxx.cal_data_cfg=mrvl/WlanCalData_sd8987.conf",
			sizeof(kernel_param));
#if 0
		strlcat(kernel_param, " sd8987.antcfg=0x13",
			sizeof(kernel_param));
#endif
		strlcat(kernel_param, " sd8xxx.cfg80211_wext=0xc",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.mac_addr=",
			sizeof(kernel_param));
		strlcat(kernel_param, wifi_mac_addr, sizeof(kernel_param));
		strlcat(kernel_param,
			" sd8xxx.fw_name=mrvl/sd8987_wlan.bin",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.fw_serial=0",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.auto_ds=1",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.ps_mode=1",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.cntry_txpwr=1",
			sizeof(kernel_param));
		strlcat(kernel_param, " sd8xxx.country_ie_ignore=1",
                        sizeof(kernel_param));
		strlcat(kernel_param,
			" bt8xxx.fw_name=mrvl/sd8987_bt.bin",
			sizeof(kernel_param));
		if (boot_mode == BOOTMODE_RECOVERY)
			strlcat(kernel_param, " bt8xxx.boot_mode=recovery",
				sizeof(kernel_param));
		else if (boot_mode == BOOTMODE_NORMAL)
			strlcat(kernel_param, " bt8xxx.boot_mode=normal",
				sizeof(kernel_param));
	}

#if 1
	snprintf(tmp_buf, sizeof(tmp_buf), " warm_boot=%d", warm_boot);
	strlcat(kernel_param, tmp_buf, sizeof(kernel_param));

	//prng_init(kernel_param, sizeof(kernel_param));
#endif

	if (led_driver_type != LED_DRIVER_TYPE_LP5018) {
		skip_led_lp5018 = 1;
	} else if (led_driver_type != LED_DRIVER_TYPE_AW210XX) {
		skip_led_aw210xx = 1;
	}

	snprintf(tmp_buf, sizeof(tmp_buf), " leds_lp5018.skip=%d leds_aw210xx.skip=%d", skip_led_lp5018, skip_led_aw210xx);
	strlcat(kernel_param, tmp_buf, sizeof(kernel_param));

	/* update FDT with kernel cmdline and ramdisk parameters */
	dtb_start = (unsigned char*)((uintmax_t) Mkbootimg_hdr.second_addr);
	dtb_size  = MAX_KERNEL_DTB_SIZE;
	set_fdt_addr((uintmax_t)dtb_start, dtb_size);

#if defined(PV_COMP)
#if defined(OPP_V1)
	lgpl_printf("update opp V1 table to dtb\n");
extern int get_vcpu_opp(unsigned int * opp);
	int count = 0;
	count = get_vcpu_opp(opp);
	rv = fdt_add_opp((void *)dtb_start, dtb_size, opp, count);
	if(rv < 0) {
		lgpl_printf("fdt add opp rror!\n");
	}
#elif defined(OPP_V2)
	lgpl_printf("update opp V2 table to dtb\n");
extern void get_vcpu_vh_vl(unsigned int *opp);
	get_vcpu_vh_vl(opp);
	fdt_set_vcpu_opp((void *)(uintptr_t)dtb_start, opp[1], opp[0]);
#endif
#endif

	if(is_AS370_A0())
		fdt_set_chip_rev((void *)dtb_start, dtb_size, 0xa0);
	else
		fdt_set_chip_rev((void *)dtb_start, dtb_size, 0xa1);

	fdt_select_pmic((void *)dtb_start);

	fdt_set_memory((void *)dtb_start, BOARD_MEM_SIZE);
	fdt_set_ramoops((void *)dtb_start, BOARD_MEM_SIZE);

	lgpl_printf("Set dtb param for linux: ramdisk start 0x%X, size 0x%X\n", Mkbootimg_hdr.ramdisk_addr, Mkbootimg_hdr.ramdisk_size);
	if((rv=set_fdt((void *)dtb_start, dtb_size, Mkbootimg_hdr.ramdisk_addr, Mkbootimg_hdr.ramdisk_size, kernel_param)))
	{
		lgpl_printf("set fdt error %d!\n", rv);
	}

	get_bootinfo(&system_info);
	get_chip_version(&system_info.chip_ver);
	rv = fdt_add_system_info((void *)dtb_start, dtb_size, "/chosen", (const system_info_t *)&system_info);
	if(rv) {
		lgpl_printf("fdt add prop rror!\n");
	}
}

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 (!warm_boot) { /* set by detect_reset_state() */
			lgpl_printf("cold boot\n");
			increase_android_crash_counter(); /* cold boot only */
		} else {
			lgpl_printf("warm boot\n");
		}
#endif
	} else {
		lgpl_printf("Boot recovery image\n");
#ifndef DISABLE_CRASH_COUNTER
		set_android_crash_counter(0);
#endif
	}

	if(!warm_boot){
		cold_boot_show_led_done_boot();
	}

	unsigned long jump_p[8] = {0};
	UNUSED(params); UNUSED(arch);
	lgpl_printf("Start kernel at 0x%08x, dtb at 0x%08x\n", Mkbootimg_hdr.kernel_addr, Mkbootimg_hdr.second_addr);
	tz_nw_enter_boot_stage(TZ_BOOT_STAGE_LINUX, TZ_BOOT_MODE_NORMAL);

	jump_p[0] = Mkbootimg_hdr.second_addr; /* DTB_IMG_ADDR */
	Jump((void *)((uintmax_t)(Mkbootimg_hdr.kernel_addr)), jump_p);
}

/*
 * return: the block address after the last block of the image
 */
static long long flash_read_generic(unsigned int start, unsigned int end, unsigned char* data_buff,
					unsigned int data_size)
{
	unsigned int block_size = 0;

#ifdef ENABLE_NAND
	unsigned int offset = 0, read_size = 0, rest_size = 0;
	char * buf = (char *)data_buff;

	block_size = nand_data.szofblk;

	if((start & (block_size - 1)) || (end & (block_size - 1)))
		return -1;
	if( (end < start) || (end - start < data_size))
		return -1;

	offset = start;
	rest_size = data_size;

	for(; offset < end; offset += block_size) {
		read_size = (rest_size > block_size) ? block_size : rest_size;

		if(is_block_bad((loff_t)offset)) {
			debug_printf("Bad block found @0x%08x.\n", offset);
			continue;
		}
		if(read_size != (unsigned int)mv_nand_read_block(offset, buf, read_size)) {
			lgpl_printf("Read failed @ 0x%08x\n", offset);
			return -1;
		}

		rest_size -= read_size;
		buf += read_size;

		if(rest_size == 0) // done
			break;
	}

#elif defined(ENABLE_EMMC)
	block_size = get_block_size();

	if((start & (block_size - 1)) || (end & (block_size - 1)))
		return -1;
	if( (end < start) || (end - start < data_size))
		return -1;

	if(read_flash(start, data_size, data_buff) != 0) {
		lgpl_printf("EMMC Read flash failed !\n");
		return -1;
	}
#endif

	return data_size;
}

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';
}

typedef struct BTLD_CTX {
	unsigned blk_addr;

	unsigned char lastk[1024];
	char partition_info_buff[1024*2];
} BTLD_CTX;
BTLD_CTX g;

/*
 * 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;

	lgpl_printf("**************************************************************** \n");
	lgpl_printf("                  Read and setup kernel Image                    \n");
	lgpl_printf("**************************************************************** \n");

#ifndef CONFIG_AB_BOOTFLOW
	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_FLASH);
	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_FLASH);
	} 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_FLASH);
	}
#else
	int ab_ret = 0;
	int mode = check_android_boot_mode();

	if (mode == BOOTMODE_BOOTUSB) {
		ret = load_android_image(BOOTMODE_BOOTUSB, BOOT_SRC_USB);
		Bootmode = BOOTMODE_BOOTUSB;
	} else if(mode == BOOTMODE_FACTORY) {
		ret = load_android_image(BOOTMODE_FACTORY, BOOT_SRC_FLASH);
		Bootmode = BOOTMODE_FACTORY;
	} else {
		int ab_mode = mode;
		lgpl_printf("Check2 slot index = 0x%x !\n", ab_mode);
		if((ab_mode != BOOTSEL_A) && (ab_mode != BOOTSEL_B)) {
			dbg_printf(PRN_ERR, "No Bootable slot found for image loading, spinning...!!\n");
			return -1;
		}

		Bootmode = BOOTMODE_NORMAL;
retry_image_load:
		ab_ret = try_abmode(ab_mode);
		ret = load_android_image(Bootmode, BOOT_SRC_FLASH);
		if (ret != 0) {
			if (ab_ret > 0) {
				// slot A or B's retry count is used up, try the other slot
				dbg_printf(PRN_ERR, "Invalid slot_%s boot image! reset to backup image...\n", slot_name(ab_mode));
				reset_soc();
				while(1);
			} else if (ab_ret < 0) {
				// all retry counts of A and B are used up
				lgpl_printf("FATAL ERROR! all retry counts of slot A and B are used up!\n");
			} else {
				// slot A or B's retry count > 0
				dbg_printf(PRN_ERR, "Failed to load slot_%s boot image! Retry again...\n", slot_name(ab_mode));
				goto retry_image_load;
			}
		}
	}

#ifndef DISABLE_CRASH_COUNTER
	if (Bootmode == BOOTMODE_NORMAL) {
		int crash_counter = flash_ts_get_int(ANDROID_CRASH_COUNTER_KEY, 0);
		lgpl_printf("fts: %s: %d\n", ANDROID_CRASH_COUNTER_KEY, 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);
				/* For AB boot, set "boot-command" to "boot-recovery" will FDR */
				write_android_bootloader_message("boot-recovery", "", "");
				set_android_crash_counter(0);
			}
		}
	}
#endif // DISABLE_CRASH_COUNTER
#endif // CONFIG_AB_BOOTFLOW
	if (ret != 0 && Bootmode != BOOTMODE_BOOTUSB) {
		/* Booting from NAND failed, try boot from USB */
		lgpl_printf("Booting from flash 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;
	}

#if 0 /** FIXME: should be put back later */
	/* Turn on Red LED indicating booting */
	set_led_backlight(LED_RED_PWM, 100);
#endif

	/*
	 * setup linux boot parameter
	 */
	//HACK:FIXME:
	lgpl_printf("HACK! skip setup_android_kernel_param\n");
	//setup_android_kernel_param(Bootmode);
	//FIXME: for bringup
	//set_ramdisk_to_dts();
	setup_android_kernel_param_new(Bootmode);

	return 0;
}

// if here, Image-3 is copied into DDR
int Image_3_Verify_Jump(void)
{
	// any state reach here should start/resume kernel

	/* flush the console output */
	putchar('\n');
	start_android_kernel();

	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)

//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_flash_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;

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

#ifdef ENABLE_NAND
	iVT_OFFSET = nand_data.szofblk - sizeof(g.partition_info_buff);

	for(i=1; i<9;i++) {
		/* don't check bad block for block 0 to 9
		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);

		/* check calculated version table size*/
		if(vt_size >= 1024)
		{
			lgpl_printf("ERROR: vt_size is too big!!!\n");
			continue;
		}

		/* 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;
	}

#elif defined(ENABLE_EMMC)
	iVT_OFFSET = get_block_size() - sizeof(g.partition_info_buff);

	for(i=1; i<3; i++) {

		// go get vt
		switch_flash_part(i);
		ret = read_flash((long long)iVT_OFFSET, sizeof(g.partition_info_buff), (unsigned char *)g.partition_info_buff);
		if(ret != 0) {
			lgpl_printf("Error: 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);

		/* check calculated version table size*/
		if(vt_size >= 1024)
		{
			lgpl_printf("ERROR: vt_size is too big!!!\n");
			continue;
		}

		/* 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;
	}

	switch_flash_part(0);	//switch to user partition for default

	if(i >= 3) {
		lgpl_printf("Scan partition info error!\n");
		return -1;
	}
#endif

	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");
}

#ifdef LOAD_LED_CAL_DATA
void ext4fs_set_pt(unsigned int start, unsigned int size);
int ext4fs_probe();
int ext4fs_open(const char *filename, loff_t *len);
void ext4fs_close(void);
int ext4_read_file(const char *filename, void *buf, loff_t offset, loff_t len, loff_t *len_read);

#define REALLOC(a, size) do { if (a) free(a); a = malloc(size); } while(0)

int load_cal_file_ext4(char *partition, char *file_name, char **buf, size_t *buf_len) {
	char *file_buf = *buf;
	loff_t file_len = 0;
	struct gpt_ent gpt_ext4;

	int pt_index = find_partition(partition);
	if (pt_index < 0) {
		lgpl_printf("failed to find partition: %s\n", partition);
		return -1;
	}

	fetch_partition_info(pt_index, &gpt_ext4);
	ext4fs_set_pt(gpt_ext4.ent_lba_start, (gpt_ext4.ent_lba_end - gpt_ext4.ent_lba_start));
	if (ext4fs_probe() < 0) {
		lgpl_printf("failed to probe/mount partition: %s\n", partition);
		return -2;
	}

	if (ext4fs_open(file_name, &file_len)) {
		lgpl_printf("failed to open: %s\n", file_name);
		return -3;
	}

	if ((file_len + 1) > (loff_t)(*buf_len)) {
		REALLOC(file_buf, file_len + 1);
		if (file_buf == NULL) {
			lgpl_printf("failed to malloc buffer: %d\n", file_len + 1);
			return -4;
		}
	}

	ext4_read_file(file_name, file_buf, 0, 0, &file_len);
	file_buf[file_len] = '\0'; /* NULL termination */

	ext4fs_close();
	* buf = file_buf;
	* buf_len = file_len + 1;
	return 0;
}

/* re-usable part: need strchr, realloc */
size_t buf_getline(char **pline, char *buf, size_t *buf_pos, size_t buf_len) {
	size_t line_size = 0;
	size_t pos = *buf_pos;

	if (buf && pos < buf_len) {
		char * p = buf + pos;
		char * p_end = strchr(p, '\n');
		if (p_end == NULL) p_end = buf + buf_len;
		line_size = p_end - p + 1;
	}

	if (line_size > 0) {
		char *pdst = *pline;
		REALLOC(pdst, line_size + 1);
		if (pdst) {
			memcpy(pdst, buf + pos, line_size);
			pdst[line_size] = '\0' /* NULL termination */;

			*pline = pdst;
			*buf_pos = pos + line_size;
		}
	}

	return line_size;
}

LED_LUT lut = {0};

const int header_version_1_0_size = 12;
const char header_version_1_0[] = "LED LUT v1.0";

const int led_item_name_len = 10;
const char * led_item_name[4] = {
	"four_dot_0",
	"four_dot_1",
	"four_dot_2",
	"four_dot_3",
};

#define LINE_MATCH(line, line_size, str, str_size) \
	(line_size == str_size + 1 && memcmp(line, str, str_size) == 0)

#define DUP_STR(dst, src, len) \
	memcpy(dst, src, len); dst[len] = '\0';

unsigned long simple_strtoul(const char *cp, char **endp,unsigned int base);

/* color_name:r,g,b;brightness */
void parse_lut_color(struct led_item *led, char *line, size_t line_size) {
	char *p1 = strchr(line, ':');
	char *p2 = strchr(line, ',');
	char *p3 = strrchr(line, ',');
	char *p4 = strchr(line, ';');
	if (p1 && p2 && p3 && p4 && line < p1 && p1 < p2 && p2 < p3 && p3 < p4 && p4 < line + line_size) {
		int name_len = (int)(p1 - line);
		unsigned long r = simple_strtoul(p1 + 1, (char **)NULL, 0);
		unsigned long g = simple_strtoul(p2 + 1, (char **)NULL, 0);
		unsigned long b = simple_strtoul(p3 + 1, (char **)NULL, 0);
		unsigned long v = simple_strtoul(p4 + 1, (char **)NULL, 0);
		if (led->color_count < MAX_LUT_COLOR
			&& name_len < MAX_LED_NAME_LEN && v <= 255
			&& r <= 255 && g <= 255 && b <= 255) {
			struct led_color * color = &led->color[led->color_count];
			DUP_STR(color->name, line, name_len);
#ifdef DEBUG_LED
			lgpl_printf("%10s -- r/g/b: %3lu %3lu %3lu brightness: %3lu\n",
				color->name, r, g, b, v);
#endif
			color->r = (unsigned char)r;
			color->g = (unsigned char)g;
			color->b = (unsigned char)b;
			color->brightness = (unsigned char)v;
			led->color_count ++;
		}
	}
}

void parse_lut_cal_table(LED_LUT * lut, char * line, size_t line_size) {
	if (lut->header_version == 0) {
		if (LINE_MATCH(line, line_size, header_version_1_0, header_version_1_0_size)) {
			lut->header_version = 1;
			lut->led_index_found = 0;
		}
	} else if (lut->header_version == 1) {
		if (lut->led_index_found == 0) { /* four_dot_0 */
			if (LINE_MATCH(line, line_size, led_item_name[0], led_item_name_len)) {
				lut->led_index_found = 1;
				lut->led_index = 0;
				DUP_STR(lut->led[0].name, line, led_item_name_len);
#ifdef DEBUG_LED
				lgpl_printf("%s\n", lut->led[0].name);
#endif
			}
		} else if (lut->led_index < NUM_LED - 1 &&
			LINE_MATCH(line, line_size, led_item_name[lut->led_index + 1], led_item_name_len)) {
			lut->led_index ++;
			DUP_STR(lut->led[lut->led_index].name, line, led_item_name_len);
#ifdef DEBUG_LED
			lgpl_printf("%s\n", lut->led[lut->led_index].name);
#endif
		} else if (line_size > 1) { /* colors */
			parse_lut_color(&lut->led[lut->led_index], line, line_size);
		}
	} else { /* unknown */
		lgpl_printf("XXX -- {line_size: %d} %s", line_size, line);
	}
}

#define VALIDATE(value, expected_value, msg) do { \
	if ((value) != (expected_value)) { \
		lgpl_printf("%s not match\n", msg); err_cnt ++; \
	} else { \
		lgpl_printf("%s: %d\n", msg, expected_value); \
	} \
} while(0)

int validate_lut_table(LED_LUT * lut) {
	int err_cnt = 0;
	int i, j;

	VALIDATE(lut->led_index, NUM_LED - 1, "led_index");
	for (i = 0; i <= lut->led_index; i ++) {
		struct led_item * led = &lut->led[i];
		lgpl_printf("led: %d\n", i); \
		VALIDATE(led->color_count, COLORS_PER_LED, "color_count");
		for (j = 0; j < led->color_count; j ++) {
			struct led_color * c = &led->color[j];
			lgpl_printf("%10s -- r/g/b: %3u %3u %3u brightness: %3u\n",
				c->name, c->r, c->g, c->b, c->brightness);
		}
	}

	lut->validated = (err_cnt == 0);
	VALIDATE(err_cnt, 0, "error found");
	return err_cnt;
}

static int load_factory_cal_data_ext4() {
	char *buf = NULL;
	size_t buf_len = 0;
	size_t buf_pos = 0;

	char *line = NULL;
	size_t line_size = 0;

	if (load_cal_file_ext4(LED_CAL_PARTITION, LED_CAL_FILE_NAME, &buf, &buf_len)) {
		lgpl_printf("LED cal file loading failed\n");
		return -1;
	}

	while ((line_size = buf_getline(&line, buf, &buf_pos, buf_len)) > 0) {
		parse_lut_cal_table(&lut, line, line_size);
	}

	validate_lut_table(&lut);

	if (buf) free(buf);
	if (line) free(line);
	return 0;
}
#endif

void StartBootLoader(int block_size, int page_size, int ecc_strength)
{
	int ret = 0;

	//FIXME: use the parameter from sysinit/miniloader
	block_size = 262144;
	page_size = 4096;
	ecc_strength = 48;
	lgpl_printf("Board: %s\n", BOARD_NAME);

	chip_init();
	timer_init();
	power_tuning();
	board_init();

#if 0
	/* Turn on red LED */
	set_led_backlight(LED_RED_PWM, 100);

	/* 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);
	}
#endif


#if 0 /* HACK: FIXME */
	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
#endif
	lgpl_printf("Clock configuration:\n");
#if 0 /* HACK: FIXME */
	list_speed(2);
#endif

#if 0 /* HACK: FIXME */
#error "should not be here"
	/* BG2-CDP Z1/Z2 */
	/* adjust vcore */
        diag_i2c_vcore_control(VCORE_MASTER_ID, VCORE_VOLT_INDEX);
#endif

	// initialize flash
#ifdef ENABLE_NAND
	nand_drv_open(block_size, page_size, ecc_strength, 0, 0);

#elif defined(ENABLE_EMMC)
	init_flash();
	block_size = get_block_size();
	page_size = get_page_size();

	if(get_partition_info((void *)(uintptr_t)ANDROID_IMG_BUF)) {
		lgpl_printf("\nError: No valid GPT found!\n");
		while(1);
	}

#ifdef LOAD_LED_CAL_DATA
	load_factory_cal_data_ext4();
#endif
#endif

	nand_data.szofpg = page_size;
	nand_data.szofblk = block_size;
	nand_data.t = ecc_strength;
	if(gen_flash_partition_info()){
		lgpl_printf("Cannot get Flash partition info\n");
		while(1);
	}

#ifdef ENABLE_NAND
	nand_drv_open(block_size, page_size, ecc_strength, 0, 32);
#endif

	UtilMemReset();
    //TODO: Bootloader fast logo
#ifdef ENABLE_EMMC
	struct gpt_ent gpt_fts;
	int num = find_partition(FTS_NAME);

	if(num < 0) {
		lgpl_printf("DEBUG:fail to find fts from GPT\n");
		while(1);
	}
	fetch_partition_info(num, &gpt_fts);
	ret = flash_ts_init((gpt_fts.ent_lba_start)/1024, (gpt_fts.ent_lba_end - gpt_fts.ent_lba_start + 1)/1024);
#else /* init fts by version table info */

	ret = flash_ts_init(vt_fts.part1_start_blkind, vt_fts.part1_blks);
#endif
	if (ret){
		lgpl_printf("flash_ts_init() init fail.\n");
		while(1);
	}

	warm_boot = detect_reset_state();

	detect_led_driver(&led_driver_type);

	if (!warm_boot) {
		cold_boot_show_led_init_boot(led_driver_type);
	}

	{
		// 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.

		if(Image_Load_And_Start())
		{
			lgpl_printf("tbdzz---- Img_Ld_And_Start error! Spinning now!\n");
#if defined(BOOTLOADER_SHOWLOGO)
			fastlogo_init();
			showlogo_start(1);
#endif
			while(1);
		}
	}

#if BOOTLOADER_SHOWLOGO
	// Still needed. Otherwise kernel fast logo doesn't display.
	fastlogo_init();
	showlogo_start(0);
	showlogo_stop();
#else
#if BOOTLOADER_INIT_AVPLL
	MV_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)
{
}