mango/arch/x86_64/acpi/smp.c

#include <arch/acpi.h>
#include <socks/printk.h>
#include <socks/vm.h>
#include <socks/cpu.h>

#define AP_TRAMPOLINE_PADDR   0x8000
#define LAPIC_IPI_STATUS_REG  0x0280
#define LAPIC_IPI_DEST_REG    0x0310
#define LAPIC_IPI_ICR_REG     0x0300

struct acpi_madt_record {
	uint8_t r_type;
	uint8_t r_length;
} __packed;

struct lapic_record {
	uint8_t l_cpu_id;
	uint8_t l_apic_id;
	uint32_t l_flags;
} __packed;

struct lapic_override_record {
	uint16_t l_reserved;
	uint64_t l_lapic_ptr;
} __packed;

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 init IPI */
	send_ipi(bsp_lapic, lapic-> l_apic_id, 0xC500);
	send_ipi(bsp_lapic, lapic-> l_apic_id, 0x8500);
	/* send startup IPI */
	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;
}

static void *__used find_lapic(struct acpi_madt *madt)
{
	phys_addr_t local_apic = madt->m_lapic_ptr;

	unsigned char *p = (unsigned char *)madt + sizeof *madt;
	unsigned char *madt_end = (unsigned char *)madt + madt->m_base.s_length;

	while (p < madt_end) {
		struct acpi_madt_record *rec = (struct acpi_madt_record *)p;
		struct lapic_override_record *lapic;

		switch (rec->r_type) {
		case ACPI_MADT_LAPIC_OVERRIDE:
			lapic = (struct lapic_override_record *)(rec + 1);
			return vm_phys_to_virt(lapic->l_lapic_ptr);
		default:
			break;
		}

		p += rec->r_length;
	}

	return vm_phys_to_virt(local_apic);
}

/*
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;
}
*/

kern_status_t acpi_scan_cpu_topology(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);

	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;
}