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; }