/* Copyright © 2019 Michal Schulz https://github.com/michalsc This Source Code Form is subject to the terms of the Mozilla Public License, v. 2.0. If a copy of the MPL was not distributed with this file, You can obtain one at http://mozilla.org/MPL/2.0/. */ #include #include "A64.h" #include "config.h" #include "support.h" #include "mmu.h" #include "tlsf.h" #include "M68k.h" #include "cache.h" #define FULL_CONTEXT 1 #define SAVE_SHORT_CONTEXT \ " stp x0, x1, [sp, -176]! \n" \ " stp x2, x3, [sp, #1*16] \n" \ " stp x4, x5, [sp, #2*16] \n" \ " stp x6, x7, [sp, #3*16] \n" \ " stp x8, x9, [sp, #4*16] \n" \ " stp x10, x11, [sp, #5*16] \n" \ " stp x12, x13, [sp, #6*16] \n" \ " stp x14, x15, [sp, #7*16] \n" \ " stp x16, x17, [sp, #8*16] \n" \ " stp x18, x30, [sp, #9*16] \n" #define SAVE_FULL_CONTEXT \ " stp x0, x1, [sp, -256]! \n" \ " stp x2, x3, [sp, #1*16] \n" \ " stp x4, x5, [sp, #2*16] \n" \ " stp x6, x7, [sp, #3*16] \n" \ " stp x8, x9, [sp, #4*16] \n" \ " stp x10, x11, [sp, #5*16] \n" \ " stp x12, x13, [sp, #6*16] \n" \ " stp x14, x15, [sp, #7*16] \n" \ " stp x16, x17, [sp, #8*16] \n" \ " stp x18, x19, [sp, #9*16] \n" \ " stp x20, x21, [sp, #10*16] \n" \ " stp x22, x23, [sp, #11*16] \n" \ " stp x24, x25, [sp, #12*16] \n" \ " stp x26, x27, [sp, #13*16] \n" \ " stp x28, x29, [sp, #14*16] \n" \ " mov x0, sp \n" \ " stp x30, x0, [sp, #15*16] \n" #define LOAD_FULL_CONTEXT \ " ldp x2, x3, [sp, #1*16] \n" \ " ldp x4, x5, [sp, #2*16] \n" \ " ldp x6, x7, [sp, #3*16] \n" \ " ldp x8, x9, [sp, #4*16] \n" \ " ldp x10, x11, [sp, #5*16] \n" \ " ldp x12, x13, [sp, #6*16] \n" \ " ldp x14, x15, [sp, #7*16] \n" \ " ldp x16, x17, [sp, #8*16] \n" \ " ldp x18, x19, [sp, #9*16] \n" \ " ldp x20, x21, [sp, #10*16] \n" \ " ldp x22, x23, [sp, #11*16] \n" \ " ldp x24, x25, [sp, #12*16] \n" \ " ldp x26, x27, [sp, #13*16] \n" \ " ldp x28, x29, [sp, #14*16] \n" \ " ldr x30, [sp, #15*16] \n" \ " ldp x0, x1, [sp], #256 \n" #define LOAD_SHORT_CONTEXT \ " ldp x2, x3, [sp, #1*16] \n" \ " ldp x4, x5, [sp, #2*16] \n" \ " ldp x6, x7, [sp, #3*16] \n" \ " ldp x8, x9, [sp, #4*16] \n" \ " ldp x10, x11, [sp, #5*16] \n" \ " ldp x12, x13, [sp, #6*16] \n" \ " ldp x14, x15, [sp, #7*16] \n" \ " ldp x16, x17, [sp, #8*16] \n" \ " ldp x18, x30, [sp, #9*16] \n" \ " ldp x0, x1, [sp], #176 \n" #if FULL_CONTEXT #define SAVE_CONTEXT SAVE_FULL_CONTEXT #define LOAD_CONTEXT LOAD_FULL_CONTEXT #else #define SAVE_CONTEXT SAVE_SHORT_CONTEXT #define LOAD_CONTEXT LOAD_SHORT_CONTEXT #endif struct INT_shadow { uint16_t INTENA; uint16_t INTREQ; uint8_t ARMPending; } INT_shadow; void __attribute__((used)) __stub_vectors() { asm volatile( " .section .vectors \n" " .balign 0x800 \n" "curr_el_sp0_sync: \n" // The exception handler for a synchronous SAVE_CONTEXT // exception from the current EL using SP0. " mov x0, #0 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "curr_el_sp0_irq: \n" // The exception handler for an IRQ exception SAVE_CONTEXT // from the current EL using SP0. " mov x0, #0x80 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "curr_el_sp0_fiq: \n" // The exception handler for an FIQ exception SAVE_CONTEXT // from the current EL using SP0. " mov x0, #0x100 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "curr_el_sp0_serror: \n" // The exception handler for a System Error SAVE_CONTEXT // exception from the current EL using SP0. " mov x0, #0x180 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "curr_el_spx_sync: \n" // The exception handler for a synchrous SAVE_CONTEXT // exception from the current EL using the " mov x0, #0x200 \n" // current SP. " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "curr_el_spx_irq: \n" // The exception handler for an IRQ exception from " stp x0, x1, [sp, -16]! \n" // the current EL using the current SP. " mrs x0, SPSR_EL1 \n" // Get SPSR " orr x0, x0, #0x080 \n" // Disable IRQ interrupt so that we are not disturbed on return " msr SPSR_EL1, x0 \n" " adrp x1, INT_shadow \n" // Load INTENA shadow " add x1, x1, :lo12:INT_shadow \n" " ldrh w0, [x1, #%[intena]] \n" " and w0, w0, #0x6000 \n" // Check if INTEN and EXTER are active " cmp w0, #0x6000 \n" " mov w0, #1 \n" // Set ARM int pending so that we can fake INTREQ " strb w0, [x1, #%[armpend]] \n" " b.ne 1f \n" // Skip setting pint register if interrupt was not enabled " mrs x1, TPIDRRO_EL0 \n" // Load CPU context " mov w0, #6 \n" // Set level 6 IRQ " strb w0, [x1, #%[pint]] \n" "1: ldp x0, x1, [sp], #16 \n" // Restore scratch registers " eret \n" " \n" " .balign 0x80 \n" "curr_el_spx_fiq: \n" // The exception handler for an FIQ from " stp x0, x1, [sp, -16]! \n" // the current EL using the current SP. " mrs x0, SPSR_EL1 \n" // Get SPSR " orr x0, x0, #0x0c0 \n" // Disable IRQ and FIQ interrupts so that we are not disturbed on return " msr SPSR_EL1, x0 \n" " adrp x1, INT_shadow \n" // Load INTENA shadow " add x1, x1, :lo12:INT_shadow \n" " ldrh w0, [x1, #%[intena]] \n" " and w0, w0, #0x6000 \n" // Check if INTEN and EXTER are active " cmp w0, #0x6000 \n" " mov w0, #1 \n" // Set ARM int pending so that we can fake INTREQ " strb w0, [x1, #%[armpend]] \n" " b.ne 1f \n" // Skip setting pint register if interrupt was not enabled " mrs x1, TPIDRRO_EL0 \n" // Load CPU context " mov w0, #6 \n" // Set level 6 IRQ " strb w0, [x1, #%[pint]] \n" "1: ldp x0, x1, [sp], #16 \n" // Restore scratch registers " eret \n" " \n" " .balign 0x80 \n" "curr_el_spx_serror: \n" // The exception handler for a System Error " stp x0, x1, [sp, -16]! \n" // exception from the current EL using the " mrs x0, SPSR_EL1 \n" // Get SPSR " orr x0, x0, #0x1c0 \n" // Disable SError, IRQ and FIQ interrupts so that we are not disturbed on return " msr SPSR_EL1, x0 \n" " mrs x1, TPIDRRO_EL0 \n" // Load CPU context " mov w0, #7 \n" // Set level 7 IRQ " strb w0, [x1, #%[perr]] \n" " ldp x0, x1, [sp], #16 \n" // Restore scratch registers " eret \n" " \n" " .balign 0x80 \n" "lower_el_aarch64_sync: \n" // The exception handler for a synchronous SAVE_CONTEXT // exception from a lower EL (AArch64). " mov x0, #0x400 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch64_irq: \n" // The exception handler for an IRQ from a lower EL SAVE_CONTEXT // (AArch64). " mov x0, #0x480 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch64_fiq: \n" // The exception handler for an FIQ from a lower EL SAVE_CONTEXT // (AArch64). " mov x0, #0x500 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch64_serror: \n" // The exception handler for a System Error SAVE_CONTEXT // exception from a lower EL(AArch64). " mov x0, #0x580 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch32_sync: \n" // The exception handler for a synchronous SAVE_CONTEXT // exception from a lower EL (AArch32). " mov x0, #0x600 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch32_irq: \n" // The exception handler for an IRQ from a lower EL SAVE_CONTEXT // (AArch32). " mov x0, #0x680 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch32_fiq: \n" // The exception handler for an FIQ from a lower EL SAVE_CONTEXT // (AArch32). " mov x0, #0x700 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" " .balign 0x80 \n" "lower_el_aarch32_serror: \n" // The exception handler for a System Error SAVE_CONTEXT // exception from a lower EL(AArch32). " mov x0, #0x780 \n" " mov x1, sp \n" " bl SYSHandler \n" " b ExceptionExit \n" " \n" "ExceptionExit: \n" LOAD_CONTEXT " eret \n" " \n" " .section .text \n" : :[pint]"i"(__builtin_offsetof(struct M68KState, INT.ARM)), [perr]"i"(__builtin_offsetof(struct M68KState, INT.ARM_err)), [intena]"i"(__builtin_offsetof(struct INT_shadow, INTENA)), [armpend]"i"(__builtin_offsetof(struct INT_shadow, ARMPending)) );} static int getOPsize(uint32_t opcode) { int size = 0; switch (opcode & 0xc0000000) { case 0x00000000: size = 1; break; case 0x40000000: size = 2; break; case 0x80000000: size = 4; break; case 0xc0000000: size = 8; break; } return size; } #undef D #define D(x) /* x */ #ifdef PISTORM #include "ps_protocol.h" #include int board_idx; struct ExpansionBoard **board; uint32_t rom_mapped = 0; uint32_t overlay = 1; uint32_t z2_ram_autoconf = 1; uint64_t z2_ram_base = 0; uint32_t swap_df0_with_dfx = 0; uint32_t spoof_df0_id = 0; uint32_t move_slow_to_chip = 0; // Amiga specific registers enum { CIAAPRA = 0xBFE001, CIABPRB = 0xBFD100, INTENA = 0xDFF09A, INTENAR = 0xDFF01C, INTREQ = 0xDFF09C, INTREQR = 0xDFF01E, }; int block_c0; extern int zorro_disable; int SYSWriteValToAddr(uint64_t value, uint64_t value2, int size, uint64_t far) { D(kprintf("[JIT:SYS] SYSWriteValToAddr(0x%x, %d, %p)\n", value, size, far)); /* Allow single wrap around the address space. This provides mirror areas for simplified aarch64 pointer arithmetic */ if ((far >> 32) == 1 || (far >> 32) == 0xffffffff) { far &= 0xffffffff; } if (far == INTENA) { if (value & 0x8000) { INT_shadow.INTENA |= value & 0x7fff; } else { INT_shadow.INTENA &= ~(value & 0x7fff); } if (INT_shadow.ARMPending && (INT_shadow.INTENA & 0x6000) == 0x6000) { struct M68KState *ctx; asm volatile("mrs %0, TPIDRRO_EL0\n":"=r"(ctx)); ctx->INT.ARM = 0x01; } } if (far == INTREQ) { if (value & 0x8000) { INT_shadow.INTREQ |= value & 0x3fff; } else { INT_shadow.INTREQ &= ~(value & 0x3fff); } if ((value & 0xa000) == 0x2000) { struct M68KState *ctx; asm volatile("mrs %0, TPIDRRO_EL0\n":"=r"(ctx)); ctx->INT.ARM = 0; INT_shadow.ARMPending = 0; } } if (far == 0xdeadbeef && size == 1) { kprintf("%c", value); return 1; } if (far >= 0xff000000) { kprintf("Z3 write access with far %08x, size %d, value %08x\n", far, size, value); } if (far >= 0x1000000) { return 1; // Unmapped Z3 address } if (far == CIAAPRA && size == 1) { if ((value & 1) != overlay) { kprintf("[JIT:SYS] OVL bit changing to %d\n", value & 1); overlay = value & 1; extern int fast_page0; /* If fast_page_zero is active either map to ROM or to physical ARM RAM at address 0 */ if (fast_page0) { if (overlay) mmu_map(0xf80000, 0x0, 4096, MMU_ACCESS | MMU_ISHARE | MMU_ALLOW_EL0 | MMU_READ_ONLY | MMU_ATTR_CACHED, 0); else mmu_map(0x0, 0x0, 4096, MMU_ACCESS | MMU_ISHARE | MMU_ALLOW_EL0 | MMU_ATTR_CACHED, 0); } } } if (far == CIABPRB) { if (swap_df0_with_dfx) { const int SEL0_BITNUM = 3; if ((value & ((1 << (SEL0_BITNUM + swap_df0_with_dfx)) | 0x80)) == 0x80) { // If drive selected but motor off, Amiga is reading drive ID. spoof_df0_id = 1; } else { spoof_df0_id = 0; } // If the value for SEL0/SELx differ if (((value >> SEL0_BITNUM) & 1) != ((value >> (SEL0_BITNUM + swap_df0_with_dfx)) & 1)) { // Invert both bits to swap them around value ^= ((1 << SEL0_BITNUM) | (1 << (SEL0_BITNUM + swap_df0_with_dfx))); } } } if (!zorro_disable && far >= 0xe80000 && far <= 0xe8ffff && board[board_idx]) { if (board[board_idx]->is_z3) { if (far == 0xe80044) { board[board_idx]->map_base = (value & 0xffff) << 16; board[board_idx]->map(board[board_idx]); board_idx++; } } else { if (far == 0xe80048) { board[board_idx]->map_base = (value & 0xff) << 16; board[board_idx]->map(board[board_idx]); board_idx++; } } if (far == 0xe8004c || far == 0xe8004e) { board_idx++; } return 1; } if (move_slow_to_chip) { if (far >= 0x080000 && far <= 0x0FFFFF) { // A500 JP2 connects Agnus' A19 input to A23 instead of A19 by default, and decodes trapdoor memory at 0xC00000 instead of 0x080000. // We can move the trapdoor to chipram simply by rewriting the address. far += 0xB80000; } else if (far >= 0xC00000 && far <= 0xC7FFFF) { // Block accesses through to trapdoor at slow ram address, otherwise it will be detected at 0x080000 and 0xC00000. return 1; } } if ((far >= 0xc00000 && far <= 0xc7ffff) && block_c0) { return 1; } switch(size) { case 1: ps_write_8(far, value); break; case 2: ps_write_16(far, value); break; case 4: ps_write_32(far, value); break; case 8: ps_write_64(far, value); break; case 16: { uint128_t val; val.hi = value; val.lo = value2; ps_write_128(far, val); break; } } return 1; } int SYSReadValFromAddr(uint64_t *value, uint64_t *value2, int size, uint64_t far) { D(kprintf("[JIT:SYS] SYSReadValFromAddr(%d, %p)\n", size, far)); /* Allow single wrap around the address space. This provides mirror areas for simplified aarch64 pointer arithmetic */ if ((far >> 32) == 1 || (far >> 32) == 0xffffffff) { far &= 0xffffffff; } if (far >= 0x1000000) { // Unmapped Z3 address static const uint64_t u64[5] = { 0xBAD00BAD00BAD00BULL, 0xAD00BAD00BAD00BAULL, 0x00BAD00BAD00BAD0ULL, 0x0BAD00BAD00BAD00ULL }; uintptr_t p64 = (uintptr_t)&u64[2] - (15 - (far & 15)); switch (size) { case 1: *value = *(uint8_t*)p64; break; case 2: *value = *(uint16_t*)p64; break; case 4: *value = *(uint32_t*)p64; break; case 8: *value = *(uint64_t*)p64; break; case 16: *value = *(uint64_t*)p64; *value2 = *(uint64_t*)(p64 + 8); break; } return 1; } if ((far >= 0xc00000 && far <= 0xc7ffff) && block_c0) { *value = 0; return 1; } if (!zorro_disable && far >= 0xe80000 && far <= 0xe8ffff && size == 1) { while(board[board_idx] && !board[board_idx]->enabled) { board_idx++; } if (board[board_idx]) { const uint8_t *rom = board[board_idx]->rom_file; *value = rom[far - 0xe80000]; return 1; } } if (rom_mapped && overlay) { if (far < 0x80000) { switch (size) { case 1: *value = *(uint8_t*)(0xffffff9000e00000 + far); break; case 2: *value = *(uint16_t*)(0xffffff9000e00000 + far); break; case 4: *value = *(uint32_t*)(0xffffff9000e00000 + far); break; case 8: *value = *(uint64_t*)(0xffffff9000e00000 + far); break; } return 1; } } if (move_slow_to_chip) { if (far >= 0x080000 && far <= 0x0FFFFF) { // A500 JP2 connects Agnus' A19 input to A23 instead of A19 by default, and decodes trapdoor memory at 0xC00000 instead of 0x080000. // We can move the trapdoor to chipram simply by rewriting the address. far += 0xB80000; } else if (far >= 0xC00000 && far <= 0xC7FFFF) { // Block accesses through to trapdoor at slow ram address, otherwise it will be detected at 0x080000 and 0xC00000. *value = 0; return 1; } } switch(size) { case 1: *value = ps_read_8(far); break; case 2: *value = ps_read_16(far); break; case 4: *value = ps_read_32(far); break; case 8: *value = ps_read_64(far); break; case 16: { uint128_t v = ps_read_128(far); *value = v.hi; *value2 = v.lo; } } if ((far & ~1) == INTENAR) { if (size == 2) INT_shadow.INTENA = *value; else { if (far & 1) { INT_shadow.INTENA = (INT_shadow.INTENA & 0xff00) | (*value & 0xff); } else { INT_shadow.INTENA = (INT_shadow.INTENA & 0x00ff) | ((*value & 0xff) << 8); } } } if ((far & ~1) == INTREQR) { if (size == 2) { INT_shadow.INTREQ = *value; if (INT_shadow.ARMPending) *value |= 0x2000; } else { if (far & 1) { INT_shadow.INTREQ = (INT_shadow.INTREQ & 0xff00) | (*value & 0xff); } else { INT_shadow.INTREQ = (INT_shadow.INTREQ & 0x00ff) | ((*value & 0xff) << 8); if (INT_shadow.ARMPending) *value |= 0x20; } } } if (far == CIAAPRA) { if (swap_df0_with_dfx && spoof_df0_id) { // DF0 doesn't emit a drive type ID on RDY pin // If swapping DF0 with DF1-3 we need to provide this ID so that DF0 continues to function. *value = (*value & 0xDF); // Spoof drive id for swapped DF0 by setting RDY low } } if (far == CIABPRB) { if (swap_df0_with_dfx) { const int SEL0_BITNUM = 3; // SEL0 = 0x80, SEL1 = 0x10, SEL2 = 0x20, SEL3 = 0x40 // If the value for SEL0/SELx differ if (((*value >> SEL0_BITNUM) & 1) != ((*value >> (SEL0_BITNUM + swap_df0_with_dfx)) & 1)) { // Invert both bits to swap them around *value ^= ((1 << SEL0_BITNUM) | (1 << (SEL0_BITNUM + swap_df0_with_dfx))); } } } return 1; } #else int SYSWriteValToAddr(uint64_t value, uint64_t value2, int size, uint64_t far) { D(kprintf("[JIT:SYS] SYSWriteValToAddr(0x%x, %d, %p)\n", value, size, far)); switch(size) { case 1: *(uint8_t*)(far + 0xffffff9000000000) = value; break; case 2: *(uint16_t*)(far + 0xffffff9000000000) = value; break; case 4: *(uint32_t*)(far + 0xffffff9000000000) = value; break; case 8: *(uint64_t*)(far + 0xffffff9000000000) = value; break; case 16: *(uint64_t*)(far + 0xffffff9000000000) = value; *(uint64_t*)(far + 0xffffff9000000008) = value2; break; } return 1; } int SYSReadValFromAddr(uint64_t *value, uint64_t *value2, int size, uint64_t far) { D(kprintf("[JIT:SYS] SYSReadValFromAddr(%d, %p)\n", size, far)); switch(size) { case 1: *value = *(uint8_t*)(far + 0xffffff9000000000); break; case 2: *value = *(uint16_t*)(far + 0xffffff9000000000); break; case 4: *value = *(uint32_t*)(far + 0xffffff9000000000); break; case 8: *value = *(uint64_t*)(far + 0xffffff9000000000); break; case 16: *value = *(uint64_t*)(far + 0xffffff9000000000); *value2 = *(uint64_t*)(far + 0xffffff9000000008); break; } return 1; } #endif #undef D #define D(x) /* x */ #define DWARN(x) /* x */ int SYSValidateUnit(uint32_t vector, uint64_t *ctx, uint64_t elr, uint64_t spsr, uint64_t esr, uint64_t far) { (void)vector; (void)ctx; (void)elr; (void)spsr; (void)esr; struct M68KTranslationUnit *unit; uint16_t *m68k_pc; uintptr_t corrected_far = far | (0xff00000000000000ULL); // Fix the topmost bits uintptr_t unit_far = corrected_far & ~0x0000001000000000; // Clear the executable region bit /* Adjust far of the M68KTranslationUnit */ unit_far = unit_far - __builtin_offsetof(struct M68KTranslationUnit, mt_ARMCode); unit = (void*)unit_far; m68k_pc = unit->mt_M68kAddress; /* Check the unit. The function will free entry if unit was wrong */ unit = M68K_VerifyUnit(unit); if (unit) { unit->mt_ARMEntryPoint = (void*)corrected_far; elr = corrected_far; asm volatile("msr ELR_EL1, %0"::"r"(elr)); return 1; } else { unit = M68K_GetTranslationUnit(m68k_pc); if (unit) { // Put simple return function to elr asm volatile("msr ELR_EL1, %0"::"r"(unit->mt_ARMEntryPoint)); // Avoid short loop path by invalidating the "last m68k PC" counter. That should trigger full search and translation asm volatile("msr tpidr_el1,%0"::"r"(0xffffffff)); return 1; } } return 0; } uint32_t get_fpn_as_single(int fpn) { uint32_t ret = 0; switch (fpn) { case 0: asm volatile("mov %w0, v0.s[0]":"=r"(ret)); break; case 1: asm volatile("mov %w0, v1.s[0]":"=r"(ret)); break; case 2: asm volatile("mov %w0, v2.s[0]":"=r"(ret)); break; case 3: asm volatile("mov %w0, v3.s[0]":"=r"(ret)); break; case 4: asm volatile("mov %w0, v4.s[0]":"=r"(ret)); break; case 5: asm volatile("mov %w0, v5.s[0]":"=r"(ret)); break; case 6: asm volatile("mov %w0, v6.s[0]":"=r"(ret)); break; case 7: asm volatile("mov %w0, v7.s[0]":"=r"(ret)); break; case 8: asm volatile("mov %w0, v8.s[0]":"=r"(ret)); break; case 9: asm volatile("mov %w0, v9.s[0]":"=r"(ret)); break; case 10: asm volatile("mov %w0, v10.s[0]":"=r"(ret)); break; case 11: asm volatile("mov %w0, v11.s[0]":"=r"(ret)); break; case 12: asm volatile("mov %w0, v12.s[0]":"=r"(ret)); break; case 13: asm volatile("mov %w0, v13.s[0]":"=r"(ret)); break; case 14: asm volatile("mov %w0, v14.s[0]":"=r"(ret)); break; case 15: asm volatile("mov %w0, v15.s[0]":"=r"(ret)); break; default: break; } return ret; } uint64_t get_fpn_as_double(int fpn) { uint64_t ret = 0; switch (fpn) { case 0: asm volatile("mov %0, v0.d[0]":"=r"(ret)); break; case 1: asm volatile("mov %0, v1.d[0]":"=r"(ret)); break; case 2: asm volatile("mov %0, v2.d[0]":"=r"(ret)); break; case 3: asm volatile("mov %0, v3.d[0]":"=r"(ret)); break; case 4: asm volatile("mov %0, v4.d[0]":"=r"(ret)); break; case 5: asm volatile("mov %0, v5.d[0]":"=r"(ret)); break; case 6: asm volatile("mov %0, v6.d[0]":"=r"(ret)); break; case 7: asm volatile("mov %0, v7.d[0]":"=r"(ret)); break; case 8: asm volatile("mov %0, v8.d[0]":"=r"(ret)); break; case 9: asm volatile("mov %0, v9.d[0]":"=r"(ret)); break; case 10: asm volatile("mov %0, v10.d[0]":"=r"(ret)); break; case 11: asm volatile("mov %0, v11.d[0]":"=r"(ret)); break; case 12: asm volatile("mov %0, v12.d[0]":"=r"(ret)); break; case 13: asm volatile("mov %0, v13.d[0]":"=r"(ret)); break; case 14: asm volatile("mov %0, v14.d[0]":"=r"(ret)); break; case 15: asm volatile("mov %0, v15.d[0]":"=r"(ret)); break; default: break; } return ret; } void set_fpn_as_single(int fpn, uint32_t value) { switch (fpn) { case 0: asm volatile("mov v0.s[0], %w0"::"r"(value)); break; case 1: asm volatile("mov v1.s[0], %w0"::"r"(value)); break; case 2: asm volatile("mov v2.s[0], %w0"::"r"(value)); break; case 3: asm volatile("mov v3.s[0], %w0"::"r"(value)); break; case 4: asm volatile("mov v4.s[0], %w0"::"r"(value)); break; case 5: asm volatile("mov v5.s[0], %w0"::"r"(value)); break; case 6: asm volatile("mov v6.s[0], %w0"::"r"(value)); break; case 7: asm volatile("mov v7.s[0], %w0"::"r"(value)); break; case 8: asm volatile("mov v8.s[0], %w0"::"r"(value)); break; case 9: asm volatile("mov v9.s[0], %w0"::"r"(value)); break; case 10: asm volatile("mov v10.s[0], %w0"::"r"(value)); break; case 11: asm volatile("mov v11.s[0], %w0"::"r"(value)); break; case 12: asm volatile("mov v12.s[0], %w0"::"r"(value)); break; case 13: asm volatile("mov v13.s[0], %w0"::"r"(value)); break; case 14: asm volatile("mov v14.s[0], %w0"::"r"(value)); break; case 15: asm volatile("mov v15.s[0], %w0"::"r"(value)); break; default: break; } } void set_fpn_as_double(int fpn, uint64_t value) { switch (fpn) { case 0: asm volatile("mov v0.d[0], %0"::"r"(value)); break; case 1: asm volatile("mov v1.d[0], %0"::"r"(value)); break; case 2: asm volatile("mov v2.d[0], %0"::"r"(value)); break; case 3: asm volatile("mov v3.d[0], %0"::"r"(value)); break; case 4: asm volatile("mov v4.d[0], %0"::"r"(value)); break; case 5: asm volatile("mov v5.d[0], %0"::"r"(value)); break; case 6: asm volatile("mov v6.d[0], %0"::"r"(value)); break; case 7: asm volatile("mov v7.d[0], %0"::"r"(value)); break; case 8: asm volatile("mov v8.d[0], %0"::"r"(value)); break; case 9: asm volatile("mov v9.d[0], %0"::"r"(value)); break; case 10: asm volatile("mov v10.d[0], %0"::"r"(value)); break; case 11: asm volatile("mov v11.d[0], %0"::"r"(value)); break; case 12: asm volatile("mov v12.d[0], %0"::"r"(value)); break; case 13: asm volatile("mov v13.d[0], %0"::"r"(value)); break; case 14: asm volatile("mov v14.d[0], %0"::"r"(value)); break; case 15: asm volatile("mov v15.d[0], %0"::"r"(value)); break; default: break; } } int SYSPageFaultWriteHandler(uint32_t vector, uint64_t *ctx, uint64_t elr, uint64_t spsr, uint64_t esr, uint64_t far) { int handled = 0; int size = 0; uint64_t value = 0; uint32_t opcode = LE32(*(uint32_t *)elr); (void)vector; (void)spsr; if (esr & (1 << 24)) { kprintf("PageFault with valid instruction syndrome: %08x\n", esr); } size = getOPsize(opcode); D(kprintf("[JIT:SYS] Fage fault: opcode %08x, %s %p size %d\n", opcode, "write to", far, size)); /**** MISC ****/ if ((opcode & 0xffffffe0) == 0xd50b7e20) { /* Cache flushes on PiStorm-mapped region, ignore */ handled = 1; } /**** Floating point stores ****/ /* STUR */ if ((opcode & 0xbfe00c00) == 0xbc000000) { if (size == 8) { value = get_fpn_as_double(opcode & 31); } else { value = get_fpn_as_single(opcode & 31); } uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((int16_t)(opcode >> 5)) >> 7; if (ptr != far) { DWARN(kprintf("address mismatch in STUR %c%d. FAR = %08x, reg = %08x!\n", size == 4 ? 'S' : 'D', opcode & 31, far, ptr)); far = ptr; } handled = SYSWriteValToAddr(value, 0, size, far); } /* STS/STD post- and pre-index*/ else if ((opcode & 0xbfe00400) == 0xbc000400) { int pre_index = opcode & 0x800; if (size == 8) { value = get_fpn_as_double(opcode & 31); } else { value = get_fpn_as_single(opcode & 31); } uint64_t ptr = ctx[(opcode >> 5) & 31]; int16_t offset = ((int16_t)(opcode >> 5)) >> 7; // Pre-index? if (pre_index) ptr += offset; if (ptr != far) { DWARN(kprintf("address mismatch in STR %c%d %s-index. FAR = %08x, reg = %08x!\n", size == 4 ? 'S' : 'D', opcode & 31, pre_index ? "pre":"post",far, ptr)); far = ptr; } handled = SYSWriteValToAddr(value, 0, size, far); ctx[(opcode >> 5) & 31] += offset; } /* STS/STD unsigned offset */ else if ((opcode & 0xbfc00000) == 0xbd000000) { uint64_t ptr = ctx[(opcode >> 5) & 31]; if (size == 8) { value = get_fpn_as_double(opcode & 31); } else { value = get_fpn_as_single(opcode & 31); } ptr += ((opcode >> 10) & 0xfff) * size; if (far != ptr) { DWARN(kprintf("address mismatch in STR %c%d unsigned offset FAR = %08x, reg = %08x!\n", size == 4 ? 'S' : 'D', opcode & 31, far, ptr)); far = ptr; } handled = SYSWriteValToAddr(value, 0, size, far); } /* STS/STD reg */ else if ((opcode & 0xbfe04c00) == 0xbc204800) { int option = (opcode >> 13) & 7; int s = (opcode & 0x1000); if (size == 8) { value = get_fpn_as_double(opcode & 31); } else { value = get_fpn_as_single(opcode & 31); } uint64_t ptr = ctx[(opcode >> 5) & 31]; uint64_t rm = ctx[(opcode >> 16) & 31]; switch (option) { case 0b010: // UXTW rm &= 0xffffffffULL; break; case 0b110: // SXTW if (rm & 0x80000000) rm |= 0xffffffff00000000ULL; else rm &= 0x00000000ffffffffULL; break; } if (s) { if (size == 4) rm <<= 2; else rm <<= 3; } if (far != ptr + rm) { DWARN(kprintf("address mismatch in STR %c%d reg FAR = %08x, reg = %08x!\n", size == 4 ? 'S' : 'D', opcode & 31, far, ptr)); far = ptr + rm; } handled = SYSWriteValToAddr(value, 0, size, far); } /**** Integer stores ****/ /* STUR */ else if ((opcode & 0x3fe00c00) == 0x38000000) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((int16_t)(opcode >> 5)) >> 7; if (ptr != far) { kprintf("address mismatch in STUR %c%d. FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, far, ptr); far = ptr; } handled = SYSWriteValToAddr(value, 0, size, far); } /* STR immediate post-/pre-index */ else if ((opcode & 0x3fe00400) == 0x38000400) { int pre_index = opcode & 0x800; uint64_t ptr = ctx[(opcode >> 5) & 31]; int16_t offset = ((int16_t)(opcode >> 5)) >> 7; // Pre-index? if (pre_index) { ptr += offset; } if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; if (ptr != far) { kprintf("address mismatch in STR %c%d %s-index. FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, pre_index ? "pre":"post",far, ptr); far = ptr; } handled = SYSWriteValToAddr(value, 0, size, far); ctx[(opcode >> 5) & 31] += offset; } /* STR unsigned offset */ else if ((opcode & 0x3fc00000) == 0x39000000) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; uint16_t offset = ((opcode >> 10) & 0xfff) * size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { kprintf("address mismatch in STR unsigned offset far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); far = ptr + offset; } handled = SYSWriteValToAddr(value, 0, size, far); } /* STR register */ else if ((opcode & 0x3fe00c00) == 0x38200800) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; int option = (opcode >> 13) & 7; int s = (opcode & 0x1000); uint64_t ptr = ctx[(opcode >> 5) & 31]; uint64_t rm = ctx[(opcode >> 16) & 31]; switch (option) { case 0b010: // UXTW rm &= 0xffffffffULL; break; case 0b110: // SXTW if (rm & 0x80000000) rm |= 0xffffffff00000000ULL; else rm &= 0x00000000ffffffffULL; break; } if (s) { switch(size) { case 2: rm <<= 1; break; case 4: rm <<= 2; break; case 8: rm <<= 3; break; } } if (far != ptr + rm) { kprintf("address mismatch in STR %c%d reg FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, far, ptr); far = ptr + rm; } handled = SYSWriteValToAddr(value, 0, size, far); } /* ST(L)XR register - no exclusive in this case!!! But m68k bus does not support it anyway */ else if ((opcode & 0x3fe07c00) == 0x08007c00) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr != far) { kprintf("address mismatch in STR(L)XR FAR = %08x, reg = %08x!\n", far, ptr); far = ptr; } // Mark the store as successful ctx[(opcode >> 16) & 31] = 0; handled = SYSWriteValToAddr(value, 0, size, far); } /* STP */ else if ((opcode & 0x7fc00000) == 0x29000000) { if (opcode & 0x80000000) size = 8; else size = 4; int16_t offset = size * (((int16_t)(opcode >> 6)) >> 9); uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { kprintf("address mismatch in STP offset far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); // STP may fail on either register access, hence, update FAR accordingly now far = ptr + offset; } if (size == 4) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; value <<= 32; if (((opcode >> 10) & 31) != 31) value |= (ctx[(opcode >> 10) & 31]) & 0xFFFFFFFFULL; handled = SYSWriteValToAddr(value, 0, 8, far); } else { uint128_t v; if ((opcode & 31) == 31) v.hi = 0; else v.hi = ctx[opcode & 31]; if (((opcode >> 10) & 31) == 31) v.lo = 0; else v.lo = ctx[(opcode >> 10) & 31]; handled = SYSWriteValToAddr(v.hi, v.lo, 16, far); } } /* STP post index */ else if ((opcode & 0x7fc00000) == 0x28800000) { int16_t offset = ((int16_t)(opcode >> 6)) >> 9; if (opcode & 0x80000000) size = 8; else size = 4; offset *= size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr != far) { kprintf("address mismatch in STP post index far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); far = ptr; } if (size == 4) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; value <<= 32; if (((opcode >> 10) & 31) != 31) value |= (ctx[(opcode >> 10) & 31]) & 0xFFFFFFFFULL; handled = SYSWriteValToAddr(value, 0, 8, far); } else { uint128_t v; if ((opcode & 31) == 31) v.hi = 0; else v.hi = ctx[opcode & 31]; if (((opcode >> 10) & 31) == 31) v.lo = 0; else v.lo = ctx[(opcode >> 10) & 31]; handled = SYSWriteValToAddr(v.hi, v.lo, 16, far); } ctx[(opcode >> 5) & 31] += offset; } /* STP pre index */ else if ((opcode & 0x7fc00000) == 0x29800000) { int16_t offset = ((int16_t)(opcode >> 6)) >> 9; if (opcode & 0x80000000) size = 8; else size = 4; offset *= size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { kprintf("address mismatch in STP pre index far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); far = ptr + offset; } if (size == 4) { if ((opcode & 31) == 31) value = 0; else value = ctx[opcode & 31]; value <<= 32; if (((opcode >> 10) & 31) != 31) value |= (ctx[(opcode >> 10) & 31]) & 0xFFFFFFFFULL; handled = SYSWriteValToAddr(value, 0, 8, far); } else { uint128_t v; if ((opcode & 31) == 31) v.hi = 0; else v.hi = ctx[opcode & 31]; if (((opcode >> 10) & 31) == 31) v.lo = 0; else v.lo = ctx[(opcode >> 10) & 31]; handled = SYSWriteValToAddr(v.hi, v.lo, 16, far); } ctx[(opcode >> 5) & 31] += offset; } if (!handled) { kprintf("[JIT:SYS] Unhandled page fault: opcode %08x, write to %p\n", opcode, far); } elr += 4; asm volatile("msr ELR_EL1, %0"::"r"(elr)); return handled; } int SYSPageFaultReadHandler(uint32_t vector, uint64_t *ctx, uint64_t elr, uint64_t spsr, uint64_t esr, uint64_t far) { int handled = 0; int size = 0; uint64_t value = 0; uint32_t opcode = LE32(*(uint32_t *)elr); (void)vector; (void)spsr; if (esr & (1 << 24)) { kprintf("PageFault with valid instruction syndrome: %08x\n", esr); } size = getOPsize(opcode); D(kprintf("[JIT:SYS] Fage fault: opcode %08x, %s %p size %d\n", opcode, "read from", far, size)); /**** Floating point loads ****/ /* FLDS */ if ((opcode & 0xfee00c00) == 0xbc400000) { handled = SYSReadValFromAddr(&value, NULL, 4, far); if (handled) { set_fpn_as_single(opcode & 31, value); } } /* FLDS unsigned offset */ else if ((opcode & 0xff400000) == 0xbd400000) { uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((opcode >> 10) & 0xfff) * 4; if (far != ptr) { kprintf("address mismatch in FLDS unsigned offset far = %08x, reg = %08x!\n", far, ptr); far = ptr; } handled = SYSReadValFromAddr(&value, NULL, 4, far); if (handled) { set_fpn_as_single(opcode & 31, value); } } /* FLDS pre-index */ else if ((opcode & 0xfee00c00) == 0xbc400400) { int16_t offset = ((int16_t)(opcode >> 5)) >> 7; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (far != ptr + offset) { kprintf("address mismatch in FLDS pre-index far = %08x, reg = %08x!\n", far, ptr + offset); far = ptr + offset; } handled = SYSReadValFromAddr(&value, NULL, 4, far); if (handled) { set_fpn_as_single(opcode & 31, value); ctx[(opcode >> 5) & 31] += offset; } } /* FLDS post-index */ else if ((opcode & 0xfee00c00) == 0xbc400c00) { int16_t offset = ((int16_t)(opcode >> 5)) >> 7; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (far != ptr) { kprintf("address mismatch in FLDS post-index far = %08x, reg = %08x!\n", far, ptr); far = ptr; } handled = SYSReadValFromAddr(&value, NULL, 4, far); if (handled) { set_fpn_as_single(opcode & 31, value); ctx[(opcode >> 5) & 31] += offset; } } /* FLDD */ else if ((opcode & 0xfee00c00) == 0xfc400000) { handled = SYSReadValFromAddr(&value, NULL, 8, far); if (handled) { set_fpn_as_double(opcode & 31, value); } } /* FLDD unsigned offset */ else if ((opcode & 0xff400000) == 0xfd400000) { uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((opcode >> 10) & 0xfff) * 8; if (far != ptr) { kprintf("address mismatch in FLDD unsigned offset far = %08x, reg = %08x!\n", far, ptr); far = ptr; } handled = SYSReadValFromAddr(&value, NULL, 8, far); if (handled) { set_fpn_as_double(opcode & 31, value); } } /* FLDD reg offset */ else if ((opcode & 0xfee00c00) == 0xfc600800) { handled = SYSReadValFromAddr(&value, NULL, 8, far); if (handled) { set_fpn_as_double(opcode & 31, value); } } /* FLDD pre-index */ else if ((opcode & 0xfee00c00) == 0xfc400400) { int16_t offset = ((int16_t)(opcode >> 5)) >> 7; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (far != ptr + offset) { kprintf("address mismatch in FLDD pre-index far = %08x, reg = %08x!\n", far, ptr + offset); far = ptr + offset; } handled = SYSReadValFromAddr(&value, NULL, 8, far); if (handled) { set_fpn_as_double(opcode & 31, value); ctx[(opcode >> 5) & 31] += offset; } } /* FLDD post-index */ else if ((opcode & 0xfee00c00) == 0xbc400c00) { int16_t offset = ((int16_t)(opcode >> 5)) >> 7; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (far != ptr) { kprintf("address mismatch in FLDD post-index far = %08x, reg = %08x!\n", far, ptr); far = ptr; } handled = SYSReadValFromAddr(&value, NULL, 8, far); if (handled) { set_fpn_as_double(opcode & 31, value); ctx[(opcode >> 5) & 31] += offset; } } /**** Integer loads ****/ /* LDP */ else if ((opcode & 0x7fc00000) == 0x29400000) { if (opcode & 0x80000000) size = 8; else size = 4; int16_t offset = size * (((int16_t)(opcode >> 6)) >> 9); uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { DWARN(kprintf("address mismatch in LDP offset far = %08x, reg = %08x, off = %d, opcode=%08x, esr=%08x!\n", far, ptr, offset, opcode, esr)); // In case of LDP/STP it is possible that fetches are out of order, so the exception can trigger on second register or on first register far = ptr + offset; } if (size == 4) { uint64_t tmp; handled = SYSReadValFromAddr(&tmp, NULL, 8, far); ctx[opcode & 31] = tmp >> 32; ctx[(opcode >> 10) & 31] = tmp & 0xffffffffULL; } else { handled = SYSReadValFromAddr(&ctx[opcode & 31], &ctx[(opcode >> 10) & 31], 16, far); } } /* LDP post- and pre-index */ else if ((opcode & 0x7ec00000) == 0x28c00000) { int16_t offset = ((int16_t)(opcode >> 6)) >> 9; if (opcode & 0x80000000) size = 8; else size = 4; offset *= size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (opcode & 0x01000000) { if (ptr + offset != far) { DWARN(kprintf("address mismatch in LDP pre index far = %08x, reg = %08x, off = %d, opcode=%08x, esr=%08x!\n", far, ptr, offset, opcode, esr)); far = ptr + offset; } } else { if (ptr != far) { DWARN(kprintf("address mismatch in LDP post index far = %08x, reg = %08x, off = %d, opcode=%08x, esr=%08x!\n", far, ptr, offset, opcode, esr)); far = ptr; } } if (size == 4) { uint64_t tmp; handled = SYSReadValFromAddr(&tmp, NULL, 8, far); ctx[opcode & 31] = tmp >> 32; ctx[(opcode >> 10) & 31] = tmp & 0xffffffffULL; } else { handled = SYSReadValFromAddr(&ctx[opcode & 31], &ctx[(opcode >> 10) & 31], 16, far); } ctx[(opcode >> 5) & 31] += offset; } /* LDPSW */ if ((opcode & 0xffc00000) == 0x69400000) { size = 4; int16_t offset = size * (((int16_t)(opcode >> 6)) >> 9); uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { DWARN(kprintf("address mismatch in LDPSW offset far = %08x, reg = %08x, off = %d!\n", far, ptr, offset)); far = ptr + offset; } uint64_t tmp; handled = SYSReadValFromAddr(&tmp, NULL, 2 * size, far); if (handled) { ctx[opcode & 31] = tmp >> 32; if (ctx[opcode & 31] & 0x80000000) ctx[opcode & 31] |= 0xffffffff00000000ULL; ctx[(opcode >> 10) & 31] = tmp & 0xffffffffULL; if (ctx[(opcode >> 10) & 31] & 0x80000000) ctx[(opcode >> 10) & 31] |= 0xffffffff00000000ULL; } } /* LDPSW post index */ else if ((opcode & 0xffc00000) == 0x68c00000) { int16_t offset = ((int16_t)(opcode >> 6)) >> 9; size = 4; offset *= size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr != far) { DWARN(kprintf("address mismatch in LDPSW post index far = %08x, reg = %08x, off = %d!\n", far, ptr, offset)); far = ptr; } uint64_t tmp; handled = SYSReadValFromAddr(&tmp, NULL, 2 * size, far); if (handled) { ctx[opcode & 31] = tmp >> 32; if (ctx[opcode & 31] & 0x80000000) ctx[opcode & 31] |= 0xffffffff00000000ULL; ctx[(opcode >> 10) & 31] = tmp & 0xffffffffULL; if (ctx[(opcode >> 10) & 31] & 0x80000000) ctx[(opcode >> 10) & 31] |= 0xffffffff00000000ULL; } ctx[(opcode >> 5) & 31] += offset; } /* LDPSW pre index */ else if ((opcode & 0xffc00000) == 0x69c00000) { int16_t offset = ((int16_t)(opcode >> 6)) >> 9; size = 4; offset *= size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { DWARN(kprintf("address mismatch in LDPSW pre index far = %08x, reg = %08x, off = %d!\n", far, ptr, offset)); far = ptr + offset; } uint64_t tmp; handled = SYSReadValFromAddr(&tmp, NULL, 2 * size, far); if (handled) { ctx[opcode & 31] = tmp >> 32; if (ctx[opcode & 31] & 0x80000000) ctx[opcode & 31] |= 0xffffffff00000000ULL; ctx[(opcode >> 10) & 31] = tmp & 0xffffffffULL; if (ctx[(opcode >> 10) & 31] & 0x80000000) ctx[(opcode >> 10) & 31] |= 0xffffffff00000000ULL; } ctx[(opcode >> 5) & 31] += offset; } /* LDR (literal) */ else if ((opcode & 0xbf000000) == 0x18000000 || (opcode & 0xff000000) == 0x98000000) { int sext = 0; if (opcode & 0x40000000) size = 8; else size = 4; if (opcode & 0x80000000) sext = 1; handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); if (handled & sext) { if (ctx[opcode & 31] & 0x80000000) ctx[opcode & 31] |= 0xffffffff00000000ULL; } } /* LDR register */ else if ((opcode & 0x3fe00c00) == 0x38600800) { int option = (opcode >> 13) & 7; int s = (opcode & 0x1000); uint64_t ptr = ctx[(opcode >> 5) & 31]; uint64_t rm = ctx[(opcode >> 16) & 31]; switch (option) { case 0b010: // UXTW rm &= 0xffffffffULL; break; case 0b110: // SXTW if (rm & 0x80000000) rm |= 0xffffffff00000000ULL; else rm &= 0x00000000ffffffffULL; break; } if (s) { switch(size) { case 2: rm <<= 1; break; case 4: rm <<= 2; break; case 8: rm <<= 3; break; } } if (far != ptr + rm) { kprintf("address mismatch in LDR %c%d reg FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, far, ptr); far = ptr + rm; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); } /* LDXR register - no exclusive in this case!!! But m68k bus does not support it anyway */ else if ((opcode & 0x3ffffc00) == 0x085f7c00) { handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); } /* LDR immediate */ else if ((opcode & 0x3fc00000) == 0x39400000) { uint64_t offset = (opcode >> 10) & 0xfff; offset *= size; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (ptr + offset != far) { kprintf("address mismatch in LDR imm far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); far = ptr + offset; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); } /* LDUR(B/W) */ else if ((opcode & 0x3fe00c00) == 0x38400000) { uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((int16_t)(opcode >> 5)) >> 7; if (ptr != far) { kprintf("address mismatch in LDUR %c%d. FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, far, ptr); far = ptr; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); } /* LDR immediate, post- and pre-index */ else if ((opcode & 0x3fe00400) == 0x38400400) { int64_t offset = ((int16_t)(opcode >> 5)) >> 7; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (opcode & (1 << 11 )) { if (ptr + offset != far) { kprintf("address mismatch in LDR pre-index far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); far = ptr + offset; } } else { if (ptr != far) { kprintf("address mismatch in LDR post-index far = %08x, reg = %08x, off = %d!\n", far, ptr, offset); far = ptr; } } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); if (handled) ctx[(opcode >> 5) & 31] += offset; } /* LDRSW/LDRSB/LDRSH register */ else if ((opcode & 0x3fa00c00) == 0x38a00800) { int sext64 = 1; if (opcode & (1 << 22)) sext64 = 0; int option = (opcode >> 13) & 7; int s = (opcode & 0x1000); uint64_t ptr = ctx[(opcode >> 5) & 31]; uint64_t rm = ctx[(opcode >> 16) & 31]; switch (option) { case 0b010: // UXTW rm &= 0xffffffffULL; break; case 0b110: // SXTW if (rm & 0x80000000) rm |= 0xffffffff00000000ULL; else rm &= 0x00000000ffffffffULL; break; } if (s) { switch(size) { case 2: rm <<= 1; break; case 4: rm <<= 2; break; case 8: rm <<= 3; break; } } if (far != ptr + rm) { kprintf("address mismatch in LDRSW/B/H %c%d reg FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, far, ptr); far = ptr + rm; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); if (handled) { int sext = 0; switch (size) { case 1: sext = ctx[opcode & 31] & 0x80; if (sext) ctx[opcode & 31] |= 0xffffff00; break; case 2: sext = ctx[opcode & 31] & 0x8000; if (sext) ctx[opcode & 31] |= 0xffff0000; break; case 4: sext = ctx[opcode & 31] & 0x80000000; break; } if (sext && sext64) { ctx[opcode & 31] |= 0xffffffff00000000ULL; } } } /* LDRSW/LDRSB/LDRSH immediate */ else if ((opcode & 0x3f800000) == 0x39800000) { int sext64 = 1; if (opcode & (1 << 22)) sext64 = 0; uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((opcode >> 10) & 0xfff) * size; if (ptr != far) { kprintf("address mismatch in LDRSW/B/H imm. FAR = %08x, reg = %08x!\n", far, ptr); far = ptr; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); if (handled) { int sext = 0; switch (size) { case 1: sext = ctx[opcode & 31] & 0x80; if (sext) ctx[opcode & 31] |= 0xffffff00; break; case 2: sext = ctx[opcode & 31] & 0x8000; if (sext) ctx[opcode & 31] |= 0xffff0000; break; case 4: sext = ctx[opcode & 31] & 0x80000000; break; } if (sext && sext64) { ctx[opcode & 31] |= 0xffffffff00000000ULL; } } } /* LDURSW/LDURSB/LDURSH immediate */ else if ((opcode & 0x3fa00c00) == 0x38800000) { int sext64 = 1; if (opcode & (1 << 22)) sext64 = 0; uint64_t ptr = ctx[(opcode >> 5) & 31]; ptr += ((int16_t)(opcode >> 5)) >> 7; if (ptr != far) { kprintf("address mismatch in LDURS %c%d. FAR = %08x, reg = %08x!\n", size == 4 ? 'W' : 'X', opcode & 31, far, ptr); far = ptr; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); if (handled) { int sext = 0; switch (size) { case 1: sext = ctx[opcode & 31] & 0x80; if (sext) ctx[opcode & 31] |= 0xffffff00; break; case 2: sext = ctx[opcode & 31] & 0x8000; if (sext) ctx[opcode & 31] |= 0xffff0000; break; case 4: sext = ctx[opcode & 31] & 0x80000000; break; } if (sext && sext64) { ctx[opcode & 31] |= 0xffffffff00000000ULL; } } } /* LDRSW/LDRSB/LDRSH post- and pre-index */ else if ((opcode & 0x3fa00400) == 0x38800400) { int16_t offset = ((int16_t)(opcode >> 5)) >> 7; int sext64 = 1; if (opcode & (1 << 22)) sext64 = 0; uint64_t ptr = ctx[(opcode >> 5) & 31]; if (opcode & 0x800) { ptr += offset; } if (far != ptr) { kprintf("address mismatch in LDRSW/B/H post-/pre-index. FAR = %08x, reg = %08x!\n", far, ptr); far = ptr; } handled = SYSReadValFromAddr(&ctx[opcode & 31], NULL, size, far); if (handled) { int sext = 0; switch (size) { case 1: sext = ctx[opcode & 31] & 0x80; if (sext) ctx[opcode & 31] |= 0xffffff00; break; case 2: sext = ctx[opcode & 31] & 0x8000; if (sext) ctx[opcode & 31] |= 0xffff0000; break; case 4: sext = ctx[opcode & 31] & 0x80000000; break; } if (sext && sext64) { ctx[opcode & 31] |= 0xffffffff00000000ULL; } ctx[(opcode >> 5) & 31] += offset; } } if (!handled) { kprintf("[JIT:SYS] Unhandled page fault: opcode %08x, read from %p\n", opcode, far); } elr += 4; asm volatile("msr ELR_EL1, %0"::"r"(elr)); return handled; } #undef D #define D(x) x void SYSHandler(uint32_t vector, uint64_t *ctx) { int handled = 0; uint64_t elr, spsr, esr, far; asm volatile("mrs %0, ELR_EL1; mrs %1, SPSR_EL1":"=r"(elr),"=r"(spsr)); asm volatile("mrs %0, ESR_EL1":"=r"(esr)); asm volatile("mrs %0, FAR_EL1":"=r"(far)); if ((vector & 0x1ff) == 0x00 && (esr & 0xf8000000) == 0x90000000) { int writeFault = (esr & (1 << 6)) != 0; handled = writeFault ? SYSPageFaultWriteHandler(vector, ctx, elr, spsr, esr, far) : SYSPageFaultReadHandler(vector, ctx, elr, spsr, esr, far); } else if ((vector & 0x1ff) == 0x00 && (esr & 0xf8000000) == 0x80000000) { if ((far >> 56) == 0xaa) { handled = SYSValidateUnit(vector, ctx, elr, spsr, esr, far); } } else if ((esr & ~0xffff) == 0x56000000) { handled = 1; if ((esr & 0xffff) == 0x100) { kprintf("[JIT:SYS] AArch64 RegDump:\n"); for (int i=0; i < 8; i++) { kprintf("[JIT:SYS] X%02d=%p X%02d=%p X%02d=%p X%02d=%p\n", 4*i, ctx[4*i], 4*i+1, ctx[4*i+1], 4*i+2, ctx[4*i+2], 4*i+3, ctx[4*i+3]); } uint64_t spsr; asm volatile("mrs %0, SPSR_EL1":"=r"(spsr)); kprintf("[JIT:SYS] SPSR=%08x\n", spsr); } if ((esr & 0xffff) == 0x101) { uint64_t sr; asm volatile("mrs %0, tpidr_el0":"=r"(sr)); kprintf("[JIT:SYS] M68k RegDump:\n[JIT] "); int reg_dn[] = {REG_D0, REG_D1, REG_D2, REG_D3, REG_D4, REG_D5, REG_D6, REG_D7}; int reg_an[] = {REG_A0, REG_A1, REG_A2, REG_A3, REG_A4, REG_A5, REG_A6, REG_A7}; for (int i=0; i < 8; i++) { if (i==4) kprintf("\n[JIT] "); kprintf(" D%d = 0x%08x", i, BE32(ctx[reg_dn[i]])); } kprintf("\n[JIT] "); for (int i=0; i < 8; i++) { if (i==4) kprintf("\n[JIT] "); kprintf(" A%d = 0x%08x", i, BE32(ctx[reg_an[i]])); } kprintf("\n[JIT] "); kprintf(" PC = 0x%08x SR = ", BE32(ctx[REG_PC])); kprintf("T%d|", sr >> 14); if (sr & SR_S) kprintf("S"); else kprintf("."); if (sr & SR_M) kprintf("M|"); else kprintf(".|"); kprintf("IPM%d|", (sr >> 8) & 7); if (sr & SR_X) kprintf("X"); else kprintf("."); if (sr & SR_N) kprintf("N"); else kprintf("."); if (sr & SR_Z) kprintf("Z"); else kprintf("."); if (sr & SR_V) kprintf("V"); else kprintf("."); if (sr & SR_C) kprintf("C"); else kprintf("."); kprintf("\n"); } if ((esr & 0xffff) == 0x102) { uint8_t *from = (uint8_t*)(intptr_t)(*(uint32_t *)elr); uint32_t len = *(uint32_t *)(elr + 4); kprintf("[SYS:JIT] RAM dump from 0x%08x, len 0x%x\n[SYS:JIT] ", from, len); while (len) { if ((len & 15) == 0) kprintf("\n[SYS:JIT] "); kprintf("%02x ", *from++); len--; } kprintf("\n"); elr += 8; asm volatile("msr ELR_EL1, %0"::"r"(elr)); } if ((esr & 0xffff) == 0x103) { uint16_t *from = (uint16_t*)(intptr_t)(*(uint32_t *)elr); uint32_t len = (*(uint32_t *)(elr + 4)) / 2; kprintf("[SYS:JIT] RAM dump from 0x%08x, len 0x%x\n[SYS:JIT] ", from, len); while (len) { if ((len & 7) == 0) kprintf("\n[SYS:JIT] "); kprintf("%04x ", *from++); len--; } kprintf("\n"); elr += 8; asm volatile("msr ELR_EL1, %0"::"r"(elr)); } } if (!handled) { kprintf("[JIT:SYS] Exception with vector %04x. ELR=%p, SPSR=%08x, ESR=%p, FAR=%p\n", vector, elr, spsr, esr, far); kprintf("[JIT:SYS] Failed instruction: %08x\n", LE32(*(uint32_t*)elr)); for (int i=0; i < 16; i++) { kprintf("[JIT:SYS] X%02d=%p X%02d=%p\n", 2*i, ctx[2*i], 2*i+1, ctx[2*i+1]); } while(1) { asm volatile("wfe"); }; } }