On Tue, Apr 20, 2021 at 09:00:02PM +0200, Andrew Jones wrote:
> The method can also be smc and it will be when running on bare metal.
>
> Signed-off-by: Andrew Jones <drjones@xxxxxxxxxx>
> ---
> arm/cstart.S | 22 ++++++++++++++++++++++
> arm/cstart64.S | 22 ++++++++++++++++++++++
> arm/selftest.c | 34 +++++++---------------------------
> lib/arm/asm/psci.h | 10 ++++++++--
> lib/arm/psci.c | 37 +++++++++++++++++++++++++++++--------
> lib/arm/setup.c | 2 ++
> 6 files changed, 90 insertions(+), 37 deletions(-)
>
> diff --git a/arm/cstart.S b/arm/cstart.S
> index 446966de350d..2401d92cdadc 100644
> --- a/arm/cstart.S
> +++ b/arm/cstart.S
> @@ -95,6 +95,28 @@ start:
>
> .text
>
> +/*
> + * psci_invoke_hvc / psci_invoke_smc
> + *
> + * Inputs:
> + * r0 -- function_id
> + * r1 -- arg0
> + * r2 -- arg1
> + * r3 -- arg2
> + *
> + * Outputs:
> + * r0 -- return code
> + */
> +.globl psci_invoke_hvc
> +psci_invoke_hvc:
> + hvc #0
> + mov pc, lr
> +
> +.globl psci_invoke_smc
> +psci_invoke_smc:
> + smc #0
> + mov pc, lr
> +
> enable_vfp:
> /* Enable full access to CP10 and CP11: */
> mov r0, #(3 << 22 | 3 << 20)
> diff --git a/arm/cstart64.S b/arm/cstart64.S
> index 42ba3a3ca249..7610e28f06dd 100644
> --- a/arm/cstart64.S
> +++ b/arm/cstart64.S
> @@ -109,6 +109,28 @@ start:
>
> .text
>
> +/*
> + * psci_invoke_hvc / psci_invoke_smc
> + *
> + * Inputs:
> + * x0 -- function_id
> + * x1 -- arg0
> + * x2 -- arg1
> + * x3 -- arg2
> + *
> + * Outputs:
> + * x0 -- return code
> + */
> +.globl psci_invoke_hvc
> +psci_invoke_hvc:
> + hvc #0
> + ret
> +
> +.globl psci_invoke_smc
> +psci_invoke_smc:
> + smc #0
> + ret
> +
> get_mmu_off:
> adrp x0, auxinfo
> ldr x0, [x0, :lo12:auxinfo + 8]
> diff --git a/arm/selftest.c b/arm/selftest.c
> index 4495b161cdd5..9f459ed3d571 100644
> --- a/arm/selftest.c
> +++ b/arm/selftest.c
> @@ -400,33 +400,13 @@ static void check_vectors(void *arg __unused)
> exit(report_summary());
> }
>
> -static bool psci_check(void)
> +static void psci_print(void)
> {
> - const struct fdt_property *method;
> - int node, len, ver;
> -
> - node = fdt_node_offset_by_compatible(dt_fdt(), -1, "arm,psci-0.2");
> - if (node < 0) {
> - printf("PSCI v0.2 compatibility required\n");
> - return false;
> - }
> -
> - method = fdt_get_property(dt_fdt(), node, "method", &len);
> - if (method == NULL) {
> - printf("bad psci device tree node\n");
> - return false;
> - }
> -
> - if (len < 4 || strcmp(method->data, "hvc") != 0) {
> - printf("psci method must be hvc\n");
> - return false;
> - }
> -
> - ver = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
> - printf("PSCI version %d.%d\n", PSCI_VERSION_MAJOR(ver),
> - PSCI_VERSION_MINOR(ver));
> -
> - return true;
> + int ver = psci_invoke(PSCI_0_2_FN_PSCI_VERSION, 0, 0, 0);
> + report_info("PSCI version: %d.%d", PSCI_VERSION_MAJOR(ver),
> + PSCI_VERSION_MINOR(ver));
> + report_info("PSCI method: %s", psci_invoke == psci_invoke_hvc ?
> + "hvc" : "smc");
> }
>
> static void cpu_report(void *data __unused)
> @@ -465,7 +445,7 @@ int main(int argc, char **argv)
>
> } else if (strcmp(argv[1], "smp") == 0) {
>
> - report(psci_check(), "PSCI version");
> + psci_print();
> on_cpus(cpu_report, NULL);
> while (!cpumask_full(&ready))
> cpu_relax();
> diff --git a/lib/arm/asm/psci.h b/lib/arm/asm/psci.h
> index 7b956bf5987d..2820c0a3afc7 100644
> --- a/lib/arm/asm/psci.h
> +++ b/lib/arm/asm/psci.h
> @@ -3,8 +3,14 @@
> #include <libcflat.h>
> #include <linux/psci.h>
>
> -extern int psci_invoke(unsigned long function_id, unsigned long arg0,
> - unsigned long arg1, unsigned long arg2);
> +typedef int (*psci_invoke_fn)(unsigned long function_id, unsigned long arg0,
> + unsigned long arg1, unsigned long arg2);
> +extern psci_invoke_fn psci_invoke;
> +extern int psci_invoke_hvc(unsigned long function_id, unsigned long arg0,
> + unsigned long arg1, unsigned long arg2);
> +extern int psci_invoke_smc(unsigned long function_id, unsigned long arg0,
> + unsigned long arg1, unsigned long arg2);
Hmm, I forgot to change function_id to 'unsigned int'.
> +extern void psci_set_conduit(void);
> extern int psci_cpu_on(unsigned long cpuid, unsigned long entry_point);
> extern void psci_system_reset(void);
> extern int cpu_psci_cpu_boot(unsigned int cpu);
> diff --git a/lib/arm/psci.c b/lib/arm/psci.c
> index 936c83948b6a..168786dcf792 100644
> --- a/lib/arm/psci.c
> +++ b/lib/arm/psci.c
> @@ -6,22 +6,23 @@
> *
> * This work is licensed under the terms of the GNU LGPL, version 2.
> */
> +#include <devicetree.h>
> #include <asm/psci.h>
> #include <asm/setup.h>
> #include <asm/page.h>
> #include <asm/smp.h>
>
> -__attribute__((noinline))
> -int psci_invoke(unsigned long function_id, unsigned long arg0,
> - unsigned long arg1, unsigned long arg2)
> +extern void halt(void);
And, this is a left over from an earlier version of psci_invoke_none.
Will fix these things for v3.
> +
> +static int psci_invoke_none(unsigned long function_id, unsigned long arg0,
> + unsigned long arg1, unsigned long arg2)
> {
> - asm volatile(
> - "hvc #0"
> - : "+r" (function_id)
> - : "r" (arg0), "r" (arg1), "r" (arg2));
> - return function_id;
> + printf("No PSCI method configured! Can't invoke...\n");
> + return PSCI_RET_NOT_PRESENT;
> }
>
> +psci_invoke_fn psci_invoke = psci_invoke_none;
> +
> int psci_cpu_on(unsigned long cpuid, unsigned long entry_point)
> {
> #ifdef __arm__
> @@ -56,3 +57,23 @@ void psci_system_off(void)
> int err = psci_invoke(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0);
> printf("CPU%d unable to do system off (error = %d)\n", smp_processor_id(), err);
> }
> +
> +void psci_set_conduit(void)
> +{
> + const void *fdt = dt_fdt();
> + const struct fdt_property *method;
> + int node, len;
> +
> + node = fdt_node_offset_by_compatible(fdt, -1, "arm,psci-0.2");
> + assert_msg(node >= 0, "PSCI v0.2 compatibility required");
> +
> + method = fdt_get_property(fdt, node, "method", &len);
> + assert(method != NULL && len == 4);
> +
> + if (strcmp(method->data, "hvc") == 0)
> + psci_invoke = psci_invoke_hvc;
> + else if (strcmp(method->data, "smc") == 0)
> + psci_invoke = psci_invoke_smc;
> + else
> + assert_msg(false, "Unknown PSCI conduit: %s", method->data);
> +}
> diff --git a/lib/arm/setup.c b/lib/arm/setup.c
> index a5ebec3c5a12..07d52d2e5fe6 100644
> --- a/lib/arm/setup.c
> +++ b/lib/arm/setup.c
> @@ -25,6 +25,7 @@
> #include <asm/processor.h>
> #include <asm/smp.h>
> #include <asm/timer.h>
> +#include <asm/psci.h>
>
> #include "io.h"
>
> @@ -266,6 +267,7 @@ void setup(const void *fdt, phys_addr_t freemem_start)
> mem_regions_add_assumed();
> mem_init(PAGE_ALIGN((unsigned long)freemem));
>
> + psci_set_conduit();
> cpu_init();
>
> /* cpu_init must be called before thread_info_init */
> --
> 2.30.2
>
Thanks,
drew
