Merge pull request #191 from danh-arm/jc/tf-issues/218

Add support for PSCI SYSTEM_OFF and SYSTEM_RESET APIs v2

bl31/bl31.mk

@@ -50,7 +50,8 @@ BL31_SOURCES		+=	bl31/bl31_main.c				\
 				services/std_svc/psci/psci_entry.S		\
 				services/std_svc/psci/psci_helpers.S		\
 				services/std_svc/psci/psci_main.c		\
-				services/std_svc/psci/psci_setup.c
+				services/std_svc/psci/psci_setup.c		\
+				services/std_svc/psci/psci_system_off.c
 
 BL31_LINKERFILE		:=	bl31/bl31.ld.S
 

bl32/tsp/aarch64/tsp_entrypoint.S

@@ -162,6 +162,8 @@ func tsp_vector_table
 	b	tsp_cpu_resume_entry
 	b	tsp_cpu_suspend_entry
 	b	tsp_fiq_entry
+	b	tsp_system_off_entry
+	b	tsp_system_reset_entry
 
 	/*---------------------------------------------
 	 * This entrypoint is used by the TSPD when this
@@ -177,6 +179,28 @@ func tsp_cpu_off_entry
 	bl	tsp_cpu_off_main
 	restore_args_call_smc
 
+	/*---------------------------------------------
+	 * This entrypoint is used by the TSPD when the
+	 * system is about to be switched off (through
+	 * a SYSTEM_OFF psci call) to ask the TSP to
+	 * perform any necessary bookkeeping.
+	 * ---------------------------------------------
+	 */
+func tsp_system_off_entry
+	bl	tsp_system_off_main
+	restore_args_call_smc
+
+	/*---------------------------------------------
+	 * This entrypoint is used by the TSPD when the
+	 * system is about to be reset (through a
+	 * SYSTEM_RESET psci call) to ask the TSP to
+	 * perform any necessary bookkeeping.
+	 * ---------------------------------------------
+	 */
+func tsp_system_reset_entry
+	bl	tsp_system_reset_main
+	restore_args_call_smc
+
 	/*---------------------------------------------
 	 * This entrypoint is used by the TSPD when this
 	 * cpu is turned on using a CPU_ON psci call to

bl32/tsp/tsp_main.c

@@ -296,6 +296,72 @@ tsp_args_t *tsp_cpu_resume_main(uint64_t suspend_level,
 	return set_smc_args(TSP_RESUME_DONE, 0, 0, 0, 0, 0, 0, 0);
 }
 
+/*******************************************************************************
+ * This function performs any remaining bookkeeping in the test secure payload
+ * before the system is switched off (in response to a psci SYSTEM_OFF request)
+ ******************************************************************************/
+tsp_args_t *tsp_system_off_main(uint64_t arg0,
+				uint64_t arg1,
+				uint64_t arg2,
+				uint64_t arg3,
+				uint64_t arg4,
+				uint64_t arg5,
+				uint64_t arg6,
+				uint64_t arg7)
+{
+	uint64_t mpidr = read_mpidr();
+	uint32_t linear_id = platform_get_core_pos(mpidr);
+
+	/* Update this cpu's statistics */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%x SYSTEM_OFF request\n", mpidr);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets requests\n", mpidr,
+	     tsp_stats[linear_id].smc_count,
+	     tsp_stats[linear_id].eret_count);
+	spin_unlock(&console_lock);
+#endif
+
+	/* Indicate to the SPD that we have completed this request */
+	return set_smc_args(TSP_SYSTEM_OFF_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
+
+/*******************************************************************************
+ * This function performs any remaining bookkeeping in the test secure payload
+ * before the system is reset (in response to a psci SYSTEM_RESET request)
+ ******************************************************************************/
+tsp_args_t *tsp_system_reset_main(uint64_t arg0,
+				uint64_t arg1,
+				uint64_t arg2,
+				uint64_t arg3,
+				uint64_t arg4,
+				uint64_t arg5,
+				uint64_t arg6,
+				uint64_t arg7)
+{
+	uint64_t mpidr = read_mpidr();
+	uint32_t linear_id = platform_get_core_pos(mpidr);
+
+	/* Update this cpu's statistics */
+	tsp_stats[linear_id].smc_count++;
+	tsp_stats[linear_id].eret_count++;
+
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	spin_lock(&console_lock);
+	INFO("TSP: cpu 0x%x SYSTEM_RESET request\n", mpidr);
+	INFO("TSP: cpu 0x%x: %d smcs, %d erets requests\n", mpidr,
+	     tsp_stats[linear_id].smc_count,
+	     tsp_stats[linear_id].eret_count);
+	spin_unlock(&console_lock);
+#endif
+
+	/* Indicate to the SPD that we have completed this request */
+	return set_smc_args(TSP_SYSTEM_RESET_DONE, 0, 0, 0, 0, 0, 0, 0);
+}
+
 /*******************************************************************************
  * TSP fast smc handler. The secure monitor jumps to this function by
  * doing the ERET after populating X0-X7 registers. The arguments are received

fdts/fvp-base-gicv2-psci.dts

@@ -57,6 +57,8 @@
 		cpu_suspend = <0xc4000001>;
 		cpu_off = <0x84000002>;
 		cpu_on = <0xc4000003>;
+		sys_poweroff = <0x84000008>;
+		sys_reset = <0x84000009>;
 	};
 
 	cpus {

fdts/fvp-base-gicv2legacy-psci.dts

@@ -57,6 +57,8 @@
 		cpu_suspend = <0xc4000001>;
 		cpu_off = <0x84000002>;
 		cpu_on = <0xc4000003>;
+		sys_poweroff = <0x84000008>;
+		sys_reset = <0x84000009>;
 	};
 
 	cpus {

fdts/fvp-base-gicv3-psci.dts

@@ -229,7 +229,7 @@
 				<0 0 41 &gic 0 0 0 41 4>,
 				<0 0 42 &gic 0 0 0 42 4>;
 
-		/include/ "rtsm_ve-motherboard.dtsi"
+		/include/ "rtsm_ve-motherboard-no_psci.dtsi"
 	};
 
 	panels {

fdts/fvp-foundation-gicv2-psci.dts

@@ -57,6 +57,8 @@
 		cpu_suspend = <0xc4000001>;
 		cpu_off = <0x84000002>;
 		cpu_on = <0xc4000003>;
+		sys_poweroff = <0x84000008>;
+		sys_reset = <0x84000009>;
 	};
 
 	cpus {

fdts/fvp-foundation-gicv2legacy-psci.dts

@@ -57,6 +57,8 @@
 		cpu_suspend = <0xc4000001>;
 		cpu_off = <0x84000002>;
 		cpu_on = <0xc4000003>;
+		sys_poweroff = <0x84000008>;
+		sys_reset = <0x84000009>;
 	};
 
 	cpus {

fdts/fvp-foundation-gicv3-psci.dts

@@ -205,6 +205,6 @@
 				<0 0 41 &gic 0 0 0 41 4>,
 				<0 0 42 &gic 0 0 0 42 4>;
 
-		/include/ "fvp-foundation-motherboard.dtsi"
+		/include/ "fvp-foundation-motherboard-no_psci.dtsi"
 	};
 };

fdts/fvp-foundation-motherboard-no_psci.dtsi

+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of the ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+	motherboard {
+		arm,v2m-memory-map = "rs1";
+		compatible = "arm,vexpress,v2m-p1", "simple-bus";
+		#address-cells = <2>; /* SMB chipselect number and offset */
+		#size-cells = <1>;
+		#interrupt-cells = <1>;
+		ranges;
+
+		ethernet@2,02000000 {
+			compatible = "smsc,lan91c111";
+			reg = <2 0x02000000 0x10000>;
+			interrupts = <15>;
+		};
+
+		v2m_clk24mhz: clk24mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <24000000>;
+			clock-output-names = "v2m:clk24mhz";
+		};
+
+		v2m_refclk1mhz: refclk1mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <1000000>;
+			clock-output-names = "v2m:refclk1mhz";
+		};
+
+		v2m_refclk32khz: refclk32khz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+			clock-output-names = "v2m:refclk32khz";
+		};
+
+		iofpga@3,00000000 {
+			compatible = "arm,amba-bus", "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 3 0 0x200000>;
+
+			v2m_sysreg: sysreg@010000 {
+				compatible = "arm,vexpress-sysreg";
+				reg = <0x010000 0x1000>;
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+
+			v2m_sysctl: sysctl@020000 {
+				compatible = "arm,sp810", "arm,primecell";
+				reg = <0x020000 0x1000>;
+				clocks = <&v2m_refclk32khz>, <&v2m_refclk1mhz>, <&v2m_clk24mhz>;
+				clock-names = "refclk", "timclk", "apb_pclk";
+				#clock-cells = <1>;
+				clock-output-names = "timerclken0", "timerclken1", "timerclken2", "timerclken3";
+			};
+
+			v2m_serial0: uart@090000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x090000 0x1000>;
+				interrupts = <5>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial1: uart@0a0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0a0000 0x1000>;
+				interrupts = <6>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial2: uart@0b0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0b0000 0x1000>;
+				interrupts = <7>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial3: uart@0c0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0c0000 0x1000>;
+				interrupts = <8>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			wdt@0f0000 {
+				compatible = "arm,sp805", "arm,primecell";
+				reg = <0x0f0000 0x1000>;
+				interrupts = <0>;
+				clocks = <&v2m_refclk32khz>, <&v2m_clk24mhz>;
+				clock-names = "wdogclk", "apb_pclk";
+			};
+
+			v2m_timer01: timer@110000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x110000 0x1000>;
+				interrupts = <2>;
+				clocks = <&v2m_sysctl 0>, <&v2m_sysctl 1>, <&v2m_clk24mhz>;
+				clock-names = "timclken1", "timclken2", "apb_pclk";
+			};
+
+			v2m_timer23: timer@120000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x120000 0x1000>;
+				interrupts = <3>;
+				clocks = <&v2m_sysctl 2>, <&v2m_sysctl 3>, <&v2m_clk24mhz>;
+				clock-names = "timclken1", "timclken2", "apb_pclk";
+			};
+
+			rtc@170000 {
+				compatible = "arm,pl031", "arm,primecell";
+				reg = <0x170000 0x1000>;
+				interrupts = <4>;
+				clocks = <&v2m_clk24mhz>;
+				clock-names = "apb_pclk";
+			};
+
+			virtio_block@0130000 {
+				compatible = "virtio,mmio";
+				reg = <0x130000 0x1000>;
+				interrupts = <0x2a>;
+			};
+		};
+
+		v2m_fixed_3v3: fixedregulator@0 {
+			compatible = "regulator-fixed";
+			regulator-name = "3V3";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+		};
+
+
+		mcc {
+			compatible = "arm,vexpress,config-bus", "simple-bus";
+			arm,vexpress,config-bridge = <&v2m_sysreg>;
+
+			reset@0 {
+				compatible = "arm,vexpress-reset";
+				arm,vexpress-sysreg,func = <5 0>;
+			};
+
+			muxfpga@0 {
+				compatible = "arm,vexpress-muxfpga";
+				arm,vexpress-sysreg,func = <7 0>;
+			};
+
+			shutdown@0 {
+				compatible = "arm,vexpress-shutdown";
+				arm,vexpress-sysreg,func = <8 0>;
+			};
+
+			reboot@0 {
+				compatible = "arm,vexpress-reboot";
+				arm,vexpress-sysreg,func = <9 0>;
+			};
+
+			dvimode@0 {
+				compatible = "arm,vexpress-dvimode";
+				arm,vexpress-sysreg,func = <11 0>;
+			};
+		};
+	};

fdts/fvp-foundation-motherboard.dtsi

@@ -169,25 +169,37 @@
 			compatible = "arm,vexpress,config-bus", "simple-bus";
 			arm,vexpress,config-bridge = <&v2m_sysreg>;
 
-			reset@0 {
-				compatible = "arm,vexpress-reset";
-				arm,vexpress-sysreg,func = <5 0>;
-			};
+			/*
+			 * Not supported in FVP models
+			 *
+			 * reset@0 {
+			 * 	compatible = "arm,vexpress-reset";
+			 * 	arm,vexpress-sysreg,func = <5 0>;
+			 * };
+			 */
 
 			muxfpga@0 {
 				compatible = "arm,vexpress-muxfpga";
 				arm,vexpress-sysreg,func = <7 0>;
 			};
 
-			shutdown@0 {
-				compatible = "arm,vexpress-shutdown";
-				arm,vexpress-sysreg,func = <8 0>;
-			};
-
-			reboot@0 {
-				compatible = "arm,vexpress-reboot";
-				arm,vexpress-sysreg,func = <9 0>;
-			};
+			/*
+			 * Not used - Superseded by PSCI sys_poweroff
+			 *
+			 * shutdown@0 {
+			 * 	compatible = "arm,vexpress-shutdown";
+			 * 	arm,vexpress-sysreg,func = <8 0>;
+			 * };
+			 */
+
+			/*
+			 * Not used - Superseded by PSCI sys_reset
+			 *
+			 * reboot@0 {
+			 * 	compatible = "arm,vexpress-reboot";
+			 * 	arm,vexpress-sysreg,func = <9 0>;
+			 * };
+			 */
 
 			dvimode@0 {
 				compatible = "arm,vexpress-dvimode";

fdts/rtsm_ve-motherboard-no_psci.dtsi

+/*
+ * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+	motherboard {
+		arm,v2m-memory-map = "rs1";
+		compatible = "arm,vexpress,v2m-p1", "simple-bus";
+		#address-cells = <2>; /* SMB chipselect number and offset */
+		#size-cells = <1>;
+		#interrupt-cells = <1>;
+		ranges;
+
+		flash@0,00000000 {
+			compatible = "arm,vexpress-flash", "cfi-flash";
+			reg = <0 0x00000000 0x04000000>,
+			      <4 0x00000000 0x04000000>;
+			bank-width = <4>;
+		};
+
+		vram@2,00000000 {
+			compatible = "arm,vexpress-vram";
+			reg = <2 0x00000000 0x00800000>;
+		};
+
+		ethernet@2,02000000 {
+			compatible = "smsc,lan91c111";
+			reg = <2 0x02000000 0x10000>;
+			interrupts = <15>;
+		};
+
+		v2m_clk24mhz: clk24mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <24000000>;
+			clock-output-names = "v2m:clk24mhz";
+		};
+
+		v2m_refclk1mhz: refclk1mhz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <1000000>;
+			clock-output-names = "v2m:refclk1mhz";
+		};
+
+		v2m_refclk32khz: refclk32khz {
+			compatible = "fixed-clock";
+			#clock-cells = <0>;
+			clock-frequency = <32768>;
+			clock-output-names = "v2m:refclk32khz";
+		};
+
+		iofpga@3,00000000 {
+			compatible = "arm,amba-bus", "simple-bus";
+			#address-cells = <1>;
+			#size-cells = <1>;
+			ranges = <0 3 0 0x200000>;
+
+			v2m_sysreg: sysreg@010000 {
+				compatible = "arm,vexpress-sysreg";
+				reg = <0x010000 0x1000>;
+				gpio-controller;
+				#gpio-cells = <2>;
+			};
+
+			v2m_sysctl: sysctl@020000 {
+				compatible = "arm,sp810", "arm,primecell";
+				reg = <0x020000 0x1000>;
+				clocks = <&v2m_refclk32khz>, <&v2m_refclk1mhz>, <&v2m_clk24mhz>;
+				clock-names = "refclk", "timclk", "apb_pclk";
+				#clock-cells = <1>;
+				clock-output-names = "timerclken0", "timerclken1", "timerclken2", "timerclken3";
+			};
+
+			aaci@040000 {
+				compatible = "arm,pl041", "arm,primecell";
+				reg = <0x040000 0x1000>;
+				interrupts = <11>;
+				clocks = <&v2m_clk24mhz>;
+				clock-names = "apb_pclk";
+			};
+
+			mmci@050000 {
+				compatible = "arm,pl180", "arm,primecell";
+				reg = <0x050000 0x1000>;
+				interrupts = <9 10>;
+				cd-gpios = <&v2m_sysreg 0 0>;
+				wp-gpios = <&v2m_sysreg 1 0>;
+				max-frequency = <12000000>;
+				vmmc-supply = <&v2m_fixed_3v3>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "mclk", "apb_pclk";
+			};
+
+			kmi@060000 {
+				compatible = "arm,pl050", "arm,primecell";
+				reg = <0x060000 0x1000>;
+				interrupts = <12>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "KMIREFCLK", "apb_pclk";
+			};
+
+			kmi@070000 {
+				compatible = "arm,pl050", "arm,primecell";
+				reg = <0x070000 0x1000>;
+				interrupts = <13>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "KMIREFCLK", "apb_pclk";
+			};
+
+			v2m_serial0: uart@090000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x090000 0x1000>;
+				interrupts = <5>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial1: uart@0a0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0a0000 0x1000>;
+				interrupts = <6>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial2: uart@0b0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0b0000 0x1000>;
+				interrupts = <7>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			v2m_serial3: uart@0c0000 {
+				compatible = "arm,pl011", "arm,primecell";
+				reg = <0x0c0000 0x1000>;
+				interrupts = <8>;
+				clocks = <&v2m_clk24mhz>, <&v2m_clk24mhz>;
+				clock-names = "uartclk", "apb_pclk";
+			};
+
+			wdt@0f0000 {
+				compatible = "arm,sp805", "arm,primecell";
+				reg = <0x0f0000 0x1000>;
+				interrupts = <0>;
+				clocks = <&v2m_refclk32khz>, <&v2m_clk24mhz>;
+				clock-names = "wdogclk", "apb_pclk";
+			};
+
+			v2m_timer01: timer@110000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x110000 0x1000>;
+				interrupts = <2>;
+				clocks = <&v2m_sysctl 0>, <&v2m_sysctl 1>, <&v2m_clk24mhz>;
+				clock-names = "timclken1", "timclken2", "apb_pclk";
+			};
+
+			v2m_timer23: timer@120000 {
+				compatible = "arm,sp804", "arm,primecell";
+				reg = <0x120000 0x1000>;
+				interrupts = <3>;
+				clocks = <&v2m_sysctl 2>, <&v2m_sysctl 3>, <&v2m_clk24mhz>;
+				clock-names = "timclken1", "timclken2", "apb_pclk";
+			};
+
+			rtc@170000 {
+				compatible = "arm,pl031", "arm,primecell";
+				reg = <0x170000 0x1000>;
+				interrupts = <4>;
+				clocks = <&v2m_clk24mhz>;
+				clock-names = "apb_pclk";
+			};
+
+			clcd@1f0000 {
+				compatible = "arm,pl111", "arm,primecell";
+				reg = <0x1f0000 0x1000>;
+				interrupts = <14>;
+				clocks = <&v2m_oscclk1>, <&v2m_clk24mhz>;
+				clock-names = "clcdclk", "apb_pclk";
+				mode = "XVGA";
+				use_dma = <0>;
+				framebuffer = <0x18000000 0x00180000>;
+			};
+
+			virtio_block@0130000 {
+				compatible = "virtio,mmio";
+				reg = <0x130000 0x1000>;
+				interrupts = <0x2a>;
+			};
+		};
+
+		v2m_fixed_3v3: fixedregulator@0 {
+			compatible = "regulator-fixed";
+			regulator-name = "3V3";
+			regulator-min-microvolt = <3300000>;
+			regulator-max-microvolt = <3300000>;
+			regulator-always-on;
+		};
+
+		mcc {
+			compatible = "arm,vexpress,config-bus", "simple-bus";
+			arm,vexpress,config-bridge = <&v2m_sysreg>;
+
+			v2m_oscclk1: osc@1 {
+				/* CLCD clock */
+				compatible = "arm,vexpress-osc";
+				arm,vexpress-sysreg,func = <1 1>;
+				freq-range = <23750000 63500000>;
+				#clock-cells = <0>;
+				clock-output-names = "v2m:oscclk1";
+			};
+
+			reset@0 {
+				compatible = "arm,vexpress-reset";
+				arm,vexpress-sysreg,func = <5 0>;
+			};
+
+			muxfpga@0 {
+				compatible = "arm,vexpress-muxfpga";
+				arm,vexpress-sysreg,func = <7 0>;
+			};
+
+			shutdown@0 {
+				compatible = "arm,vexpress-shutdown";
+				arm,vexpress-sysreg,func = <8 0>;
+			};
+
+			reboot@0 {
+				compatible = "arm,vexpress-reboot";
+				arm,vexpress-sysreg,func = <9 0>;
+			};
+
+			dvimode@0 {
+				compatible = "arm,vexpress-dvimode";
+				arm,vexpress-sysreg,func = <11 0>;
+			};
+		};
+	};

fdts/rtsm_ve-motherboard.dtsi

@@ -236,25 +236,37 @@
 				clock-output-names = "v2m:oscclk1";
 			};
 
-			reset@0 {
-				compatible = "arm,vexpress-reset";
-				arm,vexpress-sysreg,func = <5 0>;
-			};
+			/*
+			 * Not supported in FVP models
+			 *
+			 * reset@0 {
+			 * 	compatible = "arm,vexpress-reset";
+			 * 	arm,vexpress-sysreg,func = <5 0>;
+			 * };
+			 */
 
 			muxfpga@0 {
 				compatible = "arm,vexpress-muxfpga";
 				arm,vexpress-sysreg,func = <7 0>;
 			};
 
-			shutdown@0 {
-				compatible = "arm,vexpress-shutdown";
-				arm,vexpress-sysreg,func = <8 0>;
-			};
-
-			reboot@0 {
-				compatible = "arm,vexpress-reboot";
-				arm,vexpress-sysreg,func = <9 0>;
-			};
+			/*
+			 * Not used - Superseded by PSCI sys_poweroff
+			 *
+			 * shutdown@0 {
+			 * 	compatible = "arm,vexpress-shutdown";
+			 * 	arm,vexpress-sysreg,func = <8 0>;
+			 * };
+			 */
+
+			/*
+			 * Not used - Superseded by PSCI sys_reset
+			 *
+			 * reboot@0 {
+			 * 	compatible = "arm,vexpress-reboot";
+			 * 	arm,vexpress-sysreg,func = <9 0>;
+			 * };
+			 */
 
 			dvimode@0 {
 				compatible = "arm,vexpress-dvimode";

include/bl31/services/psci.h

@@ -52,10 +52,9 @@
 #define PSCI_SYSTEM_RESET		0x84000009
 
 /*
- * Number of PSCI calls (above) implemented. System off and reset aren't
- * implemented as yet
+ * Number of PSCI calls (above) implemented
  */
-#define PSCI_NUM_CALLS			13
+#define PSCI_NUM_CALLS			15
 
 /*******************************************************************************
  * PSCI Migrate and friends
@@ -154,6 +153,8 @@ typedef struct plat_pm_ops {
 	int (*affinst_suspend_finish)(unsigned long,
 				      unsigned int,
 				      unsigned int);
+	void (*system_off)(void) __dead2;
+	void (*system_reset)(void) __dead2;
 } plat_pm_ops_t;
 
 /*******************************************************************************
@@ -170,6 +171,8 @@ typedef struct spd_pm_ops {
 	void (*svc_suspend_finish)(uint64_t suspend_level);
 	void (*svc_migrate)(uint64_t __unused1, uint64_t __unused2);
 	int32_t (*svc_migrate_info)(uint64_t *__unused);
+	void (*svc_system_off)(void);
+	void (*svc_system_reset)(void);
 } spd_pm_ops_t;
 
 /*******************************************************************************
@@ -182,8 +185,6 @@ int psci_affinity_info(unsigned long, unsigned int);
 int psci_migrate(unsigned int);
 unsigned int psci_migrate_info_type(void);
 unsigned long psci_migrate_info_up_cpu(void);
-void psci_system_off(void);
-void psci_system_reset(void);
 int psci_cpu_on(unsigned long,
 		unsigned long,
 		unsigned long);

include/bl32/tsp/tsp.h

@@ -41,6 +41,8 @@
 #define TSP_SUSPEND_DONE	0xf2000003
 #define TSP_RESUME_DONE		0xf2000004
 #define TSP_PREEMPTED		0xf2000005
+#define TSP_SYSTEM_OFF_DONE	0xf2000008
+#define TSP_SYSTEM_RESET_DONE	0xf2000009
 
 /*
  * Function identifiers to handle FIQs through the synchronous handling model.
@@ -114,6 +116,8 @@ typedef struct tsp_vectors {
 	tsp_vector_isn_t cpu_resume_entry;
 	tsp_vector_isn_t cpu_suspend_entry;
 	tsp_vector_isn_t fiq_entry;
+	tsp_vector_isn_t system_off_entry;
+	tsp_vector_isn_t system_reset_entry;
 } tsp_vectors_t;
 
 

plat/fvp/fvp_def.h

@@ -120,10 +120,23 @@
 
 /* V2M motherboard system registers & offsets */
 #define VE_SYSREGS_BASE		0x1c010000
-#define V2M_SYS_ID			0x0
-#define V2M_SYS_LED			0x8
+#define V2M_SYS_ID		0x0
+#define V2M_SYS_SWITCH		0x4
+#define V2M_SYS_LED		0x8
 #define V2M_SYS_CFGDATA		0xa0
 #define V2M_SYS_CFGCTRL		0xa4
+#define V2M_SYS_CFGSTATUS	0xa8
+
+#define CFGCTRL_START		(1 << 31)
+#define CFGCTRL_RW		(1 << 30)
+#define CFGCTRL_FUNC_SHIFT	20
+#define CFGCTRL_FUNC(fn)	(fn << CFGCTRL_FUNC_SHIFT)
+#define FUNC_CLK_GEN		0x01
+#define FUNC_TEMP		0x04
+#define FUNC_DB_RESET		0x05
+#define FUNC_SCC_CFG		0x06
+#define FUNC_SHUTDOWN		0x08
+#define FUNC_REBOOT		0x09
 
 /* Load address of BL33 in the FVP port */
 #define NS_IMAGE_OFFSET		(DRAM1_BASE + 0x8000000) /* DRAM + 128MB */