acpi: calibrate local APIC timer on BSP

arch/x86_64/acpi/ap_trampoline.S

@@ -1,4 +1,3 @@
-
 	.global acpi_bsp_lapic_id
 	.type acpi_bsp_lapic_id, @function
 
@@ -18,11 +17,66 @@ acpi_bsp_lapic_id:
 	pop %rbp
 	ret
 
-	.global ap_trampoline
-	.type ap_trampoline, @function
-	.code16
 
-ap_trampoline:
-	mov $0xFF, %ax
+	.extern __ap_stack_top
+	.extern __bsp_done
+
+	.extern ap_startup
+	.type ap_startup, @function
+
+	.global ap_trampoline_code
+
+	# this code will be relocated to 0x8000, sets up environment for calling a C function
+	.code16
+ap_trampoline_code:
 	cli
+	cld
+	ljmp    $0, $0x8040
+	.align 16
+_L8010_GDT_table:
+	.long 0, 0
+	.long 0x0000FFFF, 0x00CF9A00    # flat code
+	.long 0x0000FFFF, 0x008F9200    # flat data
+	.long 0x00000068, 0x00CF8900    # tss
+_L8030_GDT_value:
+	.word _L8030_GDT_value - _L8010_GDT_table - 1
+	.long 0x8010
+	.long 0, 0
+	.align 64
+_L8040:
+	xorw    %ax, %ax
+	movw    %ax, %ds
+	lgdtl   0x8030
+	movl    %cr0, %eax
+	orl     $1, %eax
+	movl    %eax, %cr0
+	ljmp    $8, $0x8060
+
+	.align 32
+	.code32
+_L8060:
+	movw    $16, %ax
+	movw    %ax, %ds
+	movw    %ax, %ss
+	# get our Local APIC ID
+	mov     $1, %eax
+	cpuid
+	shrl    $24, %ebx
+	movl    %ebx, %edi
+
+	movl    $0x9000, %eax
+	movl    (%eax), %eax
+	mov     %eax, %cr3
 	hlt
+	# set up 32k stack, one for each core. It is important that all core must have its own stack
+	#shll    $15, %ebx
+	#movl    __ap_stack_top, %esp
+	#subl    %ebx, %esp
+	#pushl   %edi
+	# spinlock, wait for the BSP to finish
+#1:  	pause
+	#cmpb    $0, __bsp_done
+	#jz      1b
+	#lock    incb aprunning
+	# jump into C code (should never return)
+	#ljmp    $8, $ap_startup

arch/x86_64/acpi/apic.cpp

@@ -119,6 +119,28 @@ static void ioapic_init(uintptr_t base, unsigned int int_base)
 	queue_push_back(&io_apics, &apic->io_entry);
 }
 
+void local_apic_config_timer(void)
+{
+	local_apic lapic(lapic_base);
+	lapic.write(local_apic::TIMER_DIV, 0x3);
+	lapic.write(local_apic::TIMER_INITCOUNT, (uint32_t)-1);
+	pit_wait(100);
+	lapic.write(local_apic::LVT_TIMER, APIC_LVT_INT_MASKED);
+
+	/* mask IRQ0 to block interrupts from the PIT. */
+	io_apic *irq0_apic = get_ioapic_for_irq(0);
+	irq0_apic->mask_irq(0);
+
+	uint32_t total_ticks = 0xFFFFFFFF - lapic.read(local_apic::TIMER_CURCOUNT);
+
+	total_ticks /= 100;
+	printk("total_ticks=%u", total_ticks);
+
+	lapic.write(local_apic::LVT_TIMER, IRQ0 | APIC_LVT_TIMER_MODE_PERIODIC);
+	lapic.write(local_apic::TIMER_DIV, 0x3);
+	lapic.write(local_apic::TIMER_INITCOUNT, total_ticks);
+}
+
 static void parse_legacy_irq_override(struct acpi_madt *madt)
 {
 	unsigned char *p = (unsigned char *)madt + sizeof *madt;
@@ -193,8 +215,11 @@ kern_status_t apic_init(void)
         disable_8259();
 
 	apic_enabled = 1;
+	irq_enable();
+
+	pit_start(1000);
+	local_apic_config_timer();
 
-	pit_start(10);
 	printk("acpi: all APIC online");
 	return KERN_OK;
 }

arch/x86_64/acpi/io_apic.cpp

@@ -62,4 +62,30 @@ void io_apic::map_irq(unsigned int src, unsigned int dest)
 
 	write_irq(src, irq);
 }
+
+void io_apic::mask_irq(unsigned int vec)
+{
+	irq_entry irq;
+	if (read_irq(vec, irq) != KERN_OK) {
+		return;
+	}
+
+	irq.irq_mask = 1;
+	write_irq(vec, irq);
+}
+
+void io_apic::unmask_irq(unsigned int vec, unsigned int dest_cpu)
+{
+	irq_entry irq;
+	if (read_irq(vec, irq) != KERN_OK) {
+		return;
+	}
+
+	irq.irq_mask = 0;
+	if (dest_cpu != (unsigned int)-1) {
+		irq.irq_dest = dest_cpu;
+	}
+
+	write_irq(vec, irq);
+}
 }

arch/x86_64/acpi/smp.c

@@ -6,6 +6,7 @@
 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;
@@ -30,15 +31,14 @@ static int __used init_ap(struct acpi_madt_record *rec, void *bsp_lapic, uint8_t
 		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;
 }
+*/
 
 /*
 kern_status_t acpi_smp_init(void)

arch/x86_64/include/arch/acpi/io_apic.hpp

@@ -33,6 +33,8 @@ namespace arch::acpi {
 		kern_status_t read_irq(unsigned int index, irq_entry &entry);
 		void write_irq(unsigned int index, const irq_entry &entry);
 		void map_irq(unsigned int src, unsigned int dest);
+		void mask_irq(unsigned int vec);
+		void unmask_irq(unsigned int vec, unsigned int dest_cpu = (unsigned int)-1);
 	};
 }
 

arch/x86_64/include/arch/acpi/local_apic.hpp

@@ -3,11 +3,22 @@
 
 #include <stdint.h>
 
+#define APIC_LVT_INT_MASKED 0x10000
+#define APIC_LVT_TIMER_MODE_PERIODIC 0x20000
+
 namespace arch::acpi {
 	class local_apic {
 		uint32_t *base_ = nullptr;
 
 	public:
+		enum {
+			EOI = 0xB0,
+			LVT_TIMER = 320,
+			TIMER_INITCOUNT = 0x380,
+			TIMER_CURCOUNT = 0x390,
+			TIMER_DIV = 0x3E0,
+		};
+
 		local_apic(uint32_t *base);
 
 		uint32_t read(uint32_t reg);

arch/x86_64/include/socks/machine/cpu.h

@@ -9,6 +9,9 @@ extern "C" {
 #endif
 
 #define ml_cpu_block_get_id(p) ((p)->c_cpu_id)
+#define ml_cpu_block_get_data(p) ((p)->c_data)
+
+struct cpu_data;
 
 typedef struct ml_cpu_block {
 	struct ml_cpu_block *c_this;
@@ -18,6 +21,8 @@ typedef struct ml_cpu_block {
 
 	struct idt_ptr c_idt_ptr;
 	unsigned int c_cpu_id;
+
+	struct cpu_data *c_data;
 } ml_cpu_block;
 
 #define ml_cpu_pause() __asm__ __volatile__("hlt")

arch/x86_64/init.c

@@ -61,6 +61,7 @@ int ml_init(uintptr_t arg)
 	cpu_data_t *this_cpu = get_this_cpu();
 	this_cpu->c_flags = CPU_ONLINE;
 	this_cpu->c_id = this_cpu();
+	g_bootstrap_cpu.c_data = this_cpu;
 	put_cpu(this_cpu);
 
 	vm_zone_descriptor_t vm_zones[] = {