Use generic load/store functions in instructions

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);