From bba31d7fbc06bfdac175e38649ca9b2331e44a5a Mon Sep 17 00:00:00 2001 From: Tobias Klauser Date: Mon, 22 Nov 2010 11:32:41 +0100 Subject: Use generic load/store functions in instructions --- instruction.c | 50 ++++++++++++++++++++++++++++++--------- memory.c | 10 ++++++++ memory.h | 2 ++ nios2.c | 76 +++++++++++++++++++++++++++++------------------------------ nios2.h | 5 ++-- 5 files changed, 91 insertions(+), 52 deletions(-) diff --git a/instruction.c b/instruction.c index 0ab99c8..6fd06dc 100644 --- a/instruction.c +++ b/instruction.c @@ -60,7 +60,7 @@ static int call(struct nios2 *cpu, uint32_t code) J_TYPE(instr, code); cpu->gp_regs[ra] = cpu->pc + 4; - cpu->pc = (instr->imm26 * 4) | (cpu->pc & 0xF0000000); + cpu->pc = (cpu->pc & 0xF0000000) | (instr->imm26 * 4); return PC_INC_BY_INSTR; } @@ -70,7 +70,7 @@ static int jmpi(struct nios2 *cpu, uint32_t code) { J_TYPE(instr, code); - cpu->pc = (instr->imm26 * 4) | (cpu->pc & 0xF0000000); + cpu->pc = (cpu->pc & 0xF0000000) | (instr->imm26 * 4); return PC_INC_BY_INSTR; } @@ -87,7 +87,7 @@ static int ldbu(struct nios2 *cpu, uint32_t code) uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16; uint8_t byte; - if (nios2_load_byte(cpu, addr, &byte)) + if (nios2_load(cpu, addr, &byte, 1)) return INSTR_ERR; gp_regs[instr->b] = (uint32_t) byte; @@ -113,8 +113,9 @@ static int stb(struct nios2 *cpu, uint32_t code) I_TYPE(instr, code); uint32_t *gp_regs = cpu->gp_regs; uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16; + uint8_t data = gp_regs[instr->b] & 0xFF; - if (nios2_store_byte(cpu, addr, gp_regs[instr->b] & 0xFF)) + if (nios2_store(cpu, addr, &data, 1)) return INSTR_ERR; return PC_INC_NORMAL; @@ -138,7 +139,7 @@ static int ldb(struct nios2 *cpu, uint32_t code) uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16; uint8_t byte; - if (nios2_load_byte(cpu, addr, &byte)) + if (nios2_load(cpu, addr, &byte, 1)) return INSTR_ERR; gp_regs[instr->b] = (int32_t) byte; @@ -146,10 +147,37 @@ static int ldb(struct nios2 *cpu, uint32_t code) return PC_INC_NORMAL; } -/* */ -static int ldhu(struct nios2 *cpu __unused, uint32_t code __unused) +/* + * if ((signed) rA >= (signed) @(IMM16)) + * rB <- 1 + * else + * rB <- 0 + */ +static int cmpgei(struct nios2 *cpu, uint32_t code) { - /* TODO */ + I_TYPE(instr, code); + uint32_t *gp_regs = cpu->gp_regs; + + if ((int32_t) gp_regs[instr->a] >= (int32_t) ((int16_t) instr->imm16)) + gp_regs[instr->b] = 1; + else + gp_regs[instr->b] = 0; + + return PC_INC_NORMAL; +} + +/* rB <- 0x0000 : Mem16[rA + @IMM16)] */ +static int ldhu(struct nios2 *cpu, uint32_t code) +{ + I_TYPE(instr, code); + uint32_t *gp_regs = cpu->gp_regs; + uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16; + uint16_t halfword; + + if (nios2_load(cpu, addr, &halfword, 2)) + return INSTR_ERR; + + gp_regs[instr->b] = (int32_t) halfword; return PC_INC_NORMAL; } @@ -199,7 +227,7 @@ static int ldh(struct nios2 *cpu __unused, uint32_t code __unused) } /* - * if ((signed) rA < (signed) @(IMM16) + * if ((signed) rA < (signed) @(IMM16)) * rB <- 1 * else * rB <- 0 @@ -268,7 +296,7 @@ static int ldw(struct nios2 *cpu, uint32_t code) uint32_t *gp_regs = cpu->gp_regs; uint32_t addr = gp_regs[instr->a] + (int16_t) instr->imm16; - if (nios2_load_word(cpu, addr, &gp_regs[instr->b])) + if (nios2_load(cpu, addr, &gp_regs[instr->b], 4)) return INSTR_ERR; return PC_INC_NORMAL; @@ -409,7 +437,7 @@ static struct instruction i_type_instructions[I_TYPE_COUNT] = { [STB] = INSTRUCTION(stb), [BR] = INSTRUCTION(br), [LDB] = INSTRUCTION(ldb), - [CMPGEI] = INSTRUCTION_UNIMPLEMENTED(cmpgei), + [CMPGEI] = INSTRUCTION(cmpgei), [LDHU] = INSTRUCTION(ldhu), [ANDI] = INSTRUCTION(andi), [STH] = INSTRUCTION(sth), diff --git a/memory.c b/memory.c index 553766a..226eadf 100644 --- a/memory.c +++ b/memory.c @@ -27,6 +27,16 @@ void memory_set_byte(struct memory *mem, int32_t addr, uint8_t data) base[addr - mem->image_base] = data; } +uint16_t memory_get_halfword(struct memory *mem, int32_t addr) +{ + return mem->base[(addr - mem->image_base) / 2]; +} + +void memory_set_halfword(struct memory *mem, int32_t addr, uint16_t data) +{ + mem->base[(addr - mem->image_base) / 2] = data; +} + uint32_t memory_get_word(struct memory *mem, int32_t addr) { return mem->base[(addr - mem->image_base) / 4]; diff --git a/memory.h b/memory.h index 80de90b..191b190 100644 --- a/memory.h +++ b/memory.h @@ -22,6 +22,8 @@ struct memory { extern uint8_t memory_get_byte(struct memory *mem, int32_t addr); extern void memory_set_byte(struct memory *mem, int32_t addr, uint8_t data); +extern uint16_t memory_get_halfword(struct memory *mem, int32_t addr); +extern void memory_set_halfword(struct memory *mem, int32_t addr, uint16_t data); extern uint32_t memory_get_word(struct memory *mem, int32_t addr); extern void memory_set_word(struct memory *mem, int32_t addr, uint32_t data); diff --git a/nios2.c b/nios2.c index a8ff9c4..2b2fec2 100644 --- a/nios2.c +++ b/nios2.c @@ -91,25 +91,38 @@ static bool is_mem_addr(struct memory *mem, uint32_t addr) return false; } -int nios2_load_byte(struct nios2 *cpu, uint32_t addr, uint8_t *data) +int nios2_load(struct nios2 *cpu, uint32_t addr, void *data, size_t size) { struct memory *mem = cpu->mem; - dbg("ldb %08x\n", addr); + dbg("load%zu %08x\n", size * 8, addr); if (is_mem_addr(mem, addr)) { - dbg("load byte MEM\n"); - *data = memory_get_byte(mem, addr); + dbg("load MEM\n"); + switch (size) { + case 1: + *((uint8_t *) data) = memory_get_byte(mem, addr); + break; + case 2: + *((uint16_t *) data) = memory_get_halfword(mem, addr); + break; + case 4: + *((uint32_t *) data) = memory_get_word(mem, addr); + break; + default: + err("Invalid size for load at %08x: %zu\n", addr, size); + return -1; + } } else { /* must be I/O */ struct device *dev = device_get_by_addr(addr); - dbg("load byte I/O\n"); + dbg("load I/O\n"); if (unlikely(dev == NULL)) { err("Reading data outside of device range (0x%08x)\n", addr); return -1; } - if (dev->read(dev, addr, data, 1) != 1) { + if (dev->read(dev, addr, data, size) != 1) { err("Read from device '%s' failed\n", dev->name); return -1; } @@ -118,52 +131,39 @@ int nios2_load_byte(struct nios2 *cpu, uint32_t addr, uint8_t *data) return 0; } -int nios2_store_byte(struct nios2 *cpu, uint32_t addr, uint8_t data) +int nios2_store(struct nios2 *cpu, uint32_t addr, void *data, size_t size) { struct memory *mem = cpu->mem; - dbg("stb %02x @ %08x\n", data, addr); + dbg("store%zu %08x\n", size, addr); if (is_mem_addr(mem, addr)) { - dbg("store byte MEM\n"); - memory_set_byte(mem, addr, data); - } else { /* must be I/O */ - struct device *dev = device_get_by_addr(addr); - dbg("store byte I/O\n"); - if (unlikely(dev == NULL)) { - err("Writing data outside of device range (0x%08x)\n", addr); + dbg("store MEM\n"); + switch (size) { + case 1: + memory_set_byte(mem, addr, *((uint8_t *) data)); + break; + case 2: + memory_set_halfword(mem, addr, *((uint16_t *) data)); + break; + case 4: + memory_set_word(mem, addr, *((uint32_t *) data)); + break; + default: + err("Invalid size for store at %08x: %zu\n", addr, size); return -1; } - - if (dev->write(dev, addr, &data, 1) != 1) { - err("Write to device '%s' failed\n", dev->name); - return -1; - } - } - - return 0; -} - -int nios2_load_word(struct nios2 *cpu, uint32_t addr, uint32_t *data) -{ - struct memory *mem = cpu->mem; - - dbg("ldw %08x\n", addr); - - if (is_mem_addr(mem, addr)) { - dbg("load word MEM\n"); - *data = memory_get_word(mem, addr); } else { /* must be I/O */ struct device *dev = device_get_by_addr(addr); - dbg("load byte I/O\n"); + dbg("store I/O\n"); if (unlikely(dev == NULL)) { - err("Reading data outside of device range (0x%08x)\n", addr); + err("Writing data outside of device range (0x%08x)\n", addr); return -1; } - if (dev->read(dev, addr, (uint8_t *) data, 4) != 1) { - err("Read from device '%s' failed\n", dev->name); + if (dev->write(dev, addr, data, size) != 1) { + err("Write to device '%s' failed\n", dev->name); return -1; } } diff --git a/nios2.h b/nios2.h index b4ab4ff..a24ab1d 100644 --- a/nios2.h +++ b/nios2.h @@ -105,9 +105,8 @@ extern void nios2_exception(struct nios2 *cpu, uint8_t cause); extern uint32_t nios2_fetch_instr(struct nios2 *cpu); extern int nios2_execute_instr(struct nios2 *cpu, uint32_t instr); -extern int nios2_load_byte(struct nios2 *cpu, uint32_t addr, uint8_t *data); -extern int nios2_store_byte(struct nios2 *cpu, uint32_t addr, uint8_t data); -extern int nios2_load_word(struct nios2 *cpu, uint32_t addr, uint32_t *data); +extern int nios2_load(struct nios2 *cpu, uint32_t addr, void *data, size_t size); +extern int nios2_store(struct nios2 *cpu, uint32_t addr, void *data, size_t size); extern void nios2_dump_registers(struct nios2 *cpu); -- cgit v1.2.3-54-g00ecf