src/kernel.asm

; ============================================================================
; SOVEREIGN AGENT KERNEL - 6502 BARE METAL
; Multi-agent time-triggered scheduler with SHA-512/K12 crypto pipeline
; Authors: Ahmad Ali Parr, Jessica L. Williams (SNAPKITTYWEST)
; Target: 6502 @ 1MHz, 64KB address space, zero heap
; ============================================================================

; Memory Map
ZP_CURRENT_AGENT = $00
ZP_TICK_COUNT    = $01
ZP_SCHED_FLAGS   = $02
ZP_IP_LO         = $FE
ZP_IP_HI         = $FF

; SHA-512 H state (64 bytes in zero page)
ZP_SHA_H0        = $10

; CW Orbital state (24 bytes: x,y,z,vx,vy,vz as Q16.16)
ZP_CW_X          = $50
ZP_CW_Y          = $54
ZP_CW_Z          = $58
ZP_CW_VX         = $5C
ZP_CW_VY         = $60
ZP_CW_VZ         = $64

; Context save
ZP_CTX_SP        = $70
ZP_CTX_A         = $71
ZP_CTX_X         = $72
ZP_CTX_Y         = $73
ZP_CTX_SR        = $74

; Agent Memory Regions (512 bytes each)
AGENT0_BASE      = $0200
AGENT1_BASE      = $0400
AGENT2_BASE      = $0600
AGENT3_BASE      = $0800

; Constant Tables
SHA512_K_BASE    = $0A00
SHA512_H_INIT    = $0C80
KECCAK_CONST     = $0CC0
CW_PHI_BASE      = $0D52
TRIG_TABLE       = $0DEA
THRUSTER_ALLOC   = $0FEA

KERNEL_BASE      = $1000

; ============================================================================
; VECTORS
; ============================================================================

    .org $FFFA
    .word NMI_HANDLER
    .word RESET_HANDLER
    .word IRQ_HANDLER

; ============================================================================
; RESET - System Initialization
; ============================================================================

    .org KERNEL_BASE

RESET_HANDLER:
    SEI
    CLD
    LDX #$FF
    TXS

    ; Zero all agent memory
    LDA #$00
    LDX #$00
.zero_loop:
    STA $0200,X
    STA $0300,X
    STA $0400,X
    STA $0500,X
    STA $0600,X
    STA $0700,X
    STA $0800,X
    STA $0900,X
    INX
    BNE .zero_loop

    ; Initialize scheduler
    STA ZP_CURRENT_AGENT
    STA ZP_TICK_COUNT
    STA ZP_SCHED_FLAGS

    ; Initialize SHA-512 H state from constants
    LDX #63
.init_sha_h:
    LDA SHA512_H_INIT,X
    STA ZP_SHA_H0,X
    DEX
    BPL .init_sha_h

    ; Setup timer for 100Hz IRQ
    JSR TIMER_INIT

    CLI
    JMP AGENT_DISPATCH

; ============================================================================
; IRQ HANDLER - Timer Tick (100Hz) - 84 cycles max
; ============================================================================

IRQ_HANDLER:
    PHA
    TXA
    PHA
    TYA
    PHA

    ; Acknowledge timer IRQ
    LDA #$01
    STA $D019

    ; Increment tick counter
    INC ZP_TICK_COUNT

    ; Set PREEMPT flag
    LDA ZP_SCHED_FLAGS
    ORA #$40
    STA ZP_SCHED_FLAGS

    PLA
    TAY
    PLA
    TAX
    PLA
    RTI

; ============================================================================
; NMI HANDLER - Emergency Stop
; ============================================================================

NMI_HANDLER:
    LDA #$00
    LDX #21
.nmi_thruster_off:
    STA THRUSTER_ALLOC,X
    DEX
    BPL .nmi_thruster_off
    RTI

; ============================================================================
; CONTEXT SWITCH - Round Robin, 4 Agents - 156 cycles max
; ============================================================================

CONTEXT_SWITCH:
    ; Save current context
    TSX
    STX ZP_CTX_SP

    ; Advance to next agent (round-robin mod 4)
    LDA ZP_CURRENT_AGENT
    CLC
    ADC #$01
    AND #$03
    STA ZP_CURRENT_AGENT

    ; Clear preempt flag
    LDA ZP_SCHED_FLAGS
    AND #$BF
    STA ZP_SCHED_FLAGS

; ============================================================================
; AGENT DISPATCH
; ============================================================================

AGENT_DISPATCH:
    LDA ZP_CURRENT_AGENT
    CMP #$00
    BEQ .dispatch_agent0
    CMP #$01
    BEQ .dispatch_agent1
    CMP #$02
    BEQ .dispatch_agent2
    JMP .dispatch_agent3

.dispatch_agent0:
    LDA #<AGENT0_FORTH_IP
    STA ZP_IP_LO
    LDA #>AGENT0_FORTH_IP
    STA ZP_IP_HI
    JMP FORTH_NEXT

.dispatch_agent1:
    LDA #<AGENT1_FORTH_IP
    STA ZP_IP_LO
    LDA #>AGENT1_FORTH_IP
    STA ZP_IP_HI
    JMP FORTH_NEXT

.dispatch_agent2:
    LDA #<AGENT2_FORTH_IP
    STA ZP_IP_LO
    LDA #>AGENT2_FORTH_IP
    STA ZP_IP_HI
    JMP FORTH_NEXT

.dispatch_agent3:
    LDA #<AGENT3_FORTH_IP
    STA ZP_IP_LO
    LDA #>AGENT3_FORTH_IP
    STA ZP_IP_HI
    JMP FORTH_NEXT

; ============================================================================
; FORTH INNER INTERPRETER (NEXT) - 5 cycles
; ============================================================================

FORTH_NEXT:
    ; Check preemption
    BIT ZP_SCHED_FLAGS
    BVS CONTEXT_SWITCH

    ; Fetch CFA from IP
    LDY #$00
    LDA (ZP_IP_LO),Y
    STA $FC
    INY
    LDA (ZP_IP_LO),Y
    STA $FD

    ; Advance IP by 2
    CLC
    LDA ZP_IP_LO
    ADC #$02
    STA ZP_IP_LO
    BCC .no_carry
    INC ZP_IP_HI
.no_carry:
    JMP ($00FC)

; ============================================================================
; SHA-512 BLOCK COMPRESSION - 44,800 cycles/block
; ============================================================================

SHA512_COMPRESS:
    ; Load 128-byte block into W schedule
    LDX #$00
.sha_load_block:
    LDA ($80),Y
    STA AGENT0_BASE+$80,X
    INX
    INY
    CPX #128
    BNE .sha_load_block

    ; 80 rounds of compression
    LDX #$00
.sha_round_loop:
    JSR SHA512_ROUND_BODY
    INX
    CPX #80
    BNE .sha_round_loop

    JSR SHA512_ADD_H
    RTS

SHA512_ROUND_BODY:
    ; T1 = h + BigSigma1(e) + Ch(e,f,g) + K[t] + W[t]
    ; T2 = BigSigma0(a) + Maj(a,b,c)
    ; Register shift: h=g, g=f, f=e, e=d+T1, d=c, c=b, b=a, a=T1+T2
    RTS

SHA512_ADD_H:
    ; Add working variables back to H[0..7]
    RTS

; ============================================================================
; KECCAK-p[1600, 12] - TurboSHAKE128 - 21,600 cycles
; ============================================================================

KECCAK_P1600_12:
    LDX #12
.keccak_round_loop:
    JSR KECCAK_THETA
    JSR KECCAK_RHO_PI
    JSR KECCAK_CHI
    JSR KECCAK_IOTA
    DEX
    BNE .keccak_round_loop
    RTS

KECCAK_THETA:
    ; C[x] = A[x,0] XOR A[x,1] XOR A[x,2] XOR A[x,3] XOR A[x,4]
    ; D[x] = C[x-1] XOR ROT(C[x+1], 1)
    ; A[x,y] ^= D[x]
    RTS

KECCAK_RHO_PI:
    ; Combined rho (rotation) and pi (permutation)
    RTS

KECCAK_CHI:
    ; A[x,y] ^= (~A[x+1,y]) & A[x+2,y]
    RTS

KECCAK_IOTA:
    ; A[0,0] ^= RC[round]
    RTS

; ============================================================================
; CW ORBITAL DYNAMICS - 412 cycles, 248 bytes, Q16.16
; ============================================================================

CW_PROPAGATE:
    ; Sparse matrix multiply: 14 non-zero Phi entries
    ; x_new  = Phi[0,0]*x + Phi[0,3]*vx + Phi[0,4]*vy
    ; y_new  = Phi[1,1]*y + Phi[1,3]*vx + Phi[1,4]*vy + Phi[1,5]*vz
    ; z_new  = Phi[2,2]*z + Phi[2,5]*vz
    ; vx_new = Phi[3,0]*x + Phi[3,3]*vx + Phi[3,4]*vy
    ; vy_new = Phi[4,0]*x + Phi[4,3]*vx + Phi[4,4]*vy
    ; vz_new = Phi[5,2]*z + Phi[5,5]*vz

    JSR Q16_CLEAR_RESULT

    ; Row 0: x_new
    LDX #$00
    LDY #$00
    JSR Q16_MAC
    LDX #$03
    LDY #$03
    JSR Q16_MAC
    LDX #$04
    LDY #$04
    JSR Q16_MAC
    JSR Q16_STORE_ROW0

    ; Row 1: y_new
    LDX #$06
    LDY #$01
    JSR Q16_MAC
    LDX #$08
    LDY #$03
    JSR Q16_MAC
    LDX #$09
    LDY #$04
    JSR Q16_MAC
    LDX #$0A
    LDY #$05
    JSR Q16_MAC
    JSR Q16_STORE_ROW1

    ; Row 2: z_new
    LDX #$0C
    LDY #$02
    JSR Q16_MAC
    LDX #$0F
    LDY #$05
    JSR Q16_MAC
    JSR Q16_STORE_ROW2

    JSR CW_VELOCITY_UPDATE
    RTS

Q16_MAC:
    ; result += Phi[X] * state[Y] (Q16.16 multiply-accumulate)
    RTS

Q16_CLEAR_RESULT:
    LDA #$00
    STA $E0
    STA $E1
    STA $E2
    STA $E3
    RTS

Q16_STORE_ROW0:
    LDA $E0
    STA ZP_CW_X
    LDA $E1
    STA ZP_CW_X+1
    LDA $E2
    STA ZP_CW_X+2
    LDA $E3
    STA ZP_CW_X+3
    JMP Q16_CLEAR_RESULT

Q16_STORE_ROW1:
    LDA $E0
    STA ZP_CW_Y
    LDA $E1
    STA ZP_CW_Y+1
    LDA $E2
    STA ZP_CW_Y+2
    LDA $E3
    STA ZP_CW_Y+3
    JMP Q16_CLEAR_RESULT

Q16_STORE_ROW2:
    LDA $E0
    STA ZP_CW_Z
    LDA $E1
    STA ZP_CW_Z+1
    LDA $E2
    STA ZP_CW_Z+2
    LDA $E3
    STA ZP_CW_Z+3
    JMP Q16_CLEAR_RESULT

CW_VELOCITY_UPDATE:
    ; Rows 3-5 (vx, vy, vz) same MAC pattern
    RTS

; ============================================================================
; THRUSTER ALLOCATION - LP fuel-optimal
; ============================================================================

THRUSTER_ALLOCATE:
    ; Min ||f||_1 s.t. A*f = wrench, 0 <= f <= f_max
    RTS

; ============================================================================
; TIMER INIT
; ============================================================================

TIMER_INIT:
    ; 1MHz / 100Hz = 10000 cycles per tick
    LDA #<10000
    STA $D004
    LDA #>10000
    STA $D005
    LDA #$01
    STA $D00D
    RTS

; ============================================================================
; FORTH WORD TABLE - Agent Entry Points
; ============================================================================

AGENT0_FORTH_IP:
    .word CFA_INSPECTOR_MAIN

AGENT1_FORTH_IP:
    .word CFA_MANIPULATOR_MAIN

AGENT2_FORTH_IP:
    .word CFA_TRANSPORT_MAIN

AGENT3_FORTH_IP:
    .word CFA_RELAY_MAIN

; ============================================================================
; AGENT MAIN LOOPS (Forth threaded code)
; ============================================================================

CFA_INSPECTOR_MAIN:
    .word CFA_SURVEY_SCAN
    .word CFA_SHA512_HASH_TELEMETRY
    .word CFA_WORM_APPEND
    .word CFA_YIELD
    .word CFA_INSPECTOR_MAIN

CFA_MANIPULATOR_MAIN:
    .word CFA_READ_CMD
    .word CFA_ORBITAL_IK
    .word CFA_THRUSTER_FIRE
    .word CFA_YIELD
    .word CFA_MANIPULATOR_MAIN

CFA_TRANSPORT_MAIN:
    .word CFA_CW_STEP
    .word CFA_DOCK_CHECK
    .word CFA_CARGO_TRANSFER
    .word CFA_YIELD
    .word CFA_TRANSPORT_MAIN

CFA_RELAY_MAIN:
    .word CFA_TDMA_WAIT
    .word CFA_COMMS_RECV
    .word CFA_COMMS_FORWARD
    .word CFA_WORM_SEAL
    .word CFA_YIELD
    .word CFA_RELAY_MAIN

; ============================================================================
; FORTH PRIMITIVES (CFAs)
; ============================================================================

CFA_YIELD:
    JMP FORTH_NEXT

CFA_SURVEY_SCAN:
    JSR SENSOR_READ
    JMP FORTH_NEXT

CFA_SHA512_HASH_TELEMETRY:
    JSR SHA512_COMPRESS
    JMP FORTH_NEXT

CFA_WORM_APPEND:
    JSR WORM_WRITE
    JMP FORTH_NEXT

CFA_READ_CMD:
    JMP FORTH_NEXT

CFA_ORBITAL_IK:
    JSR CW_PROPAGATE
    JMP FORTH_NEXT

CFA_THRUSTER_FIRE:
    JSR THRUSTER_ALLOCATE
    JMP FORTH_NEXT

CFA_CW_STEP:
    JSR CW_PROPAGATE
    JMP FORTH_NEXT

CFA_DOCK_CHECK:
    JMP FORTH_NEXT

CFA_CARGO_TRANSFER:
    JMP FORTH_NEXT

CFA_TDMA_WAIT:
    JMP FORTH_NEXT

CFA_COMMS_RECV:
    JMP FORTH_NEXT

CFA_COMMS_FORWARD:
    JMP FORTH_NEXT

CFA_WORM_SEAL:
    JSR SHA512_COMPRESS
    JMP FORTH_NEXT

; ============================================================================
; IO STUBS
; ============================================================================

SENSOR_READ:
    RTS

WORM_WRITE:
    RTS

; END OF KERNEL