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 *
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(size: sizeof(*msg_context), GFP_KERNEL);
192
193	if (!msg_context)
194	return ERR_PTR(error: -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, ptr: msg_context,
202	start: 0, end: 0, GFP_KERNEL);
203	mutex_unlock(lock: &instance->context_map_lock);
204
205	if (handle < 0) {
206	kfree(objp: msg_context);
207	return ERR_PTR(error: 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 *
217	lookup_msg_context(struct vchiq_mmal_instance *instance, int handle)
218	{
219	return idr_find(&instance->context_map, id: handle);
220	}
221
222	static void
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, id: msg_context->handle);
229	mutex_unlock(lock: &instance->context_map_lock);
230	kfree(objp: msg_context);
231	}
232
233	/* deals with receipt of event to host message */
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	*/
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(v: &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	*/
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: instance->vchiq_instance, service: instance->service_handle);
296	ret = vchiq_bulk_receive(instance: instance->vchiq_instance, service: instance->service_handle,
297	data: msg_context->u.bulk.buffer->buffer,
298	/* Actual receive needs to be a multiple
299	* of 4 bytes
300	*/
301	size: (len + 3) & ~3,
302	userdata: msg_context,
303	mode: VCHIQ_BULK_MODE_CALLBACK);
304
305	vchiq_release_service(instance: instance->vchiq_instance, service: 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 */
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(wq: msg_context->instance->bulk_wq,
352	work: &msg_context->u.bulk.buffer_to_host_work);
353
354	return 0;
355	}
356
357	/* data in message, memcpy from packet into output buffer */
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
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(v: &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: instance->vchiq_instance, service: instance->service_handle);
440
441	ret = vchiq_queue_kernel_message(instance: instance->vchiq_instance, handle: instance->service_handle, data: &m,
442	size: sizeof(struct mmal_msg_header) +
443	sizeof(m.u.buffer_from_host));
444	if (ret)
445	atomic_dec(v: &port->buffers_with_vpu);
446
447	vchiq_release_service(instance: instance->vchiq_instance, service: instance->service_handle);
448
449	return ret;
450	}
451
452	/* deals with receipt of buffer to host message */
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(work: &msg_context->u.bulk.work);
529	}
530
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(work: &msg_context->u.bulk.work);
538	}
539
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(work: &msg_context->u.bulk.work);
548	}
549
550	/* incoming event service callback */
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(instance: vchiq_instance, service: 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(instance: vchiq_instance, service: 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(instance: vchiq_instance, service: 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(instance: vchiq_instance, service: 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(instance: vchiq_instance, service: handle, header);
594	break;
595	}
596
597	msg_context = lookup_msg_context(instance,
598	handle: msg->h.context);
599	if (!msg_context) {
600	pr_err("received invalid message context %u!\n",
601	msg->h.context);
602	vchiq_release_message(instance: vchiq_instance, service: 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, msg_context: bulk_ctx);
629	break;
630
631	case VCHIQ_BULK_RECEIVE_ABORTED:
632	bulk_abort_cb(instance, msg_context: 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
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(ptr: msg_context))
670	return PTR_ERR(ptr: msg_context);
671
672	init_completion(x: &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: instance->vchiq_instance, service: instance->service_handle);
682
683	ret = vchiq_queue_kernel_message(instance: instance->vchiq_instance, handle: instance->service_handle, data: msg,
684	size: sizeof(struct mmal_msg_header) +
685	payload_len);
686
687	vchiq_release_service(instance: instance->vchiq_instance, service: 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(x: &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
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
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
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, p: &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, msg: &m,
809	payload_len: sizeof(m.u.port_info_set),
810	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
828
829	return ret;
830	}
831
832	/* use port info get message to retrieve port information */
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, msg: &m,
848	payload_len: sizeof(m.u.port_info_get),
849	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
923
924	return ret;
925	}
926
927	/* create component on vc */
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, msg: &m,
945	payload_len: sizeof(m.u.component_create),
946	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
972
973	return ret;
974	}
975
976	/* destroys a component on vc */
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, msg: &m,
989	payload_len: sizeof(m.u.component_destroy),
990	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1005
1006	return ret;
1007	}
1008
1009	/* enable a component on vc */
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, msg: &m,
1022	payload_len: sizeof(m.u.component_enable),
1023	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1037
1038	return ret;
1039	}
1040
1041	/* disable a component on vc */
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, msg: &m,
1054	payload_len: sizeof(m.u.component_disable),
1055	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1070
1071	return ret;
1072	}
1073
1074	/* get version of mmal implementation */
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, msg: &m,
1086	payload_len: sizeof(m.u.version),
1087	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1102
1103	return ret;
1104	}
1105
1106	/* do a port action with a port as a parameter */
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, p: &m.u.port_action_port.port);
1122
1123	ret = send_synchronous_mmal_msg(instance, msg: &m,
1124	payload_len: sizeof(m.u.port_action_port),
1125	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1144
1145	return ret;
1146	}
1147
1148	/* do a port action with handles as parameters */
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, msg: &m,
1171	payload_len: sizeof(m.u.port_action_handle),
1172	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1192
1193	return ret;
1194	}
1195
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, msg: &m,
1214	payload_len: (4 * sizeof(u32)) + value_size,
1215	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1233
1234	return ret;
1235	}
1236
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, msg: &m,
1254	payload_len: sizeof(struct
1255	mmal_msg_port_parameter_get),
1256	msg_out: &rmsg, msg_handle: &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: instance->vchiq_instance, service: instance->service_handle, header: rmsg_handle);
1292
1293	return ret;
1294	}
1295
1296	/* disables a port and drains buffers from it */
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	action_type: 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(entry: 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(lock: &port->slock, flags);
1339
1340	ret = port_info_get(instance, port);
1341	}
1342
1343	return ret;
1344	}
1345
1346	/* enable a 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	action_type: 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, buf: mmalbuf);
1373	if (ret)
1374	goto done;
1375
1376	list_del(entry: 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
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(lock: &instance->vchiq_mutex);
1411
1412	return ret;
1413	}
1414	EXPORT_SYMBOL_GPL(vchiq_mmal_port_set_format);
1415
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_id: parameter, value, value_size);
1426
1427	mutex_unlock(lock: &instance->vchiq_mutex);
1428
1429	return ret;
1430	}
1431	EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_set);
1432
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_id: parameter, value, value_size);
1443
1444	mutex_unlock(lock: &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	*/
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(lock: &instance->vchiq_mutex);
1476
1477	return ret;
1478	}
1479	EXPORT_SYMBOL_GPL(vchiq_mmal_port_enable);
1480
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(lock: &instance->vchiq_mutex);
1491	return 0;
1492	}
1493
1494	ret = port_disable(instance, port);
1495
1496	mutex_unlock(lock: &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	*/
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, port: 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, port: src,
1527	action_type: MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT,
1528	connect_component_handle: src->connected->component->handle,
1529	connect_port_handle: 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, port: 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, port: 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, port: src,
1572	action_type: MMAL_MSG_PORT_ACTION_TYPE_CONNECT,
1573	connect_component_handle: dst->component->handle, connect_port_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(lock: &instance->vchiq_mutex);
1585
1586	return ret;
1587	}
1588	EXPORT_SYMBOL_GPL(vchiq_mmal_port_connect_tunnel);
1589
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, buf: 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(new: &buffer->list, head: &port->buffers);
1602	spin_unlock_irqrestore(lock: &port->slock, flags);
1603	}
1604
1605	return 0;
1606	}
1607	EXPORT_SYMBOL_GPL(vchiq_mmal_submit_buffer);
1608
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(ptr: msg_context))
1615	return (PTR_ERR(ptr: msg_context));
1616
1617	buf->msg_context = msg_context;
1618	return 0;
1619	}
1620	EXPORT_SYMBOL_GPL(mmal_vchi_buffer_init);
1621
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	*/
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(list: &component->control.buffers);
1680	ret = port_info_get(instance, port: &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(list: &component->input[idx].buffers);
1690	ret = port_info_get(instance, port: &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(list: &component->output[idx].buffers);
1701	ret = port_info_get(instance, port: &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(list: &component->clock[idx].buffers);
1712	ret = port_info_get(instance, port: &component->clock[idx]);
1713	if (ret < 0)
1714	goto release_component;
1715	}
1716
1717	*component_out = component;
1718
1719	mutex_unlock(lock: &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(lock: &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	*/
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(lock: &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	*/
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(lock: &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(lock: &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	*/
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(lock: &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(lock: &instance->vchiq_mutex);
1805
1806	return ret;
1807	}
1808	EXPORT_SYMBOL_GPL(vchiq_mmal_component_disable);
1809
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(lock: &instance->vchiq_mutex);
1821
1822	return ret;
1823	}
1824	EXPORT_SYMBOL_GPL(vchiq_mmal_version);
1825
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: instance->vchiq_instance, service: instance->service_handle);
1837
1838	status = vchiq_close_service(instance: instance->vchiq_instance, service: instance->service_handle);
1839	if (status != 0)
1840	pr_err("mmal-vchiq: VCHIQ close failed\n");
1841
1842	mutex_unlock(lock: &instance->vchiq_mutex);
1843
1844	vchiq_shutdown(instance: instance->vchiq_instance);
1845	destroy_workqueue(wq: instance->bulk_wq);
1846
1847	idr_destroy(&instance->context_map);
1848
1849	kfree(objp: instance);
1850
1851	return status;
1852	}
1853	EXPORT_SYMBOL_GPL(vchiq_mmal_finalise);
1854
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(pinstance: &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(instance: 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(size: 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(idr: &instance->context_map, base: 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(instance: vchiq_instance, params: &params,
1919	pservice: &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: instance->vchiq_instance, service: instance->service_handle);
1927
1928	*out_instance = instance;
1929
1930	return 0;
1931
1932	err_close_services:
1933	vchiq_close_service(instance: instance->vchiq_instance, service: instance->service_handle);
1934	destroy_workqueue(wq: instance->bulk_wq);
1935	err_free:
1936	kfree(objp: instance);
1937	err_shutdown_vchiq:
1938	vchiq_shutdown(instance: 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