mspdebug/sim.c

/* MSPDebug - debugging tool for the eZ430
 * Copyright (C) 2009, 2010 Daniel Beer
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#include <stdlib.h>
#include <string.h>
#include <stdio.h>
#include <ctype.h>
#include "device.h"
#include "dis.h"
#include "util.h"
#include "stab.h"
#include "parse.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);

#define MEM_IO_END 0x200

/* PC at the start of the current instruction */
static u_int16_t current_insn;

static void io_prefix(const char *prefix, u_int16_t addr, int is_byte)
{
	char name[64];
	u_int16_t pc = current_insn;

	if (!stab_nearest(pc, name, sizeof(name), &pc)) {
		printf("%s", name);
		if (pc)
			printf("+0x%x", pc);
	} else {
		printf("0x%04x", pc);
	}

	printf(": IO %s.%c: 0x%04x", prefix, is_byte ? 'B' : 'W', addr);
	if (!stab_nearest(addr, name, sizeof(name), &addr)) {
		printf(" (%s", name);
		if (addr)
			printf("+0x%x", addr);
		printf(")");
	}
}

static int fetch_io(u_int16_t addr, int is_byte, u_int32_t *data_ret)
{
	io_prefix("READ", addr, is_byte);

	for (;;) {
		char text[128];
		int len;
		int data;

		printf("? ");
		fflush(stdout);
		if (!fgets(text, sizeof(text), stdin)) {
			printf("\nAborted IO request\n");
			return -1;
		}

		len = strlen(text);
		while (len && isspace(text[len - 1]))
			len--;
		text[len] = 0;

		if (!len) {
			if (data_ret) {
				if (is_byte)
					*data_ret = MEM_GETB(addr);
				else
					*data_ret = MEM_GETW(addr);
			}
			return 0;
		}

		if (!addr_exp(text, &data)) {
			if (data_ret)
				*data_ret = data;
			return 0;
		}
	}

	return 0;
}

static void store_io(u_int16_t addr, int is_byte, u_int16_t data)
{
	io_prefix("WRITE", addr, is_byte);

	if (is_byte) {
		printf(" => 0x%02x\n", data & 0xff);
		MEM_SETB(addr, data & 0xff);
	} else {
		printf(" => 0x%04x\n", data);
		MEM_SETW(addr, data);
	}
}

static int fetch_operand(int amode, int reg, int is_byte,
			 u_int16_t *addr_ret, u_int32_t *data_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) {
			if (data_ret)
				*data_ret = 0;
			return 0;
		}
		if (data_ret)
			*data_ret = sim_regs[reg] & mask;
		return 0;

	case MSP430_AMODE_INDEXED:
		if (reg == MSP430_REG_R3) {
			if (data_ret)
				*data_ret = 1;
			return 0;
		}

		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) {
			if (data_ret)
				*data_ret = 4;
			return 0;
		}

		if (reg == MSP430_REG_R3) {
			if (data_ret)
				*data_ret = 2;
			return 0;
		}
		addr = sim_regs[reg];
		break;

	case MSP430_AMODE_INDIRECT_INC:
		if (reg == MSP430_REG_SR) {
			if (data_ret)
				*data_ret = 8;
			return 0;
		}
		if (reg == MSP430_REG_R3) {
			if (data_ret)
				*data_ret = mask;
			return 0;
		}
		addr = sim_regs[reg];
		sim_regs[reg] += 2;
		break;
	}

	if (addr_ret)
		*addr_ret = addr;

	if (data_ret) {
		if (addr < MEM_IO_END)
			return fetch_io(addr, is_byte, data_ret);

		*data_ret = MEM_GETW(addr) & mask;
	}

	return 0;
}

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 (addr < MEM_IO_END)
		store_io(addr, is_byte, 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;
	u_int32_t mask = is_byte ? 0xff : 0xffff;

	if (fetch_operand(amode_src, sreg, is_byte, NULL, &src_data) < 0)
		return -1;
	if (fetch_operand(amode_dst, dreg, is_byte, &dst_addr,
			  opcode == MSP430_OP_MOV ? NULL : &dst_data) < 0)
		return -1;

	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 || opcode == MSP430_OP_CMP)
			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 & mask))
			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 & mask))
			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 & mask) ? 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 & mask) ? 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, current_insn);
		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_int32_t mask = is_byte ? 0xff : 0xffff;
	u_int16_t src_addr = 0;
	u_int32_t src_data;
	u_int32_t res_data;

	if (fetch_operand(amode, reg, is_byte, &src_addr, &src_data) < 0)
		return -1;

	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 & mask))
			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 & mask) ? 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, current_insn);
		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;
	int ret;

	/* Fetch the instruction */
	current_insn = sim_regs[MSP430_REG_PC];
	ins = MEM_GETW(current_insn);
	sim_regs[MSP430_REG_PC] += 2;

	/* Handle different instruction types */
	if ((ins & 0xf000) >= 0x4000)
		ret = step_double(ins);
	else if ((ins & 0xf000) >= 0x2000)
		ret = step_jump(ins);
	else
		ret = step_single(ins);

	/* If things went wrong, restart at the current instruction */
	if (ret < 0)
		sim_regs[MSP430_REG_PC] = current_insn;

	return ret;
}

/************************************************************************
 * 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)
{
	if (memory) {
		free(memory);
		memory = NULL;
	}
}

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

	case DEVICE_CTL_ERASE:
		memset(memory, 0xff, MEM_SIZE);
		return 0;

	case DEVICE_CTL_HALT:
		run_mode = RUN_HALTED;
		return 0;

	case DEVICE_CTL_STEP:
		return step_cpu();

	case DEVICE_CTL_RUN_BP:
		run_mode = RUN_TO_BREAKPOINT;
		ctrlc_reset();
		return 0;

	case DEVICE_CTL_RUN:
		run_mode = RUN_FREE;
		ctrlc_reset();
		return 0;
	}

	return -1;
}

static int run_burst(void)
{
	int i = 1000000;

	while (i--) {
		if (run_mode == RUN_TO_BREAKPOINT &&
		    sim_regs[MSP430_REG_PC] == run_breakpoint) {
			printf("Breakpoint reached\n");
			run_mode = RUN_HALTED;
			return 0;
		}

		if (sim_regs[MSP430_REG_SR] & MSP430_SR_CPUOFF) {
			run_mode = RUN_HALTED;
			printf("CPU disabled\n");
			return 0;
		}

		if (step_cpu() < 0) {
			run_mode = RUN_HALTED;
			return -1;
		}
	}

	return 1;
}

static device_status_t sim_wait(int blocking)
{
	if (run_mode != RUN_HALTED) {
		do {
			int ret = run_burst();

			if (ret < 0)
				return DEVICE_STATUS_ERROR;
			if (!ret)
				return DEVICE_STATUS_HALTED;

			if (ctrlc_check()) {
				ctrlc_reset();
				return DEVICE_STATUS_INTR;
			}
		} while (blocking);

		return DEVICE_STATUS_RUNNING;
	}

	return DEVICE_STATUS_HALTED;
}

static int sim_breakpoint(u_int16_t addr)
{
	run_breakpoint = addr;
	return 0;
}

static int sim_getregs(u_int16_t *regs)
{
	memcpy(regs, sim_regs, sizeof(sim_regs));
	return 0;
}

static int sim_setregs(const u_int16_t *regs)
{
	memcpy(sim_regs, regs, sizeof(sim_regs));
	return 0;
}

static int sim_readmem(u_int16_t addr, u_int8_t *mem, int len)
{
	if (addr + len > MEM_SIZE)
		len = MEM_SIZE - addr;

	memcpy(mem, memory + addr, len);
	return 0;
}

static int sim_writemem(u_int16_t addr, const u_int8_t *mem, int len)
{
	if (addr + len > MEM_SIZE)
		len = MEM_SIZE - addr;

	memcpy(memory + addr, mem, len);
	return 0;
}

static const struct device sim_device = {
	.close		= sim_close,
	.control	= sim_control,
	.wait		= sim_wait,
	.breakpoint	= sim_breakpoint,
	.getregs	= sim_getregs,
	.setregs	= sim_setregs,
	.readmem	= sim_readmem,
	.writemem	= sim_writemem
};

const struct device *sim_open(void)
{
	memory = malloc(MEM_SIZE);
	if (!memory) {
		perror("sim: can't allocate memory");
		return NULL;
	}

	memset(memory, 0xff, MEM_SIZE);
	printf("Simulation started, 0x%x bytes of RAM\n", MEM_SIZE);
	return &sim_device;
}