1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
|
// SPDX-License-Identifier: BSD-2-Clause
/*
* Copyright (c) 2016-2020, Linaro Limited
* Copyright (c) 2014, STMicroelectronics International N.V.
*/
#include <arm.h>
#include <compiler.h>
#include <config.h>
#include <kernel/misc.h>
#include <kernel/thread.h>
#include <platform_config.h>
#include <sm/optee_smc.h>
#include <sm/sm.h>
#include <sm/std_smc.h>
#include <string.h>
#include "sm_private.h"
#include <console.h>
#include <mm/core_memprot.h>
#include <rzn1_regauth.h>
#define OEM_SVC_PUTC 0x83000001
#define OEM_SVC_SYSREG 0x83000010
static regauth_t *get_regauth(unsigned long paddr)
{
unsigned int min = 0;
unsigned int max = sizeof(regauth)/sizeof(regauth[0]);
unsigned int pos = max/2;
while ( min < max ) {
pos = (min+max)/2;
if ( regauth[pos].paddr == paddr ) return regauth+pos;
if ( regauth[pos].paddr < paddr ) min = pos+1; else max = pos;
}
return NULL;
}
static uint32_t oem_sysreg (unsigned long addr, unsigned long mask, uint32_t *pvalue)
{
regauth_t *auth;
volatile unsigned long *reg;
// Allow registers not in the list
if ( !(auth = get_regauth(addr)) ) {
static regauth_t pass = {.rmask = ~0UL,.wmask = ~0UL};
auth = &pass;
}
// Sanity check
reg = (unsigned long*)core_mmu_get_va(addr, MEM_AREA_IO_SEC);
// Perform allowed operations
if (mask) { // Write
mask &= auth->wmask;
if ( mask == 0UL ) { // Bits to write are protected
DMSG("Blocking write of 0x%x to register 0x%lx (0x%lx)", *pvalue, addr, *reg);
} else if ( mask == ~0UL ) { // Full write
*reg = *pvalue;
} else { // Partial write with read ahead
*reg = ( *reg & ~mask ) | ( *pvalue & mask );
}
} else { // Read
if ( auth->rmask == 0UL ) {
DMSG("Blocking read of register 0x%lx (0x%lx)", addr, *reg);
} else {
*pvalue = *reg & auth->rmask;
}
}
return 0;
}
enum sm_handler_ret __weak sm_platform_handler(struct sm_ctx *ctx __unused)
{
return SM_HANDLER_PENDING_SMC;
}
static void smc_arch_handler(struct thread_smc_args *args)
{
uint32_t smc_fid = args->a0;
uint32_t feature_fid = args->a1;
switch (smc_fid) {
case ARM_SMCCC_VERSION:
args->a0 = SMCCC_V_1_1;
break;
case ARM_SMCCC_ARCH_FEATURES:
switch (feature_fid) {
case ARM_SMCCC_VERSION:
case ARM_SMCCC_ARCH_SOC_ID:
args->a0 = ARM_SMCCC_RET_SUCCESS;
break;
default:
args->a0 = ARM_SMCCC_RET_NOT_SUPPORTED;
break;
}
break;
case ARM_SMCCC_ARCH_SOC_ID:
args->a0 = ARM_SMCCC_RET_NOT_SUPPORTED;
break;
case ARM_SMCCC_ARCH_WORKAROUND_1:
case ARM_SMCCC_ARCH_WORKAROUND_2:
args->a0 = ARM_SMCCC_RET_NOT_REQUIRED;
break;
default:
args->a0 = OPTEE_SMC_RETURN_UNKNOWN_FUNCTION;
break;
}
}
uint32_t sm_from_nsec(struct sm_ctx *ctx)
{
uint32_t *nsec_r0 = (uint32_t *)(&ctx->nsec.r0);
struct thread_smc_args *args = (struct thread_smc_args *)nsec_r0;
/*
* Check that struct sm_ctx has the different parts properly
* aligned since the stack pointer will be updated to point at
* different parts of this struct.
*/
COMPILE_TIME_ASSERT(!(offsetof(struct sm_ctx, sec.r0) % 8));
COMPILE_TIME_ASSERT(!(offsetof(struct sm_ctx, nsec.r0) % 8));
COMPILE_TIME_ASSERT(!(sizeof(struct sm_ctx) % 8));
if (IS_ENABLED(CFG_SM_PLATFORM_HANDLER) &&
sm_platform_handler(ctx) == SM_HANDLER_SMC_HANDLED)
return SM_EXIT_TO_NON_SECURE;
switch (OPTEE_SMC_OWNER_NUM(args->a0)) {
case OPTEE_SMC_OWNER_STANDARD:
if (IS_ENABLED(CFG_PSCI_ARM32)) {
smc_std_handler(args, &ctx->nsec);
return SM_EXIT_TO_NON_SECURE;
}
break;
case OPTEE_SMC_OWNER_ARCH:
smc_arch_handler(args);
return SM_EXIT_TO_NON_SECURE;
default:
break;
}
switch ( nsec_r0[0] ) {
case OEM_SVC_PUTC:
console_putc(nsec_r0[1]);
return false; /* Return to non secure state */
case OEM_SVC_SYSREG:
nsec_r0[0] = oem_sysreg(nsec_r0[1],nsec_r0[2],nsec_r0+3);
nsec_r0[1] = nsec_r0[3];
return false; /* Return to non secure state */
default:
break;
}
sm_save_unbanked_regs(&ctx->nsec.ub_regs);
sm_restore_unbanked_regs(&ctx->sec.ub_regs);
memcpy(&ctx->sec.r0, args, sizeof(*args));
if (OPTEE_SMC_IS_FAST_CALL(ctx->sec.r0))
ctx->sec.mon_lr = (uint32_t)&thread_vector_table.fast_smc_entry;
else
ctx->sec.mon_lr = (uint32_t)&thread_vector_table.std_smc_entry;
return SM_EXIT_TO_SECURE;
}
|
