/* read register by GDB */
void ARMV5::regRead(uint32_t* data, uint16_t index)
{
if (index <= FPS) {
rfRead(data, index, CPSR_MODE(rf.cpsr));
if (index == PC) {
*data -= 4;
}
}
else {
switch (index) {
case PID:
*data = cp15.c13_context;
break;
case SYS:
*data = cp15.c1_sys;
break;
case TTBR:
*data = cp15.c2_ttbr;
break;
// case DFSR: *data = cp15.c5_dfsr; break;
// case IFSR: *data = cp15.c5_ifsr; break;
// case FAR: *data = cp15.c6_far; break;
case DOM:
*data = 0;
for (int i = 0; i < 16; i++) {
*data |= (cp15.c3_domain[i] << (i * 2));
}
break;
//case INST: *data = inst_arm; break;
//case CYCLE: *data = cycle_total; break;
case INST:
*data = 0;
printm(core_id, d_armv5, "gdb INST not implemented yet");
break;
case CYCLE:
*data = 0;
printm(core_id, d_armv5, "gdb CYCLE not implemented yet");
break;
default:
*data = 0;
printm(core_id, d_armv5, "no such register");
break;
}
}
}
void computeRC() {
uint8_t chan;
//Data comes from the ATMEGA128RF internal transceiver
if(rfAvailable()>=RC_CHANS*sizeof(int16_t)) {
memcpy(rcData, rfRead(), RC_CHANS*sizeof(int16_t));
#if defined(FAILSAFE)
failsafeCnt = 0;
#endif
}
// rcData comes from MSP and overrides RX Data until rcSerialCount reaches 0
if (rcSerialCount > 0){
for (chan = 0; chan < RC_CHANS; chan++) {
if (chan<8) {
rcSerialCount --;
if (rcSerial[chan] >900) {rcData[chan] = rcSerial[chan];} // only relevant channels are overridden
}
}
}
}
void ARMV5::addr_mode3_imm()
{
bool p = inst & M(24);
bool u = inst & M(23);
bool w = inst & M(21);
bool l = inst & M(20);
uint8_t rn = SMM(inst, 19, 16);
uint8_t immedH = SMM(inst, 11, 8);
bool s = inst & M(6);
bool h = inst & M(5);
uint8_t immedL = SMM(inst, 3, 0);
uint8_t offset_8 = 0;
uint32_t rn_val = 0;
uint32_t rn_val_old = 0;
uint32_t addr = 0;
rfRead(&rn_val, rn, CPSR_MODE(rf.cpsr));
rn_val_old = rn_val;
if (rn == 15) {
printb(core_id, d_armv5_decode, "addr_mode3_reg rn = 15");
}
if (p && !w) {
offset_8 = (immedH << 4) | immedL;
if (u) {
addr = rn_val + offset_8;
}
else {
addr = rn_val - offset_8;
}
}
else if (p && w) {
offset_8 = (immedH << 4) | immedL;
if (u) {
addr = rn_val + offset_8;
}
else {
addr = rn_val - offset_8;
}
rfWrite(addr, rn, CPSR_MODE(rf.cpsr));
}
else if (!p && !w) {
addr = rn_val;
offset_8 = (immedH << 4) | immedL;
if (u) {
rn_val += offset_8;
}
else {
rn_val -= offset_8;
}
rfWrite(rn_val, rn, CPSR_MODE(rf.cpsr));
}
else {
printb(core_id, d_armv5_decode, "addr_mode3_imm decode error");
}
uint8_t tmp = 0;
if (l) {
tmp |= M(2);
}
if (s) {
tmp |= M(1);
}
if (h) {
tmp |= M(0);
}
switch (tmp) {
case B8(001):
arm_strh(addr, rn_val_old);
break;
case B8(010):
arm_ldrd(addr, rn_val_old);
break;
case B8(011):
arm_strd(addr, rn_val_old);
break;
case B8(101):
arm_ldrh(addr, rn_val_old);
break;
case B8(110):
arm_ldrsb(addr, rn_val_old);
break;
case B8(111):
arm_ldrsh(addr, rn_val_old);
break;
default:
printb(core_id, d_armv5_decode, "addr_mode3_imm decode error");
}
}
void ARMV5::addr_mode3_reg()
{
uint8_t SBZ = SMM(inst, 11, 8);
if (SBZ != 0) {
printb(core_id, d_armv5_decode, "addr_mode3_reg check error");
}
bool p = inst & M(24);
bool u = inst & M(23);
bool w = inst & M(21);
bool l = inst & M(20);
uint8_t rn = SMM(inst, 19, 16);
bool s = inst & M(6);
bool h = inst & M(5);
uint8_t rm = SMM(inst, 3, 0);
uint32_t rn_val = 0;
uint32_t rm_val = 0;
uint32_t rn_val_old = 0;
uint32_t addr = 0;
rfRead(&rn_val, rn, CPSR_MODE(rf.cpsr));
rfRead(&rm_val, rm, CPSR_MODE(rf.cpsr));
rn_val_old = rn_val;
if (rn == 15) {
printb(core_id, d_armv5_decode, "addr_mode3_reg rn = 15");
}
if (rm == 15) {
printb(core_id, d_armv5_decode, "addr_mode3_reg rm = 15");
}
if (p && !w) {
if (u) {
addr = rn_val + rm_val;
}
else {
addr = rn_val - rm_val;
}
}
else if (p && w) {
if (u) {
addr = rn_val + rm_val;
}
else {
addr = rn_val - rm_val;
}
rfWrite(addr, rn, CPSR_MODE(rf.cpsr));
}
else if (!p && !w) {
addr = rn_val;
if (u) {
rn_val += rm_val;
}
else {
rn_val -= rm_val;
}
rfWrite(rn_val, rn, CPSR_MODE(rf.cpsr));
}
else {
printb(core_id, d_armv5_decode, "addr_mode3_reg p-bit w-bit decode error");
}
uint8_t tmp = 0;
if (l) {
tmp |= M(2);
}
if (s) {
tmp |= M(1);
}
if (h) {
tmp |= M(0);
}
switch (tmp) {
case B8(001):
arm_strh(addr, rn_val_old);
break;
case B8(010):
arm_ldrd(addr, rn_val_old);
break;
case B8(011):
arm_strd(addr, rn_val_old);
break;
case B8(101):
arm_ldrh(addr, rn_val_old);
break;
case B8(110):
arm_ldrsb(addr, rn_val_old);
break;
case B8(111):
arm_ldrsh(addr, rn_val_old);
break;
default:
printb(core_id, d_armv5_decode, "addr_mode3_reg l-bit s-bit h-bit decode error");
}
}
