Change exception handling, more instructions implemented

5 files changed, 214 insertions, 59 deletions

diff --git a/instruction.c b/instruction.c
index 6fd06dc..8b10093 100644
--- a/instruction.c
+++ b/instruction.c
@@ -14,6 +14,12 @@
 #include "instruction.h"
 #include "nios2.h"
 
+#if 1
+# define instr_dbg(fmt, args...) dbg(fmt, ##args)
+#else
+# define instr_dbg(fmt, args...)
+#endif
+
 static inline uint32_t get_opcode(uint32_t code)
 {
 	I_TYPE(instr, code);
@@ -62,6 +68,11 @@ static int call(struct nios2 *cpu, uint32_t code)
 	cpu->gp_regs[ra] = cpu->pc + 4;
 	cpu->pc = (cpu->pc & 0xF0000000) | (instr->imm26 * 4);
 
+	if (cpu->pc == 0) {
+		err("PC = 0\n");
+		return INSTR_ERR;
+	}
+
 	return PC_INC_BY_INSTR;
 }
 
@@ -340,7 +351,7 @@ static int muli(struct nios2 *cpu, uint32_t code)
 
 	/* Raise exception if instruction is not implemented */
 	if (!cpu->has_mul)
-		return EXCEPTION(NIOS2_EX_UNIMPLEMENTED);
+		return EXCEPTION(cpu, NIOS2_EX_UNIMPLEMENTED);
 
 	tmp = gp_regs[instr->a] * ((int16_t) instr->imm16);
 	gp_regs[instr->b] = (uint32_t) (tmp & BIT_MASK_FILL(32));
@@ -377,6 +388,25 @@ static int andhi(struct nios2 *cpu, uint32_t code)
 	return PC_INC_NORMAL;
 }
 
+/*
+ * if (rA >= rB)
+ *   PC <- PC + 4 + @(IMM16)
+ * else
+ *   PC <- PC + 4
+ */
+static int bgeu(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	if (gp_regs[instr->a] >= gp_regs[instr->b])
+		cpu->pc += 4 + (int16_t)(instr->imm16 & 0xFFFC);
+	else
+		cpu->pc += 4;
+
+	return PC_INC_BY_INSTR;
+}
+
 /* */
 static int initd(struct nios2 *cpu __unused, uint32_t code __unused)
 {
@@ -463,7 +493,7 @@ static struct instruction i_type_instructions[I_TYPE_COUNT] = {
 	[LDHUIO]	= INSTRUCTION_UNIMPLEMENTED(ldhuio),
 	[ANDHI]		= INSTRUCTION(andhi),
 	[STHIO]		= INSTRUCTION_UNIMPLEMENTED(sthio),
-	[BGEU]		= INSTRUCTION_UNIMPLEMENTED(bgeu),
+	[BGEU]		= INSTRUCTION(bgeu),
 	[LDHIO]		= INSTRUCTION_UNIMPLEMENTED(ldhio),
 	[CMPLTUI]	= INSTRUCTION_UNIMPLEMENTED(cmpltui),
 	[CUSTOM]	= INSTRUCTION_UNIMPLEMENTED(custom),
@@ -742,11 +772,11 @@ static int divu(struct nios2 *cpu, uint32_t code)
 	uint32_t *gp_regs = cpu->gp_regs;
 
 	if (!cpu->has_div)
-		return EXCEPTION(NIOS2_EX_UNIMPLEMENTED);
+		return EXCEPTION(cpu, NIOS2_EX_UNIMPLEMENTED);
 
 	/* Division by zero? */
 	if (gp_regs[instr->b] == 0)
-		return EXCEPTION(NIOS2_EX_DIV_ERR);
+		return EXCEPTION(cpu, NIOS2_EX_DIV_ERR);
 
 	gp_regs[instr->c] = gp_regs[instr->a] / gp_regs[instr->b];
 
@@ -761,17 +791,17 @@ static int div(struct nios2 *cpu, uint32_t code)
 	int32_t a, b;
 
 	if (!cpu->has_div)
-		return EXCEPTION(NIOS2_EX_UNIMPLEMENTED);
+		return EXCEPTION(cpu, NIOS2_EX_UNIMPLEMENTED);
 
 	a = (int32_t) gp_regs[instr->a];
 	b = (int32_t) gp_regs[instr->b];
 
 	/* Division by zero? */
 	if (b == 0)
-		return EXCEPTION(NIOS2_EX_DIV_ERR);
+		return EXCEPTION(cpu, NIOS2_EX_DIV_ERR);
 	/* Divide overflow? */
 	if ((uint32_t) a == 0x80000000 && (uint32_t) b == 0xFFFFFFFF)
-		return EXCEPTION(NIOS2_EX_DIV_ERR);
+		return EXCEPTION(cpu, NIOS2_EX_DIV_ERR);
 
 	gp_regs[instr->c] = (uint32_t)(a / b);
 
@@ -785,7 +815,7 @@ static int rdctl(struct nios2 *cpu, uint32_t code)
 
 	/* Instruction only allowed in supervisor mode */
 	if (!nios2_in_supervisor_mode(cpu))
-		return EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);
+		return EXCEPTION(cpu, NIOS2_EX_SUPERVISOR_ONLY_I);
 
 	cpu->gp_regs[instr->c] = cpu->ctrl_regs[instr->imm5];
 
@@ -807,18 +837,9 @@ static int initi(struct nios2 *cpu __unused, uint32_t code __unused)
  * ea <- PC + 4
  * PC <- exception handler address
  */
-static int trap(struct nios2 *cpu, uint32_t code __unused)
+static int trap(struct nios2 *cpu __unused, uint32_t code __unused)
 {
-	uint32_t *gp_regs = cpu->gp_regs;
-	uint32_t *ctrl_regs = cpu->ctrl_regs;
-
-	ctrl_regs[estatus] = ctrl_regs[status];
-	clear_bit(NIOS2_STATUS_PIE, &ctrl_regs[status]);
-	clear_bit(NIOS2_STATUS_U, &ctrl_regs[status]);
-	gp_regs[ea] = cpu->pc + 4;
-	cpu->pc = cpu->exception_handler_addr;
-
-	return PC_INC_BY_INSTR;
+	return EXCEPTION(cpu, NIOS2_EX_TRAP);
 }
 
 /* ctlN <- rA */
@@ -828,7 +849,7 @@ static int wrctl(struct nios2 *cpu, uint32_t code)
 
 	/* Instruction only allowed in supervisor mode */
 	if (!nios2_in_supervisor_mode(cpu))
-		return EXCEPTION(NIOS2_EX_SUPERVISOR_ONLY_I);
+		return EXCEPTION(cpu, NIOS2_EX_SUPERVISOR_ONLY_I);
 
 	cpu->ctrl_regs[instr->imm5] = cpu->gp_regs[instr->a];
 
@@ -846,6 +867,12 @@ static int add(struct nios2 *cpu, uint32_t code)
 	return PC_INC_NORMAL;
 }
 
+/* */
+static int __break(struct nios2 *cpu, uint32_t code)
+{
+	return INSTR_BREAK;
+}
+
 /* rC <- rA - rB */
 static int sub(struct nios2 *cpu, uint32_t code)
 {
@@ -916,7 +943,7 @@ static struct instruction r_type_instructions[R_TYPE_COUNT] = {
 	[WRCTL]		= INSTRUCTION(wrctl),
 	[CMPLTU]	= INSTRUCTION_UNIMPLEMENTED(cmpltu),
 	[ADD]		= INSTRUCTION(add),
-	[BREAK]		= INSTRUCTION_UNIMPLEMENTED(break),
+	[BREAK]		= { "break", __break },
 	[SYNC]		= INSTRUCTION(nop),
 	[SUB]		= INSTRUCTION(sub),
 	[SRAI]		= INSTRUCTION(srai),
@@ -932,7 +959,7 @@ static int handle_r_type_instr(struct nios2 *cpu, uint32_t code)
 	if (unlikely(opx >= R_TYPE_COUNT))
 		return INSTR_ERR;
 
-//	dbg("  R: %s (%08x)\n", r_type_instructions[opx].name, code);
+	instr_dbg("  R: %s (%08x)\n", r_type_instructions[opx].name, code);
 	handle_instr = r_type_instructions[opx].handler;
 	if (unlikely(handle_instr == NULL))
 		return INSTR_ERR;
@@ -946,7 +973,7 @@ instruction_handler instruction_get_handler(uint32_t code)
 
 	if (unlikely(op >= I_TYPE_COUNT))
 		return NULL;
-//	dbg("I: %s (%08x)\n", i_type_instructions[op].name, code);
+	instr_dbg("I: %s (%08x)\n", i_type_instructions[op].name, code);
 	return i_type_instructions[op].handler;
 }
 
diff --git a/instruction.h b/instruction.h
index 1992b6a..defa546 100644
--- a/instruction.h
+++ b/instruction.h
@@ -198,11 +198,16 @@ enum {
 #define INSTR_ERR		-1	/* Error in instruction */
 #define PC_INC_NORMAL		0	/* Normal PC increment after instruction */
 #define PC_INC_BY_INSTR		1	/* PC got incremented by instruction */
-#define INSTR_EXCEPTION_BASE	255	/* Instruction generated an exception */
-
-#define EXCEPTION(cause)	(INSTR_EXCEPTION_BASE + (cause))
-#define IS_EXCEPTION(ret)	((ret) >= INSTR_EXCEPTION_BASE)
-#define EXCEPTION_CAUSE(ret)	((uint8_t)((ret) - INSTR_EXCEPTION_BASE))
+#define INSTR_BREAK		2	/* Break encountered */
+#define INSTR_EXCEPTION		255	/* Instruction generated an exception
+					   (the exception cause will be stored
+					   in struct nios2 */
+
+#define EXCEPTION(cpu, cause)			\
+	({					\
+	  (cpu)->exception_cause = cause;	\
+	  INSTR_EXCEPTION;			\
+	})
 
 /* Forward declaration */
 struct nios2;
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));
diff --git a/nios2.h b/nios2.h
index a24ab1d..f777369 100644
--- a/nios2.h
+++ b/nios2.h
@@ -19,6 +19,8 @@
 /* there are really 32, but 16-31 are reserved for future use */
 #define NIOS2_CTRL_REG_COUNT	16
 
+struct tlb_entry;
+
 struct nios2 {
 	/* General-Purpose Registers */
 	uint32_t gp_regs[NIOS2_GP_REG_COUNT];
@@ -27,7 +29,9 @@ struct nios2 {
 	/* Program counter */
 	uint32_t pc;
 	/* User or Supervisor mode */
-	int mode;
+	unsigned int mode;
+	/* Exception cause */
+	unsigned int exception_cause;
 
 	/* Configurable processor features */
 	bool has_mul;	/* mul, muli */
@@ -36,11 +40,21 @@ struct nios2 {
 
 	bool has_mmu;	/* Memory Management Unit */
 
+	/*
+	struct tlb_entry tlb[TLB_NUM_LINES][TLB_NUM_WAYS];
+	*/
+
 	struct memory *mem;
 	/* Exception handler address */
 	uint32_t exception_handler_addr;
 };
 
+/* processor modes */
+enum {
+	NIOS2_SUPERVISOR_MODE,
+	NIOS2_USER_MODE,	/* only used with MMU */
+};
+
 /* Aliases for general-purpose registers */
 enum {
 	zero = 0,
@@ -76,23 +90,74 @@ enum {
 };
 
 /* status register fields */
-#define NIOS2_STATUS_PIE	0
-#define NIOS2_STATUS_U		1
-#define NIOS2_STATUS_EH		2
+#define NIOS2_STATUS_PIE_MASK		0x00000001
+#define NIOS2_STATUS_U_MASK		0x00000002
+#define NIOS2_STATUS_EH_MASK		0x00000004
+
+/* exception register fields */
+#define NIOS2_EXCEPTION_CAUSE_MASK	0x0000007C
+#define NIOS2_EXCEPTION_CAUSE_OFF	2
 
 /* exception causes */
 #define NIOS2_EX_RESET			0
 #define NIOS2_EX_PROC_ONLY_RESET	1
 #define NIOS2_EX_IRQ			2
+#define NIOS2_EX_TRAP			3
 #define NIOS2_EX_UNIMPLEMENTED		4
 #define NIOS2_EX_DIV_ERR		8
 #define NIOS2_EX_SUPERVISOR_ONLY_I	9
 #define NIOS2_EX_SUPERVISOR_ONLY_D	11
 #define NIOS2_EX_FAST_TLB_MISS		12
 
-enum {
-	NIOS2_SUPERVISOR_MODE,
-	NIOS2_USER_MODE,
+#define NIOS2_EX_NONE			255
+
+/* pteaddr register fields */
+#define NIOS2_PTEADDR_VPN_SHIFT		2
+#define NIOS2_PTEADDR_VPN_MASK		0x007FFFFC
+#define NIOS2_PTEADDR_PTBASE_SHIFT	22
+#define NIOS2_PTEADDR_PTBASE_MASK	0xFFC00000
+
+/* tlbacc register fields */
+#define NIOS2_TLBACC_PFN_MASK	0x000FFFFF
+#define NIOS2_TLBACC_G_MASK	0x00100000
+#define NIOS2_TLBACC_X_MASK	0x00200000
+#define NIOS2_TLBACC_W_MASK	0x00400000
+#define NIOS2_TLBACC_R_MASK	0x00800000
+#define NIOS2_TLBACC_C_MASK	0x01000000
+
+/* tlbmisc register fields */
+#define NIOS2_TLBMISC_D_MASK	0x00000001
+#define NIOS2_TLBMISC_PERM_MASK	0x00000002
+#define NIOS2_TLBMISC_BAD_MASK	0x00000004
+#define NIOS2_TLBMISC_DBL_MASK	0x00000008
+#define NIOS2_TLBMISC_PID_MASK	0x0007FFF0
+#define NIOS2_TLBMISC_PID_OFF	4
+#define NIOS2_TLBMISC_WE_MASK	0x00040000
+#define NIOS2_TLBMISC_RD_MASK	0x00080000
+#define NIOS2_TLBMISC_WAY_MASK	0x0007FFF0
+#define NIOS2_TLBMISC_WAY_OFF	20
+
+/* MMU-specific stuff */
+
+#define TLB_NUM_ENTRIES	256
+#define TLB_NUM_WAYS	16
+#define TLB_NUM_LINES	(TLB_NUM_ENTRIES / TLB_NUM_WAYS)
+
+struct tlb_entry {
+	/* TLB Tag Portion */
+	uint32_t vpn;
+	uint16_t pid;
+	uint8_t g;
+
+	/* TLB Data Portion Contents */
+	uint32_t pfn;
+	uint8_t c;
+	uint8_t r;
+	uint8_t w;
+	uint8_t x;
+
+	/* TLB Data */
+	uint32_t buffer;
 };
 
 extern void nios2_cpu_reset(struct nios2 *cpu);
@@ -101,7 +166,7 @@ extern void nios2_simulate(struct nios2 *cpu);
 extern void nios2_cpu_inc_pc(struct nios2 *cpu);
 extern bool nios2_in_user_mode(struct nios2 *cpu);
 extern bool nios2_in_supervisor_mode(struct nios2 *cpu);
-extern void nios2_exception(struct nios2 *cpu, uint8_t cause);
+extern void nios2_handle_exception(struct nios2 *cpu);
 extern uint32_t nios2_fetch_instr(struct nios2 *cpu);
 extern int nios2_execute_instr(struct nios2 *cpu, uint32_t instr);
 
diff --git a/simulator.c b/simulator.c
index 1507f22..92c07b5 100644
--- a/simulator.c
+++ b/simulator.c
@@ -9,6 +9,7 @@
  * for more details.
  */
 
+#include <ctype.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <signal.h>
@@ -23,10 +24,40 @@
 static bool simulator_running;
 static struct nios2 *this_cpu = NULL;
 
-static void sigint_handler(int sig __unused)
+static void simulator_pause(void)
 {
-	info("SIGINT caught\n");
-	simulator_running = false;
+	do {
+		char input;
+
+		info("\n=== Simulator paused ===\n\n"
+		     "Please choose an option:\n"
+		     "  d  enter debug mode\n"
+		     "  r  resume simulator\n"
+		     "  q  quit simulator\n"
+		     "> ");
+
+		input = tolower(getchar());
+		do {} while (getchar() != '\n');
+		switch (input) {
+		case 'd':
+			dbg("Entering debug mode\n");
+			return;
+		case 'r':
+			return;
+		case 'q':
+			simulator_running = false;
+			return;
+		default:
+			continue;
+		}
+	} while (true);
+}
+
+static void sigint_handler(int sig)
+{
+	signal(sig, SIG_IGN);
+	simulator_pause();
+	signal(SIGINT, sigint_handler);
 }
 
 static void sigsegv_handler(int sig __unused)
@@ -58,28 +89,36 @@ void simulator_run(struct nios2 *cpu)
 	memory_dump(cpu->mem, cpu->pc, 32);
 
 	simulator_running = true;
-	while (simulator_running) {
+	while (true) {
 		instr = nios2_fetch_instr(cpu);
 
 		ret = nios2_execute_instr(cpu, instr);
-
-		if (ret == PC_INC_NORMAL)
+		switch (ret) {
+		case PC_INC_NORMAL:
 			nios2_cpu_inc_pc(cpu);
-		else if (ret == PC_INC_BY_INSTR)
-			;
-		else if (IS_EXCEPTION(ret)) {
+			break;
+		case PC_INC_BY_INSTR:
+			/* do nothing */
+			break;
+		case INSTR_BREAK:
+			simulator_pause();
+			break;
+		case INSTR_EXCEPTION:
 			dbg("Exception\n");
-			nios2_exception(cpu, EXCEPTION_CAUSE(ret));
-		} else if (ret == INSTR_UNIMPL) {
+			nios2_handle_exception(cpu);
+		case INSTR_UNIMPL:
 			simulator_running = false;
 			break;
-		} else {
+		default:
 			err("Invalid instruction 0x%08x at PC 0x%08x\n", instr, cpu->pc);
 			nios2_dump_registers(cpu);
 			simulator_running = false;
 			break;
 		}
 
+		if (!simulator_running)
+			break;
+
 		nios2_simulate(cpu);
 		device_simulate_all();
 	}
@@ -90,11 +129,6 @@ void simulator_run(struct nios2 *cpu)
 #if 0
 static int32_t simulator_mode = SIM_MODE;
 
-static void set_init_pc(struct NIOS_CPU * cpu, uint32_t pc)
-{
-	cpu->pc = pc;
-}
-
 static void quit_debug_mode(struct NIOS_CPU * cpu)
 {
 	simulator_mode = SIM_MODE;