sys64738
/
mspdebug
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
mspdebug/drivers/sim.c

810 lines
17 KiB
C
Raw Blame History

/* 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"
#include "ctrlc.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]);
if (reg != MSP430_REG_SR)
addr += dev->regs[reg];
dev->regs[MSP430_REG_PC] += 2;
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] = is_byte ? data & 0xFF : 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;
uint32_t shiftMask = 0x000f;
uint32_t i = 0;
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 ^ res_data) & msb)
dev->regs[MSP430_REG_SR] |= MSP430_SR_V;
break;
case MSP430_OP_DADD:
res_data = 0;
if (dev->regs[MSP430_REG_SR] & MSP430_SR_C)
res_data++;
shiftMask = 0x000f;
for(i = 0; i < 4; ++i)
{
res_data += (src_data & shiftMask) + (dst_data & shiftMask);
if( (res_data & (0x1f << (i*4))) > (9 << (i*4)))
res_data += 6 << (i*4);
shiftMask = shiftMask << 4;
}
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;
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 = 3;
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;
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
};
Reference in New Issue View Git Blame Copy Permalink