680 lines
26 KiB
Rust
680 lines
26 KiB
Rust
#![allow(dead_code)]
|
||
#![allow(unused_variables)]
|
||
|
||
use chip_definitions::IOAdress;
|
||
use decoder::{self, IncrementMode, Instruction};
|
||
use devices::DeviceTree;
|
||
use regs::{GeneralPurposeRegister, GeneralPurposeRegisterPair, StatusFlag};
|
||
|
||
use std::fmt;
|
||
|
||
use slog;
|
||
|
||
#[derive(Debug)]
|
||
pub enum CPUError {
|
||
UnimplementedInstruction(decoder::Instruction),
|
||
OutOfBoundsException,
|
||
UnsupportedAddress,
|
||
DecodingError(decoder::DecodingError),
|
||
Breakpoint,
|
||
Exit,
|
||
}
|
||
|
||
// CPU
|
||
#[derive(Debug)]
|
||
pub struct CPU {
|
||
// General purpose registers
|
||
pub registers: [u8; 32],
|
||
|
||
// PC
|
||
pub pc: u32,
|
||
|
||
// The same is true for the status register
|
||
pub sreg: u8,
|
||
|
||
logger: slog::Logger,
|
||
}
|
||
|
||
impl fmt::Display for CPU {
|
||
fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result {
|
||
write!(
|
||
f,
|
||
"CPU @ 0x{:X} (file offset = 0x{:X}) ",
|
||
self.pc,
|
||
2 * self.pc
|
||
)?;
|
||
write!(f, "SREG: 0x{:02X}: ", self.sreg)?;
|
||
for i in 0..7 {
|
||
let t = self.sreg & (1 << i);
|
||
if t > 0 {
|
||
write!(f, "{}", StatusFlag::try_from(t).unwrap())?;
|
||
} else {
|
||
write!(f, "-")?;
|
||
}
|
||
}
|
||
writeln!(f)?;
|
||
for i in 0..32 {
|
||
write!(f, " R{:-2}={:02X} ", i, self.registers[i])?;
|
||
if (i + 1) % 10 == 0 {
|
||
writeln!(f)?;
|
||
}
|
||
}
|
||
writeln!(f)
|
||
}
|
||
}
|
||
|
||
impl CPU {
|
||
pub fn new(logger: slog::Logger) -> Self {
|
||
CPU {
|
||
registers: [0u8; 32],
|
||
pc: 0, // Reset vector
|
||
sreg: 0, // Uninitialized as well
|
||
logger,
|
||
}
|
||
}
|
||
|
||
pub fn get_sp(&self, mem: &mut DeviceTree) -> u16 {
|
||
let spl: u16 = mem.read(IOAdress::SPL as u32) as u16;
|
||
let sph: u16 = mem.read(IOAdress::SPH as u32) as u16;
|
||
sph << 8 | spl
|
||
}
|
||
|
||
fn set_sp(&self, mem: &mut DeviceTree, val: u16) {
|
||
mem.write(IOAdress::SPL as u32, val as u8);
|
||
mem.write(IOAdress::SPH as u32, (val >> 8) as u8);
|
||
}
|
||
|
||
fn test_flag(&self, flag: StatusFlag) -> bool {
|
||
(self.sreg & (flag as u8)) > 0
|
||
}
|
||
|
||
fn set_flag(&mut self, flag: StatusFlag) {
|
||
self.sreg |= flag as u8;
|
||
}
|
||
|
||
fn clear_flag(&mut self, flag: StatusFlag) {
|
||
self.sreg &= !(flag as u8);
|
||
}
|
||
|
||
fn set_clear_flag(&mut self, flag: StatusFlag, test: bool) {
|
||
// print!("[{}->{}] ", flag, test);
|
||
if test {
|
||
self.set_flag(flag);
|
||
} else {
|
||
self.clear_flag(flag);
|
||
}
|
||
}
|
||
|
||
fn get_register_pair(&self, r: &GeneralPurposeRegisterPair) -> u16 {
|
||
(u16::from(self.registers[r.high()]) << 8) | u16::from(self.registers[r.low()])
|
||
}
|
||
|
||
fn set_register_pair(&mut self, r: &GeneralPurposeRegisterPair, v: u16) {
|
||
self.registers[r.high()] = (v >> 8) as u8;
|
||
self.registers[r.low()] = (v & 0xFF) as u8;
|
||
}
|
||
|
||
fn get_register(&self, r: &GeneralPurposeRegister) -> u8 {
|
||
self.registers[r.as_usize()]
|
||
}
|
||
|
||
fn set_register(&mut self, r: &GeneralPurposeRegister, v: u8) {
|
||
self.registers[r.as_usize()] = v;
|
||
}
|
||
|
||
fn ram_write(&self, device_tree: &mut DeviceTree, addr: u16, val: u8) -> Result<(), CPUError> {
|
||
device_tree.write(addr as u32, val);
|
||
Ok(())
|
||
}
|
||
|
||
fn ram_read(&self, device_tree: &mut DeviceTree, addr: u16) -> Result<u8, CPUError> {
|
||
Ok(device_tree.read(addr as u32))
|
||
}
|
||
|
||
fn push(&mut self, device_tree: &mut DeviceTree, val: u8) -> Result<(), CPUError> {
|
||
let sp = self.get_sp(device_tree);
|
||
self.ram_write(device_tree, sp, val)?;
|
||
self.set_sp(device_tree, sp.wrapping_sub(1));
|
||
Ok(())
|
||
}
|
||
|
||
fn pop(&mut self, device_tree: &mut DeviceTree) -> Result<u8, CPUError> {
|
||
let sp = self.get_sp(device_tree);
|
||
self.set_sp(device_tree, sp.wrapping_add(1));
|
||
self.ram_read(device_tree, sp.wrapping_add(1))
|
||
}
|
||
|
||
// Flag update functions on a single value:
|
||
fn update_flags_zns_8(&mut self, v: u8) {
|
||
self.set_clear_flag(StatusFlag::Zero, v == 0);
|
||
self.set_clear_flag(StatusFlag::Negative, v & 0x80 == 0x80);
|
||
let V = self.test_flag(StatusFlag::TwosComplementOverflow);
|
||
self.set_clear_flag(StatusFlag::SignBit, (v & 0x80 == 0x80) ^ V);
|
||
}
|
||
|
||
fn update_flags_zns_16(&mut self, v: u16) {
|
||
self.set_clear_flag(StatusFlag::Zero, v == 0);
|
||
self.set_clear_flag(StatusFlag::Negative, v & 0x8000 == 0x8000);
|
||
let V = self.test_flag(StatusFlag::TwosComplementOverflow);
|
||
self.set_clear_flag(StatusFlag::SignBit, (v & 0x8000 == 0x8000) ^ V);
|
||
}
|
||
|
||
// Copied from simavr, let's trust them for now.
|
||
fn update_flags_add_zns(&mut self, result: u8, d: u8, r: u8) {
|
||
let add_carry = (d & r) | (r & !result) | (!result & d);
|
||
self.set_clear_flag(StatusFlag::HalfCarry, add_carry & 0b1000 != 0);
|
||
self.set_clear_flag(StatusFlag::Carry, add_carry & 0b1000_0000 != 0);
|
||
self.set_clear_flag(
|
||
StatusFlag::TwosComplementOverflow,
|
||
((d & r & !result) | (!d & !r & result)) & 0x80 == 0x80,
|
||
);
|
||
self.update_flags_zns_8(result);
|
||
}
|
||
|
||
fn update_flags_sub_zns(&mut self, result: u8, d: u8, r: u8) {
|
||
let sub_carry = (!d & r) | (r & result) | (result & !d);
|
||
self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
|
||
self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
|
||
self.set_clear_flag(
|
||
StatusFlag::TwosComplementOverflow,
|
||
((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80,
|
||
);
|
||
self.update_flags_zns_8(result);
|
||
}
|
||
|
||
fn update_flags_Rzns(&mut self, r: u8) {
|
||
if r != 0 {
|
||
self.clear_flag(StatusFlag::Zero);
|
||
}
|
||
self.set_clear_flag(StatusFlag::Negative, r & 0x80 == 0x80);
|
||
let b = self.test_flag(StatusFlag::TwosComplementOverflow);
|
||
self.set_clear_flag(StatusFlag::SignBit, r & 0x80 == 0x80 && b);
|
||
}
|
||
|
||
fn update_flags_sub_Rzns(&mut self, result: u8, d: u8, r: u8) {
|
||
let sub_carry = (!d & r) | (r & result) | (result & !d);
|
||
self.set_clear_flag(StatusFlag::HalfCarry, sub_carry & 0b1000 != 0);
|
||
self.set_clear_flag(StatusFlag::Carry, sub_carry & 0b1000_0000 != 0);
|
||
self.set_clear_flag(
|
||
StatusFlag::TwosComplementOverflow,
|
||
((d & !r & !result) | (!d & r & result)) & 0x80 == 0x80,
|
||
);
|
||
self.update_flags_Rzns(result);
|
||
}
|
||
|
||
fn update_flags_zcvs(&mut self, result: u8, vr: u8) {
|
||
// Workaround lexicalic lifetimes.
|
||
let z = result == 0;
|
||
let c = vr & 1 == 1;
|
||
let n = self.test_flag(StatusFlag::Negative);
|
||
self.set_clear_flag(StatusFlag::Zero, z);
|
||
self.set_clear_flag(StatusFlag::Carry, c);
|
||
self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
|
||
self.set_clear_flag(StatusFlag::SignBit, c);
|
||
}
|
||
|
||
fn update_flags_zcnvs(&mut self, result: u8, vr: u8) {
|
||
let z = result == 0;
|
||
let c = vr & 1 == 1;
|
||
let n = result & 0x80 == 0x80;
|
||
self.set_clear_flag(StatusFlag::Zero, z);
|
||
self.set_clear_flag(StatusFlag::Negative, n);
|
||
self.set_clear_flag(StatusFlag::Carry, c);
|
||
self.set_clear_flag(StatusFlag::TwosComplementOverflow, n ^ c);
|
||
self.set_clear_flag(StatusFlag::SignBit, c);
|
||
}
|
||
|
||
fn update_flags_znv0s(&mut self, v: u8) {
|
||
self.clear_flag(StatusFlag::TwosComplementOverflow);
|
||
self.update_flags_zns_8(v);
|
||
}
|
||
|
||
// Returns # of ticks the executed instruction took
|
||
pub fn step(
|
||
&mut self,
|
||
rom: &mut [u8],
|
||
device_tree: &mut DeviceTree,
|
||
) -> Result<usize, CPUError> {
|
||
// Instruction fetch
|
||
if (self.pc as usize) * 2 >= rom.len() {
|
||
return Err(CPUError::OutOfBoundsException);
|
||
}
|
||
|
||
let ins = match decoder::decode(&rom[(self.pc as usize) * 2..]) {
|
||
Ok(v) => v,
|
||
Err(e) => return Err(CPUError::DecodingError(e)),
|
||
};
|
||
debug!(
|
||
self.logger,
|
||
"CPU: pc={:06X} sp={:04X} Fetch: {: <40}",
|
||
self.pc,
|
||
self.get_sp(device_tree),
|
||
format!("{}", ins)
|
||
);
|
||
|
||
self.pc += (ins.size() / 2) as u32;
|
||
|
||
// Instruction execute
|
||
match ins {
|
||
Instruction::JMP(v) => self.pc = v,
|
||
Instruction::CLR(ref r) => {
|
||
self.set_register(r, 0);
|
||
self.update_flags_znv0s(0);
|
||
}
|
||
Instruction::EOR(ref d, ref r) => {
|
||
self.registers[d.as_usize()] ^= self.registers[r.as_usize()];
|
||
let r = self.registers[d.as_usize()];
|
||
self.update_flags_znv0s(r);
|
||
}
|
||
Instruction::LDI(ref r, v) => self.set_register(r, v),
|
||
Instruction::SER(ref r) => self.set_register(r, 0xFF),
|
||
Instruction::RJMP(v) => {
|
||
self.pc = self.pc.wrapping_add(v as _);
|
||
if v == -1 && !self.test_flag(StatusFlag::GlobalInterruptEnable) {
|
||
info!(self.logger, "HALTED ");
|
||
return Err(CPUError::Exit);
|
||
}
|
||
}
|
||
Instruction::CLR_FLAG(v) => self.clear_flag(v),
|
||
Instruction::SET_FLAG(v) => self.set_flag(v),
|
||
Instruction::CPI(ref r, v) => {
|
||
let rv = self.get_register(r);
|
||
self.update_flags_sub_zns(rv.wrapping_sub(v), rv, v);
|
||
}
|
||
Instruction::CP(ref r, ref i) => {
|
||
let rv = self.get_register(r);
|
||
let iv = self.get_register(i);
|
||
self.update_flags_sub_zns(rv.wrapping_sub(iv), rv, iv);
|
||
}
|
||
Instruction::CPC(ref d, ref r) => {
|
||
let rd: u8 = self.get_register(d);
|
||
let rr: u8 = self.get_register(r);
|
||
|
||
let s = if self.test_flag(StatusFlag::Carry) {
|
||
rd.wrapping_sub(rr).wrapping_sub(1)
|
||
} else {
|
||
rd.wrapping_sub(rr)
|
||
};
|
||
|
||
self.update_flags_sub_Rzns(s, rd, rr);
|
||
}
|
||
Instruction::BR_IF(offset, flag, test) => {
|
||
if self.test_flag(flag) == test {
|
||
self.pc = self.pc.wrapping_add(offset as _);
|
||
}
|
||
}
|
||
Instruction::ST(ref ptr, ref src_reg, ref inc_mode) => {
|
||
let base = self.get_register_pair(ptr);
|
||
let addr = match *inc_mode {
|
||
IncrementMode::None => base,
|
||
IncrementMode::PreDecrement => {
|
||
self.set_register_pair(ptr, base.wrapping_sub(1));
|
||
base.wrapping_sub(1)
|
||
}
|
||
IncrementMode::PostIncrement => {
|
||
self.set_register_pair(ptr, base.wrapping_add(1));
|
||
base
|
||
}
|
||
IncrementMode::ConstantOffset(o) => base.wrapping_add(u16::from(o)),
|
||
};
|
||
self.ram_write(device_tree, addr, self.get_register(src_reg))?;
|
||
}
|
||
Instruction::LD(ref dst_reg, ref ptr, ref inc_mode) => {
|
||
let base = self.get_register_pair(ptr);
|
||
/*
|
||
// TODO: RAMPX/Y/Z
|
||
if ptr.low() == 26 && ram[IOAdress::RAMPX as usize] > 0 {
|
||
panic!("Unexpected");
|
||
} else if ptr.low() == 28 && ram[IOAdress::RAMPY as usize] > 0 {
|
||
panic!("Unexpected");
|
||
} else if ptr.low() == 30 && ram[IOAdress::RAMPZ as usize] > 0 {
|
||
panic!("Unexpected");
|
||
}
|
||
*/
|
||
let addr = match *inc_mode {
|
||
IncrementMode::None => base,
|
||
IncrementMode::PreDecrement => {
|
||
self.set_register_pair(ptr, base.wrapping_sub(1));
|
||
base.wrapping_sub(1)
|
||
}
|
||
IncrementMode::PostIncrement => {
|
||
self.set_register_pair(ptr, base.wrapping_add(1));
|
||
base
|
||
}
|
||
IncrementMode::ConstantOffset(o) => base.wrapping_add(u16::from(o)),
|
||
};
|
||
let v = self.ram_read(device_tree, addr)?;
|
||
self.set_register(dst_reg, v);
|
||
}
|
||
Instruction::ELPM(ref dst_reg, ref inc_mode) => {
|
||
let Zb = self.get_register_pair(&30u8.into());
|
||
// TODO: Only use required bits, other read as zero (according to datasheet)
|
||
let Z = Zb as usize | (device_tree.read(IOAdress::RAMPZ as u32) as usize) << 16;
|
||
|
||
match *inc_mode {
|
||
IncrementMode::PostIncrement => {
|
||
self.set_register_pair(&30u8.into(), Zb.wrapping_add(1));
|
||
device_tree.write(IOAdress::RAMPZ as u32, (Z.wrapping_add(1) >> 16) as u8);
|
||
}
|
||
_ => {
|
||
// This instruction does only support None + PostIncrement
|
||
panic!("Invalid increment mode for ELPM: {:?}", inc_mode);
|
||
}
|
||
}
|
||
if Z >= rom.len() {
|
||
warn!(
|
||
self.logger,
|
||
"ELPM OOB: RAMPZ={:02X} Z={:04X} len={:06X} ",
|
||
Z >> 16,
|
||
Zb,
|
||
rom.len()
|
||
);
|
||
// return Err(CPUError::OutOfBoundsException);
|
||
return Ok(0xFF); // Hack I kno but emulator does it like that
|
||
}
|
||
let d = rom[Z as usize];
|
||
self.set_register(dst_reg, d);
|
||
}
|
||
Instruction::LPM(ref dst_reg, ref inc_mode) => {
|
||
let Z = self.get_register_pair(&30u8.into());
|
||
let d = rom[Z as usize];
|
||
self.set_register(dst_reg, d);
|
||
match *inc_mode {
|
||
IncrementMode::PostIncrement => {
|
||
self.set_register_pair(&30u8.into(), Z.wrapping_add(1));
|
||
}
|
||
_ => {
|
||
// This instruction does only support None + PostIncrement
|
||
panic!("Invalid increment mode for LPM: {:?}", inc_mode);
|
||
}
|
||
}
|
||
}
|
||
Instruction::OUT(ref addr, ref val) => {
|
||
let val = self.get_register(val);
|
||
self.ram_write(device_tree, *addr, val)?;
|
||
}
|
||
Instruction::IN(ref reg, ref addr) => {
|
||
let v = self.ram_read(device_tree, *addr)?;
|
||
self.set_register(reg, v);
|
||
}
|
||
Instruction::CALL(ref addr) => {
|
||
let ret_to = self.pc;
|
||
self.push(device_tree, ((ret_to >> 16) & 0xFF) as u8)?;
|
||
self.push(device_tree, ((ret_to >> 8) & 0xFF) as u8)?;
|
||
self.push(device_tree, (ret_to & 0xFF) as u8)?;
|
||
self.pc = *addr;
|
||
}
|
||
Instruction::RCALL(ref addr) => {
|
||
let ret_to = self.pc;
|
||
self.push(device_tree, ((ret_to >> 16) & 0xFF) as u8)?;
|
||
self.push(device_tree, ((ret_to >> 8) & 0xFF) as u8)?;
|
||
self.push(device_tree, (ret_to & 0xFF) as u8)?;
|
||
self.pc = (self.pc as i32 + *addr as i32) as u32;
|
||
}
|
||
Instruction::RET => {
|
||
let mut ret_to = self.pop(device_tree)? as u32;
|
||
ret_to += (self.pop(device_tree)? as u32) << 8;
|
||
ret_to += (self.pop(device_tree)? as u32) << 16;
|
||
self.pc = ret_to as _;
|
||
}
|
||
Instruction::POP(ref reg) => {
|
||
let v = self.pop(device_tree)?;
|
||
self.registers[reg.as_usize()] = v;
|
||
}
|
||
Instruction::PUSH(ref reg) => {
|
||
let v = self.registers[reg.as_usize()];
|
||
self.push(device_tree, v)?;
|
||
}
|
||
Instruction::SUBI(ref d, ref v) => {
|
||
let dv = self.get_register(d);
|
||
let vres = dv.wrapping_sub(*v);
|
||
|
||
self.update_flags_sub_zns(vres, dv, *v);
|
||
self.set_register(d, vres);
|
||
}
|
||
Instruction::SBC(ref d, ref r) => {
|
||
let dv = self.get_register(d);
|
||
let rv = self.get_register(r);
|
||
let mut vres = dv.wrapping_sub(rv);
|
||
if self.test_flag(StatusFlag::Carry) {
|
||
vres = vres.wrapping_sub(1);
|
||
}
|
||
|
||
self.update_flags_sub_Rzns(vres, dv, rv);
|
||
self.set_register(d, vres);
|
||
}
|
||
Instruction::SBCI(ref d, ref v) => {
|
||
let dv = self.get_register(d);
|
||
let mut vres = dv.wrapping_sub(*v);
|
||
if self.test_flag(StatusFlag::Carry) {
|
||
vres = vres.wrapping_sub(1);
|
||
}
|
||
|
||
self.update_flags_sub_Rzns(vres, dv, *v);
|
||
self.set_register(d, vres);
|
||
}
|
||
Instruction::MOVW(ref d, ref r) => {
|
||
let t = self.get_register_pair(r);
|
||
self.set_register_pair(d, t);
|
||
}
|
||
Instruction::OR(ref d, ref r) => {
|
||
let t = self.get_register(d) | self.get_register(r);
|
||
self.set_register(d, t);
|
||
self.update_flags_znv0s(t);
|
||
}
|
||
Instruction::ORI(ref d, ref v) => {
|
||
let t = self.get_register(d) | *v;
|
||
self.set_register(d, t);
|
||
self.update_flags_znv0s(t);
|
||
}
|
||
Instruction::AND(ref d, ref r) => {
|
||
let t = self.get_register(d) & self.get_register(r);
|
||
self.set_register(d, t);
|
||
self.update_flags_znv0s(t);
|
||
}
|
||
Instruction::TST(ref r) => {
|
||
let t = self.get_register(r);
|
||
self.update_flags_znv0s(t);
|
||
}
|
||
Instruction::ANDI(ref d, ref v) => {
|
||
let t = self.get_register(d) & *v;
|
||
self.set_clear_flag(StatusFlag::Zero, t == 0);
|
||
self.set_register(d, t);
|
||
}
|
||
Instruction::ADD(ref d, ref r) => {
|
||
let t = self.get_register(d).wrapping_add(self.get_register(r));
|
||
let dd = self.get_register(d);
|
||
self.set_clear_flag(StatusFlag::Carry, t < dd);
|
||
self.set_clear_flag(StatusFlag::Zero, t == 0);
|
||
self.set_register(d, t);
|
||
}
|
||
Instruction::ADC(ref d, ref r) => {
|
||
let mut t = self.get_register(d).wrapping_add(self.get_register(r));
|
||
if self.test_flag(StatusFlag::Carry) {
|
||
t = t.wrapping_add(1);
|
||
}
|
||
let dd = self.get_register(d);
|
||
self.set_clear_flag(StatusFlag::Carry, t < dd);
|
||
self.set_clear_flag(StatusFlag::Zero, t == 0);
|
||
self.set_register(d, t);
|
||
}
|
||
Instruction::SUB(ref d, ref r) => {
|
||
let dv = self.get_register(d);
|
||
let rv = self.get_register(r);
|
||
let res = dv.wrapping_sub(rv);
|
||
self.update_flags_sub_zns(res, dv, rv);
|
||
self.set_register(d, res);
|
||
}
|
||
Instruction::MOV(ref d, ref r) => {
|
||
let v = self.get_register(r);
|
||
self.set_register(d, v);
|
||
}
|
||
Instruction::ADIW(ref d, ref v) => {
|
||
let dv = self.get_register_pair(d);
|
||
let res = dv.wrapping_add(*v);
|
||
self.set_register_pair(d, res);
|
||
self.set_clear_flag(
|
||
StatusFlag::TwosComplementOverflow,
|
||
(dv & !res) & 0x8000 == 0x8000,
|
||
);
|
||
self.set_clear_flag(StatusFlag::Carry, (!dv & res) & 0x8000 == 0x8000);
|
||
self.update_flags_zns_16(res);
|
||
}
|
||
Instruction::SBIW(ref d, ref v) => {
|
||
let dv = self.get_register_pair(d);
|
||
let res = dv.wrapping_sub(*v as _);
|
||
self.set_clear_flag(
|
||
StatusFlag::TwosComplementOverflow,
|
||
(!dv & res) & 0x8000 == 0x8000,
|
||
);
|
||
self.set_clear_flag(StatusFlag::Carry, (dv & !res) & 0x8000 == 0x8000);
|
||
self.update_flags_zns_16(res);
|
||
self.set_register_pair(d, res);
|
||
}
|
||
Instruction::DEC(ref r) => {
|
||
let rv = self.get_register(r);
|
||
let res = rv.wrapping_sub(1);
|
||
self.set_register(r, res);
|
||
self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x7F);
|
||
self.update_flags_zns_8(res);
|
||
}
|
||
Instruction::INC(ref r) => {
|
||
let rv = self.get_register(r);
|
||
let res = rv.wrapping_add(1);
|
||
self.set_register(r, res);
|
||
|
||
self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
|
||
self.update_flags_zns_8(res);
|
||
}
|
||
Instruction::STS16(ref addr, ref r) => {
|
||
let rampd = device_tree.read(IOAdress::RAMPD as u32);
|
||
if rampd != 0 {
|
||
panic!("This is unexpected (for now)");
|
||
}
|
||
self.ram_write(device_tree, *addr, self.get_register(r))?;
|
||
}
|
||
Instruction::STS8(ref addr, ref r) => {
|
||
self.ram_write(device_tree, *addr as u16, self.get_register(r))?;
|
||
}
|
||
Instruction::LDS16(ref r, ref addr) => {
|
||
let rampd = device_tree.read(IOAdress::RAMPD as u32);
|
||
if rampd != 0 {
|
||
panic!("This is unexpected (for now)");
|
||
}
|
||
let v = self.ram_read(device_tree, *addr)?;
|
||
self.set_register(r, v);
|
||
}
|
||
Instruction::LDS8(ref r, ref addr) => {
|
||
let v = self.ram_read(device_tree, *addr as u16)?;
|
||
self.set_register(r, v);
|
||
}
|
||
Instruction::LSL(ref r) => {
|
||
let v = self.get_register(r);
|
||
self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
|
||
self.set_register(r, v << 1);
|
||
self.set_clear_flag(StatusFlag::Zero, (v << 1) == 0);
|
||
}
|
||
Instruction::ROL(ref r) => {
|
||
let v = self.get_register(r);
|
||
let c = if self.test_flag(StatusFlag::Carry) {
|
||
1
|
||
} else {
|
||
0
|
||
};
|
||
self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
|
||
self.set_register(r, v << 1 | c);
|
||
self.set_clear_flag(StatusFlag::Zero, (v << 1) | c == 0);
|
||
}
|
||
Instruction::ASR(ref r) => {
|
||
let rv = self.get_register(r);
|
||
let res = rv >> 1 | (rv & 0x80);
|
||
self.set_register(r, res);
|
||
self.update_flags_zcnvs(res, rv);
|
||
}
|
||
Instruction::LSR(ref r) => {
|
||
let rv = self.get_register(r);
|
||
self.set_register(r, rv >> 1);
|
||
self.clear_flag(StatusFlag::Negative);
|
||
self.update_flags_zcvs(rv >> 1, rv);
|
||
}
|
||
Instruction::ROR(ref r) => {
|
||
let rv = self.get_register(r);
|
||
let c = if self.test_flag(StatusFlag::Carry) {
|
||
0x80
|
||
} else {
|
||
0
|
||
};
|
||
let res = rv >> 1 | c;
|
||
self.set_register(r, res);
|
||
self.update_flags_zcnvs(res, rv);
|
||
}
|
||
Instruction::SBRS(ref r, ref bit) => {
|
||
let r = self.get_register(r);
|
||
if (r & (1 << *bit)) > 0 {
|
||
self.pc += 1;
|
||
}
|
||
}
|
||
Instruction::SBRC(ref r, ref bit) => {
|
||
let r = self.get_register(r);
|
||
if (r & (1 << *bit)) == 0 {
|
||
self.pc += 1;
|
||
}
|
||
}
|
||
Instruction::CPSE(ref r, ref d) => {
|
||
if self.get_register(r) == self.get_register(d) {
|
||
// TODO: assume 2b instruction after this one.
|
||
self.pc += 1;
|
||
}
|
||
}
|
||
Instruction::COM(ref r) => {
|
||
let rv = self.get_register(r);
|
||
let res = 0xFFu8.wrapping_sub(rv);
|
||
self.set_register(r, res);
|
||
self.update_flags_znv0s(res);
|
||
self.set_flag(StatusFlag::Carry);
|
||
}
|
||
Instruction::NEG(ref r) => {
|
||
let rv = self.get_register(r);
|
||
let res = 0u8.wrapping_sub(rv);
|
||
self.set_register(r, res);
|
||
self.set_clear_flag(StatusFlag::HalfCarry, (rv | res) & 0x08 == 0x08);
|
||
self.set_clear_flag(StatusFlag::TwosComplementOverflow, res == 0x80);
|
||
self.set_clear_flag(StatusFlag::Carry, res != 0);
|
||
self.update_flags_zns_8(rv);
|
||
}
|
||
Instruction::MUL(ref r, ref d) => {
|
||
// R1:R0 ← Rd × Rr(unsigned ← unsigned × unsigned)
|
||
let r = u16::from(self.get_register(r));
|
||
let d = u16::from(self.get_register(d));
|
||
let v = r * d;
|
||
self.registers[0] = (v & 0xFF) as u8;
|
||
self.registers[1] = ((v >> 8) & 0xFF) as u8;
|
||
self.set_clear_flag(StatusFlag::Carry, v & 0x80 == 0x80);
|
||
self.set_clear_flag(StatusFlag::Zero, v == 0);
|
||
}
|
||
Instruction::BST(ref r, ref v) => {
|
||
let r = self.get_register(r);
|
||
self.set_clear_flag(StatusFlag::BitCopyStorage, r & (1 << *v) != 0);
|
||
}
|
||
Instruction::BLD(ref r, ref v) => {
|
||
let mut rv = self.get_register(r);
|
||
if self.test_flag(StatusFlag::BitCopyStorage) {
|
||
rv |= 1 << *v;
|
||
}
|
||
self.set_register(r, rv);
|
||
}
|
||
Instruction::SWAP(ref r) => {
|
||
let rv = self.get_register(r);
|
||
self.set_register(r, rv >> 4 | ((rv << 4) & 0xF0));
|
||
}
|
||
Instruction::BREAK => {
|
||
return Err(CPUError::Breakpoint);
|
||
}
|
||
Instruction::NOP => {}
|
||
_ => return Err(CPUError::UnimplementedInstruction(ins)),
|
||
}
|
||
|
||
Ok(ins.cycles())
|
||
}
|
||
}
|