Branch data Line data Source code
1 : : // SPDX-License-Identifier: GPL-2.0
2 : : /*
3 : : * VideoCore Shared Memory CMA allocator
4 : : *
5 : : * Copyright: 2018, Raspberry Pi (Trading) Ltd
6 : : * Copyright 2011-2012 Broadcom Corporation. All rights reserved.
7 : : *
8 : : * Based on vmcs_sm driver from Broadcom Corporation.
9 : : *
10 : : */
11 : :
12 : : /* ---- Include Files ----------------------------------------------------- */
13 : : #include <linux/completion.h>
14 : : #include <linux/kernel.h>
15 : : #include <linux/kthread.h>
16 : : #include <linux/list.h>
17 : : #include <linux/mutex.h>
18 : : #include <linux/semaphore.h>
19 : : #include <linux/slab.h>
20 : : #include <linux/types.h>
21 : :
22 : : #include "vc_sm_cma_vchi.h"
23 : :
24 : : #define VC_SM_VER 1
25 : : #define VC_SM_MIN_VER 0
26 : :
27 : : /* ---- Private Constants and Types -------------------------------------- */
28 : :
29 : : /* Command blocks come from a pool */
30 : : #define SM_MAX_NUM_CMD_RSP_BLKS 32
31 : :
32 : : struct sm_cmd_rsp_blk {
33 : : struct list_head head; /* To create lists */
34 : : /* To be signaled when the response is there */
35 : : struct completion cmplt;
36 : :
37 : : u32 id;
38 : : u16 length;
39 : :
40 : : u8 msg[VC_SM_MAX_MSG_LEN];
41 : :
42 : : uint32_t wait:1;
43 : : uint32_t sent:1;
44 : : uint32_t alloc:1;
45 : :
46 : : };
47 : :
48 : : struct sm_instance {
49 : : u32 num_connections;
50 : : VCHI_SERVICE_HANDLE_T vchi_handle[VCHI_MAX_NUM_CONNECTIONS];
51 : : struct task_struct *io_thread;
52 : : struct completion io_cmplt;
53 : :
54 : : vpu_event_cb vpu_event;
55 : :
56 : : /* Mutex over the following lists */
57 : : struct mutex lock;
58 : : u32 trans_id;
59 : : struct list_head cmd_list;
60 : : struct list_head rsp_list;
61 : : struct list_head dead_list;
62 : :
63 : : struct sm_cmd_rsp_blk free_blk[SM_MAX_NUM_CMD_RSP_BLKS];
64 : :
65 : : /* Mutex over the free_list */
66 : : struct mutex free_lock;
67 : : struct list_head free_list;
68 : :
69 : : struct semaphore free_sema;
70 : :
71 : : };
72 : :
73 : : /* ---- Private Variables ------------------------------------------------ */
74 : :
75 : : /* ---- Private Function Prototypes -------------------------------------- */
76 : :
77 : : /* ---- Private Functions ------------------------------------------------ */
78 : : static int
79 : : bcm2835_vchi_msg_queue(VCHI_SERVICE_HANDLE_T handle,
80 : : void *data,
81 : : unsigned int size)
82 : : {
83 : 207 : return vchi_queue_kernel_message(handle,
84 : : data,
85 : : size);
86 : : }
87 : :
88 : : static struct
89 : 207 : sm_cmd_rsp_blk *vc_vchi_cmd_create(struct sm_instance *instance,
90 : : enum vc_sm_msg_type id, void *msg,
91 : : u32 size, int wait)
92 : : {
93 : : struct sm_cmd_rsp_blk *blk;
94 : : struct vc_sm_msg_hdr_t *hdr;
95 : :
96 [ - + ]: 207 : if (down_interruptible(&instance->free_sema)) {
97 : : blk = kmalloc(sizeof(*blk), GFP_KERNEL);
98 [ # # ]: 0 : if (!blk)
99 : : return NULL;
100 : :
101 : 0 : blk->alloc = 1;
102 : : init_completion(&blk->cmplt);
103 : : } else {
104 : 207 : mutex_lock(&instance->free_lock);
105 : : blk =
106 : 207 : list_first_entry(&instance->free_list,
107 : : struct sm_cmd_rsp_blk, head);
108 : : list_del(&blk->head);
109 : 207 : mutex_unlock(&instance->free_lock);
110 : : }
111 : :
112 : 207 : blk->sent = 0;
113 : 207 : blk->wait = wait;
114 : 207 : blk->length = sizeof(*hdr) + size;
115 : :
116 : : hdr = (struct vc_sm_msg_hdr_t *)blk->msg;
117 : 207 : hdr->type = id;
118 : 207 : mutex_lock(&instance->lock);
119 : 207 : instance->trans_id++;
120 : : /*
121 : : * Retain the top bit for identifying asynchronous events, or VPU cmds.
122 : : */
123 : 207 : instance->trans_id &= ~0x80000000;
124 : 207 : hdr->trans_id = instance->trans_id;
125 : 207 : blk->id = instance->trans_id;
126 : 207 : mutex_unlock(&instance->lock);
127 : :
128 [ + - ]: 207 : if (size)
129 : 207 : memcpy(hdr->body, msg, size);
130 : :
131 : 207 : return blk;
132 : : }
133 : :
134 : : static void
135 : 207 : vc_vchi_cmd_delete(struct sm_instance *instance, struct sm_cmd_rsp_blk *blk)
136 : : {
137 [ - + ]: 207 : if (blk->alloc) {
138 : 0 : kfree(blk);
139 : 207 : return;
140 : : }
141 : :
142 : 207 : mutex_lock(&instance->free_lock);
143 : 207 : list_add(&blk->head, &instance->free_list);
144 : 207 : mutex_unlock(&instance->free_lock);
145 : 207 : up(&instance->free_sema);
146 : : }
147 : :
148 : 0 : static void vc_sm_cma_vchi_rx_ack(struct sm_instance *instance,
149 : : struct sm_cmd_rsp_blk *cmd,
150 : : struct vc_sm_result_t *reply,
151 : : u32 reply_len)
152 : : {
153 : 0 : mutex_lock(&instance->lock);
154 [ # # ]: 0 : list_for_each_entry(cmd,
155 : : &instance->rsp_list,
156 : : head) {
157 [ # # ]: 0 : if (cmd->id == reply->trans_id)
158 : : break;
159 : : }
160 : 0 : mutex_unlock(&instance->lock);
161 : :
162 [ # # ]: 0 : if (&cmd->head == &instance->rsp_list) {
163 : : //pr_debug("%s: received response %u, throw away...",
164 : 0 : pr_err("%s: received response %u, throw away...",
165 : : __func__,
166 : : reply->trans_id);
167 [ # # ]: 0 : } else if (reply_len > sizeof(cmd->msg)) {
168 : 0 : pr_err("%s: reply too big (%u) %u, throw away...",
169 : : __func__, reply_len,
170 : : reply->trans_id);
171 : : } else {
172 : 0 : memcpy(cmd->msg, reply,
173 : : reply_len);
174 : 0 : complete(&cmd->cmplt);
175 : : }
176 : 0 : }
177 : :
178 : 207 : static int vc_sm_cma_vchi_videocore_io(void *arg)
179 : : {
180 : : struct sm_instance *instance = arg;
181 : : struct sm_cmd_rsp_blk *cmd = NULL, *cmd_tmp;
182 : : struct vc_sm_result_t *reply;
183 : : u32 reply_len;
184 : : s32 status;
185 : : int svc_use = 1;
186 : :
187 : : while (1) {
188 [ + - ]: 621 : if (svc_use)
189 : 621 : vchi_service_release(instance->vchi_handle[0]);
190 : : svc_use = 0;
191 : :
192 [ - + ]: 621 : if (wait_for_completion_interruptible(&instance->io_cmplt))
193 : 0 : continue;
194 : :
195 : 414 : vchi_service_use(instance->vchi_handle[0]);
196 : : svc_use = 1;
197 : :
198 : : do {
199 : : /*
200 : : * Get new command and move it to response list
201 : : */
202 : 621 : mutex_lock(&instance->lock);
203 [ + + ]: 1242 : if (list_empty(&instance->cmd_list)) {
204 : : /* no more commands to process */
205 : 414 : mutex_unlock(&instance->lock);
206 : : break;
207 : : }
208 : 207 : cmd = list_first_entry(&instance->cmd_list,
209 : : struct sm_cmd_rsp_blk, head);
210 : 207 : list_move(&cmd->head, &instance->rsp_list);
211 : 207 : cmd->sent = 1;
212 : 207 : mutex_unlock(&instance->lock);
213 : :
214 : : /* Send the command */
215 : : status =
216 : 414 : bcm2835_vchi_msg_queue(instance->vchi_handle[0],
217 : 207 : cmd->msg, cmd->length);
218 [ - + ]: 207 : if (status) {
219 : 0 : pr_err("%s: failed to queue message (%d)",
220 : : __func__, status);
221 : : }
222 : :
223 : : /* If no reply is needed then we're done */
224 [ + - ]: 207 : if (!cmd->wait) {
225 : 207 : mutex_lock(&instance->lock);
226 : : list_del(&cmd->head);
227 : 207 : mutex_unlock(&instance->lock);
228 : 207 : vc_vchi_cmd_delete(instance, cmd);
229 : 207 : continue;
230 : : }
231 : :
232 [ # # ]: 0 : if (status) {
233 : 0 : complete(&cmd->cmplt);
234 : 0 : continue;
235 : : }
236 : :
237 : : } while (1);
238 : :
239 [ + + ]: 1035 : while (!vchi_msg_peek(instance->vchi_handle[0], (void **)&reply,
240 : : &reply_len, VCHI_FLAGS_NONE)) {
241 [ + - ]: 207 : if (reply->trans_id & 0x80000000) {
242 : : /* Async event or cmd from the VPU */
243 [ + - ]: 207 : if (instance->vpu_event)
244 : 207 : instance->vpu_event(instance, reply,
245 : : reply_len);
246 : : } else {
247 : 0 : vc_sm_cma_vchi_rx_ack(instance, cmd, reply,
248 : : reply_len);
249 : : }
250 : :
251 : 207 : vchi_msg_remove(instance->vchi_handle[0]);
252 : : }
253 : :
254 : : /* Go through the dead list and free them */
255 : 414 : mutex_lock(&instance->lock);
256 [ - + ]: 414 : list_for_each_entry_safe(cmd, cmd_tmp, &instance->dead_list,
257 : : head) {
258 : : list_del(&cmd->head);
259 : 0 : vc_vchi_cmd_delete(instance, cmd);
260 : : }
261 : 414 : mutex_unlock(&instance->lock);
262 : : }
263 : :
264 : : return 0;
265 : : }
266 : :
267 : 207 : static void vc_sm_cma_vchi_callback(void *param,
268 : : const VCHI_CALLBACK_REASON_T reason,
269 : : void *msg_handle)
270 : : {
271 : : struct sm_instance *instance = param;
272 : :
273 : : (void)msg_handle;
274 : :
275 [ + - - ]: 207 : switch (reason) {
276 : : case VCHI_CALLBACK_MSG_AVAILABLE:
277 : 207 : complete(&instance->io_cmplt);
278 : 207 : break;
279 : :
280 : : case VCHI_CALLBACK_SERVICE_CLOSED:
281 : 0 : pr_info("%s: service CLOSED!!", __func__);
282 : : default:
283 : : break;
284 : : }
285 : 207 : }
286 : :
287 : 207 : struct sm_instance *vc_sm_cma_vchi_init(VCHI_INSTANCE_T vchi_instance,
288 : : unsigned int num_connections,
289 : : vpu_event_cb vpu_event)
290 : : {
291 : : u32 i;
292 : : struct sm_instance *instance;
293 : : int status;
294 : :
295 : : pr_debug("%s: start", __func__);
296 : :
297 [ - + ]: 207 : if (num_connections > VCHI_MAX_NUM_CONNECTIONS) {
298 : 0 : pr_err("%s: unsupported number of connections %u (max=%u)",
299 : : __func__, num_connections, VCHI_MAX_NUM_CONNECTIONS);
300 : :
301 : 0 : goto err_null;
302 : : }
303 : : /* Allocate memory for this instance */
304 : 207 : instance = kzalloc(sizeof(*instance), GFP_KERNEL);
305 : :
306 : : /* Misc initialisations */
307 : 207 : mutex_init(&instance->lock);
308 : : init_completion(&instance->io_cmplt);
309 : 207 : INIT_LIST_HEAD(&instance->cmd_list);
310 : 207 : INIT_LIST_HEAD(&instance->rsp_list);
311 : 207 : INIT_LIST_HEAD(&instance->dead_list);
312 : 207 : INIT_LIST_HEAD(&instance->free_list);
313 : : sema_init(&instance->free_sema, SM_MAX_NUM_CMD_RSP_BLKS);
314 : 207 : mutex_init(&instance->free_lock);
315 [ + + ]: 6831 : for (i = 0; i < SM_MAX_NUM_CMD_RSP_BLKS; i++) {
316 : : init_completion(&instance->free_blk[i].cmplt);
317 : 6624 : list_add(&instance->free_blk[i].head, &instance->free_list);
318 : : }
319 : :
320 : : /* Open the VCHI service connections */
321 : 207 : instance->num_connections = num_connections;
322 [ + + ]: 414 : for (i = 0; i < num_connections; i++) {
323 : 207 : struct service_creation params = {
324 : : .version = VCHI_VERSION_EX(VC_SM_VER, VC_SM_MIN_VER),
325 : : .service_id = VC_SM_SERVER_NAME,
326 : : .callback = vc_sm_cma_vchi_callback,
327 : : .callback_param = instance,
328 : : };
329 : :
330 : 207 : status = vchi_service_open(vchi_instance,
331 : : ¶ms, &instance->vchi_handle[i]);
332 [ - + ]: 207 : if (status) {
333 : 0 : pr_err("%s: failed to open VCHI service (%d)",
334 : : __func__, status);
335 : :
336 : 0 : goto err_close_services;
337 : : }
338 : : }
339 : :
340 : : /* Create the thread which takes care of all io to/from videoocore. */
341 : 207 : instance->io_thread = kthread_create(&vc_sm_cma_vchi_videocore_io,
342 : : (void *)instance, "SMIO");
343 [ - + ]: 207 : if (!instance->io_thread) {
344 : 0 : pr_err("%s: failed to create SMIO thread", __func__);
345 : :
346 : 0 : goto err_close_services;
347 : : }
348 : 207 : instance->vpu_event = vpu_event;
349 : 207 : set_user_nice(instance->io_thread, -10);
350 : 207 : wake_up_process(instance->io_thread);
351 : :
352 : : pr_debug("%s: success - instance %p", __func__, instance);
353 : 207 : return instance;
354 : :
355 : : err_close_services:
356 [ # # ]: 0 : for (i = 0; i < instance->num_connections; i++) {
357 [ # # ]: 0 : if (instance->vchi_handle[i])
358 : 0 : vchi_service_close(instance->vchi_handle[i]);
359 : : }
360 : 0 : kfree(instance);
361 : : err_null:
362 : : pr_debug("%s: FAILED", __func__);
363 : : return NULL;
364 : : }
365 : :
366 : 0 : int vc_sm_cma_vchi_stop(struct sm_instance **handle)
367 : : {
368 : : struct sm_instance *instance;
369 : : u32 i;
370 : :
371 [ # # ]: 0 : if (!handle) {
372 : 0 : pr_err("%s: invalid pointer to handle %p", __func__, handle);
373 : 0 : goto lock;
374 : : }
375 : :
376 [ # # ]: 0 : if (!*handle) {
377 : 0 : pr_err("%s: invalid handle %p", __func__, *handle);
378 : 0 : goto lock;
379 : : }
380 : :
381 : : instance = *handle;
382 : :
383 : : /* Close all VCHI service connections */
384 [ # # ]: 0 : for (i = 0; i < instance->num_connections; i++) {
385 : : s32 success;
386 : :
387 : 0 : vchi_service_use(instance->vchi_handle[i]);
388 : :
389 : 0 : success = vchi_service_close(instance->vchi_handle[i]);
390 : : }
391 : :
392 : 0 : kfree(instance);
393 : :
394 : 0 : *handle = NULL;
395 : 0 : return 0;
396 : :
397 : : lock:
398 : : return -EINVAL;
399 : : }
400 : :
401 : 207 : static int vc_sm_cma_vchi_send_msg(struct sm_instance *handle,
402 : : enum vc_sm_msg_type msg_id, void *msg,
403 : : u32 msg_size, void *result, u32 result_size,
404 : : u32 *cur_trans_id, u8 wait_reply)
405 : : {
406 : : int status = 0;
407 : : struct sm_instance *instance = handle;
408 : : struct sm_cmd_rsp_blk *cmd_blk;
409 : :
410 [ - + ]: 207 : if (!handle) {
411 : 0 : pr_err("%s: invalid handle", __func__);
412 : 0 : return -EINVAL;
413 : : }
414 [ - + ]: 207 : if (!msg) {
415 : 0 : pr_err("%s: invalid msg pointer", __func__);
416 : 0 : return -EINVAL;
417 : : }
418 : :
419 : 207 : cmd_blk =
420 : 207 : vc_vchi_cmd_create(instance, msg_id, msg, msg_size, wait_reply);
421 [ - + ]: 207 : if (!cmd_blk) {
422 : 0 : pr_err("[%s]: failed to allocate global tracking resource",
423 : : __func__);
424 : 0 : return -ENOMEM;
425 : : }
426 : :
427 [ + - ]: 207 : if (cur_trans_id)
428 : 207 : *cur_trans_id = cmd_blk->id;
429 : :
430 : 207 : mutex_lock(&instance->lock);
431 : 207 : list_add_tail(&cmd_blk->head, &instance->cmd_list);
432 : 207 : mutex_unlock(&instance->lock);
433 : 207 : complete(&instance->io_cmplt);
434 : :
435 [ - + ]: 207 : if (!wait_reply)
436 : : /* We're done */
437 : : return 0;
438 : :
439 : : /* Wait for the response */
440 [ # # ]: 0 : if (wait_for_completion_interruptible(&cmd_blk->cmplt)) {
441 : 0 : mutex_lock(&instance->lock);
442 [ # # ]: 0 : if (!cmd_blk->sent) {
443 : : list_del(&cmd_blk->head);
444 : 0 : mutex_unlock(&instance->lock);
445 : 0 : vc_vchi_cmd_delete(instance, cmd_blk);
446 : 0 : return -ENXIO;
447 : : }
448 : :
449 : 0 : list_move(&cmd_blk->head, &instance->dead_list);
450 : 0 : mutex_unlock(&instance->lock);
451 : 0 : complete(&instance->io_cmplt);
452 : 0 : return -EINTR; /* We're done */
453 : : }
454 : :
455 [ # # ]: 0 : if (result && result_size) {
456 : 0 : memcpy(result, cmd_blk->msg, result_size);
457 : : } else {
458 : : struct vc_sm_result_t *res =
459 : : (struct vc_sm_result_t *)cmd_blk->msg;
460 [ # # ]: 0 : status = (res->success == 0) ? 0 : -ENXIO;
461 : : }
462 : :
463 : 0 : mutex_lock(&instance->lock);
464 : : list_del(&cmd_blk->head);
465 : 0 : mutex_unlock(&instance->lock);
466 : 0 : vc_vchi_cmd_delete(instance, cmd_blk);
467 : 0 : return status;
468 : : }
469 : :
470 : 0 : int vc_sm_cma_vchi_free(struct sm_instance *handle, struct vc_sm_free_t *msg,
471 : : u32 *cur_trans_id)
472 : : {
473 : 0 : return vc_sm_cma_vchi_send_msg(handle, VC_SM_MSG_TYPE_FREE,
474 : : msg, sizeof(*msg), 0, 0, cur_trans_id, 0);
475 : : }
476 : :
477 : 0 : int vc_sm_cma_vchi_import(struct sm_instance *handle, struct vc_sm_import *msg,
478 : : struct vc_sm_import_result *result, u32 *cur_trans_id)
479 : : {
480 : 0 : return vc_sm_cma_vchi_send_msg(handle, VC_SM_MSG_TYPE_IMPORT,
481 : : msg, sizeof(*msg), result, sizeof(*result),
482 : : cur_trans_id, 1);
483 : : }
484 : :
485 : 207 : int vc_sm_cma_vchi_client_version(struct sm_instance *handle,
486 : : struct vc_sm_version *msg,
487 : : struct vc_sm_result_t *result,
488 : : u32 *cur_trans_id)
489 : : {
490 : 207 : return vc_sm_cma_vchi_send_msg(handle, VC_SM_MSG_TYPE_CLIENT_VERSION,
491 : : //msg, sizeof(*msg), result, sizeof(*result),
492 : : //cur_trans_id, 1);
493 : : msg, sizeof(*msg), NULL, 0,
494 : : cur_trans_id, 0);
495 : : }
496 : :
497 : 0 : int vc_sm_vchi_client_vc_mem_req_reply(struct sm_instance *handle,
498 : : struct vc_sm_vc_mem_request_result *msg,
499 : : uint32_t *cur_trans_id)
500 : : {
501 : 0 : return vc_sm_cma_vchi_send_msg(handle,
502 : : VC_SM_MSG_TYPE_VC_MEM_REQUEST_REPLY,
503 : : msg, sizeof(*msg), 0, 0, cur_trans_id,
504 : : 0);
505 : : }
|
