Implemented CPU core emulator.

dis.h

@@ -171,8 +171,6 @@ typedef enum {
 	MSP430_OP_TST           = 0x10017
 } msp430_op_t;
 
-#define MSP430_OP_IS_JUMP(o) ((o) >= MSP430_OP_JNZ && (o) <= MSP430_OP_JMP)
-
 /* This represents a decoded instruction. All decoded addresses are
  * absolute or register-indexed, depending on the addressing mode.
  *

main.c

@@ -21,7 +21,6 @@
 #include <stdlib.h>
 #include <string.h>
 #include <errno.h>
-#include <signal.h>
 #include <unistd.h>
 
 #include "dis.h"
@@ -736,10 +735,6 @@ static int cmd_help(char **arg)
 	return 0;
 }
 
-static void sigint_handler(int signum)
-{
-}
-
 static void process_command(char *arg)
 {
 	const char *cmd_text;
@@ -758,14 +753,9 @@ static void process_command(char *arg)
 
 static void reader_loop(void)
 {
-	const static struct sigaction siga = {
-		.sa_handler = sigint_handler,
-		.sa_flags = 0
-	};
-
 	printf("\n");
 	cmd_help(NULL);
-	sigaction(SIGINT, &siga, NULL);
+	ctrlc_init();
 
 	for (;;) {
 		char buf[128];
@@ -810,7 +800,7 @@ static void usage(const char *progname)
 "    -B device\n"
 "        Debug the FET itself through the bootloader.\n"
 "    -s\n"
-"        Start in simulation mode (only memory IO is allowed).\n"
+"        Start in simulation mode.\n"
 "\n"
 "By default, the first RF2500 device on the USB bus is opened.\n"
 "\n"

mspdebug.man

@@ -60,9 +60,8 @@ this would allow firmware updates to FET430UIF devices.
 The argument should be the filename of the appropriate tty device.
 .IP "\-s"
 Do not connect to any hardware device, but instead start in simulation
-mode. A 64k buffer is allocated to simulate the device memory. All of
+mode. A 64k buffer is allocated to simulate the device memory. The CPU
-the memory inspection and programming commands can be used on this
+core alone is emulated (no peripheral emulation).
-buffer, but CPU control is not supported.
 
 This mode is intended for testing of changes to MSPDebug, and for
 aiding the disassembly of MSP430 binaries (as all binary and symbol

sim.c

@@ -20,10 +20,352 @@
 #include <string.h>
 #include <stdio.h>
 #include "device.h"
+#include "dis.h"
+#include "util.h"
 
 #define MEM_SIZE	65536
 
 static u_int8_t *memory;
+static u_int16_t sim_regs[DEVICE_NUM_REGS];
+
+#define MEM_GETB(offset) (memory[offset])
+#define MEM_SETB(offset, value) (memory[offset] = (value))
+#define MEM_GETW(offset) \
+	(memory[offset] | (memory[(offset + 1) & 0xffff] << 8))
+#define MEM_SETW(offset, value) \
+	do {							\
+		memory[offset] = (value) & 0xff;		\
+		memory[(offset + 1) & 0xffff] = (value) >> 8;	\
+	} while (0);
+
+static u_int16_t fetch_operand(int amode, int reg, int is_byte,
+			       u_int16_t *addr_ret)
+{
+	u_int16_t addr = 0;
+	u_int32_t mask = is_byte ? 0xff : 0xffff;
+
+	switch (amode) {
+	case MSP430_AMODE_REGISTER:
+		if (reg == MSP430_REG_R3)
+			return 0;
+		return sim_regs[reg] & mask;
+
+	case MSP430_AMODE_INDEXED:
+		if (reg == MSP430_REG_R3)
+			return 1;
+
+		addr = MEM_GETW(sim_regs[MSP430_REG_PC]);
+		sim_regs[MSP430_REG_PC] += 2;
+
+		if (reg != MSP430_REG_SR)
+			addr += sim_regs[reg];
+		break;
+
+	case MSP430_AMODE_INDIRECT:
+		if (reg == MSP430_REG_SR)
+			return 4;
+		if (reg == MSP430_REG_R3)
+			return 2;
+		addr = sim_regs[reg];
+		break;
+
+	case MSP430_AMODE_INDIRECT_INC:
+		if (reg == MSP430_REG_SR)
+			return 8;
+		if (reg == MSP430_REG_R3)
+			return mask;
+		addr = sim_regs[reg];
+		sim_regs[reg] += 2;
+		break;
+	}
+
+	if (addr_ret)
+		*addr_ret = addr;
+
+	return MEM_GETW(addr) & mask;
+}
+
+static void store_operand(int amode, int reg, int is_byte,
+			  u_int16_t addr, u_int16_t data)
+{
+	if (amode == MSP430_AMODE_REGISTER)
+		sim_regs[reg] = data;
+	else if (is_byte)
+		MEM_SETB(addr, data);
+	else
+		MEM_SETW(addr, data);
+}
+
+#define ARITH_BITS (MSP430_SR_V | MSP430_SR_N | MSP430_SR_Z | MSP430_SR_C)
+
+static int step_double(u_int16_t ins)
+{
+	u_int16_t opcode = ins & 0xf000;
+	int sreg = (ins >> 8) & 0xf;
+	int amode_dst = (ins >> 7) & 1;
+	int is_byte = ins & 0x0040;
+	int amode_src = (ins >> 4) & 0x3;
+	int dreg = ins & 0x000f;
+	u_int32_t src_data;
+	u_int16_t dst_addr = 0;
+	u_int32_t dst_data;
+	u_int32_t res_data;
+	u_int32_t msb = is_byte ? 0x80 : 0x8000;
+
+	src_data = fetch_operand(amode_src, sreg, is_byte, NULL);
+	dst_data = fetch_operand(amode_dst, dreg, is_byte, &dst_addr);
+
+	switch (opcode) {
+	case MSP430_OP_MOV:
+		res_data = src_data;
+		break;
+
+	case MSP430_OP_SUB:
+	case MSP430_OP_SUBC:
+	case MSP430_OP_CMP:
+		src_data = ~src_data;
+	case MSP430_OP_ADD:
+	case MSP430_OP_ADDC:
+		if (opcode == MSP430_OP_ADDC || opcode == MSP430_OP_SUBC)
+			res_data = (sim_regs[MSP430_REG_SR] &
+				    MSP430_SR_C) ? 1 : 0;
+		else if (opcode == MSP430_OP_SUB)
+			res_data = 1;
+		else
+			res_data = 0;
+
+		res_data += src_data;
+		res_data += dst_data;
+
+		sim_regs[MSP430_REG_SR] &= ~ARITH_BITS;
+		if (!res_data)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_Z;
+		if (res_data & msb)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_N;
+		if (res_data & (msb << 1))
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_C;
+		if (!((src_data ^ dst_data) & msb) &&
+		    (src_data ^ dst_data) & msb)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_V;
+		break;
+
+	case MSP430_OP_DADD:
+		res_data = src_data + dst_data;
+		if (sim_regs[MSP430_REG_SR] & MSP430_SR_C)
+			res_data++;
+
+		sim_regs[MSP430_REG_SR] &= ~ARITH_BITS;
+		if (!res_data)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_Z;
+		if (res_data == 1)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_N;
+		if ((is_byte && res_data > 99) ||
+		    (!is_byte && res_data > 9999))
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_C;
+		break;
+
+	case MSP430_OP_BIT:
+	case MSP430_OP_AND:
+		res_data = src_data & dst_data;
+
+		sim_regs[MSP430_REG_SR] &= ~ARITH_BITS;
+		sim_regs[MSP430_REG_SR] |=
+			res_data ? MSP430_SR_C : MSP430_SR_Z;
+		if (res_data & msb)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_N;
+		break;
+
+	case MSP430_OP_BIC:
+		res_data = dst_data & ~src_data;
+		break;
+
+	case MSP430_OP_BIS:
+		res_data = dst_data | src_data;
+		break;
+
+	case MSP430_OP_XOR:
+		res_data = dst_data ^ src_data;
+		sim_regs[MSP430_REG_SR] &= ~ARITH_BITS;
+		sim_regs[MSP430_REG_SR] |=
+			res_data ? MSP430_SR_C : MSP430_SR_Z;
+		if (res_data & msb)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_N;
+		if (src_data & dst_data & msb)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_V;
+		break;
+
+	default:
+		fprintf(stderr, "sim: invalid double-operand opcode: "
+			"0x%04x (PC = 0x%04x)\n",
+			opcode, sim_regs[MSP430_REG_PC]);
+		return -1;
+	}
+
+	if (opcode != MSP430_OP_CMP && opcode != MSP430_OP_BIT)
+		store_operand(amode_dst, dreg, is_byte, dst_addr, res_data);
+
+	return 0;
+}
+
+static int step_single(u_int16_t ins)
+{
+	u_int16_t opcode = ins & 0xff80;
+	int is_byte = ins & 0x0040;
+	int amode = (ins >> 4) & 0x3;
+	int reg = ins & 0x000f;
+	u_int16_t msb = is_byte ? 0x80 : 0x8000;
+	u_int16_t src_addr = 0;
+	u_int32_t src_data;
+	u_int32_t res_data;
+
+	src_data = fetch_operand(amode, reg, is_byte, &src_addr);
+
+	switch (opcode) {
+	case MSP430_OP_RRC:
+	case MSP430_OP_RRA:
+		res_data = (src_data >> 1) & ~msb;
+		if (opcode == MSP430_OP_RRC) {
+			if (sim_regs[MSP430_REG_SR] & MSP430_SR_C)
+				res_data |= msb;
+		} else {
+			res_data |= src_data & msb;
+		}
+
+		sim_regs[MSP430_REG_SR] &= ~ARITH_BITS;
+		if (!res_data)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_Z;
+		if (res_data & msb)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_N;
+		if (src_data & 1)
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_C;
+		break;
+
+	case MSP430_OP_SWPB:
+		res_data = ((src_data & 0xff) << 8) | ((src_data >> 8) & 0xff);
+		break;
+
+	case MSP430_OP_SXT:
+		res_data = src_data & 0xff;
+		sim_regs[MSP430_REG_SR] &= ~ARITH_BITS;
+
+		if (src_data & 0x80) {
+			res_data |= 0xff00;
+			sim_regs[MSP430_REG_SR] |= MSP430_SR_N;
+		}
+
+		sim_regs[MSP430_REG_SR] |= res_data ? MSP430_SR_C : MSP430_SR_Z;
+		break;
+
+	case MSP430_OP_PUSH:
+		sim_regs[MSP430_REG_SP] -= 2;
+		MEM_SETW(sim_regs[MSP430_REG_SP], src_data);
+		break;
+
+	case MSP430_OP_CALL:
+		sim_regs[MSP430_REG_SP] -= 2;
+		MEM_SETW(sim_regs[MSP430_REG_SP], sim_regs[MSP430_REG_PC]);
+		sim_regs[MSP430_REG_PC] = src_data;
+		break;
+
+	case MSP430_OP_RETI:
+		sim_regs[MSP430_REG_SR] = MEM_GETW(sim_regs[MSP430_REG_SP]);
+		sim_regs[MSP430_REG_SP] += 2;
+		sim_regs[MSP430_REG_PC] = MEM_GETW(sim_regs[MSP430_REG_SP]);
+		sim_regs[MSP430_REG_SP] += 2;
+		break;
+
+	default:
+		fprintf(stderr, "sim: unknown single-operand opcode: 0x%04x "
+			"(PC = 0x%04x)\n", opcode, sim_regs[MSP430_REG_PC]);
+		return -1;
+	}
+
+	if (opcode != MSP430_OP_PUSH && opcode != MSP430_OP_CALL &&
+	    opcode != MSP430_OP_RETI)
+		store_operand(amode, reg, is_byte, src_addr, res_data);
+
+	return 0;
+}
+
+static int step_jump(u_int16_t ins)
+{
+	u_int16_t opcode = ins & 0xfc00;
+	u_int16_t pc_offset = (ins & 0x03ff) << 1;
+	u_int16_t sr = sim_regs[MSP430_REG_SR];
+
+	if (pc_offset & 0x0400)
+		pc_offset |= 0xff800;
+
+	switch (opcode) {
+	case MSP430_OP_JNZ:
+		sr = !(sr & MSP430_SR_Z);
+		break;
+
+	case MSP430_OP_JZ:
+		sr &= MSP430_SR_Z;
+		break;
+
+	case MSP430_OP_JNC:
+		sr = !(sr & MSP430_SR_C);
+		break;
+
+	case MSP430_OP_JC:
+		sr &= MSP430_SR_C;
+		break;
+
+	case MSP430_OP_JN:
+		sr &= MSP430_SR_N;
+		break;
+
+	case MSP430_OP_JGE:
+		sr = ((sr & MSP430_SR_N) ? 1 : 0) !=
+			((sr & MSP430_SR_V) ? 1 : 0);
+		break;
+
+	case MSP430_OP_JL:
+		sr = ((sr & MSP430_SR_N) ? 1 : 0) ==
+			((sr & MSP430_SR_V) ? 1 : 0);
+		break;
+
+	case MSP430_OP_JMP:
+		sr = 1;
+		break;
+	}
+
+	if (sr)
+		sim_regs[MSP430_REG_PC] += pc_offset;
+
+	return 0;
+}
+
+static int step_cpu(void)
+{
+	u_int16_t ins;
+
+	/* Fetch the instruction */
+	ins = MEM_GETW(sim_regs[MSP430_REG_PC]);
+	sim_regs[0] += 2;
+
+	/* Handle different instruction types */
+	if ((ins & 0xf000) >= 0x4000)
+		return step_double(ins);
+	else if ((ins & 0xf000) >= 0x2000)
+		return step_jump(ins);
+	else
+		return step_single(ins);
+}
+
+/************************************************************************
+ * Device interface
+ */
+
+static enum {
+	RUN_HALTED = 0,
+	RUN_FREE,
+	RUN_TO_BREAKPOINT
+} run_mode;
+
+static u_int16_t run_breakpoint;
 
 static void sim_close(void)
 {
@@ -36,6 +378,11 @@ static void sim_close(void)
 static int sim_control(device_ctl_t action)
 {
 	switch (action) {
+	case DEVICE_CTL_RESET:
+		memset(sim_regs, 0, sizeof(sim_regs));
+		sim_regs[MSP430_REG_PC] = MEM_GETW(0xfffe);
+		break;
+
 	case DEVICE_CTL_ERASE:
 		memset(memory, 0xff, MEM_SIZE);
 		return 0;
@@ -43,9 +390,16 @@ static int sim_control(device_ctl_t action)
 	case DEVICE_CTL_HALT:
 		return 0;
 
-	default:
+	case DEVICE_CTL_STEP:
-		fprintf(stderr, "sim: CPU control is not implemented\n");
+		return step_cpu();
-		break;
+
+	case DEVICE_CTL_RUN_BP:
+		run_mode = RUN_TO_BREAKPOINT;
+		return 0;
+
+	case DEVICE_CTL_RUN:
+		run_mode = RUN_FREE;
+		return 0;
 	}
 
 	return -1;
@@ -53,23 +407,46 @@ static int sim_control(device_ctl_t action)
 
 static int sim_wait(void)
 {
+	if (run_mode != RUN_HALTED) {
+		ctrlc_reset();
+
+		for (;;) {
+			if (run_mode == RUN_TO_BREAKPOINT &&
+			    sim_regs[MSP430_REG_PC] == run_breakpoint)
+				break;
+
+			if (ctrlc_check()) {
+				run_mode = RUN_HALTED;
+				return 1;
+			}
+
+			if (step_cpu() < 0) {
+				run_mode = RUN_HALTED;
+				return -1;
+			}
+		}
+
+		run_mode = RUN_HALTED;
+	}
+
 	return 0;
 }
 
 static int sim_breakpoint(u_int16_t addr)
 {
+	run_breakpoint = addr;
 	return 0;
 }
 
 static int sim_getregs(u_int16_t *regs)
 {
-	fprintf(stderr, "sim: register fetch is not implemented\n");
+	memcpy(regs, sim_regs, sizeof(sim_regs));
-	return -1;
+	return 0;
 }
 
 static int sim_setregs(const u_int16_t *regs)
 {
-	fprintf(stderr, "sim: register store is not implemented\n");
+	memcpy(sim_regs, regs, sizeof(regs));
 	return -1;
 }
 
@@ -110,6 +487,7 @@ const struct device *sim_open(void)
 		return NULL;
 	}
 
+	memset(memory, 0xff, MEM_SIZE);
 	printf("Simulation started, 0x%x bytes of RAM\n", MEM_SIZE);
 	return &sim_device;
 }

util.c

@@ -22,6 +22,7 @@
 #include <termios.h>
 #include <unistd.h>
 #include <errno.h>
+#include <signal.h>
 #include "util.h"
 
 void hexdump(int addr, const u_int8_t *data, int len)
@@ -175,3 +176,30 @@ int open_serial(const char *device, int rate)
 
 	return fd;
 }
+
+static volatile int ctrlc_flag;
+
+static void sigint_handler(int signum)
+{
+	ctrlc_flag = 1;
+}
+
+void ctrlc_init(void)
+{
+	const static struct sigaction siga = {
+		.sa_handler = sigint_handler,
+		.sa_flags = 0
+	};
+
+	sigaction(SIGINT, &siga, NULL);
+}
+
+void ctrlc_reset(void)
+{
+	ctrlc_flag = 0;
+}
+
+int ctrlc_check(void)
+{
+	return ctrlc_flag;
+}

util.h

@@ -31,4 +31,8 @@ int open_serial(const char *device, int rate);
 int read_with_timeout(int fd, u_int8_t *data, int len);
 int write_all(int fd, const u_int8_t *data, int len);
 
+void ctrlc_init(void);
+void ctrlc_reset(void);
+int ctrlc_check(void);
+
 #endif