/*
 * Copyright (C) 2014 Nest labs, Inc.
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 */

#include <asm/arch/clock.h>
#include <asm/arch/iomux.h>
#include <asm/arch/imx-regs.h>
#include <asm/arch/mx6-pins.h>
#include <asm/arch/sys_proto.h>
#include <asm/arch/crm_regs.h>
#include <asm/gpio.h>
#include <asm/imx-common/iomux-v3.h>
#include <asm/imx-common/boot_mode.h>
#include <asm/io.h>
#include <linux/sizes.h>
#include <common.h>
#include <fsl_esdhc.h>
#include <mmc.h>
#include <miiphy.h>
#include <netdev.h>
#ifdef CONFIG_SYS_I2C_MXC
#include <i2c.h>
#include <asm/imx-common/mxc_i2c.h>
#endif
#include <fuse.h>
#include <nand.h>
#include <rsa-imx.h>
#include <flintstone_lpgpr.h>

#include "../fs/ubifs/ubifs.h"

DECLARE_GLOBAL_DATA_PTR;

/*
 * Ascertain the reset cause and save it to the LPGPR register in SNVS.
 *
 * Passing the reset cause to Linux is important for debugging and presenting
 * appropriate boot UI. This function draws on three sources of information:
 *
 * 1) The SOC SRC Reset Status Register (SRC_SRSR)
 *        - This contains the reset cause as seen by the SOC e.g. WDOG, POR
 * 2) The SOC Watchdog Reset Status Register (WDOG1_WRSR)
 *        - This indicates the source of the last reset generated due to WDOG
 * 3) The STBYDLY field from the PMIC's PWRCTL register
 *        - This field is not used by F1 (the STANDBY pin is not connected),
 *          and is reset when the PMIC is power cycled, so we can repurpose it
 *          to detect a cold reset. If it contains the default value of 0x1,
 *          write 0x2.
 * 4) The PMIC embedded memory register MEMA
 *        - The PMIC has general purpose embedded memory which is not reset
 *          until the device has been unpowered for >20 seconds. Use this
 *          to determine whether the battery has been deeply discharged.
 *
 * By configuring Linux to only reset the SOC on SW reboot and watchdog reset,
 * i.e. not use the reset controller, U-Boot can distinguish between, for
 * example, a watchdog bite and a 5-key reset. Upon encountering a non-POR
 * reset this function resets the PMIC to power cycle the rest of the
 * hardware.
 *
 */
void save_reset_cause(void)
{
	u8 pmic_data = 0;
	u8 stbydly = 0;
	u8 mema = 0;
	u8 wrsr = 0;
	u32 cause = get_imx_reset_cause();
	u32 lpgpr = readl(SNVS_BASE_ADDR + SNVS_LPGPR);
	enum pmic_state pmic = unknown;
	enum wdog_state wdog = unknown;
	struct wdog_regs *wdog1 = (struct wdog_regs *)WDOG1_BASE_ADDR;

	/* Check for PMIC power cycle (cold reset) */
	i2c_set_bus_num(CONFIG_PMIC_I2C_BUS);
	i2c_init(CONFIG_SYS_I2C_SPEED, CONFIG_PMIC_I2C_SLAVE);
	i2c_read(CONFIG_PMIC_I2C_SLAVE, PFUZE200_PWRCTL, 1, &pmic_data, 1);
	stbydly = (pmic_data & PFUZE200_PWRCTL_STBYDLY_MASK)
		>> PFUZE200_PWRCTL_STBYDLY_SHIFT;

	switch (stbydly) {
	case PMIC_DEFAULT_STBYDLY_VALUE: /* PMIC power cycled */
		pmic_data &= ~PFUZE200_PWRCTL_STBYDLY_MASK;
		pmic_data |= PMIC_NON_DEFAULT_STBYDLY_VALUE
			<< PFUZE200_PWRCTL_STBYDLY_SHIFT;
		i2c_write(CONFIG_PMIC_I2C_SLAVE,
			PFUZE200_PWRCTL, 1, &pmic_data, 1);

		/* If we caused this cold reset, state is already in SNVS.
		 * Continue booting */
		if (lpgpr & LPGPR_RESET_CAUSE_WRITTEN) {
			printf("U-Boot-caused reset, continuing\n");
			clrbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR,
				LPGPR_RESET_CAUSE_WRITTEN);
			return;
		}

		/* Check embedded PMIC memory */
		i2c_read(CONFIG_PMIC_I2C_SLAVE, PFUZE200_MEMA, 1, &mema, 1);
		switch (mema) {
		case PMIC_NON_DEFAULT_MEMA_VALUE: /* No deep discharge */
			pmic = reset;
			break;
		case PMIC_DEFAULT_MEMA_VALUE: /* Deep discharge */
			pmic = resurrected;
			/* Fall through */
		default: /* Unexpected result */
			/* pmic initialized to unknown */
			pmic_data = PMIC_NON_DEFAULT_MEMA_VALUE;
			i2c_write(CONFIG_PMIC_I2C_SLAVE,
				PFUZE200_MEMA, 1, &pmic_data, 1);
			break;
		}
		break;
	case 0x2: /* PMIC not power cycled */
		pmic = still_on;
		break;
	default: /* Unexpected result */
		/* pmic initialized to unknown */
		break;
	}

	/* Check watchdog reset status */
	wrsr = readb(&wdog1->wrsr);
	if (wrsr & WDOG_WRSR_SFTW)
		wdog = sftw;
	else if (wrsr & WDOG_WRSR_TOUT)
		wdog = tout;
	else
		; /* POR case covered by SRC_SRSR */

	clrsetbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR,
		LPGPR_PMIC_STATE_MASK,
		(pmic << LPGPR_PMIC_STATE_SHIFT) & LPGPR_PMIC_STATE_MASK);

	clrsetbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR,
		LPGPR_SRSR_MASK, (cause << LPGPR_SRSR_SHIFT) & LPGPR_SRSR_MASK);

	/* Repurpose bit 1 of SRSR, normally unused, to fit in WARM_BOOT */
	setbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR,
		(cause >> LPGPR_SRSR_WARM_BOOT_ADJ) & LPGPR_SRSR_WARM_BOOT_MASK);

	clrsetbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR,
		LPGPR_WRSR_MASK, (wdog << LPGPR_WRSR_SHIFT) & LPGPR_WRSR_MASK);

	clrsetbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR,
		LPGPR_WRSR_MASK, (wdog << LPGPR_WRSR_SHIFT) & LPGPR_WRSR_MASK);

	setbits_le32(SNVS_BASE_ADDR + SNVS_LPGPR, LPGPR_RESET_CAUSE_WRITTEN);

	/* If non-POR reset (see get_imx_reset_cause()
	 * and SRC_SRSR register documentation) */
	if (cause != 0x1 && cause != 0x11) {
		/* Cold reboot (reset PMIC) */
		printf("Toggling WDOG_L\n");
		gpio_direction_output(8, 1);
		gpio_set_value(8, 0);
	}
}


#ifdef CONFIG_NEST_PLIST
static char start_pattern[256];
static char data[4096];

static int ubifs_initialized;

static int read_plist(const char *volume,
		      const char *file,
		      const char *key,
		      const char *variable)
{
	char *start_ptr, *stop_ptr;

	/* Initialize UBIFS (can skip if done before) */
	if (ubifs_initialized == 0) {
		ubifs_init();
		ubifs_initialized = 1;
	}

	/* Mount UBIFS Volume */
	if (uboot_ubifs_mount((char *)volume)) {
		printf("** Could not mount volume **\n");
		return CMD_RET_FAILURE;
	}

	/* Read the file */
	if (ubifs_load((char *)file, (u32) data, sizeof(data))) {
		printf("** Could not read Property List file **\n");
		return CMD_RET_FAILURE;
	}

	/* Make sure its null terminated */
	data[sizeof(data) - 1] = 0;

	/* Do a naive parsing of the XML for now */
	snprintf(start_pattern, sizeof(start_pattern), "<key>%s</key>\n\t<string>", key);

	start_ptr = strstr(data, start_pattern);
	if (!start_ptr) {
		printf("** Could not find key **\n");
		return CMD_RET_FAILURE;
	}

	start_ptr += strlen(start_pattern);

	stop_ptr = strstr(start_ptr, "</string>");
	if (!stop_ptr) {
		printf("** Could not find end of value **\n");
		return CMD_RET_FAILURE;
	}

	*stop_ptr = 0;

	/* Update environment */
	setenv(variable, start_ptr);

	return CMD_RET_SUCCESS;
}
#endif

/* FCT DETECTION GPIO *********************************************************/
#ifdef CONFIG_FCT_DETECTION
#define FCT_GPIO_PAD_CTRL  (PAD_CTL_PKE | PAD_CTL_PUE |		\
	PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_MED |		\
	PAD_CTL_DSE_40ohm   | PAD_CTL_SRE_FAST  | PAD_CTL_HYS)

static iomux_v3_cfg_t const fct_gpio_pads[] = {
	MX6_PAD_SD4_RESET_B__GPIO6_IO_22 | MUX_PAD_CTRL(FCT_GPIO_PAD_CTRL),
};

static void setup_iomux_fct_gpio(void)
{
	imx_iomux_v3_setup_multiple_pads(fct_gpio_pads, ARRAY_SIZE(fct_gpio_pads));
}
#endif

/* Disable Piezo enable GPIO *************************************************/
#ifdef CONFIG_MACH_DIAMOND3
static void setup_piezo_enable_gpio(void)
{
	imx_iomux_v3_setup_pad(MX6_PAD_GPIO1_IO05__GPIO1_IO_5 | MUX_PAD_CTRL(NO_PAD_CTRL));
	gpio_direction_output(IMX_GPIO_NR(1, 5), 1);
}
#endif

/* UART ***********************************************************************/
#define UART_PAD_CTRL  (PAD_CTL_PKE | PAD_CTL_PUE |		\
	PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_MED |		\
	PAD_CTL_DSE_40ohm   | PAD_CTL_SRE_FAST  | PAD_CTL_HYS)

static iomux_v3_cfg_t const uart1_pads[] = {
	MX6_PAD_ENET2_CRS__UART1_TX | MUX_PAD_CTRL(UART_PAD_CTRL),
	MX6_PAD_ENET2_COL__UART1_RX | MUX_PAD_CTRL(UART_PAD_CTRL),
};

static void setup_iomux_uart(void)
{
	imx_iomux_v3_setup_multiple_pads(uart1_pads, ARRAY_SIZE(uart1_pads));
}

/* Mux out the UART for security */
static iomux_v3_cfg_t const uart1_pads_disabled[] = {
	MX6_PAD_ENET2_CRS__GPIO2_IO_7 | MUX_PAD_CTRL(NO_PAD_CTRL),
	MX6_PAD_ENET2_COL__GPIO2_IO_6 | MUX_PAD_CTRL(NO_PAD_CTRL),
};

void disable_iomux_uart(void)
{
	imx_iomux_v3_setup_multiple_pads(uart1_pads_disabled, ARRAY_SIZE(uart1_pads_disabled));
}

/* I2C ************************************************************************/
#ifdef CONFIG_SYS_I2C_MXC
#define I2C_PAD_CTRL    (PAD_CTL_PKE | PAD_CTL_PUE |            \
	PAD_CTL_PUS_100K_UP | PAD_CTL_SPEED_MED |               \
	PAD_CTL_DSE_40ohm | PAD_CTL_HYS |			\
	PAD_CTL_ODE)
#define PC MUX_PAD_CTRL(I2C_PAD_CTRL)

struct i2c_pads_info i2c_pad_info4 = {
	.scl = {
		.i2c_mode = MX6_PAD_USB_H_STROBE__I2C4_SCL | PC,
		.gpio_mode = MX6_PAD_USB_H_STROBE__GPIO7_IO_11 | PC,
		.gp = IMX_GPIO_NR(7, 11),
	},
	.sda = {
		.i2c_mode = MX6_PAD_USB_H_DATA__I2C4_SDA | PC,
		.gpio_mode = MX6_PAD_USB_H_DATA__GPIO7_IO_10 | PC,
		.gp = IMX_GPIO_NR(7, 10),
	},
};

struct i2c_pads_info i2c_pad_info2 = {
	.scl = {
		.i2c_mode = MX6_PAD_GPIO1_IO02__I2C2_SCL | PC,
		.gpio_mode = MX6_PAD_GPIO1_IO02__GPIO1_IO_2 | PC,
		.gp = IMX_GPIO_NR(1, 2),
	},
	.sda = {
		.i2c_mode = MX6_PAD_GPIO1_IO03__I2C2_SDA | PC,
		.gpio_mode = MX6_PAD_GPIO1_IO03__GPIO1_IO_3 | PC,
		.gp = IMX_GPIO_NR(1, 3),
	},
};
#endif

/* DDR ************************************************************************/
int dram_init(void)
{
	gd->ram_size = PHYS_SDRAM_SIZE;

	return 0;
}

/* NAND ***********************************************************************/
#ifdef CONFIG_SYS_USE_NAND

#define GPMI_PAD_CTRL0 (PAD_CTL_PKE | PAD_CTL_PUE | PAD_CTL_PUS_100K_UP)
#define GPMI_PAD_CTRL1 (PAD_CTL_DSE_40ohm | PAD_CTL_SPEED_MED | \
			PAD_CTL_SRE_FAST)
#define GPMI_PAD_CTRL2 (GPMI_PAD_CTRL0 | GPMI_PAD_CTRL1)

iomux_v3_cfg_t gpmi_pads[] = {
	MX6_PAD_NAND_CLE__RAWNAND_CLE		| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_ALE__RAWNAND_ALE		| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_WP_B__RAWNAND_WP_B	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_READY_B__RAWNAND_READY_B	| MUX_PAD_CTRL(GPMI_PAD_CTRL0),
	MX6_PAD_NAND_CE0_B__RAWNAND_CE0_B	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_RE_B__RAWNAND_RE_B	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_WE_B__RAWNAND_WE_B	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA00__RAWNAND_DATA00	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA01__RAWNAND_DATA01	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA02__RAWNAND_DATA02	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA03__RAWNAND_DATA03	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA04__RAWNAND_DATA04	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA05__RAWNAND_DATA05	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA06__RAWNAND_DATA06	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
	MX6_PAD_NAND_DATA07__RAWNAND_DATA07	| MUX_PAD_CTRL(GPMI_PAD_CTRL2),
};

static void setup_gpmi_nand(void)
{
	struct mxc_ccm_reg *mxc_ccm = (struct mxc_ccm_reg *)CCM_BASE_ADDR;

	/* config gpmi nand iomux */
	imx_iomux_v3_setup_multiple_pads(gpmi_pads, ARRAY_SIZE(gpmi_pads));

	/* Disable the QSPI2 root clock */
	clrbits_le32(&mxc_ccm->CCGR4, MXC_CCM_CCGR4_QSPI2_ENFC_MASK
				| MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_GPMI_IO_MASK);

	/* config gpmi and bch clock to 100 MHz */
	clrsetbits_le32(&mxc_ccm->cs2cdr,
			MXC_CCM_CS2CDR_QSPI2_CLK_PODF_MASK |
			MXC_CCM_CS2CDR_QSPI2_CLK_PRED_MASK |
			MXC_CCM_CS2CDR_QSPI2_CLK_SEL_MASK,
			MXC_CCM_CS2CDR_QSPI2_CLK_PODF(0) |
			MXC_CCM_CS2CDR_QSPI2_CLK_PRED(3) |
			MXC_CCM_CS2CDR_QSPI2_CLK_SEL(3));

	/* enable gpmi and bch clock gating */
	setbits_le32(&mxc_ccm->CCGR4,
		     MXC_CCM_CCGR4_RAWNAND_U_BCH_INPUT_APB_MASK |
		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_BCH_MASK |
		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_BCH_INPUT_GPMI_IO_MASK |
		     MXC_CCM_CCGR4_RAWNAND_U_GPMI_INPUT_APB_MASK |
		     MXC_CCM_CCGR4_PL301_MX6QPER1_BCH_MASK |
		     MXC_CCM_CCGR4_QSPI2_ENFC_MASK);

	/* enable apbh clock gating */
	setbits_le32(&mxc_ccm->CCGR0, MXC_CCM_CCGR0_APBHDMA_MASK);
}
#endif

/* PMIC ************************************************************************/
#ifdef CONFIG_PFUZE100_PMIC_I2C

/* set all switchers to APS mode in normal and PFM mode in standby */
static int setup_pmic_mode(void)
{
	unsigned char pmic_mode = PFUZE200_SWMODE_APS_PFM;
	unsigned char pmic_mode_3b = PFUZE200_SWMODE_APS_APS;

	i2c_set_bus_num(CONFIG_PMIC_I2C_BUS);
	i2c_init(CONFIG_SYS_I2C_SPEED, CONFIG_PMIC_I2C_SLAVE);

	if (i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SWABMODE, 1, &pmic_mode, 1) ||
	    i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW2MODE,  1, &pmic_mode, 1) ||
	    i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW3AMODE, 1, &pmic_mode, 1) ||
	    i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW3BMODE, 1, &pmic_mode_3b, 1)) {
		printf("Set PMIC switcher mode error!\n");
		return -1;
	}

	return 0;
}
#endif


/* USB ************************************************************************/
#ifdef CONFIG_USB_EHCI_MX6
iomux_v3_cfg_t const usb_otg1_pads[] = {
	MX6_PAD_GPIO1_IO09__USB_OTG1_PWR | MUX_PAD_CTRL(NO_PAD_CTRL),
	MX6_PAD_GPIO1_IO10__ANATOP_OTG1_ID | MUX_PAD_CTRL(NO_PAD_CTRL)
};

iomux_v3_cfg_t const usb_otg2_pads[] = {
	MX6_PAD_GPIO1_IO12__USB_OTG2_PWR | MUX_PAD_CTRL(NO_PAD_CTRL),
};

int board_ehci_hcd_init(int port)
{
	switch (port) {
	case 0:
		imx_iomux_v3_setup_multiple_pads(usb_otg1_pads,
			ARRAY_SIZE(usb_otg1_pads));
		break;
	case 1:
		imx_iomux_v3_setup_multiple_pads(usb_otg2_pads,
			ARRAY_SIZE(usb_otg2_pads));
		break;
	default:
		printf("MXC USB port %d not yet supported\n", port);
		return 1;
	}
	return 0;
}
#endif

#ifdef CONFIG_IMX_UDC
iomux_v3_cfg_t const otg_udc_pads[] = {
	(MX6_PAD_GPIO1_IO10__ANATOP_OTG1_ID | MUX_PAD_CTRL(NO_PAD_CTRL)),
};
void udc_pins_setting(void)
{
	imx_iomux_v3_setup_multiple_pads(otg_udc_pads,
		ARRAY_SIZE(otg_udc_pads));
}
#endif

/* WIFI ***********************************************************************/
#define USDHC2_PWR_GPIO        IMX_GPIO_NR(1, 4)

iomux_v3_cfg_t const wifi_pads[] = {
	MX6_PAD_GPIO1_IO04__GPIO1_IO_4 | MUX_PAD_CTRL(NO_PAD_CTRL),
};

int board_wifi_init(void)
{
	/* Enable WIFI chip on so it is detected by the initial scan in Linux */
	imx_iomux_v3_setup_multiple_pads(wifi_pads, ARRAY_SIZE(wifi_pads));
	gpio_direction_output(USDHC2_PWR_GPIO, 1);

	return 0;
}

/* RSA unlock token check ******************************************************/
#define OTP_BANK_UID		0
#define OTP_WORD_UID_L		1
#define OTP_WORD_UID_H		2
#define OTP_BANK_SEC_CONFIG	0
#define OTP_WORD_SEC_CONFIG	6
#define SEC_CONFIG_CLOSED	0x00000002
#define UNLOCK_TOKEN_SIZE_BYTES	256
#define UNLOCK_TOKEN_BASE64_LEN	344        // The 256-Byte unlock token should ALWAYS be 344 Bytes long when base64 encoded
#define UNLOCK_TOKEN_ADDRESS	0x82FFF000 // This is the address where the DFU tool needs to stick the unlock token when doing DFU
#define UNLOCK_TOKEN_PARTITION	"ubi0:system-config"
#define SHA256_CHUNK_SZ		64

#ifdef CONFIG_UNLOCK_TOKEN_PREFIX
// If CONFIG_UNLOCK_TOKEN_PREFIX is defined (basically all but the D3 legacy devices),
// check that all unlock tokens are prefixed with it.
// This disambiguates tokens accidentally generated for the wrong build product.
#define UNLOCK_TOKEN_PREFIX_LEN     (strlen(CONFIG_UNLOCK_TOKEN_PREFIX))
#else
#define UNLOCK_TOKEN_PREFIX_LEN     0
#endif

/* Lookup the 6-bit value for a base64-encoded character.
 * Invalid characters will return 0x7f and '=' end padding characters will always return zero.
 */
static char base64_lookup(char in) {
	uint8_t ret = 0x7f;

	if ((in >= 'A') && (in <= 'Z')) {
		ret = in - 'A';
	} else if ((in >= 'a') && (in <= 'z')) {
		ret = in - 'a' + 26;
	} else if ((in >= '0') && (in <= '9')) {
		ret = in - '0' + 52;
	} else if (in == '+') {
		ret = 62;
	} else if (in == '/') {
		ret = 63;
	} else if (in == '=') {
		ret = 0;
	}

	return ret;
}

/* Decode a base64-encoded string.
 * This implementation assumes the lengths have been checked, such that the encoded data will always fit
 * The source and destination buffers may be the same, but should not otherwise overlap.
 * The destination buffer must be large enough hold the output if the expected input has two '=' padding
 * characters and those characters are replaced with other valid base64 words.
 */
static int base64_decode(char *encoded_data, char *decoded_data, bool silent) {
	int idx_in, idx_out,
	    len = strlen(encoded_data),
	    ret = 0;

	if (((len % 4) != 0) || (len == 0)) {
		printf("%s: Invalid length: %d\n", __func__, len);
		decoded_data[0] = 0;
		ret = -1;
	} else {
		idx_in = idx_out = 0;
		while ((!ret) && (idx_in < len)) {
			char a = base64_lookup(encoded_data[idx_in++]),
			     b = base64_lookup(encoded_data[idx_in++]),
			     c = base64_lookup(encoded_data[idx_in++]),
			     d = base64_lookup(encoded_data[idx_in++]);

			if ((a > 0x3f) || (b > 0x3f) || (c > 0x3f) || (d > 0x3f)) {
				if (!silent) {
					printf("%s: Invalid character near Byte %i\n", __func__, (idx_in - 3));
				}
				ret = -1;
			} else {
				decoded_data[idx_out++] = ((a & 0x3f) << 2) | ((b & 0x30) >> 4);
				decoded_data[idx_out++] = ((b & 0x0f) << 4) | ((c & 0x3c) >> 2);
				decoded_data[idx_out++] = ((c & 0x03) << 6) | (d & 0x3f);
			}
		}
	}

	return ret;
}

/* Check for a valid unlock token, either in RAM or in the filesystem.
 * The unlock token is the UID string (ASCII hex encoded, with no CR/LF appended),
 * signed with the private unlocking key and base64 encoded.
 *
 * Tokens can be generated as follows (for UIDs '2a1429d4de730bfc' and '102551d4df669959'):
 * > device_UIDs=("2a1429d4de730bfc" "102551d4df669959")
 * > unlock_key=/dev/shm/diamond3-unlock.key
 * > unlock_token_folder=${BuildRoot}/keys/
 * > for device_UID in "${device_UIDs[@]}"; do
 * >   echo -n "${device_UID}" | openssl dgst -sha256 -passin env:passcode -sign ${unlock_key} | base64 --wrap=0 > ${unlock_token_folder}/${device_UID}
 * > done
 *
 * The token can be loaded at boot time when booting via DFU by loading it to UNLOCK_TOKEN_ADDRESS
 * The token can be permanently stored by copying the token file (filename is the UID) to the root of UNLOCK_TOKEN_PARTITION
 */
static bool board_unlock_token_check(char *UID_str)
{
	bool unlocked = false;
	void *blob = ((void *)gd_fdt_blob());
	int sig_node, unlock_node;
	uint8_t hash[SHA256_SUM_LEN];
	sha256_context ctx;
	struct imx_rsa_public_key key;
	char *unlock_token_source;
	char *unlock_data = ((char *)UNLOCK_TOKEN_ADDRESS);
	int ret = 0;

	// Stretch the UID ASCII hex string into a SHA-256 hash so that we can
	// pad it in a standard way
	sha256_starts(&ctx);
	sha256_update(&ctx, (uint8_t *)UID_str, strlen(UID_str));
	sha256_finish(&ctx, hash);

	// Locate the signature DT node
	sig_node = fdt_subnode_offset(blob, 0, FIT_SIG_NODENAME);
	if (sig_node < 0) {
		printf("%s: No signature node found\n", __func__);
		ret = -1;
	}
	if (!ret) {
		// Locate the public unlocking key DT node
		unlock_node = fdt_subnode_offset(blob, sig_node, "unlock");
		if (unlock_node < 0) {
			printf("%s: No public unlocking key node found\n", __func__);
			ret = -1;
		}
	}
	if (!ret) {
		// Read the public key from the DT unlock_node into memory
		ret = rsa_imx_get_key(blob, unlock_node, &key);
		if (ret) {
			printf("%s: RSA failed to load public unlocking key: %d\n", __func__, ret);
		}
	}
	if (!ret) {
		// Use the public key to check if there is a valid signature in memory
		unlock_data[UNLOCK_TOKEN_PREFIX_LEN + UNLOCK_TOKEN_BASE64_LEN] = 0;
		if (strlen(unlock_data) != (UNLOCK_TOKEN_PREFIX_LEN + UNLOCK_TOKEN_BASE64_LEN)) {
			ret = -1;
#ifdef CONFIG_UNLOCK_TOKEN_PREFIX
		} else if (strncmp(unlock_data, CONFIG_UNLOCK_TOKEN_PREFIX, UNLOCK_TOKEN_PREFIX_LEN)) {
			ret = -1;
#endif
		} else if (base64_decode(&unlock_data[UNLOCK_TOKEN_PREFIX_LEN], unlock_data, true) != 0)  {
			ret = -1;
		} else {
			ret = rsa_imx_verify_key(&key, (uint8_t *)unlock_data, UNLOCK_TOKEN_SIZE_BYTES, hash);
			unlock_token_source = "RAM";
		}

		if (ret) {
			unlock_data[UNLOCK_TOKEN_PREFIX_LEN + UNLOCK_TOKEN_BASE64_LEN] = 0;

			// Init UBIFS
			ubifs_init();

			// Mount the partition containing the token
			if (uboot_ubifs_mount(UNLOCK_TOKEN_PARTITION)) {
				printf("Could not mount " UNLOCK_TOKEN_PARTITION "\n");
			// Attempt to read a file with a name that matches the UID. This will fail silently if the file is absent.
			} else if (ubifs_load(UID_str, (u32) unlock_data, (UNLOCK_TOKEN_PREFIX_LEN + UNLOCK_TOKEN_BASE64_LEN))) {
				// Do nothing
			} else if (strlen(unlock_data) != (UNLOCK_TOKEN_PREFIX_LEN + UNLOCK_TOKEN_BASE64_LEN)) {
				printf("%s: Unlock token length incorrect: %d\n", __func__, strlen(unlock_data));
#ifdef CONFIG_UNLOCK_TOKEN_PREFIX
			} else if (strncmp(unlock_data, CONFIG_UNLOCK_TOKEN_PREFIX, UNLOCK_TOKEN_PREFIX_LEN)) {
				printf("%s: Unlock token prefix invalid\n", __func__);
#endif
			} else if (base64_decode(&unlock_data[UNLOCK_TOKEN_PREFIX_LEN], unlock_data, false) != 0)  {
				printf("%s: Unlock token encoding invalid\n", __func__);
			} else {
				ret = rsa_imx_verify_key(&key, (uint8_t *)unlock_data, UNLOCK_TOKEN_SIZE_BYTES, hash);
				unlock_token_source = "filesystem";
			}
		}

		if (ret) {
			printf("Unlocking token not present or invalid (%d)\n", ret);
		} else {
			printf("Unlocking token verified from %s\n", unlock_token_source);
			unlocked = true;
		}
	}

	return unlocked;
}

/* Get the 64-bit unique silicon ID and convert it to ASCII hex so that it can be used to generate an unlock token
 * Also get the SOC SEC_CONFIG value burned into OCOTP so that we can determine the SOC's security state
 */
int board_get_UID_sec_config(char *UID_str, int maxlen, uint32_t *sec_config) {
	uint32_t UID[2];                         // The 64-bit UID
	int ret;

	memset(UID_str, 0x00, maxlen);

	ret = fuse_read(OTP_BANK_UID, OTP_WORD_UID_L, &UID[0]);
	if (!ret)
		ret = fuse_read(OTP_BANK_UID, OTP_WORD_UID_H, &UID[1]);
	if (!ret)
		ret = fuse_read(OTP_BANK_SEC_CONFIG, OTP_WORD_SEC_CONFIG, sec_config);
	if (!ret)
		snprintf(UID_str, maxlen, "%08x%08x", UID[1], UID[0]);

	return ret;
}

/* Switch the system into either the fully locked or fully unlocked state */
static void board_update_unlocked(bool unlocked)
{
	void *blob = ((void *)gd_fdt_blob());
	int sig_node;
	int ret = 0;

	// Update the system state based on locked/unlocked status
	if (unlocked) {
		// Update the control FDT's boot config to disable ITB signature checking, by finding all
		// subnodes in the control FDT that contain a "required" property and set the value to "none".
		sig_node = fdt_subnode_offset(blob, 0, FIT_SIG_NODENAME);
		if (sig_node < 0) {
			// This will happen on systems that don't have the public
			// key tree, so we don't make any noise about it.
			debug("%s: Unable to get sig node: %d\n", __func__, ret);
		} else {
			int noffset = fdt_first_subnode(blob, sig_node);
			while ((noffset >= 0) && (!ret)) {
				if (fdt_getprop(blob, noffset, "required", NULL)) {
					ret = fdt_setprop(blob, noffset, "required", "none", 4);
				}
				noffset = fdt_next_subnode(blob, noffset);
			}
		}

		// Use the boot delay defined in the environment (via CONFIG_BOOTDELAY)

	} else {
		// Give any outgoing characters time to clear the UART before we mux it out
		udelay(100);

		// Override the default env variable 'console', which gets passed to kernel cmdline and point it to /dev/null
		setenv("console", "/dev/null");

		// Always autoboot with no delay and don't check for an abort
		setenv("bootdelay", "-2");

		// Mux out the UART
		disable_iomux_uart();

		// Flush out any data received before the UART was muxed out
		while (tstc()) {
			getc();
		}
	}

	if (ret) {
		printf("%s: Failed updating security config: %d\n", __func__, ret);
	}
}

/* Other Board Init Functions *************************************************/
#define CELL_POWER_GPIO IMX_GPIO_NR(5, 12)

int board_early_init_f(void)
{
#ifdef CONFIG_FCT_DETECTION
	setup_iomux_fct_gpio();
#endif

#ifdef CONFIG_MACH_DIAMOND3
	setup_piezo_enable_gpio();
#endif

	setup_iomux_uart();
	return 0;
}

int board_init(void)
{
	/* Address of boot parameters */
	gd->bd->bi_boot_params = PHYS_SDRAM + 0x100;

#ifdef CONFIG_SYS_USE_NAND
	setup_gpmi_nand();
#endif

#ifdef CONFIG_SYS_I2C_MXC
	setup_i2c(3, CONFIG_SYS_I2C_SPEED, 0x7f, &i2c_pad_info4);
#endif

	save_reset_cause();

	return 0;
}

int board_late_init(void)
{
	char UID_str[SHA256_CHUNK_SZ];           // The UID's ASCII hex string representation. Size needs to be multiple of the SHA-256 chunk size.
	uint32_t sec_config;
	bool unlocked = false;

#ifdef CONFIG_PFUZE100_PMIC_I2C
	setup_pmic_mode();
#endif

	/* Power off cellular module */
	gpio_direction_output(CELL_POWER_GPIO, 1);

	/* Power on the wifi */
	board_wifi_init();

#ifdef CONFIG_NEST_PLIST

#ifndef CONFIG_ENV_IS_IN_UBI
	/*
	 * When the environment is in UBI, the UBIFS partition will be open and
	 * since this operation takes a considerable amount of time, we should
	 * skip it here.  This is equivalent to "ubi part ubipart"
	 */
	if (ubi_part(MTD_PARTITION_UBI, NULL)) {
		printf("** Cannot find mtd partition\n");
	}
#endif

#ifdef CONFIG_PLIST_NLMODEL
	/* Overwrite default nlmodel with value from provision plist */
	read_plist("ubi0:provision", "data.plist", "nlmodel", "nlmodel");
#endif

#ifdef CONFIG_PLIST_BOOTSIDE
	/* Overwrite default bootside with value from OSM state plist */
	read_plist("ubi0:system-config", "osm.plist", "bootside", "bootside");
#endif

#ifdef CONFIG_PLIST_BRIGHTNESS
	/* Overwrite default brightness with value from data uboot plist */
	read_plist("ubi0:data", "uboot.plist", "brightness", "brightness");
#endif

#ifdef CONFIG_PLIST_ANIMATION
	/* Overwrite default animation with value from data uboot plist */
	read_plist("ubi0:data", "uboot.plist", "animation", "animation");
#endif

#ifdef CONFIG_PLIST_ALARM
	/* Overwrite default brightness with value from data uboot plist */
	read_plist("ubi0:data", "uboot.plist", "alarm", "alarm");
#endif

#ifdef CONFIG_PREBOOT_ANIMATION_PARAM
	/* Enable pre-boot LED animation depending on plist value */
	setenv("preboot_animation", "1"); /* Set default in case of error */
	read_plist("ubi0:system-config", "uboot.plist", "preboot_animation", "preboot_animation");
#endif

#endif /* CONFIG_NEST_PLIST */

	/* Get board UID and SEC_CONFIG so that we can determine system security state */
	if (board_get_UID_sec_config(UID_str, SHA256_CHUNK_SZ, &sec_config)) {
		printf("%s: Error reading fuses\n", __func__);
	} else {
		printf("UID:   %s\n", UID_str);

		if ((sec_config & SEC_CONFIG_CLOSED) == 0) {
			/* Always unlock units that are still in open mode (i.e. SEC_CONFIG[1] != 1).
			 * This enables the release bootloader to be used at the factory without requiring
			 * unlocking tokens for units that haven't been through the shipping settings station.
			 * It also enables development on devices that aren't secure, or for which keys have
			 * not yet been generated.
			 */
			printf("Secure boot not enabled, device unlocked\n");
			unlocked = true;
		} else {
			/* Check for the presence of the unlock token */
			unlocked = board_unlock_token_check(UID_str);
		}
	}

	/* Update state based on locked/unlocked status */
	board_update_unlocked(unlocked);

	return 0;
}

u32 get_board_rev(void)
{
	return get_cpu_rev();
}

int checkboard(void)
{
	puts("Board: Nest Flintstone\n");

	return 0;
}

#ifdef CONFIG_LDO_BYPASS_CHECK
void ldo_mode_set(int ldo_bypass)
{
	unsigned char sw1ab_vol = PFUZE200_SW1AB_SETP(1300);
	unsigned char sw2_vol = PFUZE200_SW2_SETP(1300);

	if(ldo_bypass)
	{
		/*Clock down A9 to 400Mhz */
		prep_anatop_bypass();

#ifdef CONFIG_MACH_DIAMOND3
		/* D3 pmic has VDD_ARM and VDD_SOC OTPed to 1.35v.
		   Step down both to 1.3v before enable ldo bypass since 1.3v
		   is the datasheet max for ldo bypass mode */
		i2c_set_bus_num(CONFIG_PMIC_I2C_BUS);
		i2c_init(CONFIG_SYS_I2C_SPEED, CONFIG_PMIC_I2C_SLAVE);

		if (i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW1ABVOL, 1, &sw1ab_vol, 1) ||
		    i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW2VOL, 1, &sw2_vol, 1)) {
		    puts("Error when configuring PMIC switcher voltage!\n");
		}
		/* Wait for rail to settle down*/
		udelay(100);
#endif

		/* Enable ldo bypass mode */
		set_anatop_bypass(0);

		/* We are in ldo bypass mode now, step down VDD ARM and SOC to 1.175v for 800Mhz core */
		sw1ab_vol = PFUZE200_SW1AB_SETP(1175);
		sw2_vol = PFUZE200_SW2_SETP(1225);
		if (i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW1ABVOL, 1, &sw1ab_vol, 1) ||
		    i2c_write(CONFIG_PMIC_I2C_SLAVE, PFUZE200_SW2VOL, 1, &sw2_vol, 1)) {
		    puts("Error when configuring PMIC switcher voltage!\n");
		}
		/*wait for rail to settle down*/
		udelay(100);

		/* Restore A9 to 800Mhz */
		finish_anatop_bypass();
		puts("LDO bypass mode configured\n");
	}

}
#endif