static void reset_registers(void) {
uint32_t vectortable = read_word(VECTOR_TABLE_OFFSET);
// R[0..12] = bits(32) UNKNOWN {nop}
SW(&sp_main,read_word(vectortable) & 0xfffffffc);
// sp_process = ((bits(30) UNKNOWN):'00')
SW(&sp_process, SR(&sp_process) & ~0x3);
CORE_reg_write(LR_REG, 0xFFFFFFFF);
uint32_t tmp = read_word(vectortable+4);
bool tbit = tmp & 0x1;
CORE_reg_write(PC_REG, tmp & 0xfffffffe);
if (! (tmp & 0x1) ) {
WARN("Reset vector %08x at %08x invalid\n", tmp, vectortable+4);
CORE_ERR_unpredictable("Thumb bit must be set for M-series\n");
}
// ASPR = bits(32) UNKNOWN {nop}
union ipsr_t ipsr = CORE_ipsr_read();
ipsr.bits.exception = 0;
CORE_ipsr_write(ipsr);
union epsr_t epsr = CORE_epsr_read();
epsr.bits.T = tbit;
epsr.bits.ICI_IT_top = 0;
epsr.bits.ICI_IT_bot = 0;
CORE_epsr_write(epsr);
}
void strb_imm(uint8_t rt, uint8_t rn, uint32_t imm32,
bool index, bool add, bool wback) {
DBG2("strb r%02d, [r%02d, #%08x]\n", rt, rn, imm32);
uint32_t rn_val = CORE_reg_read(rn);
uint32_t rt_val = CORE_reg_read(rt);
uint32_t offset_addr;
if (add) {
offset_addr = rn_val + imm32;
} else {
offset_addr = rn_val - imm32;
}
uint32_t address;
if (index) {
address = offset_addr;
} else {
address = rn_val;
}
DBG2("address: %08x\n", address);
write_byte(address, rt_val & 0xff);
if (wback) {
CORE_reg_write(rn, offset_addr);
}
DBG2("strb_imm ran\n");
}
void pop(uint16_t registers) {
uint32_t address = CORE_reg_read(SP_REG);
int i;
for (i = 0; i <= 14; i++) {
if (registers & (1 << i)) {
CORE_reg_write(i, read_word(address));
address += 4;
}
}
if (registers & (1 << 15)) {
LoadWritePC(read_word(address));
}
CORE_reg_write(SP_REG, CORE_reg_read(SP_REG) + 4 * hamming(registers));
}
static void reset_registers(void) {
DBG2("begin\n");
uint32_t vectortable = read_word(VECTOR_TABLE_OFFSET);
// R[0..12] = bits(32) UNKNOWN {nop}
SW(&sp_main,read_word(vectortable) & 0xfffffffc);
// sp_process = ((bits(30) UNKNOWN):'00')
SW(&sp_process, SR(&sp_process) & ~0x3);
CORE_reg_write(LR_REG, 0xFFFFFFFF);
uint32_t tmp = read_word(vectortable+4);
bool tbit = tmp & 0x1;
CORE_reg_write(PC_REG, tmp & 0xfffffffe);
if (!tbit) {
WARN("Reset vector %08x at %08x invalid\n", tmp, vectortable+4);
CORE_ERR_unpredictable("Thumb bit must be set for M-series\n");
}
CORE_CurrentMode_write(Mode_Thread);
// ASPR = bits(32) UNKNOWN {nop}
union ipsr_t ipsr_temp = CORE_ipsr_read();
ipsr_temp.bits.exception = 0;
CORE_ipsr_write(ipsr_temp);
union epsr_t epsr_temp = CORE_epsr_read();
epsr_temp.bits.T = tbit;
epsr_temp.bits.ICI_IT_top = 0;
epsr_temp.bits.ICI_IT_bot = 0;
CORE_epsr_write(epsr_temp);
///
// B1.4.3: The special-purpose mask registers
SW(&physical_primask, 0);
SW(&physical_faultmask, 0);
SW(&physical_basepri, 0);
SW(&physical_control.storage, 0);
DBG2("end\n");
}
void orr_imm(uint8_t rd, uint8_t rn, bool setflags,
union apsr_t apsr, uint32_t imm32, bool carry) {
uint32_t result = CORE_reg_read(rn) | imm32;
CORE_reg_write(rd, result);
if (setflags) {
apsr.bits.N = HIGH_BIT(result);
apsr.bits.Z = result == 0;
apsr.bits.C = carry;
CORE_apsr_write(apsr);
}
}
void bic_imm(union apsr_t apsr, uint8_t setflags,
uint8_t rd, uint8_t rn, uint32_t imm32, uint8_t carry) {
uint32_t result = CORE_reg_read(rn) & (~imm32);
CORE_reg_write(rd, result);
if (setflags) {
apsr.bits.N = HIGH_BIT(result);
apsr.bits.Z = result == 0;
apsr.bits.C = carry;
CORE_apsr_write(apsr);
}
}
static void ldm(uint8_t rn, uint16_t registers, uint8_t wback) {
uint32_t address = CORE_reg_read(rn);
int i;
for (i = 0; i <= 14; i++) { // stupid arm inclusive for
if (registers & (1 << i)) {
CORE_reg_write(i, read_word(address));
address += 4;
}
}
if (registers & 0x8000) {
//LoadWritePC(MemA[address, 4]);
CORE_ERR_not_implemented("ldm PC");
}
if (wback && ((registers & (1 << rn)) == 0)) {
CORE_reg_write(rn, CORE_reg_read(rn) + 4U*hamming(registers));
}
DBG2("ldm had loads of fun\n");
}
void bic_reg(uint8_t rd, uint8_t rn, uint8_t rm,
bool setflags, enum SRType shift_t, uint8_t shift_n) {
union apsr_t apsr = CORE_apsr_read();
uint32_t shifted;
bool carry_out;
Shift_C(CORE_reg_read(rm), 32, shift_t, shift_n, apsr.bits.C,
&shifted, &carry_out);
uint32_t result = CORE_reg_read(rn) & ~shifted;
CORE_reg_write(rd, result);
if (setflags) {
apsr.bits.N = HIGH_BIT(result);
apsr.bits.Z = result == 0;
apsr.bits.C = carry_out;
CORE_apsr_write(apsr);
}
}
void orr_reg(uint8_t setflags, uint8_t rd, uint8_t rn, uint8_t rm,
enum SRType shift_t, uint8_t shift_n) {
uint32_t result;
bool carry_out;
union apsr_t apsr = CORE_apsr_read();
Shift_C(CORE_reg_read(rm), 32, shift_t, shift_n, apsr.bits.C, &result, &carry_out);
result = CORE_reg_read(rn) | result;
CORE_reg_write(rd, result);
if (setflags) {
apsr.bits.N = HIGH_BIT(result);
apsr.bits.Z = result == 0;
apsr.bits.C = carry_out;
CORE_apsr_write(apsr);
}
}
void and_imm(union apsr_t apsr, uint8_t setflags, uint8_t rd, uint8_t rn,
uint32_t imm32, uint8_t carry) {
uint32_t rn_val = CORE_reg_read(rn);
uint32_t result = rn_val & imm32;
if (rd == 15) {
// ALUWritePC(result);
CORE_ERR_not_implemented("ALUWritePC and_imm\n");
} else {
CORE_reg_write(rd, result);
if (setflags) {
apsr.bits.N = HIGH_BIT(result);
apsr.bits.Z = result == 0;
apsr.bits.C = carry;
CORE_apsr_write(apsr);
}
}
DBG2("and_imm done\n");
}
void str_imm(uint8_t rt, uint8_t rn, uint32_t imm32,
bool index, bool add, bool wback) {
uint32_t rn_val = CORE_reg_read(rn);
uint32_t offset_addr;
if (add)
offset_addr = rn_val + imm32;
else
offset_addr = rn_val - imm32;
uint32_t address;
if (index)
address = offset_addr;
else
address = rn_val;
uint32_t rt_val = CORE_reg_read(rt);
write_word_unaligned(address, rt_val);
if (wback)
CORE_reg_write(rn, offset_addr);
}
void strd_imm(uint8_t rt, uint8_t rt2, uint8_t rn, uint32_t imm32,
bool index, bool add, bool wback) {
uint32_t offset_addr;
if (add) {
offset_addr = CORE_reg_read(rn) + imm32;
} else {
offset_addr = CORE_reg_read(rn) - imm32;
}
uint32_t address;
if (index) {
address = offset_addr;
} else {
address = CORE_reg_read(rn);
}
write_word_aligned(address, CORE_reg_read(rt));
write_word_aligned(address + 4, CORE_reg_read(rt2));
if (wback) {
CORE_reg_write(rn, offset_addr);
}
}