1
2
3
4
5
6#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
7
8#include <linux/delay.h>
9#include <linux/device.h>
10#include <linux/i2c.h>
11#include <linux/slab.h>
12#include <linux/tee_drv.h>
13#include "optee_private.h"
14#include "optee_smc.h"
15#include "optee_rpc_cmd.h"
16
17struct wq_entry {
18 struct list_head link;
19 struct completion c;
20 u32 key;
21};
22
23void optee_wait_queue_init(struct optee_wait_queue *priv)
24{
25 mutex_init(&priv->mu);
26 INIT_LIST_HEAD(&priv->db);
27}
28
29void optee_wait_queue_exit(struct optee_wait_queue *priv)
30{
31 mutex_destroy(&priv->mu);
32}
33
34static void handle_rpc_func_cmd_get_time(struct optee_msg_arg *arg)
35{
36 struct timespec64 ts;
37
38 if (arg->num_params != 1)
39 goto bad;
40 if ((arg->params[0].attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
41 OPTEE_MSG_ATTR_TYPE_VALUE_OUTPUT)
42 goto bad;
43
44 ktime_get_real_ts64(&ts);
45 arg->params[0].u.value.a = ts.tv_sec;
46 arg->params[0].u.value.b = ts.tv_nsec;
47
48 arg->ret = TEEC_SUCCESS;
49 return;
50bad:
51 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
52}
53
54#if IS_REACHABLE(CONFIG_I2C)
55static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
56 struct optee_msg_arg *arg)
57{
58 struct tee_param *params;
59 struct i2c_adapter *adapter;
60 struct i2c_msg msg = { };
61 size_t i;
62 int ret = -EOPNOTSUPP;
63 u8 attr[] = {
64 TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT,
65 TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INPUT,
66 TEE_IOCTL_PARAM_ATTR_TYPE_MEMREF_INOUT,
67 TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_OUTPUT,
68 };
69
70 if (arg->num_params != ARRAY_SIZE(attr)) {
71 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
72 return;
73 }
74
75 params = kmalloc_array(arg->num_params, sizeof(struct tee_param),
76 GFP_KERNEL);
77 if (!params) {
78 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
79 return;
80 }
81
82 if (optee_from_msg_param(params, arg->num_params, arg->params))
83 goto bad;
84
85 for (i = 0; i < arg->num_params; i++) {
86 if (params[i].attr != attr[i])
87 goto bad;
88 }
89
90 adapter = i2c_get_adapter(params[0].u.value.b);
91 if (!adapter)
92 goto bad;
93
94 if (params[1].u.value.a & OPTEE_RPC_I2C_FLAGS_TEN_BIT) {
95 if (!i2c_check_functionality(adapter,
96 I2C_FUNC_10BIT_ADDR)) {
97 i2c_put_adapter(adapter);
98 goto bad;
99 }
100
101 msg.flags = I2C_M_TEN;
102 }
103
104 msg.addr = params[0].u.value.c;
105 msg.buf = params[2].u.memref.shm->kaddr;
106 msg.len = params[2].u.memref.size;
107
108 switch (params[0].u.value.a) {
109 case OPTEE_RPC_I2C_TRANSFER_RD:
110 msg.flags |= I2C_M_RD;
111 break;
112 case OPTEE_RPC_I2C_TRANSFER_WR:
113 break;
114 default:
115 i2c_put_adapter(adapter);
116 goto bad;
117 }
118
119 ret = i2c_transfer(adapter, &msg, 1);
120
121 if (ret < 0) {
122 arg->ret = TEEC_ERROR_COMMUNICATION;
123 } else {
124 params[3].u.value.a = msg.len;
125 if (optee_to_msg_param(arg->params, arg->num_params, params))
126 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
127 else
128 arg->ret = TEEC_SUCCESS;
129 }
130
131 i2c_put_adapter(adapter);
132 kfree(params);
133 return;
134bad:
135 kfree(params);
136 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
137}
138#else
139static void handle_rpc_func_cmd_i2c_transfer(struct tee_context *ctx,
140 struct optee_msg_arg *arg)
141{
142 arg->ret = TEEC_ERROR_NOT_SUPPORTED;
143}
144#endif
145
146static struct wq_entry *wq_entry_get(struct optee_wait_queue *wq, u32 key)
147{
148 struct wq_entry *w;
149
150 mutex_lock(&wq->mu);
151
152 list_for_each_entry(w, &wq->db, link)
153 if (w->key == key)
154 goto out;
155
156 w = kmalloc(sizeof(*w), GFP_KERNEL);
157 if (w) {
158 init_completion(&w->c);
159 w->key = key;
160 list_add_tail(&w->link, &wq->db);
161 }
162out:
163 mutex_unlock(&wq->mu);
164 return w;
165}
166
167static void wq_sleep(struct optee_wait_queue *wq, u32 key)
168{
169 struct wq_entry *w = wq_entry_get(wq, key);
170
171 if (w) {
172 wait_for_completion(&w->c);
173 mutex_lock(&wq->mu);
174 list_del(&w->link);
175 mutex_unlock(&wq->mu);
176 kfree(w);
177 }
178}
179
180static void wq_wakeup(struct optee_wait_queue *wq, u32 key)
181{
182 struct wq_entry *w = wq_entry_get(wq, key);
183
184 if (w)
185 complete(&w->c);
186}
187
188static void handle_rpc_func_cmd_wq(struct optee *optee,
189 struct optee_msg_arg *arg)
190{
191 if (arg->num_params != 1)
192 goto bad;
193
194 if ((arg->params[0].attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
195 OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
196 goto bad;
197
198 switch (arg->params[0].u.value.a) {
199 case OPTEE_RPC_WAIT_QUEUE_SLEEP:
200 wq_sleep(&optee->wait_queue, arg->params[0].u.value.b);
201 break;
202 case OPTEE_RPC_WAIT_QUEUE_WAKEUP:
203 wq_wakeup(&optee->wait_queue, arg->params[0].u.value.b);
204 break;
205 default:
206 goto bad;
207 }
208
209 arg->ret = TEEC_SUCCESS;
210 return;
211bad:
212 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
213}
214
215static void handle_rpc_func_cmd_wait(struct optee_msg_arg *arg)
216{
217 u32 msec_to_wait;
218
219 if (arg->num_params != 1)
220 goto bad;
221
222 if ((arg->params[0].attr & OPTEE_MSG_ATTR_TYPE_MASK) !=
223 OPTEE_MSG_ATTR_TYPE_VALUE_INPUT)
224 goto bad;
225
226 msec_to_wait = arg->params[0].u.value.a;
227
228
229 msleep_interruptible(msec_to_wait);
230
231 arg->ret = TEEC_SUCCESS;
232 return;
233bad:
234 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
235}
236
237static void handle_rpc_supp_cmd(struct tee_context *ctx,
238 struct optee_msg_arg *arg)
239{
240 struct tee_param *params;
241
242 arg->ret_origin = TEEC_ORIGIN_COMMS;
243
244 params = kmalloc_array(arg->num_params, sizeof(struct tee_param),
245 GFP_KERNEL);
246 if (!params) {
247 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
248 return;
249 }
250
251 if (optee_from_msg_param(params, arg->num_params, arg->params)) {
252 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
253 goto out;
254 }
255
256 arg->ret = optee_supp_thrd_req(ctx, arg->cmd, arg->num_params, params);
257
258 if (optee_to_msg_param(arg->params, arg->num_params, params))
259 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
260out:
261 kfree(params);
262}
263
264static struct tee_shm *cmd_alloc_suppl(struct tee_context *ctx, size_t sz)
265{
266 u32 ret;
267 struct tee_param param;
268 struct optee *optee = tee_get_drvdata(ctx->teedev);
269 struct tee_shm *shm;
270
271 param.attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
272 param.u.value.a = OPTEE_RPC_SHM_TYPE_APPL;
273 param.u.value.b = sz;
274 param.u.value.c = 0;
275
276 ret = optee_supp_thrd_req(ctx, OPTEE_RPC_CMD_SHM_ALLOC, 1, ¶m);
277 if (ret)
278 return ERR_PTR(-ENOMEM);
279
280 mutex_lock(&optee->supp.mutex);
281
282 shm = tee_shm_get_from_id(optee->supp.ctx, param.u.value.c);
283 mutex_unlock(&optee->supp.mutex);
284 return shm;
285}
286
287static void handle_rpc_func_cmd_shm_alloc(struct tee_context *ctx,
288 struct optee_msg_arg *arg,
289 struct optee_call_ctx *call_ctx)
290{
291 phys_addr_t pa;
292 struct tee_shm *shm;
293 size_t sz;
294 size_t n;
295
296 arg->ret_origin = TEEC_ORIGIN_COMMS;
297
298 if (!arg->num_params ||
299 arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
300 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
301 return;
302 }
303
304 for (n = 1; n < arg->num_params; n++) {
305 if (arg->params[n].attr != OPTEE_MSG_ATTR_TYPE_NONE) {
306 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
307 return;
308 }
309 }
310
311 sz = arg->params[0].u.value.b;
312 switch (arg->params[0].u.value.a) {
313 case OPTEE_RPC_SHM_TYPE_APPL:
314 shm = cmd_alloc_suppl(ctx, sz);
315 break;
316 case OPTEE_RPC_SHM_TYPE_KERNEL:
317 shm = tee_shm_alloc(ctx, sz, TEE_SHM_MAPPED | TEE_SHM_PRIV);
318 break;
319 default:
320 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
321 return;
322 }
323
324 if (IS_ERR(shm)) {
325 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
326 return;
327 }
328
329 if (tee_shm_get_pa(shm, 0, &pa)) {
330 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
331 goto bad;
332 }
333
334 sz = tee_shm_get_size(shm);
335
336 if (tee_shm_is_registered(shm)) {
337 struct page **pages;
338 u64 *pages_list;
339 size_t page_num;
340
341 pages = tee_shm_get_pages(shm, &page_num);
342 if (!pages || !page_num) {
343 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
344 goto bad;
345 }
346
347 pages_list = optee_allocate_pages_list(page_num);
348 if (!pages_list) {
349 arg->ret = TEEC_ERROR_OUT_OF_MEMORY;
350 goto bad;
351 }
352
353 call_ctx->pages_list = pages_list;
354 call_ctx->num_entries = page_num;
355
356 arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT |
357 OPTEE_MSG_ATTR_NONCONTIG;
358
359
360
361
362 arg->params[0].u.tmem.buf_ptr = virt_to_phys(pages_list) |
363 (tee_shm_get_page_offset(shm) &
364 (OPTEE_MSG_NONCONTIG_PAGE_SIZE - 1));
365 arg->params[0].u.tmem.size = tee_shm_get_size(shm);
366 arg->params[0].u.tmem.shm_ref = (unsigned long)shm;
367
368 optee_fill_pages_list(pages_list, pages, page_num,
369 tee_shm_get_page_offset(shm));
370 } else {
371 arg->params[0].attr = OPTEE_MSG_ATTR_TYPE_TMEM_OUTPUT;
372 arg->params[0].u.tmem.buf_ptr = pa;
373 arg->params[0].u.tmem.size = sz;
374 arg->params[0].u.tmem.shm_ref = (unsigned long)shm;
375 }
376
377 arg->ret = TEEC_SUCCESS;
378 return;
379bad:
380 tee_shm_free(shm);
381}
382
383static void cmd_free_suppl(struct tee_context *ctx, struct tee_shm *shm)
384{
385 struct tee_param param;
386
387 param.attr = TEE_IOCTL_PARAM_ATTR_TYPE_VALUE_INOUT;
388 param.u.value.a = OPTEE_RPC_SHM_TYPE_APPL;
389 param.u.value.b = tee_shm_get_id(shm);
390 param.u.value.c = 0;
391
392
393
394
395
396
397
398
399
400
401
402
403 tee_shm_put(shm);
404
405 optee_supp_thrd_req(ctx, OPTEE_RPC_CMD_SHM_FREE, 1, ¶m);
406}
407
408static void handle_rpc_func_cmd_shm_free(struct tee_context *ctx,
409 struct optee_msg_arg *arg)
410{
411 struct tee_shm *shm;
412
413 arg->ret_origin = TEEC_ORIGIN_COMMS;
414
415 if (arg->num_params != 1 ||
416 arg->params[0].attr != OPTEE_MSG_ATTR_TYPE_VALUE_INPUT) {
417 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
418 return;
419 }
420
421 shm = (struct tee_shm *)(unsigned long)arg->params[0].u.value.b;
422 switch (arg->params[0].u.value.a) {
423 case OPTEE_RPC_SHM_TYPE_APPL:
424 cmd_free_suppl(ctx, shm);
425 break;
426 case OPTEE_RPC_SHM_TYPE_KERNEL:
427 tee_shm_free(shm);
428 break;
429 default:
430 arg->ret = TEEC_ERROR_BAD_PARAMETERS;
431 }
432 arg->ret = TEEC_SUCCESS;
433}
434
435static void free_pages_list(struct optee_call_ctx *call_ctx)
436{
437 if (call_ctx->pages_list) {
438 optee_free_pages_list(call_ctx->pages_list,
439 call_ctx->num_entries);
440 call_ctx->pages_list = NULL;
441 call_ctx->num_entries = 0;
442 }
443}
444
445void optee_rpc_finalize_call(struct optee_call_ctx *call_ctx)
446{
447 free_pages_list(call_ctx);
448}
449
450static void handle_rpc_func_cmd(struct tee_context *ctx, struct optee *optee,
451 struct tee_shm *shm,
452 struct optee_call_ctx *call_ctx)
453{
454 struct optee_msg_arg *arg;
455
456 arg = tee_shm_get_va(shm, 0);
457 if (IS_ERR(arg)) {
458 pr_err("%s: tee_shm_get_va %p failed\n", __func__, shm);
459 return;
460 }
461
462 switch (arg->cmd) {
463 case OPTEE_RPC_CMD_GET_TIME:
464 handle_rpc_func_cmd_get_time(arg);
465 break;
466 case OPTEE_RPC_CMD_WAIT_QUEUE:
467 handle_rpc_func_cmd_wq(optee, arg);
468 break;
469 case OPTEE_RPC_CMD_SUSPEND:
470 handle_rpc_func_cmd_wait(arg);
471 break;
472 case OPTEE_RPC_CMD_SHM_ALLOC:
473 free_pages_list(call_ctx);
474 handle_rpc_func_cmd_shm_alloc(ctx, arg, call_ctx);
475 break;
476 case OPTEE_RPC_CMD_SHM_FREE:
477 handle_rpc_func_cmd_shm_free(ctx, arg);
478 break;
479 case OPTEE_RPC_CMD_I2C_TRANSFER:
480 handle_rpc_func_cmd_i2c_transfer(ctx, arg);
481 break;
482 default:
483 handle_rpc_supp_cmd(ctx, arg);
484 }
485}
486
487
488
489
490
491
492
493
494
495void optee_handle_rpc(struct tee_context *ctx, struct optee_rpc_param *param,
496 struct optee_call_ctx *call_ctx)
497{
498 struct tee_device *teedev = ctx->teedev;
499 struct optee *optee = tee_get_drvdata(teedev);
500 struct tee_shm *shm;
501 phys_addr_t pa;
502
503 switch (OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0)) {
504 case OPTEE_SMC_RPC_FUNC_ALLOC:
505 shm = tee_shm_alloc(ctx, param->a1,
506 TEE_SHM_MAPPED | TEE_SHM_PRIV);
507 if (!IS_ERR(shm) && !tee_shm_get_pa(shm, 0, &pa)) {
508 reg_pair_from_64(¶m->a1, ¶m->a2, pa);
509 reg_pair_from_64(¶m->a4, ¶m->a5,
510 (unsigned long)shm);
511 } else {
512 param->a1 = 0;
513 param->a2 = 0;
514 param->a4 = 0;
515 param->a5 = 0;
516 }
517 break;
518 case OPTEE_SMC_RPC_FUNC_FREE:
519 shm = reg_pair_to_ptr(param->a1, param->a2);
520 tee_shm_free(shm);
521 break;
522 case OPTEE_SMC_RPC_FUNC_FOREIGN_INTR:
523
524
525
526
527
528
529 break;
530 case OPTEE_SMC_RPC_FUNC_CMD:
531 shm = reg_pair_to_ptr(param->a1, param->a2);
532 handle_rpc_func_cmd(ctx, optee, shm, call_ctx);
533 break;
534 default:
535 pr_warn("Unknown RPC func 0x%x\n",
536 (u32)OPTEE_SMC_RETURN_GET_RPC_FUNC(param->a0));
537 break;
538 }
539
540 param->a0 = OPTEE_SMC_CALL_RETURN_FROM_RPC;
541}
542
