mspdebug/drivers/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 "output.h"
#include "sim.h"
#include "simio_cpu.h"

#define MEM_SIZE	65536
#define MEM_IO_END      0x200

struct sim_device {
	struct device           base;

	uint8_t                 memory[MEM_SIZE];
	uint16_t                regs[DEVICE_NUM_REGS];

	int                     running;
	uint16_t                current_insn;

	int			watchpoint_hit;
};

#define MEM_GETB(dev, offset) ((dev)->memory[offset])
#define MEM_SETB(dev, offset, value) ((dev)->memory[offset] = (value))
#define MEM_GETW(dev, offset)					\
	((dev)->memory[offset] |				\
	 ((dev)->memory[(offset + 1) & 0xffff] << 8))
#define MEM_SETW(dev, offset, value)					\
	do {								\
		(dev)->memory[offset & ~1] = (value) & 0xff;			\
		(dev)->memory[offset | 1] = (value) >> 8;	\
	} while (0);

static void watchpoint_check(struct sim_device *dev, uint16_t addr,
			     int is_write)
{
	int i;

	for (i = 0; i < DEVICE_MAX_BREAKPOINTS; i++) {
		const struct device_breakpoint *bp =
			&dev->base.breakpoints[i];

		if ((bp->flags & DEVICE_BP_ENABLED) &&
		    (bp->addr == addr) &&
		    ((bp->type == DEVICE_BPTYPE_WATCH ||
		      (bp->type == DEVICE_BPTYPE_READ && !is_write) ||
		      (bp->type == DEVICE_BPTYPE_WRITE && is_write)))) {
			dev->watchpoint_hit = 1;
			return;
		}
	}
}

static int fetch_operand(struct sim_device *dev,
			 int amode, int reg, int is_byte,
			 uint16_t *addr_ret, uint32_t *data_ret)
{
	uint16_t addr = 0;
	uint32_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 = dev->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(dev, dev->regs[MSP430_REG_PC]);
		dev->regs[MSP430_REG_PC] += 2;

		if (reg != MSP430_REG_SR)
			addr += dev->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 = dev->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 = dev->regs[reg];
		dev->regs[reg] += (is_byte && reg != MSP430_REG_PC) ? 1 : 2;
		break;
	}

	if (addr_ret)
		*addr_ret = addr;

	if (data_ret) {
		watchpoint_check(dev, addr, 0);

		*data_ret = MEM_GETW(dev, addr) & mask;

		if (addr < MEM_IO_END) {
			int ret;

			if (is_byte) {
				uint8_t x = *data_ret;

				ret = simio_read_b(addr, &x);
				*data_ret = x;
			} else {
				uint16_t x = *data_ret;

				ret = simio_read(addr, &x);
				*data_ret = x;
			}

			return ret;
		}
	}

	return 0;
}

static int store_operand(struct sim_device *dev,
			 int amode, int reg, int is_byte,
			 uint16_t addr, uint16_t data)
{
	if (amode == MSP430_AMODE_REGISTER) {
		dev->regs[reg] = data;
		return 0;
	}

	watchpoint_check(dev, addr, 1);

	if (is_byte)
		MEM_SETB(dev, addr, data);
	else
		MEM_SETW(dev, addr, data);

	if (addr < MEM_IO_END) {
		if (is_byte)
			return simio_write_b(addr, data);

		return simio_write(addr, data);
	}

	return 0;
}

#define ARITH_BITS (MSP430_SR_V | MSP430_SR_N | MSP430_SR_Z | MSP430_SR_C)

static int step_double(struct sim_device *dev, uint16_t ins)
{
	uint16_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;
	uint32_t src_data;
	uint16_t dst_addr = 0;
	uint32_t dst_data;
	uint32_t res_data;
	uint32_t msb = is_byte ? 0x80 : 0x8000;
	uint32_t mask = is_byte ? 0xff : 0xffff;
	int cycles;

	if (amode_dst == MSP430_AMODE_REGISTER && dreg == MSP430_REG_PC) {
		if (amode_src == MSP430_AMODE_REGISTER ||
		    amode_src == MSP430_AMODE_INDIRECT)
			cycles = 2;
		else
			cycles = 3;
	} else {
		if (amode_src == MSP430_AMODE_INDIRECT ||
		    amode_src == MSP430_AMODE_INDIRECT_INC)
			cycles = 2;
		else if (amode_src == MSP430_AMODE_INDEXED)
			cycles = 3;
		else
			cycles = 1;

		if (amode_dst == MSP430_AMODE_INDEXED)
			cycles += 3;
	}

	if (fetch_operand(dev, amode_src, sreg, is_byte, NULL, &src_data) < 0)
		return -1;
	if (fetch_operand(dev, 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) & mask;
	case MSP430_OP_ADD:
	case MSP430_OP_ADDC:
		if (opcode == MSP430_OP_ADDC || opcode == MSP430_OP_SUBC)
			res_data = (dev->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;

		dev->regs[MSP430_REG_SR] &= ~ARITH_BITS;
		if (!(res_data & mask))
			dev->regs[MSP430_REG_SR] |= MSP430_SR_Z;
		if (res_data & msb)
			dev->regs[MSP430_REG_SR] |= MSP430_SR_N;
		if (res_data & (msb << 1))
			dev->regs[MSP430_REG_SR] |= MSP430_SR_C;
		if (!((src_data ^ dst_data) & msb) &&
		    (src_data ^ dst_data) & msb)
			dev->regs[MSP430_REG_SR] |= MSP430_SR_V;
		break;

	case MSP430_OP_DADD:
		res_data = src_data + dst_data;
		if (dev->regs[MSP430_REG_SR] & MSP430_SR_C)
			res_data++;

		dev->regs[MSP430_REG_SR] &= ~ARITH_BITS;
		if (!(res_data & mask))
			dev->regs[MSP430_REG_SR] |= MSP430_SR_Z;
		if (res_data == 1)
			dev->regs[MSP430_REG_SR] |= MSP430_SR_N;
		if ((is_byte && res_data > 99) ||
		    (!is_byte && res_data > 9999))
			dev->regs[MSP430_REG_SR] |= MSP430_SR_C;
		break;

	case MSP430_OP_BIT:
	case MSP430_OP_AND:
		res_data = src_data & dst_data;

		dev->regs[MSP430_REG_SR] &= ~ARITH_BITS;
		dev->regs[MSP430_REG_SR] |=
			(res_data & mask) ? MSP430_SR_C : MSP430_SR_Z;
		if (res_data & msb)
			dev->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;
		dev->regs[MSP430_REG_SR] &= ~ARITH_BITS;
		dev->regs[MSP430_REG_SR] |=
			(res_data & mask) ? MSP430_SR_C : MSP430_SR_Z;
		if (res_data & msb)
			dev->regs[MSP430_REG_SR] |= MSP430_SR_N;
		if (src_data & dst_data & msb)
			dev->regs[MSP430_REG_SR] |= MSP430_SR_V;
		break;

	default:
		printc_err("sim: invalid double-operand opcode: "
			"0x%04x (PC = 0x%04x)\n",
			opcode, dev->current_insn);
		return -1;
	}

	if (opcode != MSP430_OP_CMP && opcode != MSP430_OP_BIT &&
		store_operand(dev, amode_dst, dreg, is_byte,
			      dst_addr, res_data) < 0)
		return -1;

	return cycles;
}

static int step_single(struct sim_device *dev, uint16_t ins)
{
	uint16_t opcode = ins & 0xff80;
	int is_byte = ins & 0x0040;
	int amode = (ins >> 4) & 0x3;
	int reg = ins & 0x000f;
	uint16_t msb = is_byte ? 0x80 : 0x8000;
	uint32_t mask = is_byte ? 0xff : 0xffff;
	uint16_t src_addr = 0;
	uint32_t src_data;
	uint32_t res_data = 0;
	int cycles = 1;

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

	if (amode == MSP430_AMODE_INDEXED)
		cycles = 4;
	else if (amode == MSP430_AMODE_REGISTER)
		cycles = 1;
	else
		cycles = 5;

	switch (opcode) {
	case MSP430_OP_RRC:
	case MSP430_OP_RRA:
		res_data = (src_data >> 1) & ~msb;
		if (opcode == MSP430_OP_RRC) {
			if (dev->regs[MSP430_REG_SR] & MSP430_SR_C)
				res_data |= msb;
		} else {
			res_data |= src_data & msb;
		}

		dev->regs[MSP430_REG_SR] &= ~ARITH_BITS;
		if (!(res_data & mask))
			dev->regs[MSP430_REG_SR] |= MSP430_SR_Z;
		if (res_data & msb)
			dev->regs[MSP430_REG_SR] |= MSP430_SR_N;
		if (src_data & 1)
			dev->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;
		dev->regs[MSP430_REG_SR] &= ~ARITH_BITS;

		if (src_data & 0x80) {
			res_data |= 0xff00;
			dev->regs[MSP430_REG_SR] |= MSP430_SR_N;
		}

		dev->regs[MSP430_REG_SR] |=
			(res_data & mask) ? MSP430_SR_C : MSP430_SR_Z;
		break;

	case MSP430_OP_PUSH:
		dev->regs[MSP430_REG_SP] -= 2;
		MEM_SETW(dev, dev->regs[MSP430_REG_SP], src_data);

		if (amode == MSP430_AMODE_REGISTER)
			cycles = 3;
		else if (amode == MSP430_AMODE_INDIRECT ||
			 (amode == MSP430_AMODE_INDIRECT_INC &&
			  reg == MSP430_REG_PC))
			cycles = 4;
		else
			cycles = 5;
		break;

	case MSP430_OP_CALL:
		dev->regs[MSP430_REG_SP] -= 2;
		MEM_SETW(dev, dev->regs[MSP430_REG_SP],
			 dev->regs[MSP430_REG_PC]);
		dev->regs[MSP430_REG_PC] = src_data;

		if (amode == MSP430_AMODE_REGISTER ||
		    amode == MSP430_AMODE_INDIRECT)
			cycles = 4;
		else
			cycles = 5;
		break;

	case MSP430_OP_RETI:
		dev->regs[MSP430_REG_SR] =
			MEM_GETW(dev, dev->regs[MSP430_REG_SP]);
		dev->regs[MSP430_REG_SP] += 2;
		dev->regs[MSP430_REG_PC] =
			MEM_GETW(dev, dev->regs[MSP430_REG_SP]);
		dev->regs[MSP430_REG_SP] += 2;
		cycles = 5;
		break;

	default:
		printc_err("sim: unknown single-operand opcode: 0x%04x "
			"(PC = 0x%04x)\n", opcode, dev->current_insn);
		return -1;
	}

	if (opcode != MSP430_OP_PUSH && opcode != MSP430_OP_CALL &&
	    opcode != MSP430_OP_RETI &&
		store_operand(dev, amode, reg, is_byte, src_addr, res_data) < 0)
		return -1;

	return cycles;
}

static int step_jump(struct sim_device *dev, uint16_t ins)
{
	uint16_t opcode = ins & 0xfc00;
	uint16_t pc_offset = (ins & 0x03ff) << 1;
	uint16_t sr = dev->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)
		dev->regs[MSP430_REG_PC] += pc_offset;

	return 2;
}

/* Fetch and execute one instruction. Return the number of CPU cycles
 * it would have taken, or -1 if an error occurs.
 */
static int step_cpu(struct sim_device *dev)
{
	uint16_t ins;
	int ret;

	/* Fetch the instruction */
	dev->current_insn = dev->regs[MSP430_REG_PC];
	ins = MEM_GETW(dev, dev->current_insn);
	dev->regs[MSP430_REG_PC] += 2;

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

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

	return ret;
}

static void do_reset(struct sim_device *dev)
{
	simio_step(dev->regs[MSP430_REG_SR], 4);
	memset(dev->regs, 0, sizeof(dev->regs));
	dev->regs[MSP430_REG_PC] = MEM_GETW(dev, 0xfffe);
	dev->regs[MSP430_REG_SR] = 0;
	simio_reset();
}

static int step_system(struct sim_device *dev)
{
	int count = 1;
	int irq;
	uint16_t status = dev->regs[MSP430_REG_SR];

	irq = simio_check_interrupt();
	if (irq == 15) {
		do_reset(dev);
		return 0;
	} else if (((status & MSP430_SR_GIE) && irq >= 0) || irq >= 14) {
		if (irq >= 16) {
			printc_err("sim: invalid interrupt number: %d\n", irq);
			return -1;
		}

		dev->regs[MSP430_REG_SP] -= 2;
		MEM_SETW(dev, dev->regs[MSP430_REG_SP],
			 dev->regs[MSP430_REG_PC]);

		dev->regs[MSP430_REG_SP] -= 2;
		MEM_SETW(dev, dev->regs[MSP430_REG_SP],
			 dev->regs[MSP430_REG_SR]);

		dev->regs[MSP430_REG_SR] &=
			~(MSP430_SR_GIE | MSP430_SR_CPUOFF);
		dev->regs[MSP430_REG_PC] = MEM_GETW(dev, 0xffe0 + irq * 2);

		simio_ack_interrupt(irq);
		count = 6;
	} else if (!(status & MSP430_SR_CPUOFF)) {
		count = step_cpu(dev);
		if (count < 0)
			return -1;
	}

	simio_step(status, count);
	return 0;
}

/************************************************************************
 * Device interface
 */

static void sim_destroy(device_t dev_base)
{
	free(dev_base);
}

static int sim_readmem(device_t dev_base, address_t addr,
		       uint8_t *mem, address_t len)
{
	struct sim_device *dev = (struct sim_device *)dev_base;

	if (addr > MEM_SIZE || (addr + len) < addr ||
	    (addr + len) > MEM_SIZE) {
		printc_err("sim: memory read out of range\n");
		return -1;
	}

	if (addr + len > MEM_SIZE)
		len = MEM_SIZE - addr;

	/* Read byte IO addresses */
	while (len && (addr < 0x100)) {
		simio_read_b(addr, mem);
		mem++;
		len--;
		addr++;
	}

	/* Read word IO addresses */
	while (len > 2 && (addr < 0x200)) {
		uint16_t data = 0;

		simio_read(addr, &data);
		mem[0] = data & 0xff;
		mem[1] = data >> 8;
		mem += 2;
		len -= 2;
		addr += 2;
	}

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

static int sim_writemem(device_t dev_base, address_t addr,
			const uint8_t *mem, address_t len)
{
	struct sim_device *dev = (struct sim_device *)dev_base;

	if (addr > MEM_SIZE || (addr + len) < addr ||
	    (addr + len) > MEM_SIZE) {
		printc_err("sim: memory write out of range\n");
		return -1;
	}

	/* Write byte IO addresses */
	while (len && (addr < 0x100)) {
		simio_write_b(addr, *mem);
		mem++;
		len--;
		addr++;
	}

	/* Write word IO addresses */
	while (len > 2 && (addr < 0x200)) {
		simio_write(addr, ((uint16_t)mem[1] << 8) | mem[0]);
		mem += 2;
		len -= 2;
		addr += 2;
	}

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

static int sim_getregs(device_t dev_base, address_t *regs)
{
	struct sim_device *dev = (struct sim_device *)dev_base;
	int i;

	for (i = 0; i < DEVICE_NUM_REGS; i++)
		regs[i] = dev->regs[i];
	return 0;
}

static int sim_setregs(device_t dev_base, const address_t *regs)
{
	struct sim_device *dev = (struct sim_device *)dev_base;
	int i;

	for (i = 0; i < DEVICE_NUM_REGS; i++)
		dev->regs[i] = regs[i];
	return 0;
}

static int sim_ctl(device_t dev_base, device_ctl_t op)
{
	struct sim_device *dev = (struct sim_device *)dev_base;

	switch (op) {
	case DEVICE_CTL_RESET:
		do_reset(dev);
		return 0;

	case DEVICE_CTL_HALT:
		dev->running = 0;
		return 0;

	case DEVICE_CTL_STEP:
		return step_system(dev);

	case DEVICE_CTL_RUN:
		dev->running = 1;
		return 0;
	}

	return 0;
}

static int sim_erase(device_t dev_base, device_erase_type_t type,
		     address_t addr)
{
	struct sim_device *dev = (struct sim_device *)dev_base;

	switch (type) {
	case DEVICE_ERASE_MAIN:
		memset(dev->memory + 0x2000, 0xff, MEM_SIZE - 0x2000);
		break;

	case DEVICE_ERASE_ALL:
		memset(dev->memory, 0xff, MEM_SIZE);
		break;

	case DEVICE_ERASE_SEGMENT:
		addr &= ~0x3f;
		addr &= (MEM_SIZE - 1);
		memset(dev->memory + addr, 0xff, 64);
		break;
	}

	return 0;
}

static device_status_t sim_poll(device_t dev_base)
{
	struct sim_device *dev = (struct sim_device *)dev_base;
	int count = 1000000;

	if (!dev->running)
		return DEVICE_STATUS_HALTED;

	ctrlc_reset();
	dev->watchpoint_hit = 0;
	while (count > 0) {
		int i;

		for (i = 0; i < dev->base.max_breakpoints; i++) {
			struct device_breakpoint *bp =
				&dev->base.breakpoints[i];

			if ((bp->flags & DEVICE_BP_ENABLED) &&
			    (bp->type == DEVICE_BPTYPE_BREAK) &&
			    dev->regs[MSP430_REG_PC] == bp->addr) {
				dev->running = 0;
				return DEVICE_STATUS_HALTED;
			}
		}

		if (step_system(dev) < 0) {
			dev->running = 0;
			return DEVICE_STATUS_ERROR;
		}

		if (dev->watchpoint_hit) {
			dev->running = 0;
			return DEVICE_STATUS_HALTED;
		}

		if (ctrlc_check())
			return DEVICE_STATUS_INTR;

		count--;
	}

	return DEVICE_STATUS_RUNNING;
}

static device_t sim_open(const struct device_args *args)
{
	struct sim_device *dev = malloc(sizeof(*dev));

	(void)args;

	if (!dev) {
		pr_error("can't allocate memory for simulation");
		return NULL;
	}

	memset(dev, 0, sizeof(*dev));

	dev->base.type = &device_sim;
	dev->base.max_breakpoints = DEVICE_MAX_BREAKPOINTS;

	memset(dev->memory, 0xff, sizeof(dev->memory));
	memset(dev->regs, 0xff, sizeof(dev->regs));

	dev->running = 0;
	dev->current_insn = 0;

	printc_dbg("Simulation started, 0x%x bytes of RAM\n", MEM_SIZE);
	return (device_t)dev;
}

const struct device_class device_sim = {
	.name		= "sim",
	.help		= "Simulation mode.",
	.open		= sim_open,
	.destroy	= sim_destroy,
	.readmem	= sim_readmem,
	.writemem	= sim_writemem,
	.erase		= sim_erase,
	.getregs	= sim_getregs,
	.setregs	= sim_setregs,
	.ctl		= sim_ctl,
	.poll		= sim_poll
};