From 9a1312e741193778718ed37b3779c2b964649e9d Mon Sep 17 00:00:00 2001 From: Tobias Klauser Date: Wed, 24 Nov 2010 18:35:39 +0100 Subject: Change exception handling, more instructions implemented --- instruction.c | 73 ++++++++++++++++++++++++++++++++++++----------------- instruction.h | 15 +++++++---- nios2.c | 36 +++++++++++++++++++++----- nios2.h | 81 +++++++++++++++++++++++++++++++++++++++++++++++++++++------ simulator.c | 68 ++++++++++++++++++++++++++++++++++++------------- 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 #include #include #include @@ -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; -- cgit v1.2.3-54-g00ecf