Module Name: src Committed By: jmcneill Date: Wed Jun 28 23:48:23 UTC 2017
Modified Files: src/sys/arch/arm/conf: files.arm src/sys/arch/arm/fdt: files.fdt Added Files: src/sys/arch/arm/arm: psci.c psci.h psci_arm.S src/sys/arch/arm/fdt: psci_fdt.c psci_fdt.h Log Message: Add support for ARM Power State Coordination Interface (PSCI). To generate a diff of this commit: cvs rdiff -u -r0 -r1.1 src/sys/arch/arm/arm/psci.c \ src/sys/arch/arm/arm/psci.h src/sys/arch/arm/arm/psci_arm.S cvs rdiff -u -r1.132 -r1.133 src/sys/arch/arm/conf/files.arm cvs rdiff -u -r1.11 -r1.12 src/sys/arch/arm/fdt/files.fdt cvs rdiff -u -r0 -r1.1 src/sys/arch/arm/fdt/psci_fdt.c \ src/sys/arch/arm/fdt/psci_fdt.h Please note that diffs are not public domain; they are subject to the copyright notices on the relevant files.
Modified files: Index: src/sys/arch/arm/conf/files.arm diff -u src/sys/arch/arm/conf/files.arm:1.132 src/sys/arch/arm/conf/files.arm:1.133 --- src/sys/arch/arm/conf/files.arm:1.132 Fri May 26 21:17:46 2017 +++ src/sys/arch/arm/conf/files.arm Wed Jun 28 23:48:23 2017 @@ -1,4 +1,4 @@ -# $NetBSD: files.arm,v 1.132 2017/05/26 21:17:46 jmcneill Exp $ +# $NetBSD: files.arm,v 1.133 2017/06/28 23:48:23 jmcneill Exp $ # temporary define to allow easy moving to ../arch/arm/arm32 defflag ARM32 @@ -91,6 +91,11 @@ obsolete defflag ARMFPE file arch/arm/vfp/vfp_init.c arm32 #file arch/arm/vfp/pmap_vfp.S arm32 & fpu_vfp +# Power State Coordination Interface (PSCI) +device psci +file arch/arm/arm/psci.c psci +file arch/arm/arm/psci_arm.S psci + # PMAP_DEBUG (heavily abused option) defflag PMAP_DEBUG Index: src/sys/arch/arm/fdt/files.fdt diff -u src/sys/arch/arm/fdt/files.fdt:1.11 src/sys/arch/arm/fdt/files.fdt:1.12 --- src/sys/arch/arm/fdt/files.fdt:1.11 Thu Jun 8 21:01:06 2017 +++ src/sys/arch/arm/fdt/files.fdt Wed Jun 28 23:48:22 2017 @@ -1,4 +1,4 @@ -# $NetBSD: files.fdt,v 1.11 2017/06/08 21:01:06 jmcneill Exp $ +# $NetBSD: files.fdt,v 1.12 2017/06/28 23:48:22 jmcneill Exp $ include "dev/pckbport/files.pckbport" @@ -38,5 +38,8 @@ file arch/arm/fdt/aaci_fdt.c aaci_fdt attach plrtc at fdt with plrtc_fdt file arch/arm/fdt/plrtc_fdt.c plrtc_fdt +attach psci at fdt with psci_fdt +file arch/arm/fdt/psci_fdt.c psci_fdt + # Console parameters defparam opt_fdt_arm.h CONSADDR Added files: Index: src/sys/arch/arm/arm/psci.c diff -u /dev/null src/sys/arch/arm/arm/psci.c:1.1 --- /dev/null Wed Jun 28 23:48:23 2017 +++ src/sys/arch/arm/arm/psci.c Wed Jun 28 23:48:23 2017 @@ -0,0 +1,120 @@ +/* $NetBSD: psci.c,v 1.1 2017/06/28 23:48:23 jmcneill Exp $ */ + +/*- + * Copyright (c) 2017 Jared McNeill <jmcne...@invisible.ca> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 "opt_diagnostic.h" + +#include <sys/cdefs.h> +__KERNEL_RCSID(0, "$NetBSD: psci.c,v 1.1 2017/06/28 23:48:23 jmcneill Exp $"); + +#include <sys/param.h> +#include <sys/bus.h> +#include <sys/cpu.h> +#include <sys/device.h> + +#include <arm/arm/psci.h> + +/* PSCI methods */ +#define PSCI_VERSION 0x84000000 +#define PSCI_SYSTEM_OFF 0x84000008 +#define PSCI_SYSTEM_RESET 0x84000009 +#if defined(__aarch64__) +#define PSCI_CPU_ON 0xc4000002 +#else +#define PSCI_CPU_ON 0x84000002 +#endif + +static psci_fn psci_call_fn; + +static uint32_t psci_functions[PSCI_FUNC_MAX] = { + [PSCI_FUNC_VERSION] = PSCI_VERSION, + [PSCI_FUNC_SYSTEM_OFF] = PSCI_SYSTEM_OFF, + [PSCI_FUNC_SYSTEM_RESET] = PSCI_SYSTEM_RESET, + [PSCI_FUNC_CPU_ON] = PSCI_CPU_ON, +}; + +static int +psci_call(register_t fid, register_t arg1, register_t arg2, register_t arg3) +{ + KASSERT(psci_call_fn != NULL); + + if (fid == 0) + return PSCI_NOT_SUPPORTED; + + return psci_call_fn(fid, arg1, arg2, arg3); +} + +uint32_t +psci_version(void) +{ + if (psci_functions[PSCI_FUNC_VERSION] == 0) { + /* Pre-0.2 implementation */ + return __SHIFTIN(0, PSCI_VERSION_MAJOR) | + __SHIFTIN(1, PSCI_VERSION_MINOR); + } + + return psci_call(psci_functions[PSCI_FUNC_VERSION], 0, 0, 0); +} + +int +psci_cpu_on(register_t target_cpu, register_t entry_point_address, + register_t context_id) +{ + return psci_call(psci_functions[PSCI_FUNC_CPU_ON], target_cpu, + entry_point_address, context_id); +} + +void +psci_system_off(void) +{ + psci_call(psci_functions[PSCI_FUNC_SYSTEM_OFF], 0, 0, 0); +} + +void +psci_system_reset(void) +{ + psci_call(psci_functions[PSCI_FUNC_SYSTEM_RESET], 0, 0, 0); +} + +void +psci_init(psci_fn fn) +{ + psci_call_fn = fn; +} + +void +psci_clearfunc(void) +{ + for (int i = 0; i < PSCI_FUNC_MAX; i++) + psci_setfunc(i, 0); +} + +void +psci_setfunc(enum psci_function func, uint32_t fid) +{ + psci_functions[func] = fid; +} Index: src/sys/arch/arm/arm/psci.h diff -u /dev/null src/sys/arch/arm/arm/psci.h:1.1 --- /dev/null Wed Jun 28 23:48:23 2017 +++ src/sys/arch/arm/arm/psci.h Wed Jun 28 23:48:23 2017 @@ -0,0 +1,106 @@ +/* $NetBSD: psci.h,v 1.1 2017/06/28 23:48:23 jmcneill Exp $ */ + +/*- + * Copyright (c) 2017 Jared McNeill <jmcne...@invisible.ca> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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. + */ + +#ifndef _ARM_PSCI_H +#define _ARM_PSCI_H + +/* + * List of supported PSCI functions. + */ +enum psci_function { + PSCI_FUNC_VERSION, + PSCI_FUNC_CPU_ON, + PSCI_FUNC_SYSTEM_OFF, + PSCI_FUNC_SYSTEM_RESET, + PSCI_FUNC_MAX +}; + +/* + * PSCI error codes + */ +#define PSCI_SUCCESS 0 +#define PSCI_NOT_SUPPORTED -1 +#define PSCI_INVALID_PARAMETERS -2 +#define PSCI_DENIED -3 +#define PSCI_ALREADY_ON -4 +#define PSCI_ON_PENDING -5 +#define PSCI_INTERNAL_FAILURE -6 +#define PSCI_NOT_PRESENT -7 +#define PSCI_DISABLED -8 +#define PSCI_INVALID_ADDRESS -9 + +/* + * PSCI call method interface. + */ +typedef int (*psci_fn)(register_t, register_t, register_t, register_t); + +/* + * Set the PSCI call method. Pass either psci_call_smc or psci_call_hvc. + */ +void psci_init(psci_fn); + +/* + * PSCI call methods, implemented in psci.S + */ +int psci_call_smc(register_t, register_t, register_t, register_t); +int psci_call_hvc(register_t, register_t, register_t, register_t); + +/* + * Clear PSCI function table. The default table includes function IDs for + * PSCI 0.2+. A PSCI 0.1 implementation will use its own function ID mappings. + */ +void psci_clearfunc(void); + +/* + * Set PSCI function ID for a given PSCI function. + */ +void psci_setfunc(enum psci_function, uint32_t); + +/* + * Return the version of PSCI implemented. + */ +uint32_t psci_version(void); +#define PSCI_VERSION_MAJOR __BITS(31,16) +#define PSCI_VERSION_MINOR __BITS(15,0) + +/* + * Power up a core. Args: target_cpu, entry_point_address, context_id + */ +int psci_cpu_on(register_t, register_t, register_t); + +/* + * Shut down the system. + */ +void psci_system_off(void); + +/* + * Reset the system. + */ +void psci_system_reset(void); + +#endif /* _ARM_PSCI_H */ Index: src/sys/arch/arm/arm/psci_arm.S diff -u /dev/null src/sys/arch/arm/arm/psci_arm.S:1.1 --- /dev/null Wed Jun 28 23:48:23 2017 +++ src/sys/arch/arm/arm/psci_arm.S Wed Jun 28 23:48:23 2017 @@ -0,0 +1,47 @@ +/* $NetBSD: psci_arm.S,v 1.1 2017/06/28 23:48:23 jmcneill Exp $ */ + +/*- + * Copyright (c) 2017 Jared McNeill <jmcne...@invisible.ca> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 <machine/asm.h> + +RCSID("$NetBSD: psci_arm.S,v 1.1 2017/06/28 23:48:23 jmcneill Exp $"); + + .arch armv7a + .arch_extension sec + .arch_extension virt + +/* Invoke PSCI function using Secure Monitor Call interface */ +ENTRY(psci_call_smc) + smc #0 + RET +END(psci_call_smc) + +/* Invoke PSCI function using Hypervisor Call interface */ +ENTRY(psci_call_hvc) + hvc #0 + RET +END(psci_call_hvc) Index: src/sys/arch/arm/fdt/psci_fdt.c diff -u /dev/null src/sys/arch/arm/fdt/psci_fdt.c:1.1 --- /dev/null Wed Jun 28 23:48:23 2017 +++ src/sys/arch/arm/fdt/psci_fdt.c Wed Jun 28 23:48:22 2017 @@ -0,0 +1,176 @@ +/* $NetBSD: psci_fdt.c,v 1.1 2017/06/28 23:48:22 jmcneill Exp $ */ + +/*- + * Copyright (c) 2017 Jared McNeill <jmcne...@invisible.ca> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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 <sys/cdefs.h> +__KERNEL_RCSID(0, "$NetBSD: psci_fdt.c,v 1.1 2017/06/28 23:48:22 jmcneill Exp $"); + +#include <sys/param.h> +#include <sys/bus.h> +#include <sys/device.h> +#include <sys/systm.h> +#include <sys/kernel.h> + +#include <dev/fdt/fdtvar.h> + +#include <arm/locore.h> +#include <arm/armreg.h> + +#include <arm/arm/psci.h> +#include <arm/fdt/psci_fdt.h> + +static int psci_fdt_match(device_t, cfdata_t, void *); +static void psci_fdt_attach(device_t, device_t, void *); + +static int psci_fdt_init(const int); + +static const char * const compatible[] = { + "arm,psci", + "arm,psci-0.2", + "arm,psci-1.0", + NULL +}; + +CFATTACH_DECL_NEW(psci_fdt, 0, psci_fdt_match, psci_fdt_attach, NULL, NULL); + +static int +psci_fdt_match(device_t parent, cfdata_t cf, void *aux) +{ + struct fdt_attach_args * const faa = aux; + + return of_match_compatible(faa->faa_phandle, compatible); +} + +static void +psci_fdt_attach(device_t parent, device_t self, void *aux) +{ + struct fdt_attach_args * const faa = aux; + const int phandle = faa->faa_phandle; + + psci_fdt_init(phandle); + + const uint32_t ver = psci_version(); + const u_int ver_maj = __SHIFTOUT(ver, PSCI_VERSION_MAJOR); + const u_int ver_min = __SHIFTOUT(ver, PSCI_VERSION_MINOR); + + aprint_naive("\n"); + aprint_normal(": PSCI %u.%u\n", ver_maj, ver_min); +} + +static int +psci_fdt_init(const int phandle) +{ + char method[4]; + uint32_t val; + + if (!of_hasprop(phandle, "method")) { + aprint_error("PSCI: missing 'method' property\n"); + return EINVAL; + } + + OF_getprop(phandle, "method", method, sizeof(method)); + if (strcmp(method, "smc") == 0) + psci_init(psci_call_smc); + else if (strcmp(method, "hvc") == 0) + psci_init(psci_call_hvc); + else { + aprint_error("PSCI: unsupported method '%s'\n", method); + return EINVAL; + } + + const char * const compat_0_1[] = { "arm,psci", NULL }; + if (of_match_compatible(phandle, compat_0_1)) { + psci_clearfunc(); + if (of_getprop_uint32(phandle, "cpu_on", &val) == 0) + psci_setfunc(PSCI_FUNC_CPU_ON, val); + } + + return 0; +} + +void +psci_fdt_bootstrap(void) +{ + extern void cortex_mpstart(void); + bus_addr_t mpidr; + uint32_t bp_mpidr; + int child; + + const int cpus = OF_finddevice("/cpus"); + if (cpus == -1) { + aprint_error("PSCI: no /cpus node found\n"); + arm_cpu_max = 1; + return; + } + + /* Count CPUs */ + arm_cpu_max = 0; + for (child = OF_child(cpus); child; child = OF_peer(child)) + if (fdtbus_status_okay(child)) + arm_cpu_max++; + + const int phandle = OF_finddevice("/psci"); + if (phandle == -1) { + aprint_error("PSCI: no /psci node found\n"); + return; + } + + if (psci_fdt_init(phandle) != 0) + return; + + /* MPIDR affinity levels of boot processor. */ + bp_mpidr = armreg_mpidr_read() & (MPIDR_AFF2|MPIDR_AFF1|MPIDR_AFF0); + + /* Boot APs */ + uint32_t started = 0; + for (child = OF_child(cpus); child; child = OF_peer(child)) { + if (!fdtbus_status_okay(child)) + continue; + if (fdtbus_get_reg(child, 0, &mpidr, NULL) != 0) + continue; + if (mpidr == bp_mpidr) + continue; /* BP already started */ + + /* XXX NetBSD requires all CPUs to be in the same cluster */ + const u_int bp_clid = __SHIFTOUT(bp_mpidr, CORTEXA9_MPIDR_CLID); + const u_int clid = __SHIFTOUT(mpidr, CORTEXA9_MPIDR_CLID); + if (bp_clid != clid) + continue; + + const u_int cpuid = __SHIFTOUT(mpidr, CORTEXA9_MPIDR_CPUID); + int ret = psci_cpu_on(cpuid, (register_t)cortex_mpstart, 0); + if (ret == PSCI_SUCCESS) + started |= __BIT(cpuid); + } + + /* Wait for APs to start */ + for (u_int i = 0x10000000; i > 0; i--) { + arm_dmb(); + if (arm_cpu_hatched == started) + break; + } +} Index: src/sys/arch/arm/fdt/psci_fdt.h diff -u /dev/null src/sys/arch/arm/fdt/psci_fdt.h:1.1 --- /dev/null Wed Jun 28 23:48:23 2017 +++ src/sys/arch/arm/fdt/psci_fdt.h Wed Jun 28 23:48:22 2017 @@ -0,0 +1,35 @@ +/* $NetBSD: psci_fdt.h,v 1.1 2017/06/28 23:48:22 jmcneill Exp $ */ + +/*- + * Copyright (c) 2017 Jared McNeill <jmcne...@invisible.ca> + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. 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. + * + * THIS SOFTWARE IS PROVIDED BY THE AUTHOR ``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 AUTHOR 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. + */ + +#ifndef _ARM_PSCI_FDT_H +#define _ARM_PSCI_FDT_H + +/* Initialize PSCI and boot APs */ +void psci_fdt_bootstrap(void); + +#endif /* !_ARM_PSCI_FDT_H */