int vmpu_fault_recovery_bus(uint32_t pc, uint32_t sp, uint32_t fault_addr, uint32_t fault_status)
{
uint16_t opcode;
uint32_t r0, r1;
uint32_t cnt_max, cnt;
int found;
/* Check for attacks. */
if (!vmpu_public_flash_addr(pc)) {
HALT_ERROR(NOT_ALLOWED, "This is not the PC (0x%08X) your were searching for", pc);
}
/* Check fault register; the following two configurations are allowed:
* 0x04 - imprecise data bus fault, no stacking/unstacking errors.
* 0x82 - precise data bus fault, no stacking/unstacking errors. */
/* Note: Currently the faulting address argument is not used, since it
* is saved in r0 for managed bus faults. */
switch (fault_status) {
case 0x82:
cnt_max = 0;
break;
case 0x04:
cnt_max = UVISOR_NOP_CNT;
break;
default:
return -1;
}
/* Parse the instruction opcode. */
cnt = 0;
do {
/* Fetch the opcode from memory. */
opcode = vmpu_unpriv_uint16_read(pc - (cnt << 1));
/* Test the lower 8 bits for imm5 = 0, Rn = 0, Rt = 1. */
found = TRUE;
switch(opcode & 0xFF) {
/* If using r0 and r1, we expect a strX instruction. */
case VMPU_OPCODE16_LOWER_R0_R1_MASK:
/* Fetch r0 and r1. */
r0 = vmpu_unpriv_uint32_read(sp);
r1 = vmpu_unpriv_uint32_read(sp+4);
/* Check if there is an ACL mapping this access. */
if ((vmpu_fault_find_acl(r0, sizeof(uint32_t)) & UVISOR_TACL_UWRITE) == 0) {
return -1;
};
/* Test the upper 8 bits for the desired opcode and imm5 = 0. */
switch (opcode >> 8) {
case VMPU_OPCODE16_UPPER_STR_MASK:
*((uint32_t *) r0) = (uint32_t) r1;
break;
case VMPU_OPCODE16_UPPER_STRH_MASK:
*((uint16_t *) r0) = (uint16_t) r1;
break;
case VMPU_OPCODE16_UPPER_STRB_MASK:
*((uint8_t *) r0) = (uint8_t) r1;
break;
default:
found = FALSE;
break;
}
if (found) {
/* DPRINTF("Executed privileged access: 0x%08X written to 0x%08X\n\r", r1, r0); */
}
break;
/* If using r0 only, we expect a ldrX instruction. */
case VMPU_OPCODE16_LOWER_R0_R0_MASK:
/* Fetch r0. */
r0 = vmpu_unpriv_uint32_read(sp);
/* Check if there is an ACL mapping this access. */
if ((vmpu_fault_find_acl(r0, sizeof(uint32_t)) & UVISOR_TACL_UREAD) == 0) {
return -1;
};
/* Test the upper 8 bits for the desired opcode and imm5 = 0. */
switch (opcode >> 8) {
case VMPU_OPCODE16_UPPER_LDR_MASK:
r1 = (uint32_t) *((uint32_t *) r0);
break;
case VMPU_OPCODE16_UPPER_LDRH_MASK:
r1 = (uint16_t) *((uint16_t *) r0);
break;
case VMPU_OPCODE16_UPPER_LDRB_MASK:
r1 = (uint8_t) *((uint8_t *) r0);
break;
default:
found = FALSE;
break;
}
if (found) {
/* The result is stored back to the stack (r0). */
vmpu_unpriv_uint32_write(sp, r1);
/* DPRINTF("Executed privileged access: read 0x%08X from 0x%08X\n\r", r1, r0); */
}
break;
default:
found = FALSE;
break;
}
/* Parse the next opcode. */
cnt++;
} while (!found && cnt < cnt_max);
/* Return an error if the opcode was not found. */
if (!found) {
return -1;
}
/* Otherwise execution continues from the instruction following the fault. */
/* Note: We assume the instruction is 16 bits wide and skip possible NOPs. */
vmpu_unpriv_uint32_write(sp + (6 << 2), pc + ((UVISOR_NOP_CNT + 2 - cnt) << 1));
/* Success. */
return 0;
}
int vmpu_fault_recovery_bus(uint32_t pc, uint32_t sp, uint32_t fault_addr, uint32_t fault_status)
{
uint16_t opcode;
uint32_t r0, r1;
uint32_t cnt_max, cnt;
int found;
/* check for attacks */
if(!VMPU_FLASH_ADDR(pc))
HALT_ERROR(NOT_ALLOWED, "This is not the PC (0x%08X) your were searching for", pc);
/* check fault register; the following two configurations are allowed:
* 0x04 - imprecise data bus fault, no stacking/unstacking errors
* 0x82 - precise data bus fault, no stacking/unstacking errors */
/* note: currently the faulting address argument is not used, since it
* is saved in r0 for managed bus faults */
switch(fault_status) {
case 0x82:
cnt_max = 0;
break;
case 0x04:
cnt_max = UVISOR_NOP_CNT;
break;
default:
return -1;
}
/* parse opcode */
cnt = 0;
do
{
/* fetch opcode from memory */
opcode = vmpu_unpriv_uint16_read(pc - (cnt << 1));
/* test lower 8bits for (partially)imm5 == 0, Rn = 0, Rt = 1 */
found = TRUE;
switch(opcode & 0xFF)
{
/* if using r0 and r1, we expect a strX instruction */
case VMPU_OPCODE16_LOWER_R0_R1_MASK:
/* fetch r0 and r1 */
r0 = vmpu_unpriv_uint32_read(sp);
r1 = vmpu_unpriv_uint32_read(sp+4);
/* check ACls */
if((vmpu_fault_find_acl(r0,sizeof(uint32_t)) & UVISOR_TACL_UWRITE) == 0) {
return -1;
};
/* test upper 8bits for opcode and (partially)imm5 == 0 */
switch(opcode >> 8)
{
case VMPU_OPCODE16_UPPER_STR_MASK:
*((uint32_t *) r0) = (uint32_t) r1;
break;
case VMPU_OPCODE16_UPPER_STRH_MASK:
*((uint16_t *) r0) = (uint16_t) r1;
break;
case VMPU_OPCODE16_UPPER_STRB_MASK:
*((uint8_t *) r0) = (uint8_t) r1;
break;
default:
found = FALSE;
break;
}
if (found) {
/* DPRINTF("Executed privileged access: 0x%08X written to 0x%08X\n\r", r1, r0); */
}
break;
/* if using r0 only, we expect a ldrX instruction */
case VMPU_OPCODE16_LOWER_R0_R0_MASK:
/* fetch r0 */
r0 = vmpu_unpriv_uint32_read(sp);
/* check ACls */
if((vmpu_fault_find_acl(r0,sizeof(uint32_t)) & UVISOR_TACL_UREAD) == 0) {
return -1;
};
/* test upper 8bits for opcode and (partially)imm5 == 0 */
switch(opcode >> 8)
{
case VMPU_OPCODE16_UPPER_LDR_MASK:
r1 = (uint32_t) *((uint32_t *) r0);
break;
case VMPU_OPCODE16_UPPER_LDRH_MASK:
r1 = (uint16_t) *((uint16_t *) r0);
break;
case VMPU_OPCODE16_UPPER_LDRB_MASK:
r1 = (uint8_t) *((uint8_t *) r0);
break;
default:
found = FALSE;
break;
}
if(found)
{
/* the result is stored back to the stack (r0) */
vmpu_unpriv_uint32_write(sp, r1);
/* DPRINTF("Executed privileged access: read 0x%08X from 0x%08X\n\r", r1, r0); */
}
break;
default:
found = FALSE;
break;
}
/* parse next opcode */
cnt++;
}
while(!found && cnt < cnt_max);
/* return error if opcode was not found */
if(!found)
return -1;
/* otherwise execution continues from the instruction following the fault */
/* note: we assume the instruction is 16 bits wide and skip possible NOPs */
vmpu_unpriv_uint32_write(sp + (6 << 2), pc + ((UVISOR_NOP_CNT + 2 - cnt) << 1));
/* success */
return 0;
}
