Implement basic instruction handling

8 files changed, 317 insertions, 50 deletions

diff --git a/Makefile b/Makefile
index 5233213..88796be 100644
--- a/Makefile
+++ b/Makefile
@@ -1,13 +1,13 @@
 P	= nios2sim-ng
 VERSION	= 0.1
 
-OBJS	= main.o image.o elf.o srec.o
+OBJS	= main.o image.o elf.o srec.o nios2.o instruction.o
 #OBJS	+= load_image.o nor_flash.o simulator.o simulator.o niosii.o i_type_handler.o j_type_handler.o \
 #	   r_type_handler.o custom_instr.o io_device.o jtag_uart.o timer.o uart_core.o
 
 DEFS	= -DVERSION="\"$(VERSION)\"" -DDEBUG
 
-CFLAGS	+= -g -W -Wall $(DEFS)
+CFLAGS	+= -g -W -Wall -Wno-packed-bitfield-compat $(DEFS)
 LDFLAGS	+= -lelf
 
 all: prepare $(P)
diff --git a/bits.h b/bits.h
new file mode 100644
index 0000000..8374527
--- /dev/null
+++ b/bits.h
@@ -0,0 +1,16 @@
+/*
+ * Copyright (C) 2010 Tobias Klauser <tklauser@distanz.ch>
+ *
+ * This file is part of nios2sim-ng.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#ifndef _BITS_H_
+#define _BITS_H_
+
+#define BIT_MASK(n)	(((n) == 64) ? ~0ULL : ((1ULL << (n))-1))
+
+#endif /* _BITS_H_ */
diff --git a/instruction.c b/instruction.c
index ef7e9f9..b6671d1 100644
--- a/instruction.c
+++ b/instruction.c
@@ -8,24 +8,45 @@
  * for more details.
  */
 
+#include <stdio.h>
+
 #include "nios2sim-ng.h"
 #include "instruction.h"
 #include "nios2.h"
 
+static inline uint32_t get_opcode(uint32_t code)
+{
+	I_TYPE(instr, code);
+	return instr->op;
+}
+
+static inline uint32_t get_opxcode(uint32_t code)
+{
+	R_TYPE(instr, code);
+	return instr->opx.opx11;
+}
+
+static int unsupported(struct nios2 *cpu, uint32_t code)
+{
+	info("Unsupported instructtion %08x @ PC %08x\n", code, cpu->pc);
+
+	return PC_INC_NORMAL;
+}
+
 /*
  * J-Type instructions
  */
 
-uint32_t call(struct nios2 *cpu, uint32_t opcode)
+static int call(struct nios2 *cpu, uint32_t code)
 {
-	J_TYPE(instr, opcode);
+	J_TYPE(instr, code);
 
 	return PC_INC_BY_INSTR;
 }
 
-uint32_t jmpi(struct nios2 *cpu, uint32_t opcode)
+static int jmpi(struct nios2 *cpu, uint32_t code)
 {
-	J_TYPE(instr, opcode);
+	J_TYPE(instr, code);
 
 	return PC_INC_BY_INSTR;
 }
@@ -34,26 +55,179 @@ uint32_t jmpi(struct nios2 *cpu, uint32_t opcode)
  * I-Type instructions
  */
 
-uint32_t ldbu(struct nios2 *cpu, uint32_t opcode)
+static int ldbu(struct nios2 *cpu, uint32_t code)
 {
-	I_TYPE(instr, opcode);
+	I_TYPE(instr, code);
 
 	return PC_INC_NORMAL;
 }
 
-uint32_t addi(struct nios2 *cpu, uint32_t opcode)
+static int addi(struct nios2 *cpu, uint32_t code)
 {
-	I_TYPE(instr, opcode);
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
 
 	/* rB <- rA + IMM16 */
-	cpu->gp_regs[instr->b] = cpu->gp_regs[instr->a] + (int16_t) (instr->imm16);
+	gp_regs[instr->b] = gp_regs[instr->a] + (int16_t) (instr->imm16);
+	return PC_INC_NORMAL;
+}
+
+static int ldhu(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+
+	return PC_INC_NORMAL;
+}
+
+static int andi(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rB <- rA & IMM16 */
+	gp_regs[instr->b] = gp_regs[instr->a] & (int32_t) (instr->imm16 & BIT_MASK(16));
+	return PC_INC_NORMAL;
+}
+
+static int ori(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rB <- rA | IMM16 */
+	gp_regs[instr->b] = gp_regs[instr->a] | (int32_t) (instr->imm16 & BIT_MASK(16));
+	return PC_INC_NORMAL;
+}
+
+static int xori(struct nios2 *cpu, uint32_t code)
+{
+	I_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rB <- rA ^ IMM16 */
+	gp_regs[instr->b] = gp_regs[instr->a] ^ (int32_t) (instr->imm16 & BIT_MASK(16));
+	return PC_INC_NORMAL;
+}
+
+/*
+ * R-Type instructions
+ */
+
+static int and(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA & rB */
+	gp_regs[instr->c] = gp_regs[instr->a] & gp_regs[instr->b];
+	return PC_INC_NORMAL;
+}
+
+static int or(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA | rB */
+	gp_regs[instr->c] = gp_regs[instr->a] | gp_regs[instr->b];
+	return PC_INC_NORMAL;
+}
+
+static int add(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA + rB */
+	gp_regs[instr->c] = gp_regs[instr->a] + gp_regs[instr->b];
+	return PC_INC_NORMAL;
+}
+
+static int sync(struct nios2 *cpu, uint32_t code)
+{
+	/* Nothing to do here */
+	return PC_INC_NORMAL;
+}
 
+static int sub(struct nios2 *cpu, uint32_t code)
+{
+	R_TYPE(instr, code);
+	uint32_t *gp_regs = cpu->gp_regs;
+
+	/* rC <- rA - rB */
+	gp_regs[instr->c] = gp_regs[instr->a] - gp_regs[instr->b];
 	return PC_INC_NORMAL;
 }
 
-instruction_handler i_type_inst_handlers[I_TYPE_COUNT] = {
-	[CALL]	= call,
-	[JMPI]	= jmpi,
-	[LDBU]	= ldbu,
-	[ADDI]	= addi,
+static instruction_handler r_type_instr_handlers[R_TYPE_COUNT] = {
+	[AND]	= and,
+	[OR]	= or,
+	[ADD]	= add,
+
+	[BREAK]	= unsupported,
+	[SYNC]	= sync,
+	[SUB]	= sub,
+	[SRAI]	= unsupported,
+	[SRA]	= unsupported,
+};
+
+static int handle_r_type_instr(struct nios2 *cpu, uint32_t code)
+{
+	uint32_t opx;
+	instruction_handler handle_instr;
+
+	opx = get_opxcode(code);
+	if (unlikely(opx >= R_TYPE_COUNT)) {
+		err("Invalid OPX code %08x\n", opx);
+		return INSTR_ERR;
+	}
+
+	handle_instr = r_type_instr_handlers[opx];
+	if (unlikely(handle_instr == NULL)) {
+		err("Invalid instruction %08x\n", code);
+		return INSTR_ERR;
+	}
+
+	return handle_instr(cpu, code);
+}
+
+static instruction_handler i_type_instr_handlers[I_TYPE_COUNT] = {
+	[CALL]		= call,
+	[JMPI]		= jmpi,
+	[LDBU]		= ldbu,
+	[ADDI]		= addi,
+	[STB]		= unsupported,
+	[BR]		= unsupported,
+	[LDB]		= unsupported,
+	[CMPGEI]	= unsupported,
+	[LDHU]		= ldhu,
+	[ANDI]		= andi,
+	[STH]		= unsupported,
+	[BGE]		= unsupported,
+	[LDH]		= unsupported,
+	[CMPLTI]	= unsupported,
+	[INITDA]	= unsupported,
+	[ORI]		= ori,
+	[STW]		= unsupported,
+	[BLT]		= unsupported,
+	[LDW]		= unsupported,
+	[CMPNEI]	= unsupported,
+	[FLUSHDA]	= unsupported,
+	[XORI]		= xori,
+	[BNE]		= unsupported,
+
+	[BLTU]		= unsupported,
+	[LDWIO]		= unsupported,
+	[R_TYPE]	= handle_r_type_instr,
+	[FLUSHD]	= unsupported,
+	[XORHI]		= unsupported,
 };
+
+instruction_handler instruction_get_handler(uint32_t code)
+{
+	uint32_t op = get_opcode(code);
+
+	if (unlikely(op >= I_TYPE_COUNT))
+		return NULL;
+	return i_type_instr_handlers[op];
+}
diff --git a/instruction.h b/instruction.h
index 57fd1b6..f53c995 100644
--- a/instruction.h
+++ b/instruction.h
@@ -196,9 +196,15 @@ enum {
 };
 #define R_TYPE_COUNT	0x40
 
+#define INSTR_ERR	-1
+#define PC_INC_NORMAL	0
+#define PC_INC_BY_INSTR	1
+
 /* Forward declaration */
 struct nios2;
 
-typedef uint32_t (*instruction_handler)(struct nios2 *cpu, uint32_t opcode);
+typedef int (*instruction_handler)(struct nios2 *cpu, uint32_t opcode);
+
+extern instruction_handler instruction_get_handler(uint32_t code);
 
 #endif /* _INSTRUCTION_H_ */
diff --git a/main.c b/main.c
index 6403d7c..13526bf 100644
--- a/main.c
+++ b/main.c
@@ -31,6 +31,10 @@
 #include "nios2sim-ng.h"
 #include "image.h"
 
+/* XXX: TMP */
+#include "instruction.h"
+#include "nios2.h"
+
 #define PROGRAM_NAME	"nios2sim-ng"
 #define PROGRAM_VERSION	VERSION		/* Set in Makefile */
 
@@ -45,11 +49,13 @@ static void usage(const int status)
 			"                   specify kernel command line\n"
 			"  -m MEMSIZE, --memsize=MEMSIZE\n"
 			"                   set memory size for simulator (default %zu%s)\n"
-			"  -d, --debug      enable debug mode\n"
+			"  -M, --mmu        simulate Nios II with MMU\n"
 			"  -e, --elf        image is in ELF format (default)\n"
 			"  -s, --srec       image is in SREC format\n"
-			"  -h, --help       print this help and exit\n"
+			"  -d, --debug      enable debug mode\n"
+			"  -v, --verbose    enable verbose outputs\n"
 			"  -V, --version    print version information and exit\n"
+			"  -h, --help       print this help and exit\n"
 			"\n"
 			"Example:\n"
 			"  %s -s -m 32M -d -c console=ttyJ0 init=/bin/sh\n"
@@ -60,22 +66,19 @@ static void usage(const int status)
 	exit(status);
 }
 static const struct option long_opts[] = {
+	{ "cmdline", required_argument, NULL, 'c' },
+	{ "memsize", required_argument, NULL, 'm' },
+	{ "mmu", no_argument, NULL, 'M' },
 	{ "elf", no_argument, NULL, 'e' },
 	{ "srec", no_argument, NULL, 's' },
-	{ "mem_size", required_argument, NULL, 'm' },
 	{ "debug", no_argument, NULL, 'd' },
-	{ "base_addr", required_argument, NULL, 'b' },
-	{ "sys_map", required_argument, NULL, 'p' },
-	{ "cmdline", required_argument, NULL, 'c' },
-	{ "initrd", required_argument, NULL, 'r' },
-	{ "fs_image", required_argument, NULL, 'f' },
 	{ "verbose", no_argument, NULL, 'v' },
 	{ "version", no_argument, NULL, 'V' },
 	{ "help", no_argument, NULL, 'h' },
 	{ NULL, 0, NULL, 0 }
 };
 
-static const char *short_opts = "sm:db:p:c:r:f:vVh";
+static const char *short_opts = "c:m:MesdvVh";
 
 static size_t parse_mem_size(char *opt)
 {
@@ -115,37 +118,28 @@ int main(int argc, char *argv[])
 	int image_format = FORMAT_ELF;
 	uint8_t *mem_base;
 	size_t mem_size = DEFAULT_MEM_SIZE;
+	bool has_mmu = false;
 	int c;
 
 	while ((c = getopt_long(argc, argv, short_opts, long_opts, NULL)) != -1) {
 		switch(c) {
+		case 'c':
+			cmdline = optarg;
+			break;
+		case 'm':
+			mem_size = parse_mem_size(optarg);
+			break;
+		case 'M':
+			has_mmu = true;
 		case 'e':
 			image_format = FORMAT_ELF;
 			break;
 		case 's':
 			image_format = FORMAT_SREC;
 			break;
-		case 'm':
-			mem_size = parse_mem_size(optarg);
-			break;
 		case 'd':
 //			set_debug_mode(get_nios_cpu());
 			break;
-		case 'b':
-//			sscanf(addr, "0x%X\n", &image_info.base_addr);
-			break;
-		case 'p':
-//			load_symbol_file(optarg);
-			break;
-		case 'c':
-			cmdline = optarg;
-			break;
-		case 'r':
-//			set_initrd(optarg);
-			break;
-		case 'f':
-//			set_fs_image(optarg);
-			break;
 		case 'v':
 			verbose = true;
 			break;
@@ -173,13 +167,16 @@ int main(int argc, char *argv[])
 		exit(EXIT_FAILURE);
 	}
 
+	memset(mem_base, 0x00, mem_size);
+
 	if (image_load(image_path, image_format, mem_base, mem_size))
 		exit(EXIT_FAILURE);
 
-	vinfo("  Image file:   %s\n", image_path);
-	vinfo("  Image format: %s\n", image_format_str(image_format));
-	vinfo("  Memory size:  %zu %sbytes\n", size_scale(mem_size), size_postfix(mem_size));
-	vinfo("  Memory base:  %p\n", mem_base);
+	vinfo("  Image file:          %s\n", image_path);
+	vinfo("  Image format:        %s\n", image_format_str(image_format));
+	vinfo("  Memory size:         %zu %sbytes\n", size_scale(mem_size), size_postfix(mem_size));
+	vinfo("  Memory base:         %p\n", mem_base);
+	vinfo("  Kernel command line: %s\n", cmdline);
 
 #if 0
 	printf("Base address:0x%08X, Entry address:0x%08X\n",image_info.base_addr,
diff --git a/nios2.c b/nios2.c
new file mode 100644
index 0000000..4586c20
--- /dev/null
+++ b/nios2.c
@@ -0,0 +1,39 @@
+/*
+ * Copyright (C) 2010 Tobias Klauser <tklauser@distanz.ch>
+ *
+ * This file is part of nios2sim-ng.
+ *
+ * This file is subject to the terms and conditions of the GNU General Public
+ * License. See the file "COPYING" in the main directory of this archive
+ * for more details.
+ */
+
+#include <stdio.h>
+#include <string.h>
+
+#include "nios2sim-ng.h"
+#include "nios2.h"
+
+void nios2_cpu_reset(struct nios2 *cpu)
+{
+	memset(cpu, 0x00, sizeof(struct nios2));
+}
+
+uint32_t nios2_cpu_fetch_instr(struct nios2 *cpu, uint32_t *mem_base)
+{
+	uint32_t instr = 0;
+
+	return instr;
+}
+
+int nios2_cpu_execute_instr(struct nios2 *cpu, uint32_t instr)
+{
+	instruction_handler handle_instr = instruction_get_handler(instr);
+
+	if (unlikely(handle_instr == NULL)) {
+		err("Invalid instruction %08x\n", instr);
+		return -1;
+	}
+
+	return handle_instr(cpu, instr);
+}
diff --git a/nios2.h b/nios2.h
index a299bc3..15edbe8 100644
--- a/nios2.h
+++ b/nios2.h
@@ -27,7 +27,41 @@ struct nios2 {
 	uint32_t pc;
 };
 
-#define PC_INC_NORMAL	0
-#define PC_INC_BY_INSTR	1
+/* Aliases for general-purpors registers */
+enum {
+	zero = 0,
+	at,
+	et = 24,
+	bt,
+	gp,
+	sp,
+	fp,
+	ea,
+	ba,
+	ra,
+};
+
+/* Aliases for control registers */
+enum {
+	status = 0,
+	estatus,
+	bstatus,
+	ienable,
+	ipending,
+	cpuid,
+	/* reserved */
+	exception = 7,
+	pteaddr,
+	tlbacc,
+	tlbmisc,
+	/* reserved */
+	badaddr = 12,
+	config,
+	mpubase,
+	mpuacc,
+
+};
+
+extern void nios2_cpu_reset(struct nios2 *cpu);
 
 #endif /* _NIOS2_H_ */
diff --git a/nios2sim-ng.h b/nios2sim-ng.h
index 6b45405..e4f4bcf 100644
--- a/nios2sim-ng.h
+++ b/nios2sim-ng.h
@@ -9,6 +9,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 
+#include "bits.h"
 #include "compiler.h"
 
 extern bool verbose;