static int
psci_attach(device_t dev)
{
struct psci_softc *sc = device_get_softc(dev);
const struct ofw_compat_data *ocd;
psci_initfn_t psci_init;
phandle_t node;
if (psci_softc != NULL)
return (ENXIO);
ocd = ofw_bus_search_compatible(dev, compat_data);
psci_init = (psci_initfn_t)ocd->ocd_data;
KASSERT(psci_init != NULL, ("PSCI init function cannot be NULL"));
node = ofw_bus_get_node(dev);
sc->psci_call = psci_get_callfn(node);
if (sc->psci_call == NULL)
return (ENXIO);
if (psci_init(dev))
return (ENXIO);
psci_softc = sc;
return (0);
}
void __init smp_init_cpus(void)
{
int rc;
/* initialize PSCI and set a global variable */
psci_init();
if ( (rc = arch_smp_init()) < 0 )
{
printk(XENLOG_WARNING "SMP init failed (%d)\n"
"Using only 1 CPU\n", rc);
return;
}
if ( acpi_disabled )
dt_smp_init_cpus();
else
acpi_smp_init_cpus();
}
/* Parse the device tree and build the logical map array containing
* MPIDR values related to logical cpus
* Code base on Linux arch/arm/kernel/devtree.c
*/
void __init smp_init_cpus(void)
{
register_t mpidr;
struct dt_device_node *cpus = dt_find_node_by_path("/cpus");
struct dt_device_node *cpu;
unsigned int i, j;
unsigned int cpuidx = 1;
static u32 tmp_map[NR_CPUS] __initdata =
{
[0 ... NR_CPUS - 1] = MPIDR_INVALID
};
bool_t bootcpu_valid = 0;
int rc;
/* scan the DTB for a PSCI node and set a global variable */
psci_init();
if ( (rc = arch_smp_init()) < 0 )
{
printk(XENLOG_WARNING "SMP init failed (%d)\n"
"Using only 1 CPU\n", rc);
return;
}
mpidr = boot_cpu_data.mpidr.bits & MPIDR_HWID_MASK;
if ( !cpus )
{
printk(XENLOG_WARNING "WARNING: Can't find /cpus in the device tree.\n"
"Using only 1 CPU\n");
return;
}
dt_for_each_child_node( cpus, cpu )
{
const __be32 *prop;
u64 addr;
u32 reg_len, hwid;
if ( !dt_device_type_is_equal(cpu, "cpu") )
continue;
if ( dt_n_size_cells(cpu) != 0 )
printk(XENLOG_WARNING "cpu node `%s`: #size-cells %d\n",
dt_node_full_name(cpu), dt_n_size_cells(cpu));
prop = dt_get_property(cpu, "reg", ®_len);
if ( !prop )
{
printk(XENLOG_WARNING "cpu node `%s`: has no reg property\n",
dt_node_full_name(cpu));
continue;
}
if ( reg_len < dt_cells_to_size(dt_n_addr_cells(cpu)) )
{
printk(XENLOG_WARNING "cpu node `%s`: reg property too short\n",
dt_node_full_name(cpu));
continue;
}
addr = dt_read_number(prop, dt_n_addr_cells(cpu));
hwid = addr;
if ( hwid != addr )
{
printk(XENLOG_WARNING "cpu node `%s`: hwid overflow %"PRIx64"\n",
dt_node_full_name(cpu), addr);
continue;
}
/*
* 8 MSBs must be set to 0 in the DT since the reg property
* defines the MPIDR[23:0]
*/
if ( hwid & ~MPIDR_HWID_MASK )
{
printk(XENLOG_WARNING "cpu node `%s`: invalid hwid value (0x%x)\n",
dt_node_full_name(cpu), hwid);
continue;
}
/*
* Duplicate MPIDRs are a recipe for disaster. Scan all initialized
* entries and check for duplicates. If any found just skip the node.
* temp values values are initialized to MPIDR_INVALID to avoid
* matching valid MPIDR[23:0] values.
*/
for ( j = 0; j < cpuidx; j++ )
{
if ( tmp_map[j] == hwid )
{
printk(XENLOG_WARNING
"cpu node `%s`: duplicate /cpu reg properties %"PRIx32" in the DT\n",
dt_node_full_name(cpu), hwid);
break;
}
}
if ( j != cpuidx )
continue;
/*
* Build a stashed array of MPIDR values. Numbering scheme requires
* that if detected the boot CPU must be assigned logical id 0. Other
* CPUs get sequential indexes starting from 1. If a CPU node
* with a reg property matching the boot CPU MPIDR is detected,
* this is recorded and so that the logical map build from DT is
* validated and can be used to set the map.
*/
if ( hwid == mpidr )
{
i = 0;
bootcpu_valid = 1;
}
else
i = cpuidx++;
if ( cpuidx > NR_CPUS )
{
printk(XENLOG_WARNING
"DT /cpu %u node greater than max cores %u, capping them\n",
cpuidx, NR_CPUS);
cpuidx = NR_CPUS;
break;
}
if ( (rc = arch_cpu_init(i, cpu)) < 0 )
{
printk("cpu%d init failed (hwid %x): %d\n", i, hwid, rc);
tmp_map[i] = MPIDR_INVALID;
}
else
tmp_map[i] = hwid;
}
if ( !bootcpu_valid )
{
printk(XENLOG_WARNING "DT missing boot CPU MPIDR[23:0]\n"
"Using only 1 CPU\n");
return;
}
for ( i = 0; i < cpuidx; i++ )
{
if ( tmp_map[i] == MPIDR_INVALID )
continue;
cpumask_set_cpu(i, &cpu_possible_map);
cpu_logical_map(i) = tmp_map[i];
}
}