From abb6c10f3a5be99396c303e60d286606ddc72e17 Mon Sep 17 00:00:00 2001
From: Tobias Klauser <tklauser@distanz.ch>
Date: Fri, 19 Nov 2010 14:06:15 +0100
Subject: Implement memory load/store operations

---
 nios2.c | 94 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++--
 1 file changed, 92 insertions(+), 2 deletions(-)

(limited to 'nios2.c')

diff --git a/nios2.c b/nios2.c
index 86dee5e..a8ff9c4 100644
--- a/nios2.c
+++ b/nios2.c
@@ -13,6 +13,8 @@
 
 #include "nios2sim-ng.h"
 #include "nios2.h"
+#include "memory.h"
+#include "device.h"
 
 void nios2_cpu_reset(struct nios2 *cpu)
 {
@@ -81,6 +83,94 @@ int nios2_execute_instr(struct nios2 *cpu, uint32_t instr)
 	return handle_instr(cpu, instr);
 }
 
+static bool is_mem_addr(struct memory *mem, uint32_t addr)
+{
+	if (addr >= mem->image_base && addr < (mem->image_base + mem->size))
+		return true;
+	else
+		return false;
+}
+
+int nios2_load_byte(struct nios2 *cpu, uint32_t addr, uint8_t *data)
+{
+	struct memory *mem = cpu->mem;
+
+	dbg("ldb %08x\n", addr);
+
+	if (is_mem_addr(mem, addr)) {
+		dbg("load byte MEM\n");
+		*data = memory_get_byte(mem, addr);
+	} else {	/* must be I/O */
+		struct device *dev = device_get_by_addr(addr);
+		dbg("load byte 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) {
+			err("Read from device '%s' failed\n", dev->name);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
+int nios2_store_byte(struct nios2 *cpu, uint32_t addr, uint8_t data)
+{
+	struct memory *mem = cpu->mem;
+
+	dbg("stb %02x @ %08x\n", data, 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);
+			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");
+
+		if (unlikely(dev == NULL)) {
+			err("Reading 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);
+			return -1;
+		}
+	}
+
+	return 0;
+}
+
 static const char *nios2_gp_registers[] = {
 	"zero",
 	"at",
@@ -143,14 +233,14 @@ void nios2_dump_registers(struct nios2 *cpu)
 
 	info("General-purpose registers:\n");
 	for (i = 0; i < NIOS2_GP_REG_COUNT; i++) {
-		info("  %8s 0x%08x", nios2_gp_registers[i], gp_regs[i]);
+		info(" %10s 0x%08x", nios2_gp_registers[i], gp_regs[i]);
 		if ((i + 1) % 4 == 0)
 			info("\n");
 	}
 
 	info("Control registers:\n");
 	for (i = 0; i < NIOS2_CTRL_REG_COUNT; i++) {
-		info("  %10s 0x%08x", nios2_ctrl_registers[i], ctrl_regs[i]);
+		info(" %10s 0x%08x", nios2_ctrl_registers[i], ctrl_regs[i]);
 		if ((i + 1) % 4 == 0)
 			info("\n");
 	}
-- 
cgit v1.2.3-54-g00ecf