143 lines
3.5 KiB
C
143 lines
3.5 KiB
C
#include <arch/acpi.h>
|
|
#include <socks/printk.h>
|
|
#include <socks/vm.h>
|
|
#include <socks/cpu.h>
|
|
|
|
extern uint8_t acpi_bsp_lapic_id(void);
|
|
extern char ap_trampoline[];
|
|
|
|
/*
|
|
static int __used send_ipi(void *lapic, unsigned int target_id, uint32_t payload)
|
|
{
|
|
uintptr_t lapic_ptr = (uintptr_t)lapic;
|
|
|
|
*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_STATUS_REG)) = 0;
|
|
*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_DEST_REG)) = (*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_DEST_REG)) & 0x00ffffff) | (target_id << 24);
|
|
*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_ICR_REG)) = (*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_ICR_REG)) & 0xfff00000) | payload;
|
|
do { __asm__ __volatile__ ("pause" : : : "memory"); }while(*((volatile uint32_t*)(lapic_ptr + LAPIC_IPI_ICR_REG)) & (1 << 12));
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int __used init_ap(struct acpi_madt_record *rec, void *bsp_lapic, uint8_t bsp_id)
|
|
{
|
|
struct lapic_record *lapic = (struct lapic_record *)(rec + 1);
|
|
if (!(lapic->l_flags & 0x1) && !(lapic->l_flags & 0x2)) {
|
|
return -1;
|
|
}
|
|
|
|
if (lapic->l_apic_id == bsp_id) {
|
|
printk("acpi: core %u online [BSP]", lapic->l_apic_id);
|
|
return 0;
|
|
}
|
|
|
|
send_ipi(bsp_lapic, lapic-> l_apic_id, 0xC500);
|
|
send_ipi(bsp_lapic, lapic-> l_apic_id, 0x8500);
|
|
send_ipi(bsp_lapic, lapic-> l_apic_id, 0x4600 | (AP_TRAMPOLINE_PADDR >> VM_PAGE_SHIFT));
|
|
|
|
printk("acpi: core %u online [AP]", lapic->l_apic_id);
|
|
return 0;
|
|
}
|
|
*/
|
|
|
|
/*
|
|
kern_status_t acpi_smp_init(void)
|
|
{
|
|
struct acpi_madt *madt = (struct acpi_madt *)acpi_find_sdt(ACPI_SIG_MADT);
|
|
if (!madt) {
|
|
return KERN_UNSUPPORTED;
|
|
}
|
|
|
|
uint8_t bsp_id = acpi_bsp_lapic_id();
|
|
void *bsp_lapic = find_lapic(madt);
|
|
void *ap_trampoline_dest = vm_phys_to_virt(AP_TRAMPOLINE_PADDR);
|
|
memcpy(ap_trampoline_dest, ap_trampoline, VM_PAGE_SIZE);
|
|
|
|
unsigned char *p = (unsigned char *)madt + sizeof *madt;
|
|
unsigned char *madt_end = (unsigned char *)madt + madt->m_base.s_length;
|
|
|
|
unsigned int nr_processors = 0;
|
|
|
|
while (p < madt_end) {
|
|
struct acpi_madt_record *rec = (struct acpi_madt_record *)p;
|
|
|
|
switch (rec->r_type) {
|
|
case ACPI_MADT_LAPIC:
|
|
if (init_ap(rec, bsp_lapic, bsp_id) == 0) {
|
|
nr_processors++;
|
|
}
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
|
|
p += rec->r_length;
|
|
}
|
|
|
|
printk("acpi: found %u logical cores", nr_processors);
|
|
return KERN_OK;
|
|
}
|
|
*/
|
|
|
|
static void no_smp_config(void)
|
|
{
|
|
cpu_set_available(0);
|
|
cpu_set_online(0);
|
|
|
|
ml_cpu_block *self = ml_this_cpu();
|
|
self->c_cpu_id = 0;
|
|
}
|
|
|
|
kern_status_t acpi_scan_cpu_topology(void)
|
|
{
|
|
struct acpi_madt *madt = (struct acpi_madt *)acpi_find_sdt(ACPI_SIG_MADT);
|
|
if (!madt) {
|
|
no_smp_config();
|
|
return KERN_UNSUPPORTED;
|
|
}
|
|
|
|
uint8_t bsp_id = acpi_bsp_lapic_id();
|
|
|
|
//void *bsp_lapic = find_lapic(madt);
|
|
|
|
unsigned char *p = (unsigned char *)madt + sizeof *madt;
|
|
unsigned char *madt_end = (unsigned char *)madt + madt->m_base.s_length;
|
|
|
|
unsigned int nr_processors = 0;
|
|
|
|
while (p < madt_end) {
|
|
struct acpi_madt_record *rec = (struct acpi_madt_record *)p;
|
|
|
|
if (rec->r_type != ACPI_MADT_LAPIC) {
|
|
p += rec->r_length;
|
|
continue;
|
|
}
|
|
|
|
struct lapic_record *lapic = (struct lapic_record *)(rec + 1);
|
|
|
|
if (!(lapic->l_flags & 0x1) && !(lapic->l_flags & 0x2)) {
|
|
/* processor cannot be used */
|
|
continue;
|
|
}
|
|
|
|
cpu_set_available(lapic->l_apic_id);
|
|
nr_processors++;
|
|
|
|
p += rec->r_length;
|
|
}
|
|
|
|
cpu_set_online(bsp_id);
|
|
|
|
/* store the BSP ID in the current cpu block */
|
|
ml_cpu_block *self = ml_this_cpu();
|
|
self->c_cpu_id = bsp_id;
|
|
|
|
printk("acpi: found %u logical cores", nr_processors);
|
|
return KERN_OK;
|
|
}
|
|
|
|
kern_status_t smp_init(void)
|
|
{
|
|
return KERN_OK;
|
|
}
|