; ============================================================================
; hdgl_smp_substrate.asm
; ============================================================================
;
; WHAT THIS DOES:
;   Before Alpine handoff, the BSP (Bootstrap Processor) wakes a secondary CPU
;   core via the APIC INIT/SIPI sequence.
;   The AP (Application Processor) enters 64-bit mode and runs the float4096
;   analog substrate tick loop forever.
;   Alpine runs normally on the BSP (core 0).
;   The substrate runs concurrently on the AP (core 1).
;
;   Shared memory at 0x7000 carries the live substrate state to Alpine:
;     SHM_STATUS   = 2 (TICKING) when the AP is running
;     SHM_DN_AGG   = live 8-nibble Dn aggregate (updates every tick)
;     SHM_D_SLOTS  = D1..D32 float64 projections (updates every 128 ticks)
;     SHM_D_BITS   = 32-bit binary pool (0xFFFF0000 pattern from the attached spec)
;     SHM_PSI[8]   = per-strand ternary polarity
;
;   ALPINE ACCESS:
;     /dev/mem mmap at 0x7000 (with phi_analog_module.c or direct)
;     /dev/phi0 via phi_analog_module.ko (clean kernel interface)
;
;   QEMU: with -smp 2 this works in emulation. Without SMP, smp_detect returns
;   ZF=1 and the substrate runs in single-threaded timer-interrupt mode instead.
;
; SEAM (BSP side, add to hdgl_router64.asm before alpine_handoff):
;   call .smp_copy_ap_code
;   call .smp_launch_substrate
;   ; then proceed with alpine_handoff as normal
;
; ============================================================================

; ── Shared Memory Map ─────────────────────────────────────────────────────────
SHM_BASE     equ 0x7000
SHM_MAGIC    equ 0x7000     ; dq "HDGLSUBS" = 0x48_44_47_4C_53_55_42_53
SHM_STATUS   equ 0x7008     ; dd: 0=uninit, 1=ready, 2=ticking
SHM_TICK     equ 0x700C     ; dd: tick counter (wraps at 2^32)
SHM_DN_AGG   equ 0x7010     ; dd: 8-nibble Dn aggregate → [0x10100C]
SHM_PSI      equ 0x7014     ; 8 × dd: per-strand ternary PSI (-1/0/+1)
SHM_D_SLOTS  equ 0x7034     ; 32 × dq: D1..D32 float64 projections
SHM_D_BITS   equ 0x7134     ; dd: 32-bit binary pool (0xFFFF0000 pattern)
SHM_TSTAMP   equ 0x7138     ; dq: AP rdtsc at last tick
SHM_F4096_SAMPLE equ 0x7140 ; 213 × u64 = 1704B: float4096 snapshot of m[2048]
; SHM ends at ~0x77E8 — well within the first 4K page.

APIC_BASE_PA equ 0xFEE00000 ; xAPIC memory-mapped base (identity-mapped in our PTs)
APIC_ICR_LO  equ 0x300      ; Interrupt Command Register low  (write → sends IPI)
APIC_ICR_HI  equ 0x310      ; Interrupt Command Register high (destination)

; AP startup address (must be 4K-aligned below 1MB)
; SIPI vector = 0x08 → AP starts at 0x08 × 0x1000 = 0x8000
AP_STARTUP_ADDR equ 0x8000
AP_STACK_TOP    equ 0x6F00  ; AP stack (below 0x7000 SHM region)

STRAND_PSI      equ 0x2A0140
F4096_M_BASE    equ 0x300000
CELL_BYTES      equ 1704
F4096_OUT_BASE  equ 0x1080000

; ─────────────────────────────────────────────────────────────────────────────
; BSP SIDE — called before alpine_handoff
; ─────────────────────────────────────────────────────────────────────────────

; ── smp_detect ────────────────────────────────────────────────────────────────
; OUT: ECX = number of logical CPUs (1 = single-core, ≥2 = SMP available)
;      ZF=1 if single-core (no AP to launch)
.smp_detect:
    push rax
    push rbx
    push rdx
    mov  eax, 1
    cpuid                          ; EDX bit 28 = HTT, EBX bits 23:16 = count
    mov  eax, ebx
    shr  eax, 16
    and  eax, 0xFF                 ; logical CPU count
    mov  ecx, eax
    cmp  ecx, 1                   ; ZF=1 if single-core
    pop  rdx
    pop  rbx
    pop  rax
    ret

; ── smp_copy_ap_code ──────────────────────────────────────────────────────────
; Copy the AP real-mode startup stub to 0x8000.
; Also patches in the GDT pointer locations so the AP can load them.
.smp_copy_ap_code:
    push rsi
    push rdi
    push rcx
    ; Copy AP real-mode stub to 0x8000
    lea  rsi, [.ap_stub_16bit]
    mov  rdi, AP_STARTUP_ADDR
    mov  rcx, .ap_stub_16bit_end - .ap_stub_16bit
    rep  movsb
    ; Write the GDT32 pointer at 0x8100 (AP 16-bit code loads from there)
    ; We store the actual GDT pointer from our runtime binary
    lea  rsi, [gdt32_ptr]          ; gdt32_ptr defined in hdgl_router64.asm
    mov  rdi, 0x8100
    mov  rcx, 6                    ; 2B limit + 4B base
    rep  movsb
    ; Write the GDT64 pointer at 0x8108
    lea  rsi, [gdt64_ptr]          ; gdt64_ptr defined in hdgl_router64.asm
    mov  rdi, 0x8108
    mov  rcx, 10                   ; 2B limit + 8B base
    rep  movsb
    ; CR3 value: write our BSP's CR3 at 0x8114 so AP uses the same page tables
    mov  rax, cr3
    mov  [0x8114], rax
    pop  rcx
    pop  rdi
    pop  rsi
    ret

; ── smp_shm_init ──────────────────────────────────────────────────────────────
; Zero the SHM region and write magic number.
.smp_shm_init:
    push rdi
    push rcx
    push rax
    mov  rdi, SHM_BASE
    xor  rax, rax
    mov  ecx, 0x800 / 8            ; 2KB SHM region
    rep  stosq
    mov  rax, 0x48444C535542530    ; "HDGLSUBS" packed
    mov  qword [SHM_MAGIC], rax
    pop  rax
    pop  rcx
    pop  rdi
    ret

; ── smp_launch_substrate ──────────────────────────────────────────────────────
; Full INIT/SIPI sequence to wake the AP and start the substrate.
; Called from BSP, before alpine_handoff.
.smp_launch_substrate:
    push rax
    push rcx
    push r8

    ; Init SHM
    call .smp_shm_init

    ; Check SMP
    call .smp_detect
    cmp  ecx, 1
    jle  .smp_no_ap               ; single-core: fall through to timer mode

    ; Copy AP startup code
    call .smp_copy_ap_code

    ; APIC ICR at 0xFEE00300 (identity-mapped)
    mov  r8, APIC_BASE_PA

    ; Step 1: INIT IPI to all-except-self
    mov  dword [r8 + APIC_ICR_HI], 0x00000000
    mov  dword [r8 + APIC_ICR_LO], 0x000C4500   ; INIT, level, all-excl-self

    ; Wait ~10ms: spin ~30M cycles (rough, good enough for INIT de-assert)
    mov  ecx, 30000000
.smp_wait_init:
    pause
    dec  ecx
    jnz  .smp_wait_init

    ; Step 2: SIPI #1 (vector 0x08 → AP starts at 0x8000)
    mov  dword [r8 + APIC_ICR_HI], 0x00000000
    mov  dword [r8 + APIC_ICR_LO], 0x000C4608   ; SIPI, vector=0x08

    ; Wait ~200µs
    mov  ecx, 600000
.smp_wait_s1:
    pause
    dec  ecx
    jnz  .smp_wait_s1

    ; Step 3: SIPI #2 (spec requires two for proper wake)
    mov  dword [r8 + APIC_ICR_HI], 0x00000000
    mov  dword [r8 + APIC_ICR_LO], 0x000C4608

    ; Step 4: Wait for AP to signal READY (SHM_STATUS = 1)
    mov  ecx, 30000000             ; ~30ms timeout
.smp_wait_ready:
    pause
    cmp  dword [SHM_STATUS], 1
    je   .smp_ap_signaled
    dec  ecx
    jnz  .smp_wait_ready
    ; AP didn't respond: fall through to timer mode
    jmp  .smp_no_ap

.smp_ap_signaled:
    ; AP is up and running the substrate loop
    mov  rsi, .msg_smp_ready
    call .com1_str
    jmp  .smp_launch_done

.smp_no_ap:
    ; No AP available: install PIT timer interrupt for substrate tick
    ; (single-core mode: substrate runs in IRQ0 handler at 1kHz)
    call .smp_install_timer_substrate
    mov  rsi, .msg_smp_timer
    call .com1_str

.smp_launch_done:
    pop  r8
    pop  rcx
    pop  rax
    ret

.msg_smp_ready: db "[SMP] AP substrate: TICKING on core 1", 13, 10, 0
.msg_smp_timer: db "[SMP] single-core: substrate via PIT timer", 13, 10, 0

; ── smp_install_timer_substrate ───────────────────────────────────────────────
; Single-core fallback: install PIT timer at IRQ0 to tick the substrate at ~1kHz.
; The timer ISR runs f4096_lattice_tick and updates SHM every interrupt.
.smp_install_timer_substrate:
    push rax
    push rdx
    ; PIT channel 0: mode 2 (rate generator), frequency = 1193182 / divisor
    ; For 1kHz: divisor = 1193
    mov  al, 0x36                  ; channel 0, lobyte/hibyte, mode 3
    out  0x43, al
    mov  ax, 1193
    out  0x40, al                  ; low byte
    mov  al, ah
    out  0x40, al                  ; high byte
    ; Install ISR at IDT vector 0x20 (IRQ0)
    ; In our firmware, interrupts are not used — we poll.
    ; For single-core: run substrate inline at 1/N phi_ticks instead.
    ; Set a flag so analog_lattice_tick calls f4096_lattice_tick every 16 ticks.
    mov  byte [.smp_timer_mode], 1
    pop  rdx
    pop  rax
    ret

.smp_timer_mode: db 0              ; 1 = timer mode (single-core substrate)

; ─────────────────────────────────────────────────────────────────────────────
; AP SIDE — 16-bit real-mode stub copied to 0x8000
; This is what the AP runs immediately after receiving the SIPI.
; It enters 64-bit mode and falls into the substrate loop.
; ─────────────────────────────────────────────────────────────────────────────

[BITS 16]
.ap_stub_16bit:
    cli
    xor  ax, ax
    mov  ds, ax
    mov  es, ax
    mov  ss, ax
    mov  sp, 0x7EF0               ; small real-mode stack for AP init

    ; A20 (already on from BSP; belt-and-suspenders)
    in   al, 0x92
    or   al, 0x02
    out  0x92, al

    ; Load GDT32 (written to 0x8100 by smp_copy_ap_code)
    lgdt [0x8100]

    ; Protected mode
    mov  eax, cr0
    or   eax, 0x01
    mov  cr0, eax
    jmp  0x08:.ap_pm32

[BITS 32]
.ap_pm32:
    mov  ax, 0x10
    mov  ds, ax
    mov  es, ax
    mov  ss, ax
    mov  esp, 0x7EEC

    ; Enable PAE + SSE2 (identical to BSP sequence)
    mov  eax, cr4
    or   eax, 0x620               ; PAE(5) + OSFXSR(9) + OSXMMEXCPT(10)
    mov  cr4, eax

    ; EFER.LME
    mov  ecx, 0xC0000080
    rdmsr
    or   eax, 0x100
    wrmsr

    ; CR3 from BSP (written to 0x8114 by smp_copy_ap_code)
    mov  eax, [0x8114]
    mov  cr3, eax

    ; Paging + MP (not EM)
    mov  eax, cr0
    and  eax, 0xFFFFFFFB
    or   eax, 0x80000002
    mov  cr0, eax

    ; Load GDT64 (written to 0x8108 by smp_copy_ap_code)
    lgdt [0x8108]
    jmp  0x08:.ap_lm64

[BITS 64]
.ap_lm64:
    mov  ax, 0x10
    mov  ds, ax
    mov  es, ax
    mov  ss, ax
    ; AP stack below SHM region
    mov  rsp, AP_STACK_TOP

    ; SSE2 enable (same as BSP)
    mov  rax, cr0
    and  eax, 0xFFFFFFFB
    or   eax, 0x00000002
    mov  cr0, rax
    mov  rax, cr4
    or   rax, 0x00000600
    mov  cr4, rax

    ; Signal READY to BSP
    mov  dword [SHM_STATUS], 1

    ; Run substrate loop
    jmp  .ap_substrate_loop

; ── AP substrate loop: ticks continuously, writes SHM ────────────────────────
.ap_substrate_loop:
    mov  dword [SHM_STATUS], 2    ; TICKING

.ap_tick:
    ; Run one complete float4096 lattice tick
    call .f4096_lattice_tick      ; from hdgl_float4096_arith.asm

    ; Update SHM_TICK
    inc  dword [SHM_TICK]

    ; SHM_DN_AGG: the Dn aggregate from fold_to_digital
    mov  eax, [0x10100C]
    mov  [SHM_DN_AGG], eax

    ; SHM_PSI[0..7]: per-strand ternary polarity
    xor  ecx, ecx
.ap_psi_update:
    cmp  ecx, 8
    jge  .ap_psi_done
    mov  r8, rcx
    shl  r8, 9                    ; strand_index × 512 = base of strand's PSI bytes
    xor  ebx, ebx
    mov  edx, 512
.ap_psi_inner:
    movsx eax, byte [STRAND_PSI + r8 + rdx - 1]
    add  ebx, eax
    dec  edx
    jnz  .ap_psi_inner
    test ebx, ebx
    js   .ap_psi_neg
    jz   .ap_psi_zero
    mov  dword [SHM_PSI + rcx*4], 1
    jmp  .ap_psi_cont
.ap_psi_neg:
    mov  dword [SHM_PSI + rcx*4], 0xFFFFFFFF
    jmp  .ap_psi_cont
.ap_psi_zero:
    mov  dword [SHM_PSI + rcx*4], 0
.ap_psi_cont:
    inc  ecx
    jmp  .ap_psi_update
.ap_psi_done:

    ; SHM_D_SLOTS + SHM_D_BITS: D1..D32 projections and binary pool
    ; D-slots map to glyph instances 0..31 (Strands A-H, 4 per strand)
    xor  r9d, r9d                 ; D-slot index 0..31
    xor  r10d, r10d               ; binary accumulator

.ap_dslot_update:
    cmp  r9d, 32
    jge  .ap_dslot_done

    ; Project m[r9] from float4096 to f64
    mov  r8, r9
    imul r8, CELL_BYTES
    lea  rsi, [r8 + F4096_M_BASE]
    call .f4096_to_f64            ; OUT: xmm0 = f64 value
    movsd [SHM_D_SLOTS + r9*8], xmm0

    ; Binary threshold: xmm0 > sqrt(phi)?
    mov  rax, 0x3FF45A3146A88456  ; sqrt(phi) CORRECTED
    mov  [rsp-8], rax
    movsd xmm1, [rsp-8]
    comisd xmm0, xmm1
    jb   .ap_dslot_zero
    mov  eax, 1
    mov  ecx, r9d
    shl  eax, cl
    or   r10d, eax
.ap_dslot_zero:
    inc  r9d
    jmp  .ap_dslot_update

.ap_dslot_done:
    mov  [SHM_D_BITS], r10d

    ; SHM_TSTAMP: rdtsc for profiling
    rdtsc
    shl  rdx, 32
    or   rax, rdx
    mov  [SHM_TSTAMP], rax

    ; SHM_F4096_SAMPLE: one full float4096 cell for debugging (cell 2048, mid-point)
    ; Copy every 256 ticks to avoid SHM contention
    test dword [SHM_TICK], 0xFF
    jnz  .ap_no_sample
    lea  rsi, [2048 * CELL_BYTES + F4096_M_BASE]
    mov  rdi, SHM_F4096_SAMPLE
    mov  ecx, CELL_BYTES / 8
    rep  movsq
.ap_no_sample:

    ; Memory fence: ensure writes visible to BSP before it reads SHM
    mfence

    jmp  .ap_tick

.ap_stub_16bit_end:

; stubs (resolved in flat binary)
.f4096_lattice_tick: ret
.f4096_to_f64:       pxor xmm0, xmm0
                     ret
.com1_str:           ret
