Implement VTOR

pipe01 · Jun 13, 2024 · bae0da8 · bae0da8
1 parent 52579fc
commit bae0da8
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 14 deletions.
diff --git a/include/peripherals/scb.h b/include/peripherals/scb.h
@@ -29,6 +29,7 @@ static_assert(sizeof(SCB_CCR_t) == 4, "SCB_CCR_t size is incorrect");
 
 PERIPHERAL(SCB, scb, cpu_t *cpu)
 
-uint32_t scb_get_prigroup(SCB_t *scb);
-SCB_CCR_t scb_get_ccr(SCB_t *scb);
-uint32_t scb_get_cpacr(SCB_t *scb);
+uint32_t scb_get_prigroup(SCB_t *);
+SCB_CCR_t scb_get_ccr(SCB_t *);
+uint32_t scb_get_cpacr(SCB_t *);
+uint32_t scb_get_vtor_tbloff(SCB_t *);
diff --git a/src/component/spi/spinorflash.c b/src/component/spi/spinorflash.c
@@ -26,7 +26,7 @@ enum
     COMMAND_RDI = 0xAB,   // Release from Deep Power-Down and Read Device ID
 };
 
-#define MAX_COMMAND_SIZE 32
+#define MAX_COMMAND_SIZE 200
 
 #define READ_UINT24(data, start) (((data)[(start)] << 16) | ((data)[(start) + 1] << 8) | (data)[(start) + 2])
 

diff --git a/src/cpu.c b/src/cpu.c
@@ -84,7 +84,7 @@
 
 #define MAX_EXECUTING_EXCEPTIONS 64
 
-static_assert(__STDC_IEC_559__, "Floating point operations are not IEEE 754 compliant");
+// static_assert(__STDC_IEC_559__, "Floating point operations are not IEEE 754 compliant");
 
 typedef struct
 {
@@ -832,14 +832,7 @@ static void cpu_exception_taken(cpu_t *cpu, arm_exception ex)
     cpu->exception_regs_count++;
 #endif
 
-    uint32_t tmp;
-
-    tmp = memreg_read(cpu->mem, 4 * ex);
-    cpu->core_regs[ARM_REG_PC] = tmp & ~1;
-    cpu->branched = true;
-
-    if ((tmp & 1) != 1)
-        fault_take(FAULT_CPU_PC_ALIGNMENT);
+    cpu_jump_exception(cpu, ex);
 
     cpu->mode = ARM_MODE_HANDLER;
 
@@ -3123,7 +3116,9 @@ bool cpu_mem_write(cpu_t *cpu, uint32_t addr, uint8_t value)
 
 void cpu_jump_exception(cpu_t *cpu, arm_exception ex)
 {
-    cpu_reg_write(cpu, ARM_REG_PC, READ_UINT32(cpu->program, ex * 4));
+    uint32_t vtor = scb_get_vtor_tbloff(cpu->scb);
+
+    cpu_reg_write(cpu, ARM_REG_PC, READ_UINT32(cpu->program, vtor + ex * 4));
 }
 
 int16_t cpu_get_exception_priority(cpu_t *cpu, arm_exception ex)

diff --git a/src/peripherals/scb.c b/src/peripherals/scb.c
@@ -13,6 +13,7 @@ struct SCB_inst_t
     uint32_t prigroup;
     uint32_t scr;
     SCB_CCR_t ccr;
+    uint32_t vtor;
 
     cpu_t *cpu;
 };
@@ -28,6 +29,7 @@ OPERATION(scb)
         scb->scr = 0;
         scb->ccr.value = 0;
         scb->ccr.STKALIGN = 1;
+        scb->vtor = 0;
         return MEMREG_RESULT_OK;
     }
 
@@ -81,6 +83,9 @@ OPERATION(scb)
 
         return MEMREG_RESULT_OK;
 
+    case 0x08: // VTOR
+        OP_RETURN_REG(scb->vtor, WORD);
+
     case 0x0C: // AIRCR
         OP_ASSERT_SIZE(op, WORD);
 
@@ -147,3 +152,8 @@ uint32_t scb_get_cpacr(SCB_t *scb)
 {
     return scb->cpacr;
 }
+
+uint32_t scb_get_vtor_tbloff(SCB_t *scb)
+{
+    return scb->vtor & 0xFFFFFF80;
+}