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

@@ -0,0 +1,197 @@
+/*
+ * 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";
+			 * Not supported in FVP models
-				arm,vexpress-sysreg,func = <5 0>;
+			 *
-			};
+			 * 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";
+			 * Not used - Superseded by PSCI sys_poweroff
-				arm,vexpress-sysreg,func = <8 0>;
+			 *
-			};
+			 * shutdown@0 {
+			 * 	compatible = "arm,vexpress-shutdown";
+			 * 	arm,vexpress-sysreg,func = <8 0>;
+			 * };
+			 */
 
-			reboot@0 {
+			/*
-				compatible = "arm,vexpress-reboot";
+			 * Not used - Superseded by PSCI sys_reset
-				arm,vexpress-sysreg,func = <9 0>;
+			 *
-			};
+			 * 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

@@ -0,0 +1,264 @@
+/*
+ * 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";
+			 * Not supported in FVP models
-				arm,vexpress-sysreg,func = <5 0>;
+			 *
-			};
+			 * 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";
+			 * Not used - Superseded by PSCI sys_poweroff
-				arm,vexpress-sysreg,func = <8 0>;
+			 *
-			};
+			 * shutdown@0 {
+			 * 	compatible = "arm,vexpress-shutdown";
+			 * 	arm,vexpress-sysreg,func = <8 0>;
+			 * };
+			 */
 
-			reboot@0 {
+			/*
-				compatible = "arm,vexpress-reboot";
+			 * Not used - Superseded by PSCI sys_reset
-				arm,vexpress-sysreg,func = <9 0>;
+			 *
-			};
+			 * 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
+ * Number of PSCI calls (above) implemented
- * implemented as yet
  */
-#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

@@ -121,9 +121,22 @@
 /* V2M motherboard system registers & offsets */
 #define VE_SYSREGS_BASE		0x1c010000
 #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 */

plat/fvp/fvp_pm.c

@@ -33,6 +33,7 @@
 #include <assert.h>
 #include <bakery_lock.h>
 #include <cci400.h>
+#include <debug.h>
 #include <mmio.h>
 #include <platform.h>
 #include <plat_config.h>
@@ -364,17 +365,41 @@ int fvp_affinst_suspend_finish(unsigned long mpidr,
 	return fvp_affinst_on_finish(mpidr, afflvl, state);
 }
 
+/*******************************************************************************
+ * FVP handlers to shutdown/reboot the system
+ ******************************************************************************/
+static void __dead2 fvp_system_off(void)
+{
+	/* Write the System Configuration Control Register */
+	mmio_write_32(VE_SYSREGS_BASE + V2M_SYS_CFGCTRL,
+		CFGCTRL_START | CFGCTRL_RW | CFGCTRL_FUNC(FUNC_SHUTDOWN));
+	wfi();
+	ERROR("FVP System Off: operation not handled.\n");
+	panic();
+}
+
+static void __dead2 fvp_system_reset(void)
+{
+	/* Write the System Configuration Control Register */
+	mmio_write_32(VE_SYSREGS_BASE + V2M_SYS_CFGCTRL,
+		CFGCTRL_START | CFGCTRL_RW | CFGCTRL_FUNC(FUNC_REBOOT));
+	wfi();
+	ERROR("FVP System Reset: operation not handled.\n");
+	panic();
+}
 
 /*******************************************************************************
  * Export the platform handlers to enable psci to invoke them
  ******************************************************************************/
 static const plat_pm_ops_t fvp_plat_pm_ops = {
-	fvp_affinst_standby,
+	.affinst_standby = fvp_affinst_standby,
-	fvp_affinst_on,
+	.affinst_on = fvp_affinst_on,
-	fvp_affinst_off,
+	.affinst_off = fvp_affinst_off,
-	fvp_affinst_suspend,
+	.affinst_suspend = fvp_affinst_suspend,
-	fvp_affinst_on_finish,
+	.affinst_on_finish = fvp_affinst_on_finish,
-	fvp_affinst_suspend_finish,
+	.affinst_suspend_finish = fvp_affinst_suspend_finish,
+	.system_off = fvp_system_off,
+	.system_reset = fvp_system_reset
 };
 
 /*******************************************************************************

services/spd/tspd/tspd_main.c

@@ -437,6 +437,8 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
 	 */
 	case TSP_OFF_DONE:
 	case TSP_SUSPEND_DONE:
+	case TSP_SYSTEM_OFF_DONE:
+	case TSP_SYSTEM_RESET_DONE:
 		if (ns)
 			SMC_RET1(handle, SMC_UNK);
 

services/spd/tspd/tspd_pm.c

@@ -192,17 +192,60 @@ static int32_t tspd_cpu_migrate_info(uint64_t *resident_cpu)
 	return TSP_MIGRATE_INFO;
 }
 
+/*******************************************************************************
+ * System is about to be switched off. Allow the TSPD/TSP to perform
+ * any actions needed.
+ ******************************************************************************/
+static void tspd_system_off(void)
+{
+	uint64_t mpidr = read_mpidr();
+	uint32_t linear_id = platform_get_core_pos(mpidr);
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
+
+	/* Program the entry point */
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->system_off_entry);
+
+	/* Enter the TSP. We do not care about the return value because we
+	 * must continue the shutdown anyway */
+	tspd_synchronous_sp_entry(tsp_ctx);
+}
+
+/*******************************************************************************
+ * System is about to be reset. Allow the TSPD/TSP to perform
+ * any actions needed.
+ ******************************************************************************/
+static void tspd_system_reset(void)
+{
+	uint64_t mpidr = read_mpidr();
+	uint32_t linear_id = platform_get_core_pos(mpidr);
+	tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+
+	assert(tsp_vectors);
+	assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON);
+
+	/* Program the entry point */
+	cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->system_reset_entry);
+
+	/* Enter the TSP. We do not care about the return value because we
+	 * must continue the reset anyway */
+	tspd_synchronous_sp_entry(tsp_ctx);
+}
+
 /*******************************************************************************
  * Structure populated by the TSP Dispatcher to be given a chance to perform any
  * TSP bookkeeping before PSCI executes a power mgmt.  operation.
  ******************************************************************************/
 const spd_pm_ops_t tspd_pm = {
-	tspd_cpu_on_handler,
+	.svc_on = tspd_cpu_on_handler,
-	tspd_cpu_off_handler,
+	.svc_off = tspd_cpu_off_handler,
-	tspd_cpu_suspend_handler,
+	.svc_suspend = tspd_cpu_suspend_handler,
-	tspd_cpu_on_finish_handler,
+	.svc_on_finish = tspd_cpu_on_finish_handler,
-	tspd_cpu_suspend_finish_handler,
+	.svc_suspend_finish = tspd_cpu_suspend_finish_handler,
-	NULL,
+	.svc_migrate = NULL,
-	tspd_cpu_migrate_info
+	.svc_migrate_info = tspd_cpu_migrate_info,
+	.svc_system_off = tspd_system_off,
+	.svc_system_reset = tspd_system_reset
 };
-

services/std_svc/psci/psci_common.c

@@ -446,3 +446,33 @@ void psci_register_spd_pm_hook(const spd_pm_ops_t *pm)
 {
 	psci_spd_pm = pm;
 }
+
+/*******************************************************************************
+ * This function prints the state of all affinity instances present in the
+ * system
+ ******************************************************************************/
+void psci_print_affinity_map(void)
+{
+#if LOG_LEVEL >= LOG_LEVEL_INFO
+	aff_map_node_t *node;
+	unsigned int idx;
+	/* This array maps to the PSCI_STATE_X definitions in psci.h */
+	static const char *psci_state_str[] = {
+		"ON",
+		"OFF",
+		"ON_PENDING",
+		"SUSPEND"
+	};
+
+	INFO("PSCI Affinity Map:\n");
+	for (idx = 0; idx < PSCI_NUM_AFFS ; idx++) {
+		node = &psci_aff_map[idx];
+		if (!(node->state & PSCI_AFF_PRESENT)) {
+			continue;
+		}
+		INFO("  AffInst: Level %u, MPID 0x%lx, State %s\n",
+				node->level, node->mpidr,
+				psci_state_str[psci_get_state(node)]);
+	}
+#endif
+}

services/std_svc/psci/psci_main.c

@@ -250,6 +250,14 @@ uint64_t psci_smc_handler(uint32_t smc_fid,
 		case PSCI_MIG_INFO_UP_CPU_AARCH32:
 			SMC_RET1(handle, psci_migrate_info_up_cpu());
 
+		case PSCI_SYSTEM_OFF:
+			psci_system_off();
+			/* We should never return from psci_system_off() */
+
+		case PSCI_SYSTEM_RESET:
+			psci_system_reset();
+			/* We should never return from psci_system_reset() */
+
 		default:
 			break;
 		}

services/std_svc/psci/psci_private.h

@@ -99,6 +99,7 @@ void psci_acquire_afflvl_locks(int start_afflvl,
 void psci_release_afflvl_locks(int start_afflvl,
 				int end_afflvl,
 				mpidr_aff_map_nodes_t mpidr_nodes);
+void psci_print_affinity_map(void);
 
 /* Private exported functions from psci_setup.c */
 int psci_get_aff_map_nodes(unsigned long mpidr,
@@ -132,4 +133,8 @@ unsigned int psci_afflvl_suspend_finish(int, int);
 void psci_do_pwrdown_cache_maintenance(uint32_t affinity_level);
 void psci_do_pwrup_cache_maintenance(void);
 
+/* Private exported functions from psci_system_off.c */
+void __dead2 psci_system_off(void);
+void __dead2 psci_system_reset(void);
+
 #endif /* __PSCI_PRIVATE_H__ */

services/std_svc/psci/psci_system_off.c

@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 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.
+ */
+
+#include <stddef.h>
+#include <arch_helpers.h>
+#include <debug.h>
+#include <platform.h>
+#include "psci_private.h"
+
+void psci_system_off(void)
+{
+	/* Check platform support */
+	if (!psci_plat_pm_ops->system_off) {
+		ERROR("Platform has not exported a PSCI System Off hook.\n");
+		panic();
+	}
+
+	psci_print_affinity_map();
+
+	/* Notify the Secure Payload Dispatcher */
+	if (psci_spd_pm && psci_spd_pm->svc_system_off) {
+		psci_spd_pm->svc_system_off();
+	}
+
+	/* Call the platform specific hook */
+	psci_plat_pm_ops->system_off();
+
+	/* This function does not return. We should never get here */
+}
+
+void psci_system_reset(void)
+{
+	/* Check platform support */
+	if (!psci_plat_pm_ops->system_reset) {
+		ERROR("Platform has not exported a PSCI System Reset hook.\n");
+		panic();
+	}
+
+	psci_print_affinity_map();
+
+	/* Notify the Secure Payload Dispatcher */
+	if (psci_spd_pm && psci_spd_pm->svc_system_reset) {
+		psci_spd_pm->svc_system_reset();
+	}
+
+	/* Call the platform specific hook */
+	psci_plat_pm_ops->system_reset();
+
+	/* This function does not return. We should never get here */
+}