diff options
Diffstat (limited to 'firmware/irq.c')
-rw-r--r-- | firmware/irq.c | 37 |
1 files changed, 24 insertions, 13 deletions
diff --git a/firmware/irq.c b/firmware/irq.c index aa4cd9b..aaa141d 100644 --- a/firmware/irq.c +++ b/firmware/irq.c @@ -13,6 +13,27 @@ uint32_t *irq(uint32_t *regs, uint32_t irqs) static unsigned int ext_irq_5_count = 0; static unsigned int timer_irq_count = 0; + // checking compressed isa q0 reg handling + { + uint32_t pc = (regs[0] & 1) ? regs[0] - 3 : regs[0] - 4; + uint32_t instr = *(uint16_t*)pc; + + if ((instr & 3) == 3) + instr = instr | (*(uint16_t*)(pc + 2)) << 16; + + if (((instr & 3) != 3) != (regs[0] & 1)) { + print_str("Mismatch between q0 LSB and decoded instruction word! q0=0x"); + print_hex(regs[0], 8); + print_str(", instr=0x"); + if ((instr & 3) == 3) + print_hex(instr, 8); + else + print_hex(instr, 4); + print_str("\n"); + __asm__ volatile ("sbreak"); + } + } + if ((irqs & (1<<4)) != 0) { ext_irq_4_count++; // print_str("[EXT-IRQ-4]"); @@ -30,21 +51,11 @@ uint32_t *irq(uint32_t *regs, uint32_t irqs) if ((irqs & 6) != 0) { - uint32_t pc = regs[0] - 2; - uint16_t *instr_hwords = (uint16_t*)pc; + uint32_t pc = (regs[0] & 1) ? regs[0] - 3 : regs[0] - 4; uint32_t instr = *(uint16_t*)pc; - if ((*instr_hwords & 3) == 3) { - pc -= 2; - instr = (instr << 16) | *(uint16_t*)pc; - } else { - int cnt_3 = 0; - while ((*(--instr_hwords) & 3) == 3 && cnt_3 < 20) cnt_3++; - if ((cnt_3 & 1) != 0) { - pc -= 2; - instr = (instr << 16) | *(uint16_t*)pc; - } - } + if ((instr & 3) == 3) + instr = instr | (*(uint16_t*)(pc + 2)) << 16; print_str("\n"); print_str("------------------------------------------------------------\n"); |