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