1/*
2 * Copyright 2020, Data61, CSIRO (ABN 41 687 119 230)
3 *
4 * SPDX-License-Identifier: GPL-2.0-only
5 */
6#include <elfloader_common.h>
7#include <devices_gen.h>
8#include <drivers/common.h>
9#include <drivers/smp.h>
10#include <armv/machine.h>
11#include <psci.h>
12#include <armv/smp.h>
13
14#include <printf.h>
15#include <types.h>
16
17static int smp_psci_cpu_on(UNUSED struct elfloader_device *dev,
18                           UNUSED struct elfloader_cpu *cpu, UNUSED void *entry, UNUSED void *stack)
19{
20#if CONFIG_MAX_NUM_NODES > 1
21    if (cpu->extra_data == PSCI_METHOD_HVC) {
22        printf("HVC is not supported for PSCI!\n");
23        return -1;
24    }
25    secondary_data.entry = entry;
26    secondary_data.stack = stack;
27    dmb();
28    int ret = psci_cpu_on(cpu->cpu_id, (unsigned long)&secondary_startup, 0);
29    if (ret != PSCI_SUCCESS) {
30        printf("Failed to bring up core 0x%x with error %d\n", cpu->cpu_id, ret);
31        return -1;
32    }
33
34    return 0;
35#else
36    return -1;
37#endif
38}
39
40static int smp_psci_init(struct elfloader_device *dev,
41                         UNUSED void *match_data)
42{
43    smp_register_handler(dev);
44    return 0;
45}
46
47
48static const struct dtb_match_table smp_psci_matches[] = {
49    { .compatible = "arm,psci-0.2" },
50    { .compatible = "arm,psci-1.0" },
51    { .compatible = NULL /* sentinel */ },
52};
53
54static const struct elfloader_smp_ops smp_psci_ops = {
55    .enable_method = "psci",
56    .cpu_on = &smp_psci_cpu_on,
57};
58
59static const struct elfloader_driver smp_psci = {
60    .match_table = smp_psci_matches,
61    .type = DRIVER_SMP,
62    .init = &smp_psci_init,
63    .ops = &smp_psci_ops,
64};
65
66ELFLOADER_DRIVER(smp_psci);
67