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