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