diff --git a/app/aboot/aboot.c b/app/aboot/aboot.c
index c9a1bb6..1e134ba 100644
--- a/app/aboot/aboot.c
+++ b/app/aboot/aboot.c
@@ -2,7 +2,7 @@
  * Copyright (c) 2009, Google Inc.
  * All rights reserved.
  *
- * Copyright (c) 2009-2015, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2009-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are met:
@@ -773,10 +773,6 @@
 
 	enter_critical_section();
 
-	/* Initialise wdog to catch early kernel crashes */
-#if WDOG_SUPPORT
-	msm_wdog_init();
-#endif
 	/* do any platform specific cleanup before kernel entry */
 	platform_uninit();
 
@@ -891,7 +887,7 @@
 	{
 		case RED:
 #if FBCON_DISPLAY_MSG
-			display_bootverify_menu_thread(DISPLAY_MENU_RED);
+			display_bootverify_menu(DISPLAY_MENU_RED);
 			wait_for_users_action();
 #else
 			dprintf(CRITICAL,
@@ -902,7 +898,7 @@
 			break;
 		case YELLOW:
 #if FBCON_DISPLAY_MSG
-			display_bootverify_menu_thread(DISPLAY_MENU_YELLOW);
+			display_bootverify_menu(DISPLAY_MENU_YELLOW);
 			wait_for_users_action();
 #else
 			dprintf(CRITICAL,
@@ -1181,7 +1177,7 @@
 	if(boot_verify_get_state() == ORANGE)
 	{
 #if FBCON_DISPLAY_MSG
-		display_bootverify_menu_thread(DISPLAY_MENU_ORANGE);
+		display_bootverify_menu(DISPLAY_MENU_ORANGE);
 		wait_for_users_action();
 #else
 		dprintf(CRITICAL,
@@ -2006,7 +2002,7 @@
 		}
 
 #if FBCON_DISPLAY_MSG
-		display_unlock_menu_thread(type);
+		display_unlock_menu(type);
 		fastboot_okay("");
 		return;
 #else
@@ -3085,9 +3081,10 @@
 {
 	char *p = NULL;
 	const char *delim = " \t\n\r";
+	char *sp;
 
 	if (arg) {
-		p = strtok((char *)arg, delim);
+		p = strtok_r((char *)arg, delim, &sp);
 		if (p) {
 			if (!strncmp(p, "0", 1)) {
 				device.charger_screen_enabled = 0;
@@ -3566,6 +3563,11 @@
 {
 	unsigned reboot_mode = 0;
 
+	/* Initialise wdog to catch early lk crashes */
+#if WDOG_SUPPORT
+	msm_wdog_init();
+#endif
+
 	/* Setup page size information for nv storage */
 	if (target_is_emmc_boot())
 	{
@@ -3621,15 +3623,9 @@
 	if (keys_get_state(KEY_VOLUMEUP) && keys_get_state(KEY_VOLUMEDOWN))
 	{
 		dprintf(ALWAYS,"dload mode key sequence detected\n");
-		if (set_download_mode(EMERGENCY_DLOAD))
-		{
-			dprintf(CRITICAL,"dload mode not supported by target\n");
-		}
-		else
-		{
-			reboot_device(DLOAD);
-			dprintf(CRITICAL,"Failed to reboot into dload mode\n");
-		}
+		reboot_device(EMERGENCY_DLOAD);
+		dprintf(CRITICAL,"Failed to reboot into dload mode\n");
+
 		boot_into_fastboot = true;
 	}
 	if (!boot_into_fastboot)
@@ -3728,7 +3724,7 @@
 	/* initialize and start fastboot */
 	fastboot_init(target_get_scratch_address(), target_get_max_flash_size());
 #if FBCON_DISPLAY_MSG
-	display_fastboot_menu_thread();
+	display_fastboot_menu();
 #endif
 }
 
diff --git a/dev/gcdb/display/include/panel_hx8394f_720p_video.h b/dev/gcdb/display/include/panel_hx8394f_720p_video.h
index a3819b7..4442d3b 100644
--- a/dev/gcdb/display/include/panel_hx8394f_720p_video.h
+++ b/dev/gcdb/display/include/panel_hx8394f_720p_video.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -86,8 +86,8 @@
 static char hx8394f_720p_video_on_cmd4[] = {
 	0x16, 0x00, 0x29, 0xC0,
 	0xB4, 0x6C, 0x6D, 0x6C,
-	0x6D, 0x6C, 0x6D, 0x01,
-	0x01, 0xFF, 0x75, 0x00,
+	0x6A, 0x6C, 0x6D, 0x01,
+	0x05, 0xFF, 0x77, 0x00,
 	0x3f, 0x6C, 0x6D, 0x6C,
 	0x6D, 0x6C, 0x6D, 0x01,
 	0x01, 0xFF, 0xFF, 0xFF,
diff --git a/dev/gcdb/display/include/panel_jdi_4k_dualdsi_video_nofbc.h b/dev/gcdb/display/include/panel_jdi_4k_dualdsi_video_nofbc.h
index ae73869..0fe301a 100644
--- a/dev/gcdb/display/include/panel_jdi_4k_dualdsi_video_nofbc.h
+++ b/dev/gcdb/display/include/panel_jdi_4k_dualdsi_video_nofbc.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -178,7 +178,7 @@
 };
 
 static struct labibb_desc jdi_4k_dualdsi_video_nofbc_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 #endif /*_PANEL_JDI_4K_DUALDSI_VIDEO_NOFBC__H_*/
diff --git a/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_cmd.h b/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_cmd.h
index f1a39b0..2e8f306 100644
--- a/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_cmd.h
+++ b/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_cmd.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -319,7 +319,7 @@
 };
 
 static struct labibb_desc nt35597_wqxga_dsc_cmd_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_video.h b/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_video.h
index 572d2d5..f1f5247 100644
--- a/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_video.h
+++ b/dev/gcdb/display/include/panel_nt35597_wqxga_dsc_video.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -306,7 +306,7 @@
 };
 
 static struct labibb_desc nt35597_wqxga_dsc_video_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_cmd.h b/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_cmd.h
index 17248ba..432d40b 100644
--- a/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_cmd.h
+++ b/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_cmd.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -260,7 +260,7 @@
 };
 
 static struct labibb_desc nt35597_wqxga_dualdsi_cmd_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_video.h b/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_video.h
index 556d657..bc14996 100644
--- a/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_video.h
+++ b/dev/gcdb/display/include/panel_nt35597_wqxga_dualdsi_video.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -252,7 +252,7 @@
 };
 
 static struct labibb_desc nt35597_wqxga_dualdsi_video_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/gcdb/display/include/panel_r69006_1080p_cmd.h b/dev/gcdb/display/include/panel_r69006_1080p_cmd.h
index dc84080..01c8d17 100755
--- a/dev/gcdb/display/include/panel_r69006_1080p_cmd.h
+++ b/dev/gcdb/display/include/panel_r69006_1080p_cmd.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -376,10 +376,9 @@
 };
 
 static struct labibb_desc r69006_1080p_cmd_labibb = {
-	0, 1, 5700000, 5700000, 5700000, 5700000, 3, 3, 1
+	0, 1, 5700000, 5700000, 5700000, 5700000, 3, 3, 1, 0
 };
 
-
 #define R69006_1080P_CMD_PANEL_ON_DELAY 32
 
 #define R69006_1080P_CMD_SIGNATURE 0xFFFF
diff --git a/dev/gcdb/display/include/panel_r69006_1080p_video.h b/dev/gcdb/display/include/panel_r69006_1080p_video.h
index aaf679c..82be298 100755
--- a/dev/gcdb/display/include/panel_r69006_1080p_video.h
+++ b/dev/gcdb/display/include/panel_r69006_1080p_video.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -301,7 +301,7 @@
 };
 
 static struct labibb_desc r69006_1080p_video_labibb = {
-	0, 1, 5700000, 5700000, 5700000, 5700000, 3, 3, 1
+	0, 1, 5700000, 5700000, 5700000, 5700000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/gcdb/display/include/panel_r69007_wqxga_cmd.h b/dev/gcdb/display/include/panel_r69007_wqxga_cmd.h
index 70a5225..dab049b 100644
--- a/dev/gcdb/display/include/panel_r69007_wqxga_cmd.h
+++ b/dev/gcdb/display/include/panel_r69007_wqxga_cmd.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -368,7 +368,7 @@
 };
 
 static struct labibb_desc r69007_wqxga_cmd_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/gcdb/display/include/panel_sharp_wqxga_dualdsi_video.h b/dev/gcdb/display/include/panel_sharp_wqxga_dualdsi_video.h
index 470ddda..f317ed3 100644
--- a/dev/gcdb/display/include/panel_sharp_wqxga_dualdsi_video.h
+++ b/dev/gcdb/display/include/panel_sharp_wqxga_dualdsi_video.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -156,7 +156,7 @@
 };
 
 static struct labibb_desc sharp_wqxga_dualdsi_video_labibb = {
-	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1
+	0, 1, 5500000, 5500000, 5500000, 5500000, 3, 3, 1, 0
 };
 
 /*---------------------------------------------------------------------------*/
diff --git a/dev/pmic/pmi8994/include/pm_app_smbchg.h b/dev/pmic/pmi8994/include/pm_app_smbchg.h
index 81a7b39..0a55b99 100644
--- a/dev/pmic/pmi8994/include/pm_app_smbchg.h
+++ b/dev/pmic/pmi8994/include/pm_app_smbchg.h
@@ -202,5 +202,6 @@
 bool pm_appsbl_charging_in_progress();
 pm_err_flag_type pm_appsbl_set_dcin_suspend();
 bool pm_app_display_shutdown_in_prgs();
+pm_err_flag_type pm_smbchg_get_charger_path(uint32 device_index, pm_smbchg_usb_chgpth_pwr_pth_type* charger_path);
 #endif  //PM_APP_SMBCHG__H
 
diff --git a/dev/pmic/pmi8994/pm_app_smbchg.c b/dev/pmic/pmi8994/pm_app_smbchg.c
index c3a03be..7ee25d9 100644
--- a/dev/pmic/pmi8994/pm_app_smbchg.c
+++ b/dev/pmic/pmi8994/pm_app_smbchg.c
@@ -87,7 +87,6 @@
 
 char panel_name[256];
 
-pm_err_flag_type pm_smbchg_get_charger_path(uint32 device_index, pm_smbchg_usb_chgpth_pwr_pth_type* charger_path);
 pm_err_flag_type pm_appsbl_chg_config_vbat_low_threshold(uint32 device_index, pm_smbchg_specific_data_type *chg_param_ptr);
 static void display_thread_initialize();
 static void pm_app_ima_read_voltage(uint32_t *);
diff --git a/dev/qpnp_wled/include/qpnp_wled.h b/dev/qpnp_wled/include/qpnp_wled.h
index dcd4a8a..3f03703 100644
--- a/dev/qpnp_wled/include/qpnp_wled.h
+++ b/dev/qpnp_wled/include/qpnp_wled.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
 
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -44,6 +44,10 @@
 #define QPNP_WLED_SWITCH_FREQ_REG(b)           (b + 0x4C)
 #define QPNP_WLED_OVP_REG(b)                   (b + 0x4D)
 #define QPNP_WLED_ILIM_REG(b)                  (b + 0x4E)
+#define QPNP_WLED_VLOOP_COMP_RES(b)            (b + 0x55)
+#define QPNP_WLED_VLOOP_COMP_GM(b)             (b + 0x56)
+#define QPNP_WLED_PSM_CTRL(b)                  (b + 0x5B)
+#define QPNP_WLED_TEST4(b)		       (b + 0xE5)
 
 #define QPNP_WLED_EN_MASK                      0x7F
 #define QPNP_WLED_EN_SHIFT                     7
@@ -134,6 +138,9 @@
 #define QPNP_WLED_MODULE_RDY_SHIFT             7
 #define QPNP_WLED_MODULE_EN_MASK               0x7F
 #define QPNP_WLED_MODULE_EN_SHIFT              7
+#define QPNP_IBB_SWIRE_RDY_MASK               0x40
+#define QPNP_IBB_SWIRE_RDY_SHIFT              6
+#define QPNP_IBB_MODULE_EN_MASK               0x80
 #define QPNP_WLED_DISP_SEL_MASK                0x7F
 #define QPNP_WLED_DISP_SEL_SHIFT               7
 
@@ -148,6 +155,7 @@
 #define QPNP_WLED_IBB_PWRDN_DLY_MAX_MS         3
 #define IBB_LAB_VREG_STEP_SIZE                 100000
 #define QPNP_LABIBB_OUTPUT_VOLTAGE             0x41
+#define QPNP_LABIBB_PS_CTL		       0x50
 #define QPNP_LAB_OUTPUT_OVERRIDE_EN            BIT(7)
 #define QPNP_LAB_SET_VOLTAGE_MASK              (BIT(4) - 1)
 #define QPNP_IBB_SET_VOLTAGE_MASK              (BIT(6) - 1)
@@ -260,6 +268,8 @@
 	uint32_t ibb_max_volt;
 	uint32_t ibb_init_volt;
 	uint32_t lab_init_volt;
+	bool lab_ibb_swire_control;
+	bool wled_avdd_control;
 };
 
 struct qpnp_wled_config_data {
@@ -273,6 +283,8 @@
 	uint32_t ibb_max_volt;
 	uint32_t ibb_init_volt;
 	uint32_t lab_init_volt;
+	bool lab_ibb_swire_control;
+	bool wled_avdd_control;
 };
 /* WLED Initial Setup */
 int qpnp_wled_init(struct qpnp_wled_config_data *config);
diff --git a/dev/qpnp_wled/qpnp_wled.c b/dev/qpnp_wled/qpnp_wled.c
index 0ae44a3..9cedbb7 100644
--- a/dev/qpnp_wled/qpnp_wled.c
+++ b/dev/qpnp_wled/qpnp_wled.c
@@ -1,4 +1,4 @@
- /* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+ /* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
 
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -95,6 +95,21 @@
 	return 0;
 }
 
+static int qpnp_wled_ibb_swire_rdy(struct qpnp_wled *wled,
+				uint16_t base_addr, bool state)
+{
+	uint8_t reg;
+
+	reg = pm8x41_wled_reg_read(
+			QPNP_WLED_MODULE_EN_REG(base_addr));
+	/* Do not enable IBB module when SWIRE ready is set */
+	reg &= ~(QPNP_IBB_SWIRE_RDY_MASK | QPNP_IBB_MODULE_EN_MASK);
+	reg |= (state << QPNP_IBB_SWIRE_RDY_SHIFT);
+	pm8x41_wled_reg_write(QPNP_WLED_MODULE_EN_REG(base_addr), reg);
+
+	return 0;
+}
+
 int qpnp_ibb_enable(bool state)
 {
 	int rc = 0;
@@ -119,7 +134,10 @@
 		pm8x41_wled_reg_write(QPNP_WLED_LAB_IBB_RDY_REG(gwled->lab_base), reg);
 	}
 
-	rc = qpnp_wled_enable(gwled, gwled->ibb_base, state);
+	if (gwled->disp_type_amoled && gwled->lab_ibb_swire_control)
+		rc = qpnp_wled_ibb_swire_rdy(gwled, gwled->ibb_base, state);
+	else
+		rc = qpnp_wled_enable(gwled, gwled->ibb_base, state);
 
 	return rc;
 }
@@ -141,12 +159,14 @@
 			return;
 		}
 	}
-	rc = qpnp_wled_enable(gwled, gwled->ctrl_base, enable);
 
-	if (rc) {
-		dprintf(CRITICAL,"wled %sable failed\n",
-					enable ? "en" : "dis");
-		return;
+	if (!gwled->disp_type_amoled || !gwled->wled_avdd_control) {
+		rc = qpnp_wled_enable(gwled, gwled->ctrl_base, enable);
+		if (rc) {
+			dprintf(CRITICAL, "wled %sable failed\n",
+						enable ? "en" : "dis");
+			return;
+		}
 	}
 
 }
@@ -195,6 +215,21 @@
 	if (rc < 0)
 		return rc;
 
+	/* Recommended WLED MDOS settings for AMOLED */
+	if (wled->disp_type_amoled) {
+		pm8x41_wled_reg_write(QPNP_WLED_VLOOP_COMP_RES(wled->ctrl_base),
+			0x8F);
+		pm8x41_wled_reg_write(QPNP_WLED_VLOOP_COMP_GM(wled->ctrl_base),
+			0x81);
+		pm8x41_wled_reg_write(QPNP_WLED_PSM_CTRL(wled->ctrl_base),
+			0x83);
+
+		rc = qpnp_wled_sec_access(wled, wled->ctrl_base);
+		if (rc)
+			return rc;
+		pm8x41_wled_reg_write(QPNP_WLED_TEST4(wled->ctrl_base), 0x13);
+	}
+
 	/* Configure the FEEDBACK OUTPUT register */
 	reg = pm8x41_wled_reg_read(
 			QPNP_WLED_FDBK_OP_REG(wled->ctrl_base));
@@ -402,6 +437,11 @@
 	if (rc < 0)
 		return rc;
 
+	/* Disable LAB pulse skipping for AMOLED */
+	if (wled->disp_type_amoled)
+		pm8x41_wled_reg_write(wled->lab_base +
+			QPNP_LABIBB_PS_CTL, 0x00);
+
 	/* IBB active bias */
 	if (wled->ibb_pwrup_dly_ms > QPNP_WLED_IBB_PWRUP_DLY_MAX_MS)
 		wled->ibb_pwrup_dly_ms = QPNP_WLED_IBB_PWRUP_DLY_MAX_MS;
@@ -493,6 +533,8 @@
 	wled->ibb_max_volt = config->ibb_max_volt;
 	wled->ibb_init_volt = config->ibb_init_volt;
 	wled->lab_init_volt = config->lab_init_volt;
+	wled->lab_ibb_swire_control = config->lab_ibb_swire_control;
+	wled->wled_avdd_control = config->wled_avdd_control;
 
 	return 0;
 }
@@ -531,49 +573,68 @@
 	uint32_t new_uV;
 	uint8_t val, mask=0;
 
-	if (wled->lab_min_volt < wled->lab_init_volt) {
-		dprintf(CRITICAL,"qpnp_lab_regulator_set_voltage failed, min_uV %d is less than init volt %d\n",
-		wled->lab_min_volt, wled->lab_init_volt);
-		return rc;
+	if (!wled->disp_type_amoled || !wled->lab_ibb_swire_control) {
+		if (wled->lab_min_volt < wled->lab_init_volt) {
+			dprintf(CRITICAL,"qpnp_lab_regulator_set_voltage failed, min_uV %d is less than init volt %d\n",
+			wled->lab_min_volt, wled->lab_init_volt);
+			return rc;
+		}
+
+		val = (((wled->lab_min_volt - wled->lab_init_volt) +
+		(IBB_LAB_VREG_STEP_SIZE - 1)) / IBB_LAB_VREG_STEP_SIZE);
+		new_uV = val * IBB_LAB_VREG_STEP_SIZE + wled->lab_init_volt;
+
+		if (new_uV > wled->lab_max_volt) {
+			dprintf(CRITICAL,"qpnp_ibb_regulator_set_voltage unable to set voltage (%d %d)\n",
+			wled->lab_min_volt, wled->lab_max_volt);
+			return rc;
+		}
+		val |= QPNP_LAB_OUTPUT_OVERRIDE_EN;
+		mask = pm8x41_wled_reg_read(wled->lab_base +
+				QPNP_LABIBB_OUTPUT_VOLTAGE);
+		mask &= ~(QPNP_LAB_SET_VOLTAGE_MASK
+				| QPNP_LAB_OUTPUT_OVERRIDE_EN);
+		mask |= val & (QPNP_LAB_SET_VOLTAGE_MASK
+				| QPNP_LAB_OUTPUT_OVERRIDE_EN);
+
+		pm8x41_wled_reg_write(wled->lab_base +
+				QPNP_LABIBB_OUTPUT_VOLTAGE, mask);
+		udelay(2);
+
+		/*
+		 * IBB Set Voltage.
+		 * For AMOLED panels, the IBB voltage needs to be
+		 * controlled by panel.
+		 */
+		if (wled->ibb_min_volt < wled->ibb_init_volt) {
+			dprintf(CRITICAL, "qpnp_ibb_regulator_set_voltage failed, min_uV %d is less than init volt %d\n",
+			wled->ibb_min_volt, wled->ibb_init_volt);
+			return rc;
+		}
+
+		val = (((wled->ibb_min_volt - wled->ibb_init_volt) +
+			(IBB_LAB_VREG_STEP_SIZE - 1)) / IBB_LAB_VREG_STEP_SIZE);
+		new_uV = val * IBB_LAB_VREG_STEP_SIZE + wled->ibb_init_volt;
+		if (new_uV > wled->ibb_max_volt) {
+			dprintf(CRITICAL, "qpnp_ibb_regulator_set_voltage unable to set voltage %d %d\n",
+			wled->ibb_min_volt, wled->ibb_max_volt);
+			return rc;
+		}
+		val |= QPNP_LAB_OUTPUT_OVERRIDE_EN;
+		mask = pm8x41_wled_reg_read(wled->ibb_base +
+			QPNP_LABIBB_OUTPUT_VOLTAGE);
+		udelay(2);
+		mask &= ~(QPNP_IBB_SET_VOLTAGE_MASK |
+			QPNP_LAB_OUTPUT_OVERRIDE_EN);
+		mask |= (val & (QPNP_IBB_SET_VOLTAGE_MASK |
+			QPNP_LAB_OUTPUT_OVERRIDE_EN));
+
+		pm8x41_wled_reg_write(wled->ibb_base +
+			QPNP_LABIBB_OUTPUT_VOLTAGE, mask);
+	} else {
+		pm8x41_wled_reg_write(wled->ibb_base +
+			QPNP_LABIBB_OUTPUT_VOLTAGE, 0x00);
 	}
 
-	val = (((wled->lab_min_volt - wled->lab_init_volt) + (IBB_LAB_VREG_STEP_SIZE - 1)) / IBB_LAB_VREG_STEP_SIZE);
-	new_uV = val * IBB_LAB_VREG_STEP_SIZE + wled->lab_init_volt;
-
-	if (new_uV > wled->lab_max_volt) {
-		dprintf(CRITICAL,"qpnp_ibb_regulator_set_voltage unable to set voltage (%d %d)\n",
-		wled->lab_min_volt, wled->lab_max_volt);
-		return rc;
-	}
-	val |= QPNP_LAB_OUTPUT_OVERRIDE_EN;
-	mask = pm8x41_wled_reg_read(wled->lab_base + QPNP_LABIBB_OUTPUT_VOLTAGE);
-	mask &= ~(QPNP_LAB_SET_VOLTAGE_MASK | QPNP_LAB_OUTPUT_OVERRIDE_EN);
-	mask |= val & (QPNP_LAB_SET_VOLTAGE_MASK | QPNP_LAB_OUTPUT_OVERRIDE_EN);
-
-	pm8x41_wled_reg_write(wled->lab_base + QPNP_LABIBB_OUTPUT_VOLTAGE, mask);
-	udelay(2);
-
-	/* IBB Set Voltage */
-	if (wled->ibb_min_volt < wled->ibb_init_volt) {
-		dprintf(CRITICAL, "qpnp_ibb_regulator_set_voltage failed, min_uV %d is less than init volt %d\n",
-		wled->ibb_min_volt, wled->ibb_init_volt);
-		return rc;
-	}
-
-	val = (((wled->ibb_min_volt - wled->ibb_init_volt) + (IBB_LAB_VREG_STEP_SIZE - 1)) / IBB_LAB_VREG_STEP_SIZE);
-	new_uV = val * IBB_LAB_VREG_STEP_SIZE + wled->ibb_init_volt;
-	if (new_uV > wled->ibb_max_volt) {
-		dprintf(CRITICAL,"qpnp_ibb_regulator_set_voltage unable to set voltage %d %d\n",
-		wled->ibb_min_volt, wled->ibb_max_volt);
-		return rc;
-	}
-	val |= QPNP_LAB_OUTPUT_OVERRIDE_EN;
-	mask = pm8x41_wled_reg_read(wled->ibb_base + QPNP_LABIBB_OUTPUT_VOLTAGE);
-	udelay(2);
-	mask &= ~(QPNP_IBB_SET_VOLTAGE_MASK | QPNP_LAB_OUTPUT_OVERRIDE_EN);
-	mask |= (val & (QPNP_IBB_SET_VOLTAGE_MASK | QPNP_LAB_OUTPUT_OVERRIDE_EN));
-
-	pm8x41_wled_reg_write(wled->ibb_base + QPNP_LABIBB_OUTPUT_VOLTAGE,mask);
-
 	return 0;
 }
diff --git a/include/platform.h b/include/platform.h
index 31de506..f97273a 100644
--- a/include/platform.h
+++ b/include/platform.h
@@ -58,7 +58,7 @@
 uint32_t check_alarm_boot(void);
 void platform_uninit_timer(void);
 void reboot_device(unsigned);
-int set_download_mode(enum dload_mode mode);
+int set_download_mode(enum reboot_reason mode);
 uint32_t platform_get_smem_base_addr();
 uint32_t platform_get_sclk_count(void);
 void clock_config_cdc(uint32_t interface);
diff --git a/platform/msm_shared/debug.c b/platform/msm_shared/debug.c
index b8c3266..7c5a9b1 100644
--- a/platform/msm_shared/debug.c
+++ b/platform/msm_shared/debug.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2009, Google Inc.
  * All rights reserved.
- * Copyright (c) 2009-2015, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2009-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -151,16 +151,9 @@
 #if PON_VIB_SUPPORT
 	vib_turn_off();
 #endif
-	if (set_download_mode(NORMAL_DLOAD) == 0)
-	{
-		dprintf(CRITICAL, "HALT: reboot into dload mode...\n");
-		reboot_device(DLOAD);
-		dprintf(CRITICAL, "HALT: reboot_device failed\n");
-	}
-	else
-	{
-		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
-	}
+	dprintf(CRITICAL, "HALT: reboot into dload mode...\n");
+	reboot_device(NORMAL_DLOAD);
+
 	dprintf(CRITICAL, "HALT: spinning forever...\n");
 	for (;;) ;
 }
diff --git a/platform/msm_shared/display_menu.c b/platform/msm_shared/display_menu.c
index bb988df..abc7fac 100644
--- a/platform/msm_shared/display_menu.c
+++ b/platform/msm_shared/display_menu.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
@@ -51,21 +51,21 @@
 				"applications to stop working properly.\n\n"\
 				"To prevent unauthorized access to your personal data, "\
 				"unlocking the bootloader will also delete all personal "\
-				"data from your phone(a \"fatory data reset\").\n\n"\
+				"data from your phone(a \"factory data reset\").\n\n"\
 				"Press the Volume Up/Down buttons to select Yes "\
 				"or No. Then press the Power button to continue.\n";
 
-#define YELLOW_WARNING_MSG	"Your device has loaded a diffrent operating "\
+#define YELLOW_WARNING_MSG	"Your device has loaded a different operating "\
 				"system\n\nTo learn more, visit:\n"
 
-#define ORANGE_WARNING_MSG	"Your device has been unlocker and cann't "\
+#define ORANGE_WARNING_MSG	"Your device has been unlocked and can't "\
 				"be trusted\n\nTo learn more, visit:\n"
 
 #define RED_WARNING_MSG	"Your device has failed verification and may "\
 				"not work properly\n\nTo learn more, visit:\n"
 
 static bool is_thread_start = false;
-struct select_msg_info msg_info;
+static struct select_msg_info msg_info;
 
 #if VERIFIED_BOOT
 struct boot_verify_info {
@@ -100,15 +100,14 @@
 	struct select_msg_info *select_msg;
 	select_msg = &msg_info;
 
-	while(1) {
-		if (select_msg->msg_timeout == true &&
-			select_msg->msg_volume_key_pressed == false)
-			break;
-		if (select_msg->msg_power_key_pressed == true)
-			break;
-
+	mutex_acquire(&select_msg->msg_lock);
+	while(!select_msg->info.is_timeout == true) {
+		mutex_release(&select_msg->msg_lock);
 		thread_sleep(10);
+		mutex_acquire(&select_msg->msg_lock);
 	}
+	mutex_release(&select_msg->msg_lock);
+
 	fbcon_clear();
 	display_image_on_screen();
 }
@@ -159,9 +158,9 @@
 			else
 				diff = max_x/factor - strlen(str);
 			for (i = 0; i < diff; i++) {
-				strcat(str_target, " ");
+				strlcat(str_target, " ", max_x);
 			}
-			strcat(str_target, str);
+			strlcat(str_target, str, max_x);
 			return str_target;
 		} else {
 			free(str_target);
@@ -171,10 +170,12 @@
 	return str;
 }
 
-void display_unlock_menu(struct select_msg_info *unlock_msg_info, int type)
+/* msg_lock need to be holded when call this function. */
+void display_unlock_menu_renew(struct select_msg_info *unlock_msg_info, int type)
 {
 	fbcon_clear();
-	memset(unlock_msg_info, 0, sizeof(struct select_msg_info));
+	memset(&unlock_msg_info->info, 0, sizeof(struct menu_info));
+
 	display_fbcon_menu_message("Unlock bootloader?\n",
 		FBCON_UNLOCK_TITLE_MSG, big_factor);
 	fbcon_draw_line(FBCON_COMMON_MSG);
@@ -182,33 +183,37 @@
 	display_fbcon_menu_message((char*)unlock_menu_common_msg,
 		FBCON_COMMON_MSG, common_factor);
 	fbcon_draw_line(FBCON_COMMON_MSG);
-	unlock_msg_info->option_start[0] = fbcon_get_current_line();
+	unlock_msg_info->info.option_start[0] = fbcon_get_current_line();
 	display_fbcon_menu_message("Yes\n",
 		FBCON_COMMON_MSG, big_factor);
-	unlock_msg_info->option_bg[0] = fbcon_get_current_bg();
+	unlock_msg_info->info.option_bg[0] = fbcon_get_current_bg();
 	display_fbcon_menu_message("Unlock bootloader(may void warranty)\n",
 		FBCON_COMMON_MSG, common_factor);
-	unlock_msg_info->option_end[0] = fbcon_get_current_line();
+	unlock_msg_info->info.option_end[0] = fbcon_get_current_line();
 	fbcon_draw_line(FBCON_COMMON_MSG);
-	unlock_msg_info->option_start[1] = fbcon_get_current_line();
+	unlock_msg_info->info.option_start[1] = fbcon_get_current_line();
 	display_fbcon_menu_message("No\n",
 		FBCON_COMMON_MSG, big_factor);
-	unlock_msg_info->option_bg[1] = fbcon_get_current_bg();
+	unlock_msg_info->info.option_bg[1] = fbcon_get_current_bg();
 	display_fbcon_menu_message("Do not unlock bootloader and restart phone\n",
 		FBCON_COMMON_MSG, common_factor);
-	unlock_msg_info->option_end[1] = fbcon_get_current_line();
+	unlock_msg_info->info.option_end[1] = fbcon_get_current_line();
 	fbcon_draw_line(FBCON_COMMON_MSG);
 
 	if (type == UNLOCK)
-		unlock_msg_info->msg_type = DISPLAY_MENU_UNLOCK;
+		unlock_msg_info->info.msg_type = DISPLAY_MENU_UNLOCK;
 	else if (type == UNLOCK_CRITICAL)
-		unlock_msg_info->msg_type = DISPLAY_MENU_UNLOCK_CRITICAL;
+		unlock_msg_info->info.msg_type = DISPLAY_MENU_UNLOCK_CRITICAL;
 
-	unlock_msg_info->option_num = 2;
+	unlock_msg_info->info.option_num = 2;
+
+	/* Initialize the option index */
+	unlock_msg_info->info.option_index= 2;
 }
 
 #if VERIFIED_BOOT
-void display_boot_verified_menu(struct select_msg_info *msg_info, int type)
+/* msg_lock need to be holded when call this function. */
+void display_bootverify_menu_renew(struct select_msg_info *msg_info, int type)
 {
 	unsigned char* fp_buf = NULL;
 	char fp_str_temp[EVP_MAX_MD_SIZE] = {'\0'};
@@ -222,7 +227,7 @@
 	unsigned int i = 0;
 
 	fbcon_clear();
-	memset(msg_info, 0, sizeof(struct select_msg_info));
+	memset(&msg_info->info, 0, sizeof(struct menu_info));
 
 	/* Align Right */
 	str_target = str_align_right(str1, big_factor);
@@ -247,14 +252,14 @@
 	if (type == DISPLAY_MENU_YELLOW) {
 		fp_buf = get_boot_fingerprint(&fp_size);
 		if (fp_buf != NULL) {
-			strncpy(fp_str_temp, (char const *)fp_buf, fp_size);
+			strlcpy(fp_str_temp, (char const *)fp_buf, fp_size);
 			for (i = 0; i < fp_size; i++) {
 				if(i == fp_size - 1)
-					sprintf(str_temp, "%02x", fp_str_temp[i]);
+					snprintf(str_temp, sizeof(str_temp), "%02x", fp_str_temp[i]);
 				else
-					sprintf(str_temp, "%02x-", fp_str_temp[i]);
+					snprintf(str_temp, sizeof(str_temp), "%02x-", fp_str_temp[i]);
 
-				strcat(fp_str, str_temp);
+				strlcat(fp_str, str_temp, sizeof(fp_str));
 			}
 		}
 		display_fbcon_menu_message("ID:", FBCON_COMMON_MSG, common_factor);
@@ -264,19 +269,24 @@
 	display_fbcon_menu_message("\n\nIf no key pressed:\n"\
 		"Your device will boot in 5 seconds\n\n", FBCON_COMMON_MSG, common_factor);
 
-	msg_info->msg_type = type;
+	msg_info->info.msg_type = type;
 	if(str_target) {
 		free(str_target);
 	}
+
+	/* Initialize the time out time */
+	msg_info->info.timeout_time = 5000; //5s
 }
 #endif
 
-void display_boot_verified_option(struct select_msg_info *msg_info)
+/* msg_lock need to be holded when call this function. */
+void display_bootverify_option_menu_renew(struct select_msg_info *msg_info)
 {
 	int i = 0;
 	int len = 0;
+
 	fbcon_clear();
-	memset(msg_info, 0, sizeof(struct select_msg_info));
+	memset(&msg_info->info, 0, sizeof(struct menu_info));
 
 	len = ARRAY_SIZE(verify_option_menu);
 	display_fbcon_menu_message("Options menu:\n\n",
@@ -286,28 +296,36 @@
 
 	for (i = 0; i < len; i++) {
 		fbcon_draw_line(FBCON_COMMON_MSG);
-		msg_info->option_start[i] = fbcon_get_current_line();
+		msg_info->info.option_start[i] = fbcon_get_current_line();
 		display_fbcon_menu_message(verify_option_menu[i],
 			FBCON_COMMON_MSG, common_factor);
-		msg_info->option_bg[i]= fbcon_get_current_bg();
-		msg_info->option_end[i]= fbcon_get_current_line();
+		msg_info->info.option_bg[i]= fbcon_get_current_bg();
+		msg_info->info.option_end[i]= fbcon_get_current_line();
 	}
 
 	fbcon_draw_line(FBCON_COMMON_MSG);
-	msg_info->msg_type = DISPLAY_MENU_MORE_OPTION;
-	msg_info->option_num = len;
+	msg_info->info.msg_type = DISPLAY_MENU_MORE_OPTION;
+	msg_info->info.option_num = len;
+
+	/* Initialize the option index */
+	msg_info->info.option_index= len;
 }
 
-void display_fastboot_menu(struct select_msg_info *fastboot_msg_info,
-	int option_index)
+/* msg_lock need to be holded when call this function. */
+void display_fastboot_menu_renew(struct select_msg_info *fastboot_msg_info)
 {
 	int len;
 	int msg_type = FBCON_COMMON_MSG;
 	char msg_buf[64];
 	char msg[128];
 
+	/* The fastboot menu is switched base on the option index
+	 * So it's need to store the index for the menu switching
+	 */
+	uint32_t option_index = fastboot_msg_info->info.option_index;
+
 	fbcon_clear();
-	memset(fastboot_msg_info, 0, sizeof(struct select_msg_info));
+	memset(&fastboot_msg_info->info, 0, sizeof(struct menu_info));
 
 	len = ARRAY_SIZE(fastboot_option_menu);
 	switch(option_index) {
@@ -366,8 +384,21 @@
 		is_device_locked()? "locked":"unlocked");
 	display_fbcon_menu_message(msg, FBCON_RED_MSG, common_factor);
 
-	fastboot_msg_info->msg_type = DISPLAY_MENU_FASTBOOT;
-	fastboot_msg_info->option_num = len;
+	fastboot_msg_info->info.msg_type = DISPLAY_MENU_FASTBOOT;
+	fastboot_msg_info->info.option_num = len;
+	fastboot_msg_info->info.option_index = option_index;
+}
+
+void msg_lock_init()
+{
+	static bool is_msg_lock_init = false;
+	struct select_msg_info *msg_lock_info;
+	msg_lock_info = &msg_info;
+
+	if (!is_msg_lock_init) {
+		mutex_init(&msg_lock_info->msg_lock);
+		is_msg_lock_init = true;
+	}
 }
 
 static void display_menu_thread_start(struct select_msg_info *msg_info)
@@ -382,43 +413,77 @@
 			return;
 		}
 		thread_resume(thr);
+		is_thread_start = true;
 	}
-
-	is_thread_start = true;
 }
 
-void display_unlock_menu_thread(int type)
+/* The fuction be called after device in fastboot mode,
+ * so it's no need to initialize the msg_lock again
+ */
+void display_unlock_menu(int type)
 {
 	struct select_msg_info *unlock_menu_msg_info;
 	unlock_menu_msg_info = &msg_info;
 
 	set_message_factor();
-	display_unlock_menu(unlock_menu_msg_info, type);
+
+	msg_lock_init();
+	mutex_acquire(&unlock_menu_msg_info->msg_lock);
+
+	/* Initialize the last_msg_type */
+	unlock_menu_msg_info->last_msg_type =
+		unlock_menu_msg_info->info.msg_type;
+
+	display_unlock_menu_renew(unlock_menu_msg_info, type);
+	mutex_release(&unlock_menu_msg_info->msg_lock);
 
 	dprintf(INFO, "creating unlock keys detect thread\n");
 	display_menu_thread_start(unlock_menu_msg_info);
 }
 
-void display_fastboot_menu_thread()
+void display_fastboot_menu()
 {
 	struct select_msg_info *fastboot_menu_msg_info;
 	fastboot_menu_msg_info = &msg_info;
 
 	set_message_factor();
-	display_fastboot_menu(fastboot_menu_msg_info, 0);
+
+	msg_lock_init();
+	mutex_acquire(&fastboot_menu_msg_info->msg_lock);
+
+	/* There are 4 pages for fastboot menu:
+	 * Page: Start/Fastboot/Recovery/Poweroff
+	 * The menu is switched base on the option index
+	 * Initialize the option index and last_msg_type
+	 */
+	fastboot_menu_msg_info->info.option_index = 0;
+	fastboot_menu_msg_info->last_msg_type =
+		fastboot_menu_msg_info->info.msg_type;
+
+	display_fastboot_menu_renew(fastboot_menu_msg_info);
+	mutex_release(&fastboot_menu_msg_info->msg_lock);
 
 	dprintf(INFO, "creating fastboot menu keys detect thread\n");
 	display_menu_thread_start(fastboot_menu_msg_info);
 }
 
 #if VERIFIED_BOOT
-void display_bootverify_menu_thread(int type)
+void display_bootverify_menu(int type)
 {
 	struct select_msg_info *bootverify_menu_msg_info;
 	bootverify_menu_msg_info = &msg_info;
 
 	set_message_factor();
-	display_boot_verified_menu(bootverify_menu_msg_info, type);
+
+	msg_lock_init();
+	mutex_acquire(&bootverify_menu_msg_info->msg_lock);
+
+	/* Initialize the last_msg_type */
+	bootverify_menu_msg_info->last_msg_type =
+		bootverify_menu_msg_info->info.msg_type;
+
+	display_bootverify_menu_renew(bootverify_menu_msg_info, type);
+	mutex_release(&bootverify_menu_msg_info->msg_lock);
 
 	dprintf(INFO, "creating boot verify keys detect thread\n");
 	display_menu_thread_start(bootverify_menu_msg_info);
diff --git a/platform/msm_shared/dload_util.c b/platform/msm_shared/dload_util.c
index 7455a19..f00e4df 100644
--- a/platform/msm_shared/dload_util.c
+++ b/platform/msm_shared/dload_util.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2013, 2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -38,15 +38,17 @@
 #define EMERGENCY_DLOAD_COOKIE_2    0x77777777
 
 void dload_util_write_cookie(uint32_t target_dload_mode_addr,
-		enum dload_mode mode)
+		enum reboot_reason mode)
 {
 	if (mode == NORMAL_DLOAD)
 	{
+#if !DISABLE_DLOAD_MODE
 		writel(NORMAL_DLOAD_COOKIE_0, target_dload_mode_addr);
 		writel(NORMAL_DLOAD_COOKIE_1,
 				target_dload_mode_addr + sizeof(uint32_t));
+#endif
 	}
-	else
+	else if (mode == EMERGENCY_DLOAD)
 	{
 		writel(EMERGENCY_DLOAD_COOKIE_0, target_dload_mode_addr);
 		writel(EMERGENCY_DLOAD_COOKIE_1,
diff --git a/platform/msm_shared/hsusb.c b/platform/msm_shared/hsusb.c
index 6a8e9e6..5f8d3e4 100644
--- a/platform/msm_shared/hsusb.c
+++ b/platform/msm_shared/hsusb.c
@@ -906,6 +906,12 @@
 	/* create our configuration descriptor */
 	size = 9 + udc_ifc_desc_size(the_gadget);
 	desc = udc_descriptor_alloc(TYPE_CONFIGURATION, 0, size);
+	if(!desc)
+	{
+		dprintf(CRITICAL, "Failed to allocate device descriptor\n");
+		ASSERT(0);
+	}
+
 	data = desc->data;
 	data[0] = 0x09;
 	data[2] = size;
diff --git a/platform/msm_shared/include/display_menu.h b/platform/msm_shared/include/display_menu.h
index b6167e4..93a1f2b 100644
--- a/platform/msm_shared/include/display_menu.h
+++ b/platform/msm_shared/include/display_menu.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
@@ -30,6 +30,7 @@
 #define __PLATFORM_MSM_SHARED_DISPLAY_MENU_H
 
 #include <openssl/evp.h>
+#include <kernel/mutex.h>
 
 #define SELECT_OPTION_MAX	5
 
@@ -43,24 +44,30 @@
 	DISPLAY_MENU_UNLOCK_CRITICAL,
 };
 
-struct select_msg_info {
+struct menu_info {
 	uint32_t	option_start[SELECT_OPTION_MAX];
 	uint32_t	option_end[SELECT_OPTION_MAX];
 	uint32_t	option_bg[SELECT_OPTION_MAX];
 	uint32_t	option_num;
+	uint32_t	option_index;
 	uint32_t	msg_type;
-	bool		msg_timeout;
-	bool		msg_power_key_pressed;
-	bool		msg_volume_key_pressed;
+	uint32_t	timeout_time;
+	bool		is_timeout;
+};
+
+struct select_msg_info {
+	struct menu_info	info;
+	uint32_t		last_msg_type;
+	mutex_t			msg_lock;
 };
 
 void wait_for_users_action(void);
-void display_unlock_menu(struct select_msg_info *msg_info, int type);
-void display_boot_verified_menu(struct select_msg_info *msg_info, int type);
-void display_boot_verified_option(struct select_msg_info *msg_info);
-void display_fastboot_menu(struct select_msg_info *fastboot_msg_info,
-	int option_index);
-void display_bootverify_menu_thread(int type);
-void display_fastboot_menu_thread();
-void display_unlock_menu_thread(int type);
+void display_unlock_menu_renew(struct select_msg_info *msg_info, int type);
+void display_bootverify_menu_renew(struct select_msg_info *msg_info, int type);
+void display_bootverify_option_menu_renew(struct select_msg_info *msg_info);
+void display_fastboot_menu_renew(struct select_msg_info *fastboot_msg_info);
+void display_bootverify_menu(int type);
+void display_fastboot_menu();
+void display_unlock_menu(int type);
+void msg_lock_init();
 #endif				/* __PLATFORM_MSM_SHARED_DISPLAY_MENU_H */
diff --git a/platform/msm_shared/include/dload_util.h b/platform/msm_shared/include/dload_util.h
index d8e02b0..8f691e5 100644
--- a/platform/msm_shared/include/dload_util.h
+++ b/platform/msm_shared/include/dload_util.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2013, 2016, The Linux Foundation. All rights reserved.
 
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -30,16 +30,9 @@
 #define __DLOAD_UTIL_H
 
 #include <sys/types.h>
-
-#define DLOAD 1
-
-enum dload_mode {
-	NORMAL_DLOAD,
-	EMERGENCY_DLOAD,
-	NORMAL_MODE
-};
+#include <reboot.h>
 
 void dload_util_write_cookie(uint32_t target_dload_mode_addr,
-		enum dload_mode mode);
+		enum reboot_reason mode);
 
 #endif
diff --git a/platform/msm_shared/include/menu_keys_detect.h b/platform/msm_shared/include/menu_keys_detect.h
index 85bd44e..2d51aa1 100644
--- a/platform/msm_shared/include/menu_keys_detect.h
+++ b/platform/msm_shared/include/menu_keys_detect.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
@@ -45,13 +45,6 @@
 	POWER_KEY,
 };
 
-enum pages_type {
-	UNLOCK_PAGE = 0,
-	BOOT_VERIFY_PAGE1,
-	BOOT_VERIFY_PAGE2,
-	FASTBOOT_PAGE,
-};
-
 int select_msg_keys_detect(void *param);
 void keys_detect_init();
 #endif				/* __PLATFORM_MSM_SHARED_KEYS_DETECT_H */
diff --git a/platform/msm_shared/include/msm_panel.h b/platform/msm_shared/include/msm_panel.h
index d2a338f..5671e02 100755
--- a/platform/msm_shared/include/msm_panel.h
+++ b/platform/msm_shared/include/msm_panel.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -373,6 +373,7 @@
 	char pwr_up_delay; /* ndx to => 1250, 2500, 5000 and 10000 us */
 	char pwr_down_delay; /* ndx to => 1250, 2500, 5000 and 10000 us */
 	char ibb_discharge_en;
+	bool swire_control;
 };
 
 struct msm_panel_info {
diff --git a/platform/msm_shared/include/scm.h b/platform/msm_shared/include/scm.h
index 2d454ed..128d14b 100644
--- a/platform/msm_shared/include/scm.h
+++ b/platform/msm_shared/include/scm.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2011-2016, The Linux Foundation. All rights reserved.
 
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -29,6 +29,8 @@
 #ifndef __SCM_H__
 #define __SCM_H__
 
+#include <reboot.h>
+
 /* ARM SCM format support related flags */
 #define SIP_SVC_CALLS                          0x02000000
 #define MAKE_SIP_SCM_CMD(svc_id, cmd_id)       ((((svc_id << 8) | (cmd_id)) & 0xFFFF) | SIP_SVC_CALLS)
@@ -461,9 +463,10 @@
 /* Is armv8 supported */
 bool is_scm_armv8_support();
 
-int scm_dload_mode(int mode);
+int scm_dload_mode(enum reboot_reason mode);
 int scm_device_enter_dload();
 int scm_call2_atomic(uint32_t svc, uint32_t cmd, uint32_t arg1, uint32_t arg2);
 uint32_t scm_io_write(uint32_t address, uint32_t val);
 int is_scm_call_available(uint32_t svc_id, uint32_t cmd_id);
+int scm_disable_sdi();
 #endif
diff --git a/platform/msm_shared/menu_keys_detect.c b/platform/msm_shared/menu_keys_detect.c
index 28766a7..8173133 100644
--- a/platform/msm_shared/menu_keys_detect.c
+++ b/platform/msm_shared/menu_keys_detect.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
@@ -42,14 +42,13 @@
 #include <platform/iomap.h>
 #include <platform.h>
 #include <reboot.h>
+#include <sys/types.h>
 #include <../../../app/aboot/recovery.h>
 #include <../../../app/aboot/devinfo.h>
 
 #define KEY_DETECT_FREQUENCY		50
-#define KEY_PRESS_TIMEOUT		5000
 
-static uint32_t wait_time = 0;
-static int old_device_type = -1;
+static time_t before_time;
 
 extern int target_volume_up();
 extern uint32_t target_volume_down();
@@ -57,8 +56,7 @@
 extern void shutdown_device();
 
 typedef uint32_t (*keys_detect_func)(void);
-typedef uint32_t (*keys_action_func)(struct select_msg_info* msg_info,
-	uint32_t option_index);
+typedef void (*keys_action_func)(struct select_msg_info* msg_info);
 
 struct keys_stru {
 	int type;
@@ -77,6 +75,26 @@
 	keys_action_func enter_action_func;
 };
 
+static uint32_t verify_index_action[] = {
+		[0] = POWEROFF,
+		[1] = RESTART,
+		[2] = RECOVER,
+		[3] = FASTBOOT,
+		[4] = BACK,
+};
+
+static uint32_t fastboot_index_action[] = {
+		[0] = RESTART,
+		[1] = FASTBOOT,
+		[2] = RECOVER,
+		[3] = POWEROFF,
+};
+
+static uint32_t unlock_index_action[] = {
+		[0] = RECOVER,
+		[1] = RESTART,
+};
+
 static int is_key_pressed(int keys_type)
 {
 	int count = 0;
@@ -91,276 +109,237 @@
 	return 0;
 }
 
-static void update_device_status(unsigned reason, int type)
+static void update_device_status(struct select_msg_info* msg_info, int reason)
 {
-	if (reason == RECOVER) {
-		if (type == DISPLAY_MENU_UNLOCK) {
-			set_device_unlock_value(UNLOCK, TRUE);
-		} else if (type == DISPLAY_MENU_UNLOCK_CRITICAL) {
-			set_device_unlock_value(UNLOCK_CRITICAL, TRUE);
-		}
+	fbcon_clear();
+	switch (reason) {
+		case RECOVER:
+			if (msg_info->info.msg_type == DISPLAY_MENU_UNLOCK) {
+				set_device_unlock_value(UNLOCK, TRUE);
+			} else if (msg_info->info.msg_type == DISPLAY_MENU_UNLOCK_CRITICAL) {
+				set_device_unlock_value(UNLOCK_CRITICAL, TRUE);
+			}
 
-		if (type == DISPLAY_MENU_UNLOCK ||
-			type == DISPLAY_MENU_UNLOCK_CRITICAL) {
-			/* wipe data */
-			struct recovery_message msg;
+			if (msg_info->info.msg_type == DISPLAY_MENU_UNLOCK ||
+				msg_info->info.msg_type == DISPLAY_MENU_UNLOCK_CRITICAL) {
+				/* wipe data */
+				struct recovery_message msg;
 
-			snprintf(msg.recovery, sizeof(msg.recovery), "recovery\n--wipe_data");
-			write_misc(0, &msg, sizeof(msg));
-		}
+				snprintf(msg.recovery, sizeof(msg.recovery), "recovery\n--wipe_data");
+				write_misc(0, &msg, sizeof(msg));
+			}
+			reboot_device(RECOVERY_MODE);
+			break;
+		case RESTART:
+			reboot_device(0);
+			break;
+		case POWEROFF:
+			shutdown_device();
+			break;
+		case FASTBOOT:
+			reboot_device(FASTBOOT_MODE);
+			break;
+		case CONTINUE:
+			display_image_on_screen();
 
-		reboot_device(RECOVERY_MODE);
-	} else if (reason == RESTART) {
-		reboot_device(0);
-	} else if (reason == POWEROFF) {
-		shutdown_device();
-	} else if (reason == FASTBOOT) {
-		reboot_device(FASTBOOT_MODE);
-	} else if (reason == CONTINUE) {
-		fbcon_clear();
-		display_image_on_screen();
+			/* Continue boot, no need to detect the keys'status */
+			msg_info->info.is_timeout = true;
+			break;
+		case BACK:
+			display_bootverify_menu_renew(msg_info, msg_info->last_msg_type);
+			before_time = current_time();
+
+			break;
 	}
 }
 
-static void update_volume_up_bg(struct select_msg_info* msg_info, uint32_t option_index)
+/* msg_lock need to be holded when call this function. */
+static void update_volume_up_bg(struct select_msg_info* msg_info)
 {
-	if (option_index == msg_info->option_num - 1) {
-		fbcon_draw_msg_background(msg_info->option_start[0],
-			msg_info->option_end[0],
-			msg_info->option_bg[0], 0);
+	if (msg_info->info.option_index == msg_info->info.option_num - 1) {
+		fbcon_draw_msg_background(msg_info->info.option_start[0],
+			msg_info->info.option_end[0],
+			msg_info->info.option_bg[0], 0);
 
-		fbcon_draw_msg_background(msg_info->option_start[msg_info->option_num - 1],
-			msg_info->option_end[msg_info->option_num - 1],
-			msg_info->option_bg[msg_info->option_num - 1], 1);
+		fbcon_draw_msg_background(msg_info->info.option_start[msg_info->info.option_num - 1],
+			msg_info->info.option_end[msg_info->info.option_num - 1],
+			msg_info->info.option_bg[msg_info->info.option_num - 1], 1);
 	} else {
-		fbcon_draw_msg_background(msg_info->option_start[option_index],
-			msg_info->option_end[option_index],
-			msg_info->option_bg[option_index], 1);
+		fbcon_draw_msg_background(msg_info->info.option_start[msg_info->info.option_index],
+			msg_info->info.option_end[msg_info->info.option_index],
+			msg_info->info.option_bg[msg_info->info.option_index], 1);
 
-		fbcon_draw_msg_background(msg_info->option_start[option_index + 1],
-			msg_info->option_end[option_index + 1],
-			msg_info->option_bg[option_index + 1], 0);
+		fbcon_draw_msg_background(msg_info->info.option_start[msg_info->info.option_index + 1],
+			msg_info->info.option_end[msg_info->info.option_index + 1],
+			msg_info->info.option_bg[msg_info->info.option_index + 1], 0);
 	}
 }
 
-static void update_volume_down_bg(struct select_msg_info* msg_info, uint32_t option_index)
+/* msg_lock need to be holded when call this function. */
+static void update_volume_down_bg(struct select_msg_info* msg_info)
 {
-	if (option_index == 0) {
-		fbcon_draw_msg_background(msg_info->option_start[0],
-			msg_info->option_end[0],
-			msg_info->option_bg[0], 1);
+	if (msg_info->info.option_index == 0) {
+		fbcon_draw_msg_background(msg_info->info.option_start[0],
+			msg_info->info.option_end[0],
+			msg_info->info.option_bg[0], 1);
 
-		fbcon_draw_msg_background(msg_info->option_start[msg_info->option_num - 1],
-			msg_info->option_end[msg_info->option_num - 1],
-			msg_info->option_bg[msg_info->option_num - 1], 0);
+		fbcon_draw_msg_background(msg_info->info.option_start[msg_info->info.option_num - 1],
+			msg_info->info.option_end[msg_info->info.option_num - 1],
+			msg_info->info.option_bg[msg_info->info.option_num - 1], 0);
 	} else {
-		fbcon_draw_msg_background(msg_info->option_start[option_index],
-			msg_info->option_end[option_index],
-			msg_info->option_bg[option_index], 1);
+		fbcon_draw_msg_background(msg_info->info.option_start[msg_info->info.option_index],
+			msg_info->info.option_end[msg_info->info.option_index],
+			msg_info->info.option_bg[msg_info->info.option_index], 1);
 
-		fbcon_draw_msg_background(msg_info->option_start[option_index - 1],
-			msg_info->option_end[option_index - 1],
-			msg_info->option_bg[option_index - 1], 0);
+		fbcon_draw_msg_background(msg_info->info.option_start[msg_info->info.option_index - 1],
+			msg_info->info.option_end[msg_info->info.option_index - 1],
+			msg_info->info.option_bg[msg_info->info.option_index - 1], 0);
 	}
 }
 
 /* update select option's background when volume up key is pressed */
-static uint32_t menu_volume_up_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
+static void menu_volume_up_func (struct select_msg_info* msg_info)
 {
-	if (option_index == msg_info->option_num ||
-		option_index == 0) {
-		option_index = msg_info->option_num - 1;
-	} else if (option_index > 0) {
-		option_index--;
+	if (msg_info->info.option_index == 0)
+		msg_info->info.option_index = msg_info->info.option_num - 1;
+	else
+		msg_info->info.option_index--;
+
+	if (msg_info->info.msg_type == DISPLAY_MENU_FASTBOOT) {
+		display_fastboot_menu_renew(msg_info);
+	} else {
+		update_volume_up_bg(msg_info);
 	}
-
-	update_volume_up_bg(msg_info, option_index);
-
-	return option_index;
 }
 
 /* update select option's background when volume down key is pressed */
-static uint32_t menu_volume_down_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
+static void menu_volume_down_func (struct select_msg_info* msg_info)
 {
-	option_index++;
-	if (option_index >= msg_info->option_num)
-		option_index = 0;
+	msg_info->info.option_index++;
+	if (msg_info->info.option_index >= msg_info->info.option_num)
+		msg_info->info.option_index = 0;
 
-	update_volume_down_bg(msg_info, option_index);
-
-	return option_index;
-}
-
-/* enter to boot verify page2 if volume key is pressed */
-static uint32_t boot_page1_volume_keys_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
-{
-	keys_detect_init();
-	old_device_type = msg_info->msg_type;
-	display_boot_verified_option(msg_info);
-	msg_info->msg_volume_key_pressed = true;
-	option_index = msg_info->option_num;
-
-	return option_index;
-}
-
-/* update device's status via select option */
-static uint32_t unlock_power_key_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
-{
-	int device_state = -1;
-	if (option_index == 0)
-		device_state = RECOVER;
-	else if (option_index == 1)
-		device_state = RESTART;
-
-	update_device_status(device_state, msg_info->msg_type);
-	return 0;
-}
-
-/* continue booting when power key is pressed at boot-verify page1 */
-static uint32_t boot_page1_power_key_func (struct select_msg_info* msg_info,
-	uint32_t option_index){
-	msg_info->msg_power_key_pressed = true;
-	update_device_status(CONTINUE, msg_info->msg_type);
-	return option_index;
-}
-
-/* update device's status via select option */
-static uint32_t boot_page2_power_key_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
-{
-	if (option_index == BACK) {
-		wait_time = 0;
-		msg_info->msg_timeout = false;
-		option_index = msg_info->option_num;
-		display_boot_verified_menu(msg_info,
-			old_device_type);
+	if (msg_info->info.msg_type == DISPLAY_MENU_FASTBOOT) {
+		display_fastboot_menu_renew(msg_info);
 	} else {
-		msg_info->msg_power_key_pressed = true;
-		update_device_status(option_index, msg_info->msg_type);
+		update_volume_down_bg(msg_info);
 	}
-	return option_index;
 }
 
-static uint32_t fastboot_volume_up_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
+/* enter to boot verification option page if volume key is pressed */
+static void boot_warning_volume_keys_func (struct select_msg_info* msg_info)
 {
-	if (option_index == msg_info->option_num ||
-		option_index == 0) {
-		option_index = msg_info->option_num - 1;
-	} else if (option_index > 0) {
-		option_index--;
-	}
-
-	display_fastboot_menu(msg_info, option_index);
-
-	return option_index;
-}
-
-static uint32_t fastboot_volume_down_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
-{
-	option_index++;
-	if (option_index > msg_info->option_num)
-		option_index = 1;
-	if (option_index == msg_info->option_num)
-		option_index = 0;
-
-	display_fastboot_menu(msg_info, option_index);
-
-	return option_index;
+	msg_info->last_msg_type = msg_info->info.msg_type;
+	display_bootverify_option_menu_renew(msg_info);
 }
 
 /* update device's status via select option */
-static uint32_t fastboot_power_key_func (struct select_msg_info* msg_info,
-	uint32_t option_index)
+static void power_key_func(struct select_msg_info* msg_info)
 {
-	int device_state[] = {RESTART, FASTBOOT, RECOVER, POWEROFF};
+	int reason = -1;
 
-	if(option_index < sizeof(device_state)) {
-		update_device_status(device_state[option_index], msg_info->msg_type);
-	} else {
-		dprintf(CRITICAL, "ERRPR: option index is overflow!!!\n");
-		return 1;
+	switch (msg_info->info.msg_type) {
+		case DISPLAY_MENU_YELLOW:
+		case DISPLAY_MENU_ORANGE:
+		case DISPLAY_MENU_RED:
+			reason = CONTINUE;
+			break;
+		case DISPLAY_MENU_MORE_OPTION:
+			if(msg_info->info.option_index < ARRAY_SIZE(verify_index_action))
+				reason = verify_index_action[msg_info->info.option_index];
+			break;
+		case DISPLAY_MENU_UNLOCK:
+		case DISPLAY_MENU_UNLOCK_CRITICAL:
+			if(msg_info->info.option_index < ARRAY_SIZE(unlock_index_action))
+				reason = unlock_index_action[msg_info->info.option_index];
+			break;
+		case DISPLAY_MENU_FASTBOOT:
+			if(msg_info->info.option_index < ARRAY_SIZE(fastboot_index_action))
+				reason = fastboot_index_action[msg_info->info.option_index];
+			break;
+		default:
+			dprintf(CRITICAL,"Unsupported menu type\n");
+			break;
 	}
 
-	return 0;
+	if (reason != -1) {
+		update_device_status(msg_info, reason);
+	}
 }
 
-/* initialize different page's function
- * UNLOCK_PAGE/BOOT_VERIFY_PAGE2:
+/* Initialize different page's function
+ * DISPLAY_MENU_UNLOCK/DISPLAY_MENU_UNLOCK_CRITICAL
+ * DISPLAY_MENU_MORE_OPTION/DISPLAY_MENU_FASTBOOT:
  *	up_action_func: update select option's background when volume up
  *	is pressed
  *	down_action_func: update select option's background when volume up
  *	is pressed
  *	enter_action_func: update device's status via select option
- * BOOT_VERIFY_PAGE1:
+ * DISPLAY_MENU_YELLOW/DISPLAY_MENU_ORANGE/DISPLAY_MENU_RED:
  *	up_action_func/down_action_func: enter BOOT_VERIFY_PAGE2 when volume
  *	key is pressed
  *	enter_action_func: continue booting
  */
 static struct pages_action menu_pages_action[] = {
-	[UNLOCK_PAGE] = {
+	[DISPLAY_MENU_UNLOCK] = {
 		menu_volume_up_func,
 		menu_volume_down_func,
-		unlock_power_key_func,
+		power_key_func,
 	},
-	[BOOT_VERIFY_PAGE1] = {
-		boot_page1_volume_keys_func,
-		boot_page1_volume_keys_func,
-		boot_page1_power_key_func,
-	},
-	[BOOT_VERIFY_PAGE2] = {
+	[DISPLAY_MENU_UNLOCK_CRITICAL] = {
 		menu_volume_up_func,
 		menu_volume_down_func,
-		boot_page2_power_key_func,
+		power_key_func,
 	},
-	[FASTBOOT_PAGE] = {
-		fastboot_volume_up_func,
-		fastboot_volume_down_func,
-		fastboot_power_key_func,
+	[DISPLAY_MENU_YELLOW] = {
+		boot_warning_volume_keys_func,
+		boot_warning_volume_keys_func,
+		power_key_func,
+	},
+	[DISPLAY_MENU_ORANGE] = {
+		boot_warning_volume_keys_func,
+		boot_warning_volume_keys_func,
+		power_key_func,
+	},
+	[DISPLAY_MENU_RED] = {
+		boot_warning_volume_keys_func,
+		boot_warning_volume_keys_func,
+		power_key_func,
+	},
+	[DISPLAY_MENU_MORE_OPTION] = {
+		menu_volume_up_func,
+		menu_volume_down_func,
+		power_key_func,
+	},
+	[DISPLAY_MENU_FASTBOOT] = {
+		menu_volume_up_func,
+		menu_volume_down_func,
+		power_key_func,
 	},
 
 };
 
 void keys_detect_init()
 {
-	wait_time = 0;
+	/* Waiting for all keys are released */
+	while(1) {
+		if(!keys[VOLUME_UP].keys_pressed_func() &&
+			!keys[VOLUME_DOWN].keys_pressed_func() &&
+			!keys[POWER_KEY].keys_pressed_func()) {
+			break;
+		}
+		thread_sleep(KEY_DETECT_FREQUENCY);
+	}
+
+	before_time = current_time();
 }
 
 int select_msg_keys_detect(void *param) {
 	struct select_msg_info *msg_info = (struct select_msg_info*)param;
-	uint32_t current_page_index;
-	uint32_t option_index = msg_info->option_num;
 
+	msg_lock_init();
 	keys_detect_init();
 	while(1) {
-		/* get page's index via different message type */
-		switch(msg_info->msg_type) {
-		case DISPLAY_MENU_UNLOCK:
-		case DISPLAY_MENU_UNLOCK_CRITICAL:
-			current_page_index = UNLOCK_PAGE;
-			break;
-		case DISPLAY_MENU_MORE_OPTION:
-			current_page_index = BOOT_VERIFY_PAGE2;
-			break;
-		case DISPLAY_MENU_FASTBOOT:
-			current_page_index = FASTBOOT_PAGE;
-			break;
-		default:
-			current_page_index = BOOT_VERIFY_PAGE1;
-			break;
-		}
-
-		/* device will continue booting when user has no action
-		 * on BOOT_VERIFY_PAGE1
-		 */
-		if (wait_time > KEY_PRESS_TIMEOUT)
-			msg_info->msg_timeout = true;
-
 		/* 1: update select option's index, default it is the total option number
 		 *  volume up: index decrease, the option will scroll up from
 		 * 	the bottom to top if the key is pressed firstly.
@@ -371,17 +350,34 @@
 		 * 2: update device's status via select option's index
 		 */
 		if (is_key_pressed(VOLUME_UP)) {
-			option_index =
-				menu_pages_action[current_page_index].up_action_func(msg_info, option_index);
+			mutex_acquire(&msg_info->msg_lock);
+			menu_pages_action[msg_info->info.msg_type].up_action_func(msg_info);
+			mutex_release(&msg_info->msg_lock);
 		} else if (is_key_pressed(VOLUME_DOWN)) {
-			option_index =
-				menu_pages_action[current_page_index].down_action_func(msg_info, option_index);
+			mutex_acquire(&msg_info->msg_lock);
+			menu_pages_action[msg_info->info.msg_type].down_action_func(msg_info);
+			mutex_release(&msg_info->msg_lock);
 		} else if (is_key_pressed(POWER_KEY)) {
-			option_index =
-				menu_pages_action[current_page_index].enter_action_func(msg_info, option_index);
+			mutex_acquire(&msg_info->msg_lock);
+			menu_pages_action[msg_info->info.msg_type].enter_action_func(msg_info);
+			mutex_release(&msg_info->msg_lock);
 		}
 
-		wait_time += KEY_DETECT_FREQUENCY;
+		/* Never time out if the timeout_time is 0 */
+		mutex_acquire(&msg_info->msg_lock);
+		if(msg_info->info.timeout_time) {
+			if (msg_info->info.is_timeout) {
+				mutex_release(&msg_info->msg_lock);
+				return 0;
+			}
+
+			if ((current_time() - before_time) > msg_info->info.timeout_time) {
+				msg_info->info.is_timeout = true;
+				mutex_release(&msg_info->msg_lock);
+				return 0;
+			}
+		}
+		mutex_release(&msg_info->msg_lock);
 		thread_sleep(KEY_DETECT_FREQUENCY);
 	}
 
diff --git a/platform/msm_shared/reboot.c b/platform/msm_shared/reboot.c
index 4c21d11..f3441cd 100644
--- a/platform/msm_shared/reboot.c
+++ b/platform/msm_shared/reboot.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are
@@ -76,7 +76,7 @@
 	hard_restart_reason = REG_READ(PON_SOFT_RB_SPARE);
 	REG_WRITE(PON_SOFT_RB_SPARE, hard_restart_reason & 0x03);
 
-	/* Extract the bits 5 to 7 and return */
+	/* Extract the bits 2 to 7 and return */
 	return (hard_restart_reason & 0xFC) >> 2;
 }
 
@@ -103,11 +103,11 @@
 	uint8_t value;
 #endif
 
-	/* Need to clear the SW_RESET_ENTRY register and
-	 * write to the BOOT_MISC_REG for known reset cases
-	 */
-	if(reboot_reason != DLOAD)
-		scm_dload_mode(NORMAL_MODE);
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
 
 #if USE_PON_REBOOT_REG
 	value = REG_READ(PON_SOFT_RB_SPARE);
@@ -120,13 +120,19 @@
 	 * For other cases do a hard reset
 	 */
 #if USE_PON_REBOOT_REG
-	if(reboot_reason == DLOAD)
+	if(reboot_reason == NORMAL_DLOAD || reboot_reason == EMERGENCY_DLOAD) {
 #else
-	if(reboot_reason == FASTBOOT_MODE || (reboot_reason == DLOAD) || (reboot_reason == RECOVERY_MODE))
+	if(reboot_reason == FASTBOOT_MODE || reboot_reason == NORMAL_DLOAD ||
+		reboot_reason == EMERGENCY_DLOAD || reboot_reason == RECOVERY_MODE) {
 #endif
 		reset_type = PON_PSHOLD_WARM_RESET;
-	else
+#if DISABLE_DLOAD_MODE
+		if (reboot_reason == NORMAL_DLOAD)
+			reset_type = PON_PSHOLD_HARD_RESET;
+#endif
+	} else {
 		reset_type = PON_PSHOLD_HARD_RESET;
+	}
 
 	pmic_reset_configure(reset_type);
 
diff --git a/platform/msm_shared/reboot.h b/platform/msm_shared/reboot.h
index 4773255..58085df 100644
--- a/platform/msm_shared/reboot.h
+++ b/platform/msm_shared/reboot.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -28,21 +28,30 @@
  */
 #ifndef __REBOOT_H__
 #define __REBOOT_H__
+
+enum reboot_reason {
 #if USE_PON_REBOOT_REG
-#define RECOVERY_MODE     0x01
-#define FASTBOOT_MODE     0x02
-#define ALARM_BOOT        0x03
-#define DM_VERITY_LOGGING   0x04
-#define DM_VERITY_ENFORCING 0x05
-#define DM_VERITY_KEYSCLEAR 0x06
+	/* hard reset reason */
+	REBOOT_MODE_UNKNOWN	= 0x00,
+	RECOVERY_MODE		= 0x01,
+	FASTBOOT_MODE		= 0x02,
+	ALARM_BOOT		= 0x03,
+	DM_VERITY_LOGGING	= 0x04,
+	DM_VERITY_ENFORCING	= 0x05,
+	DM_VERITY_KEYSCLEAR	= 0x06,
 #else
-#define FASTBOOT_MODE     0x77665500
-#define RECOVERY_MODE     0x77665502
-#define ALARM_BOOT        0x77665503
-#define DM_VERITY_LOGGING    0x77665508
-#define DM_VERITY_ENFORCING  0x77665509
-#define DM_VERITY_KEYSCLEAR  0x7766550A
+	REBOOT_MODE_UNKNOWN	= 0x00,
+	RECOVERY_MODE		= 0x77665502,
+	FASTBOOT_MODE		= 0x77665500,
+	ALARM_BOOT		= 0x77665503,
+	DM_VERITY_LOGGING	= 0x77665508,
+	DM_VERITY_ENFORCING	= 0x77665509,
+	DM_VERITY_KEYSCLEAR	= 0x7766550A,
 #endif
+	/* warm reset start from  0xF0000000 */
+	NORMAL_DLOAD		= 0xF0000001,
+	EMERGENCY_DLOAD,
+};
 
 #define RTC_TRG           4
 #define PON_SOFT_RB_SPARE 0x88F
diff --git a/platform/msm_shared/scm.c b/platform/msm_shared/scm.c
index f9a7377..05b5b6c 100644
--- a/platform/msm_shared/scm.c
+++ b/platform/msm_shared/scm.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2011-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -65,7 +65,6 @@
 	if (!scm_initialized)
 	{
 		scm_init();
-		scm_initialized = true;
 	}
 #endif
 
@@ -113,6 +112,12 @@
 
 	if (ret < 0)
 		dprintf(CRITICAL, "Failed to initialize SCM\n");
+
+	scm_initialized = true;
+
+#if DISABLE_DLOAD_MODE
+	scm_disable_sdi();
+#endif
 }
 
 /**
@@ -1332,16 +1337,34 @@
 	return ret;
 }
 
+int scm_disable_sdi()
+{
+	int ret = 0;
+
+	scm_check_boot_fuses();
+
+	/* Make WDOG_DEBUG DISABLE scm call only in non-secure boot */
+	if(!(secure_boot_enabled || wdog_debug_fuse_disabled)) {
+		ret = scm_call2_atomic(SCM_SVC_BOOT, WDOG_DEBUG_DISABLE, 1, 0);
+		if(ret)
+			dprintf(CRITICAL, "Failed to disable secure wdog debug: %d\n", ret);
+	}
+	return ret;
+}
+
 #if PLATFORM_USE_SCM_DLOAD
-int scm_dload_mode(int mode)
+int scm_dload_mode(enum reboot_reason mode)
 {
 	int ret = 0;
 	uint32_t dload_type;
 
 	dprintf(SPEW, "DLOAD mode: %d\n", mode);
-	if (mode == NORMAL_DLOAD)
+	if (mode == NORMAL_DLOAD) {
 		dload_type = SCM_DLOAD_MODE;
-	else if(mode == EMERGENCY_DLOAD)
+#if DISABLE_DLOAD_MODE
+		return 0;
+#endif
+	} else if(mode == EMERGENCY_DLOAD)
 		dload_type = SCM_EDLOAD_MODE;
 	else
 		dload_type = 0;
@@ -1359,16 +1382,11 @@
 		return ret;
 	}
 
-	scm_check_boot_fuses();
-
-	/* Make WDOG_DEBUG DISABLE scm call only in non-secure boot */
-	if(!(secure_boot_enabled || wdog_debug_fuse_disabled)) {
-		ret = scm_call2_atomic(SCM_SVC_BOOT, WDOG_DEBUG_DISABLE, 1, 0);
-		if(ret)
-			dprintf(CRITICAL, "Failed to disable the wdog debug \n");
-	}
-
+#if !DISABLE_DLOAD_MODE
+	return scm_disable_sdi();
+#else
 	return ret;
+#endif
 }
 
 bool scm_device_enter_dload()
diff --git a/platform/msm_shared/wdog.c b/platform/msm_shared/wdog.c
index d1f4033..a6af333 100644
--- a/platform/msm_shared/wdog.c
+++ b/platform/msm_shared/wdog.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
 
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -35,11 +35,37 @@
 #include <target.h>
 #include <scm.h>
 #include <dload_util.h>
+#include <kernel/thread.h>
+
+#define WDOG_FEED_FREQUENCY 1000
+
+static int wdog_feed_handler(void *param) {
+	while(1) {
+		writel(1, APPS_WDOG_RESET_REG);
+		thread_sleep(WDOG_FEED_FREQUENCY);
+	}
+	return 0;
+}
+
+static void wdog_feed_func_thread(void)
+{
+	static bool is_thread_start;
+	thread_t *thr;
+
+	if (!is_thread_start) {
+		thr = thread_create("wdogfeed", wdog_feed_handler,
+			0, DEFAULT_PRIORITY, DEFAULT_STACK_SIZE);
+		if (!thr) {
+			dprintf(CRITICAL, "ERROR: create feed dog thread failed!!\n");
+			return;
+		}
+		thread_resume(thr);
+		is_thread_start = true;
+	}
+}
 
 void msm_wdog_init()
 {
-	uint32_t ret;
-
 	/* Set Bite and Bark times  10s */
 	writel(0x77FD3, APPS_WDOG_BARK_VAL_REG);
 	writel(0x77FD3, APPS_WDOG_BITE_VAL_REG);
@@ -50,13 +76,7 @@
 	/* Enable WDOG */
 	writel((readl(APPS_WDOG_CTL_REG) | 0x1), APPS_WDOG_CTL_REG);
 
-	/* Write to the Boot MISC register to put the device in dload mode*/
-	ret = scm_call2_atomic(SCM_SVC_BOOT, SCM_DLOAD_CMD, SCM_DLOAD_MODE, 0);
+	wdog_feed_func_thread();
 
-	if (ret) {
-		ret = scm_io_write(TCSR_BOOT_MISC_DETECT,SCM_DLOAD_MODE);
-		if(ret) {
-			dprintf(CRITICAL, "Failed to write to boot misc: %d\n", ret);
-		}
-	}
+	set_download_mode(NORMAL_DLOAD);
 }
diff --git a/platform/msmtitanium/include/platform/iomap.h b/platform/msmtitanium/include/platform/iomap.h
old mode 100755
new mode 100644
index 64779cb..d99c20b
--- a/platform/msmtitanium/include/platform/iomap.h
+++ b/platform/msmtitanium/include/platform/iomap.h
@@ -76,6 +76,12 @@
 #define MPM2_MPM_PS_HOLD                   0x004AB000
 #define MPM2_MPM_SLEEP_TIMETICK_COUNT_VAL  0x004A3000
 
+#define PMI_SLAVE_BASE           2
+#define PMI_FIRST_SLAVE_OFFSET   0
+#define PMI_SECOND_SLAVE_OFFSET  1
+#define PMI_FIRST_SLAVE_ADDR_BASE   (( PMI_SLAVE_BASE + PMI_FIRST_SLAVE_OFFSET ) << 16)
+#define PMI_SECOND_SLAVE_ADDR_BASE  (( PMI_SLAVE_BASE + PMI_SECOND_SLAVE_OFFSET) << 16)
+
 /* CRYPTO ENGINE */
 #define  MSM_CE1_BASE                      0x073A000
 #define  MSM_CE1_BAM_BASE                  0x0704000
@@ -211,7 +217,7 @@
 #define TCSR_TZ_WONCE               0x193D000
 #define TCSR_BOOT_MISC_DETECT       0x193D100
 
-#define DDR_START                          0x80000000
+#define DDR_START                          get_ddr_start()
 #define ABOOT_FORCE_KERNEL_ADDR            DDR_START + 0x8000
 #define ABOOT_FORCE_KERNEL64_ADDR          DDR_START + 0x80000
 #define ABOOT_FORCE_RAMDISK_ADDR           DDR_START + 0x2000000
diff --git a/platform/msmtitanium/platform.c b/platform/msmtitanium/platform.c
old mode 100755
new mode 100644
index 83d0b3b..32f51f5
--- a/platform/msmtitanium/platform.c
+++ b/platform/msmtitanium/platform.c
@@ -110,7 +110,7 @@
 	uint32_t i;
 	uint32_t sections;
 	uint32_t table_size = ARRAY_SIZE(mmu_section_table);
-	uint32_t ddr_start = get_ddr_start();
+	uint32_t ddr_start = DDR_START;
 	uint32_t smem_addr = platform_get_smem_base_addr();
 
 	/*Mapping the ddr start address for loading the kernel about 90 MB*/
diff --git a/project/msm8952.mk b/project/msm8952.mk
index f0896ec..41f6e92 100644
--- a/project/msm8952.mk
+++ b/project/msm8952.mk
@@ -90,6 +90,9 @@
 DEFINES += MDTP_SUPPORT=1
 endif
 
+#Disable the dload mode
+DEFINES += DISABLE_DLOAD_MODE=0
+
 ENABLE_WDOG_SUPPORT := 0
 ifeq ($(ENABLE_WDOG_SUPPORT),1)
 DEFINES += WDOG_SUPPORT=1
diff --git a/project/msmtitanium.mk b/project/msmtitanium.mk
index 6cc2ada..75a9f7e 100755
--- a/project/msmtitanium.mk
+++ b/project/msmtitanium.mk
@@ -31,7 +31,7 @@
 DEFINES += BAM_V170=1
 
 #Enable the feature of long press power on
-#DEFINES += LONG_PRESS_POWER_ON=1
+DEFINES += LONG_PRESS_POWER_ON=1
 
 #Disable thumb mode
 ENABLE_THUMB := false
@@ -44,12 +44,16 @@
 endif
 
 #enable power on vibrator feature
-#ENABLE_PON_VIB_SUPPORT := true
+ENABLE_HAP_VIB_SUPPORT := true
 
 ifeq ($(EMMC_BOOT),1)
 DEFINES += _EMMC_BOOT=1
 endif
 
+ifeq ($(ENABLE_HAP_VIB_SUPPORT),true)
+DEFINES += PON_VIB_SUPPORT=1
+endif
+
 ifeq ($(ENABLE_PON_VIB_SUPPORT),true)
 DEFINES += PON_VIB_SUPPORT=1
 endif
diff --git a/target/fsm9010/init.c b/target/fsm9010/init.c
index 76382aa..35c19c8 100644
--- a/target/fsm9010/init.c
+++ b/target/fsm9010/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2013-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -328,8 +328,23 @@
 	return restart_reason;
 }
 
+int set_download_mode(enum reboot_reason mode)
+{
+	if (mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD)
+		dload_util_write_cookie(mode == NORMAL_DLOAD ?
+			DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
+
+	return 0;
+}
+
 void reboot_device(unsigned reboot_reason)
 {
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
+
 	/* Write the reboot reason */
 	writel(reboot_reason, RESTART_REASON_ADDR_V2);
 
@@ -352,14 +367,6 @@
 	dprintf(CRITICAL, "Rebooting failed\n");
 }
 
-int set_download_mode(enum dload_mode mode)
-{
-	dload_util_write_cookie(mode == NORMAL_DLOAD ?
-		DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
-
-	return 0;
-}
-
 /* Returns 1 if target supports continuous splash screen. */
 int target_cont_splash_screen()
 {
diff --git a/target/fsm9900/init.c b/target/fsm9900/init.c
index cc06e0e..4c2b319 100644
--- a/target/fsm9900/init.c
+++ b/target/fsm9900/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2013-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -379,8 +379,23 @@
 	return restart_reason;
 }
 
+int set_download_mode(enum reboot_reason mode)
+{
+	if (mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD)
+		dload_util_write_cookie(mode == NORMAL_DLOAD ?
+			DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
+
+	return 0;
+}
+
 void reboot_device(unsigned reboot_reason)
 {
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
+
 	/* Write the reboot reason */
 	writel(reboot_reason, RESTART_REASON_ADDR_V2);
 
@@ -403,14 +418,6 @@
 	dprintf(CRITICAL, "Rebooting failed\n");
 }
 
-int set_download_mode(enum dload_mode mode)
-{
-	dload_util_write_cookie(mode == NORMAL_DLOAD ?
-		DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
-
-	return 0;
-}
-
 /* Check if MSM needs VBUS mimic for USB */
 static int target_needs_vbus_mimic()
 {
diff --git a/target/init.c b/target/init.c
index b8a818b..7f8a432 100644
--- a/target/init.c
+++ b/target/init.c
@@ -1,7 +1,7 @@
 /*
  * Copyright (c) 2008 Travis Geiselbrecht
  *
- * Copyright (c) 2015, The Linux Foundation. All rights reserved.
+ * Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Permission is hereby granted, free of charge, to any person obtaining
  * a copy of this software and associated documentation files
@@ -37,6 +37,7 @@
 #include <smem.h>
 #include <pm8x41_adc.h>
 #include <pm8x41_hw.h>
+#include <scm.h>
 
 #if CHECK_BAT_VOLTAGE
 #include <pm_fg_adc_usr.h>
@@ -110,9 +111,17 @@
 	return 0;
 }
 
-__WEAK int set_download_mode(enum dload_mode mode)
+__WEAK int set_download_mode(enum reboot_reason mode)
 {
-	return -1;
+	if(mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD) {
+#if PLATFORM_USE_SCM_DLOAD
+		return scm_dload_mode(mode);
+#else
+		return -1;
+#endif
+	}
+
+	return 0;
 }
 
 __WEAK unsigned target_pause_for_battery_charge(void)
diff --git a/target/msm8226/init.c b/target/msm8226/init.c
index 81ee581..31a75b4 100644
--- a/target/msm8226/init.c
+++ b/target/msm8226/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2014, 2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -381,10 +381,28 @@
 	return restart_reason;
 }
 
+int set_download_mode(enum reboot_reason mode)
+{
+	if (mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD) {
+		dload_util_write_cookie(mode == NORMAL_DLOAD ?
+			DLOAD_MODE_ADDR : EMERGENCY_DLOAD_MODE_ADDR, mode);
+
+		pm8x41_clear_pmic_watchdog();
+	}
+
+	return 0;
+}
+
 void reboot_device(unsigned reboot_reason)
 {
 	int ret = 0;
 
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
+
 	writel(reboot_reason, RESTART_REASON_ADDR);
 
 	/* Configure PMIC for warm reset */
@@ -554,16 +572,6 @@
 	return _emmc_recovery_init();
 }
 
-int set_download_mode(enum dload_mode mode)
-{
-	dload_util_write_cookie(mode == NORMAL_DLOAD ?
-		DLOAD_MODE_ADDR : EMERGENCY_DLOAD_MODE_ADDR, mode);
-
-	pm8x41_clear_pmic_watchdog();
-
-	return 0;
-}
-
 static void set_sdc_power_ctrl()
 {
 	/* Drive strength configs for sdc pins */
diff --git a/target/msm8610/init.c b/target/msm8610/init.c
index 46f5f7e..0816e17 100644
--- a/target/msm8610/init.c
+++ b/target/msm8610/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013-2014, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2013-2014, 2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -51,6 +51,7 @@
 #include <platform/clock.h>
 #include <platform/timer.h>
 #include <crypto5_wrapper.h>
+#include <scm.h>
 
 #if LONG_PRESS_POWER_ON
 #include <shutdown_detect.h>
@@ -358,10 +359,28 @@
 	return restart_reason;
 }
 
+int set_download_mode(enum reboot_reason mode)
+{
+	if (mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD) {
+		dload_util_write_cookie(mode == NORMAL_DLOAD ?
+			DLOAD_MODE_ADDR : EMERGENCY_DLOAD_MODE_ADDR, mode);
+
+		pm8x41_clear_pmic_watchdog();
+	}
+
+	return 0;
+}
+
 void reboot_device(unsigned reboot_reason)
 {
 	int ret = 0;
 
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
+
 	writel(reboot_reason, RESTART_REASON_ADDR);
 
 	/* Configure PMIC for warm reset */
@@ -529,16 +548,6 @@
 	crypto_init_params(&ce_params);
 }
 
-int set_download_mode(enum dload_mode mode)
-{
-	dload_util_write_cookie(mode == NORMAL_DLOAD ?
-		DLOAD_MODE_ADDR : EMERGENCY_DLOAD_MODE_ADDR, mode);
-
-	pm8x41_clear_pmic_watchdog();
-
-	return 0;
-}
-
 /* Configure PMIC and Drop PS_HOLD for shutdown */
 void shutdown_device()
 {
diff --git a/target/msm8909/init.c b/target/msm8909/init.c
index e439c04..67aadfd 100644
--- a/target/msm8909/init.c
+++ b/target/msm8909/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -612,7 +612,7 @@
 	}
 }
 
-int set_download_mode(enum dload_mode mode)
+int set_download_mode(enum reboot_reason mode)
 {
 	int ret = 0;
 	ret = scm_dload_mode(mode);
diff --git a/target/msm8916/init.c b/target/msm8916/init.c
index 693db04..c6a43b7 100644
--- a/target/msm8916/init.c
+++ b/target/msm8916/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -407,7 +407,7 @@
 	}
 }
 
-int set_download_mode(enum dload_mode mode)
+int set_download_mode(enum reboot_reason mode)
 {
 	int ret = 0;
 	ret = scm_dload_mode(mode);
diff --git a/target/msm8952/init.c b/target/msm8952/init.c
index 88ff454..1d577d6 100644
--- a/target/msm8952/init.c
+++ b/target/msm8952/init.c
@@ -424,7 +424,7 @@
 	return hard_restart_reason;
 }
 
-int set_download_mode(enum dload_mode mode)
+int set_download_mode(enum reboot_reason mode)
 {
 	int ret = 0;
 	ret = scm_dload_mode(mode);
@@ -444,18 +444,19 @@
 	uint8_t reset_type = 0;
 	uint32_t ret = 0;
 
-	/* Need to clear the SW_RESET_ENTRY register and
-	 * write to the BOOT_MISC_REG for known reset cases
-	 */
-	if(reboot_reason != DLOAD)
-		scm_dload_mode(NORMAL_MODE);
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
 
 	writel(reboot_reason, RESTART_REASON_ADDR);
 
 	/* For Reboot-bootloader and Dload cases do a warm reset
 	 * For Reboot cases do a hard reset
 	 */
-	if((reboot_reason == FASTBOOT_MODE) || (reboot_reason == DLOAD) || (reboot_reason == RECOVERY_MODE))
+	if((reboot_reason == FASTBOOT_MODE) || (reboot_reason == NORMAL_DLOAD) ||
+		(reboot_reason == EMERGENCY_DLOAD) || (reboot_reason == RECOVERY_MODE))
 		reset_type = PON_PSHOLD_WARM_RESET;
 	else
 		reset_type = PON_PSHOLD_HARD_RESET;
diff --git a/target/msm8952/target_display.c b/target/msm8952/target_display.c
index 7d3c0d6..fa9faa1 100644
--- a/target/msm8952/target_display.c
+++ b/target/msm8952/target_display.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -434,39 +434,53 @@
 	struct qpnp_wled_config_data config = {0};
 	struct labibb_desc *labibb;
 	int display_type = 0;
+	bool swire_control = 0;
+	bool wled_avdd_control = 0;
 
 	labibb = pinfo->labibb;
 
 	if (labibb)
 		display_type = labibb->amoled_panel;
 
+	if (display_type) {
+		swire_control = labibb->swire_control;
+		wled_avdd_control = true;
+	} else {
+		swire_control = false;
+		wled_avdd_control = false;
+	}
+
 	config.display_type = display_type;
 	config.lab_init_volt = 4600000;	/* fixed, see pmi register */
 	config.ibb_init_volt = 1400000;	/* fixed, see pmi register */
+	config.lab_ibb_swire_control = swire_control;
+	config.wled_avdd_control = wled_avdd_control;
 
-	if (labibb && labibb->force_config) {
-		config.lab_min_volt = labibb->lab_min_volt;
-		config.lab_max_volt = labibb->lab_max_volt;
-		config.ibb_min_volt = labibb->ibb_min_volt;
-		config.ibb_max_volt = labibb->ibb_max_volt;
-		config.pwr_up_delay = labibb->pwr_up_delay;
-		config.pwr_down_delay = labibb->pwr_down_delay;
-		config.ibb_discharge_en = labibb->ibb_discharge_en;
-	} else {
-		/* default */
-		config.pwr_up_delay = 3;
-		config.pwr_down_delay =  3;
-		config.ibb_discharge_en = 1;
-		if (display_type) {	/* amoled */
-			config.lab_min_volt = 4600000;
-			config.lab_max_volt = 4600000;
-			config.ibb_min_volt = 4000000;
-			config.ibb_max_volt = 4000000;
-		} else { /* lcd */
-			config.lab_min_volt = 5500000;
-			config.lab_max_volt = 5500000;
-			config.ibb_min_volt = 5500000;
-			config.ibb_max_volt = 5500000;
+	if(!swire_control) {
+		if (labibb && labibb->force_config) {
+			config.lab_min_volt = labibb->lab_min_volt;
+			config.lab_max_volt = labibb->lab_max_volt;
+			config.ibb_min_volt = labibb->ibb_min_volt;
+			config.ibb_max_volt = labibb->ibb_max_volt;
+			config.pwr_up_delay = labibb->pwr_up_delay;
+			config.pwr_down_delay = labibb->pwr_down_delay;
+			config.ibb_discharge_en = labibb->ibb_discharge_en;
+		} else {
+			/* default */
+			config.pwr_up_delay = 3;
+			config.pwr_down_delay =  3;
+			config.ibb_discharge_en = 1;
+			if (display_type) {	/* amoled */
+				config.lab_min_volt = 4600000;
+				config.lab_max_volt = 4600000;
+				config.ibb_min_volt = 4000000;
+				config.ibb_max_volt = 4000000;
+			} else { /* lcd */
+				config.lab_min_volt = 5500000;
+				config.lab_max_volt = 5500000;
+				config.ibb_min_volt = 5500000;
+				config.ibb_max_volt = 5500000;
+			}
 		}
 	}
 
diff --git a/target/msm8974/init.c b/target/msm8974/init.c
index 266e091..73043df 100644
--- a/target/msm8974/init.c
+++ b/target/msm8974/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2014, 2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -516,11 +516,26 @@
 	return restart_reason;
 }
 
+int set_download_mode(enum reboot_reason mode)
+{
+	if (mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD)
+		dload_util_write_cookie(mode == NORMAL_DLOAD ?
+			DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
+
+	return 0;
+}
+
 void reboot_device(unsigned reboot_reason)
 {
 	uint32_t soc_ver = 0;
 	uint8_t reset_type = 0;
 
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
+
 	soc_ver = board_soc_version();
 
 	/* Write the reboot reason */
@@ -548,14 +563,6 @@
 	dprintf(CRITICAL, "Rebooting failed\n");
 }
 
-int set_download_mode(enum dload_mode mode)
-{
-	dload_util_write_cookie(mode == NORMAL_DLOAD ?
-		DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
-
-	return 0;
-}
-
 /* Check if MSM needs VBUS mimic for USB */
 static int target_needs_vbus_mimic()
 {
@@ -830,4 +837,4 @@
 uint32_t target_get_pmic()
 {
 	return PMIC_IS_PM8941;
-}
\ No newline at end of file
+}
diff --git a/target/msm8994/init.c b/target/msm8994/init.c
index 066b50f..07fff1e 100644
--- a/target/msm8994/init.c
+++ b/target/msm8994/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2013-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2013-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -500,11 +500,31 @@
 	return restart_reason;
 }
 
+int set_download_mode(enum reboot_reason mode)
+{
+	if (mode == NORMAL_DLOAD || mode == EMERGENCY_DLOAD) {
+		if (platform_is_msm8994())
+			dload_util_write_cookie(mode == NORMAL_DLOAD ?
+				DLOAD_MODE_ADDR : EMERGENCY_DLOAD_MODE_ADDR, mode);
+		else
+			dload_util_write_cookie(mode == NORMAL_DLOAD ?
+				DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
+	}
+
+	return 0;
+}
+
 void reboot_device(unsigned reboot_reason)
 {
 	uint8_t reset_type = 0;
 	uint32_t restart_reason_addr;
 
+	/* Set cookie for dload mode */
+	if(set_download_mode(reboot_reason)) {
+		dprintf(CRITICAL, "HALT: set_download_mode not supported\n");
+		return;
+	}
+
 	if (platform_is_msm8994())
 		restart_reason_addr = RESTART_REASON_ADDR;
 	else
@@ -636,18 +656,6 @@
 	return DDR_CFG_DLY_VAL;
 }
 
-int set_download_mode(enum dload_mode mode)
-{
-	if (platform_is_msm8994())
-		dload_util_write_cookie(mode == NORMAL_DLOAD ?
-			DLOAD_MODE_ADDR : EMERGENCY_DLOAD_MODE_ADDR, mode);
-	else
-		dload_util_write_cookie(mode == NORMAL_DLOAD ?
-			DLOAD_MODE_ADDR_V2 : EMERGENCY_DLOAD_MODE_ADDR_V2, mode);
-
-	return 0;
-}
-
 uint32_t target_get_pmic()
 {
 	return PMIC_IS_PMI8994;
diff --git a/target/msm8996/init.c b/target/msm8996/init.c
index c6d7d79..42a0614 100644
--- a/target/msm8996/init.c
+++ b/target/msm8996/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2015 The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2016 The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -73,6 +73,8 @@
 #define VIBRATE_TIME 250
 #endif
 
+#include <pm_smbchg_usb_chgpth.h>
+
 #define CE_INSTANCE             1
 #define CE_EE                   0
 #define CE_FIFO_SIZE            64
@@ -563,23 +565,28 @@
 {
 	uint8_t pon_reason = pm8x41_get_pon_reason();
 	uint8_t is_cold_boot = pm8x41_get_is_cold_boot();
-	dprintf(INFO, "%s : pon_reason is %d cold_boot:%d\n", __func__,
-		pon_reason, is_cold_boot);
+	pm_smbchg_usb_chgpth_pwr_pth_type charger_path = PM_SMBCHG_USB_CHGPTH_PWR_PATH__INVALID;
+	dprintf(INFO, "%s : pon_reason is %d cold_boot:%d charger path: %d\n", __func__,
+		pon_reason, is_cold_boot, charger_path);
 	/* In case of fastboot reboot,adb reboot or if we see the power key
 	* pressed we do not want go into charger mode.
 	* fastboot reboot is warm boot with PON hard reset bit not set
 	* adb reboot is a cold boot with PON hard reset bit set
 	*/
+	pm_smbchg_get_charger_path(1, &charger_path);
 	if (is_cold_boot &&
 			(!(pon_reason & HARD_RST)) &&
 			(!(pon_reason & KPDPWR_N)) &&
-			((pon_reason & PON1)))
+			((pon_reason & PON1)) &&
+			((charger_path == PM_SMBCHG_USB_CHGPTH_PWR_PATH__DC_CHARGER) ||
+			(charger_path == PM_SMBCHG_USB_CHGPTH_PWR_PATH__USB_CHARGER)))
+
 		return 1;
 	else
 		return 0;
 }
 
-int set_download_mode(enum dload_mode mode)
+int set_download_mode(enum reboot_reason mode)
 {
 	int ret = 0;
 	ret = scm_dload_mode(mode);
diff --git a/target/msmtitanium/init.c b/target/msmtitanium/init.c
old mode 100755
new mode 100644
index 06d63ce..b40246a
--- a/target/msmtitanium/init.c
+++ b/target/msmtitanium/init.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions are
@@ -63,6 +63,11 @@
 #include <shutdown_detect.h>
 #endif
 
+#if PON_VIB_SUPPORT
+#include <vibrator.h>
+#define VIBRATE_TIME 250
+#endif
+
 #define PMIC_ARB_CHANNEL_NUM    0
 #define PMIC_ARB_OWNER_ID       0
 #define TLMM_VOL_UP_BTN_GPIO    85
@@ -230,6 +235,10 @@
 	shutdown_detect();
 #endif
 
+#if PON_VIB_SUPPORT
+	vib_timed_turn_on(VIBRATE_TIME);
+#endif
+
 
 	if (target_use_signed_kernel())
 		target_crypto_init_params();
@@ -322,7 +331,8 @@
 {
 	return board_baseband();
 }
-int set_download_mode(enum dload_mode mode)
+
+int set_download_mode(enum reboot_reason mode)
 {
 	int ret = 0;
 	ret = scm_dload_mode(mode);
diff --git a/target/msmtitanium/rules.mk b/target/msmtitanium/rules.mk
index b338bea..e09b699 100644
--- a/target/msmtitanium/rules.mk
+++ b/target/msmtitanium/rules.mk
@@ -10,11 +10,14 @@
 BASE_ADDR        := 0x80000000
 SCRATCH_ADDR     := 0x90000000
 
+DEFINES += PMI_CONFIGURED=1
+
 MODULES += \
 	dev/keys \
 	dev/vib \
 	lib/ptable \
 	dev/pmic/pm8x41 \
+	dev/qpnp_haptic \
 	lib/libfdt
 
 DEFINES += \