Change exception handling, more instructions implemented

1 files changed, 30 insertions, 6 deletions

diff --git a/nios2.c b/nios2.c
index 2b2fec2..c5a974e 100644
--- a/nios2.c
+++ b/nios2.c
@@ -24,6 +24,7 @@ void nios2_cpu_reset(struct nios2 *cpu)
 
 	cpu->pc = 0;
 	cpu->mode = NIOS2_SUPERVISOR_MODE;
+	cpu->exception_cause = NIOS2_EX_NONE;
 }
 
 void nios2_cpu_init(struct nios2 *cpu)
@@ -61,9 +62,32 @@ bool nios2_in_supervisor_mode(struct nios2 *cpu)
 	return !nios2_in_user_mode(cpu);
 }
 
-void nios2_exception(struct nios2 *cpu, uint8_t cause)
+void nios2_handle_exception(struct nios2 *cpu)
 {
-	cpu->ctrl_regs[exception] = (cause << 2) & 0x7C;
+	uint32_t *gp_regs = cpu->gp_regs;
+	uint32_t *ctrl_regs = cpu->ctrl_regs;
+
+	if (cpu->exception_cause == NIOS2_EX_NONE)
+		return;
+
+	/* Store the exception cause */
+	ctrl_regs[exception] = (cpu->exception_cause << NIOS2_EXCEPTION_CAUSE_OFF)
+			       & NIOS2_EXCEPTION_CAUSE_MASK;
+	/* Clear status.PIE */
+	ctrl_regs[status] &= ~NIOS2_STATUS_PIE_MASK;
+	/* Clear status.U */
+	ctrl_regs[status] &= ~NIOS2_STATUS_U_MASK;
+
+	if (!cpu->has_mmu
+	    || (cpu->has_mmu && !(ctrl_regs[status] & NIOS2_STATUS_EH_MASK))) {
+		/* Copy status control register to estatus control register */
+		ctrl_regs[estatus] = cpu->ctrl_regs[status];
+		/* Save address of the instruction following the exception to ea */
+		gp_regs[ea] = cpu->pc + 4;
+	}
+
+	/* TODO */
+
 }
 
 uint32_t nios2_fetch_instr(struct nios2 *cpu)
@@ -95,10 +119,10 @@ int nios2_load(struct nios2 *cpu, uint32_t addr, void *data, size_t size)
 {
 	struct memory *mem = cpu->mem;
 
-	dbg("load%zu %08x\n", size * 8, addr);
+//	dbg("load%zu %08x\n", size * 8, addr);
 
 	if (is_mem_addr(mem, addr)) {
-		dbg("load MEM\n");
+//		dbg("load MEM\n");
 		switch (size) {
 		case 1:
 			*((uint8_t *) data) = memory_get_byte(mem, addr);
@@ -135,10 +159,10 @@ int nios2_store(struct nios2 *cpu, uint32_t addr, void *data, size_t size)
 {
 	struct memory *mem = cpu->mem;
 
-	dbg("store%zu %08x\n", size, addr);
+//	dbg("store%zu %08x\n", size, addr);
 
 	if (is_mem_addr(mem, addr)) {
-		dbg("store MEM\n");
+//		dbg("store MEM\n");
 		switch (size) {
 		case 1:
 			memory_set_byte(mem, addr, *((uint8_t *) data));