[PATCH 3/6 gnumach] smp: Remove hardcoded AP_BOOT_ADDR

[Damien Zammit]

This took some time to figure out.
Involves a hand-crafted 16 bit assembly instruction [1]
because it requires an immediate for the memory address
of far jump.  This required self-modifying code
to inject the next instruction, therefore I added a near
jump to clear the instruction cache queue in case the pipeline
cached the unmodified jump location.

[1] Intel Architecture Software Developer's Manual,
    Volume 2: Instruction Set Reference Manual
---
 i386/i386/cpuboot.S     | 36 +++++++++++++++++++++++++++++-------
 i386/i386/mp_desc.c     |  5 +++--
 i386/i386/mp_desc.h     |  7 +++++--
 i386/i386at/model_dep.c | 18 ++++++++++++++++++
 4 files changed, 55 insertions(+), 11 deletions(-)

diff --git a/i386/i386/cpuboot.S b/i386/i386/cpuboot.S
index 13d9160e..b2f9e520 100644
--- a/i386/i386/cpuboot.S
+++ b/i386/i386/cpuboot.S
@@ -23,15 +23,14 @@
 #include <i386/seg.h>
 #include <i386/gdt.h>
 
-#define AP_BOOT_ADDR   0x7000
-#define M(addr)                (addr - apboot + AP_BOOT_ADDR)
+#define M(addr)                (addr - apboot)
 #define CR0_CLEAR_FLAGS_CACHE_ENABLE   (CR0_CD | CR0_NW)
 #define CR0_SET_FLAGS  (CR0_CLEAR_FLAGS_CACHE_ENABLE | CR0_PE)
-#define CR0_CLEAR_FLAGS (CR0_PG | CR0_AM | CR0_WP | CR0_NE | CR0_TS | CR0_EM | 
CR0_MP)
+#define CR0_CLEAR_FLAGS        (CR0_PG | CR0_AM | CR0_WP | CR0_NE | CR0_TS | 
CR0_EM | CR0_MP)
 #define BOOT_CS                0x8
 #define BOOT_DS                0x10
 
-.text
+.data
 
 .align 16
 apboot_idt_ptr:
@@ -101,16 +100,22 @@ apboot_percpu_med:
 apboot_percpu_high:
        .byte 0
 
-.globl apboot, apbootend
+.globl apboot, apbootend, gdt_descr_tmp
 .align 16
 .code16
 
 apboot:
 _apboot:
+       /* This is now address CS:0 in real mode */
+
+       /* Set data seg same as code seg */
+       mov     %cs, %dx
+       mov     %dx, %ds
+
        cli
        xorl    %eax, %eax
        movl    %eax, %cr3
-       mov     %ax, %ds
+
        mov     %ax, %es
        mov     %ax, %fs
        mov     %ax, %gs
@@ -123,9 +128,26 @@ _apboot:
        orl     $CR0_SET_FLAGS, %eax
        movl    %eax, %cr0
 
-       ljmp    $BOOT_CS, $M(0f)
+       xorl    %eax, %eax
+       mov     %cs, %ax
+       shll    $4, %eax
+       addl    $M(0f), %eax
+       movl    %eax, M(ljmp_offset32)
+
+       /* Flush cached instruction queue */
+       jmp     1f
+1:
+
+       /* ljmpl with relocation */
+       .byte 0x66
+       .byte 0xea
+ljmp_offset32:
+       .long 0xffffffff
+       .word BOOT_CS
+
 0:
        .code32
+       /* Protected mode! */
        movw    $BOOT_DS, %ax
        movw    %ax, %ds
        movw    %ax, %es
diff --git a/i386/i386/mp_desc.c b/i386/i386/mp_desc.c
index 860bbd9e..071aa292 100644
--- a/i386/i386/mp_desc.c
+++ b/i386/i386/mp_desc.c
@@ -99,6 +99,7 @@ interrupt_stack_alloc(void)
  */
 int bspdone;
 
+phys_addr_t apboot_addr;
 extern void *apboot, *apbootend;
 extern volatile ApicLocalUnit* lapic;
 
@@ -297,7 +298,7 @@ cpu_start(int cpu)
 
     printf("Trying to enable: %d\n", apic_id);
 
-    smp_startup_cpu(apic_id, AP_BOOT_ADDR);
+    smp_startup_cpu(apic_id, apboot_addr);
 
     printf("Started cpu %d (lapic id %04x)\n", cpu, apic_id);
 
@@ -310,7 +311,7 @@ start_other_cpus(void)
        int ncpus = smp_get_numcpus();
 
        //Copy cpu initialization assembly routine
-       memcpy((void*)phystokv(AP_BOOT_ADDR), (void*) &apboot,
+       memcpy((void*) phystokv(apboot_addr), (void*) &apboot,
               (uint32_t)&apbootend - (uint32_t)&apboot);
 
        unsigned cpu;
diff --git a/i386/i386/mp_desc.h b/i386/i386/mp_desc.h
index fea42cd3..bcc68662 100644
--- a/i386/i386/mp_desc.h
+++ b/i386/i386/mp_desc.h
@@ -46,8 +46,6 @@
 #include "gdt.h"
 #include "ldt.h"
 
-#define AP_BOOT_ADDR   0x7000
-
 /*
  * The descriptor tables are together in a structure
  * allocated one per processor (except for the boot processor).
@@ -78,6 +76,11 @@ extern uint8_t solid_intstack[];
 
 extern int bspdone;
 
+/*
+ * Address to hold AP boot code, held in ASM
+ */
+extern phys_addr_t apboot_addr;
+
 /*
  * Each CPU calls this routine to set up its descriptor tables.
  */
diff --git a/i386/i386at/model_dep.c b/i386/i386at/model_dep.c
index 9dbe7e01..e0995c96 100644
--- a/i386/i386at/model_dep.c
+++ b/i386/i386at/model_dep.c
@@ -66,6 +66,7 @@
 #include <i386/locore.h>
 #include <i386/model_dep.h>
 #include <i386/smp.h>
+#include <i386/seg.h>
 #include <i386at/acpi_parse_apic.h>
 #include <i386at/autoconf.h>
 #include <i386at/biosmem.h>
@@ -125,6 +126,9 @@ char *kernel_cmdline = "";
 
 extern char    version[];
 
+/* Realmode temporary GDT */
+extern struct pseudo_descriptor gdt_descr_tmp;
+
 /* If set, reboot the system on ctrl-alt-delete.  */
 boolean_t      rebootflag = FALSE;     /* exported to kdintr */
 
@@ -207,6 +211,20 @@ void machine_init(void)
         */
        pmap_unmap_page_zero();
 #endif
+
+#ifdef APIC
+       /*
+        * Grab an early page for AP boot code
+        */
+       /* FIXME: this may not allocate from below 1MB, if within first 16MB */
+       apboot_addr = vm_page_to_pa(vm_page_grab_contig(PAGE_SIZE, 
VM_PAGE_SEL_DMA));
+       assert (apboot_addr < 0x100000);
+
+       /*
+        * Patch the realmode gdt with the correct offset
+        */
+       gdt_descr_tmp.linear_base += apboot_addr;
+#endif
 }
 
 /* Conserve power on processor CPU.  */
-- 
2.43.0