xref: /freebsd/sys/contrib/vchiq/interface/vchiq_arm/vchiq_arm.c (revision 7d8f797b725e3efc0a4256554654780df83c456c)
1 /**
2  * Copyright (c) 2010-2012 Broadcom. All rights reserved.
3  *
4  * Redistribution and use in source and binary forms, with or without
5  * modification, are permitted provided that the following conditions
6  * are met:
7  * 1. Redistributions of source code must retain the above copyright
8  *    notice, this list of conditions, and the following disclaimer,
9  *    without modification.
10  * 2. Redistributions in binary form must reproduce the above copyright
11  *    notice, this list of conditions and the following disclaimer in the
12  *    documentation and/or other materials provided with the distribution.
13  * 3. The names of the above-listed copyright holders may not be used
14  *    to endorse or promote products derived from this software without
15  *    specific prior written permission.
16  *
17  * ALTERNATIVELY, this software may be distributed under the terms of the
18  * GNU General Public License ("GPL") version 2, as published by the Free
19  * Software Foundation.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
22  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
23  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
24  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
25  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
26  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
27  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
28  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
29  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
30  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
31  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
32  */
33 
34 
35 #include "vchiq_core.h"
36 #include "vchiq_ioctl.h"
37 #include "vchiq_arm.h"
38 
39 #define DEVICE_NAME "vchiq"
40 
41 /* Override the default prefix, which would be vchiq_arm (from the filename) */
42 #undef MODULE_PARAM_PREFIX
43 #define MODULE_PARAM_PREFIX DEVICE_NAME "."
44 
45 #define VCHIQ_MINOR 0
46 
47 /* Some per-instance constants */
48 #define MAX_COMPLETIONS 16
49 #define MAX_SERVICES 64
50 #define MAX_ELEMENTS 8
51 #define MSG_QUEUE_SIZE 64
52 
53 #define KEEPALIVE_VER 1
54 #define KEEPALIVE_VER_MIN KEEPALIVE_VER
55 
56 MALLOC_DEFINE(M_VCHIQ, "vchiq_cdev", "VideoCore cdev memroy");
57 
58 /* Run time control of log level, based on KERN_XXX level. */
59 int vchiq_arm_log_level = VCHIQ_LOG_DEFAULT;
60 int vchiq_susp_log_level = VCHIQ_LOG_ERROR;
61 
62 #define SUSPEND_TIMER_TIMEOUT_MS 100
63 #define SUSPEND_RETRY_TIMER_TIMEOUT_MS 1000
64 
65 #define VC_SUSPEND_NUM_OFFSET 3 /* number of values before idle which are -ve */
66 static const char *const suspend_state_names[] = {
67 	"VC_SUSPEND_FORCE_CANCELED",
68 	"VC_SUSPEND_REJECTED",
69 	"VC_SUSPEND_FAILED",
70 	"VC_SUSPEND_IDLE",
71 	"VC_SUSPEND_REQUESTED",
72 	"VC_SUSPEND_IN_PROGRESS",
73 	"VC_SUSPEND_SUSPENDED"
74 };
75 #define VC_RESUME_NUM_OFFSET 1 /* number of values before idle which are -ve */
76 static const char *const resume_state_names[] = {
77 	"VC_RESUME_FAILED",
78 	"VC_RESUME_IDLE",
79 	"VC_RESUME_REQUESTED",
80 	"VC_RESUME_IN_PROGRESS",
81 	"VC_RESUME_RESUMED"
82 };
83 /* The number of times we allow force suspend to timeout before actually
84 ** _forcing_ suspend.  This is to cater for SW which fails to release vchiq
85 ** correctly - we don't want to prevent ARM suspend indefinitely in this case.
86 */
87 #define FORCE_SUSPEND_FAIL_MAX 8
88 
89 /* The time in ms allowed for videocore to go idle when force suspend has been
90  * requested */
91 #define FORCE_SUSPEND_TIMEOUT_MS 200
92 
93 
94 static void suspend_timer_callback(unsigned long context);
95 #ifdef notyet
96 static int vchiq_proc_add_instance(VCHIQ_INSTANCE_T instance);
97 static void vchiq_proc_remove_instance(VCHIQ_INSTANCE_T instance);
98 #endif
99 
100 
101 typedef struct user_service_struct {
102 	VCHIQ_SERVICE_T *service;
103 	void *userdata;
104 	VCHIQ_INSTANCE_T instance;
105 	int is_vchi;
106 	int dequeue_pending;
107 	int message_available_pos;
108 	int msg_insert;
109 	int msg_remove;
110 	struct semaphore insert_event;
111 	struct semaphore remove_event;
112 	VCHIQ_HEADER_T * msg_queue[MSG_QUEUE_SIZE];
113 } USER_SERVICE_T;
114 
115 struct bulk_waiter_node {
116 	struct bulk_waiter bulk_waiter;
117 	int pid;
118 	struct list_head list;
119 };
120 
121 struct vchiq_instance_struct {
122 	VCHIQ_STATE_T *state;
123 	VCHIQ_COMPLETION_DATA_T completions[MAX_COMPLETIONS];
124 	int completion_insert;
125 	int completion_remove;
126 	struct semaphore insert_event;
127 	struct semaphore remove_event;
128 	struct mutex completion_mutex;
129 
130 	int connected;
131 	int closing;
132 	int pid;
133 	int mark;
134 
135 	struct list_head bulk_waiter_list;
136 	struct mutex bulk_waiter_list_mutex;
137 
138 	struct proc_dir_entry *proc_entry;
139 };
140 
141 typedef struct dump_context_struct {
142 	char __user *buf;
143 	size_t actual;
144 	size_t space;
145 	loff_t offset;
146 } DUMP_CONTEXT_T;
147 
148 static struct cdev *  vchiq_cdev;
149 VCHIQ_STATE_T g_state;
150 static DEFINE_SPINLOCK(msg_queue_spinlock);
151 
152 static const char *const ioctl_names[] = {
153 	"CONNECT",
154 	"SHUTDOWN",
155 	"CREATE_SERVICE",
156 	"REMOVE_SERVICE",
157 	"QUEUE_MESSAGE",
158 	"QUEUE_BULK_TRANSMIT",
159 	"QUEUE_BULK_RECEIVE",
160 	"AWAIT_COMPLETION",
161 	"DEQUEUE_MESSAGE",
162 	"GET_CLIENT_ID",
163 	"GET_CONFIG",
164 	"CLOSE_SERVICE",
165 	"USE_SERVICE",
166 	"RELEASE_SERVICE",
167 	"SET_SERVICE_OPTION",
168 	"DUMP_PHYS_MEM"
169 };
170 
171 vchiq_static_assert((sizeof(ioctl_names)/sizeof(ioctl_names[0])) ==
172 	(VCHIQ_IOC_MAX + 1));
173 
174 static eventhandler_tag vchiq_ehtag = NULL;
175 static d_open_t		vchiq_open;
176 static d_close_t	vchiq_close;
177 static d_ioctl_t	vchiq_ioctl;
178 
179 static struct cdevsw vchiq_cdevsw = {
180 	.d_version	= D_VERSION,
181 	.d_ioctl	= vchiq_ioctl,
182 	.d_open		= vchiq_open,
183 	.d_close	= vchiq_close,
184 	.d_name		= DEVICE_NAME,
185 };
186 
187 #if 0
188 static void
189 dump_phys_mem(void *virt_addr, uint32_t num_bytes);
190 #endif
191 
192 /****************************************************************************
193 *
194 *   add_completion
195 *
196 ***************************************************************************/
197 
198 static VCHIQ_STATUS_T
199 add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason,
200 	VCHIQ_HEADER_T *header, USER_SERVICE_T *user_service,
201 	void *bulk_userdata)
202 {
203 	VCHIQ_COMPLETION_DATA_T *completion;
204 	DEBUG_INITIALISE(g_state.local)
205 
206 	while (instance->completion_insert ==
207 		(instance->completion_remove + MAX_COMPLETIONS)) {
208 		/* Out of space - wait for the client */
209 		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
210 		vchiq_log_trace(vchiq_arm_log_level,
211 			"add_completion - completion queue full");
212 		DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT);
213 		if (down_interruptible(&instance->remove_event) != 0) {
214 			vchiq_log_info(vchiq_arm_log_level,
215 				"service_callback interrupted");
216 			return VCHIQ_RETRY;
217 		} else if (instance->closing) {
218 			vchiq_log_info(vchiq_arm_log_level,
219 				"service_callback closing");
220 			return VCHIQ_ERROR;
221 		}
222 		DEBUG_TRACE(SERVICE_CALLBACK_LINE);
223 	}
224 
225 	completion =
226 		 &instance->completions[instance->completion_insert &
227 		 (MAX_COMPLETIONS - 1)];
228 
229 	completion->header = header;
230 	completion->reason = reason;
231 	/* N.B. service_userdata is updated while processing AWAIT_COMPLETION */
232 	completion->service_userdata = user_service->service;
233 	completion->bulk_userdata = bulk_userdata;
234 
235 	if (reason == VCHIQ_SERVICE_CLOSED)
236 		/* Take an extra reference, to be held until
237 		   this CLOSED notification is delivered. */
238 		lock_service(user_service->service);
239 
240 	/* A write barrier is needed here to ensure that the entire completion
241 		record is written out before the insert point. */
242 	wmb();
243 
244 	if (reason == VCHIQ_MESSAGE_AVAILABLE)
245 		user_service->message_available_pos =
246 			instance->completion_insert;
247 	instance->completion_insert++;
248 
249 	up(&instance->insert_event);
250 
251 	return VCHIQ_SUCCESS;
252 }
253 
254 /****************************************************************************
255 *
256 *   service_callback
257 *
258 ***************************************************************************/
259 
260 static VCHIQ_STATUS_T
261 service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
262 	VCHIQ_SERVICE_HANDLE_T handle, void *bulk_userdata)
263 {
264 	/* How do we ensure the callback goes to the right client?
265 	** The service_user data points to a USER_SERVICE_T record containing
266 	** the original callback and the user state structure, which contains a
267 	** circular buffer for completion records.
268 	*/
269 	USER_SERVICE_T *user_service;
270 	VCHIQ_SERVICE_T *service;
271 	VCHIQ_INSTANCE_T instance;
272 	DEBUG_INITIALISE(g_state.local)
273 
274 	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
275 
276 	service = handle_to_service(handle);
277 	BUG_ON(!service);
278 	user_service = (USER_SERVICE_T *)service->base.userdata;
279 	instance = user_service->instance;
280 
281 	if (!instance || instance->closing)
282 		return VCHIQ_SUCCESS;
283 
284 	vchiq_log_trace(vchiq_arm_log_level,
285 		"service_callback - service %lx(%d), handle %x, reason %d, header %lx, "
286 		"instance %lx, bulk_userdata %lx",
287 		(unsigned long)user_service,
288 		service->localport, service->handle,
289 		reason, (unsigned long)header,
290 		(unsigned long)instance, (unsigned long)bulk_userdata);
291 
292 	if (header && user_service->is_vchi) {
293 		spin_lock(&msg_queue_spinlock);
294 		while (user_service->msg_insert ==
295 			(user_service->msg_remove + MSG_QUEUE_SIZE)) {
296 			spin_unlock(&msg_queue_spinlock);
297 			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
298 			DEBUG_COUNT(MSG_QUEUE_FULL_COUNT);
299 			vchiq_log_trace(vchiq_arm_log_level,
300 				"service_callback - msg queue full");
301 			/* If there is no MESSAGE_AVAILABLE in the completion
302 			** queue, add one
303 			*/
304 			if ((user_service->message_available_pos -
305 				instance->completion_remove) < 0) {
306 				VCHIQ_STATUS_T status;
307 				vchiq_log_info(vchiq_arm_log_level,
308 					"Inserting extra MESSAGE_AVAILABLE");
309 				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
310 				status = add_completion(instance, reason,
311 					NULL, user_service, bulk_userdata);
312 				if (status != VCHIQ_SUCCESS) {
313 					DEBUG_TRACE(SERVICE_CALLBACK_LINE);
314 					return status;
315 				}
316 			}
317 
318 			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
319 			if (down_interruptible(&user_service->remove_event)
320 				!= 0) {
321 				vchiq_log_info(vchiq_arm_log_level,
322 					"service_callback interrupted");
323 				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
324 				return VCHIQ_RETRY;
325 			} else if (instance->closing) {
326 				vchiq_log_info(vchiq_arm_log_level,
327 					"service_callback closing");
328 				DEBUG_TRACE(SERVICE_CALLBACK_LINE);
329 				return VCHIQ_ERROR;
330 			}
331 			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
332 			spin_lock(&msg_queue_spinlock);
333 		}
334 
335 		user_service->msg_queue[user_service->msg_insert &
336 			(MSG_QUEUE_SIZE - 1)] = header;
337 		user_service->msg_insert++;
338 		spin_unlock(&msg_queue_spinlock);
339 
340 		up(&user_service->insert_event);
341 
342 		/* If there is a thread waiting in DEQUEUE_MESSAGE, or if
343 		** there is a MESSAGE_AVAILABLE in the completion queue then
344 		** bypass the completion queue.
345 		*/
346 		if (((user_service->message_available_pos -
347 			instance->completion_remove) >= 0) ||
348 			user_service->dequeue_pending) {
349 			DEBUG_TRACE(SERVICE_CALLBACK_LINE);
350 			user_service->dequeue_pending = 0;
351 			return VCHIQ_SUCCESS;
352 		}
353 
354 		header = NULL;
355 	}
356 	DEBUG_TRACE(SERVICE_CALLBACK_LINE);
357 
358 	return add_completion(instance, reason, header, user_service,
359 		bulk_userdata);
360 }
361 
362 /****************************************************************************
363 *
364 *   user_service_free
365 *
366 ***************************************************************************/
367 static void
368 user_service_free(void *userdata)
369 {
370 	USER_SERVICE_T *user_service = userdata;
371 
372 	_sema_destroy(&user_service->insert_event);
373 	_sema_destroy(&user_service->remove_event);
374 
375 	kfree(user_service);
376 }
377 
378 /****************************************************************************
379 *
380 *   vchiq_ioctl
381 *
382 ***************************************************************************/
383 
384 static int
385 vchiq_ioctl(struct cdev *cdev, u_long cmd, caddr_t arg, int fflag,
386    struct thread *td)
387 {
388 	VCHIQ_INSTANCE_T instance;
389 	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
390 	VCHIQ_SERVICE_T *service = NULL;
391 	int ret = 0;
392 	int i, rc;
393 	DEBUG_INITIALISE(g_state.local)
394 
395 	if ((ret = devfs_get_cdevpriv((void**)&instance))) {
396 		printf("vchiq_ioctl: devfs_get_cdevpriv failed: error %d\n", ret);
397 		return (ret);
398 	}
399 
400 /* XXXBSD: HACK! */
401 #define _IOC_NR(x) ((x) & 0xff)
402 #define	_IOC_TYPE(x)	IOCGROUP(x)
403 
404 	vchiq_log_trace(vchiq_arm_log_level,
405 		 "vchiq_ioctl - instance %x, cmd %s, arg %p",
406 		(unsigned int)instance,
407 		((_IOC_TYPE(cmd) == VCHIQ_IOC_MAGIC) &&
408 		(_IOC_NR(cmd) <= VCHIQ_IOC_MAX)) ?
409 		ioctl_names[_IOC_NR(cmd)] : "<invalid>", arg);
410 
411 	switch (cmd) {
412 	case VCHIQ_IOC_SHUTDOWN:
413 		if (!instance->connected)
414 			break;
415 
416 		/* Remove all services */
417 		i = 0;
418 		while ((service = next_service_by_instance(instance->state,
419 			instance, &i)) != NULL) {
420 			status = vchiq_remove_service(service->handle);
421 			unlock_service(service);
422 			if (status != VCHIQ_SUCCESS)
423 				break;
424 		}
425 		service = NULL;
426 
427 		if (status == VCHIQ_SUCCESS) {
428 			/* Wake the completion thread and ask it to exit */
429 			instance->closing = 1;
430 			up(&instance->insert_event);
431 		}
432 
433 		break;
434 
435 	case VCHIQ_IOC_CONNECT:
436 		if (instance->connected) {
437 			ret = -EINVAL;
438 			break;
439 		}
440 		rc = lmutex_lock_interruptible(&instance->state->mutex);
441 		if (rc != 0) {
442 			vchiq_log_error(vchiq_arm_log_level,
443 				"vchiq: connect: could not lock mutex for "
444 				"state %d: %d",
445 				instance->state->id, rc);
446 			ret = -EINTR;
447 			break;
448 		}
449 		status = vchiq_connect_internal(instance->state, instance);
450 		lmutex_unlock(&instance->state->mutex);
451 
452 		if (status == VCHIQ_SUCCESS)
453 			instance->connected = 1;
454 		else
455 			vchiq_log_error(vchiq_arm_log_level,
456 				"vchiq: could not connect: %d", status);
457 		break;
458 
459 	case VCHIQ_IOC_CREATE_SERVICE: {
460 		VCHIQ_CREATE_SERVICE_T args;
461 		USER_SERVICE_T *user_service = NULL;
462 		void *userdata;
463 		int srvstate;
464 
465 		memcpy(&args, (const void*)arg, sizeof(args));
466 
467 		user_service = kmalloc(sizeof(USER_SERVICE_T), GFP_KERNEL);
468 		if (!user_service) {
469 			ret = -ENOMEM;
470 			break;
471 		}
472 
473 		if (args.is_open) {
474 			if (!instance->connected) {
475 				ret = -ENOTCONN;
476 				kfree(user_service);
477 				break;
478 			}
479 			srvstate = VCHIQ_SRVSTATE_OPENING;
480 		} else {
481 			srvstate =
482 				 instance->connected ?
483 				 VCHIQ_SRVSTATE_LISTENING :
484 				 VCHIQ_SRVSTATE_HIDDEN;
485 		}
486 
487 		userdata = args.params.userdata;
488 		args.params.callback = service_callback;
489 		args.params.userdata = user_service;
490 		service = vchiq_add_service_internal(
491 				instance->state,
492 				&args.params, srvstate,
493 				instance, user_service_free);
494 
495 		if (service != NULL) {
496 			user_service->service = service;
497 			user_service->userdata = userdata;
498 			user_service->instance = instance;
499 			user_service->is_vchi = args.is_vchi;
500 			user_service->dequeue_pending = 0;
501 			user_service->message_available_pos =
502 				instance->completion_remove - 1;
503 			user_service->msg_insert = 0;
504 			user_service->msg_remove = 0;
505 			_sema_init(&user_service->insert_event, 0);
506 			_sema_init(&user_service->remove_event, 0);
507 
508 			if (args.is_open) {
509 				status = vchiq_open_service_internal
510 					(service, instance->pid);
511 				if (status != VCHIQ_SUCCESS) {
512 					vchiq_remove_service(service->handle);
513 					service = NULL;
514 					ret = (status == VCHIQ_RETRY) ?
515 						-EINTR : -EIO;
516 					break;
517 				}
518 			}
519 
520 #ifdef VCHIQ_IOCTL_DEBUG
521 			printf("%s: [CREATE SERVICE] handle = %08x\n", __func__, service->handle);
522 #endif
523 			memcpy((void *)
524 				&(((VCHIQ_CREATE_SERVICE_T*)
525 					arg)->handle),
526 				(const void *)&service->handle,
527 				sizeof(service->handle));
528 
529 			service = NULL;
530 		} else {
531 			ret = -EEXIST;
532 			kfree(user_service);
533 		}
534 	} break;
535 
536 	case VCHIQ_IOC_CLOSE_SERVICE: {
537 		VCHIQ_SERVICE_HANDLE_T handle;
538 
539 		memcpy(&handle, (const void*)arg, sizeof(handle));
540 
541 #ifdef VCHIQ_IOCTL_DEBUG
542 		printf("%s: [CLOSE SERVICE] handle = %08x\n", __func__, handle);
543 #endif
544 
545 		service = find_service_for_instance(instance, handle);
546 		if (service != NULL)
547 			status = vchiq_close_service(service->handle);
548 		else
549 			ret = -EINVAL;
550 	} break;
551 
552 	case VCHIQ_IOC_REMOVE_SERVICE: {
553 		VCHIQ_SERVICE_HANDLE_T handle;
554 
555 		memcpy(&handle, (const void*)arg, sizeof(handle));
556 
557 #ifdef VCHIQ_IOCTL_DEBUG
558 		printf("%s: [REMOVE SERVICE] handle = %08x\n", __func__, handle);
559 #endif
560 
561 		service = find_service_for_instance(instance, handle);
562 		if (service != NULL)
563 			status = vchiq_remove_service(service->handle);
564 		else
565 			ret = -EINVAL;
566 	} break;
567 
568 	case VCHIQ_IOC_USE_SERVICE:
569 	case VCHIQ_IOC_RELEASE_SERVICE:	{
570 		VCHIQ_SERVICE_HANDLE_T handle;
571 
572 		memcpy(&handle, (const void*)arg, sizeof(handle));
573 
574 #ifdef VCHIQ_IOCTL_DEBUG
575 		printf("%s: [%s SERVICE] handle = %08x\n", __func__,
576 		    cmd == VCHIQ_IOC_USE_SERVICE ? "USE" : "RELEASE", handle);
577 #endif
578 
579 		service = find_service_for_instance(instance, handle);
580 		if (service != NULL) {
581 			status = (cmd == VCHIQ_IOC_USE_SERVICE)	?
582 				vchiq_use_service_internal(service) :
583 				vchiq_release_service_internal(service);
584 			if (status != VCHIQ_SUCCESS) {
585 				vchiq_log_error(vchiq_susp_log_level,
586 					"%s: cmd %s returned error %d for "
587 					"service %c%c%c%c:%8x",
588 					__func__,
589 					(cmd == VCHIQ_IOC_USE_SERVICE) ?
590 						"VCHIQ_IOC_USE_SERVICE" :
591 						"VCHIQ_IOC_RELEASE_SERVICE",
592 					status,
593 					VCHIQ_FOURCC_AS_4CHARS(
594 						service->base.fourcc),
595 					service->client_id);
596 				ret = -EINVAL;
597 			}
598 		} else
599 			ret = -EINVAL;
600 	} break;
601 
602 	case VCHIQ_IOC_QUEUE_MESSAGE: {
603 		VCHIQ_QUEUE_MESSAGE_T args;
604 		memcpy(&args, (const void*)arg, sizeof(args));
605 
606 #ifdef VCHIQ_IOCTL_DEBUG
607 		printf("%s: [QUEUE MESSAGE] handle = %08x\n", __func__, args.handle);
608 #endif
609 
610 		service = find_service_for_instance(instance, args.handle);
611 
612 		if ((service != NULL) && (args.count <= MAX_ELEMENTS)) {
613 			/* Copy elements into kernel space */
614 			VCHIQ_ELEMENT_T elements[MAX_ELEMENTS];
615 			if (copy_from_user(elements, args.elements,
616 				args.count * sizeof(VCHIQ_ELEMENT_T)) == 0)
617 				status = vchiq_queue_message
618 					(args.handle,
619 					elements, args.count);
620 			else
621 				ret = -EFAULT;
622 		} else {
623 			ret = -EINVAL;
624 		}
625 	} break;
626 
627 	case VCHIQ_IOC_QUEUE_BULK_TRANSMIT:
628 	case VCHIQ_IOC_QUEUE_BULK_RECEIVE: {
629 		VCHIQ_QUEUE_BULK_TRANSFER_T args;
630 		struct bulk_waiter_node *waiter = NULL;
631 		VCHIQ_BULK_DIR_T dir =
632 			(cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ?
633 			VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
634 
635 		memcpy(&args, (const void*)arg, sizeof(args));
636 
637 		service = find_service_for_instance(instance, args.handle);
638 		if (!service) {
639 			ret = -EINVAL;
640 			break;
641 		}
642 
643 		if (args.mode == VCHIQ_BULK_MODE_BLOCKING) {
644 			waiter = kzalloc(sizeof(struct bulk_waiter_node),
645 				GFP_KERNEL);
646 			if (!waiter) {
647 				ret = -ENOMEM;
648 				break;
649 			}
650 			args.userdata = &waiter->bulk_waiter;
651 		} else if (args.mode == VCHIQ_BULK_MODE_WAITING) {
652 			struct list_head *pos;
653 			lmutex_lock(&instance->bulk_waiter_list_mutex);
654 			list_for_each(pos, &instance->bulk_waiter_list) {
655 				if (list_entry(pos, struct bulk_waiter_node,
656 					list)->pid == current->p_pid) {
657 					waiter = list_entry(pos,
658 						struct bulk_waiter_node,
659 						list);
660 					list_del(pos);
661 					break;
662 				}
663 
664 			}
665 			lmutex_unlock(&instance->bulk_waiter_list_mutex);
666 			if (!waiter) {
667 				vchiq_log_error(vchiq_arm_log_level,
668 					"no bulk_waiter found for pid %d",
669 					current->p_pid);
670 				ret = -ESRCH;
671 				break;
672 			}
673 			vchiq_log_info(vchiq_arm_log_level,
674 				"found bulk_waiter %x for pid %d",
675 				(unsigned int)waiter, current->p_pid);
676 			args.userdata = &waiter->bulk_waiter;
677 		}
678 		status = vchiq_bulk_transfer
679 			(args.handle,
680 			 VCHI_MEM_HANDLE_INVALID,
681 			 args.data, args.size,
682 			 args.userdata, args.mode,
683 			 dir);
684 		if (!waiter)
685 			break;
686 		if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) ||
687 			!waiter->bulk_waiter.bulk) {
688 			if (waiter->bulk_waiter.bulk) {
689 				/* Cancel the signal when the transfer
690 				** completes. */
691 				spin_lock(&bulk_waiter_spinlock);
692 				waiter->bulk_waiter.bulk->userdata = NULL;
693 				spin_unlock(&bulk_waiter_spinlock);
694 			}
695 			_sema_destroy(&waiter->bulk_waiter.event);
696 			kfree(waiter);
697 		} else {
698 			const VCHIQ_BULK_MODE_T mode_waiting =
699 				VCHIQ_BULK_MODE_WAITING;
700 			waiter->pid = current->p_pid;
701 			lmutex_lock(&instance->bulk_waiter_list_mutex);
702 			list_add(&waiter->list, &instance->bulk_waiter_list);
703 			lmutex_unlock(&instance->bulk_waiter_list_mutex);
704 			vchiq_log_info(vchiq_arm_log_level,
705 				"saved bulk_waiter %x for pid %d",
706 				(unsigned int)waiter, current->p_pid);
707 
708 			memcpy((void *)
709 				&(((VCHIQ_QUEUE_BULK_TRANSFER_T *)
710 					arg)->mode),
711 				(const void *)&mode_waiting,
712 				sizeof(mode_waiting));
713 		}
714 	} break;
715 
716 	case VCHIQ_IOC_AWAIT_COMPLETION: {
717 		VCHIQ_AWAIT_COMPLETION_T args;
718 		int count = 0;
719 
720 		DEBUG_TRACE(AWAIT_COMPLETION_LINE);
721 		if (!instance->connected) {
722 			ret = -ENOTCONN;
723 			break;
724 		}
725 
726 		memcpy(&args, (const void*)arg, sizeof(args));
727 
728 		lmutex_lock(&instance->completion_mutex);
729 
730 		DEBUG_TRACE(AWAIT_COMPLETION_LINE);
731 		while ((instance->completion_remove ==
732 			instance->completion_insert)
733 			&& !instance->closing) {
734 			DEBUG_TRACE(AWAIT_COMPLETION_LINE);
735 			lmutex_unlock(&instance->completion_mutex);
736 			rc = down_interruptible(&instance->insert_event);
737 			lmutex_lock(&instance->completion_mutex);
738 			if (rc != 0) {
739 				DEBUG_TRACE(AWAIT_COMPLETION_LINE);
740 				vchiq_log_info(vchiq_arm_log_level,
741 					"AWAIT_COMPLETION interrupted");
742 				ret = -EINTR;
743 				break;
744 			}
745 		}
746 		DEBUG_TRACE(AWAIT_COMPLETION_LINE);
747 
748 		/* A read memory barrier is needed to stop prefetch of a stale
749 		** completion record
750 		*/
751 		rmb();
752 
753 		if (ret == 0) {
754 			int msgbufcount = args.msgbufcount;
755 			for (count = 0; count < args.count; count++) {
756 				VCHIQ_COMPLETION_DATA_T *completion;
757 				VCHIQ_SERVICE_T *service1;
758 				USER_SERVICE_T *user_service;
759 				VCHIQ_HEADER_T *header;
760 				if (instance->completion_remove ==
761 					instance->completion_insert)
762 					break;
763 				completion = &instance->completions[
764 					instance->completion_remove &
765 					(MAX_COMPLETIONS - 1)];
766 
767 				service1 = completion->service_userdata;
768 				user_service = service1->base.userdata;
769 				completion->service_userdata =
770 					user_service->userdata;
771 
772 				header = completion->header;
773 				if (header) {
774 					void __user *msgbuf;
775 					int msglen;
776 
777 					msglen = header->size +
778 						sizeof(VCHIQ_HEADER_T);
779 					/* This must be a VCHIQ-style service */
780 					if (args.msgbufsize < msglen) {
781 						vchiq_log_error(
782 							vchiq_arm_log_level,
783 							"header %x: msgbufsize"
784 							" %x < msglen %x",
785 							(unsigned int)header,
786 							args.msgbufsize,
787 							msglen);
788 						WARN(1, "invalid message "
789 							"size\n");
790 						if (count == 0)
791 							ret = -EMSGSIZE;
792 						break;
793 					}
794 					if (msgbufcount <= 0)
795 						/* Stall here for lack of a
796 						** buffer for the message. */
797 						break;
798 					/* Get the pointer from user space */
799 					msgbufcount--;
800 					if (copy_from_user(&msgbuf,
801 						(const void __user *)
802 						&args.msgbufs[msgbufcount],
803 						sizeof(msgbuf)) != 0) {
804 						if (count == 0)
805 							ret = -EFAULT;
806 						break;
807 					}
808 
809 					/* Copy the message to user space */
810 					if (copy_to_user(msgbuf, header,
811 						msglen) != 0) {
812 						if (count == 0)
813 							ret = -EFAULT;
814 						break;
815 					}
816 
817 					/* Now it has been copied, the message
818 					** can be released. */
819 					vchiq_release_message(service1->handle,
820 						header);
821 
822 					/* The completion must point to the
823 					** msgbuf. */
824 					completion->header = msgbuf;
825 				}
826 
827 				if (completion->reason ==
828 					VCHIQ_SERVICE_CLOSED)
829 					unlock_service(service1);
830 
831 				if (copy_to_user((void __user *)(
832 					(size_t)args.buf +
833 					count * sizeof(VCHIQ_COMPLETION_DATA_T)),
834 					completion,
835 					sizeof(VCHIQ_COMPLETION_DATA_T)) != 0) {
836 						if (ret == 0)
837 							ret = -EFAULT;
838 					break;
839 				}
840 
841 				instance->completion_remove++;
842 			}
843 
844 			if (msgbufcount != args.msgbufcount) {
845 				memcpy((void __user *)
846 					&((VCHIQ_AWAIT_COMPLETION_T *)arg)->
847 						msgbufcount,
848 					&msgbufcount,
849 					sizeof(msgbufcount));
850 			}
851 
852 			 if (count != args.count)
853 			 {
854 				memcpy((void __user *)
855 					&((VCHIQ_AWAIT_COMPLETION_T *)arg)->count,
856 					&count, sizeof(count));
857 			}
858 		}
859 
860 		if (count != 0)
861 			up(&instance->remove_event);
862 
863 		if ((ret == 0) && instance->closing)
864 			ret = -ENOTCONN;
865 		/*
866 		 * XXXBSD: ioctl return codes are not negative as in linux, so
867 		 * we can not indicate success with positive number of passed
868 		 * messages
869 		 */
870 		if (ret > 0)
871 			ret = 0;
872 
873 		lmutex_unlock(&instance->completion_mutex);
874 		DEBUG_TRACE(AWAIT_COMPLETION_LINE);
875 	} break;
876 
877 	case VCHIQ_IOC_DEQUEUE_MESSAGE: {
878 		VCHIQ_DEQUEUE_MESSAGE_T args;
879 		USER_SERVICE_T *user_service;
880 		VCHIQ_HEADER_T *header;
881 
882 		DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
883 		memcpy(&args, (const void*)arg, sizeof(args));
884 		service = find_service_for_instance(instance, args.handle);
885 		if (!service) {
886 			ret = -EINVAL;
887 			break;
888 		}
889 		user_service = (USER_SERVICE_T *)service->base.userdata;
890 		if (user_service->is_vchi == 0) {
891 			ret = -EINVAL;
892 			break;
893 		}
894 
895 		spin_lock(&msg_queue_spinlock);
896 		if (user_service->msg_remove == user_service->msg_insert) {
897 			if (!args.blocking) {
898 				spin_unlock(&msg_queue_spinlock);
899 				DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
900 				ret = -EWOULDBLOCK;
901 				break;
902 			}
903 			user_service->dequeue_pending = 1;
904 			do {
905 				spin_unlock(&msg_queue_spinlock);
906 				DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
907 				if (down_interruptible(
908 					&user_service->insert_event) != 0) {
909 					vchiq_log_info(vchiq_arm_log_level,
910 						"DEQUEUE_MESSAGE interrupted");
911 					ret = -EINTR;
912 					break;
913 				}
914 				spin_lock(&msg_queue_spinlock);
915 			} while (user_service->msg_remove ==
916 				user_service->msg_insert);
917 
918 			if (ret)
919 				break;
920 		}
921 
922 		BUG_ON((int)(user_service->msg_insert -
923 			user_service->msg_remove) < 0);
924 
925 		header = user_service->msg_queue[user_service->msg_remove &
926 			(MSG_QUEUE_SIZE - 1)];
927 		user_service->msg_remove++;
928 		spin_unlock(&msg_queue_spinlock);
929 
930 		up(&user_service->remove_event);
931 		if (header == NULL)
932 			ret = -ENOTCONN;
933 		else if (header->size <= args.bufsize) {
934 			/* Copy to user space if msgbuf is not NULL */
935 			if ((args.buf == NULL) ||
936 				(copy_to_user((void __user *)args.buf,
937 				header->data,
938 				header->size) == 0)) {
939 				args.bufsize = header->size;
940 				memcpy((void *)arg, &args,
941 				    sizeof(args));
942 				vchiq_release_message(
943 					service->handle,
944 					header);
945 			} else
946 				ret = -EFAULT;
947 		} else {
948 			vchiq_log_error(vchiq_arm_log_level,
949 				"header %x: bufsize %x < size %x",
950 				(unsigned int)header, args.bufsize,
951 				header->size);
952 			WARN(1, "invalid size\n");
953 			ret = -EMSGSIZE;
954 		}
955 		DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
956 	} break;
957 
958 	case VCHIQ_IOC_GET_CLIENT_ID: {
959 		VCHIQ_SERVICE_HANDLE_T handle;
960 
961 		memcpy(&handle, (const void*)arg, sizeof(handle));
962 
963 		ret = vchiq_get_client_id(handle);
964 	} break;
965 
966 	case VCHIQ_IOC_GET_CONFIG: {
967 		VCHIQ_GET_CONFIG_T args;
968 		VCHIQ_CONFIG_T config;
969 
970 		memcpy(&args, (const void*)arg, sizeof(args));
971 		if (args.config_size > sizeof(config)) {
972 			ret = -EINVAL;
973 			break;
974 		}
975 		status = vchiq_get_config(instance, args.config_size, &config);
976 		if (status == VCHIQ_SUCCESS) {
977 			if (copy_to_user((void __user *)args.pconfig,
978 				    &config, args.config_size) != 0) {
979 				ret = -EFAULT;
980 				break;
981 			}
982 		}
983 	} break;
984 
985 	case VCHIQ_IOC_SET_SERVICE_OPTION: {
986 		VCHIQ_SET_SERVICE_OPTION_T args;
987 
988 		memcpy(&args, (const void*)arg, sizeof(args));
989 
990 		service = find_service_for_instance(instance, args.handle);
991 		if (!service) {
992 			ret = -EINVAL;
993 			break;
994 		}
995 
996 		status = vchiq_set_service_option(
997 				args.handle, args.option, args.value);
998 	} break;
999 
1000 	case VCHIQ_IOC_DUMP_PHYS_MEM: {
1001 		VCHIQ_DUMP_MEM_T  args;
1002 
1003 		memcpy(&args, (const void*)arg, sizeof(args));
1004 		printf("IMPLEMENT ME: %s:%d\n", __FILE__, __LINE__);
1005 #if 0
1006 		dump_phys_mem(args.virt_addr, args.num_bytes);
1007 #endif
1008 	} break;
1009 
1010 	default:
1011 		ret = -ENOTTY;
1012 		break;
1013 	}
1014 
1015 	if (service)
1016 		unlock_service(service);
1017 
1018 	if (ret == 0) {
1019 		if (status == VCHIQ_ERROR)
1020 			ret = -EIO;
1021 		else if (status == VCHIQ_RETRY)
1022 			ret = -EINTR;
1023 	}
1024 
1025 	if ((status == VCHIQ_SUCCESS) && (ret < 0) && (ret != -EINTR) &&
1026 		(ret != -EWOULDBLOCK))
1027 		vchiq_log_info(vchiq_arm_log_level,
1028 			"  ioctl instance %lx, cmd %s -> status %d, %d",
1029 			(unsigned long)instance,
1030 			(_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
1031 				ioctl_names[_IOC_NR(cmd)] :
1032 				"<invalid>",
1033 			status, ret);
1034 	else
1035 		vchiq_log_trace(vchiq_arm_log_level,
1036 			"  ioctl instance %lx, cmd %s -> status %d, %d",
1037 			(unsigned long)instance,
1038 			(_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
1039 				ioctl_names[_IOC_NR(cmd)] :
1040 				"<invalid>",
1041 			status, ret);
1042 
1043 	/* XXXBSD: report BSD-style error to userland */
1044 	if (ret < 0)
1045 		ret = -ret;
1046 
1047 	return ret;
1048 }
1049 
1050 static void
1051 instance_dtr(void *data)
1052 {
1053 
1054    free(data, M_VCHIQ);
1055 }
1056 
1057 /****************************************************************************
1058 *
1059 *   vchiq_open
1060 *
1061 ***************************************************************************/
1062 
1063 static int
1064 vchiq_open(struct cdev *dev, int flags, int fmt __unused, struct thread *td)
1065 {
1066 	vchiq_log_info(vchiq_arm_log_level, "vchiq_open");
1067 	/* XXXBSD: do we really need this check? */
1068 	if (1) {
1069 		VCHIQ_STATE_T *state = vchiq_get_state();
1070 		VCHIQ_INSTANCE_T instance;
1071 
1072 		if (!state) {
1073 			vchiq_log_error(vchiq_arm_log_level,
1074 				"vchiq has no connection to VideoCore");
1075 			return -ENOTCONN;
1076 		}
1077 
1078 		instance = kmalloc(sizeof(*instance), GFP_KERNEL);
1079 		if (!instance)
1080 			return -ENOMEM;
1081 
1082 		instance->state = state;
1083 		/* XXXBSD: PID or thread ID? */
1084 		instance->pid = td->td_proc->p_pid;
1085 
1086 #ifdef notyet
1087 		ret = vchiq_proc_add_instance(instance);
1088 		if (ret != 0) {
1089 			kfree(instance);
1090 			return ret;
1091 		}
1092 #endif
1093 
1094 		_sema_init(&instance->insert_event, 0);
1095 		_sema_init(&instance->remove_event, 0);
1096 		lmutex_init(&instance->completion_mutex);
1097 		lmutex_init(&instance->bulk_waiter_list_mutex);
1098 		INIT_LIST_HEAD(&instance->bulk_waiter_list);
1099 
1100 		devfs_set_cdevpriv(instance, instance_dtr);
1101 	}
1102 	else {
1103 		vchiq_log_error(vchiq_arm_log_level,
1104 			"Unknown minor device");
1105 		return -ENXIO;
1106 	}
1107 
1108 	return 0;
1109 }
1110 
1111 /****************************************************************************
1112 *
1113 *   vchiq_release
1114 *
1115 ***************************************************************************/
1116 
1117 static int
1118 vchiq_close(struct cdev *dev, int flags __unused, int fmt __unused,
1119                 struct thread *td)
1120 {
1121 	int ret = 0;
1122 	if (1) {
1123 		VCHIQ_INSTANCE_T instance;
1124 		VCHIQ_STATE_T *state = vchiq_get_state();
1125 		VCHIQ_SERVICE_T *service;
1126 		int i;
1127 
1128 		if ((ret = devfs_get_cdevpriv((void**)&instance))) {
1129 			printf("devfs_get_cdevpriv failed: error %d\n", ret);
1130 			return (ret);
1131 		}
1132 
1133 		vchiq_log_info(vchiq_arm_log_level,
1134 			"vchiq_release: instance=%lx",
1135 			(unsigned long)instance);
1136 
1137 		if (!state) {
1138 			ret = -EPERM;
1139 			goto out;
1140 		}
1141 
1142 		/* Ensure videocore is awake to allow termination. */
1143 		vchiq_use_internal(instance->state, NULL,
1144 				USE_TYPE_VCHIQ);
1145 
1146 		lmutex_lock(&instance->completion_mutex);
1147 
1148 		/* Wake the completion thread and ask it to exit */
1149 		instance->closing = 1;
1150 		up(&instance->insert_event);
1151 
1152 		lmutex_unlock(&instance->completion_mutex);
1153 
1154 		/* Wake the slot handler if the completion queue is full. */
1155 		up(&instance->remove_event);
1156 
1157 		/* Mark all services for termination... */
1158 		i = 0;
1159 		while ((service = next_service_by_instance(state, instance,
1160 			&i)) !=	NULL) {
1161 			USER_SERVICE_T *user_service = service->base.userdata;
1162 
1163 			/* Wake the slot handler if the msg queue is full. */
1164 			up(&user_service->remove_event);
1165 
1166 			vchiq_terminate_service_internal(service);
1167 			unlock_service(service);
1168 		}
1169 
1170 		/* ...and wait for them to die */
1171 		i = 0;
1172 		while ((service = next_service_by_instance(state, instance, &i))
1173 			!= NULL) {
1174 			USER_SERVICE_T *user_service = service->base.userdata;
1175 
1176 			down(&service->remove_event);
1177 
1178 			BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
1179 
1180 			spin_lock(&msg_queue_spinlock);
1181 
1182 			while (user_service->msg_remove !=
1183 				user_service->msg_insert) {
1184 				VCHIQ_HEADER_T *header = user_service->
1185 					msg_queue[user_service->msg_remove &
1186 						(MSG_QUEUE_SIZE - 1)];
1187 				user_service->msg_remove++;
1188 				spin_unlock(&msg_queue_spinlock);
1189 
1190 				if (header)
1191 					vchiq_release_message(
1192 						service->handle,
1193 						header);
1194 				spin_lock(&msg_queue_spinlock);
1195 			}
1196 
1197 			spin_unlock(&msg_queue_spinlock);
1198 
1199 			unlock_service(service);
1200 		}
1201 
1202 		/* Release any closed services */
1203 		while (instance->completion_remove !=
1204 			instance->completion_insert) {
1205 			VCHIQ_COMPLETION_DATA_T *completion;
1206 			VCHIQ_SERVICE_T *service1;
1207 			completion = &instance->completions[
1208 				instance->completion_remove &
1209 				(MAX_COMPLETIONS - 1)];
1210 			service1 = completion->service_userdata;
1211 			if (completion->reason == VCHIQ_SERVICE_CLOSED)
1212 				unlock_service(service1);
1213 			instance->completion_remove++;
1214 		}
1215 
1216 		/* Release the PEER service count. */
1217 		vchiq_release_internal(instance->state, NULL);
1218 
1219 		{
1220 			struct list_head *pos, *next;
1221 			list_for_each_safe(pos, next,
1222 				&instance->bulk_waiter_list) {
1223 				struct bulk_waiter_node *waiter;
1224 				waiter = list_entry(pos,
1225 					struct bulk_waiter_node,
1226 					list);
1227 				list_del(pos);
1228 				vchiq_log_info(vchiq_arm_log_level,
1229 					"bulk_waiter - cleaned up %x "
1230 					"for pid %d",
1231 					(unsigned int)waiter, waiter->pid);
1232 		                _sema_destroy(&waiter->bulk_waiter.event);
1233 				kfree(waiter);
1234 			}
1235 		}
1236 
1237 	}
1238 	else {
1239 		vchiq_log_error(vchiq_arm_log_level,
1240 			"Unknown minor device");
1241 		ret = -ENXIO;
1242 	}
1243 
1244 out:
1245 	return ret;
1246 }
1247 
1248 /****************************************************************************
1249 *
1250 *   vchiq_dump
1251 *
1252 ***************************************************************************/
1253 
1254 void
1255 vchiq_dump(void *dump_context, const char *str, int len)
1256 {
1257 	DUMP_CONTEXT_T *context = (DUMP_CONTEXT_T *)dump_context;
1258 
1259 	if (context->actual < context->space) {
1260 		int copy_bytes;
1261 		if (context->offset > 0) {
1262 			int skip_bytes = min(len, (int)context->offset);
1263 			str += skip_bytes;
1264 			len -= skip_bytes;
1265 			context->offset -= skip_bytes;
1266 			if (context->offset > 0)
1267 				return;
1268 		}
1269 		copy_bytes = min(len, (int)(context->space - context->actual));
1270 		if (copy_bytes == 0)
1271 			return;
1272 		memcpy(context->buf + context->actual, str, copy_bytes);
1273 		context->actual += copy_bytes;
1274 		len -= copy_bytes;
1275 
1276 		/* If tne terminating NUL is included in the length, then it
1277 		** marks the end of a line and should be replaced with a
1278 		** carriage return. */
1279 		if ((len == 0) && (str[copy_bytes - 1] == '\0')) {
1280 			char cr = '\n';
1281 			memcpy(context->buf + context->actual - 1, &cr, 1);
1282 		}
1283 	}
1284 }
1285 
1286 /****************************************************************************
1287 *
1288 *   vchiq_dump_platform_instance_state
1289 *
1290 ***************************************************************************/
1291 
1292 void
1293 vchiq_dump_platform_instances(void *dump_context)
1294 {
1295 	VCHIQ_STATE_T *state = vchiq_get_state();
1296 	char buf[80];
1297 	int len;
1298 	int i;
1299 
1300 	/* There is no list of instances, so instead scan all services,
1301 		marking those that have been dumped. */
1302 
1303 	for (i = 0; i < state->unused_service; i++) {
1304 		VCHIQ_SERVICE_T *service = state->services[i];
1305 		VCHIQ_INSTANCE_T instance;
1306 
1307 		if (service && (service->base.callback == service_callback)) {
1308 			instance = service->instance;
1309 			if (instance)
1310 				instance->mark = 0;
1311 		}
1312 	}
1313 
1314 	for (i = 0; i < state->unused_service; i++) {
1315 		VCHIQ_SERVICE_T *service = state->services[i];
1316 		VCHIQ_INSTANCE_T instance;
1317 
1318 		if (service && (service->base.callback == service_callback)) {
1319 			instance = service->instance;
1320 			if (instance && !instance->mark) {
1321 				len = snprintf(buf, sizeof(buf),
1322 					"Instance %x: pid %d,%s completions "
1323 						"%d/%d",
1324 					(unsigned int)instance, instance->pid,
1325 					instance->connected ? " connected, " :
1326 						"",
1327 					instance->completion_insert -
1328 						instance->completion_remove,
1329 					MAX_COMPLETIONS);
1330 
1331 				vchiq_dump(dump_context, buf, len + 1);
1332 
1333 				instance->mark = 1;
1334 			}
1335 		}
1336 	}
1337 }
1338 
1339 /****************************************************************************
1340 *
1341 *   vchiq_dump_platform_service_state
1342 *
1343 ***************************************************************************/
1344 
1345 void
1346 vchiq_dump_platform_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
1347 {
1348 	USER_SERVICE_T *user_service = (USER_SERVICE_T *)service->base.userdata;
1349 	char buf[80];
1350 	int len;
1351 
1352 	len = snprintf(buf, sizeof(buf), "  instance %x",
1353 		(unsigned int)service->instance);
1354 
1355 	if ((service->base.callback == service_callback) &&
1356 		user_service->is_vchi) {
1357 		len += snprintf(buf + len, sizeof(buf) - len,
1358 			", %d/%d messages",
1359 			user_service->msg_insert - user_service->msg_remove,
1360 			MSG_QUEUE_SIZE);
1361 
1362 		if (user_service->dequeue_pending)
1363 			len += snprintf(buf + len, sizeof(buf) - len,
1364 				" (dequeue pending)");
1365 	}
1366 
1367 	vchiq_dump(dump_context, buf, len + 1);
1368 }
1369 
1370 #ifdef notyet
1371 /****************************************************************************
1372 *
1373 *   dump_user_mem
1374 *
1375 ***************************************************************************/
1376 
1377 static void
1378 dump_phys_mem(void *virt_addr, uint32_t num_bytes)
1379 {
1380 	int            rc;
1381 	uint8_t       *end_virt_addr = virt_addr + num_bytes;
1382 	int            num_pages;
1383 	int            offset;
1384 	int            end_offset;
1385 	int            page_idx;
1386 	int            prev_idx;
1387 	struct page   *page;
1388 	struct page  **pages;
1389 	uint8_t       *kmapped_virt_ptr;
1390 
1391 	/* Align virtAddr and endVirtAddr to 16 byte boundaries. */
1392 
1393 	virt_addr = (void *)((unsigned long)virt_addr & ~0x0fuL);
1394 	end_virt_addr = (void *)(((unsigned long)end_virt_addr + 15uL) &
1395 		~0x0fuL);
1396 
1397 	offset = (int)(long)virt_addr & (PAGE_SIZE - 1);
1398 	end_offset = (int)(long)end_virt_addr & (PAGE_SIZE - 1);
1399 
1400 	num_pages = (offset + num_bytes + PAGE_SIZE - 1) / PAGE_SIZE;
1401 
1402 	pages = kmalloc(sizeof(struct page *) * num_pages, GFP_KERNEL);
1403 	if (pages == NULL) {
1404 		vchiq_log_error(vchiq_arm_log_level,
1405 			"Unable to allocation memory for %d pages\n",
1406 			num_pages);
1407 		return;
1408 	}
1409 
1410 	down_read(&current->mm->mmap_sem);
1411 	rc = get_user_pages(current,      /* task */
1412 		current->mm,              /* mm */
1413 		(unsigned long)virt_addr, /* start */
1414 		num_pages,                /* len */
1415 		0,                        /* write */
1416 		0,                        /* force */
1417 		pages,                    /* pages (array of page pointers) */
1418 		NULL);                    /* vmas */
1419 	up_read(&current->mm->mmap_sem);
1420 
1421 	prev_idx = -1;
1422 	page = NULL;
1423 
1424 	while (offset < end_offset) {
1425 
1426 		int page_offset = offset % PAGE_SIZE;
1427 		page_idx = offset / PAGE_SIZE;
1428 
1429 		if (page_idx != prev_idx) {
1430 
1431 			if (page != NULL)
1432 				kunmap(page);
1433 			page = pages[page_idx];
1434 			kmapped_virt_ptr = kmap(page);
1435 
1436 			prev_idx = page_idx;
1437 		}
1438 
1439 		if (vchiq_arm_log_level >= VCHIQ_LOG_TRACE)
1440 			vchiq_log_dump_mem("ph",
1441 				(uint32_t)(unsigned long)&kmapped_virt_ptr[
1442 					page_offset],
1443 				&kmapped_virt_ptr[page_offset], 16);
1444 
1445 		offset += 16;
1446 	}
1447 	if (page != NULL)
1448 		kunmap(page);
1449 
1450 	for (page_idx = 0; page_idx < num_pages; page_idx++)
1451 		page_cache_release(pages[page_idx]);
1452 
1453 	kfree(pages);
1454 }
1455 
1456 /****************************************************************************
1457 *
1458 *   vchiq_read
1459 *
1460 ***************************************************************************/
1461 
1462 static ssize_t
1463 vchiq_read(struct file *file, char __user *buf,
1464 	size_t count, loff_t *ppos)
1465 {
1466 	DUMP_CONTEXT_T context;
1467 	context.buf = buf;
1468 	context.actual = 0;
1469 	context.space = count;
1470 	context.offset = *ppos;
1471 
1472 	vchiq_dump_state(&context, &g_state);
1473 
1474 	*ppos += context.actual;
1475 
1476 	return context.actual;
1477 }
1478 #endif
1479 
1480 VCHIQ_STATE_T *
1481 vchiq_get_state(void)
1482 {
1483 
1484 	if (g_state.remote == NULL)
1485 		printk(KERN_ERR "%s: g_state.remote == NULL\n", __func__);
1486 	else if (g_state.remote->initialised != 1)
1487 		printk(KERN_NOTICE "%s: g_state.remote->initialised != 1 (%d)\n",
1488 			__func__, g_state.remote->initialised);
1489 
1490 	return ((g_state.remote != NULL) &&
1491 		(g_state.remote->initialised == 1)) ? &g_state : NULL;
1492 }
1493 
1494 /*
1495  * Autosuspend related functionality
1496  */
1497 
1498 int
1499 vchiq_videocore_wanted(VCHIQ_STATE_T *state)
1500 {
1501 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
1502 	if (!arm_state)
1503 		/* autosuspend not supported - always return wanted */
1504 		return 1;
1505 	else if (arm_state->blocked_count)
1506 		return 1;
1507 	else if (!arm_state->videocore_use_count)
1508 		/* usage count zero - check for override unless we're forcing */
1509 		if (arm_state->resume_blocked)
1510 			return 0;
1511 		else
1512 			return vchiq_platform_videocore_wanted(state);
1513 	else
1514 		/* non-zero usage count - videocore still required */
1515 		return 1;
1516 }
1517 
1518 static VCHIQ_STATUS_T
1519 vchiq_keepalive_vchiq_callback(VCHIQ_REASON_T reason,
1520 	VCHIQ_HEADER_T *header,
1521 	VCHIQ_SERVICE_HANDLE_T service_user,
1522 	void *bulk_user)
1523 {
1524 	vchiq_log_error(vchiq_susp_log_level,
1525 		"%s callback reason %d", __func__, reason);
1526 	return 0;
1527 }
1528 
1529 static int
1530 vchiq_keepalive_thread_func(void *v)
1531 {
1532 	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
1533 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
1534 
1535 	VCHIQ_STATUS_T status;
1536 	VCHIQ_INSTANCE_T instance;
1537 	VCHIQ_SERVICE_HANDLE_T ka_handle;
1538 
1539 	VCHIQ_SERVICE_PARAMS_T params = {
1540 		.fourcc      = VCHIQ_MAKE_FOURCC('K', 'E', 'E', 'P'),
1541 		.callback    = vchiq_keepalive_vchiq_callback,
1542 		.version     = KEEPALIVE_VER,
1543 		.version_min = KEEPALIVE_VER_MIN
1544 	};
1545 
1546 	status = vchiq_initialise(&instance);
1547 	if (status != VCHIQ_SUCCESS) {
1548 		vchiq_log_error(vchiq_susp_log_level,
1549 			"%s vchiq_initialise failed %d", __func__, status);
1550 		goto exit;
1551 	}
1552 
1553 	status = vchiq_connect(instance);
1554 	if (status != VCHIQ_SUCCESS) {
1555 		vchiq_log_error(vchiq_susp_log_level,
1556 			"%s vchiq_connect failed %d", __func__, status);
1557 		goto shutdown;
1558 	}
1559 
1560 	status = vchiq_add_service(instance, &params, &ka_handle);
1561 	if (status != VCHIQ_SUCCESS) {
1562 		vchiq_log_error(vchiq_susp_log_level,
1563 			"%s vchiq_open_service failed %d", __func__, status);
1564 		goto shutdown;
1565 	}
1566 
1567 	while (1) {
1568 		long rc = 0, uc = 0;
1569 		if (wait_for_completion_interruptible(&arm_state->ka_evt)
1570 				!= 0) {
1571 			vchiq_log_error(vchiq_susp_log_level,
1572 				"%s interrupted", __func__);
1573 			flush_signals(current);
1574 			continue;
1575 		}
1576 
1577 		/* read and clear counters.  Do release_count then use_count to
1578 		 * prevent getting more releases than uses */
1579 		rc = atomic_xchg(&arm_state->ka_release_count, 0);
1580 		uc = atomic_xchg(&arm_state->ka_use_count, 0);
1581 
1582 		/* Call use/release service the requisite number of times.
1583 		 * Process use before release so use counts don't go negative */
1584 		while (uc--) {
1585 			atomic_inc(&arm_state->ka_use_ack_count);
1586 			status = vchiq_use_service(ka_handle);
1587 			if (status != VCHIQ_SUCCESS) {
1588 				vchiq_log_error(vchiq_susp_log_level,
1589 					"%s vchiq_use_service error %d",
1590 					__func__, status);
1591 			}
1592 		}
1593 		while (rc--) {
1594 			status = vchiq_release_service(ka_handle);
1595 			if (status != VCHIQ_SUCCESS) {
1596 				vchiq_log_error(vchiq_susp_log_level,
1597 					"%s vchiq_release_service error %d",
1598 					__func__, status);
1599 			}
1600 		}
1601 	}
1602 
1603 shutdown:
1604 	vchiq_shutdown(instance);
1605 exit:
1606 	return 0;
1607 }
1608 
1609 VCHIQ_STATUS_T
1610 vchiq_arm_init_state(VCHIQ_STATE_T *state, VCHIQ_ARM_STATE_T *arm_state)
1611 {
1612 	VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
1613 
1614 	if (arm_state) {
1615 		rwlock_init(&arm_state->susp_res_lock);
1616 
1617 		init_completion(&arm_state->ka_evt);
1618 		atomic_set(&arm_state->ka_use_count, 0);
1619 		atomic_set(&arm_state->ka_use_ack_count, 0);
1620 		atomic_set(&arm_state->ka_release_count, 0);
1621 
1622 		init_completion(&arm_state->vc_suspend_complete);
1623 
1624 		init_completion(&arm_state->vc_resume_complete);
1625 		/* Initialise to 'done' state.  We only want to block on resume
1626 		 * completion while videocore is suspended. */
1627 		set_resume_state(arm_state, VC_RESUME_RESUMED);
1628 
1629 		init_completion(&arm_state->resume_blocker);
1630 		/* Initialise to 'done' state.  We only want to block on this
1631 		 * completion while resume is blocked */
1632 		complete_all(&arm_state->resume_blocker);
1633 
1634 		init_completion(&arm_state->blocked_blocker);
1635 		/* Initialise to 'done' state.  We only want to block on this
1636 		 * completion while things are waiting on the resume blocker */
1637 		complete_all(&arm_state->blocked_blocker);
1638 
1639 		arm_state->suspend_timer_timeout = SUSPEND_TIMER_TIMEOUT_MS;
1640 		arm_state->suspend_timer_running = 0;
1641 		init_timer(&arm_state->suspend_timer);
1642 		arm_state->suspend_timer.data = (unsigned long)(state);
1643 		arm_state->suspend_timer.function = suspend_timer_callback;
1644 
1645 		arm_state->first_connect = 0;
1646 
1647 	}
1648 	return status;
1649 }
1650 
1651 /*
1652 ** Functions to modify the state variables;
1653 **	set_suspend_state
1654 **	set_resume_state
1655 **
1656 ** There are more state variables than we might like, so ensure they remain in
1657 ** step.  Suspend and resume state are maintained separately, since most of
1658 ** these state machines can operate independently.  However, there are a few
1659 ** states where state transitions in one state machine cause a reset to the
1660 ** other state machine.  In addition, there are some completion events which
1661 ** need to occur on state machine reset and end-state(s), so these are also
1662 ** dealt with in these functions.
1663 **
1664 ** In all states we set the state variable according to the input, but in some
1665 ** cases we perform additional steps outlined below;
1666 **
1667 ** VC_SUSPEND_IDLE - Initialise the suspend completion at the same time.
1668 **			The suspend completion is completed after any suspend
1669 **			attempt.  When we reset the state machine we also reset
1670 **			the completion.  This reset occurs when videocore is
1671 **			resumed, and also if we initiate suspend after a suspend
1672 **			failure.
1673 **
1674 ** VC_SUSPEND_IN_PROGRESS - This state is considered the point of no return for
1675 **			suspend - ie from this point on we must try to suspend
1676 **			before resuming can occur.  We therefore also reset the
1677 **			resume state machine to VC_RESUME_IDLE in this state.
1678 **
1679 ** VC_SUSPEND_SUSPENDED - Suspend has completed successfully. Also call
1680 **			complete_all on the suspend completion to notify
1681 **			anything waiting for suspend to happen.
1682 **
1683 ** VC_SUSPEND_REJECTED - Videocore rejected suspend. Videocore will also
1684 **			initiate resume, so no need to alter resume state.
1685 **			We call complete_all on the suspend completion to notify
1686 **			of suspend rejection.
1687 **
1688 ** VC_SUSPEND_FAILED - We failed to initiate videocore suspend.  We notify the
1689 **			suspend completion and reset the resume state machine.
1690 **
1691 ** VC_RESUME_IDLE - Initialise the resume completion at the same time.  The
1692 **			resume completion is in it's 'done' state whenever
1693 **			videcore is running.  Therfore, the VC_RESUME_IDLE state
1694 **			implies that videocore is suspended.
1695 **			Hence, any thread which needs to wait until videocore is
1696 **			running can wait on this completion - it will only block
1697 **			if videocore is suspended.
1698 **
1699 ** VC_RESUME_RESUMED - Resume has completed successfully.  Videocore is running.
1700 **			Call complete_all on the resume completion to unblock
1701 **			any threads waiting for resume.	 Also reset the suspend
1702 **			state machine to it's idle state.
1703 **
1704 ** VC_RESUME_FAILED - Currently unused - no mechanism to fail resume exists.
1705 */
1706 
1707 inline void
1708 set_suspend_state(VCHIQ_ARM_STATE_T *arm_state,
1709 	enum vc_suspend_status new_state)
1710 {
1711 	/* set the state in all cases */
1712 	arm_state->vc_suspend_state = new_state;
1713 
1714 	/* state specific additional actions */
1715 	switch (new_state) {
1716 	case VC_SUSPEND_FORCE_CANCELED:
1717 		complete_all(&arm_state->vc_suspend_complete);
1718 		break;
1719 	case VC_SUSPEND_REJECTED:
1720 		complete_all(&arm_state->vc_suspend_complete);
1721 		break;
1722 	case VC_SUSPEND_FAILED:
1723 		complete_all(&arm_state->vc_suspend_complete);
1724 		arm_state->vc_resume_state = VC_RESUME_RESUMED;
1725 		complete_all(&arm_state->vc_resume_complete);
1726 		break;
1727 	case VC_SUSPEND_IDLE:
1728 		INIT_COMPLETION(arm_state->vc_suspend_complete);
1729 		break;
1730 	case VC_SUSPEND_REQUESTED:
1731 		break;
1732 	case VC_SUSPEND_IN_PROGRESS:
1733 		set_resume_state(arm_state, VC_RESUME_IDLE);
1734 		break;
1735 	case VC_SUSPEND_SUSPENDED:
1736 		complete_all(&arm_state->vc_suspend_complete);
1737 		break;
1738 	default:
1739 		BUG();
1740 		break;
1741 	}
1742 }
1743 
1744 inline void
1745 set_resume_state(VCHIQ_ARM_STATE_T *arm_state,
1746 	enum vc_resume_status new_state)
1747 {
1748 	/* set the state in all cases */
1749 	arm_state->vc_resume_state = new_state;
1750 
1751 	/* state specific additional actions */
1752 	switch (new_state) {
1753 	case VC_RESUME_FAILED:
1754 		break;
1755 	case VC_RESUME_IDLE:
1756 		INIT_COMPLETION(arm_state->vc_resume_complete);
1757 		break;
1758 	case VC_RESUME_REQUESTED:
1759 		break;
1760 	case VC_RESUME_IN_PROGRESS:
1761 		break;
1762 	case VC_RESUME_RESUMED:
1763 		complete_all(&arm_state->vc_resume_complete);
1764 		set_suspend_state(arm_state, VC_SUSPEND_IDLE);
1765 		break;
1766 	default:
1767 		BUG();
1768 		break;
1769 	}
1770 }
1771 
1772 
1773 /* should be called with the write lock held */
1774 inline void
1775 start_suspend_timer(VCHIQ_ARM_STATE_T *arm_state)
1776 {
1777 	del_timer(&arm_state->suspend_timer);
1778 	arm_state->suspend_timer.expires = jiffies +
1779 		msecs_to_jiffies(arm_state->
1780 			suspend_timer_timeout);
1781 	add_timer(&arm_state->suspend_timer);
1782 	arm_state->suspend_timer_running = 1;
1783 }
1784 
1785 /* should be called with the write lock held */
1786 static inline void
1787 stop_suspend_timer(VCHIQ_ARM_STATE_T *arm_state)
1788 {
1789 	if (arm_state->suspend_timer_running) {
1790 		del_timer(&arm_state->suspend_timer);
1791 		arm_state->suspend_timer_running = 0;
1792 	}
1793 }
1794 
1795 static inline int
1796 need_resume(VCHIQ_STATE_T *state)
1797 {
1798 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
1799 	return (arm_state->vc_suspend_state > VC_SUSPEND_IDLE) &&
1800 			(arm_state->vc_resume_state < VC_RESUME_REQUESTED) &&
1801 			vchiq_videocore_wanted(state);
1802 }
1803 
1804 static int
1805 block_resume(VCHIQ_ARM_STATE_T *arm_state)
1806 {
1807 	int status = VCHIQ_SUCCESS;
1808 	const unsigned long timeout_val =
1809 				msecs_to_jiffies(FORCE_SUSPEND_TIMEOUT_MS);
1810 	int resume_count = 0;
1811 
1812 	/* Allow any threads which were blocked by the last force suspend to
1813 	 * complete if they haven't already.  Only give this one shot; if
1814 	 * blocked_count is incremented after blocked_blocker is completed
1815 	 * (which only happens when blocked_count hits 0) then those threads
1816 	 * will have to wait until next time around */
1817 	if (arm_state->blocked_count) {
1818 		INIT_COMPLETION(arm_state->blocked_blocker);
1819 		write_unlock_bh(&arm_state->susp_res_lock);
1820 		vchiq_log_info(vchiq_susp_log_level, "%s wait for previously "
1821 			"blocked clients", __func__);
1822 		if (wait_for_completion_interruptible_timeout(
1823 				&arm_state->blocked_blocker, timeout_val)
1824 					<= 0) {
1825 			vchiq_log_error(vchiq_susp_log_level, "%s wait for "
1826 				"previously blocked clients failed" , __func__);
1827 			status = VCHIQ_ERROR;
1828 			write_lock_bh(&arm_state->susp_res_lock);
1829 			goto out;
1830 		}
1831 		vchiq_log_info(vchiq_susp_log_level, "%s previously blocked "
1832 			"clients resumed", __func__);
1833 		write_lock_bh(&arm_state->susp_res_lock);
1834 	}
1835 
1836 	/* We need to wait for resume to complete if it's in process */
1837 	while (arm_state->vc_resume_state != VC_RESUME_RESUMED &&
1838 			arm_state->vc_resume_state > VC_RESUME_IDLE) {
1839 		if (resume_count > 1) {
1840 			status = VCHIQ_ERROR;
1841 			vchiq_log_error(vchiq_susp_log_level, "%s waited too "
1842 				"many times for resume" , __func__);
1843 			goto out;
1844 		}
1845 		write_unlock_bh(&arm_state->susp_res_lock);
1846 		vchiq_log_info(vchiq_susp_log_level, "%s wait for resume",
1847 			__func__);
1848 		if (wait_for_completion_interruptible_timeout(
1849 				&arm_state->vc_resume_complete, timeout_val)
1850 					<= 0) {
1851 			vchiq_log_error(vchiq_susp_log_level, "%s wait for "
1852 				"resume failed (%s)", __func__,
1853 				resume_state_names[arm_state->vc_resume_state +
1854 							VC_RESUME_NUM_OFFSET]);
1855 			status = VCHIQ_ERROR;
1856 			write_lock_bh(&arm_state->susp_res_lock);
1857 			goto out;
1858 		}
1859 		vchiq_log_info(vchiq_susp_log_level, "%s resumed", __func__);
1860 		write_lock_bh(&arm_state->susp_res_lock);
1861 		resume_count++;
1862 	}
1863 	INIT_COMPLETION(arm_state->resume_blocker);
1864 	arm_state->resume_blocked = 1;
1865 
1866 out:
1867 	return status;
1868 }
1869 
1870 static inline void
1871 unblock_resume(VCHIQ_ARM_STATE_T *arm_state)
1872 {
1873 	complete_all(&arm_state->resume_blocker);
1874 	arm_state->resume_blocked = 0;
1875 }
1876 
1877 /* Initiate suspend via slot handler. Should be called with the write lock
1878  * held */
1879 VCHIQ_STATUS_T
1880 vchiq_arm_vcsuspend(VCHIQ_STATE_T *state)
1881 {
1882 	VCHIQ_STATUS_T status = VCHIQ_ERROR;
1883 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
1884 
1885 	if (!arm_state)
1886 		goto out;
1887 
1888 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
1889 	status = VCHIQ_SUCCESS;
1890 
1891 
1892 	switch (arm_state->vc_suspend_state) {
1893 	case VC_SUSPEND_REQUESTED:
1894 		vchiq_log_info(vchiq_susp_log_level, "%s: suspend already "
1895 			"requested", __func__);
1896 		break;
1897 	case VC_SUSPEND_IN_PROGRESS:
1898 		vchiq_log_info(vchiq_susp_log_level, "%s: suspend already in "
1899 			"progress", __func__);
1900 		break;
1901 
1902 	default:
1903 		/* We don't expect to be in other states, so log but continue
1904 		 * anyway */
1905 		vchiq_log_error(vchiq_susp_log_level,
1906 			"%s unexpected suspend state %s", __func__,
1907 			suspend_state_names[arm_state->vc_suspend_state +
1908 						VC_SUSPEND_NUM_OFFSET]);
1909 		/* fall through */
1910 	case VC_SUSPEND_REJECTED:
1911 	case VC_SUSPEND_FAILED:
1912 		/* Ensure any idle state actions have been run */
1913 		set_suspend_state(arm_state, VC_SUSPEND_IDLE);
1914 		/* fall through */
1915 	case VC_SUSPEND_IDLE:
1916 		vchiq_log_info(vchiq_susp_log_level,
1917 			"%s: suspending", __func__);
1918 		set_suspend_state(arm_state, VC_SUSPEND_REQUESTED);
1919 		/* kick the slot handler thread to initiate suspend */
1920 		request_poll(state, NULL, 0);
1921 		break;
1922 	}
1923 
1924 out:
1925 	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, status);
1926 	return status;
1927 }
1928 
1929 void
1930 vchiq_platform_check_suspend(VCHIQ_STATE_T *state)
1931 {
1932 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
1933 	int susp = 0;
1934 
1935 	if (!arm_state)
1936 		goto out;
1937 
1938 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
1939 
1940 	write_lock_bh(&arm_state->susp_res_lock);
1941 	if (arm_state->vc_suspend_state == VC_SUSPEND_REQUESTED &&
1942 			arm_state->vc_resume_state == VC_RESUME_RESUMED) {
1943 		set_suspend_state(arm_state, VC_SUSPEND_IN_PROGRESS);
1944 		susp = 1;
1945 	}
1946 	write_unlock_bh(&arm_state->susp_res_lock);
1947 
1948 	if (susp)
1949 		vchiq_platform_suspend(state);
1950 
1951 out:
1952 	vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
1953 	return;
1954 }
1955 
1956 
1957 static void
1958 output_timeout_error(VCHIQ_STATE_T *state)
1959 {
1960 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
1961 	char service_err[50] = "";
1962 	int vc_use_count = arm_state->videocore_use_count;
1963 	int active_services = state->unused_service;
1964 	int i;
1965 
1966 	if (!arm_state->videocore_use_count) {
1967 		snprintf(service_err, 50, " Videocore usecount is 0");
1968 		goto output_msg;
1969 	}
1970 	for (i = 0; i < active_services; i++) {
1971 		VCHIQ_SERVICE_T *service_ptr = state->services[i];
1972 		if (service_ptr && service_ptr->service_use_count &&
1973 			(service_ptr->srvstate != VCHIQ_SRVSTATE_FREE)) {
1974 			snprintf(service_err, 50, " %c%c%c%c(%8x) service has "
1975 				"use count %d%s", VCHIQ_FOURCC_AS_4CHARS(
1976 					service_ptr->base.fourcc),
1977 				 service_ptr->client_id,
1978 				 service_ptr->service_use_count,
1979 				 service_ptr->service_use_count ==
1980 					 vc_use_count ? "" : " (+ more)");
1981 			break;
1982 		}
1983 	}
1984 
1985 output_msg:
1986 	vchiq_log_error(vchiq_susp_log_level,
1987 		"timed out waiting for vc suspend (%d).%s",
1988 		 arm_state->autosuspend_override, service_err);
1989 
1990 }
1991 
1992 /* Try to get videocore into suspended state, regardless of autosuspend state.
1993 ** We don't actually force suspend, since videocore may get into a bad state
1994 ** if we force suspend at a bad time.  Instead, we wait for autosuspend to
1995 ** determine a good point to suspend.  If this doesn't happen within 100ms we
1996 ** report failure.
1997 **
1998 ** Returns VCHIQ_SUCCESS if videocore suspended successfully, VCHIQ_RETRY if
1999 ** videocore failed to suspend in time or VCHIQ_ERROR if interrupted.
2000 */
2001 VCHIQ_STATUS_T
2002 vchiq_arm_force_suspend(VCHIQ_STATE_T *state)
2003 {
2004 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2005 	VCHIQ_STATUS_T status = VCHIQ_ERROR;
2006 	long rc = 0;
2007 	int repeat = -1;
2008 
2009 	if (!arm_state)
2010 		goto out;
2011 
2012 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2013 
2014 	write_lock_bh(&arm_state->susp_res_lock);
2015 
2016 	status = block_resume(arm_state);
2017 	if (status != VCHIQ_SUCCESS)
2018 		goto unlock;
2019 	if (arm_state->vc_suspend_state == VC_SUSPEND_SUSPENDED) {
2020 		/* Already suspended - just block resume and exit */
2021 		vchiq_log_info(vchiq_susp_log_level, "%s already suspended",
2022 			__func__);
2023 		status = VCHIQ_SUCCESS;
2024 		goto unlock;
2025 	} else if (arm_state->vc_suspend_state <= VC_SUSPEND_IDLE) {
2026 		/* initiate suspend immediately in the case that we're waiting
2027 		 * for the timeout */
2028 		stop_suspend_timer(arm_state);
2029 		if (!vchiq_videocore_wanted(state)) {
2030 			vchiq_log_info(vchiq_susp_log_level, "%s videocore "
2031 				"idle, initiating suspend", __func__);
2032 			status = vchiq_arm_vcsuspend(state);
2033 		} else if (arm_state->autosuspend_override <
2034 						FORCE_SUSPEND_FAIL_MAX) {
2035 			vchiq_log_info(vchiq_susp_log_level, "%s letting "
2036 				"videocore go idle", __func__);
2037 			status = VCHIQ_SUCCESS;
2038 		} else {
2039 			vchiq_log_warning(vchiq_susp_log_level, "%s failed too "
2040 				"many times - attempting suspend", __func__);
2041 			status = vchiq_arm_vcsuspend(state);
2042 		}
2043 	} else {
2044 		vchiq_log_info(vchiq_susp_log_level, "%s videocore suspend "
2045 			"in progress - wait for completion", __func__);
2046 		status = VCHIQ_SUCCESS;
2047 	}
2048 
2049 	/* Wait for suspend to happen due to system idle (not forced..) */
2050 	if (status != VCHIQ_SUCCESS)
2051 		goto unblock_resume;
2052 
2053 	do {
2054 		write_unlock_bh(&arm_state->susp_res_lock);
2055 
2056 		rc = wait_for_completion_interruptible_timeout(
2057 				&arm_state->vc_suspend_complete,
2058 				msecs_to_jiffies(FORCE_SUSPEND_TIMEOUT_MS));
2059 
2060 		write_lock_bh(&arm_state->susp_res_lock);
2061 		if (rc < 0) {
2062 			vchiq_log_warning(vchiq_susp_log_level, "%s "
2063 				"interrupted waiting for suspend", __func__);
2064 			status = VCHIQ_ERROR;
2065 			goto unblock_resume;
2066 		} else if (rc == 0) {
2067 			if (arm_state->vc_suspend_state > VC_SUSPEND_IDLE) {
2068 				/* Repeat timeout once if in progress */
2069 				if (repeat < 0) {
2070 					repeat = 1;
2071 					continue;
2072 				}
2073 			}
2074 			arm_state->autosuspend_override++;
2075 			output_timeout_error(state);
2076 
2077 			status = VCHIQ_RETRY;
2078 			goto unblock_resume;
2079 		}
2080 	} while (0 < (repeat--));
2081 
2082 	/* Check and report state in case we need to abort ARM suspend */
2083 	if (arm_state->vc_suspend_state != VC_SUSPEND_SUSPENDED) {
2084 		status = VCHIQ_RETRY;
2085 		vchiq_log_error(vchiq_susp_log_level,
2086 			"%s videocore suspend failed (state %s)", __func__,
2087 			suspend_state_names[arm_state->vc_suspend_state +
2088 						VC_SUSPEND_NUM_OFFSET]);
2089 		/* Reset the state only if it's still in an error state.
2090 		 * Something could have already initiated another suspend. */
2091 		if (arm_state->vc_suspend_state < VC_SUSPEND_IDLE)
2092 			set_suspend_state(arm_state, VC_SUSPEND_IDLE);
2093 
2094 		goto unblock_resume;
2095 	}
2096 
2097 	/* successfully suspended - unlock and exit */
2098 	goto unlock;
2099 
2100 unblock_resume:
2101 	/* all error states need to unblock resume before exit */
2102 	unblock_resume(arm_state);
2103 
2104 unlock:
2105 	write_unlock_bh(&arm_state->susp_res_lock);
2106 
2107 out:
2108 	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, status);
2109 	return status;
2110 }
2111 
2112 void
2113 vchiq_check_suspend(VCHIQ_STATE_T *state)
2114 {
2115 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2116 
2117 	if (!arm_state)
2118 		goto out;
2119 
2120 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2121 
2122 	write_lock_bh(&arm_state->susp_res_lock);
2123 	if (arm_state->vc_suspend_state != VC_SUSPEND_SUSPENDED &&
2124 			arm_state->first_connect &&
2125 			!vchiq_videocore_wanted(state)) {
2126 		vchiq_arm_vcsuspend(state);
2127 	}
2128 	write_unlock_bh(&arm_state->susp_res_lock);
2129 
2130 out:
2131 	vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
2132 	return;
2133 }
2134 
2135 
2136 int
2137 vchiq_arm_allow_resume(VCHIQ_STATE_T *state)
2138 {
2139 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2140 	int resume = 0;
2141 	int ret = -1;
2142 
2143 	if (!arm_state)
2144 		goto out;
2145 
2146 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2147 
2148 	write_lock_bh(&arm_state->susp_res_lock);
2149 	unblock_resume(arm_state);
2150 	resume = vchiq_check_resume(state);
2151 	write_unlock_bh(&arm_state->susp_res_lock);
2152 
2153 	if (resume) {
2154 		if (wait_for_completion_interruptible(
2155 			&arm_state->vc_resume_complete) < 0) {
2156 			vchiq_log_error(vchiq_susp_log_level,
2157 				"%s interrupted", __func__);
2158 			/* failed, cannot accurately derive suspend
2159 			 * state, so exit early. */
2160 			goto out;
2161 		}
2162 	}
2163 
2164 	read_lock_bh(&arm_state->susp_res_lock);
2165 	if (arm_state->vc_suspend_state == VC_SUSPEND_SUSPENDED) {
2166 		vchiq_log_info(vchiq_susp_log_level,
2167 				"%s: Videocore remains suspended", __func__);
2168 	} else {
2169 		vchiq_log_info(vchiq_susp_log_level,
2170 				"%s: Videocore resumed", __func__);
2171 		ret = 0;
2172 	}
2173 	read_unlock_bh(&arm_state->susp_res_lock);
2174 out:
2175 	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
2176 	return ret;
2177 }
2178 
2179 /* This function should be called with the write lock held */
2180 int
2181 vchiq_check_resume(VCHIQ_STATE_T *state)
2182 {
2183 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2184 	int resume = 0;
2185 
2186 	if (!arm_state)
2187 		goto out;
2188 
2189 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2190 
2191 	if (need_resume(state)) {
2192 		set_resume_state(arm_state, VC_RESUME_REQUESTED);
2193 		request_poll(state, NULL, 0);
2194 		resume = 1;
2195 	}
2196 
2197 out:
2198 	vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
2199 	return resume;
2200 }
2201 
2202 #ifdef notyet
2203 void
2204 vchiq_platform_check_resume(VCHIQ_STATE_T *state)
2205 {
2206 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2207 	int res = 0;
2208 
2209 	if (!arm_state)
2210 		goto out;
2211 
2212 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2213 
2214 	write_lock_bh(&arm_state->susp_res_lock);
2215 	if (arm_state->wake_address == 0) {
2216 		vchiq_log_info(vchiq_susp_log_level,
2217 					"%s: already awake", __func__);
2218 		goto unlock;
2219 	}
2220 	if (arm_state->vc_resume_state == VC_RESUME_IN_PROGRESS) {
2221 		vchiq_log_info(vchiq_susp_log_level,
2222 					"%s: already resuming", __func__);
2223 		goto unlock;
2224 	}
2225 
2226 	if (arm_state->vc_resume_state == VC_RESUME_REQUESTED) {
2227 		set_resume_state(arm_state, VC_RESUME_IN_PROGRESS);
2228 		res = 1;
2229 	} else
2230 		vchiq_log_trace(vchiq_susp_log_level,
2231 				"%s: not resuming (resume state %s)", __func__,
2232 				resume_state_names[arm_state->vc_resume_state +
2233 							VC_RESUME_NUM_OFFSET]);
2234 
2235 unlock:
2236 	write_unlock_bh(&arm_state->susp_res_lock);
2237 
2238 	if (res)
2239 		vchiq_platform_resume(state);
2240 
2241 out:
2242 	vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
2243 	return;
2244 
2245 }
2246 #endif
2247 
2248 
2249 
2250 VCHIQ_STATUS_T
2251 vchiq_use_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
2252 		enum USE_TYPE_E use_type)
2253 {
2254 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2255 	VCHIQ_STATUS_T ret = VCHIQ_SUCCESS;
2256 	char entity[16];
2257 	int *entity_uc;
2258 	int local_uc, local_entity_uc;
2259 
2260 	if (!arm_state)
2261 		goto out;
2262 
2263 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2264 
2265 	if (use_type == USE_TYPE_VCHIQ) {
2266 		snprintf(entity, sizeof(entity), "VCHIQ:   ");
2267 		entity_uc = &arm_state->peer_use_count;
2268 	} else if (service) {
2269 		snprintf(entity, sizeof(entity), "%c%c%c%c:%8x",
2270 			VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2271 			service->client_id);
2272 		entity_uc = &service->service_use_count;
2273 	} else {
2274 		vchiq_log_error(vchiq_susp_log_level, "%s null service "
2275 				"ptr", __func__);
2276 		ret = VCHIQ_ERROR;
2277 		goto out;
2278 	}
2279 
2280 	write_lock_bh(&arm_state->susp_res_lock);
2281 	while (arm_state->resume_blocked) {
2282 		/* If we call 'use' while force suspend is waiting for suspend,
2283 		 * then we're about to block the thread which the force is
2284 		 * waiting to complete, so we're bound to just time out. In this
2285 		 * case, set the suspend state such that the wait will be
2286 		 * canceled, so we can complete as quickly as possible. */
2287 		if (arm_state->resume_blocked && arm_state->vc_suspend_state ==
2288 				VC_SUSPEND_IDLE) {
2289 			set_suspend_state(arm_state, VC_SUSPEND_FORCE_CANCELED);
2290 			break;
2291 		}
2292 		/* If suspend is already in progress then we need to block */
2293 		if (!try_wait_for_completion(&arm_state->resume_blocker)) {
2294 			/* Indicate that there are threads waiting on the resume
2295 			 * blocker.  These need to be allowed to complete before
2296 			 * a _second_ call to force suspend can complete,
2297 			 * otherwise low priority threads might never actually
2298 			 * continue */
2299 			arm_state->blocked_count++;
2300 			write_unlock_bh(&arm_state->susp_res_lock);
2301 			vchiq_log_info(vchiq_susp_log_level, "%s %s resume "
2302 				"blocked - waiting...", __func__, entity);
2303 			if (wait_for_completion_killable(
2304 					&arm_state->resume_blocker) != 0) {
2305 				vchiq_log_error(vchiq_susp_log_level, "%s %s "
2306 					"wait for resume blocker interrupted",
2307 					__func__, entity);
2308 				ret = VCHIQ_ERROR;
2309 				write_lock_bh(&arm_state->susp_res_lock);
2310 				arm_state->blocked_count--;
2311 				write_unlock_bh(&arm_state->susp_res_lock);
2312 				goto out;
2313 			}
2314 			vchiq_log_info(vchiq_susp_log_level, "%s %s resume "
2315 				"unblocked", __func__, entity);
2316 			write_lock_bh(&arm_state->susp_res_lock);
2317 			if (--arm_state->blocked_count == 0)
2318 				complete_all(&arm_state->blocked_blocker);
2319 		}
2320 	}
2321 
2322 	stop_suspend_timer(arm_state);
2323 
2324 	local_uc = ++arm_state->videocore_use_count;
2325 	local_entity_uc = ++(*entity_uc);
2326 
2327 	/* If there's a pending request which hasn't yet been serviced then
2328 	 * just clear it.  If we're past VC_SUSPEND_REQUESTED state then
2329 	 * vc_resume_complete will block until we either resume or fail to
2330 	 * suspend */
2331 	if (arm_state->vc_suspend_state <= VC_SUSPEND_REQUESTED)
2332 		set_suspend_state(arm_state, VC_SUSPEND_IDLE);
2333 
2334 	if ((use_type != USE_TYPE_SERVICE_NO_RESUME) && need_resume(state)) {
2335 		set_resume_state(arm_state, VC_RESUME_REQUESTED);
2336 		vchiq_log_info(vchiq_susp_log_level,
2337 			"%s %s count %d, state count %d",
2338 			__func__, entity, local_entity_uc, local_uc);
2339 		request_poll(state, NULL, 0);
2340 	} else
2341 		vchiq_log_trace(vchiq_susp_log_level,
2342 			"%s %s count %d, state count %d",
2343 			__func__, entity, *entity_uc, local_uc);
2344 
2345 
2346 	write_unlock_bh(&arm_state->susp_res_lock);
2347 
2348 	/* Completion is in a done state when we're not suspended, so this won't
2349 	 * block for the non-suspended case. */
2350 	if (!try_wait_for_completion(&arm_state->vc_resume_complete)) {
2351 		vchiq_log_info(vchiq_susp_log_level, "%s %s wait for resume",
2352 			__func__, entity);
2353 		if (wait_for_completion_killable(
2354 				&arm_state->vc_resume_complete) != 0) {
2355 			vchiq_log_error(vchiq_susp_log_level, "%s %s wait for "
2356 				"resume interrupted", __func__, entity);
2357 			ret = VCHIQ_ERROR;
2358 			goto out;
2359 		}
2360 		vchiq_log_info(vchiq_susp_log_level, "%s %s resumed", __func__,
2361 			entity);
2362 	}
2363 
2364 	if (ret == VCHIQ_SUCCESS) {
2365 		VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
2366 		long ack_cnt = atomic_xchg(&arm_state->ka_use_ack_count, 0);
2367 		while (ack_cnt && (status == VCHIQ_SUCCESS)) {
2368 			/* Send the use notify to videocore */
2369 			status = vchiq_send_remote_use_active(state);
2370 			if (status == VCHIQ_SUCCESS)
2371 				ack_cnt--;
2372 			else
2373 				atomic_add(ack_cnt,
2374 					&arm_state->ka_use_ack_count);
2375 		}
2376 	}
2377 
2378 out:
2379 	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
2380 	return ret;
2381 }
2382 
2383 VCHIQ_STATUS_T
2384 vchiq_release_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service)
2385 {
2386 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2387 	VCHIQ_STATUS_T ret = VCHIQ_SUCCESS;
2388 	char entity[16];
2389 	int *entity_uc;
2390 
2391 	if (!arm_state)
2392 		goto out;
2393 
2394 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2395 
2396 	if (service) {
2397 		snprintf(entity, sizeof(entity), "%c%c%c%c:%8x",
2398 			VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2399 			service->client_id);
2400 		entity_uc = &service->service_use_count;
2401 	} else {
2402 		snprintf(entity, sizeof(entity), "PEER:   ");
2403 		entity_uc = &arm_state->peer_use_count;
2404 	}
2405 
2406 	write_lock_bh(&arm_state->susp_res_lock);
2407 	if (!arm_state->videocore_use_count || !(*entity_uc)) {
2408 		/* Don't use BUG_ON - don't allow user thread to crash kernel */
2409 		WARN_ON(!arm_state->videocore_use_count);
2410 		WARN_ON(!(*entity_uc));
2411 		ret = VCHIQ_ERROR;
2412 		goto unlock;
2413 	}
2414 	--arm_state->videocore_use_count;
2415 	--(*entity_uc);
2416 
2417 	if (!vchiq_videocore_wanted(state)) {
2418 		if (vchiq_platform_use_suspend_timer() &&
2419 				!arm_state->resume_blocked) {
2420 			/* Only use the timer if we're not trying to force
2421 			 * suspend (=> resume_blocked) */
2422 			start_suspend_timer(arm_state);
2423 		} else {
2424 			vchiq_log_info(vchiq_susp_log_level,
2425 				"%s %s count %d, state count %d - suspending",
2426 				__func__, entity, *entity_uc,
2427 				arm_state->videocore_use_count);
2428 			vchiq_arm_vcsuspend(state);
2429 		}
2430 	} else
2431 		vchiq_log_trace(vchiq_susp_log_level,
2432 			"%s %s count %d, state count %d",
2433 			__func__, entity, *entity_uc,
2434 			arm_state->videocore_use_count);
2435 
2436 unlock:
2437 	write_unlock_bh(&arm_state->susp_res_lock);
2438 
2439 out:
2440 	vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
2441 	return ret;
2442 }
2443 
2444 void
2445 vchiq_on_remote_use(VCHIQ_STATE_T *state)
2446 {
2447 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2448 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2449 	atomic_inc(&arm_state->ka_use_count);
2450 	complete(&arm_state->ka_evt);
2451 }
2452 
2453 void
2454 vchiq_on_remote_release(VCHIQ_STATE_T *state)
2455 {
2456 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2457 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2458 	atomic_inc(&arm_state->ka_release_count);
2459 	complete(&arm_state->ka_evt);
2460 }
2461 
2462 VCHIQ_STATUS_T
2463 vchiq_use_service_internal(VCHIQ_SERVICE_T *service)
2464 {
2465 	return vchiq_use_internal(service->state, service, USE_TYPE_SERVICE);
2466 }
2467 
2468 VCHIQ_STATUS_T
2469 vchiq_release_service_internal(VCHIQ_SERVICE_T *service)
2470 {
2471 	return vchiq_release_internal(service->state, service);
2472 }
2473 
2474 static void suspend_timer_callback(unsigned long context)
2475 {
2476 	VCHIQ_STATE_T *state = (VCHIQ_STATE_T *)context;
2477 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2478 	if (!arm_state)
2479 		goto out;
2480 	vchiq_log_info(vchiq_susp_log_level,
2481 		"%s - suspend timer expired - check suspend", __func__);
2482 	vchiq_check_suspend(state);
2483 out:
2484 	return;
2485 }
2486 
2487 VCHIQ_STATUS_T
2488 vchiq_use_service_no_resume(VCHIQ_SERVICE_HANDLE_T handle)
2489 {
2490 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
2491 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
2492 	if (service) {
2493 		ret = vchiq_use_internal(service->state, service,
2494 				USE_TYPE_SERVICE_NO_RESUME);
2495 		unlock_service(service);
2496 	}
2497 	return ret;
2498 }
2499 
2500 VCHIQ_STATUS_T
2501 vchiq_use_service(VCHIQ_SERVICE_HANDLE_T handle)
2502 {
2503 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
2504 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
2505 	if (service) {
2506 		ret = vchiq_use_internal(service->state, service,
2507 				USE_TYPE_SERVICE);
2508 		unlock_service(service);
2509 	}
2510 	return ret;
2511 }
2512 
2513 VCHIQ_STATUS_T
2514 vchiq_release_service(VCHIQ_SERVICE_HANDLE_T handle)
2515 {
2516 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
2517 	VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
2518 	if (service) {
2519 		ret = vchiq_release_internal(service->state, service);
2520 		unlock_service(service);
2521 	}
2522 	return ret;
2523 }
2524 
2525 void
2526 vchiq_dump_service_use_state(VCHIQ_STATE_T *state)
2527 {
2528 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2529 	int i, j = 0;
2530 	/* Only dump 64 services */
2531 	static const int local_max_services = 64;
2532 	/* If there's more than 64 services, only dump ones with
2533 	 * non-zero counts */
2534 	int only_nonzero = 0;
2535 	static const char *nz = "<-- preventing suspend";
2536 
2537 	enum vc_suspend_status vc_suspend_state;
2538 	enum vc_resume_status  vc_resume_state;
2539 	int peer_count;
2540 	int vc_use_count;
2541 	int active_services;
2542 	struct service_data_struct {
2543 		int fourcc;
2544 		int clientid;
2545 		int use_count;
2546 	} service_data[local_max_services];
2547 
2548 	if (!arm_state)
2549 		return;
2550 
2551 	read_lock_bh(&arm_state->susp_res_lock);
2552 	vc_suspend_state = arm_state->vc_suspend_state;
2553 	vc_resume_state  = arm_state->vc_resume_state;
2554 	peer_count = arm_state->peer_use_count;
2555 	vc_use_count = arm_state->videocore_use_count;
2556 	active_services = state->unused_service;
2557 	if (active_services > local_max_services)
2558 		only_nonzero = 1;
2559 
2560 	for (i = 0; (i < active_services) && (j < local_max_services); i++) {
2561 		VCHIQ_SERVICE_T *service_ptr = state->services[i];
2562 		if (!service_ptr)
2563 			continue;
2564 
2565 		if (only_nonzero && !service_ptr->service_use_count)
2566 			continue;
2567 
2568 		if (service_ptr->srvstate != VCHIQ_SRVSTATE_FREE) {
2569 			service_data[j].fourcc = service_ptr->base.fourcc;
2570 			service_data[j].clientid = service_ptr->client_id;
2571 			service_data[j++].use_count = service_ptr->
2572 							service_use_count;
2573 		}
2574 	}
2575 
2576 	read_unlock_bh(&arm_state->susp_res_lock);
2577 
2578 	vchiq_log_warning(vchiq_susp_log_level,
2579 		"-- Videcore suspend state: %s --",
2580 		suspend_state_names[vc_suspend_state + VC_SUSPEND_NUM_OFFSET]);
2581 	vchiq_log_warning(vchiq_susp_log_level,
2582 		"-- Videcore resume state: %s --",
2583 		resume_state_names[vc_resume_state + VC_RESUME_NUM_OFFSET]);
2584 
2585 	if (only_nonzero)
2586 		vchiq_log_warning(vchiq_susp_log_level, "Too many active "
2587 			"services (%d).  Only dumping up to first %d services "
2588 			"with non-zero use-count", active_services,
2589 			local_max_services);
2590 
2591 	for (i = 0; i < j; i++) {
2592 		vchiq_log_warning(vchiq_susp_log_level,
2593 			"----- %c%c%c%c:%d service count %d %s",
2594 			VCHIQ_FOURCC_AS_4CHARS(service_data[i].fourcc),
2595 			service_data[i].clientid,
2596 			service_data[i].use_count,
2597 			service_data[i].use_count ? nz : "");
2598 	}
2599 	vchiq_log_warning(vchiq_susp_log_level,
2600 		"----- VCHIQ use count count %d", peer_count);
2601 	vchiq_log_warning(vchiq_susp_log_level,
2602 		"--- Overall vchiq instance use count %d", vc_use_count);
2603 
2604 	vchiq_dump_platform_use_state(state);
2605 }
2606 
2607 VCHIQ_STATUS_T
2608 vchiq_check_service(VCHIQ_SERVICE_T *service)
2609 {
2610 	VCHIQ_ARM_STATE_T *arm_state;
2611 	VCHIQ_STATUS_T ret = VCHIQ_ERROR;
2612 
2613 	if (!service || !service->state)
2614 		goto out;
2615 
2616 	vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2617 
2618 	arm_state = vchiq_platform_get_arm_state(service->state);
2619 
2620 	read_lock_bh(&arm_state->susp_res_lock);
2621 	if (service->service_use_count)
2622 		ret = VCHIQ_SUCCESS;
2623 	read_unlock_bh(&arm_state->susp_res_lock);
2624 
2625 	if (ret == VCHIQ_ERROR) {
2626 		vchiq_log_error(vchiq_susp_log_level,
2627 			"%s ERROR - %c%c%c%c:%8x service count %d, "
2628 			"state count %d, videocore suspend state %s", __func__,
2629 			VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2630 			service->client_id, service->service_use_count,
2631 			arm_state->videocore_use_count,
2632 			suspend_state_names[arm_state->vc_suspend_state +
2633 						VC_SUSPEND_NUM_OFFSET]);
2634 		vchiq_dump_service_use_state(service->state);
2635 	}
2636 out:
2637 	return ret;
2638 }
2639 
2640 /* stub functions */
2641 void vchiq_on_remote_use_active(VCHIQ_STATE_T *state)
2642 {
2643 	(void)state;
2644 }
2645 
2646 void vchiq_platform_conn_state_changed(VCHIQ_STATE_T *state,
2647 	VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate)
2648 {
2649 	VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2650 	vchiq_log_info(vchiq_susp_log_level, "%d: %s->%s", state->id,
2651 		get_conn_state_name(oldstate), get_conn_state_name(newstate));
2652 	if (state->conn_state == VCHIQ_CONNSTATE_CONNECTED) {
2653 		write_lock_bh(&arm_state->susp_res_lock);
2654 		if (!arm_state->first_connect) {
2655 			char threadname[10];
2656 			arm_state->first_connect = 1;
2657 			write_unlock_bh(&arm_state->susp_res_lock);
2658 			snprintf(threadname, sizeof(threadname), "VCHIQka-%d",
2659 				state->id);
2660 			arm_state->ka_thread = vchiq_thread_create(
2661 				&vchiq_keepalive_thread_func,
2662 				(void *)state,
2663 				threadname);
2664 			if (arm_state->ka_thread == NULL) {
2665 				vchiq_log_error(vchiq_susp_log_level,
2666 					"vchiq: FATAL: couldn't create thread %s",
2667 					threadname);
2668 			} else {
2669 				wake_up_process(arm_state->ka_thread);
2670 			}
2671 		} else
2672 			write_unlock_bh(&arm_state->susp_res_lock);
2673 	}
2674 }
2675 
2676 /****************************************************************************
2677 *
2678 *   vchiq_init - called when the module is loaded.
2679 *
2680 ***************************************************************************/
2681 
2682 int __init vchiq_init(void);
2683 int __init
2684 vchiq_init(void)
2685 {
2686 	int err;
2687 
2688 #ifdef notyet
2689 	/* create proc entries */
2690 	err = vchiq_proc_init();
2691 	if (err != 0)
2692 		goto failed_proc_init;
2693 #endif
2694 
2695 	vchiq_cdev = make_dev(&vchiq_cdevsw, 0,
2696 	    UID_ROOT, GID_WHEEL, 0600, "vchiq");
2697 	if (!vchiq_cdev) {
2698 		printf("Failed to create /dev/vchiq");
2699 		return (-ENXIO);
2700 	}
2701 
2702 	spin_lock_init(&msg_queue_spinlock);
2703 
2704 	err = vchiq_platform_init(&g_state);
2705 	if (err != 0)
2706 		goto failed_platform_init;
2707 
2708 	vchiq_log_info(vchiq_arm_log_level,
2709 		"vchiq: initialised - version %d (min %d)",
2710 		VCHIQ_VERSION, VCHIQ_VERSION_MIN);
2711 
2712 	return 0;
2713 
2714 failed_platform_init:
2715 	if (vchiq_cdev) {
2716 		destroy_dev(vchiq_cdev);
2717 		vchiq_cdev = NULL;
2718 	}
2719 	vchiq_log_warning(vchiq_arm_log_level, "could not load vchiq");
2720 	return err;
2721 }
2722 
2723 #ifdef notyet
2724 static int vchiq_instance_get_use_count(VCHIQ_INSTANCE_T instance)
2725 {
2726 	VCHIQ_SERVICE_T *service;
2727 	int use_count = 0, i;
2728 	i = 0;
2729 	while ((service = next_service_by_instance(instance->state,
2730 		instance, &i)) != NULL) {
2731 		use_count += service->service_use_count;
2732 		unlock_service(service);
2733 	}
2734 	return use_count;
2735 }
2736 
2737 /* read the per-process use-count */
2738 static int proc_read_use_count(char *page, char **start,
2739 			       off_t off, int count,
2740 			       int *eof, void *data)
2741 {
2742 	VCHIQ_INSTANCE_T instance = data;
2743 	int len, use_count;
2744 
2745 	use_count = vchiq_instance_get_use_count(instance);
2746 	len = snprintf(page+off, count, "%d\n", use_count);
2747 
2748 	return len;
2749 }
2750 
2751 /* add an instance (process) to the proc entries */
2752 static int vchiq_proc_add_instance(VCHIQ_INSTANCE_T instance)
2753 {
2754 	char pidstr[32];
2755 	struct proc_dir_entry *top, *use_count;
2756 	struct proc_dir_entry *clients = vchiq_clients_top();
2757 	int pid = instance->pid;
2758 
2759 	snprintf(pidstr, sizeof(pidstr), "%d", pid);
2760 	top = proc_mkdir(pidstr, clients);
2761 	if (!top)
2762 		goto fail_top;
2763 
2764 	use_count = create_proc_read_entry("use_count",
2765 					   0444, top,
2766 					   proc_read_use_count,
2767 					   instance);
2768 	if (!use_count)
2769 		goto fail_use_count;
2770 
2771 	instance->proc_entry = top;
2772 
2773 	return 0;
2774 
2775 fail_use_count:
2776 	remove_proc_entry(top->name, clients);
2777 fail_top:
2778 	return -ENOMEM;
2779 }
2780 
2781 static void vchiq_proc_remove_instance(VCHIQ_INSTANCE_T instance)
2782 {
2783 	struct proc_dir_entry *clients = vchiq_clients_top();
2784 	remove_proc_entry("use_count", instance->proc_entry);
2785 	remove_proc_entry(instance->proc_entry->name, clients);
2786 }
2787 
2788 #endif
2789 
2790 /****************************************************************************
2791 *
2792 *   vchiq_exit - called when the module is unloaded.
2793 *
2794 ***************************************************************************/
2795 
2796 void vchiq_exit(void);
2797 void
2798 vchiq_exit(void)
2799 {
2800 	if (vchiq_ehtag == NULL)
2801 		EVENTHANDLER_DEREGISTER(dev_clone, vchiq_ehtag);
2802 	vchiq_ehtag = NULL;
2803 
2804 	vchiq_platform_exit(&g_state);
2805 	if (vchiq_cdev) {
2806 		destroy_dev(vchiq_cdev);
2807 		vchiq_cdev = NULL;
2808 	}
2809 }
2810