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