first draft; needs tests
parent
6a62488e15
commit
67f3a6c989
@ -0,0 +1,2 @@
|
|||||||
|
*.a
|
||||||
|
*.o
|
||||||
@ -0,0 +1,23 @@
|
|||||||
|
CROSS :=
|
||||||
|
CC := $(CROSS)gcc
|
||||||
|
CFLAGS := -Os -ggdb -g3 \
|
||||||
|
-Wall -Wextra -pedantic -Werror
|
||||||
|
|
||||||
|
CSRC := $(wildcard src/*.c)
|
||||||
|
COBJ := $(patsubst %.c,%.o,$(CSRC))
|
||||||
|
|
||||||
|
all: lib430core.a
|
||||||
|
|
||||||
|
clean:
|
||||||
|
@echo " CLEAN"
|
||||||
|
@rm -f lib430core.a $(COBJ)
|
||||||
|
|
||||||
|
lib430core.a: $(COBJ)
|
||||||
|
@echo " AR " $@
|
||||||
|
@$(CROSS)ar rc $@ $(COBJ)
|
||||||
|
@$(CROSS)size -t $@
|
||||||
|
|
||||||
|
.c.o:
|
||||||
|
@echo " CC " $<
|
||||||
|
@$(CC) $(CFLAGS) -c $< -o $@
|
||||||
|
|
||||||
@ -0,0 +1,15 @@
|
|||||||
|
SECTIONS {
|
||||||
|
. = 0x0000;
|
||||||
|
.text : {
|
||||||
|
* (.text);
|
||||||
|
. = ALIGN(8);
|
||||||
|
* (.rodata);
|
||||||
|
}
|
||||||
|
. = ALIGN(8);
|
||||||
|
.data : {
|
||||||
|
* (.data);
|
||||||
|
. = ALIGN(8);
|
||||||
|
* (.bss);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,258 @@
|
|||||||
|
#include "core.h"
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#define MSP430_SR_C (1 << 0)
|
||||||
|
#define MSP430_SR_Z (1 << 1)
|
||||||
|
#define MSP430_SR_N (1 << 2)
|
||||||
|
#define MSP430_SR_V (1 << 8)
|
||||||
|
|
||||||
|
// r0 = pc
|
||||||
|
// r1 = sp
|
||||||
|
// r2 = sr
|
||||||
|
|
||||||
|
static int msp430_do_cycle_jump(msp430_t *state, uint16_t opcode);
|
||||||
|
int msp430_do_cycle_single_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
int msp430_do_cycle_dual_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
int msp430_do_cycle_single_operand_byte(msp430_t *state, uint16_t opcode);
|
||||||
|
int msp430_do_cycle_dual_operand_byte(msp430_t *state, uint16_t opcode);
|
||||||
|
|
||||||
|
uint16_t *msp430_do_cycle_get_single_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
uint16_t *msp430_do_cycle_get_source_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
uint16_t *msp430_do_cycle_get_dest_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
|
||||||
|
static inline int msp430_get_opcode_bw(uint16_t opcode);
|
||||||
|
|
||||||
|
void msp430_init_state(msp430_t *state, void *mem)
|
||||||
|
{
|
||||||
|
for (int i = 0; i < 16; ++i)
|
||||||
|
state->reg[i] = 0;
|
||||||
|
state->mem = mem;
|
||||||
|
}
|
||||||
|
|
||||||
|
int msp430_do_cycle(msp430_t *state)
|
||||||
|
{
|
||||||
|
uint16_t pc = state->reg[0];
|
||||||
|
uint16_t opcode = *((uint16_t *)(state->mem + pc));
|
||||||
|
|
||||||
|
// Check for program end...
|
||||||
|
if (opcode == 0x4130 && state->reg[1] == 0)
|
||||||
|
return 1;
|
||||||
|
|
||||||
|
state->reg[0] = pc + 2;
|
||||||
|
|
||||||
|
int ret;
|
||||||
|
if ((opcode & 0xE000) == 0x2000) {
|
||||||
|
ret = msp430_do_cycle_jump(state, opcode);
|
||||||
|
} else {
|
||||||
|
int bw = msp430_get_opcode_bw(opcode);
|
||||||
|
if (bw == 0) {
|
||||||
|
if ((opcode & 0xFC00) == 0x1000)
|
||||||
|
ret = msp430_do_cycle_single_operand(state, opcode);
|
||||||
|
else
|
||||||
|
ret = msp430_do_cycle_dual_operand(state, opcode);
|
||||||
|
} else {
|
||||||
|
if ((opcode & 0xFC00) == 0x1000)
|
||||||
|
ret = msp430_do_cycle_single_operand_byte(state, opcode);
|
||||||
|
else
|
||||||
|
ret = msp430_do_cycle_dual_operand_byte(state, opcode);
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
|
||||||
|
static void msp430_jump_to_offset(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
uint16_t addr = opcode & 0x3FF;
|
||||||
|
if (addr & (1 << 9))
|
||||||
|
addr |= 0xFC00;
|
||||||
|
int16_t saddr = (int16_t)addr * 2;
|
||||||
|
state->reg[0] += saddr;
|
||||||
|
}
|
||||||
|
|
||||||
|
int msp430_do_cycle_jump(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
// PC to become PC + ((opcode & 0x3FF) << 1)
|
||||||
|
switch ((opcode & 0x01C0) >> 10) {
|
||||||
|
case 0:
|
||||||
|
// JNE/JZ
|
||||||
|
if ((state->reg[2] & MSP430_SR_Z) == 0)
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break;
|
||||||
|
case 1:
|
||||||
|
// JEQ/JZ
|
||||||
|
if ((state->reg[2] & MSP430_SR_Z) == MSP430_SR_Z)
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break;
|
||||||
|
case 2:
|
||||||
|
// JNC/JLO
|
||||||
|
if ((state->reg[2] & MSP430_SR_C) == 0)
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
// JC/JHS
|
||||||
|
if ((state->reg[2] & MSP430_SR_C) == MSP430_SR_C)
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break;
|
||||||
|
case 4:
|
||||||
|
// JN
|
||||||
|
if ((state->reg[2] & MSP430_SR_N) == MSP430_SR_N)
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break;
|
||||||
|
case 5: {
|
||||||
|
// JGE
|
||||||
|
uint16_t flags = state->reg[2] & (MSP430_SR_N | MSP430_SR_V);
|
||||||
|
if (flags == 0 || flags == (MSP430_SR_N | MSP430_SR_V))
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break; }
|
||||||
|
case 6: {
|
||||||
|
// JL
|
||||||
|
uint16_t flags = state->reg[2] & (MSP430_SR_N | MSP430_SR_V);
|
||||||
|
if (flags == MSP430_SR_N || flags == MSP430_SR_V)
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break; }
|
||||||
|
case 7:
|
||||||
|
// JMP
|
||||||
|
msp430_jump_to_offset(state, opcode);
|
||||||
|
break;
|
||||||
|
default:
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t *msp430_do_cycle_get_operand(msp430_t *state, int AS, int BW, int SOURCE)
|
||||||
|
{
|
||||||
|
static uint16_t constants[6] = {
|
||||||
|
0, 1, 2, -1, 4, 8
|
||||||
|
};
|
||||||
|
|
||||||
|
if (SOURCE == 0) {
|
||||||
|
if (AS == 1) {
|
||||||
|
// Operand at PC + X
|
||||||
|
// X = word after PC
|
||||||
|
uint16_t offset = *((uint16_t *)(state->mem + state->reg[0]));
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + state->reg[0] + offset);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
} else if (AS == 3) {
|
||||||
|
// Operand is X
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + state->reg[0]);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
if (SOURCE == 2) {
|
||||||
|
switch (AS) {
|
||||||
|
case 0:
|
||||||
|
// R2
|
||||||
|
return &state->reg[2];
|
||||||
|
break;
|
||||||
|
case 1: {
|
||||||
|
// Operand at X
|
||||||
|
uint16_t offset = *((uint16_t *)(state->mem + state->reg[0]));
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + offset);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
break; }
|
||||||
|
case 2:
|
||||||
|
return &constants[4];
|
||||||
|
break;
|
||||||
|
case 3:
|
||||||
|
return &constants[5];
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
} else if (SOURCE == 3) {
|
||||||
|
// Constants
|
||||||
|
return &constants[AS];
|
||||||
|
} else {
|
||||||
|
switch (AS) {
|
||||||
|
case 0:
|
||||||
|
// Register
|
||||||
|
return &state->reg[SOURCE];
|
||||||
|
break;
|
||||||
|
case 1: {
|
||||||
|
// Operand at Rn + X
|
||||||
|
uint16_t offset = *((uint16_t *)(state->mem + state->reg[0]));
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + state->reg[SOURCE] + offset);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
break; }
|
||||||
|
case 2:
|
||||||
|
// Operand at Rn
|
||||||
|
return (uint16_t *)(state->mem + state->reg[SOURCE]);
|
||||||
|
break;
|
||||||
|
case 3: {
|
||||||
|
// Operand at Rn, increment Rn
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + state->reg[SOURCE]);
|
||||||
|
state->reg[SOURCE] += BW ? 1 : 2;
|
||||||
|
return ret;
|
||||||
|
break; }
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0; // Failed...
|
||||||
|
}
|
||||||
|
|
||||||
|
inline int msp430_get_opcode_bw(uint16_t opcode)
|
||||||
|
{
|
||||||
|
return opcode & (1 << 6);
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t *msp430_do_cycle_get_single_operand(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
return msp430_do_cycle_get_operand(state,
|
||||||
|
/* AS */ (opcode & 0x30) >> 4,
|
||||||
|
/* BW */ msp430_get_opcode_bw(opcode),
|
||||||
|
/* SOURCE */ (opcode & 0xF));
|
||||||
|
}
|
||||||
|
|
||||||
|
uint16_t *msp430_do_cycle_get_source_operand(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
return msp430_do_cycle_get_operand(state,
|
||||||
|
/* AS */ (opcode & 0x30) >> 4,
|
||||||
|
/* BW */ msp430_get_opcode_bw(opcode),
|
||||||
|
/* SOURCE */ (opcode & 0xF00) >> 8);
|
||||||
|
}
|
||||||
|
uint16_t *msp430_do_cycle_get_dest_operand(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
int AD = opcode & 0x80;
|
||||||
|
int DEST = opcode & 0xF;
|
||||||
|
|
||||||
|
if (AD == 0) {
|
||||||
|
// Byte operation? Clear MSB.
|
||||||
|
if (msp430_get_opcode_bw(opcode) == 0) {
|
||||||
|
return &state->reg[DEST];
|
||||||
|
} else {
|
||||||
|
uint16_t *ret = &state->reg[DEST];
|
||||||
|
*ret &= 0xFF;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
} else {
|
||||||
|
if (DEST == 0) {
|
||||||
|
// dest at PC + X
|
||||||
|
uint16_t offset = *((uint16_t *)(state->mem + state->reg[0]));
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + state->reg[0] + offset);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
} else if (DEST == 2) {
|
||||||
|
// dest at X
|
||||||
|
uint16_t offset = *((uint16_t *)(state->mem + state->reg[0]));
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + offset);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
} else {
|
||||||
|
// dest at Rn + X
|
||||||
|
uint16_t offset = *((uint16_t *)(state->mem + state->reg[0]));
|
||||||
|
uint16_t *ret = (uint16_t *)(state->mem + state->reg[DEST] + offset);
|
||||||
|
state->reg[0] += 2;
|
||||||
|
return ret;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,15 @@
|
|||||||
|
#ifndef CORE_H
|
||||||
|
#define CORE_H
|
||||||
|
|
||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
typedef struct {
|
||||||
|
uint16_t reg[16];
|
||||||
|
uint8_t *mem;
|
||||||
|
} msp430_t;
|
||||||
|
|
||||||
|
void msp430_init_state(msp430_t *state, void *mem);
|
||||||
|
int msp430_do_cycle(msp430_t *state);
|
||||||
|
|
||||||
|
#endif // CORE_H
|
||||||
|
|
||||||
@ -0,0 +1,214 @@
|
|||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#define MSP430_SR_C (1 << 0)
|
||||||
|
#define MSP430_SR_Z (1 << 1)
|
||||||
|
#define MSP430_SR_N (1 << 2)
|
||||||
|
#define MSP430_SR_V (1 << 8)
|
||||||
|
|
||||||
|
// r0 = pc
|
||||||
|
// r1 = sp
|
||||||
|
// r2 = sr
|
||||||
|
typedef struct {
|
||||||
|
uint16_t reg[16];
|
||||||
|
uint8_t *mem;
|
||||||
|
} msp430_t;
|
||||||
|
|
||||||
|
extern uint16_t *msp430_do_cycle_get_single_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
extern uint16_t *msp430_do_cycle_get_source_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
extern uint16_t *msp430_do_cycle_get_dest_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
|
||||||
|
int msp430_do_cycle_single_operand_byte(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
uint8_t *operand = (uint8_t *)msp430_do_cycle_get_single_operand(state, opcode);
|
||||||
|
if (operand == 0)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
switch ((opcode & 0x0380) >> 7) {
|
||||||
|
case 0: {
|
||||||
|
// RRC
|
||||||
|
uint8_t res = *operand;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 1)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
res >>= 1;
|
||||||
|
if (state->reg[2] & MSP430_SR_C)
|
||||||
|
res |= 0x80;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
*operand = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 2: {
|
||||||
|
// RRA
|
||||||
|
uint8_t res = *operand;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 1)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
res >>= 1;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
*operand = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 4: {
|
||||||
|
// PUSH
|
||||||
|
uint16_t sp = state->reg[1] - 2;
|
||||||
|
*((uint16_t *)(state->mem + sp)) = *operand;
|
||||||
|
state->reg[1] = sp;
|
||||||
|
break; }
|
||||||
|
default:
|
||||||
|
return -1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int msp430_do_cycle_dual_operand_byte(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
uint8_t *src = (uint8_t *)msp430_do_cycle_get_source_operand(state, opcode);
|
||||||
|
uint8_t *dst = (uint8_t *)msp430_do_cycle_get_dest_operand(state, opcode);
|
||||||
|
if (src == 0 || dst == 0)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
switch ((opcode & 0xF000) >> 12) {
|
||||||
|
case 4:
|
||||||
|
// MOV
|
||||||
|
*dst = *src;
|
||||||
|
break;
|
||||||
|
case 5: {
|
||||||
|
// ADD
|
||||||
|
uint16_t res = *src + *dst;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFF00) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x80) ^ (*src & 0x80)) == 0 && (*src & 0x80) != (res & 0x80))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = res & 0xFF;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 6: {
|
||||||
|
// ADDC
|
||||||
|
uint16_t res = *src + *dst + (state->reg[2] & MSP430_SR_C);
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFF00) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x80) ^ (*src & 0x80)) == 0 && (*src & 0x80) != (res & 0x80))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = res & 0xFF;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 7: {
|
||||||
|
// SUBC
|
||||||
|
uint16_t res = *dst + ~(*src) + (state->reg[2] & MSP430_SR_C);
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFF00) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x80) ^ (*src & 0x80)) == 0 && (*src & 0x80) != (res & 0x80))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = res & 0xFF;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 8: {
|
||||||
|
// SUB
|
||||||
|
uint16_t res = *dst + ~(*src) + 1;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFF00) != 0) // TODO confirm
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x80) ^ (*src & 0x80)) == 0 && (*src & 0x80) != (res & 0x80))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = res & 0xFF;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 9: {
|
||||||
|
// CMP
|
||||||
|
uint16_t res = *dst + ~(*src) + 1;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int8_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFF00) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x80) ^ (*src & 0x80)) == 0 && (*src & 0x80) != (res & 0x80))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 10:
|
||||||
|
// DADD TODO
|
||||||
|
break;
|
||||||
|
case 11: {
|
||||||
|
// BIT
|
||||||
|
uint8_t res = *dst & *src;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 0x80)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 12:
|
||||||
|
// BIC
|
||||||
|
*dst &= ~(*src);
|
||||||
|
break;
|
||||||
|
case 13:
|
||||||
|
// BIS
|
||||||
|
*dst |= *src;
|
||||||
|
break;
|
||||||
|
case 14: {
|
||||||
|
// XOR
|
||||||
|
uint8_t res = *dst ^ *src;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 0x80)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if ((*dst & 0x80) && (*src & 0x80))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 15: {
|
||||||
|
// AND
|
||||||
|
uint8_t res = *dst & *src;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 0x80)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
*dst = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
default:
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,246 @@
|
|||||||
|
#include <stdint.h>
|
||||||
|
|
||||||
|
#define MSP430_SR_C (1 << 0)
|
||||||
|
#define MSP430_SR_Z (1 << 1)
|
||||||
|
#define MSP430_SR_N (1 << 2)
|
||||||
|
#define MSP430_SR_V (1 << 8)
|
||||||
|
|
||||||
|
// r0 = pc
|
||||||
|
// r1 = sp
|
||||||
|
// r2 = sr
|
||||||
|
typedef struct {
|
||||||
|
uint16_t reg[16];
|
||||||
|
uint8_t *mem;
|
||||||
|
} msp430_t;
|
||||||
|
|
||||||
|
extern uint16_t *msp430_do_cycle_get_single_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
extern uint16_t *msp430_do_cycle_get_source_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
extern uint16_t *msp430_do_cycle_get_dest_operand(msp430_t *state, uint16_t opcode);
|
||||||
|
|
||||||
|
int msp430_do_cycle_single_operand(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
uint16_t *operand = msp430_do_cycle_get_single_operand(state, opcode);
|
||||||
|
if (operand == 0)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
switch ((opcode & 0x0380) >> 7) {
|
||||||
|
case 0: {
|
||||||
|
// RRC
|
||||||
|
uint16_t res = *operand;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 1)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
res >>= 1;
|
||||||
|
if (state->reg[2] & MSP430_SR_C)
|
||||||
|
res |= 0x8000;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
*operand = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 1: {
|
||||||
|
// SWPB
|
||||||
|
uint16_t swapped = ((*operand & 0xFF00) >> 8) | ((*operand & 0x00FF) << 8);
|
||||||
|
*operand = swapped;
|
||||||
|
break; }
|
||||||
|
case 2: {
|
||||||
|
// RRA
|
||||||
|
uint16_t res = *operand;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 1)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
res >>= 1;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
*operand = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 3: {
|
||||||
|
// SXT
|
||||||
|
uint16_t sr = 0;
|
||||||
|
uint16_t res = (*operand & 0x80) ? (*operand | 0xFF00) : *operand;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
*operand = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 4: {
|
||||||
|
// PUSH
|
||||||
|
uint16_t sp = state->reg[1] - 2;
|
||||||
|
*((uint16_t *)(state->mem + sp)) = *operand;
|
||||||
|
state->reg[1] = sp;
|
||||||
|
break; }
|
||||||
|
case 5: {
|
||||||
|
// CALL
|
||||||
|
uint16_t sp = state->reg[1] - 2;
|
||||||
|
*((uint16_t *)(state->mem + sp)) = state->reg[0];
|
||||||
|
state->reg[0] = *operand;
|
||||||
|
state->reg[1] = sp;
|
||||||
|
break; }
|
||||||
|
case 6: {
|
||||||
|
// RETI
|
||||||
|
uint16_t sp = state->reg[1];
|
||||||
|
state->reg[2] = *((uint16_t *)(state->mem + sp));
|
||||||
|
state->reg[0] = *((uint16_t *)(state->mem + sp + 2));
|
||||||
|
state->reg[1] = sp + 4;
|
||||||
|
break; }
|
||||||
|
default:
|
||||||
|
return -1;
|
||||||
|
break;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
int msp430_do_cycle_dual_operand(msp430_t *state, uint16_t opcode)
|
||||||
|
{
|
||||||
|
uint16_t *src = msp430_do_cycle_get_source_operand(state, opcode);
|
||||||
|
uint16_t *dst = msp430_do_cycle_get_dest_operand(state, opcode);
|
||||||
|
if (src == 0 || dst == 0)
|
||||||
|
return -1;
|
||||||
|
|
||||||
|
switch ((opcode & 0xF000) >> 12) {
|
||||||
|
case 4:
|
||||||
|
// MOV
|
||||||
|
*dst = *src;
|
||||||
|
break;
|
||||||
|
case 5: {
|
||||||
|
// ADD
|
||||||
|
uint32_t res = *src + *dst;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFFFF0000) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x8000) ^ (*src & 0x8000)) == 0 && (*src & 0x8000) != (res & 0x8000))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = (uint16_t)res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 6: {
|
||||||
|
// ADDC
|
||||||
|
uint32_t res = *src + *dst + (state->reg[2] & MSP430_SR_C);
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFFFF0000) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x8000) ^ (*src & 0x8000)) == 0 && (*src & 0x8000) != (res & 0x8000))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = (uint16_t)res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 7: {
|
||||||
|
// SUBC
|
||||||
|
uint32_t res = *dst + ~(*src) + (state->reg[2] & MSP430_SR_C);
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFFFF0000) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x8000) ^ (*src & 0x8000)) == 0 && (*src & 0x8000) != (res & 0x8000))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = (uint16_t)res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 8: {
|
||||||
|
// SUB
|
||||||
|
uint32_t res = *dst + ~(*src) + 1;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFFFF0000) != 0) // TODO confirm
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x8000) ^ (*src & 0x8000)) == 0 && (*src & 0x8000) != (res & 0x8000))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = (uint16_t)res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 9: {
|
||||||
|
// CMP
|
||||||
|
uint32_t res = *dst + ~(*src) + 1;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if ((int16_t)res < 0)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if ((res & 0xFFFF0000) != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if (((*src & 0x8000) ^ (*src & 0x8000)) == 0 && (*src & 0x8000) != (res & 0x8000))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 10:
|
||||||
|
// DADD TODO
|
||||||
|
break;
|
||||||
|
case 11: {
|
||||||
|
// BIT
|
||||||
|
uint16_t res = *dst & *src;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 0x8000)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 12:
|
||||||
|
// BIC
|
||||||
|
*dst &= ~(*src);
|
||||||
|
break;
|
||||||
|
case 13:
|
||||||
|
// BIS
|
||||||
|
*dst |= *src;
|
||||||
|
break;
|
||||||
|
case 14: {
|
||||||
|
// XOR
|
||||||
|
uint16_t res = *dst ^ *src;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 0x8000)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
if ((*dst & 0x8000) && (*src & 0x8000))
|
||||||
|
sr |= MSP430_SR_V;
|
||||||
|
*dst = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
case 15: {
|
||||||
|
// AND
|
||||||
|
uint16_t res = *dst & *src;
|
||||||
|
uint16_t sr = 0;
|
||||||
|
if (res & 0x8000)
|
||||||
|
sr |= MSP430_SR_N;
|
||||||
|
if (res == 0)
|
||||||
|
sr |= MSP430_SR_Z;
|
||||||
|
if (res != 0)
|
||||||
|
sr |= MSP430_SR_C;
|
||||||
|
*dst = res;
|
||||||
|
state->reg[2] = sr;
|
||||||
|
break; }
|
||||||
|
default:
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,24 @@
|
|||||||
|
DEBUG := #-DDEBUG
|
||||||
|
|
||||||
|
LIB430COREDIR = ..
|
||||||
|
|
||||||
|
TEST_CSRC = test.c
|
||||||
|
TEST_OUT = $(patsubst %.c,%.o,$(TEST_CSRC))
|
||||||
|
|
||||||
|
all: $(LIB430COREDIR)/lib430core.a $(TEST_OUT)
|
||||||
|
|
||||||
|
$(LIB430COREDIR)/lib430core.a:
|
||||||
|
@$(MAKE) -C ..
|
||||||
|
|
||||||
|
.c.o:
|
||||||
|
@echo "Testing" $< "..."
|
||||||
|
@msp430-elf32-gcc -T../link.ld $< -o tmp.elf -nostdlib -lgcc -lc
|
||||||
|
@msp430-elf32-objcopy -Obinary tmp.elf tmp.bin
|
||||||
|
@echo "#define TESTBIN \\" > test.h
|
||||||
|
@od -t x1 tmp.bin | awk '{for (i=2; i<=NF; i++) printf "0x" $$i ", "}' >> test.h
|
||||||
|
@printf "\n#define TESTSIZE %u \n" $$(msp430-elf32-objdump tmp.elf -Dj .text | grep -E "^\s+[0-9a-fA-F]+:" | wc -l) >> test.h
|
||||||
|
@echo >> test.h
|
||||||
|
@gcc -Os -ggdb -g3 -Wall -W -pedantic $(DEBUG) -I../src -o tmp \
|
||||||
|
core_test.c -L$(LIB430COREDIR) -l430core
|
||||||
|
@./tmp > log
|
||||||
|
@rm log tmp.elf tmp.bin tmp test.h
|
||||||
@ -0,0 +1,52 @@
|
|||||||
|
#include "core.h"
|
||||||
|
#include "test.h"
|
||||||
|
|
||||||
|
#include <stdio.h>
|
||||||
|
|
||||||
|
//#define DEBUG
|
||||||
|
|
||||||
|
#ifdef DEBUG
|
||||||
|
static void dump_state(msp430_t *state)
|
||||||
|
{
|
||||||
|
puts("MSP430 dump state:");
|
||||||
|
printf("R0/PC: 0x%04x R1/SP: 0x%04x R2/SR: 0x%04x R3: 0x%04x\n",
|
||||||
|
state->reg[0], state->reg[1], state->reg[2], state->reg[3]);
|
||||||
|
printf("R4: 0x%04x R5: 0x%04x R6: 0x%04x R7: 0x%04x\n",
|
||||||
|
state->reg[4], state->reg[5], state->reg[6], state->reg[7]);
|
||||||
|
printf("R8: 0x%04x R9: 0x%04x R10: 0x%04x R11: 0x%04x\n",
|
||||||
|
state->reg[8], state->reg[9], state->reg[10], state->reg[11]);
|
||||||
|
printf("R12: 0x%04x R13: 0x%04x R14: 0x%04x R15: 0x%04x\n\n",
|
||||||
|
state->reg[12], state->reg[13], state->reg[14], state->reg[15]);
|
||||||
|
}
|
||||||
|
#endif // DEBUG
|
||||||
|
|
||||||
|
static uint8_t mem[0x10000] = {
|
||||||
|
TESTBIN
|
||||||
|
};
|
||||||
|
|
||||||
|
int main()
|
||||||
|
{
|
||||||
|
msp430_t state;
|
||||||
|
|
||||||
|
msp430_init_state(&state, mem);
|
||||||
|
|
||||||
|
#ifdef DEBUG
|
||||||
|
dump_state(&state);
|
||||||
|
#endif // DEBUG
|
||||||
|
int r;
|
||||||
|
do {
|
||||||
|
r = msp430_do_cycle(&state);
|
||||||
|
if (r < 0) {
|
||||||
|
printf("Failed to execute near PC=0x%04x!\n", state.reg[0]);
|
||||||
|
return 1;
|
||||||
|
//break;
|
||||||
|
}
|
||||||
|
|
||||||
|
#ifdef DEBUG
|
||||||
|
dump_state(&state);
|
||||||
|
#endif // DEBUG
|
||||||
|
} while (r != 1);
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
@ -0,0 +1,8 @@
|
|||||||
|
int main()
|
||||||
|
{
|
||||||
|
volatile int *mem = (int *)0x200;
|
||||||
|
*mem = 24;
|
||||||
|
for (int i = 0; i < 8; ++i)
|
||||||
|
*mem += 2;
|
||||||
|
return *mem;
|
||||||
|
}
|
||||||
Loading…
Reference in New Issue