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;
}
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;
}
/* Perform a register gateway operation. */
void register_gateway_perform_operation(uint32_t svc_sp, uint32_t svc_pc)
{
/* Check if the SVCall points to a register gateway. */
TRegisterGateway const * const register_gateway = (TRegisterGateway const * const) svc_pc;
int status = register_gateway_check(register_gateway);
if (status != REGISTER_GATEWAY_STATUS_OK) {
HALT_ERROR(PERMISSION_DENIED, "Register gateway 0x%08X not allowed. Error: %d.",
(uint32_t) register_gateway, status);
return;
}
/* From now on we can assume the register_gateway structure and the address
* are valid. */
/* Fetch the value from the user stack.
* This is only needed for write operations. */
uint32_t value = vmpu_unpriv_uint32_read(svc_sp);
/* De-reference the address.
* The value at *address is always needed for every operation. */
uint32_t address = register_gateway->address;
uint32_t width = (register_gateway->operation & __UVISOR_RGW_OP_WIDTH_MASK) >> __UVISOR_RGW_OP_WIDTH_POS;
uint32_t result = 0;
switch(width) {
case 4:
result = (uint32_t) *((uint32_t *) address);
break;
case 2:
result = (uint32_t) *((uint16_t *) address);
break;
case 1:
result = (uint32_t) *((uint8_t *) address);
break;
default:
HALT_ERROR(NOT_ALLOWED, "Register level gateway: Width %d not allowed.", width);
break;
}
/* Perform the actual operation.
* Read operations store the return value onto the user stack. */
uint32_t operation = (register_gateway->operation & __UVISOR_RGW_OP_TYPE_MASK) >> __UVISOR_RGW_OP_TYPE_POS;
switch (operation) {
case UVISOR_RGW_OP_READ:
return vmpu_unpriv_uint32_write(svc_sp, result);
case UVISOR_RGW_OP_READ_AND:
result &= register_gateway->mask;
return vmpu_unpriv_uint32_write(svc_sp, result);
case UVISOR_RGW_OP_WRITE:
result = value;
break;
case UVISOR_RGW_OP_WRITE_AND:
result &= (value | ~(register_gateway->mask));
break;
case UVISOR_RGW_OP_WRITE_OR:
result |= (value & register_gateway->mask);
break;
case UVISOR_RGW_OP_WRITE_XOR:
result ^= (value & register_gateway->mask);
break;
case UVISOR_RGW_OP_WRITE_REPLACE:
result = (result & ~(register_gateway->mask)) | (value & register_gateway->mask);
break;
default:
HALT_ERROR(NOT_ALLOWED, "Register level gateway: Operation 0x%08X not recognised.", operation);
break;
}
/* Store the result at the target address.
* The code runs here only if the register gateway performs a write
* operation. */
switch(width) {
case 4:
*((uint32_t *) address) = (uint32_t) result;
break;
case 2:
*((uint16_t *) address) = (uint16_t) result;
break;
case 1:
*((uint8_t *) address) = (uint8_t) result;
break;
default:
HALT_ERROR(NOT_ALLOWED, "Register level gateway: Width %d not allowed.", width);
break;
}
}
