1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Broadcom BCM2835 V4L2 driver
4  *
5  * Copyright © 2013 Raspberry Pi (Trading) Ltd.
6  *
7  * Authors: Vincent Sanders @ Collabora
8  *          Dave Stevenson @ Broadcom
9  *		(now dave.stevenson@raspberrypi.org)
10  *          Simon Mellor @ Broadcom
11  *          Luke Diamand @ Broadcom
12  *
13  * V4L2 driver MMAL vchiq interface code
14  */
15 
16 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
17 
18 #include <linux/errno.h>
19 #include <linux/kernel.h>
20 #include <linux/module.h>
21 #include <linux/mutex.h>
22 #include <linux/mm.h>
23 #include <linux/slab.h>
24 #include <linux/completion.h>
25 #include <linux/vmalloc.h>
26 #include <media/videobuf2-vmalloc.h>
27 
28 #include "../include/linux/raspberrypi/vchiq.h"
29 #include "mmal-common.h"
30 #include "mmal-vchiq.h"
31 #include "mmal-msg.h"
32 
33 /*
34  * maximum number of components supported.
35  * This matches the maximum permitted by default on the VPU
36  */
37 #define VCHIQ_MMAL_MAX_COMPONENTS 64
38 
39 /*
40  * Timeout for synchronous msg responses in seconds.
41  * Helpful to increase this if stopping in the VPU debugger.
42  */
43 #define SYNC_MSG_TIMEOUT       3
44 
45 /*#define FULL_MSG_DUMP 1*/
46 
47 #ifdef DEBUG
48 static const char *const msg_type_names[] = {
49 	"UNKNOWN",
50 	"QUIT",
51 	"SERVICE_CLOSED",
52 	"GET_VERSION",
53 	"COMPONENT_CREATE",
54 	"COMPONENT_DESTROY",
55 	"COMPONENT_ENABLE",
56 	"COMPONENT_DISABLE",
57 	"PORT_INFO_GET",
58 	"PORT_INFO_SET",
59 	"PORT_ACTION",
60 	"BUFFER_FROM_HOST",
61 	"BUFFER_TO_HOST",
62 	"GET_STATS",
63 	"PORT_PARAMETER_SET",
64 	"PORT_PARAMETER_GET",
65 	"EVENT_TO_HOST",
66 	"GET_CORE_STATS_FOR_PORT",
67 	"OPAQUE_ALLOCATOR",
68 	"CONSUME_MEM",
69 	"LMK",
70 	"OPAQUE_ALLOCATOR_DESC",
71 	"DRM_GET_LHS32",
72 	"DRM_GET_TIME",
73 	"BUFFER_FROM_HOST_ZEROLEN",
74 	"PORT_FLUSH",
75 	"HOST_LOG",
76 };
77 #endif
78 
79 static const char *const port_action_type_names[] = {
80 	"UNKNOWN",
81 	"ENABLE",
82 	"DISABLE",
83 	"FLUSH",
84 	"CONNECT",
85 	"DISCONNECT",
86 	"SET_REQUIREMENTS",
87 };
88 
89 #if defined(DEBUG)
90 #if defined(FULL_MSG_DUMP)
91 #define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)				\
92 	do {								\
93 		pr_debug(TITLE" type:%s(%d) length:%d\n",		\
94 			 msg_type_names[(MSG)->h.type],			\
95 			 (MSG)->h.type, (MSG_LEN));			\
96 		print_hex_dump(KERN_DEBUG, "<<h: ", DUMP_PREFIX_OFFSET,	\
97 			       16, 4, (MSG),				\
98 			       sizeof(struct mmal_msg_header), 1);	\
99 		print_hex_dump(KERN_DEBUG, "<<p: ", DUMP_PREFIX_OFFSET,	\
100 			       16, 4,					\
101 			       ((u8 *)(MSG)) + sizeof(struct mmal_msg_header),\
102 			       (MSG_LEN) - sizeof(struct mmal_msg_header), 1); \
103 	} while (0)
104 #else
105 #define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)				\
106 	{								\
107 		pr_debug(TITLE" type:%s(%d) length:%d\n",		\
108 			 msg_type_names[(MSG)->h.type],			\
109 			 (MSG)->h.type, (MSG_LEN));			\
110 	}
111 #endif
112 #else
113 #define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)
114 #endif
115 
116 struct vchiq_mmal_instance;
117 
118 /* normal message context */
119 struct mmal_msg_context {
120 	struct vchiq_mmal_instance *instance;
121 
122 	/* Index in the context_map idr so that we can find the
123 	 * mmal_msg_context again when servicing the VCHI reply.
124 	 */
125 	int handle;
126 
127 	union {
128 		struct {
129 			/* work struct for buffer_cb callback */
130 			struct work_struct work;
131 			/* work struct for deferred callback */
132 			struct work_struct buffer_to_host_work;
133 			/* mmal instance */
134 			struct vchiq_mmal_instance *instance;
135 			/* mmal port */
136 			struct vchiq_mmal_port *port;
137 			/* actual buffer used to store bulk reply */
138 			struct mmal_buffer *buffer;
139 			/* amount of buffer used */
140 			unsigned long buffer_used;
141 			/* MMAL buffer flags */
142 			u32 mmal_flags;
143 			/* Presentation and Decode timestamps */
144 			s64 pts;
145 			s64 dts;
146 
147 			int status;	/* context status */
148 
149 		} bulk;		/* bulk data */
150 
151 		struct {
152 			/* message handle to release */
153 			struct vchiq_header *msg_handle;
154 			/* pointer to received message */
155 			struct mmal_msg *msg;
156 			/* received message length */
157 			u32 msg_len;
158 			/* completion upon reply */
159 			struct completion cmplt;
160 		} sync;		/* synchronous response */
161 	} u;
162 
163 };
164 
165 struct vchiq_mmal_instance {
166 	unsigned int service_handle;
167 
168 	/* ensure serialised access to service */
169 	struct mutex vchiq_mutex;
170 
171 	struct idr context_map;
172 	/* protect accesses to context_map */
173 	struct mutex context_map_lock;
174 
175 	struct vchiq_mmal_component component[VCHIQ_MMAL_MAX_COMPONENTS];
176 
177 	/* ordered workqueue to process all bulk operations */
178 	struct workqueue_struct *bulk_wq;
179 
180 	/* handle for a vchiq instance */
181 	struct vchiq_instance *vchiq_instance;
182 };
183 
184 static struct mmal_msg_context *
get_msg_context(struct vchiq_mmal_instance * instance)185 get_msg_context(struct vchiq_mmal_instance *instance)
186 {
187 	struct mmal_msg_context *msg_context;
188 	int handle;
189 
190 	/* todo: should this be allocated from a pool to avoid kzalloc */
191 	msg_context = kzalloc(sizeof(*msg_context), GFP_KERNEL);
192 
193 	if (!msg_context)
194 		return ERR_PTR(-ENOMEM);
195 
196 	/* Create an ID that will be passed along with our message so
197 	 * that when we service the VCHI reply, we can look up what
198 	 * message is being replied to.
199 	 */
200 	mutex_lock(&instance->context_map_lock);
201 	handle = idr_alloc(&instance->context_map, msg_context,
202 			   0, 0, GFP_KERNEL);
203 	mutex_unlock(&instance->context_map_lock);
204 
205 	if (handle < 0) {
206 		kfree(msg_context);
207 		return ERR_PTR(handle);
208 	}
209 
210 	msg_context->instance = instance;
211 	msg_context->handle = handle;
212 
213 	return msg_context;
214 }
215 
216 static struct mmal_msg_context *
lookup_msg_context(struct vchiq_mmal_instance * instance,int handle)217 lookup_msg_context(struct vchiq_mmal_instance *instance, int handle)
218 {
219 	return idr_find(&instance->context_map, handle);
220 }
221 
222 static void
release_msg_context(struct mmal_msg_context * msg_context)223 release_msg_context(struct mmal_msg_context *msg_context)
224 {
225 	struct vchiq_mmal_instance *instance = msg_context->instance;
226 
227 	mutex_lock(&instance->context_map_lock);
228 	idr_remove(&instance->context_map, msg_context->handle);
229 	mutex_unlock(&instance->context_map_lock);
230 	kfree(msg_context);
231 }
232 
233 /* deals with receipt of event to host message */
event_to_host_cb(struct vchiq_mmal_instance * instance,struct mmal_msg * msg,u32 msg_len)234 static void event_to_host_cb(struct vchiq_mmal_instance *instance,
235 			     struct mmal_msg *msg, u32 msg_len)
236 {
237 	pr_debug("unhandled event\n");
238 	pr_debug("component:%u port type:%d num:%d cmd:0x%x length:%d\n",
239 		 msg->u.event_to_host.client_component,
240 		 msg->u.event_to_host.port_type,
241 		 msg->u.event_to_host.port_num,
242 		 msg->u.event_to_host.cmd, msg->u.event_to_host.length);
243 }
244 
245 /* workqueue scheduled callback
246  *
247  * we do this because it is important we do not call any other vchiq
248  * sync calls from within the message delivery thread
249  */
buffer_work_cb(struct work_struct * work)250 static void buffer_work_cb(struct work_struct *work)
251 {
252 	struct mmal_msg_context *msg_context =
253 		container_of(work, struct mmal_msg_context, u.bulk.work);
254 	struct mmal_buffer *buffer = msg_context->u.bulk.buffer;
255 
256 	if (!buffer) {
257 		pr_err("%s: ctx: %p, No mmal buffer to pass details\n",
258 		       __func__, msg_context);
259 		return;
260 	}
261 
262 	buffer->length = msg_context->u.bulk.buffer_used;
263 	buffer->mmal_flags = msg_context->u.bulk.mmal_flags;
264 	buffer->dts = msg_context->u.bulk.dts;
265 	buffer->pts = msg_context->u.bulk.pts;
266 
267 	atomic_dec(&msg_context->u.bulk.port->buffers_with_vpu);
268 
269 	msg_context->u.bulk.port->buffer_cb(msg_context->u.bulk.instance,
270 					    msg_context->u.bulk.port,
271 					    msg_context->u.bulk.status,
272 					    msg_context->u.bulk.buffer);
273 }
274 
275 /* workqueue scheduled callback to handle receiving buffers
276  *
277  * VCHI will allow up to 4 bulk receives to be scheduled before blocking.
278  * If we block in the service_callback context then we can't process the
279  * VCHI_CALLBACK_BULK_RECEIVED message that would otherwise allow the blocked
280  * vchiq_bulk_receive() call to complete.
281  */
buffer_to_host_work_cb(struct work_struct * work)282 static void buffer_to_host_work_cb(struct work_struct *work)
283 {
284 	struct mmal_msg_context *msg_context =
285 		container_of(work, struct mmal_msg_context,
286 			     u.bulk.buffer_to_host_work);
287 	struct vchiq_mmal_instance *instance = msg_context->instance;
288 	unsigned long len = msg_context->u.bulk.buffer_used;
289 	int ret;
290 
291 	if (!len)
292 		/* Dummy receive to ensure the buffers remain in order */
293 		len = 8;
294 	/* queue the bulk submission */
295 	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
296 	ret = vchiq_bulk_receive(instance->vchiq_instance, instance->service_handle,
297 				 msg_context->u.bulk.buffer->buffer,
298 				 /* Actual receive needs to be a multiple
299 				  * of 4 bytes
300 				  */
301 				(len + 3) & ~3,
302 				msg_context,
303 				VCHIQ_BULK_MODE_CALLBACK);
304 
305 	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
306 
307 	if (ret != 0)
308 		pr_err("%s: ctx: %p, vchiq_bulk_receive failed %d\n",
309 		       __func__, msg_context, ret);
310 }
311 
312 /* enqueue a bulk receive for a given message context */
bulk_receive(struct vchiq_mmal_instance * instance,struct mmal_msg * msg,struct mmal_msg_context * msg_context)313 static int bulk_receive(struct vchiq_mmal_instance *instance,
314 			struct mmal_msg *msg,
315 			struct mmal_msg_context *msg_context)
316 {
317 	unsigned long rd_len;
318 
319 	rd_len = msg->u.buffer_from_host.buffer_header.length;
320 
321 	if (!msg_context->u.bulk.buffer) {
322 		pr_err("bulk.buffer not configured - error in buffer_from_host\n");
323 
324 		/* todo: this is a serious error, we should never have
325 		 * committed a buffer_to_host operation to the mmal
326 		 * port without the buffer to back it up (underflow
327 		 * handling) and there is no obvious way to deal with
328 		 * this - how is the mmal servie going to react when
329 		 * we fail to do the xfer and reschedule a buffer when
330 		 * it arrives? perhaps a starved flag to indicate a
331 		 * waiting bulk receive?
332 		 */
333 
334 		return -EINVAL;
335 	}
336 
337 	/* ensure we do not overrun the available buffer */
338 	if (rd_len > msg_context->u.bulk.buffer->buffer_size) {
339 		rd_len = msg_context->u.bulk.buffer->buffer_size;
340 		pr_warn("short read as not enough receive buffer space\n");
341 		/* todo: is this the correct response, what happens to
342 		 * the rest of the message data?
343 		 */
344 	}
345 
346 	/* store length */
347 	msg_context->u.bulk.buffer_used = rd_len;
348 	msg_context->u.bulk.dts = msg->u.buffer_from_host.buffer_header.dts;
349 	msg_context->u.bulk.pts = msg->u.buffer_from_host.buffer_header.pts;
350 
351 	queue_work(msg_context->instance->bulk_wq,
352 		   &msg_context->u.bulk.buffer_to_host_work);
353 
354 	return 0;
355 }
356 
357 /* data in message, memcpy from packet into output buffer */
inline_receive(struct vchiq_mmal_instance * instance,struct mmal_msg * msg,struct mmal_msg_context * msg_context)358 static int inline_receive(struct vchiq_mmal_instance *instance,
359 			  struct mmal_msg *msg,
360 			  struct mmal_msg_context *msg_context)
361 {
362 	memcpy(msg_context->u.bulk.buffer->buffer,
363 	       msg->u.buffer_from_host.short_data,
364 	       msg->u.buffer_from_host.payload_in_message);
365 
366 	msg_context->u.bulk.buffer_used =
367 	    msg->u.buffer_from_host.payload_in_message;
368 
369 	return 0;
370 }
371 
372 /* queue the buffer availability with MMAL_MSG_TYPE_BUFFER_FROM_HOST */
373 static int
buffer_from_host(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,struct mmal_buffer * buf)374 buffer_from_host(struct vchiq_mmal_instance *instance,
375 		 struct vchiq_mmal_port *port, struct mmal_buffer *buf)
376 {
377 	struct mmal_msg_context *msg_context;
378 	struct mmal_msg m;
379 	int ret;
380 
381 	if (!port->enabled)
382 		return -EINVAL;
383 
384 	pr_debug("instance:%u buffer:%p\n", instance->service_handle, buf);
385 
386 	/* get context */
387 	if (!buf->msg_context) {
388 		pr_err("%s: msg_context not allocated, buf %p\n", __func__,
389 		       buf);
390 		return -EINVAL;
391 	}
392 	msg_context = buf->msg_context;
393 
394 	/* store bulk message context for when data arrives */
395 	msg_context->u.bulk.instance = instance;
396 	msg_context->u.bulk.port = port;
397 	msg_context->u.bulk.buffer = buf;
398 	msg_context->u.bulk.buffer_used = 0;
399 
400 	/* initialise work structure ready to schedule callback */
401 	INIT_WORK(&msg_context->u.bulk.work, buffer_work_cb);
402 	INIT_WORK(&msg_context->u.bulk.buffer_to_host_work,
403 		  buffer_to_host_work_cb);
404 
405 	atomic_inc(&port->buffers_with_vpu);
406 
407 	/* prep the buffer from host message */
408 	memset(&m, 0xbc, sizeof(m));	/* just to make debug clearer */
409 
410 	m.h.type = MMAL_MSG_TYPE_BUFFER_FROM_HOST;
411 	m.h.magic = MMAL_MAGIC;
412 	m.h.context = msg_context->handle;
413 	m.h.status = 0;
414 
415 	/* drvbuf is our private data passed back */
416 	m.u.buffer_from_host.drvbuf.magic = MMAL_MAGIC;
417 	m.u.buffer_from_host.drvbuf.component_handle = port->component->handle;
418 	m.u.buffer_from_host.drvbuf.port_handle = port->handle;
419 	m.u.buffer_from_host.drvbuf.client_context = msg_context->handle;
420 
421 	/* buffer header */
422 	m.u.buffer_from_host.buffer_header.cmd = 0;
423 	m.u.buffer_from_host.buffer_header.data =
424 		(u32)(unsigned long)buf->buffer;
425 	m.u.buffer_from_host.buffer_header.alloc_size = buf->buffer_size;
426 	m.u.buffer_from_host.buffer_header.length = 0;	/* nothing used yet */
427 	m.u.buffer_from_host.buffer_header.offset = 0;	/* no offset */
428 	m.u.buffer_from_host.buffer_header.flags = 0;	/* no flags */
429 	m.u.buffer_from_host.buffer_header.pts = MMAL_TIME_UNKNOWN;
430 	m.u.buffer_from_host.buffer_header.dts = MMAL_TIME_UNKNOWN;
431 
432 	/* clear buffer type specific data */
433 	memset(&m.u.buffer_from_host.buffer_header_type_specific, 0,
434 	       sizeof(m.u.buffer_from_host.buffer_header_type_specific));
435 
436 	/* no payload in message */
437 	m.u.buffer_from_host.payload_in_message = 0;
438 
439 	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
440 
441 	ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, &m,
442 					 sizeof(struct mmal_msg_header) +
443 					 sizeof(m.u.buffer_from_host));
444 	if (ret)
445 		atomic_dec(&port->buffers_with_vpu);
446 
447 	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
448 
449 	return ret;
450 }
451 
452 /* deals with receipt of buffer to host message */
buffer_to_host_cb(struct vchiq_mmal_instance * instance,struct mmal_msg * msg,u32 msg_len)453 static void buffer_to_host_cb(struct vchiq_mmal_instance *instance,
454 			      struct mmal_msg *msg, u32 msg_len)
455 {
456 	struct mmal_msg_context *msg_context;
457 	u32 handle;
458 
459 	pr_debug("%s: instance:%p msg:%p msg_len:%d\n",
460 		 __func__, instance, msg, msg_len);
461 
462 	if (msg->u.buffer_from_host.drvbuf.magic == MMAL_MAGIC) {
463 		handle = msg->u.buffer_from_host.drvbuf.client_context;
464 		msg_context = lookup_msg_context(instance, handle);
465 
466 		if (!msg_context) {
467 			pr_err("drvbuf.client_context(%u) is invalid\n",
468 			       handle);
469 			return;
470 		}
471 	} else {
472 		pr_err("MMAL_MSG_TYPE_BUFFER_TO_HOST with bad magic\n");
473 		return;
474 	}
475 
476 	msg_context->u.bulk.mmal_flags =
477 				msg->u.buffer_from_host.buffer_header.flags;
478 
479 	if (msg->h.status != MMAL_MSG_STATUS_SUCCESS) {
480 		/* message reception had an error */
481 		pr_warn("error %d in reply\n", msg->h.status);
482 
483 		msg_context->u.bulk.status = msg->h.status;
484 
485 	} else if (msg->u.buffer_from_host.buffer_header.length == 0) {
486 		/* empty buffer */
487 		if (msg->u.buffer_from_host.buffer_header.flags &
488 		    MMAL_BUFFER_HEADER_FLAG_EOS) {
489 			msg_context->u.bulk.status =
490 			    bulk_receive(instance, msg, msg_context);
491 			if (msg_context->u.bulk.status == 0)
492 				return;	/* successful bulk submission, bulk
493 					 * completion will trigger callback
494 					 */
495 		} else {
496 			/* do callback with empty buffer - not EOS though */
497 			msg_context->u.bulk.status = 0;
498 			msg_context->u.bulk.buffer_used = 0;
499 		}
500 	} else if (msg->u.buffer_from_host.payload_in_message == 0) {
501 		/* data is not in message, queue a bulk receive */
502 		msg_context->u.bulk.status =
503 		    bulk_receive(instance, msg, msg_context);
504 		if (msg_context->u.bulk.status == 0)
505 			return;	/* successful bulk submission, bulk
506 				 * completion will trigger callback
507 				 */
508 
509 		/* failed to submit buffer, this will end badly */
510 		pr_err("error %d on bulk submission\n",
511 		       msg_context->u.bulk.status);
512 
513 	} else if (msg->u.buffer_from_host.payload_in_message <=
514 		   MMAL_VC_SHORT_DATA) {
515 		/* data payload within message */
516 		msg_context->u.bulk.status = inline_receive(instance, msg,
517 							    msg_context);
518 	} else {
519 		pr_err("message with invalid short payload\n");
520 
521 		/* signal error */
522 		msg_context->u.bulk.status = -EINVAL;
523 		msg_context->u.bulk.buffer_used =
524 		    msg->u.buffer_from_host.payload_in_message;
525 	}
526 
527 	/* schedule the port callback */
528 	schedule_work(&msg_context->u.bulk.work);
529 }
530 
bulk_receive_cb(struct vchiq_mmal_instance * instance,struct mmal_msg_context * msg_context)531 static void bulk_receive_cb(struct vchiq_mmal_instance *instance,
532 			    struct mmal_msg_context *msg_context)
533 {
534 	msg_context->u.bulk.status = 0;
535 
536 	/* schedule the port callback */
537 	schedule_work(&msg_context->u.bulk.work);
538 }
539 
bulk_abort_cb(struct vchiq_mmal_instance * instance,struct mmal_msg_context * msg_context)540 static void bulk_abort_cb(struct vchiq_mmal_instance *instance,
541 			  struct mmal_msg_context *msg_context)
542 {
543 	pr_err("%s: bulk ABORTED msg_context:%p\n", __func__, msg_context);
544 
545 	msg_context->u.bulk.status = -EINTR;
546 
547 	schedule_work(&msg_context->u.bulk.work);
548 }
549 
550 /* incoming event service callback */
service_callback(struct vchiq_instance * vchiq_instance,enum vchiq_reason reason,struct vchiq_header * header,unsigned int handle,void * bulk_ctx)551 static int service_callback(struct vchiq_instance *vchiq_instance,
552 			    enum vchiq_reason reason, struct vchiq_header *header,
553 			    unsigned int handle, void *bulk_ctx)
554 {
555 	struct vchiq_mmal_instance *instance = vchiq_get_service_userdata(vchiq_instance, handle);
556 	u32 msg_len;
557 	struct mmal_msg *msg;
558 	struct mmal_msg_context *msg_context;
559 
560 	if (!instance) {
561 		pr_err("Message callback passed NULL instance\n");
562 		return 0;
563 	}
564 
565 	switch (reason) {
566 	case VCHIQ_MESSAGE_AVAILABLE:
567 		msg = (void *)header->data;
568 		msg_len = header->size;
569 
570 		DBG_DUMP_MSG(msg, msg_len, "<<< reply message");
571 
572 		/* handling is different for buffer messages */
573 		switch (msg->h.type) {
574 		case MMAL_MSG_TYPE_BUFFER_FROM_HOST:
575 			vchiq_release_message(vchiq_instance, handle, header);
576 			break;
577 
578 		case MMAL_MSG_TYPE_EVENT_TO_HOST:
579 			event_to_host_cb(instance, msg, msg_len);
580 			vchiq_release_message(vchiq_instance, handle, header);
581 
582 			break;
583 
584 		case MMAL_MSG_TYPE_BUFFER_TO_HOST:
585 			buffer_to_host_cb(instance, msg, msg_len);
586 			vchiq_release_message(vchiq_instance, handle, header);
587 			break;
588 
589 		default:
590 			/* messages dependent on header context to complete */
591 			if (!msg->h.context) {
592 				pr_err("received message context was null!\n");
593 				vchiq_release_message(vchiq_instance, handle, header);
594 				break;
595 			}
596 
597 			msg_context = lookup_msg_context(instance,
598 							 msg->h.context);
599 			if (!msg_context) {
600 				pr_err("received invalid message context %u!\n",
601 				       msg->h.context);
602 				vchiq_release_message(vchiq_instance, handle, header);
603 				break;
604 			}
605 
606 			/* fill in context values */
607 			msg_context->u.sync.msg_handle = header;
608 			msg_context->u.sync.msg = msg;
609 			msg_context->u.sync.msg_len = msg_len;
610 
611 			/* todo: should this check (completion_done()
612 			 * == 1) for no one waiting? or do we need a
613 			 * flag to tell us the completion has been
614 			 * interrupted so we can free the message and
615 			 * its context. This probably also solves the
616 			 * message arriving after interruption todo
617 			 * below
618 			 */
619 
620 			/* complete message so caller knows it happened */
621 			complete(&msg_context->u.sync.cmplt);
622 			break;
623 		}
624 
625 		break;
626 
627 	case VCHIQ_BULK_RECEIVE_DONE:
628 		bulk_receive_cb(instance, bulk_ctx);
629 		break;
630 
631 	case VCHIQ_BULK_RECEIVE_ABORTED:
632 		bulk_abort_cb(instance, bulk_ctx);
633 		break;
634 
635 	case VCHIQ_SERVICE_CLOSED:
636 		/* TODO: consider if this requires action if received when
637 		 * driver is not explicitly closing the service
638 		 */
639 		break;
640 
641 	default:
642 		pr_err("Received unhandled message reason %d\n", reason);
643 		break;
644 	}
645 
646 	return 0;
647 }
648 
send_synchronous_mmal_msg(struct vchiq_mmal_instance * instance,struct mmal_msg * msg,unsigned int payload_len,struct mmal_msg ** msg_out,struct vchiq_header ** msg_handle)649 static int send_synchronous_mmal_msg(struct vchiq_mmal_instance *instance,
650 				     struct mmal_msg *msg,
651 				     unsigned int payload_len,
652 				     struct mmal_msg **msg_out,
653 				     struct vchiq_header **msg_handle)
654 {
655 	struct mmal_msg_context *msg_context;
656 	int ret;
657 	unsigned long timeout;
658 
659 	/* payload size must not cause message to exceed max size */
660 	if (payload_len >
661 	    (MMAL_MSG_MAX_SIZE - sizeof(struct mmal_msg_header))) {
662 		pr_err("payload length %d exceeds max:%d\n", payload_len,
663 		       (int)(MMAL_MSG_MAX_SIZE -
664 			    sizeof(struct mmal_msg_header)));
665 		return -EINVAL;
666 	}
667 
668 	msg_context = get_msg_context(instance);
669 	if (IS_ERR(msg_context))
670 		return PTR_ERR(msg_context);
671 
672 	init_completion(&msg_context->u.sync.cmplt);
673 
674 	msg->h.magic = MMAL_MAGIC;
675 	msg->h.context = msg_context->handle;
676 	msg->h.status = 0;
677 
678 	DBG_DUMP_MSG(msg, (sizeof(struct mmal_msg_header) + payload_len),
679 		     ">>> sync message");
680 
681 	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
682 
683 	ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, msg,
684 					 sizeof(struct mmal_msg_header) +
685 					 payload_len);
686 
687 	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
688 
689 	if (ret) {
690 		pr_err("error %d queuing message\n", ret);
691 		release_msg_context(msg_context);
692 		return ret;
693 	}
694 
695 	timeout = wait_for_completion_timeout(&msg_context->u.sync.cmplt,
696 					      SYNC_MSG_TIMEOUT * HZ);
697 	if (timeout == 0) {
698 		pr_err("timed out waiting for sync completion\n");
699 		ret = -ETIME;
700 		/* todo: what happens if the message arrives after aborting */
701 		release_msg_context(msg_context);
702 		return ret;
703 	}
704 
705 	*msg_out = msg_context->u.sync.msg;
706 	*msg_handle = msg_context->u.sync.msg_handle;
707 	release_msg_context(msg_context);
708 
709 	return 0;
710 }
711 
dump_port_info(struct vchiq_mmal_port * port)712 static void dump_port_info(struct vchiq_mmal_port *port)
713 {
714 	pr_debug("port handle:0x%x enabled:%d\n", port->handle, port->enabled);
715 
716 	pr_debug("buffer minimum num:%d size:%d align:%d\n",
717 		 port->minimum_buffer.num,
718 		 port->minimum_buffer.size, port->minimum_buffer.alignment);
719 
720 	pr_debug("buffer recommended num:%d size:%d align:%d\n",
721 		 port->recommended_buffer.num,
722 		 port->recommended_buffer.size,
723 		 port->recommended_buffer.alignment);
724 
725 	pr_debug("buffer current values num:%d size:%d align:%d\n",
726 		 port->current_buffer.num,
727 		 port->current_buffer.size, port->current_buffer.alignment);
728 
729 	pr_debug("elementary stream: type:%d encoding:0x%x variant:0x%x\n",
730 		 port->format.type,
731 		 port->format.encoding, port->format.encoding_variant);
732 
733 	pr_debug("		    bitrate:%d flags:0x%x\n",
734 		 port->format.bitrate, port->format.flags);
735 
736 	if (port->format.type == MMAL_ES_TYPE_VIDEO) {
737 		pr_debug
738 		    ("es video format: width:%d height:%d colourspace:0x%x\n",
739 		     port->es.video.width, port->es.video.height,
740 		     port->es.video.color_space);
741 
742 		pr_debug("		 : crop xywh %d,%d,%d,%d\n",
743 			 port->es.video.crop.x,
744 			 port->es.video.crop.y,
745 			 port->es.video.crop.width, port->es.video.crop.height);
746 		pr_debug("		 : framerate %d/%d  aspect %d/%d\n",
747 			 port->es.video.frame_rate.numerator,
748 			 port->es.video.frame_rate.denominator,
749 			 port->es.video.par.numerator, port->es.video.par.denominator);
750 	}
751 }
752 
port_to_mmal_msg(struct vchiq_mmal_port * port,struct mmal_port * p)753 static void port_to_mmal_msg(struct vchiq_mmal_port *port, struct mmal_port *p)
754 {
755 	/* todo do readonly fields need setting at all? */
756 	p->type = port->type;
757 	p->index = port->index;
758 	p->index_all = 0;
759 	p->is_enabled = port->enabled;
760 	p->buffer_num_min = port->minimum_buffer.num;
761 	p->buffer_size_min = port->minimum_buffer.size;
762 	p->buffer_alignment_min = port->minimum_buffer.alignment;
763 	p->buffer_num_recommended = port->recommended_buffer.num;
764 	p->buffer_size_recommended = port->recommended_buffer.size;
765 
766 	/* only three writable fields in a port */
767 	p->buffer_num = port->current_buffer.num;
768 	p->buffer_size = port->current_buffer.size;
769 	p->userdata = (u32)(unsigned long)port;
770 }
771 
port_info_set(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port)772 static int port_info_set(struct vchiq_mmal_instance *instance,
773 			 struct vchiq_mmal_port *port)
774 {
775 	int ret;
776 	struct mmal_msg m;
777 	struct mmal_msg *rmsg;
778 	struct vchiq_header *rmsg_handle;
779 
780 	pr_debug("setting port info port %p\n", port);
781 	if (!port)
782 		return -1;
783 	dump_port_info(port);
784 
785 	m.h.type = MMAL_MSG_TYPE_PORT_INFO_SET;
786 
787 	m.u.port_info_set.component_handle = port->component->handle;
788 	m.u.port_info_set.port_type = port->type;
789 	m.u.port_info_set.port_index = port->index;
790 
791 	port_to_mmal_msg(port, &m.u.port_info_set.port);
792 
793 	/* elementary stream format setup */
794 	m.u.port_info_set.format.type = port->format.type;
795 	m.u.port_info_set.format.encoding = port->format.encoding;
796 	m.u.port_info_set.format.encoding_variant =
797 	    port->format.encoding_variant;
798 	m.u.port_info_set.format.bitrate = port->format.bitrate;
799 	m.u.port_info_set.format.flags = port->format.flags;
800 
801 	memcpy(&m.u.port_info_set.es, &port->es,
802 	       sizeof(union mmal_es_specific_format));
803 
804 	m.u.port_info_set.format.extradata_size = port->format.extradata_size;
805 	memcpy(&m.u.port_info_set.extradata, port->format.extradata,
806 	       port->format.extradata_size);
807 
808 	ret = send_synchronous_mmal_msg(instance, &m,
809 					sizeof(m.u.port_info_set),
810 					&rmsg, &rmsg_handle);
811 	if (ret)
812 		return ret;
813 
814 	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_SET) {
815 		/* got an unexpected message type in reply */
816 		ret = -EINVAL;
817 		goto release_msg;
818 	}
819 
820 	/* return operation status */
821 	ret = -rmsg->u.port_info_get_reply.status;
822 
823 	pr_debug("%s:result:%d component:0x%x port:%d\n", __func__, ret,
824 		 port->component->handle, port->handle);
825 
826 release_msg:
827 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
828 
829 	return ret;
830 }
831 
832 /* use port info get message to retrieve port information */
port_info_get(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port)833 static int port_info_get(struct vchiq_mmal_instance *instance,
834 			 struct vchiq_mmal_port *port)
835 {
836 	int ret;
837 	struct mmal_msg m;
838 	struct mmal_msg *rmsg;
839 	struct vchiq_header *rmsg_handle;
840 
841 	/* port info time */
842 	m.h.type = MMAL_MSG_TYPE_PORT_INFO_GET;
843 	m.u.port_info_get.component_handle = port->component->handle;
844 	m.u.port_info_get.port_type = port->type;
845 	m.u.port_info_get.index = port->index;
846 
847 	ret = send_synchronous_mmal_msg(instance, &m,
848 					sizeof(m.u.port_info_get),
849 					&rmsg, &rmsg_handle);
850 	if (ret)
851 		return ret;
852 
853 	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_GET) {
854 		/* got an unexpected message type in reply */
855 		ret = -EINVAL;
856 		goto release_msg;
857 	}
858 
859 	/* return operation status */
860 	ret = -rmsg->u.port_info_get_reply.status;
861 	if (ret != MMAL_MSG_STATUS_SUCCESS)
862 		goto release_msg;
863 
864 	if (rmsg->u.port_info_get_reply.port.is_enabled == 0)
865 		port->enabled = false;
866 	else
867 		port->enabled = true;
868 
869 	/* copy the values out of the message */
870 	port->handle = rmsg->u.port_info_get_reply.port_handle;
871 
872 	/* port type and index cached to use on port info set because
873 	 * it does not use a port handle
874 	 */
875 	port->type = rmsg->u.port_info_get_reply.port_type;
876 	port->index = rmsg->u.port_info_get_reply.port_index;
877 
878 	port->minimum_buffer.num =
879 	    rmsg->u.port_info_get_reply.port.buffer_num_min;
880 	port->minimum_buffer.size =
881 	    rmsg->u.port_info_get_reply.port.buffer_size_min;
882 	port->minimum_buffer.alignment =
883 	    rmsg->u.port_info_get_reply.port.buffer_alignment_min;
884 
885 	port->recommended_buffer.alignment =
886 	    rmsg->u.port_info_get_reply.port.buffer_alignment_min;
887 	port->recommended_buffer.num =
888 	    rmsg->u.port_info_get_reply.port.buffer_num_recommended;
889 
890 	port->current_buffer.num = rmsg->u.port_info_get_reply.port.buffer_num;
891 	port->current_buffer.size =
892 	    rmsg->u.port_info_get_reply.port.buffer_size;
893 
894 	/* stream format */
895 	port->format.type = rmsg->u.port_info_get_reply.format.type;
896 	port->format.encoding = rmsg->u.port_info_get_reply.format.encoding;
897 	port->format.encoding_variant =
898 	    rmsg->u.port_info_get_reply.format.encoding_variant;
899 	port->format.bitrate = rmsg->u.port_info_get_reply.format.bitrate;
900 	port->format.flags = rmsg->u.port_info_get_reply.format.flags;
901 
902 	/* elementary stream format */
903 	memcpy(&port->es,
904 	       &rmsg->u.port_info_get_reply.es,
905 	       sizeof(union mmal_es_specific_format));
906 	port->format.es = &port->es;
907 
908 	port->format.extradata_size =
909 	    rmsg->u.port_info_get_reply.format.extradata_size;
910 	memcpy(port->format.extradata,
911 	       rmsg->u.port_info_get_reply.extradata,
912 	       port->format.extradata_size);
913 
914 	pr_debug("received port info\n");
915 	dump_port_info(port);
916 
917 release_msg:
918 
919 	pr_debug("%s:result:%d component:0x%x port:%d\n",
920 		 __func__, ret, port->component->handle, port->handle);
921 
922 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
923 
924 	return ret;
925 }
926 
927 /* create component on vc */
create_component(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component,const char * name)928 static int create_component(struct vchiq_mmal_instance *instance,
929 			    struct vchiq_mmal_component *component,
930 			    const char *name)
931 {
932 	int ret;
933 	struct mmal_msg m;
934 	struct mmal_msg *rmsg;
935 	struct vchiq_header *rmsg_handle;
936 
937 	/* build component create message */
938 	m.h.type = MMAL_MSG_TYPE_COMPONENT_CREATE;
939 	m.u.component_create.client_component = component->client_component;
940 	strscpy_pad(m.u.component_create.name, name,
941 		    sizeof(m.u.component_create.name));
942 	m.u.component_create.pid = 0;
943 
944 	ret = send_synchronous_mmal_msg(instance, &m,
945 					sizeof(m.u.component_create),
946 					&rmsg, &rmsg_handle);
947 	if (ret)
948 		return ret;
949 
950 	if (rmsg->h.type != m.h.type) {
951 		/* got an unexpected message type in reply */
952 		ret = -EINVAL;
953 		goto release_msg;
954 	}
955 
956 	ret = -rmsg->u.component_create_reply.status;
957 	if (ret != MMAL_MSG_STATUS_SUCCESS)
958 		goto release_msg;
959 
960 	/* a valid component response received */
961 	component->handle = rmsg->u.component_create_reply.component_handle;
962 	component->inputs = rmsg->u.component_create_reply.input_num;
963 	component->outputs = rmsg->u.component_create_reply.output_num;
964 	component->clocks = rmsg->u.component_create_reply.clock_num;
965 
966 	pr_debug("Component handle:0x%x in:%d out:%d clock:%d\n",
967 		 component->handle,
968 		 component->inputs, component->outputs, component->clocks);
969 
970 release_msg:
971 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
972 
973 	return ret;
974 }
975 
976 /* destroys a component on vc */
destroy_component(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component)977 static int destroy_component(struct vchiq_mmal_instance *instance,
978 			     struct vchiq_mmal_component *component)
979 {
980 	int ret;
981 	struct mmal_msg m;
982 	struct mmal_msg *rmsg;
983 	struct vchiq_header *rmsg_handle;
984 
985 	m.h.type = MMAL_MSG_TYPE_COMPONENT_DESTROY;
986 	m.u.component_destroy.component_handle = component->handle;
987 
988 	ret = send_synchronous_mmal_msg(instance, &m,
989 					sizeof(m.u.component_destroy),
990 					&rmsg, &rmsg_handle);
991 	if (ret)
992 		return ret;
993 
994 	if (rmsg->h.type != m.h.type) {
995 		/* got an unexpected message type in reply */
996 		ret = -EINVAL;
997 		goto release_msg;
998 	}
999 
1000 	ret = -rmsg->u.component_destroy_reply.status;
1001 
1002 release_msg:
1003 
1004 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1005 
1006 	return ret;
1007 }
1008 
1009 /* enable a component on vc */
enable_component(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component)1010 static int enable_component(struct vchiq_mmal_instance *instance,
1011 			    struct vchiq_mmal_component *component)
1012 {
1013 	int ret;
1014 	struct mmal_msg m;
1015 	struct mmal_msg *rmsg;
1016 	struct vchiq_header *rmsg_handle;
1017 
1018 	m.h.type = MMAL_MSG_TYPE_COMPONENT_ENABLE;
1019 	m.u.component_enable.component_handle = component->handle;
1020 
1021 	ret = send_synchronous_mmal_msg(instance, &m,
1022 					sizeof(m.u.component_enable),
1023 					&rmsg, &rmsg_handle);
1024 	if (ret)
1025 		return ret;
1026 
1027 	if (rmsg->h.type != m.h.type) {
1028 		/* got an unexpected message type in reply */
1029 		ret = -EINVAL;
1030 		goto release_msg;
1031 	}
1032 
1033 	ret = -rmsg->u.component_enable_reply.status;
1034 
1035 release_msg:
1036 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1037 
1038 	return ret;
1039 }
1040 
1041 /* disable a component on vc */
disable_component(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component)1042 static int disable_component(struct vchiq_mmal_instance *instance,
1043 			     struct vchiq_mmal_component *component)
1044 {
1045 	int ret;
1046 	struct mmal_msg m;
1047 	struct mmal_msg *rmsg;
1048 	struct vchiq_header *rmsg_handle;
1049 
1050 	m.h.type = MMAL_MSG_TYPE_COMPONENT_DISABLE;
1051 	m.u.component_disable.component_handle = component->handle;
1052 
1053 	ret = send_synchronous_mmal_msg(instance, &m,
1054 					sizeof(m.u.component_disable),
1055 					&rmsg, &rmsg_handle);
1056 	if (ret)
1057 		return ret;
1058 
1059 	if (rmsg->h.type != m.h.type) {
1060 		/* got an unexpected message type in reply */
1061 		ret = -EINVAL;
1062 		goto release_msg;
1063 	}
1064 
1065 	ret = -rmsg->u.component_disable_reply.status;
1066 
1067 release_msg:
1068 
1069 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1070 
1071 	return ret;
1072 }
1073 
1074 /* get version of mmal implementation */
get_version(struct vchiq_mmal_instance * instance,u32 * major_out,u32 * minor_out)1075 static int get_version(struct vchiq_mmal_instance *instance,
1076 		       u32 *major_out, u32 *minor_out)
1077 {
1078 	int ret;
1079 	struct mmal_msg m;
1080 	struct mmal_msg *rmsg;
1081 	struct vchiq_header *rmsg_handle;
1082 
1083 	m.h.type = MMAL_MSG_TYPE_GET_VERSION;
1084 
1085 	ret = send_synchronous_mmal_msg(instance, &m,
1086 					sizeof(m.u.version),
1087 					&rmsg, &rmsg_handle);
1088 	if (ret)
1089 		return ret;
1090 
1091 	if (rmsg->h.type != m.h.type) {
1092 		/* got an unexpected message type in reply */
1093 		ret = -EINVAL;
1094 		goto release_msg;
1095 	}
1096 
1097 	*major_out = rmsg->u.version.major;
1098 	*minor_out = rmsg->u.version.minor;
1099 
1100 release_msg:
1101 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1102 
1103 	return ret;
1104 }
1105 
1106 /* do a port action with a port as a parameter */
port_action_port(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,enum mmal_msg_port_action_type action_type)1107 static int port_action_port(struct vchiq_mmal_instance *instance,
1108 			    struct vchiq_mmal_port *port,
1109 			    enum mmal_msg_port_action_type action_type)
1110 {
1111 	int ret;
1112 	struct mmal_msg m;
1113 	struct mmal_msg *rmsg;
1114 	struct vchiq_header *rmsg_handle;
1115 
1116 	m.h.type = MMAL_MSG_TYPE_PORT_ACTION;
1117 	m.u.port_action_port.component_handle = port->component->handle;
1118 	m.u.port_action_port.port_handle = port->handle;
1119 	m.u.port_action_port.action = action_type;
1120 
1121 	port_to_mmal_msg(port, &m.u.port_action_port.port);
1122 
1123 	ret = send_synchronous_mmal_msg(instance, &m,
1124 					sizeof(m.u.port_action_port),
1125 					&rmsg, &rmsg_handle);
1126 	if (ret)
1127 		return ret;
1128 
1129 	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) {
1130 		/* got an unexpected message type in reply */
1131 		ret = -EINVAL;
1132 		goto release_msg;
1133 	}
1134 
1135 	ret = -rmsg->u.port_action_reply.status;
1136 
1137 	pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d)\n",
1138 		 __func__,
1139 		 ret, port->component->handle, port->handle,
1140 		 port_action_type_names[action_type], action_type);
1141 
1142 release_msg:
1143 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1144 
1145 	return ret;
1146 }
1147 
1148 /* do a port action with handles as parameters */
port_action_handle(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,enum mmal_msg_port_action_type action_type,u32 connect_component_handle,u32 connect_port_handle)1149 static int port_action_handle(struct vchiq_mmal_instance *instance,
1150 			      struct vchiq_mmal_port *port,
1151 			      enum mmal_msg_port_action_type action_type,
1152 			      u32 connect_component_handle,
1153 			      u32 connect_port_handle)
1154 {
1155 	int ret;
1156 	struct mmal_msg m;
1157 	struct mmal_msg *rmsg;
1158 	struct vchiq_header *rmsg_handle;
1159 
1160 	m.h.type = MMAL_MSG_TYPE_PORT_ACTION;
1161 
1162 	m.u.port_action_handle.component_handle = port->component->handle;
1163 	m.u.port_action_handle.port_handle = port->handle;
1164 	m.u.port_action_handle.action = action_type;
1165 
1166 	m.u.port_action_handle.connect_component_handle =
1167 	    connect_component_handle;
1168 	m.u.port_action_handle.connect_port_handle = connect_port_handle;
1169 
1170 	ret = send_synchronous_mmal_msg(instance, &m,
1171 					sizeof(m.u.port_action_handle),
1172 					&rmsg, &rmsg_handle);
1173 	if (ret)
1174 		return ret;
1175 
1176 	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) {
1177 		/* got an unexpected message type in reply */
1178 		ret = -EINVAL;
1179 		goto release_msg;
1180 	}
1181 
1182 	ret = -rmsg->u.port_action_reply.status;
1183 
1184 	pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d) connect component:0x%x connect port:%d\n",
1185 		 __func__,
1186 		 ret, port->component->handle, port->handle,
1187 		 port_action_type_names[action_type],
1188 		 action_type, connect_component_handle, connect_port_handle);
1189 
1190 release_msg:
1191 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1192 
1193 	return ret;
1194 }
1195 
port_parameter_set(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,u32 parameter_id,void * value,u32 value_size)1196 static int port_parameter_set(struct vchiq_mmal_instance *instance,
1197 			      struct vchiq_mmal_port *port,
1198 			      u32 parameter_id, void *value, u32 value_size)
1199 {
1200 	int ret;
1201 	struct mmal_msg m;
1202 	struct mmal_msg *rmsg;
1203 	struct vchiq_header *rmsg_handle;
1204 
1205 	m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_SET;
1206 
1207 	m.u.port_parameter_set.component_handle = port->component->handle;
1208 	m.u.port_parameter_set.port_handle = port->handle;
1209 	m.u.port_parameter_set.id = parameter_id;
1210 	m.u.port_parameter_set.size = (2 * sizeof(u32)) + value_size;
1211 	memcpy(&m.u.port_parameter_set.value, value, value_size);
1212 
1213 	ret = send_synchronous_mmal_msg(instance, &m,
1214 					(4 * sizeof(u32)) + value_size,
1215 					&rmsg, &rmsg_handle);
1216 	if (ret)
1217 		return ret;
1218 
1219 	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_SET) {
1220 		/* got an unexpected message type in reply */
1221 		ret = -EINVAL;
1222 		goto release_msg;
1223 	}
1224 
1225 	ret = -rmsg->u.port_parameter_set_reply.status;
1226 
1227 	pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n",
1228 		 __func__,
1229 		 ret, port->component->handle, port->handle, parameter_id);
1230 
1231 release_msg:
1232 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1233 
1234 	return ret;
1235 }
1236 
port_parameter_get(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,u32 parameter_id,void * value,u32 * value_size)1237 static int port_parameter_get(struct vchiq_mmal_instance *instance,
1238 			      struct vchiq_mmal_port *port,
1239 			      u32 parameter_id, void *value, u32 *value_size)
1240 {
1241 	int ret;
1242 	struct mmal_msg m;
1243 	struct mmal_msg *rmsg;
1244 	struct vchiq_header *rmsg_handle;
1245 
1246 	m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_GET;
1247 
1248 	m.u.port_parameter_get.component_handle = port->component->handle;
1249 	m.u.port_parameter_get.port_handle = port->handle;
1250 	m.u.port_parameter_get.id = parameter_id;
1251 	m.u.port_parameter_get.size = (2 * sizeof(u32)) + *value_size;
1252 
1253 	ret = send_synchronous_mmal_msg(instance, &m,
1254 					sizeof(struct
1255 					       mmal_msg_port_parameter_get),
1256 					&rmsg, &rmsg_handle);
1257 	if (ret)
1258 		return ret;
1259 
1260 	if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_GET) {
1261 		/* got an unexpected message type in reply */
1262 		pr_err("Incorrect reply type %d\n", rmsg->h.type);
1263 		ret = -EINVAL;
1264 		goto release_msg;
1265 	}
1266 
1267 	ret = rmsg->u.port_parameter_get_reply.status;
1268 
1269 	/* port_parameter_get_reply.size includes the header,
1270 	 * whilst *value_size doesn't.
1271 	 */
1272 	rmsg->u.port_parameter_get_reply.size -= (2 * sizeof(u32));
1273 
1274 	if (ret || rmsg->u.port_parameter_get_reply.size > *value_size) {
1275 		/* Copy only as much as we have space for
1276 		 * but report true size of parameter
1277 		 */
1278 		memcpy(value, &rmsg->u.port_parameter_get_reply.value,
1279 		       *value_size);
1280 	} else {
1281 		memcpy(value, &rmsg->u.port_parameter_get_reply.value,
1282 		       rmsg->u.port_parameter_get_reply.size);
1283 	}
1284 	/* Always report the size of the returned parameter to the caller */
1285 	*value_size = rmsg->u.port_parameter_get_reply.size;
1286 
1287 	pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n", __func__,
1288 		 ret, port->component->handle, port->handle, parameter_id);
1289 
1290 release_msg:
1291 	vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1292 
1293 	return ret;
1294 }
1295 
1296 /* disables a port and drains buffers from it */
port_disable(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port)1297 static int port_disable(struct vchiq_mmal_instance *instance,
1298 			struct vchiq_mmal_port *port)
1299 {
1300 	int ret;
1301 	struct list_head *q, *buf_head;
1302 	unsigned long flags = 0;
1303 
1304 	if (!port->enabled)
1305 		return 0;
1306 
1307 	port->enabled = false;
1308 
1309 	ret = port_action_port(instance, port,
1310 			       MMAL_MSG_PORT_ACTION_TYPE_DISABLE);
1311 	if (ret == 0) {
1312 		/*
1313 		 * Drain all queued buffers on port. This should only
1314 		 * apply to buffers that have been queued before the port
1315 		 * has been enabled. If the port has been enabled and buffers
1316 		 * passed, then the buffers should have been removed from this
1317 		 * list, and we should get the relevant callbacks via VCHIQ
1318 		 * to release the buffers.
1319 		 */
1320 		spin_lock_irqsave(&port->slock, flags);
1321 
1322 		list_for_each_safe(buf_head, q, &port->buffers) {
1323 			struct mmal_buffer *mmalbuf;
1324 
1325 			mmalbuf = list_entry(buf_head, struct mmal_buffer,
1326 					     list);
1327 			list_del(buf_head);
1328 			if (port->buffer_cb) {
1329 				mmalbuf->length = 0;
1330 				mmalbuf->mmal_flags = 0;
1331 				mmalbuf->dts = MMAL_TIME_UNKNOWN;
1332 				mmalbuf->pts = MMAL_TIME_UNKNOWN;
1333 				port->buffer_cb(instance,
1334 						port, 0, mmalbuf);
1335 			}
1336 		}
1337 
1338 		spin_unlock_irqrestore(&port->slock, flags);
1339 
1340 		ret = port_info_get(instance, port);
1341 	}
1342 
1343 	return ret;
1344 }
1345 
1346 /* enable a port */
port_enable(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port)1347 static int port_enable(struct vchiq_mmal_instance *instance,
1348 		       struct vchiq_mmal_port *port)
1349 {
1350 	unsigned int hdr_count;
1351 	struct list_head *q, *buf_head;
1352 	int ret;
1353 
1354 	if (port->enabled)
1355 		return 0;
1356 
1357 	ret = port_action_port(instance, port,
1358 			       MMAL_MSG_PORT_ACTION_TYPE_ENABLE);
1359 	if (ret)
1360 		goto done;
1361 
1362 	port->enabled = true;
1363 
1364 	if (port->buffer_cb) {
1365 		/* send buffer headers to videocore */
1366 		hdr_count = 1;
1367 		list_for_each_safe(buf_head, q, &port->buffers) {
1368 			struct mmal_buffer *mmalbuf;
1369 
1370 			mmalbuf = list_entry(buf_head, struct mmal_buffer,
1371 					     list);
1372 			ret = buffer_from_host(instance, port, mmalbuf);
1373 			if (ret)
1374 				goto done;
1375 
1376 			list_del(buf_head);
1377 			hdr_count++;
1378 			if (hdr_count > port->current_buffer.num)
1379 				break;
1380 		}
1381 	}
1382 
1383 	ret = port_info_get(instance, port);
1384 
1385 done:
1386 	return ret;
1387 }
1388 
1389 /* ------------------------------------------------------------------
1390  * Exported API
1391  *------------------------------------------------------------------
1392  */
1393 
vchiq_mmal_port_set_format(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port)1394 int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance,
1395 			       struct vchiq_mmal_port *port)
1396 {
1397 	int ret;
1398 
1399 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1400 		return -EINTR;
1401 
1402 	ret = port_info_set(instance, port);
1403 	if (ret)
1404 		goto release_unlock;
1405 
1406 	/* read what has actually been set */
1407 	ret = port_info_get(instance, port);
1408 
1409 release_unlock:
1410 	mutex_unlock(&instance->vchiq_mutex);
1411 
1412 	return ret;
1413 }
1414 EXPORT_SYMBOL_GPL(vchiq_mmal_port_set_format);
1415 
vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,u32 parameter,void * value,u32 value_size)1416 int vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance *instance,
1417 				  struct vchiq_mmal_port *port,
1418 				  u32 parameter, void *value, u32 value_size)
1419 {
1420 	int ret;
1421 
1422 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1423 		return -EINTR;
1424 
1425 	ret = port_parameter_set(instance, port, parameter, value, value_size);
1426 
1427 	mutex_unlock(&instance->vchiq_mutex);
1428 
1429 	return ret;
1430 }
1431 EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_set);
1432 
vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,u32 parameter,void * value,u32 * value_size)1433 int vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance *instance,
1434 				  struct vchiq_mmal_port *port,
1435 				  u32 parameter, void *value, u32 *value_size)
1436 {
1437 	int ret;
1438 
1439 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1440 		return -EINTR;
1441 
1442 	ret = port_parameter_get(instance, port, parameter, value, value_size);
1443 
1444 	mutex_unlock(&instance->vchiq_mutex);
1445 
1446 	return ret;
1447 }
1448 EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_get);
1449 
1450 /* enable a port
1451  *
1452  * enables a port and queues buffers for satisfying callbacks if we
1453  * provide a callback handler
1454  */
vchiq_mmal_port_enable(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,vchiq_mmal_buffer_cb buffer_cb)1455 int vchiq_mmal_port_enable(struct vchiq_mmal_instance *instance,
1456 			   struct vchiq_mmal_port *port,
1457 			   vchiq_mmal_buffer_cb buffer_cb)
1458 {
1459 	int ret;
1460 
1461 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1462 		return -EINTR;
1463 
1464 	/* already enabled - noop */
1465 	if (port->enabled) {
1466 		ret = 0;
1467 		goto unlock;
1468 	}
1469 
1470 	port->buffer_cb = buffer_cb;
1471 
1472 	ret = port_enable(instance, port);
1473 
1474 unlock:
1475 	mutex_unlock(&instance->vchiq_mutex);
1476 
1477 	return ret;
1478 }
1479 EXPORT_SYMBOL_GPL(vchiq_mmal_port_enable);
1480 
vchiq_mmal_port_disable(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port)1481 int vchiq_mmal_port_disable(struct vchiq_mmal_instance *instance,
1482 			    struct vchiq_mmal_port *port)
1483 {
1484 	int ret;
1485 
1486 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1487 		return -EINTR;
1488 
1489 	if (!port->enabled) {
1490 		mutex_unlock(&instance->vchiq_mutex);
1491 		return 0;
1492 	}
1493 
1494 	ret = port_disable(instance, port);
1495 
1496 	mutex_unlock(&instance->vchiq_mutex);
1497 
1498 	return ret;
1499 }
1500 EXPORT_SYMBOL_GPL(vchiq_mmal_port_disable);
1501 
1502 /* ports will be connected in a tunneled manner so data buffers
1503  * are not handled by client.
1504  */
vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * src,struct vchiq_mmal_port * dst)1505 int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance,
1506 				   struct vchiq_mmal_port *src,
1507 				   struct vchiq_mmal_port *dst)
1508 {
1509 	int ret;
1510 
1511 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1512 		return -EINTR;
1513 
1514 	/* disconnect ports if connected */
1515 	if (src->connected) {
1516 		ret = port_disable(instance, src);
1517 		if (ret) {
1518 			pr_err("failed disabling src port(%d)\n", ret);
1519 			goto release_unlock;
1520 		}
1521 
1522 		/* do not need to disable the destination port as they
1523 		 * are connected and it is done automatically
1524 		 */
1525 
1526 		ret = port_action_handle(instance, src,
1527 					 MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT,
1528 					 src->connected->component->handle,
1529 					 src->connected->handle);
1530 		if (ret < 0) {
1531 			pr_err("failed disconnecting src port\n");
1532 			goto release_unlock;
1533 		}
1534 		src->connected->enabled = false;
1535 		src->connected = NULL;
1536 	}
1537 
1538 	if (!dst) {
1539 		/* do not make new connection */
1540 		ret = 0;
1541 		pr_debug("not making new connection\n");
1542 		goto release_unlock;
1543 	}
1544 
1545 	/* copy src port format to dst */
1546 	dst->format.encoding = src->format.encoding;
1547 	dst->es.video.width = src->es.video.width;
1548 	dst->es.video.height = src->es.video.height;
1549 	dst->es.video.crop.x = src->es.video.crop.x;
1550 	dst->es.video.crop.y = src->es.video.crop.y;
1551 	dst->es.video.crop.width = src->es.video.crop.width;
1552 	dst->es.video.crop.height = src->es.video.crop.height;
1553 	dst->es.video.frame_rate.numerator = src->es.video.frame_rate.numerator;
1554 	dst->es.video.frame_rate.denominator = src->es.video.frame_rate.denominator;
1555 
1556 	/* set new format */
1557 	ret = port_info_set(instance, dst);
1558 	if (ret) {
1559 		pr_debug("setting port info failed\n");
1560 		goto release_unlock;
1561 	}
1562 
1563 	/* read what has actually been set */
1564 	ret = port_info_get(instance, dst);
1565 	if (ret) {
1566 		pr_debug("read back port info failed\n");
1567 		goto release_unlock;
1568 	}
1569 
1570 	/* connect two ports together */
1571 	ret = port_action_handle(instance, src,
1572 				 MMAL_MSG_PORT_ACTION_TYPE_CONNECT,
1573 				 dst->component->handle, dst->handle);
1574 	if (ret < 0) {
1575 		pr_debug("connecting port %d:%d to %d:%d failed\n",
1576 			 src->component->handle, src->handle,
1577 			 dst->component->handle, dst->handle);
1578 		goto release_unlock;
1579 	}
1580 	src->connected = dst;
1581 
1582 release_unlock:
1583 
1584 	mutex_unlock(&instance->vchiq_mutex);
1585 
1586 	return ret;
1587 }
1588 EXPORT_SYMBOL_GPL(vchiq_mmal_port_connect_tunnel);
1589 
vchiq_mmal_submit_buffer(struct vchiq_mmal_instance * instance,struct vchiq_mmal_port * port,struct mmal_buffer * buffer)1590 int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance,
1591 			     struct vchiq_mmal_port *port,
1592 			     struct mmal_buffer *buffer)
1593 {
1594 	unsigned long flags = 0;
1595 	int ret;
1596 
1597 	ret = buffer_from_host(instance, port, buffer);
1598 	if (ret == -EINVAL) {
1599 		/* Port is disabled. Queue for when it is enabled. */
1600 		spin_lock_irqsave(&port->slock, flags);
1601 		list_add_tail(&buffer->list, &port->buffers);
1602 		spin_unlock_irqrestore(&port->slock, flags);
1603 	}
1604 
1605 	return 0;
1606 }
1607 EXPORT_SYMBOL_GPL(vchiq_mmal_submit_buffer);
1608 
mmal_vchi_buffer_init(struct vchiq_mmal_instance * instance,struct mmal_buffer * buf)1609 int mmal_vchi_buffer_init(struct vchiq_mmal_instance *instance,
1610 			  struct mmal_buffer *buf)
1611 {
1612 	struct mmal_msg_context *msg_context = get_msg_context(instance);
1613 
1614 	if (IS_ERR(msg_context))
1615 		return (PTR_ERR(msg_context));
1616 
1617 	buf->msg_context = msg_context;
1618 	return 0;
1619 }
1620 EXPORT_SYMBOL_GPL(mmal_vchi_buffer_init);
1621 
mmal_vchi_buffer_cleanup(struct mmal_buffer * buf)1622 int mmal_vchi_buffer_cleanup(struct mmal_buffer *buf)
1623 {
1624 	struct mmal_msg_context *msg_context = buf->msg_context;
1625 
1626 	if (msg_context)
1627 		release_msg_context(msg_context);
1628 	buf->msg_context = NULL;
1629 
1630 	return 0;
1631 }
1632 EXPORT_SYMBOL_GPL(mmal_vchi_buffer_cleanup);
1633 
1634 /* Initialise a mmal component and its ports
1635  *
1636  */
vchiq_mmal_component_init(struct vchiq_mmal_instance * instance,const char * name,struct vchiq_mmal_component ** component_out)1637 int vchiq_mmal_component_init(struct vchiq_mmal_instance *instance,
1638 			      const char *name,
1639 			      struct vchiq_mmal_component **component_out)
1640 {
1641 	int ret;
1642 	int idx;		/* port index */
1643 	struct vchiq_mmal_component *component = NULL;
1644 
1645 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1646 		return -EINTR;
1647 
1648 	for (idx = 0; idx < VCHIQ_MMAL_MAX_COMPONENTS; idx++) {
1649 		if (!instance->component[idx].in_use) {
1650 			component = &instance->component[idx];
1651 			component->in_use = true;
1652 			break;
1653 		}
1654 	}
1655 
1656 	if (!component) {
1657 		ret = -EINVAL;	/* todo is this correct error? */
1658 		goto unlock;
1659 	}
1660 
1661 	/* We need a handle to reference back to our component structure.
1662 	 * Use the array index in instance->component rather than rolling
1663 	 * another IDR.
1664 	 */
1665 	component->client_component = idx;
1666 
1667 	ret = create_component(instance, component, name);
1668 	if (ret < 0) {
1669 		pr_err("%s: failed to create component %d (Not enough GPU mem?)\n",
1670 		       __func__, ret);
1671 		goto unlock;
1672 	}
1673 
1674 	/* ports info needs gathering */
1675 	component->control.type = MMAL_PORT_TYPE_CONTROL;
1676 	component->control.index = 0;
1677 	component->control.component = component;
1678 	spin_lock_init(&component->control.slock);
1679 	INIT_LIST_HEAD(&component->control.buffers);
1680 	ret = port_info_get(instance, &component->control);
1681 	if (ret < 0)
1682 		goto release_component;
1683 
1684 	for (idx = 0; idx < component->inputs; idx++) {
1685 		component->input[idx].type = MMAL_PORT_TYPE_INPUT;
1686 		component->input[idx].index = idx;
1687 		component->input[idx].component = component;
1688 		spin_lock_init(&component->input[idx].slock);
1689 		INIT_LIST_HEAD(&component->input[idx].buffers);
1690 		ret = port_info_get(instance, &component->input[idx]);
1691 		if (ret < 0)
1692 			goto release_component;
1693 	}
1694 
1695 	for (idx = 0; idx < component->outputs; idx++) {
1696 		component->output[idx].type = MMAL_PORT_TYPE_OUTPUT;
1697 		component->output[idx].index = idx;
1698 		component->output[idx].component = component;
1699 		spin_lock_init(&component->output[idx].slock);
1700 		INIT_LIST_HEAD(&component->output[idx].buffers);
1701 		ret = port_info_get(instance, &component->output[idx]);
1702 		if (ret < 0)
1703 			goto release_component;
1704 	}
1705 
1706 	for (idx = 0; idx < component->clocks; idx++) {
1707 		component->clock[idx].type = MMAL_PORT_TYPE_CLOCK;
1708 		component->clock[idx].index = idx;
1709 		component->clock[idx].component = component;
1710 		spin_lock_init(&component->clock[idx].slock);
1711 		INIT_LIST_HEAD(&component->clock[idx].buffers);
1712 		ret = port_info_get(instance, &component->clock[idx]);
1713 		if (ret < 0)
1714 			goto release_component;
1715 	}
1716 
1717 	*component_out = component;
1718 
1719 	mutex_unlock(&instance->vchiq_mutex);
1720 
1721 	return 0;
1722 
1723 release_component:
1724 	destroy_component(instance, component);
1725 unlock:
1726 	if (component)
1727 		component->in_use = false;
1728 	mutex_unlock(&instance->vchiq_mutex);
1729 
1730 	return ret;
1731 }
1732 EXPORT_SYMBOL_GPL(vchiq_mmal_component_init);
1733 
1734 /*
1735  * cause a mmal component to be destroyed
1736  */
vchiq_mmal_component_finalise(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component)1737 int vchiq_mmal_component_finalise(struct vchiq_mmal_instance *instance,
1738 				  struct vchiq_mmal_component *component)
1739 {
1740 	int ret;
1741 
1742 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1743 		return -EINTR;
1744 
1745 	if (component->enabled)
1746 		ret = disable_component(instance, component);
1747 
1748 	ret = destroy_component(instance, component);
1749 
1750 	component->in_use = false;
1751 
1752 	mutex_unlock(&instance->vchiq_mutex);
1753 
1754 	return ret;
1755 }
1756 EXPORT_SYMBOL_GPL(vchiq_mmal_component_finalise);
1757 
1758 /*
1759  * cause a mmal component to be enabled
1760  */
vchiq_mmal_component_enable(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component)1761 int vchiq_mmal_component_enable(struct vchiq_mmal_instance *instance,
1762 				struct vchiq_mmal_component *component)
1763 {
1764 	int ret;
1765 
1766 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1767 		return -EINTR;
1768 
1769 	if (component->enabled) {
1770 		mutex_unlock(&instance->vchiq_mutex);
1771 		return 0;
1772 	}
1773 
1774 	ret = enable_component(instance, component);
1775 	if (ret == 0)
1776 		component->enabled = true;
1777 
1778 	mutex_unlock(&instance->vchiq_mutex);
1779 
1780 	return ret;
1781 }
1782 EXPORT_SYMBOL_GPL(vchiq_mmal_component_enable);
1783 
1784 /*
1785  * cause a mmal component to be enabled
1786  */
vchiq_mmal_component_disable(struct vchiq_mmal_instance * instance,struct vchiq_mmal_component * component)1787 int vchiq_mmal_component_disable(struct vchiq_mmal_instance *instance,
1788 				 struct vchiq_mmal_component *component)
1789 {
1790 	int ret;
1791 
1792 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1793 		return -EINTR;
1794 
1795 	if (!component->enabled) {
1796 		mutex_unlock(&instance->vchiq_mutex);
1797 		return 0;
1798 	}
1799 
1800 	ret = disable_component(instance, component);
1801 	if (ret == 0)
1802 		component->enabled = false;
1803 
1804 	mutex_unlock(&instance->vchiq_mutex);
1805 
1806 	return ret;
1807 }
1808 EXPORT_SYMBOL_GPL(vchiq_mmal_component_disable);
1809 
vchiq_mmal_version(struct vchiq_mmal_instance * instance,u32 * major_out,u32 * minor_out)1810 int vchiq_mmal_version(struct vchiq_mmal_instance *instance,
1811 		       u32 *major_out, u32 *minor_out)
1812 {
1813 	int ret;
1814 
1815 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1816 		return -EINTR;
1817 
1818 	ret = get_version(instance, major_out, minor_out);
1819 
1820 	mutex_unlock(&instance->vchiq_mutex);
1821 
1822 	return ret;
1823 }
1824 EXPORT_SYMBOL_GPL(vchiq_mmal_version);
1825 
vchiq_mmal_finalise(struct vchiq_mmal_instance * instance)1826 int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance)
1827 {
1828 	int status = 0;
1829 
1830 	if (!instance)
1831 		return -EINVAL;
1832 
1833 	if (mutex_lock_interruptible(&instance->vchiq_mutex))
1834 		return -EINTR;
1835 
1836 	vchiq_use_service(instance->vchiq_instance, instance->service_handle);
1837 
1838 	status = vchiq_close_service(instance->vchiq_instance, instance->service_handle);
1839 	if (status != 0)
1840 		pr_err("mmal-vchiq: VCHIQ close failed\n");
1841 
1842 	mutex_unlock(&instance->vchiq_mutex);
1843 
1844 	vchiq_shutdown(instance->vchiq_instance);
1845 	destroy_workqueue(instance->bulk_wq);
1846 
1847 	idr_destroy(&instance->context_map);
1848 
1849 	kfree(instance);
1850 
1851 	return status;
1852 }
1853 EXPORT_SYMBOL_GPL(vchiq_mmal_finalise);
1854 
vchiq_mmal_init(struct vchiq_mmal_instance ** out_instance)1855 int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance)
1856 {
1857 	int status;
1858 	int err = -ENODEV;
1859 	struct vchiq_mmal_instance *instance;
1860 	struct vchiq_instance *vchiq_instance;
1861 	struct vchiq_service_params_kernel params = {
1862 		.version		= VC_MMAL_VER,
1863 		.version_min		= VC_MMAL_MIN_VER,
1864 		.fourcc			= VCHIQ_MAKE_FOURCC('m', 'm', 'a', 'l'),
1865 		.callback		= service_callback,
1866 		.userdata		= NULL,
1867 	};
1868 
1869 	/* compile time checks to ensure structure size as they are
1870 	 * directly (de)serialised from memory.
1871 	 */
1872 
1873 	/* ensure the header structure has packed to the correct size */
1874 	BUILD_BUG_ON(sizeof(struct mmal_msg_header) != 24);
1875 
1876 	/* ensure message structure does not exceed maximum length */
1877 	BUILD_BUG_ON(sizeof(struct mmal_msg) > MMAL_MSG_MAX_SIZE);
1878 
1879 	/* mmal port struct is correct size */
1880 	BUILD_BUG_ON(sizeof(struct mmal_port) != 64);
1881 
1882 	/* create a vchi instance */
1883 	status = vchiq_initialise(&vchiq_instance);
1884 	if (status) {
1885 		pr_err("Failed to initialise VCHI instance (status=%d)\n",
1886 		       status);
1887 		return -EIO;
1888 	}
1889 
1890 	status = vchiq_connect(vchiq_instance);
1891 	if (status) {
1892 		pr_err("Failed to connect VCHI instance (status=%d)\n", status);
1893 		err = -EIO;
1894 		goto err_shutdown_vchiq;
1895 	}
1896 
1897 	instance = kzalloc(sizeof(*instance), GFP_KERNEL);
1898 
1899 	if (!instance) {
1900 		err = -ENOMEM;
1901 		goto err_shutdown_vchiq;
1902 	}
1903 
1904 	mutex_init(&instance->vchiq_mutex);
1905 
1906 	instance->vchiq_instance = vchiq_instance;
1907 
1908 	mutex_init(&instance->context_map_lock);
1909 	idr_init_base(&instance->context_map, 1);
1910 
1911 	params.userdata = instance;
1912 
1913 	instance->bulk_wq = alloc_ordered_workqueue("mmal-vchiq",
1914 						    WQ_MEM_RECLAIM);
1915 	if (!instance->bulk_wq)
1916 		goto err_free;
1917 
1918 	status = vchiq_open_service(vchiq_instance, &params,
1919 				    &instance->service_handle);
1920 	if (status) {
1921 		pr_err("Failed to open VCHI service connection (status=%d)\n",
1922 		       status);
1923 		goto err_close_services;
1924 	}
1925 
1926 	vchiq_release_service(instance->vchiq_instance, instance->service_handle);
1927 
1928 	*out_instance = instance;
1929 
1930 	return 0;
1931 
1932 err_close_services:
1933 	vchiq_close_service(instance->vchiq_instance, instance->service_handle);
1934 	destroy_workqueue(instance->bulk_wq);
1935 err_free:
1936 	kfree(instance);
1937 err_shutdown_vchiq:
1938 	vchiq_shutdown(vchiq_instance);
1939 	return err;
1940 }
1941 EXPORT_SYMBOL_GPL(vchiq_mmal_init);
1942 
1943 MODULE_DESCRIPTION("BCM2835 MMAL VCHIQ interface");
1944 MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.org>");
1945 MODULE_LICENSE("GPL");
1946