diff --git a/plat/intel/soc/common/aarch64/plat_helpers.S b/plat/intel/soc/common/aarch64/plat_helpers.S
index 27b538a..5cb9b69 100644
--- a/plat/intel/soc/common/aarch64/plat_helpers.S
+++ b/plat/intel/soc/common/aarch64/plat_helpers.S
@@ -66,12 +66,34 @@
 	ret
 endfunc plat_my_core_pos
 
+func warm_reset_req
+	str	xzr, [x4]
+	bl	plat_is_my_cpu_primary
+	cbz	x0, cpu_in_wfi
+	mov_imm x1, PLAT_SEC_ENTRY
+	str	xzr, [x1]
+	mrs	x1, rmr_el3
+	orr	x1, x1, #0x02
+	msr	rmr_el3, x1
+	isb
+	dsb	sy
+cpu_in_wfi:
+	wfi
+	b	cpu_in_wfi
+endfunc warm_reset_req
+
 func plat_get_my_entrypoint
+	ldr	x4, =L2_RESET_DONE_REG
+	ldr	x5, [x4]
+	ldr	x1, =L2_RESET_DONE_STATUS
+	cmp	x1, x5
+	b.eq	warm_reset_req
 	mov_imm	x1, PLAT_SEC_ENTRY
 	ldr	x0, [x1]
 	ret
 endfunc plat_get_my_entrypoint
 
+
 	/* ---------------------------------------------
 	 * int plat_crash_console_init(void)
 	 * Function to initialize the crash console
diff --git a/plat/intel/soc/common/include/platform_def.h b/plat/intel/soc/common/include/platform_def.h
index d6014d3..06f3a1b 100644
--- a/plat/intel/soc/common/include/platform_def.h
+++ b/plat/intel/soc/common/include/platform_def.h
@@ -19,6 +19,15 @@
 /* sysmgr.boot_scratch_cold4 & 5 used for CPU release address for SPL */
 #define PLAT_CPU_RELEASE_ADDR			0xffd12210
 
+/*
+ * sysmgr.boot_scratch_cold6 & 7 (64bit) are used to indicate L2 reset
+ * is done and HPS should trigger warm reset via RMR_EL3.
+ */
+#define L2_RESET_DONE_REG			0xFFD12218
+
+/* Magic word to indicate L2 reset is completed */
+#define L2_RESET_DONE_STATUS			0x1228E5E7
+
 /* Define next boot image name and offset */
 #define PLAT_NS_IMAGE_OFFSET			0x50000
 #define PLAT_HANDOFF_OFFSET			0xFFE3F000
diff --git a/plat/intel/soc/common/include/socfpga_reset_manager.h b/plat/intel/soc/common/include/socfpga_reset_manager.h
new file mode 100644
index 0000000..3fbf242
--- /dev/null
+++ b/plat/intel/soc/common/include/socfpga_reset_manager.h
@@ -0,0 +1,18 @@
+/*
+ * Copyright (c) 2019, Intel Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SOCFPGA_RESETMANAGER_H
+#define SOCFPGA_RESETMANAGER_H
+
+#define SOCFPGA_RSTMGR_STAT				0xffd11000
+#define SOCFPGA_RSTMGR_HDSKEN				0xffd11010
+#define SOCFPGA_RSTMGR_COLDMODRST			0xffd11034
+#define SOCFPGA_RSTMGR_HDSKTIMEOUT			0xffd11064
+
+#define SOCFPGA_RSTMGR_HDSKEN_SET			0x0000010D
+#define SOCFPGA_RSTMGR_SDMWARMRST			0x00000002
+
+#endif /* SOCFPGA_RESETMANAGER_H */
diff --git a/plat/intel/soc/common/socfpga_image_load.c b/plat/intel/soc/common/socfpga_image_load.c
index 67c02bc..a5c3279 100644
--- a/plat/intel/soc/common/socfpga_image_load.c
+++ b/plat/intel/soc/common/socfpga_image_load.c
@@ -28,5 +28,31 @@
  ******************************************************************************/
 bl_params_t *plat_get_next_bl_params(void)
 {
+	unsigned int count;
+	unsigned int img_id = 0U;
+	unsigned int link_index = 0U;
+	bl_params_node_t *bl_exec_node = NULL;
+	bl_mem_params_node_t *desc_ptr;
+
+	/* If there is no image to start with, return NULL */
+	if (bl_mem_params_desc_num == 0U)
+		return NULL;
+
+	/* Clean next_params_info in BL image node */
+	for (count = 0U; count < bl_mem_params_desc_num; count++) {
+
+		desc_ptr = &bl_mem_params_desc_ptr[link_index];
+		bl_exec_node = &desc_ptr->params_node_mem;
+		bl_exec_node->next_params_info = NULL;
+
+		/* If no next hand-off image then break out */
+		img_id = desc_ptr->next_handoff_image_id;
+		if (img_id == INVALID_IMAGE_ID)
+			break;
+
+		/* Get the index for the next hand-off image */
+		link_index = get_bl_params_node_index(img_id);
+	}
+
 	return get_next_bl_params_from_mem_params_desc();
 }
diff --git a/plat/intel/soc/common/socfpga_psci.c b/plat/intel/soc/common/socfpga_psci.c
index 65a4b09..1ba48ea 100644
--- a/plat/intel/soc/common/socfpga_psci.c
+++ b/plat/intel/soc/common/socfpga_psci.c
@@ -13,6 +13,7 @@
 
 #include "socfpga_mailbox.h"
 #include "socfpga_plat_def.h"
+#include "socfpga_reset_manager.h"
 
 
 
@@ -75,6 +76,7 @@
 	for (size_t i = 0; i <= PLAT_MAX_PWR_LVL; i++)
 		VERBOSE("%s: target_state->pwr_domain_state[%lu]=%x\n",
 			__func__, i, target_state->pwr_domain_state[i]);
+
 	/* assert core reset */
 	mmio_setbits_32(SOCFPGA_RSTMGR_MPUMODRST_OFST, 1 << cpu_id);
 
@@ -136,6 +138,31 @@
 		wfi();
 }
 
+static int socfpga_system_reset2(int is_vendor, int reset_type,
+					u_register_t cookie)
+{
+	/* disable cpuif */
+	gicv2_cpuif_disable();
+
+	/* Store magic number */
+	mmio_write_32(L2_RESET_DONE_REG, L2_RESET_DONE_STATUS);
+
+	/* Increase timeout */
+	mmio_write_32(SOCFPGA_RSTMGR_HDSKTIMEOUT, 0xffffff);
+
+	/* Enable handshakes */
+	mmio_setbits_32(SOCFPGA_RSTMGR_HDSKEN, SOCFPGA_RSTMGR_HDSKEN_SET);
+
+	/* Reset L2 module */
+	mmio_setbits_32(SOCFPGA_RSTMGR_COLDMODRST, 0x100);
+
+	while (1)
+		wfi();
+
+	/* Should not reach here */
+	return 0;
+}
+
 int socfpga_validate_power_state(unsigned int power_state,
 				psci_power_state_t *req_state)
 {
@@ -169,6 +196,7 @@
 	.pwr_domain_suspend_finish = socfpga_pwr_domain_suspend_finish,
 	.system_off = socfpga_system_off,
 	.system_reset = socfpga_system_reset,
+	.system_reset2 = socfpga_system_reset2,
 	.validate_power_state = socfpga_validate_power_state,
 	.validate_ns_entrypoint = socfpga_validate_ns_entrypoint,
 	.get_sys_suspend_power_state = socfpga_get_sys_suspend_power_state
diff --git a/plat/intel/soc/common/socfpga_sip_svc.c b/plat/intel/soc/common/socfpga_sip_svc.c
index c3c6ae0..f58b58f 100644
--- a/plat/intel/soc/common/socfpga_sip_svc.c
+++ b/plat/intel/soc/common/socfpga_sip_svc.c
@@ -7,6 +7,7 @@
 #include <assert.h>
 #include <common/debug.h>
 #include <common/runtime_svc.h>
+#include <lib/mmio.h>
 #include <tools_share/uuid.h>
 
 #include "socfpga_mailbox.h"
@@ -270,6 +271,79 @@
 	return INTEL_SIP_SMC_STATUS_OK;
 }
 
+static int is_out_of_sec_range(uint64_t reg_addr)
+{
+	switch (reg_addr) {
+	case(0xF8011100):	/* ECCCTRL1 */
+	case(0xF8011104):	/* ECCCTRL2 */
+	case(0xF8011110):	/* ERRINTEN */
+	case(0xF8011114):	/* ERRINTENS */
+	case(0xF8011118):	/* ERRINTENR */
+	case(0xF801111C):	/* INTMODE */
+	case(0xF8011120):	/* INTSTAT */
+	case(0xF8011124):	/* DIAGINTTEST */
+	case(0xF801112C):	/* DERRADDRA */
+	case(0xFFD12028):	/* SDMMCGRP_CTRL */
+	case(0xFFD12044):	/* EMAC0 */
+	case(0xFFD12048):	/* EMAC1 */
+	case(0xFFD1204C):	/* EMAC2 */
+	case(0xFFD12090):	/* ECC_INT_MASK_VALUE */
+	case(0xFFD12094):	/* ECC_INT_MASK_SET */
+	case(0xFFD12098):	/* ECC_INT_MASK_CLEAR */
+	case(0xFFD1209C):	/* ECC_INTSTATUS_SERR */
+	case(0xFFD120A0):	/* ECC_INTSTATUS_DERR */
+	case(0xFFD120C0):	/* NOC_TIMEOUT */
+	case(0xFFD120C4):	/* NOC_IDLEREQ_SET */
+	case(0xFFD120C8):	/* NOC_IDLEREQ_CLR */
+	case(0xFFD120D0):	/* NOC_IDLEACK */
+	case(0xFFD120D4):	/* NOC_IDLESTATUS */
+	case(0xFFD12200):	/* BOOT_SCRATCH_COLD0 */
+	case(0xFFD12204):	/* BOOT_SCRATCH_COLD1 */
+	case(0xFFD12220):	/* BOOT_SCRATCH_COLD8 */
+	case(0xFFD12224):	/* BOOT_SCRATCH_COLD9 */
+		return 0;
+
+	default:
+		break;
+	}
+
+	return -1;
+}
+
+/* Secure register access */
+uint32_t intel_secure_reg_read(uint64_t reg_addr, uint32_t *retval)
+{
+	if (is_out_of_sec_range(reg_addr))
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	*retval = mmio_read_32(reg_addr);
+
+	return INTEL_SIP_SMC_STATUS_OK;
+}
+
+uint32_t intel_secure_reg_write(uint64_t reg_addr, uint32_t val,
+				uint32_t *retval)
+{
+	if (is_out_of_sec_range(reg_addr))
+		return INTEL_SIP_SMC_STATUS_ERROR;
+
+	mmio_write_32(reg_addr, val);
+
+	return intel_secure_reg_read(reg_addr, retval);
+}
+
+uint32_t intel_secure_reg_update(uint64_t reg_addr, uint32_t mask,
+				 uint32_t val, uint32_t *retval)
+{
+	if (!intel_secure_reg_read(reg_addr, retval)) {
+		*retval &= ~mask;
+		*retval |= val;
+		return intel_secure_reg_write(reg_addr, *retval, retval);
+	}
+
+	return INTEL_SIP_SMC_STATUS_ERROR;
+}
+
 /*
  * This function is responsible for handling all SiP calls from the NS world
  */
@@ -283,6 +357,7 @@
 			 void *handle,
 			 u_register_t flags)
 {
+	uint32_t val = 0;
 	uint32_t status = INTEL_SIP_SMC_STATUS_OK;
 	uint32_t completed_addr[3];
 	uint32_t count = 0;
@@ -291,25 +366,25 @@
 	case SIP_SVC_UID:
 		/* Return UID to the caller */
 		SMC_UUID_RET(handle, intl_svc_uid);
-		break;
+
 	case INTEL_SIP_SMC_FPGA_CONFIG_ISDONE:
 		status = intel_mailbox_fpga_config_isdone();
 		SMC_RET4(handle, status, 0, 0, 0);
-		break;
+
 	case INTEL_SIP_SMC_FPGA_CONFIG_GET_MEM:
 		SMC_RET3(handle, INTEL_SIP_SMC_STATUS_OK,
 			INTEL_SIP_SMC_FPGA_CONFIG_ADDR,
 			INTEL_SIP_SMC_FPGA_CONFIG_SIZE -
 				INTEL_SIP_SMC_FPGA_CONFIG_ADDR);
-		break;
+
 	case INTEL_SIP_SMC_FPGA_CONFIG_START:
 		status = intel_fpga_config_start(x1);
 		SMC_RET4(handle, status, 0, 0, 0);
-		break;
+
 	case INTEL_SIP_SMC_FPGA_CONFIG_WRITE:
 		status = intel_fpga_config_write(x1, x2);
 		SMC_RET4(handle, status, 0, 0, 0);
-		break;
+
 	case INTEL_SIP_SMC_FPGA_CONFIG_COMPLETED_WRITE:
 		status = intel_fpga_config_completed_write(completed_addr,
 								&count);
@@ -317,26 +392,38 @@
 		case 1:
 			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
 				completed_addr[0], 0, 0);
-			break;
+
 		case 2:
 			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
 				completed_addr[0],
 				completed_addr[1], 0);
-			break;
+
 		case 3:
 			SMC_RET4(handle, INTEL_SIP_SMC_STATUS_OK,
 				completed_addr[0],
 				completed_addr[1],
 				completed_addr[2]);
-			break;
+
 		case 0:
 			SMC_RET4(handle, status, 0, 0, 0);
-			break;
+
 		default:
 			mailbox_clear_response();
 			SMC_RET1(handle, INTEL_SIP_SMC_STATUS_ERROR);
 		}
-		break;
+
+	case INTEL_SIP_SMC_REG_READ:
+		status = intel_secure_reg_read(x1, &val);
+		SMC_RET3(handle, status, val, x1);
+
+	case INTEL_SIP_SMC_REG_WRITE:
+		status = intel_secure_reg_write(x1, (uint32_t)x2, &val);
+		SMC_RET3(handle, status, val, x1);
+
+	case INTEL_SIP_SMC_REG_UPDATE:
+		status = intel_secure_reg_update(x1, (uint32_t)x2,
+						 (uint32_t)x3, &val);
+		SMC_RET3(handle, status, val, x1);
 
 	default:
 		return socfpga_sip_handler(smc_fid, x1, x2, x3, x4,