614 lines
24 KiB
Rust
614 lines
24 KiB
Rust
#![allow(dead_code)]
|
||
#![allow(unused_variables)]
|
||
|
||
use decoder;
|
||
use decoder::{IncrementMode, Instruction};
|
||
use regs::{StatusFlag, GeneralPurposeRegister, GeneralPurposeRegisterPair};
|
||
use chip_definitions;
|
||
|
||
use std::fmt;
|
||
|
||
#[derive(Debug)]
|
||
pub enum CPUError {
|
||
UnimplementedInstruction,
|
||
OutOfBoundsException,
|
||
UnsupportedAddress,
|
||
DecodingError(decoder::DecodingError),
|
||
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,
|
||
}
|
||
|
||
|
||
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, "-")?;
|
||
}
|
||
}
|
||
write!(f, "\n")?;
|
||
for i in 0..32 {
|
||
write!(f, " R{:-2} = 0x{:02X} ", i, self.registers[i])?;
|
||
if (i + 1) % 10 == 0{
|
||
write!(f, "\n")?;
|
||
}
|
||
}
|
||
write!(f, "\n")
|
||
}
|
||
}
|
||
|
||
impl CPU {
|
||
pub fn new() -> Self {
|
||
CPU {
|
||
registers: [0u8; 32],
|
||
pc: 0, // Reset vector
|
||
sreg: 0, // Uninitialized as well
|
||
}
|
||
}
|
||
|
||
fn get_sp(&self, mem: &[u8]) -> u16 {
|
||
mem[chip_definitions::IOAdress::SPL as usize] as u16 | ((mem[chip_definitions::IOAdress::SPH as usize] as u16) << 8)
|
||
}
|
||
|
||
fn set_sp(&self, mem: &mut [u8], val: u16) {
|
||
mem[chip_definitions::IOAdress::SPL as usize] = val as u8;
|
||
mem[chip_definitions::IOAdress::SPH as usize] = (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 {
|
||
((self.registers[r.high()] as u16) << 8) | self.registers[r.low()] as u16
|
||
}
|
||
|
||
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, ram: &mut [u8], addr: u16, val: u8) -> Result<(), CPUError> {
|
||
print!("[RAMW:{:04X}={:02X}] ", addr, val);
|
||
if addr as usize >= ram.len() {
|
||
Err(CPUError::OutOfBoundsException)
|
||
} else {
|
||
// TODO: Hooks
|
||
if addr == chip_definitions::IOAdress::USARTC0_DATA as _ {
|
||
print!("USART_OUT:{: <3} ({}) ", val, (val as char).escape_debug());
|
||
}
|
||
ram[addr as usize] = val;
|
||
Ok(())
|
||
}
|
||
}
|
||
|
||
fn ram_read(&self, ram: &[u8], addr: u16) -> Result<u8, CPUError> {
|
||
print!("[RAMR:{:04X}] ", addr);
|
||
if addr as usize >= ram.len() {
|
||
Err(CPUError::OutOfBoundsException)
|
||
} else {
|
||
// TODO: Hooks
|
||
if addr == chip_definitions::IOAdress::USARTC0_DATA as _ {
|
||
return Err(CPUError::Exit);
|
||
} else if addr == chip_definitions::IOAdress::USARTC0_STATUS as _ {
|
||
if self.pc == 0x5AC {
|
||
// USART data check -> Yes, there is indeed data available!
|
||
return Ok(0x80);
|
||
} else {
|
||
return Ok(0x20); // Usart is ready to send.
|
||
}
|
||
} else if addr == chip_definitions::IOAdress::OSC_STATUS as _ {
|
||
// HACK: Osci is set right..
|
||
return Ok(0x02);
|
||
}
|
||
Ok(ram[addr as usize])
|
||
}
|
||
}
|
||
|
||
fn push(&mut self, ram: &mut [u8], val: u8) -> Result<(), CPUError> {
|
||
let sp = self.get_sp(&ram);
|
||
self.ram_write(ram, sp, val)?;
|
||
self.set_sp(ram, sp.wrapping_sub(1));
|
||
Ok(())
|
||
}
|
||
|
||
fn pop(&mut self, ram: &mut [u8]) -> Result<u8, CPUError> {
|
||
let sp = self.get_sp(&ram);
|
||
self.set_sp(ram, sp.wrapping_add(1));
|
||
self.ram_read(ram, 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], ram: &mut [u8]) -> 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)),
|
||
};
|
||
print!("CPU: pc={:06X} sp={:04X} Fetch: {: <40} -> ", self.pc, self.get_sp(ram), 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 _),
|
||
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(o as _),
|
||
};
|
||
self.ram_write(ram, addr, self.get_register(src_reg))?;
|
||
},
|
||
Instruction::LD(ref dst_reg, ref ptr, 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(o as _),
|
||
};
|
||
let v = self.ram_read(ram, addr)?;
|
||
self.set_register(dst_reg, v);
|
||
},
|
||
Instruction::ELPM(ref dst_reg, ref inc_mode) => {
|
||
// TODO: RAMPZ
|
||
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
|
||
},
|
||
}
|
||
},
|
||
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
|
||
},
|
||
}
|
||
},
|
||
Instruction::OUT(ref addr, ref val) => {
|
||
let val = self.get_register(val);
|
||
self.ram_write(ram, *addr, val)?;
|
||
},
|
||
Instruction::IN(ref reg, ref addr) => {
|
||
let v = self.ram_read(ram, *addr)?;
|
||
self.set_register(reg, v);
|
||
}
|
||
Instruction::CALL(ref addr) => {
|
||
let ret_to = self.pc;
|
||
self.push(ram, ((ret_to >> 16) & 0xFF) as u8)?;
|
||
self.push(ram, ((ret_to >> 8) & 0xFF) as u8)?;
|
||
self.push(ram, (ret_to & 0xFF) as u8)?;
|
||
self.pc = *addr;
|
||
},
|
||
Instruction::RCALL(ref addr) => {
|
||
let ret_to = self.pc;
|
||
self.push(ram, ((ret_to >> 16) & 0xFF) as u8)?;
|
||
self.push(ram, ((ret_to >> 8) & 0xFF) as u8)?;
|
||
self.push(ram, (ret_to & 0xFF) as u8)?;
|
||
self.pc = (self.pc as i32 + *addr as i32) as u32;
|
||
},
|
||
Instruction::RET => {
|
||
let mut ret_to = self.pop(ram)? as u32;
|
||
ret_to += (self.pop(ram)? as u32) << 8;
|
||
ret_to += (self.pop(ram)? as u32) << 16;
|
||
self.pc = ret_to as _;
|
||
},
|
||
Instruction::POP(ref reg) => {
|
||
let v = self.pop(ram)?;
|
||
self.registers[reg.as_usize()] = v;
|
||
},
|
||
Instruction::PUSH(ref reg) => {
|
||
let v = self.registers[reg.as_usize()];
|
||
self.push(ram, 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) => {
|
||
self.ram_write(ram, *addr, self.get_register(r))?;
|
||
},
|
||
Instruction::STS8(ref addr, ref r) => {
|
||
self.ram_write(ram, *addr as u16, self.get_register(r))?;
|
||
},
|
||
Instruction::LDS16(ref r, ref addr) => {
|
||
let v = self.ram_read(ram, *addr)?;
|
||
self.set_register(r, v);
|
||
},
|
||
Instruction::LDS8(ref r, ref addr) => {
|
||
let v = self.ram_read(ram, *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 = self.get_register(r) as u16;
|
||
let d = self.get_register(d) as u16;
|
||
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::SWAP(ref r) => {
|
||
let rv = self.get_register(r);
|
||
self.set_register(r, rv >> 4 | ((rv << 4) & 0xF0));
|
||
},
|
||
Instruction::NOP => {},
|
||
_ => return Err(CPUError::UnimplementedInstruction)
|
||
}
|
||
|
||
Ok(ins.cycles())
|
||
}
|
||
}
|