From: Juha Riihimäki <juha.riihim...@nokia.com>
Signed-Off-By: Riku Voipio <riku.voi...@nokia.com>
Signed-off-by: Juha Riihimäki <juha.riihim...@nokia.com>
---
hw/omap3_boot.c | 32 +++++++++++++++--------
target-arm/cpu.h | 10 +++++--
target-arm/helper.c | 67 +++++++++++++++++++++++++++++++++++++++++++-----
target-arm/translate.c | 31 ++++++++++++++++++---
4 files changed, 114 insertions(+), 26 deletions(-)
diff --git a/hw/omap3_boot.c b/hw/omap3_boot.c
index c079c1d..6f3389f 100644
--- a/hw/omap3_boot.c
+++ b/hw/omap3_boot.c
@@ -98,12 +98,17 @@ static const uint8_t omap3_boot_rom[] = { /*
0x40014000-0x4001bfff */
0xff, 0xff, 0xff, 0xff, /* 0x40014048: booting parameter structure 4-7 */
0xff, 0xff, 0xff, 0xff, /* 0x4001404c: booting parameter structure 8-11 */
0x0e, 0xf0, 0xb0, 0xe1, /* 0x40014050: "movs pc, lr" */
- 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ 0xff, 0xff, 0xff, 0xff, /* 0x40014054 */
+ 0xff, 0xff, 0xff, 0xff, /* 0x40014058 */
+ 0xff, 0xff, 0xff, 0xff, /* 0x4001405c */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x40014060: monitor vector 0 (unused) */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x40014064: monitor vector 1 (unused) */
+ 0x15, 0x00, 0x00, 0xea, /* 0x40014068: monitor vector 2 (smc) */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x4001406c: monitor vector 3 (pabt) */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x40014070: monitor vector 4 (dabt) */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x40014074: monitor vector 5 (unused) */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x40014078: monitor vector 6 (irq) */
+ 0xfe, 0xff, 0xff, 0xea, /* 0x4001407c: monitor vector 7 (fiq) */
/* 0x40014080: Dead loops */
0xfe, 0xff, 0xff, 0xea, /* b 0x40014080 @ undefined exception */
0xfe, 0xff, 0xff, 0xea, /* b 0x40014084 @ swi exception */
@@ -123,9 +128,12 @@ static const uint8_t omap3_boot_rom[] = { /*
0x40014000-0x4001bfff */
0xfe, 0xff, 0xff, 0xea, /* b 0x400140bc @ reserved */
/* 0x400140c0: should perform a software reset & jump to r0 */
0x00, 0xf0, 0xa0, 0xe1, /* mov pc, r0 */
- 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
- 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
+ /* 0x400140c4: monitor mode smc vector handler */
+ 0x02, 0x00, 0x5c, 0xe3, /* cmp r12, #2 */
+ 0x50, 0x0f, 0x29, 0xee, /* mcreq p15, 1, r0, c9, c0, 2 @ l2c aux ctrl */
+ 0x03, 0x00, 0x5c, 0xe3, /* cmp r12, #3 */
+ 0x30, 0x0f, 0x01, 0xee, /* mcreq p15, 0, r0, c1, c0, 1 @ aux ctrl*/
+ 0x0e, 0xf0, 0xb0, 0xe1, /* movs pc, r14 */
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff,
@@ -153,8 +161,10 @@ static const uint8_t omap3_boot_rom[] = { /*
0x40014000-0x4001bfff */
0x08, 0x0c, 0x40, 0xe2, /* sub r0, r0, #2048 @ 2kB UND stack */
0xd3, 0xf0, 0x21, 0xe3, /* msr cpsr_c, #0xd3 @ enter SVC mode */
0x00, 0xd0, 0xa0, 0xe1, /* mov sp, r0 @ 23kB left for SVC stack */
- 0x44, 0x00, 0x04, 0xe3, /* movw r0, #0x4044 */
- 0x01, 0x00, 0x44, 0xe3, /* movt r0, #0x4001 @ r0 -> booting parameter
struct */
+ 0x60, 0x00, 0x04, 0xe3, /* movw r0, #0x4060 @ r0 -> monitor vba */
+ 0x01, 0x00, 0x44, 0xe3, /* movt r0, #0x4001 */
+ 0x30, 0x0f, 0x0c, 0xfe, /* mcr2 p15, 0, r0, c12, c0, 1 */
+ 0x1c, 0x00, 0x40, 0xe2, /* sub r0, r0, #1c @ r0 -> booting parameter
struct */
0x01, 0xf0, 0xa0, 0xe1, /* mov pc, r1 */
};
diff --git a/target-arm/cpu.h b/target-arm/cpu.h
index 3c5e181..7440163 100644
--- a/target-arm/cpu.h
+++ b/target-arm/cpu.h
@@ -41,6 +41,7 @@
#define EXCP_EXCEPTION_EXIT 8 /* Return from v7M exception. */
#define EXCP_KERNEL_TRAP 9 /* Jumped to kernel code page. */
#define EXCP_STREX 10
+#define EXCP_SMC 11 /* secure monitor call */
#define ARMV7M_EXCP_RESET 1
#define ARMV7M_EXCP_NMI 2
@@ -80,9 +81,9 @@ typedef struct CPUARMState {
uint32_t spsr;
/* Banked registers. */
- uint32_t banked_spsr[6];
- uint32_t banked_r13[6];
- uint32_t banked_r14[6];
+ uint32_t banked_spsr[7];
+ uint32_t banked_r13[7];
+ uint32_t banked_r14[7];
/* These hold r8-r12. */
uint32_t usr_regs[5];
@@ -129,6 +130,8 @@ typedef struct CPUARMState {
uint32_t c6_data;
uint32_t c9_insn; /* Cache lockdown registers. */
uint32_t c9_data;
+ uint32_t c12_vbar; /* secure/nonsecure vector base address register. */
+ uint32_t c12_mvbar; /* monitor vector base address register. */
uint32_t c13_fcse; /* FCSE PID. */
uint32_t c13_context; /* Context ID. */
uint32_t c13_tls1; /* User RW Thread register. */
@@ -308,6 +311,7 @@ enum arm_cpu_mode {
ARM_CPU_MODE_FIQ = 0x11,
ARM_CPU_MODE_IRQ = 0x12,
ARM_CPU_MODE_SVC = 0x13,
+ ARM_CPU_MODE_SMC = 0x16,
ARM_CPU_MODE_ABT = 0x17,
ARM_CPU_MODE_UND = 0x1b,
ARM_CPU_MODE_SYS = 0x1f
diff --git a/target-arm/helper.c b/target-arm/helper.c
index a726947..798e9f9 100644
--- a/target-arm/helper.c
+++ b/target-arm/helper.c
@@ -586,6 +586,8 @@ static inline int bank_number (int mode)
return 4;
case ARM_CPU_MODE_FIQ:
return 5;
+ case ARM_CPU_MODE_SMC:
+ return 6;
}
cpu_abort(cpu_single_env, "Bad mode %x\n", mode);
return -1;
@@ -835,13 +837,39 @@ void do_interrupt(CPUARMState *env)
mask = CPSR_A | CPSR_I | CPSR_F;
offset = 4;
break;
+ case EXCP_SMC:
+ if (semihosting_enabled) {
+ cpu_abort(env, "SMC handling under semihosting not implemented\n");
+ return;
+ }
+ if ((env->uncached_cpsr & CPSR_M) == ARM_CPU_MODE_SMC) {
+ env->cp15.c1_secfg &= ~1;
+ }
+ offset = env->thumb ? 2 : 0;
+ new_mode = ARM_CPU_MODE_SMC;
+ addr = 0x08;
+ mask = CPSR_A | CPSR_I | CPSR_F;
+ break;
default:
cpu_abort(env, "Unhandled exception 0x%x\n", env->exception_index);
return; /* Never happens. Keep compiler happy. */
}
- /* High vectors. */
- if (env->cp15.c1_sys & (1 << 13)) {
- addr += 0xffff0000;
+ if (arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+ if (new_mode == ARM_CPU_MODE_SMC ||
+ (env->uncached_cpsr & CPSR_M) == ARM_CPU_MODE_SMC) {
+ addr += env->cp15.c12_mvbar;
+ } else {
+ if (env->cp15.c1_sys & (1 << 13)) {
+ addr += 0xffff0000;
+ } else {
+ addr += env->cp15.c12_vbar;
+ }
+ }
+ } else {
+ /* High vectors. */
+ if (env->cp15.c1_sys & (1 << 13)) {
+ addr += 0xffff0000;
+ }
}
switch_mode (env, new_mode);
env->spsr = cpsr_read(env);
@@ -1538,6 +1566,27 @@ void HELPER(set_cp15)(CPUState *env, uint32_t insn,
uint32_t val)
/* ??? TLB lockdown not implemented. */
break;
case 12: /* Reserved. */
+ if (!op1 && !crm) {
+ switch (op2) {
+ case 0:
+ if (!arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+ goto bad_reg;
+ }
+ env->cp15.c12_vbar = val & ~0x1f;
+ break;
+ case 1:
+ if (!arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+ goto bad_reg;
+ }
+ if (!(env->cp15.c1_secfg & 1)) {
+ env->cp15.c12_mvbar = val & ~0x1f;
+ }
+ break;
+ default:
+ goto bad_reg;
+ }
+ break;
+ }
goto bad_reg;
case 13: /* Process ID. */
switch (op2) {
@@ -1856,12 +1905,16 @@ uint32_t HELPER(get_cp15)(CPUState *env, uint32_t insn)
return 0;
case 11: /* TCM DMA control. */
case 12: /* Reserved. */
- if (!op1) {
- switch (crm) {
+ if (!op1 && !crm) {
+ switch (op2) {
case 0: /* secure or nonsecure vector base address */
if (arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
- /* FIXME: implement true vector base addressing */
- return 0; /* reset value according to ARM Cortex-A8 TRM */
+ return env->cp15.c12_vbar;
+ }
+ break;
+ case 1: /* monitor vector base address */
+ if (arm_feature(env, ARM_FEATURE_TRUSTZONE)) {
+ return env->cp15.c12_mvbar;
}
break;
default:
diff --git a/target-arm/translate.c b/target-arm/translate.c
index b214bff..e5f5cfb 100644
--- a/target-arm/translate.c
+++ b/target-arm/translate.c
@@ -73,6 +73,7 @@ typedef struct DisasContext {
conditional executions state has been updated. */
#define DISAS_WFI 4
#define DISAS_SWI 5
+#define DISAS_SMC 6
static TCGv_ptr cpu_env;
/* We reuse the same 64-bit temporaries for efficiency. */
@@ -837,6 +838,12 @@ static inline void store_reg_bx(CPUState *env,
DisasContext *s,
}
}
+static inline void gen_smc(CPUState *env, DisasContext *s)
+{
+ tcg_gen_movi_i32(cpu_R[15], s->pc);
+ s->is_jmp = DISAS_SMC;
+}
+
static inline TCGv gen_ld8s(TCGv addr, int index)
{
TCGv tmp = new_tmp();
@@ -6245,8 +6252,12 @@ static void disas_arm_insn(CPUState * env, DisasContext
*s)
}
} else if ((insn & 0x0fe00000) == 0x0c400000) {
/* Coprocessor double register transfer. */
+ cpu_abort(env, "unsupported coprocessor double register
transfer\n");
} else if ((insn & 0x0f000010) == 0x0e000010) {
/* Additional coprocessor register transfer. */
+ if (!disas_coproc_insn(env, s, insn)) {
+ return;
+ }
} else if ((insn & 0x0ff10020) == 0x01000000) {
uint32_t mask;
uint32_t val;
@@ -6401,10 +6412,10 @@ static void disas_arm_insn(CPUState * env, DisasContext
*s)
s->is_jmp = DISAS_JUMP;
} else if (op1 == 3) {
/* smi/smc */
- if (!(env->cp15.c0_c2[4] & 0xf000) || IS_USER(s))
+ if (!(env->cp15.c0_c2[4] & 0xf000) || IS_USER(s)) {
goto illegal_op;
- /* TODO: real implementation; execute as NOP for now */
- /*fprintf(stderr, "smc [0x%08x] pc=0x%08x\n", insn, s->pc);*/
+ }
+ gen_smc(env, s);
} else {
goto illegal_op;
}
@@ -7991,8 +8002,11 @@ static int disas_thumb2_insn(CPUState *env, DisasContext
*s, uint16_t insn_hw1)
goto illegal_op;
if (insn & (1 << 26)) {
- /* Secure monitor call (v6Z) */
- goto illegal_op; /* not implemented. */
+ /* Secure monitor call / smc (v6Z) */
+ if (!(env->cp15.c0_c2[4] & 0xf000) || IS_USER(s)) {
+ goto illegal_op;
+ }
+ gen_smc(env, s);
} else {
op = (insn >> 20) & 7;
switch (op) {
@@ -9245,6 +9259,8 @@ static inline void
gen_intermediate_code_internal(CPUState *env,
gen_set_condexec(dc);
if (dc->is_jmp == DISAS_SWI) {
gen_exception(EXCP_SWI);
+ } else if (dc->is_jmp == DISAS_SMC) {
+ gen_exception(EXCP_SMC);
} else {
gen_exception(EXCP_DEBUG);
}
@@ -9257,6 +9273,8 @@ static inline void
gen_intermediate_code_internal(CPUState *env,
gen_set_condexec(dc);
if (dc->is_jmp == DISAS_SWI && !dc->condjmp) {
gen_exception(EXCP_SWI);
+ } else if (dc->is_jmp == DISAS_SMC && !dc->condjmp) {
+ gen_exception(EXCP_SMC);
} else {
/* FIXME: Single stepping a WFI insn will not halt
the CPU. */
@@ -9291,6 +9309,9 @@ static inline void
gen_intermediate_code_internal(CPUState *env,
case DISAS_SWI:
gen_exception(EXCP_SWI);
break;
+ case DISAS_SMC:
+ gen_exception(EXCP_SMC);
+ break;
}
if (dc->condjmp) {
gen_set_label(dc->condlabel);
--
1.6.5
