]> git.proxmox.com Git - mirror_ubuntu-zesty-kernel.git/blame - drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c
projects / mirror_ubuntu-zesty-kernel.git / blame
? search:
summary | shortlog | log | commit | commitdiff | tree
blob | blame (incremental) | history | HEAD
staging: vc04_services: fix dmac_map/unmap_area prototypes
[mirror_ubuntu-zesty-kernel.git] / drivers / staging / vc04_services / interface / vchiq_arm / vchiq_core.c
CommitLineData
71bad7f0 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#include "vchiq_core.h"
35#include "vchiq_killable.h"
36
37#define VCHIQ_SLOT_HANDLER_STACK 8192
38
39#define HANDLE_STATE_SHIFT 12
40
41#define SLOT_INFO_FROM_INDEX(state, index) (state->slot_info + (index))
42#define SLOT_DATA_FROM_INDEX(state, index) (state->slot_data + (index))
43#define SLOT_INDEX_FROM_DATA(state, data) \
44 (((unsigned int)((char *)data - (char *)state->slot_data)) / \
45 VCHIQ_SLOT_SIZE)
46#define SLOT_INDEX_FROM_INFO(state, info) \
47 ((unsigned int)(info - state->slot_info))
48#define SLOT_QUEUE_INDEX_FROM_POS(pos) \
49 ((int)((unsigned int)(pos) / VCHIQ_SLOT_SIZE))
50
51#define BULK_INDEX(x) (x & (VCHIQ_NUM_SERVICE_BULKS - 1))
52
53#define SRVTRACE_LEVEL(srv) \
54 (((srv) && (srv)->trace) ? VCHIQ_LOG_TRACE : vchiq_core_msg_log_level)
55#define SRVTRACE_ENABLED(srv, lev) \
56 (((srv) && (srv)->trace) || (vchiq_core_msg_log_level >= (lev)))
57
58struct vchiq_open_payload {
59 int fourcc;
60 int client_id;
61 short version;
62 short version_min;
63};
64
65struct vchiq_openack_payload {
66 short version;
67};
68
69enum
70{
71 QMFLAGS_IS_BLOCKING = (1 << 0),
72 QMFLAGS_NO_MUTEX_LOCK = (1 << 1),
73 QMFLAGS_NO_MUTEX_UNLOCK = (1 << 2)
74};
75
76/* we require this for consistency between endpoints */
77vchiq_static_assert(sizeof(VCHIQ_HEADER_T) == 8);
78vchiq_static_assert(IS_POW2(sizeof(VCHIQ_HEADER_T)));
79vchiq_static_assert(IS_POW2(VCHIQ_NUM_CURRENT_BULKS));
80vchiq_static_assert(IS_POW2(VCHIQ_NUM_SERVICE_BULKS));
81vchiq_static_assert(IS_POW2(VCHIQ_MAX_SERVICES));
82vchiq_static_assert(VCHIQ_VERSION >= VCHIQ_VERSION_MIN);
83
84/* Run time control of log level, based on KERN_XXX level. */
85int vchiq_core_log_level = VCHIQ_LOG_DEFAULT;
86int vchiq_core_msg_log_level = VCHIQ_LOG_DEFAULT;
87int vchiq_sync_log_level = VCHIQ_LOG_DEFAULT;
88
89static atomic_t pause_bulks_count = ATOMIC_INIT(0);
90
91static DEFINE_SPINLOCK(service_spinlock);
92DEFINE_SPINLOCK(bulk_waiter_spinlock);
93DEFINE_SPINLOCK(quota_spinlock);
94
95VCHIQ_STATE_T *vchiq_states[VCHIQ_MAX_STATES];
96static unsigned int handle_seq;
97
98static const char *const srvstate_names[] = {
99 "FREE",
100 "HIDDEN",
101 "LISTENING",
102 "OPENING",
103 "OPEN",
104 "OPENSYNC",
105 "CLOSESENT",
106 "CLOSERECVD",
107 "CLOSEWAIT",
108 "CLOSED"
109};
110
111static const char *const reason_names[] = {
112 "SERVICE_OPENED",
113 "SERVICE_CLOSED",
114 "MESSAGE_AVAILABLE",
115 "BULK_TRANSMIT_DONE",
116 "BULK_RECEIVE_DONE",
117 "BULK_TRANSMIT_ABORTED",
118 "BULK_RECEIVE_ABORTED"
119};
120
121static const char *const conn_state_names[] = {
122 "DISCONNECTED",
123 "CONNECTING",
124 "CONNECTED",
125 "PAUSING",
126 "PAUSE_SENT",
127 "PAUSED",
128 "RESUMING",
129 "PAUSE_TIMEOUT",
130 "RESUME_TIMEOUT"
131};
132
133
134static void
135release_message_sync(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header);
136
137static const char *msg_type_str(unsigned int msg_type)
138{
139 switch (msg_type) {
140 case VCHIQ_MSG_PADDING: return "PADDING";
141 case VCHIQ_MSG_CONNECT: return "CONNECT";
142 case VCHIQ_MSG_OPEN: return "OPEN";
143 case VCHIQ_MSG_OPENACK: return "OPENACK";
144 case VCHIQ_MSG_CLOSE: return "CLOSE";
145 case VCHIQ_MSG_DATA: return "DATA";
146 case VCHIQ_MSG_BULK_RX: return "BULK_RX";
147 case VCHIQ_MSG_BULK_TX: return "BULK_TX";
148 case VCHIQ_MSG_BULK_RX_DONE: return "BULK_RX_DONE";
149 case VCHIQ_MSG_BULK_TX_DONE: return "BULK_TX_DONE";
150 case VCHIQ_MSG_PAUSE: return "PAUSE";
151 case VCHIQ_MSG_RESUME: return "RESUME";
152 case VCHIQ_MSG_REMOTE_USE: return "REMOTE_USE";
153 case VCHIQ_MSG_REMOTE_RELEASE: return "REMOTE_RELEASE";
154 case VCHIQ_MSG_REMOTE_USE_ACTIVE: return "REMOTE_USE_ACTIVE";
155 }
156 return "???";
157}
158
159static inline void
160vchiq_set_service_state(VCHIQ_SERVICE_T *service, int newstate)
161{
162 vchiq_log_info(vchiq_core_log_level, "%d: srv:%d %s->%s",
163 service->state->id, service->localport,
164 srvstate_names[service->srvstate],
165 srvstate_names[newstate]);
166 service->srvstate = newstate;
167}
168
169VCHIQ_SERVICE_T *
170find_service_by_handle(VCHIQ_SERVICE_HANDLE_T handle)
171{
172 VCHIQ_SERVICE_T *service;
173
174 spin_lock(&service_spinlock);
175 service = handle_to_service(handle);
176 if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
177 (service->handle == handle)) {
178 BUG_ON(service->ref_count == 0);
179 service->ref_count++;
180 } else
181 service = NULL;
182 spin_unlock(&service_spinlock);
183
184 if (!service)
185 vchiq_log_info(vchiq_core_log_level,
186 "Invalid service handle 0x%x", handle);
187
188 return service;
189}
190
191VCHIQ_SERVICE_T *
192find_service_by_port(VCHIQ_STATE_T *state, int localport)
193{
194 VCHIQ_SERVICE_T *service = NULL;
195 if ((unsigned int)localport <= VCHIQ_PORT_MAX) {
196 spin_lock(&service_spinlock);
197 service = state->services[localport];
198 if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE)) {
199 BUG_ON(service->ref_count == 0);
200 service->ref_count++;
201 } else
202 service = NULL;
203 spin_unlock(&service_spinlock);
204 }
205
206 if (!service)
207 vchiq_log_info(vchiq_core_log_level,
208 "Invalid port %d", localport);
209
210 return service;
211}
212
213VCHIQ_SERVICE_T *
214find_service_for_instance(VCHIQ_INSTANCE_T instance,
215 VCHIQ_SERVICE_HANDLE_T handle) {
216 VCHIQ_SERVICE_T *service;
217
218 spin_lock(&service_spinlock);
219 service = handle_to_service(handle);
220 if (service && (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
221 (service->handle == handle) &&
222 (service->instance == instance)) {
223 BUG_ON(service->ref_count == 0);
224 service->ref_count++;
225 } else
226 service = NULL;
227 spin_unlock(&service_spinlock);
228
229 if (!service)
230 vchiq_log_info(vchiq_core_log_level,
231 "Invalid service handle 0x%x", handle);
232
233 return service;
234}
235
236VCHIQ_SERVICE_T *
237find_closed_service_for_instance(VCHIQ_INSTANCE_T instance,
238 VCHIQ_SERVICE_HANDLE_T handle) {
239 VCHIQ_SERVICE_T *service;
240
241 spin_lock(&service_spinlock);
242 service = handle_to_service(handle);
243 if (service &&
244 ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
245 (service->srvstate == VCHIQ_SRVSTATE_CLOSED)) &&
246 (service->handle == handle) &&
247 (service->instance == instance)) {
248 BUG_ON(service->ref_count == 0);
249 service->ref_count++;
250 } else
251 service = NULL;
252 spin_unlock(&service_spinlock);
253
254 if (!service)
255 vchiq_log_info(vchiq_core_log_level,
256 "Invalid service handle 0x%x", handle);
257
258 return service;
259}
260
261VCHIQ_SERVICE_T *
262next_service_by_instance(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance,
263 int *pidx)
264{
265 VCHIQ_SERVICE_T *service = NULL;
266 int idx = *pidx;
267
268 spin_lock(&service_spinlock);
269 while (idx < state->unused_service) {
270 VCHIQ_SERVICE_T *srv = state->services[idx++];
271 if (srv && (srv->srvstate != VCHIQ_SRVSTATE_FREE) &&
272 (srv->instance == instance)) {
273 service = srv;
274 BUG_ON(service->ref_count == 0);
275 service->ref_count++;
276 break;
277 }
278 }
279 spin_unlock(&service_spinlock);
280
281 *pidx = idx;
282
283 return service;
284}
285
286void
287lock_service(VCHIQ_SERVICE_T *service)
288{
289 spin_lock(&service_spinlock);
290 BUG_ON(!service || (service->ref_count == 0));
291 if (service)
292 service->ref_count++;
293 spin_unlock(&service_spinlock);
294}
295
296void
297unlock_service(VCHIQ_SERVICE_T *service)
298{
299 VCHIQ_STATE_T *state = service->state;
300 spin_lock(&service_spinlock);
301 BUG_ON(!service || (service->ref_count == 0));
302 if (service && service->ref_count) {
303 service->ref_count--;
304 if (!service->ref_count) {
305 BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
306 state->services[service->localport] = NULL;
307 } else
308 service = NULL;
309 }
310 spin_unlock(&service_spinlock);
311
312 if (service && service->userdata_term)
313 service->userdata_term(service->base.userdata);
314
315 kfree(service);
316}
317
318int
319vchiq_get_client_id(VCHIQ_SERVICE_HANDLE_T handle)
320{
321 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
322 int id;
323
324 id = service ? service->client_id : 0;
325 if (service)
326 unlock_service(service);
327
328 return id;
329}
330
331void *
332vchiq_get_service_userdata(VCHIQ_SERVICE_HANDLE_T handle)
333{
334 VCHIQ_SERVICE_T *service = handle_to_service(handle);
335
336 return service ? service->base.userdata : NULL;
337}
338
339int
340vchiq_get_service_fourcc(VCHIQ_SERVICE_HANDLE_T handle)
341{
342 VCHIQ_SERVICE_T *service = handle_to_service(handle);
343
344 return service ? service->base.fourcc : 0;
345}
346
347static void
348mark_service_closing_internal(VCHIQ_SERVICE_T *service, int sh_thread)
349{
350 VCHIQ_STATE_T *state = service->state;
351 VCHIQ_SERVICE_QUOTA_T *service_quota;
352
353 service->closing = 1;
354
355 /* Synchronise with other threads. */
356 mutex_lock(&state->recycle_mutex);
357 mutex_unlock(&state->recycle_mutex);
358 if (!sh_thread || (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT)) {
359 /* If we're pausing then the slot_mutex is held until resume
360 * by the slot handler. Therefore don't try to acquire this
361 * mutex if we're the slot handler and in the pause sent state.
362 * We don't need to in this case anyway. */
363 mutex_lock(&state->slot_mutex);
364 mutex_unlock(&state->slot_mutex);
365 }
366
367 /* Unblock any sending thread. */
368 service_quota = &state->service_quotas[service->localport];
369 up(&service_quota->quota_event);
370}
371
372static void
373mark_service_closing(VCHIQ_SERVICE_T *service)
374{
375 mark_service_closing_internal(service, 0);
376}
377
378static inline VCHIQ_STATUS_T
379make_service_callback(VCHIQ_SERVICE_T *service, VCHIQ_REASON_T reason,
380 VCHIQ_HEADER_T *header, void *bulk_userdata)
381{
382 VCHIQ_STATUS_T status;
383 vchiq_log_trace(vchiq_core_log_level, "%d: callback:%d (%s, %x, %x)",
384 service->state->id, service->localport, reason_names[reason],
385 (unsigned int)header, (unsigned int)bulk_userdata);
386 status = service->base.callback(reason, header, service->handle,
387 bulk_userdata);
388 if (status == VCHIQ_ERROR) {
389 vchiq_log_warning(vchiq_core_log_level,
390 "%d: ignoring ERROR from callback to service %x",
391 service->state->id, service->handle);
392 status = VCHIQ_SUCCESS;
393 }
394 return status;
395}
396
397inline void
398vchiq_set_conn_state(VCHIQ_STATE_T *state, VCHIQ_CONNSTATE_T newstate)
399{
400 VCHIQ_CONNSTATE_T oldstate = state->conn_state;
401 vchiq_log_info(vchiq_core_log_level, "%d: %s->%s", state->id,
402 conn_state_names[oldstate],
403 conn_state_names[newstate]);
404 state->conn_state = newstate;
405 vchiq_platform_conn_state_changed(state, oldstate, newstate);
406}
407
408static inline void
409remote_event_create(REMOTE_EVENT_T *event)
410{
411 event->armed = 0;
412 /* Don't clear the 'fired' flag because it may already have been set
413 ** by the other side. */
414 sema_init(event->event, 0);
415}
416
71bad7f0 417static inline int
418remote_event_wait(REMOTE_EVENT_T *event)
419{
420 if (!event->fired) {
421 event->armed = 1;
422 dsb();
423 if (!event->fired) {
424 if (down_interruptible(event->event) != 0) {
425 event->armed = 0;
426 return 0;
427 }
428 }
429 event->armed = 0;
430 wmb();
431 }
432
433 event->fired = 0;
434 return 1;
435}
436
437static inline void
438remote_event_signal_local(REMOTE_EVENT_T *event)
439{
440 event->armed = 0;
441 up(event->event);
442}
443
444static inline void
445remote_event_poll(REMOTE_EVENT_T *event)
446{
447 if (event->fired && event->armed)
448 remote_event_signal_local(event);
449}
450
451void
452remote_event_pollall(VCHIQ_STATE_T *state)
453{
454 remote_event_poll(&state->local->sync_trigger);
455 remote_event_poll(&state->local->sync_release);
456 remote_event_poll(&state->local->trigger);
457 remote_event_poll(&state->local->recycle);
458}
459
460/* Round up message sizes so that any space at the end of a slot is always big
461** enough for a header. This relies on header size being a power of two, which
462** has been verified earlier by a static assertion. */
463
464static inline unsigned int
465calc_stride(unsigned int size)
466{
467 /* Allow room for the header */
468 size += sizeof(VCHIQ_HEADER_T);
469
470 /* Round up */
471 return (size + sizeof(VCHIQ_HEADER_T) - 1) & ~(sizeof(VCHIQ_HEADER_T)
472 - 1);
473}
474
475/* Called by the slot handler thread */
476static VCHIQ_SERVICE_T *
477get_listening_service(VCHIQ_STATE_T *state, int fourcc)
478{
479 int i;
480
481 WARN_ON(fourcc == VCHIQ_FOURCC_INVALID);
482
483 for (i = 0; i < state->unused_service; i++) {
484 VCHIQ_SERVICE_T *service = state->services[i];
485 if (service &&
486 (service->public_fourcc == fourcc) &&
487 ((service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
488 ((service->srvstate == VCHIQ_SRVSTATE_OPEN) &&
489 (service->remoteport == VCHIQ_PORT_FREE)))) {
490 lock_service(service);
491 return service;
492 }
493 }
494
495 return NULL;
496}
497
498/* Called by the slot handler thread */
499static VCHIQ_SERVICE_T *
500get_connected_service(VCHIQ_STATE_T *state, unsigned int port)
501{
502 int i;
503 for (i = 0; i < state->unused_service; i++) {
504 VCHIQ_SERVICE_T *service = state->services[i];
505 if (service && (service->srvstate == VCHIQ_SRVSTATE_OPEN)
506 && (service->remoteport == port)) {
507 lock_service(service);
508 return service;
509 }
510 }
511 return NULL;
512}
513
514inline void
515request_poll(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service, int poll_type)
516{
517 uint32_t value;
518
519 if (service) {
520 do {
521 value = atomic_read(&service->poll_flags);
522 } while (atomic_cmpxchg(&service->poll_flags, value,
523 value | (1 << poll_type)) != value);
524
525 do {
526 value = atomic_read(&state->poll_services[
527 service->localport>>5]);
528 } while (atomic_cmpxchg(
529 &state->poll_services[service->localport>>5],
530 value, value | (1 << (service->localport & 0x1f)))
531 != value);
532 }
533
534 state->poll_needed = 1;
535 wmb();
536
537 /* ... and ensure the slot handler runs. */
538 remote_event_signal_local(&state->local->trigger);
539}
540
541/* Called from queue_message, by the slot handler and application threads,
542** with slot_mutex held */
543static VCHIQ_HEADER_T *
544reserve_space(VCHIQ_STATE_T *state, int space, int is_blocking)
545{
546 VCHIQ_SHARED_STATE_T *local = state->local;
547 int tx_pos = state->local_tx_pos;
548 int slot_space = VCHIQ_SLOT_SIZE - (tx_pos & VCHIQ_SLOT_MASK);
549
550 if (space > slot_space) {
551 VCHIQ_HEADER_T *header;
552 /* Fill the remaining space with padding */
553 WARN_ON(state->tx_data == NULL);
554 header = (VCHIQ_HEADER_T *)
555 (state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
556 header->msgid = VCHIQ_MSGID_PADDING;
557 header->size = slot_space - sizeof(VCHIQ_HEADER_T);
558
559 tx_pos += slot_space;
560 }
561
562 /* If necessary, get the next slot. */
563 if ((tx_pos & VCHIQ_SLOT_MASK) == 0) {
564 int slot_index;
565
566 /* If there is no free slot... */
567
568 if (down_trylock(&state->slot_available_event) != 0) {
569 /* ...wait for one. */
570
571 VCHIQ_STATS_INC(state, slot_stalls);
572
573 /* But first, flush through the last slot. */
574 state->local_tx_pos = tx_pos;
575 local->tx_pos = tx_pos;
576 remote_event_signal(&state->remote->trigger);
577
578 if (!is_blocking ||
579 (down_interruptible(
580 &state->slot_available_event) != 0))
581 return NULL; /* No space available */
582 }
583
584 BUG_ON(tx_pos ==
585 (state->slot_queue_available * VCHIQ_SLOT_SIZE));
586
587 slot_index = local->slot_queue[
588 SLOT_QUEUE_INDEX_FROM_POS(tx_pos) &
589 VCHIQ_SLOT_QUEUE_MASK];
590 state->tx_data =
591 (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
592 }
593
594 state->local_tx_pos = tx_pos + space;
595
596 return (VCHIQ_HEADER_T *)(state->tx_data + (tx_pos & VCHIQ_SLOT_MASK));
597}
598
599/* Called by the recycle thread. */
600static void
601process_free_queue(VCHIQ_STATE_T *state)
602{
603 VCHIQ_SHARED_STATE_T *local = state->local;
604 BITSET_T service_found[BITSET_SIZE(VCHIQ_MAX_SERVICES)];
605 int slot_queue_available;
606
607 /* Use a read memory barrier to ensure that any state that may have
608 ** been modified by another thread is not masked by stale prefetched
609 ** values. */
610 rmb();
611
612 /* Find slots which have been freed by the other side, and return them
613 ** to the available queue. */
614 slot_queue_available = state->slot_queue_available;
615
616 while (slot_queue_available != local->slot_queue_recycle) {
617 unsigned int pos;
618 int slot_index = local->slot_queue[slot_queue_available++ &
619 VCHIQ_SLOT_QUEUE_MASK];
620 char *data = (char *)SLOT_DATA_FROM_INDEX(state, slot_index);
621 int data_found = 0;
622
623 vchiq_log_trace(vchiq_core_log_level, "%d: pfq %d=%x %x %x",
624 state->id, slot_index, (unsigned int)data,
625 local->slot_queue_recycle, slot_queue_available);
626
627 /* Initialise the bitmask for services which have used this
628 ** slot */
629 BITSET_ZERO(service_found);
630
631 pos = 0;
632
633 while (pos < VCHIQ_SLOT_SIZE) {
634 VCHIQ_HEADER_T *header =
635 (VCHIQ_HEADER_T *)(data + pos);
636 int msgid = header->msgid;
637 if (VCHIQ_MSG_TYPE(msgid) == VCHIQ_MSG_DATA) {
638 int port = VCHIQ_MSG_SRCPORT(msgid);
639 VCHIQ_SERVICE_QUOTA_T *service_quota =
640 &state->service_quotas[port];
641 int count;
642 spin_lock(&quota_spinlock);
643 count = service_quota->message_use_count;
644 if (count > 0)
645 service_quota->message_use_count =
646 count - 1;
647 spin_unlock(&quota_spinlock);
648
649 if (count == service_quota->message_quota)
650 /* Signal the service that it
651 ** has dropped below its quota
652 */
653 up(&service_quota->quota_event);
654 else if (count == 0) {
655 vchiq_log_error(vchiq_core_log_level,
656 "service %d "
657 "message_use_count=%d "
658 "(header %x, msgid %x, "
659 "header->msgid %x, "
660 "header->size %x)",
661 port,
662 service_quota->
663 message_use_count,
664 (unsigned int)header, msgid,
665 header->msgid,
666 header->size);
667 WARN(1, "invalid message use count\n");
668 }
669 if (!BITSET_IS_SET(service_found, port)) {
670 /* Set the found bit for this service */
671 BITSET_SET(service_found, port);
672
673 spin_lock(&quota_spinlock);
674 count = service_quota->slot_use_count;
675 if (count > 0)
676 service_quota->slot_use_count =
677 count - 1;
678 spin_unlock(&quota_spinlock);
679
680 if (count > 0) {
681 /* Signal the service in case
682 ** it has dropped below its
683 ** quota */
684 up(&service_quota->quota_event);
685 vchiq_log_trace(
686 vchiq_core_log_level,
687 "%d: pfq:%d %x@%x - "
688 "slot_use->%d",
689 state->id, port,
690 header->size,
691 (unsigned int)header,
692 count - 1);
693 } else {
694 vchiq_log_error(
695 vchiq_core_log_level,
696 "service %d "
697 "slot_use_count"
698 "=%d (header %x"
699 ", msgid %x, "
700 "header->msgid"
701 " %x, header->"
702 "size %x)",
703 port, count,
704 (unsigned int)header,
705 msgid,
706 header->msgid,
707 header->size);
708 WARN(1, "bad slot use count\n");
709 }
710 }
711
712 data_found = 1;
713 }
714
715 pos += calc_stride(header->size);
716 if (pos > VCHIQ_SLOT_SIZE) {
717 vchiq_log_error(vchiq_core_log_level,
718 "pfq - pos %x: header %x, msgid %x, "
719 "header->msgid %x, header->size %x",
720 pos, (unsigned int)header, msgid,
721 header->msgid, header->size);
722 WARN(1, "invalid slot position\n");
723 }
724 }
725
726 if (data_found) {
727 int count;
728 spin_lock(&quota_spinlock);
729 count = state->data_use_count;
730 if (count > 0)
731 state->data_use_count =
732 count - 1;
733 spin_unlock(&quota_spinlock);
734 if (count == state->data_quota)
735 up(&state->data_quota_event);
736 }
737
738 state->slot_queue_available = slot_queue_available;
739 up(&state->slot_available_event);
740 }
741}
742
743/* Called by the slot handler and application threads */
744static VCHIQ_STATUS_T
745queue_message(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
746 int msgid, const VCHIQ_ELEMENT_T *elements,
747 int count, int size, int flags)
748{
749 VCHIQ_SHARED_STATE_T *local;
750 VCHIQ_SERVICE_QUOTA_T *service_quota = NULL;
751 VCHIQ_HEADER_T *header;
752 int type = VCHIQ_MSG_TYPE(msgid);
753
754 unsigned int stride;
755
756 local = state->local;
757
758 stride = calc_stride(size);
759
760 WARN_ON(!(stride <= VCHIQ_SLOT_SIZE));
761
762 if (!(flags & QMFLAGS_NO_MUTEX_LOCK) &&
763 (mutex_lock_interruptible(&state->slot_mutex) != 0))
764 return VCHIQ_RETRY;
765
766 if (type == VCHIQ_MSG_DATA) {
767 int tx_end_index;
768
769 BUG_ON(!service);
770 BUG_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
771 QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
772
773 if (service->closing) {
774 /* The service has been closed */
775 mutex_unlock(&state->slot_mutex);
776 return VCHIQ_ERROR;
777 }
778
779 service_quota = &state->service_quotas[service->localport];
780
781 spin_lock(&quota_spinlock);
782
783 /* Ensure this service doesn't use more than its quota of
784 ** messages or slots */
785 tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(
786 state->local_tx_pos + stride - 1);
787
788 /* Ensure data messages don't use more than their quota of
789 ** slots */
790 while ((tx_end_index != state->previous_data_index) &&
791 (state->data_use_count == state->data_quota)) {
792 VCHIQ_STATS_INC(state, data_stalls);
793 spin_unlock(&quota_spinlock);
794 mutex_unlock(&state->slot_mutex);
795
796 if (down_interruptible(&state->data_quota_event)
797 != 0)
798 return VCHIQ_RETRY;
799
800 mutex_lock(&state->slot_mutex);
801 spin_lock(&quota_spinlock);
802 tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(
803 state->local_tx_pos + stride - 1);
804 if ((tx_end_index == state->previous_data_index) ||
805 (state->data_use_count < state->data_quota)) {
806 /* Pass the signal on to other waiters */
807 up(&state->data_quota_event);
808 break;
809 }
810 }
811
812 while ((service_quota->message_use_count ==
813 service_quota->message_quota) ||
814 ((tx_end_index != service_quota->previous_tx_index) &&
815 (service_quota->slot_use_count ==
816 service_quota->slot_quota))) {
817 spin_unlock(&quota_spinlock);
818 vchiq_log_trace(vchiq_core_log_level,
819 "%d: qm:%d %s,%x - quota stall "
820 "(msg %d, slot %d)",
821 state->id, service->localport,
822 msg_type_str(type), size,
823 service_quota->message_use_count,
824 service_quota->slot_use_count);
825 VCHIQ_SERVICE_STATS_INC(service, quota_stalls);
826 mutex_unlock(&state->slot_mutex);
827 if (down_interruptible(&service_quota->quota_event)
828 != 0)
829 return VCHIQ_RETRY;
830 if (service->closing)
831 return VCHIQ_ERROR;
832 if (mutex_lock_interruptible(&state->slot_mutex) != 0)
833 return VCHIQ_RETRY;
834 if (service->srvstate != VCHIQ_SRVSTATE_OPEN) {
835 /* The service has been closed */
836 mutex_unlock(&state->slot_mutex);
837 return VCHIQ_ERROR;
838 }
839 spin_lock(&quota_spinlock);
840 tx_end_index = SLOT_QUEUE_INDEX_FROM_POS(
841 state->local_tx_pos + stride - 1);
842 }
843
844 spin_unlock(&quota_spinlock);
845 }
846
847 header = reserve_space(state, stride, flags & QMFLAGS_IS_BLOCKING);
848
849 if (!header) {
850 if (service)
851 VCHIQ_SERVICE_STATS_INC(service, slot_stalls);
852 /* In the event of a failure, return the mutex to the
853 state it was in */
854 if (!(flags & QMFLAGS_NO_MUTEX_LOCK))
855 mutex_unlock(&state->slot_mutex);
856 return VCHIQ_RETRY;
857 }
858
859 if (type == VCHIQ_MSG_DATA) {
860 int i, pos;
861 int tx_end_index;
862 int slot_use_count;
863
864 vchiq_log_info(vchiq_core_log_level,
865 "%d: qm %s@%x,%x (%d->%d)",
866 state->id,
867 msg_type_str(VCHIQ_MSG_TYPE(msgid)),
868 (unsigned int)header, size,
869 VCHIQ_MSG_SRCPORT(msgid),
870 VCHIQ_MSG_DSTPORT(msgid));
871
872 BUG_ON(!service);
873 BUG_ON((flags & (QMFLAGS_NO_MUTEX_LOCK |
874 QMFLAGS_NO_MUTEX_UNLOCK)) != 0);
875
876 for (i = 0, pos = 0; i < (unsigned int)count;
877 pos += elements[i++].size)
878 if (elements[i].size) {
879 if (vchiq_copy_from_user
880 (header->data + pos, elements[i].data,
881 (size_t) elements[i].size) !=
882 VCHIQ_SUCCESS) {
883 mutex_unlock(&state->slot_mutex);
884 VCHIQ_SERVICE_STATS_INC(service,
885 error_count);
886 return VCHIQ_ERROR;
887 }
888 if (i == 0) {
889 if (SRVTRACE_ENABLED(service,
890 VCHIQ_LOG_INFO))
891 vchiq_log_dump_mem("Sent", 0,
892 header->data + pos,
893 min(64u,
894 elements[0].size));
895 }
896 }
897
898 spin_lock(&quota_spinlock);
899 service_quota->message_use_count++;
900
901 tx_end_index =
902 SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos - 1);
903
904 /* If this transmission can't fit in the last slot used by any
905 ** service, the data_use_count must be increased. */
906 if (tx_end_index != state->previous_data_index) {
907 state->previous_data_index = tx_end_index;
908 state->data_use_count++;
909 }
910
911 /* If this isn't the same slot last used by this service,
912 ** the service's slot_use_count must be increased. */
913 if (tx_end_index != service_quota->previous_tx_index) {
914 service_quota->previous_tx_index = tx_end_index;
915 slot_use_count = ++service_quota->slot_use_count;
916 } else {
917 slot_use_count = 0;
918 }
919
920 spin_unlock(&quota_spinlock);
921
922 if (slot_use_count)
923 vchiq_log_trace(vchiq_core_log_level,
924 "%d: qm:%d %s,%x - slot_use->%d (hdr %p)",
925 state->id, service->localport,
926 msg_type_str(VCHIQ_MSG_TYPE(msgid)), size,
927 slot_use_count, header);
928
929 VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
930 VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
931 } else {
932 vchiq_log_info(vchiq_core_log_level,
933 "%d: qm %s@%x,%x (%d->%d)", state->id,
934 msg_type_str(VCHIQ_MSG_TYPE(msgid)),
935 (unsigned int)header, size,
936 VCHIQ_MSG_SRCPORT(msgid),
937 VCHIQ_MSG_DSTPORT(msgid));
938 if (size != 0) {
939 WARN_ON(!((count == 1) && (size == elements[0].size)));
940 memcpy(header->data, elements[0].data,
941 elements[0].size);
942 }
943 VCHIQ_STATS_INC(state, ctrl_tx_count);
944 }
945
946 header->msgid = msgid;
947 header->size = size;
948
949 {
950 int svc_fourcc;
951
952 svc_fourcc = service
953 ? service->base.fourcc
954 : VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
955
956 vchiq_log_info(SRVTRACE_LEVEL(service),
957 "Sent Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
958 msg_type_str(VCHIQ_MSG_TYPE(msgid)),
959 VCHIQ_MSG_TYPE(msgid),
960 VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
961 VCHIQ_MSG_SRCPORT(msgid),
962 VCHIQ_MSG_DSTPORT(msgid),
963 size);
964 }
965
966 /* Make sure the new header is visible to the peer. */
967 wmb();
968
969 /* Make the new tx_pos visible to the peer. */
970 local->tx_pos = state->local_tx_pos;
971 wmb();
972
973 if (service && (type == VCHIQ_MSG_CLOSE))
974 vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSESENT);
975
976 if (!(flags & QMFLAGS_NO_MUTEX_UNLOCK))
977 mutex_unlock(&state->slot_mutex);
978
979 remote_event_signal(&state->remote->trigger);
980
981 return VCHIQ_SUCCESS;
982}
983
984/* Called by the slot handler and application threads */
985static VCHIQ_STATUS_T
986queue_message_sync(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
987 int msgid, const VCHIQ_ELEMENT_T *elements,
988 int count, int size, int is_blocking)
989{
990 VCHIQ_SHARED_STATE_T *local;
991 VCHIQ_HEADER_T *header;
992
993 local = state->local;
994
995 if ((VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_RESUME) &&
996 (mutex_lock_interruptible(&state->sync_mutex) != 0))
997 return VCHIQ_RETRY;
998
999 remote_event_wait(&local->sync_release);
1000
1001 rmb();
1002
1003 header = (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state,
1004 local->slot_sync);
1005
1006 {
1007 int oldmsgid = header->msgid;
1008 if (oldmsgid != VCHIQ_MSGID_PADDING)
1009 vchiq_log_error(vchiq_core_log_level,
1010 "%d: qms - msgid %x, not PADDING",
1011 state->id, oldmsgid);
1012 }
1013
1014 if (service) {
1015 int i, pos;
1016
1017 vchiq_log_info(vchiq_sync_log_level,
1018 "%d: qms %s@%x,%x (%d->%d)", state->id,
1019 msg_type_str(VCHIQ_MSG_TYPE(msgid)),
1020 (unsigned int)header, size,
1021 VCHIQ_MSG_SRCPORT(msgid),
1022 VCHIQ_MSG_DSTPORT(msgid));
1023
1024 for (i = 0, pos = 0; i < (unsigned int)count;
1025 pos += elements[i++].size)
1026 if (elements[i].size) {
1027 if (vchiq_copy_from_user
1028 (header->data + pos, elements[i].data,
1029 (size_t) elements[i].size) !=
1030 VCHIQ_SUCCESS) {
1031 mutex_unlock(&state->sync_mutex);
1032 VCHIQ_SERVICE_STATS_INC(service,
1033 error_count);
1034 return VCHIQ_ERROR;
1035 }
1036 if (i == 0) {
1037 if (vchiq_sync_log_level >=
1038 VCHIQ_LOG_TRACE)
1039 vchiq_log_dump_mem("Sent Sync",
1040 0, header->data + pos,
1041 min(64u,
1042 elements[0].size));
1043 }
1044 }
1045
1046 VCHIQ_SERVICE_STATS_INC(service, ctrl_tx_count);
1047 VCHIQ_SERVICE_STATS_ADD(service, ctrl_tx_bytes, size);
1048 } else {
1049 vchiq_log_info(vchiq_sync_log_level,
1050 "%d: qms %s@%x,%x (%d->%d)", state->id,
1051 msg_type_str(VCHIQ_MSG_TYPE(msgid)),
1052 (unsigned int)header, size,
1053 VCHIQ_MSG_SRCPORT(msgid),
1054 VCHIQ_MSG_DSTPORT(msgid));
1055 if (size != 0) {
1056 WARN_ON(!((count == 1) && (size == elements[0].size)));
1057 memcpy(header->data, elements[0].data,
1058 elements[0].size);
1059 }
1060 VCHIQ_STATS_INC(state, ctrl_tx_count);
1061 }
1062
1063 header->size = size;
1064 header->msgid = msgid;
1065
1066 if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) {
1067 int svc_fourcc;
1068
1069 svc_fourcc = service
1070 ? service->base.fourcc
1071 : VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
1072
1073 vchiq_log_trace(vchiq_sync_log_level,
1074 "Sent Sync Msg %s(%u) to %c%c%c%c s:%u d:%d len:%d",
1075 msg_type_str(VCHIQ_MSG_TYPE(msgid)),
1076 VCHIQ_MSG_TYPE(msgid),
1077 VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
1078 VCHIQ_MSG_SRCPORT(msgid),
1079 VCHIQ_MSG_DSTPORT(msgid),
1080 size);
1081 }
1082
1083 /* Make sure the new header is visible to the peer. */
1084 wmb();
1085
1086 remote_event_signal(&state->remote->sync_trigger);
1087
1088 if (VCHIQ_MSG_TYPE(msgid) != VCHIQ_MSG_PAUSE)
1089 mutex_unlock(&state->sync_mutex);
1090
1091 return VCHIQ_SUCCESS;
1092}
1093
1094static inline void
1095claim_slot(VCHIQ_SLOT_INFO_T *slot)
1096{
1097 slot->use_count++;
1098}
1099
1100static void
1101release_slot(VCHIQ_STATE_T *state, VCHIQ_SLOT_INFO_T *slot_info,
1102 VCHIQ_HEADER_T *header, VCHIQ_SERVICE_T *service)
1103{
1104 int release_count;
1105
1106 mutex_lock(&state->recycle_mutex);
1107
1108 if (header) {
1109 int msgid = header->msgid;
1110 if (((msgid & VCHIQ_MSGID_CLAIMED) == 0) ||
1111 (service && service->closing)) {
1112 mutex_unlock(&state->recycle_mutex);
1113 return;
1114 }
1115
1116 /* Rewrite the message header to prevent a double
1117 ** release */
1118 header->msgid = msgid & ~VCHIQ_MSGID_CLAIMED;
1119 }
1120
1121 release_count = slot_info->release_count;
1122 slot_info->release_count = ++release_count;
1123
1124 if (release_count == slot_info->use_count) {
1125 int slot_queue_recycle;
1126 /* Add to the freed queue */
1127
1128 /* A read barrier is necessary here to prevent speculative
1129 ** fetches of remote->slot_queue_recycle from overtaking the
1130 ** mutex. */
1131 rmb();
1132
1133 slot_queue_recycle = state->remote->slot_queue_recycle;
1134 state->remote->slot_queue[slot_queue_recycle &
1135 VCHIQ_SLOT_QUEUE_MASK] =
1136 SLOT_INDEX_FROM_INFO(state, slot_info);
1137 state->remote->slot_queue_recycle = slot_queue_recycle + 1;
1138 vchiq_log_info(vchiq_core_log_level,
1139 "%d: release_slot %d - recycle->%x",
1140 state->id, SLOT_INDEX_FROM_INFO(state, slot_info),
1141 state->remote->slot_queue_recycle);
1142
1143 /* A write barrier is necessary, but remote_event_signal
1144 ** contains one. */
1145 remote_event_signal(&state->remote->recycle);
1146 }
1147
1148 mutex_unlock(&state->recycle_mutex);
1149}
1150
1151/* Called by the slot handler - don't hold the bulk mutex */
1152static VCHIQ_STATUS_T
1153notify_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue,
1154 int retry_poll)
1155{
1156 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
1157
1158 vchiq_log_trace(vchiq_core_log_level,
1159 "%d: nb:%d %cx - p=%x rn=%x r=%x",
1160 service->state->id, service->localport,
1161 (queue == &service->bulk_tx) ? 't' : 'r',
1162 queue->process, queue->remote_notify, queue->remove);
1163
1164 if (service->state->is_master) {
1165 while (queue->remote_notify != queue->process) {
1166 VCHIQ_BULK_T *bulk =
1167 &queue->bulks[BULK_INDEX(queue->remote_notify)];
1168 int msgtype = (bulk->dir == VCHIQ_BULK_TRANSMIT) ?
1169 VCHIQ_MSG_BULK_RX_DONE : VCHIQ_MSG_BULK_TX_DONE;
1170 int msgid = VCHIQ_MAKE_MSG(msgtype, service->localport,
1171 service->remoteport);
1172 VCHIQ_ELEMENT_T element = { &bulk->actual, 4 };
1173 /* Only reply to non-dummy bulk requests */
1174 if (bulk->remote_data) {
1175 status = queue_message(service->state, NULL,
1176 msgid, &element, 1, 4, 0);
1177 if (status != VCHIQ_SUCCESS)
1178 break;
1179 }
1180 queue->remote_notify++;
1181 }
1182 } else {
1183 queue->remote_notify = queue->process;
1184 }
1185
1186 if (status == VCHIQ_SUCCESS) {
1187 while (queue->remove != queue->remote_notify) {
1188 VCHIQ_BULK_T *bulk =
1189 &queue->bulks[BULK_INDEX(queue->remove)];
1190
1191 /* Only generate callbacks for non-dummy bulk
1192 ** requests, and non-terminated services */
1193 if (bulk->data && service->instance) {
1194 if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED) {
1195 if (bulk->dir == VCHIQ_BULK_TRANSMIT) {
1196 VCHIQ_SERVICE_STATS_INC(service,
1197 bulk_tx_count);
1198 VCHIQ_SERVICE_STATS_ADD(service,
1199 bulk_tx_bytes,
1200 bulk->actual);
1201 } else {
1202 VCHIQ_SERVICE_STATS_INC(service,
1203 bulk_rx_count);
1204 VCHIQ_SERVICE_STATS_ADD(service,
1205 bulk_rx_bytes,
1206 bulk->actual);
1207 }
1208 } else {
1209 VCHIQ_SERVICE_STATS_INC(service,
1210 bulk_aborted_count);
1211 }
1212 if (bulk->mode == VCHIQ_BULK_MODE_BLOCKING) {
1213 struct bulk_waiter *waiter;
1214 spin_lock(&bulk_waiter_spinlock);
1215 waiter = bulk->userdata;
1216 if (waiter) {
1217 waiter->actual = bulk->actual;
1218 up(&waiter->event);
1219 }
1220 spin_unlock(&bulk_waiter_spinlock);
1221 } else if (bulk->mode ==
1222 VCHIQ_BULK_MODE_CALLBACK) {
1223 VCHIQ_REASON_T reason = (bulk->dir ==
1224 VCHIQ_BULK_TRANSMIT) ?
1225 ((bulk->actual ==
1226 VCHIQ_BULK_ACTUAL_ABORTED) ?
1227 VCHIQ_BULK_TRANSMIT_ABORTED :
1228 VCHIQ_BULK_TRANSMIT_DONE) :
1229 ((bulk->actual ==
1230 VCHIQ_BULK_ACTUAL_ABORTED) ?
1231 VCHIQ_BULK_RECEIVE_ABORTED :
1232 VCHIQ_BULK_RECEIVE_DONE);
1233 status = make_service_callback(service,
1234 reason, NULL, bulk->userdata);
1235 if (status == VCHIQ_RETRY)
1236 break;
1237 }
1238 }
1239
1240 queue->remove++;
1241 up(&service->bulk_remove_event);
1242 }
1243 if (!retry_poll)
1244 status = VCHIQ_SUCCESS;
1245 }
1246
1247 if (status == VCHIQ_RETRY)
1248 request_poll(service->state, service,
1249 (queue == &service->bulk_tx) ?
1250 VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
1251
1252 return status;
1253}
1254
1255/* Called by the slot handler thread */
1256static void
1257poll_services(VCHIQ_STATE_T *state)
1258{
1259 int group, i;
1260
1261 for (group = 0; group < BITSET_SIZE(state->unused_service); group++) {
1262 uint32_t flags;
1263 flags = atomic_xchg(&state->poll_services[group], 0);
1264 for (i = 0; flags; i++) {
1265 if (flags & (1 << i)) {
1266 VCHIQ_SERVICE_T *service =
1267 find_service_by_port(state,
1268 (group<<5) + i);
1269 uint32_t service_flags;
1270 flags &= ~(1 << i);
1271 if (!service)
1272 continue;
1273 service_flags =
1274 atomic_xchg(&service->poll_flags, 0);
1275 if (service_flags &
1276 (1 << VCHIQ_POLL_REMOVE)) {
1277 vchiq_log_info(vchiq_core_log_level,
1278 "%d: ps - remove %d<->%d",
1279 state->id, service->localport,
1280 service->remoteport);
1281
1282 /* Make it look like a client, because
1283 it must be removed and not left in
1284 the LISTENING state. */
1285 service->public_fourcc =
1286 VCHIQ_FOURCC_INVALID;
1287
1288 if (vchiq_close_service_internal(
1289 service, 0/*!close_recvd*/) !=
1290 VCHIQ_SUCCESS)
1291 request_poll(state, service,
1292 VCHIQ_POLL_REMOVE);
1293 } else if (service_flags &
1294 (1 << VCHIQ_POLL_TERMINATE)) {
1295 vchiq_log_info(vchiq_core_log_level,
1296 "%d: ps - terminate %d<->%d",
1297 state->id, service->localport,
1298 service->remoteport);
1299 if (vchiq_close_service_internal(
1300 service, 0/*!close_recvd*/) !=
1301 VCHIQ_SUCCESS)
1302 request_poll(state, service,
1303 VCHIQ_POLL_TERMINATE);
1304 }
1305 if (service_flags & (1 << VCHIQ_POLL_TXNOTIFY))
1306 notify_bulks(service,
1307 &service->bulk_tx,
1308 1/*retry_poll*/);
1309 if (service_flags & (1 << VCHIQ_POLL_RXNOTIFY))
1310 notify_bulks(service,
1311 &service->bulk_rx,
1312 1/*retry_poll*/);
1313 unlock_service(service);
1314 }
1315 }
1316 }
1317}
1318
1319/* Called by the slot handler or application threads, holding the bulk mutex. */
1320static int
1321resolve_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
1322{
1323 VCHIQ_STATE_T *state = service->state;
1324 int resolved = 0;
1325 int rc;
1326
1327 while ((queue->process != queue->local_insert) &&
1328 (queue->process != queue->remote_insert)) {
1329 VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)];
1330
1331 vchiq_log_trace(vchiq_core_log_level,
1332 "%d: rb:%d %cx - li=%x ri=%x p=%x",
1333 state->id, service->localport,
1334 (queue == &service->bulk_tx) ? 't' : 'r',
1335 queue->local_insert, queue->remote_insert,
1336 queue->process);
1337
1338 WARN_ON(!((int)(queue->local_insert - queue->process) > 0));
1339 WARN_ON(!((int)(queue->remote_insert - queue->process) > 0));
1340
1341 rc = mutex_lock_interruptible(&state->bulk_transfer_mutex);
1342 if (rc != 0)
1343 break;
1344
1345 vchiq_transfer_bulk(bulk);
1346 mutex_unlock(&state->bulk_transfer_mutex);
1347
1348 if (SRVTRACE_ENABLED(service, VCHIQ_LOG_INFO)) {
1349 const char *header = (queue == &service->bulk_tx) ?
1350 "Send Bulk to" : "Recv Bulk from";
1351 if (bulk->actual != VCHIQ_BULK_ACTUAL_ABORTED)
1352 vchiq_log_info(SRVTRACE_LEVEL(service),
1353 "%s %c%c%c%c d:%d len:%d %x<->%x",
1354 header,
1355 VCHIQ_FOURCC_AS_4CHARS(
1356 service->base.fourcc),
1357 service->remoteport,
1358 bulk->size,
1359 (unsigned int)bulk->data,
1360 (unsigned int)bulk->remote_data);
1361 else
1362 vchiq_log_info(SRVTRACE_LEVEL(service),
1363 "%s %c%c%c%c d:%d ABORTED - tx len:%d,"
1364 " rx len:%d %x<->%x",
1365 header,
1366 VCHIQ_FOURCC_AS_4CHARS(
1367 service->base.fourcc),
1368 service->remoteport,
1369 bulk->size,
1370 bulk->remote_size,
1371 (unsigned int)bulk->data,
1372 (unsigned int)bulk->remote_data);
1373 }
1374
1375 vchiq_complete_bulk(bulk);
1376 queue->process++;
1377 resolved++;
1378 }
1379 return resolved;
1380}
1381
1382/* Called with the bulk_mutex held */
1383static void
1384abort_outstanding_bulks(VCHIQ_SERVICE_T *service, VCHIQ_BULK_QUEUE_T *queue)
1385{
1386 int is_tx = (queue == &service->bulk_tx);
1387 vchiq_log_trace(vchiq_core_log_level,
1388 "%d: aob:%d %cx - li=%x ri=%x p=%x",
1389 service->state->id, service->localport, is_tx ? 't' : 'r',
1390 queue->local_insert, queue->remote_insert, queue->process);
1391
1392 WARN_ON(!((int)(queue->local_insert - queue->process) >= 0));
1393 WARN_ON(!((int)(queue->remote_insert - queue->process) >= 0));
1394
1395 while ((queue->process != queue->local_insert) ||
1396 (queue->process != queue->remote_insert)) {
1397 VCHIQ_BULK_T *bulk = &queue->bulks[BULK_INDEX(queue->process)];
1398
1399 if (queue->process == queue->remote_insert) {
1400 /* fabricate a matching dummy bulk */
1401 bulk->remote_data = NULL;
1402 bulk->remote_size = 0;
1403 queue->remote_insert++;
1404 }
1405
1406 if (queue->process != queue->local_insert) {
1407 vchiq_complete_bulk(bulk);
1408
1409 vchiq_log_info(SRVTRACE_LEVEL(service),
1410 "%s %c%c%c%c d:%d ABORTED - tx len:%d, "
1411 "rx len:%d",
1412 is_tx ? "Send Bulk to" : "Recv Bulk from",
1413 VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
1414 service->remoteport,
1415 bulk->size,
1416 bulk->remote_size);
1417 } else {
1418 /* fabricate a matching dummy bulk */
1419 bulk->data = NULL;
1420 bulk->size = 0;
1421 bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
1422 bulk->dir = is_tx ? VCHIQ_BULK_TRANSMIT :
1423 VCHIQ_BULK_RECEIVE;
1424 queue->local_insert++;
1425 }
1426
1427 queue->process++;
1428 }
1429}
1430
1431/* Called from the slot handler thread */
1432static void
1433pause_bulks(VCHIQ_STATE_T *state)
1434{
1435 if (unlikely(atomic_inc_return(&pause_bulks_count) != 1)) {
1436 WARN_ON_ONCE(1);
1437 atomic_set(&pause_bulks_count, 1);
1438 return;
1439 }
1440
1441 /* Block bulk transfers from all services */
1442 mutex_lock(&state->bulk_transfer_mutex);
1443}
1444
1445/* Called from the slot handler thread */
1446static void
1447resume_bulks(VCHIQ_STATE_T *state)
1448{
1449 int i;
1450 if (unlikely(atomic_dec_return(&pause_bulks_count) != 0)) {
1451 WARN_ON_ONCE(1);
1452 atomic_set(&pause_bulks_count, 0);
1453 return;
1454 }
1455
1456 /* Allow bulk transfers from all services */
1457 mutex_unlock(&state->bulk_transfer_mutex);
1458
1459 if (state->deferred_bulks == 0)
1460 return;
1461
1462 /* Deal with any bulks which had to be deferred due to being in
1463 * paused state. Don't try to match up to number of deferred bulks
1464 * in case we've had something come and close the service in the
1465 * interim - just process all bulk queues for all services */
1466 vchiq_log_info(vchiq_core_log_level, "%s: processing %d deferred bulks",
1467 __func__, state->deferred_bulks);
1468
1469 for (i = 0; i < state->unused_service; i++) {
1470 VCHIQ_SERVICE_T *service = state->services[i];
1471 int resolved_rx = 0;
1472 int resolved_tx = 0;
1473 if (!service || (service->srvstate != VCHIQ_SRVSTATE_OPEN))
1474 continue;
1475
1476 mutex_lock(&service->bulk_mutex);
1477 resolved_rx = resolve_bulks(service, &service->bulk_rx);
1478 resolved_tx = resolve_bulks(service, &service->bulk_tx);
1479 mutex_unlock(&service->bulk_mutex);
1480 if (resolved_rx)
1481 notify_bulks(service, &service->bulk_rx, 1);
1482 if (resolved_tx)
1483 notify_bulks(service, &service->bulk_tx, 1);
1484 }
1485 state->deferred_bulks = 0;
1486}
1487
1488static int
1489parse_open(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header)
1490{
1491 VCHIQ_SERVICE_T *service = NULL;
1492 int msgid, size;
1493 int type;
1494 unsigned int localport, remoteport;
1495
1496 msgid = header->msgid;
1497 size = header->size;
1498 type = VCHIQ_MSG_TYPE(msgid);
1499 localport = VCHIQ_MSG_DSTPORT(msgid);
1500 remoteport = VCHIQ_MSG_SRCPORT(msgid);
1501 if (size >= sizeof(struct vchiq_open_payload)) {
1502 const struct vchiq_open_payload *payload =
1503 (struct vchiq_open_payload *)header->data;
1504 unsigned int fourcc;
1505
1506 fourcc = payload->fourcc;
1507 vchiq_log_info(vchiq_core_log_level,
1508 "%d: prs OPEN@%x (%d->'%c%c%c%c')",
1509 state->id, (unsigned int)header,
1510 localport,
1511 VCHIQ_FOURCC_AS_4CHARS(fourcc));
1512
1513 service = get_listening_service(state, fourcc);
1514
1515 if (service) {
1516 /* A matching service exists */
1517 short version = payload->version;
1518 short version_min = payload->version_min;
1519 if ((service->version < version_min) ||
1520 (version < service->version_min)) {
1521 /* Version mismatch */
1522 vchiq_loud_error_header();
1523 vchiq_loud_error("%d: service %d (%c%c%c%c) "
1524 "version mismatch - local (%d, min %d)"
1525 " vs. remote (%d, min %d)",
1526 state->id, service->localport,
1527 VCHIQ_FOURCC_AS_4CHARS(fourcc),
1528 service->version, service->version_min,
1529 version, version_min);
1530 vchiq_loud_error_footer();
1531 unlock_service(service);
1532 service = NULL;
1533 goto fail_open;
1534 }
1535 service->peer_version = version;
1536
1537 if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
1538 struct vchiq_openack_payload ack_payload = {
1539 service->version
1540 };
1541 VCHIQ_ELEMENT_T body = {
1542 &ack_payload,
1543 sizeof(ack_payload)
1544 };
1545
1546 if (state->version_common <
1547 VCHIQ_VERSION_SYNCHRONOUS_MODE)
1548 service->sync = 0;
1549
1550 /* Acknowledge the OPEN */
1551 if (service->sync &&
1552 (state->version_common >=
1553 VCHIQ_VERSION_SYNCHRONOUS_MODE)) {
1554 if (queue_message_sync(state, NULL,
1555 VCHIQ_MAKE_MSG(
1556 VCHIQ_MSG_OPENACK,
1557 service->localport,
1558 remoteport),
1559 &body, 1, sizeof(ack_payload),
1560 0) == VCHIQ_RETRY)
1561 goto bail_not_ready;
1562 } else {
1563 if (queue_message(state, NULL,
1564 VCHIQ_MAKE_MSG(
1565 VCHIQ_MSG_OPENACK,
1566 service->localport,
1567 remoteport),
1568 &body, 1, sizeof(ack_payload),
1569 0) == VCHIQ_RETRY)
1570 goto bail_not_ready;
1571 }
1572
1573 /* The service is now open */
1574 vchiq_set_service_state(service,
1575 service->sync ? VCHIQ_SRVSTATE_OPENSYNC
1576 : VCHIQ_SRVSTATE_OPEN);
1577 }
1578
1579 service->remoteport = remoteport;
1580 service->client_id = ((int *)header->data)[1];
1581 if (make_service_callback(service, VCHIQ_SERVICE_OPENED,
1582 NULL, NULL) == VCHIQ_RETRY) {
1583 /* Bail out if not ready */
1584 service->remoteport = VCHIQ_PORT_FREE;
1585 goto bail_not_ready;
1586 }
1587
1588 /* Success - the message has been dealt with */
1589 unlock_service(service);
1590 return 1;
1591 }
1592 }
1593
1594fail_open:
1595 /* No available service, or an invalid request - send a CLOSE */
1596 if (queue_message(state, NULL,
1597 VCHIQ_MAKE_MSG(VCHIQ_MSG_CLOSE, 0, VCHIQ_MSG_SRCPORT(msgid)),
1598 NULL, 0, 0, 0) == VCHIQ_RETRY)
1599 goto bail_not_ready;
1600
1601 return 1;
1602
1603bail_not_ready:
1604 if (service)
1605 unlock_service(service);
1606
1607 return 0;
1608}
1609
1610/* Called by the slot handler thread */
1611static void
1612parse_rx_slots(VCHIQ_STATE_T *state)
1613{
1614 VCHIQ_SHARED_STATE_T *remote = state->remote;
1615 VCHIQ_SERVICE_T *service = NULL;
1616 int tx_pos;
1617 DEBUG_INITIALISE(state->local)
1618
1619 tx_pos = remote->tx_pos;
1620
1621 while (state->rx_pos != tx_pos) {
1622 VCHIQ_HEADER_T *header;
1623 int msgid, size;
1624 int type;
1625 unsigned int localport, remoteport;
1626
1627 DEBUG_TRACE(PARSE_LINE);
1628 if (!state->rx_data) {
1629 int rx_index;
1630 WARN_ON(!((state->rx_pos & VCHIQ_SLOT_MASK) == 0));
1631 rx_index = remote->slot_queue[
1632 SLOT_QUEUE_INDEX_FROM_POS(state->rx_pos) &
1633 VCHIQ_SLOT_QUEUE_MASK];
1634 state->rx_data = (char *)SLOT_DATA_FROM_INDEX(state,
1635 rx_index);
1636 state->rx_info = SLOT_INFO_FROM_INDEX(state, rx_index);
1637
1638 /* Initialise use_count to one, and increment
1639 ** release_count at the end of the slot to avoid
1640 ** releasing the slot prematurely. */
1641 state->rx_info->use_count = 1;
1642 state->rx_info->release_count = 0;
1643 }
1644
1645 header = (VCHIQ_HEADER_T *)(state->rx_data +
1646 (state->rx_pos & VCHIQ_SLOT_MASK));
1647 DEBUG_VALUE(PARSE_HEADER, (int)header);
1648 msgid = header->msgid;
1649 DEBUG_VALUE(PARSE_MSGID, msgid);
1650 size = header->size;
1651 type = VCHIQ_MSG_TYPE(msgid);
1652 localport = VCHIQ_MSG_DSTPORT(msgid);
1653 remoteport = VCHIQ_MSG_SRCPORT(msgid);
1654
1655 if (type != VCHIQ_MSG_DATA)
1656 VCHIQ_STATS_INC(state, ctrl_rx_count);
1657
1658 switch (type) {
1659 case VCHIQ_MSG_OPENACK:
1660 case VCHIQ_MSG_CLOSE:
1661 case VCHIQ_MSG_DATA:
1662 case VCHIQ_MSG_BULK_RX:
1663 case VCHIQ_MSG_BULK_TX:
1664 case VCHIQ_MSG_BULK_RX_DONE:
1665 case VCHIQ_MSG_BULK_TX_DONE:
1666 service = find_service_by_port(state, localport);
1667 if ((!service ||
1668 ((service->remoteport != remoteport) &&
1669 (service->remoteport != VCHIQ_PORT_FREE))) &&
1670 (localport == 0) &&
1671 (type == VCHIQ_MSG_CLOSE)) {
1672 /* This could be a CLOSE from a client which
1673 hadn't yet received the OPENACK - look for
1674 the connected service */
1675 if (service)
1676 unlock_service(service);
1677 service = get_connected_service(state,
1678 remoteport);
1679 if (service)
1680 vchiq_log_warning(vchiq_core_log_level,
1681 "%d: prs %s@%x (%d->%d) - "
1682 "found connected service %d",
1683 state->id, msg_type_str(type),
1684 (unsigned int)header,
1685 remoteport, localport,
1686 service->localport);
1687 }
1688
1689 if (!service) {
1690 vchiq_log_error(vchiq_core_log_level,
1691 "%d: prs %s@%x (%d->%d) - "
1692 "invalid/closed service %d",
1693 state->id, msg_type_str(type),
1694 (unsigned int)header,
1695 remoteport, localport, localport);
1696 goto skip_message;
1697 }
1698 break;
1699 default:
1700 break;
1701 }
1702
1703 if (SRVTRACE_ENABLED(service, VCHIQ_LOG_INFO)) {
1704 int svc_fourcc;
1705
1706 svc_fourcc = service
1707 ? service->base.fourcc
1708 : VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
1709 vchiq_log_info(SRVTRACE_LEVEL(service),
1710 "Rcvd Msg %s(%u) from %c%c%c%c s:%d d:%d "
1711 "len:%d",
1712 msg_type_str(type), type,
1713 VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
1714 remoteport, localport, size);
1715 if (size > 0)
1716 vchiq_log_dump_mem("Rcvd", 0, header->data,
1717 min(64, size));
1718 }
1719
1720 if (((unsigned int)header & VCHIQ_SLOT_MASK) + calc_stride(size)
1721 > VCHIQ_SLOT_SIZE) {
1722 vchiq_log_error(vchiq_core_log_level,
1723 "header %x (msgid %x) - size %x too big for "
1724 "slot",
1725 (unsigned int)header, (unsigned int)msgid,
1726 (unsigned int)size);
1727 WARN(1, "oversized for slot\n");
1728 }
1729
1730 switch (type) {
1731 case VCHIQ_MSG_OPEN:
1732 WARN_ON(!(VCHIQ_MSG_DSTPORT(msgid) == 0));
1733 if (!parse_open(state, header))
1734 goto bail_not_ready;
1735 break;
1736 case VCHIQ_MSG_OPENACK:
1737 if (size >= sizeof(struct vchiq_openack_payload)) {
1738 const struct vchiq_openack_payload *payload =
1739 (struct vchiq_openack_payload *)
1740 header->data;
1741 service->peer_version = payload->version;
1742 }
1743 vchiq_log_info(vchiq_core_log_level,
1744 "%d: prs OPENACK@%x,%x (%d->%d) v:%d",
1745 state->id, (unsigned int)header, size,
1746 remoteport, localport, service->peer_version);
1747 if (service->srvstate ==
1748 VCHIQ_SRVSTATE_OPENING) {
1749 service->remoteport = remoteport;
1750 vchiq_set_service_state(service,
1751 VCHIQ_SRVSTATE_OPEN);
1752 up(&service->remove_event);
1753 } else
1754 vchiq_log_error(vchiq_core_log_level,
1755 "OPENACK received in state %s",
1756 srvstate_names[service->srvstate]);
1757 break;
1758 case VCHIQ_MSG_CLOSE:
1759 WARN_ON(size != 0); /* There should be no data */
1760
1761 vchiq_log_info(vchiq_core_log_level,
1762 "%d: prs CLOSE@%x (%d->%d)",
1763 state->id, (unsigned int)header,
1764 remoteport, localport);
1765
1766 mark_service_closing_internal(service, 1);
1767
1768 if (vchiq_close_service_internal(service,
1769 1/*close_recvd*/) == VCHIQ_RETRY)
1770 goto bail_not_ready;
1771
1772 vchiq_log_info(vchiq_core_log_level,
1773 "Close Service %c%c%c%c s:%u d:%d",
1774 VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
1775 service->localport,
1776 service->remoteport);
1777 break;
1778 case VCHIQ_MSG_DATA:
1779 vchiq_log_info(vchiq_core_log_level,
1780 "%d: prs DATA@%x,%x (%d->%d)",
1781 state->id, (unsigned int)header, size,
1782 remoteport, localport);
1783
1784 if ((service->remoteport == remoteport)
1785 && (service->srvstate ==
1786 VCHIQ_SRVSTATE_OPEN)) {
1787 header->msgid = msgid | VCHIQ_MSGID_CLAIMED;
1788 claim_slot(state->rx_info);
1789 DEBUG_TRACE(PARSE_LINE);
1790 if (make_service_callback(service,
1791 VCHIQ_MESSAGE_AVAILABLE, header,
1792 NULL) == VCHIQ_RETRY) {
1793 DEBUG_TRACE(PARSE_LINE);
1794 goto bail_not_ready;
1795 }
1796 VCHIQ_SERVICE_STATS_INC(service, ctrl_rx_count);
1797 VCHIQ_SERVICE_STATS_ADD(service, ctrl_rx_bytes,
1798 size);
1799 } else {
1800 VCHIQ_STATS_INC(state, error_count);
1801 }
1802 break;
1803 case VCHIQ_MSG_CONNECT:
1804 vchiq_log_info(vchiq_core_log_level,
1805 "%d: prs CONNECT@%x",
1806 state->id, (unsigned int)header);
1807 state->version_common = ((VCHIQ_SLOT_ZERO_T *)
1808 state->slot_data)->version;
1809 up(&state->connect);
1810 break;
1811 case VCHIQ_MSG_BULK_RX:
1812 case VCHIQ_MSG_BULK_TX: {
1813 VCHIQ_BULK_QUEUE_T *queue;
1814 WARN_ON(!state->is_master);
1815 queue = (type == VCHIQ_MSG_BULK_RX) ?
1816 &service->bulk_tx : &service->bulk_rx;
1817 if ((service->remoteport == remoteport)
1818 && (service->srvstate ==
1819 VCHIQ_SRVSTATE_OPEN)) {
1820 VCHIQ_BULK_T *bulk;
1821 int resolved = 0;
1822
1823 DEBUG_TRACE(PARSE_LINE);
1824 if (mutex_lock_interruptible(
1825 &service->bulk_mutex) != 0) {
1826 DEBUG_TRACE(PARSE_LINE);
1827 goto bail_not_ready;
1828 }
1829
1830 WARN_ON(!(queue->remote_insert < queue->remove +
1831 VCHIQ_NUM_SERVICE_BULKS));
1832 bulk = &queue->bulks[
1833 BULK_INDEX(queue->remote_insert)];
1834 bulk->remote_data =
1835 (void *)((int *)header->data)[0];
1836 bulk->remote_size = ((int *)header->data)[1];
1837 wmb();
1838
1839 vchiq_log_info(vchiq_core_log_level,
1840 "%d: prs %s@%x (%d->%d) %x@%x",
1841 state->id, msg_type_str(type),
1842 (unsigned int)header,
1843 remoteport, localport,
1844 bulk->remote_size,
1845 (unsigned int)bulk->remote_data);
1846
1847 queue->remote_insert++;
1848
1849 if (atomic_read(&pause_bulks_count)) {
1850 state->deferred_bulks++;
1851 vchiq_log_info(vchiq_core_log_level,
1852 "%s: deferring bulk (%d)",
1853 __func__,
1854 state->deferred_bulks);
1855 if (state->conn_state !=
1856 VCHIQ_CONNSTATE_PAUSE_SENT)
1857 vchiq_log_error(
1858 vchiq_core_log_level,
1859 "%s: bulks paused in "
1860 "unexpected state %s",
1861 __func__,
1862 conn_state_names[
1863 state->conn_state]);
1864 } else if (state->conn_state ==
1865 VCHIQ_CONNSTATE_CONNECTED) {
1866 DEBUG_TRACE(PARSE_LINE);
1867 resolved = resolve_bulks(service,
1868 queue);
1869 }
1870
1871 mutex_unlock(&service->bulk_mutex);
1872 if (resolved)
1873 notify_bulks(service, queue,
1874 1/*retry_poll*/);
1875 }
1876 } break;
1877 case VCHIQ_MSG_BULK_RX_DONE:
1878 case VCHIQ_MSG_BULK_TX_DONE:
1879 WARN_ON(state->is_master);
1880 if ((service->remoteport == remoteport)
1881 && (service->srvstate !=
1882 VCHIQ_SRVSTATE_FREE)) {
1883 VCHIQ_BULK_QUEUE_T *queue;
1884 VCHIQ_BULK_T *bulk;
1885
1886 queue = (type == VCHIQ_MSG_BULK_RX_DONE) ?
1887 &service->bulk_rx : &service->bulk_tx;
1888
1889 DEBUG_TRACE(PARSE_LINE);
1890 if (mutex_lock_interruptible(
1891 &service->bulk_mutex) != 0) {
1892 DEBUG_TRACE(PARSE_LINE);
1893 goto bail_not_ready;
1894 }
1895 if ((int)(queue->remote_insert -
1896 queue->local_insert) >= 0) {
1897 vchiq_log_error(vchiq_core_log_level,
1898 "%d: prs %s@%x (%d->%d) "
1899 "unexpected (ri=%d,li=%d)",
1900 state->id, msg_type_str(type),
1901 (unsigned int)header,
1902 remoteport, localport,
1903 queue->remote_insert,
1904 queue->local_insert);
1905 mutex_unlock(&service->bulk_mutex);
1906 break;
1907 }
1908
1909 BUG_ON(queue->process == queue->local_insert);
1910 BUG_ON(queue->process != queue->remote_insert);
1911
1912 bulk = &queue->bulks[
1913 BULK_INDEX(queue->remote_insert)];
1914 bulk->actual = *(int *)header->data;
1915 queue->remote_insert++;
1916
1917 vchiq_log_info(vchiq_core_log_level,
1918 "%d: prs %s@%x (%d->%d) %x@%x",
1919 state->id, msg_type_str(type),
1920 (unsigned int)header,
1921 remoteport, localport,
1922 bulk->actual, (unsigned int)bulk->data);
1923
1924 vchiq_log_trace(vchiq_core_log_level,
1925 "%d: prs:%d %cx li=%x ri=%x p=%x",
1926 state->id, localport,
1927 (type == VCHIQ_MSG_BULK_RX_DONE) ?
1928 'r' : 't',
1929 queue->local_insert,
1930 queue->remote_insert, queue->process);
1931
1932 DEBUG_TRACE(PARSE_LINE);
1933 WARN_ON(queue->process == queue->local_insert);
1934 vchiq_complete_bulk(bulk);
1935 queue->process++;
1936 mutex_unlock(&service->bulk_mutex);
1937 DEBUG_TRACE(PARSE_LINE);
1938 notify_bulks(service, queue, 1/*retry_poll*/);
1939 DEBUG_TRACE(PARSE_LINE);
1940 }
1941 break;
1942 case VCHIQ_MSG_PADDING:
1943 vchiq_log_trace(vchiq_core_log_level,
1944 "%d: prs PADDING@%x,%x",
1945 state->id, (unsigned int)header, size);
1946 break;
1947 case VCHIQ_MSG_PAUSE:
1948 /* If initiated, signal the application thread */
1949 vchiq_log_trace(vchiq_core_log_level,
1950 "%d: prs PAUSE@%x,%x",
1951 state->id, (unsigned int)header, size);
1952 if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) {
1953 vchiq_log_error(vchiq_core_log_level,
1954 "%d: PAUSE received in state PAUSED",
1955 state->id);
1956 break;
1957 }
1958 if (state->conn_state != VCHIQ_CONNSTATE_PAUSE_SENT) {
1959 /* Send a PAUSE in response */
1960 if (queue_message(state, NULL,
1961 VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0),
1962 NULL, 0, 0, QMFLAGS_NO_MUTEX_UNLOCK)
1963 == VCHIQ_RETRY)
1964 goto bail_not_ready;
1965 if (state->is_master)
1966 pause_bulks(state);
1967 }
1968 /* At this point slot_mutex is held */
1969 vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSED);
1970 vchiq_platform_paused(state);
1971 break;
1972 case VCHIQ_MSG_RESUME:
1973 vchiq_log_trace(vchiq_core_log_level,
1974 "%d: prs RESUME@%x,%x",
1975 state->id, (unsigned int)header, size);
1976 /* Release the slot mutex */
1977 mutex_unlock(&state->slot_mutex);
1978 if (state->is_master)
1979 resume_bulks(state);
1980 vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
1981 vchiq_platform_resumed(state);
1982 break;
1983
1984 case VCHIQ_MSG_REMOTE_USE:
1985 vchiq_on_remote_use(state);
1986 break;
1987 case VCHIQ_MSG_REMOTE_RELEASE:
1988 vchiq_on_remote_release(state);
1989 break;
1990 case VCHIQ_MSG_REMOTE_USE_ACTIVE:
1991 vchiq_on_remote_use_active(state);
1992 break;
1993
1994 default:
1995 vchiq_log_error(vchiq_core_log_level,
1996 "%d: prs invalid msgid %x@%x,%x",
1997 state->id, msgid, (unsigned int)header, size);
1998 WARN(1, "invalid message\n");
1999 break;
2000 }
2001
2002skip_message:
2003 if (service) {
2004 unlock_service(service);
2005 service = NULL;
2006 }
2007
2008 state->rx_pos += calc_stride(size);
2009
2010 DEBUG_TRACE(PARSE_LINE);
2011 /* Perform some housekeeping when the end of the slot is
2012 ** reached. */
2013 if ((state->rx_pos & VCHIQ_SLOT_MASK) == 0) {
2014 /* Remove the extra reference count. */
2015 release_slot(state, state->rx_info, NULL, NULL);
2016 state->rx_data = NULL;
2017 }
2018 }
2019
2020bail_not_ready:
2021 if (service)
2022 unlock_service(service);
2023}
2024
2025/* Called by the slot handler thread */
2026static int
2027slot_handler_func(void *v)
2028{
2029 VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
2030 VCHIQ_SHARED_STATE_T *local = state->local;
2031 DEBUG_INITIALISE(local)
2032
2033 while (1) {
2034 DEBUG_COUNT(SLOT_HANDLER_COUNT);
2035 DEBUG_TRACE(SLOT_HANDLER_LINE);
2036 remote_event_wait(&local->trigger);
2037
2038 rmb();
2039
2040 DEBUG_TRACE(SLOT_HANDLER_LINE);
2041 if (state->poll_needed) {
2042 /* Check if we need to suspend - may change our
2043 * conn_state */
2044 vchiq_platform_check_suspend(state);
2045
2046 state->poll_needed = 0;
2047
2048 /* Handle service polling and other rare conditions here
2049 ** out of the mainline code */
2050 switch (state->conn_state) {
2051 case VCHIQ_CONNSTATE_CONNECTED:
2052 /* Poll the services as requested */
2053 poll_services(state);
2054 break;
2055
2056 case VCHIQ_CONNSTATE_PAUSING:
2057 if (state->is_master)
2058 pause_bulks(state);
2059 if (queue_message(state, NULL,
2060 VCHIQ_MAKE_MSG(VCHIQ_MSG_PAUSE, 0, 0),
2061 NULL, 0, 0,
2062 QMFLAGS_NO_MUTEX_UNLOCK)
2063 != VCHIQ_RETRY) {
2064 vchiq_set_conn_state(state,
2065 VCHIQ_CONNSTATE_PAUSE_SENT);
2066 } else {
2067 if (state->is_master)
2068 resume_bulks(state);
2069 /* Retry later */
2070 state->poll_needed = 1;
2071 }
2072 break;
2073
2074 case VCHIQ_CONNSTATE_PAUSED:
2075 vchiq_platform_resume(state);
2076 break;
2077
2078 case VCHIQ_CONNSTATE_RESUMING:
2079 if (queue_message(state, NULL,
2080 VCHIQ_MAKE_MSG(VCHIQ_MSG_RESUME, 0, 0),
2081 NULL, 0, 0, QMFLAGS_NO_MUTEX_LOCK)
2082 != VCHIQ_RETRY) {
2083 if (state->is_master)
2084 resume_bulks(state);
2085 vchiq_set_conn_state(state,
2086 VCHIQ_CONNSTATE_CONNECTED);
2087 vchiq_platform_resumed(state);
2088 } else {
2089 /* This should really be impossible,
2090 ** since the PAUSE should have flushed
2091 ** through outstanding messages. */
2092 vchiq_log_error(vchiq_core_log_level,
2093 "Failed to send RESUME "
2094 "message");
2095 BUG();
2096 }
2097 break;
2098
2099 case VCHIQ_CONNSTATE_PAUSE_TIMEOUT:
2100 case VCHIQ_CONNSTATE_RESUME_TIMEOUT:
2101 vchiq_platform_handle_timeout(state);
2102 break;
2103 default:
2104 break;
2105 }
2106
2107
2108 }
2109
2110 DEBUG_TRACE(SLOT_HANDLER_LINE);
2111 parse_rx_slots(state);
2112 }
2113 return 0;
2114}
2115
2116
2117/* Called by the recycle thread */
2118static int
2119recycle_func(void *v)
2120{
2121 VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
2122 VCHIQ_SHARED_STATE_T *local = state->local;
2123
2124 while (1) {
2125 remote_event_wait(&local->recycle);
2126
2127 process_free_queue(state);
2128 }
2129 return 0;
2130}
2131
2132
2133/* Called by the sync thread */
2134static int
2135sync_func(void *v)
2136{
2137 VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
2138 VCHIQ_SHARED_STATE_T *local = state->local;
2139 VCHIQ_HEADER_T *header = (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state,
2140 state->remote->slot_sync);
2141
2142 while (1) {
2143 VCHIQ_SERVICE_T *service;
2144 int msgid, size;
2145 int type;
2146 unsigned int localport, remoteport;
2147
2148 remote_event_wait(&local->sync_trigger);
2149
2150 rmb();
2151
2152 msgid = header->msgid;
2153 size = header->size;
2154 type = VCHIQ_MSG_TYPE(msgid);
2155 localport = VCHIQ_MSG_DSTPORT(msgid);
2156 remoteport = VCHIQ_MSG_SRCPORT(msgid);
2157
2158 service = find_service_by_port(state, localport);
2159
2160 if (!service) {
2161 vchiq_log_error(vchiq_sync_log_level,
2162 "%d: sf %s@%x (%d->%d) - "
2163 "invalid/closed service %d",
2164 state->id, msg_type_str(type),
2165 (unsigned int)header,
2166 remoteport, localport, localport);
2167 release_message_sync(state, header);
2168 continue;
2169 }
2170
2171 if (vchiq_sync_log_level >= VCHIQ_LOG_TRACE) {
2172 int svc_fourcc;
2173
2174 svc_fourcc = service
2175 ? service->base.fourcc
2176 : VCHIQ_MAKE_FOURCC('?', '?', '?', '?');
2177 vchiq_log_trace(vchiq_sync_log_level,
2178 "Rcvd Msg %s from %c%c%c%c s:%d d:%d len:%d",
2179 msg_type_str(type),
2180 VCHIQ_FOURCC_AS_4CHARS(svc_fourcc),
2181 remoteport, localport, size);
2182 if (size > 0)
2183 vchiq_log_dump_mem("Rcvd", 0, header->data,
2184 min(64, size));
2185 }
2186
2187 switch (type) {
2188 case VCHIQ_MSG_OPENACK:
2189 if (size >= sizeof(struct vchiq_openack_payload)) {
2190 const struct vchiq_openack_payload *payload =
2191 (struct vchiq_openack_payload *)
2192 header->data;
2193 service->peer_version = payload->version;
2194 }
2195 vchiq_log_info(vchiq_sync_log_level,
2196 "%d: sf OPENACK@%x,%x (%d->%d) v:%d",
2197 state->id, (unsigned int)header, size,
2198 remoteport, localport, service->peer_version);
2199 if (service->srvstate == VCHIQ_SRVSTATE_OPENING) {
2200 service->remoteport = remoteport;
2201 vchiq_set_service_state(service,
2202 VCHIQ_SRVSTATE_OPENSYNC);
2203 service->sync = 1;
2204 up(&service->remove_event);
2205 }
2206 release_message_sync(state, header);
2207 break;
2208
2209 case VCHIQ_MSG_DATA:
2210 vchiq_log_trace(vchiq_sync_log_level,
2211 "%d: sf DATA@%x,%x (%d->%d)",
2212 state->id, (unsigned int)header, size,
2213 remoteport, localport);
2214
2215 if ((service->remoteport == remoteport) &&
2216 (service->srvstate ==
2217 VCHIQ_SRVSTATE_OPENSYNC)) {
2218 if (make_service_callback(service,
2219 VCHIQ_MESSAGE_AVAILABLE, header,
2220 NULL) == VCHIQ_RETRY)
2221 vchiq_log_error(vchiq_sync_log_level,
2222 "synchronous callback to "
2223 "service %d returns "
2224 "VCHIQ_RETRY",
2225 localport);
2226 }
2227 break;
2228
2229 default:
2230 vchiq_log_error(vchiq_sync_log_level,
2231 "%d: sf unexpected msgid %x@%x,%x",
2232 state->id, msgid, (unsigned int)header, size);
2233 release_message_sync(state, header);
2234 break;
2235 }
2236
2237 unlock_service(service);
2238 }
2239
2240 return 0;
2241}
2242
2243
2244static void
2245init_bulk_queue(VCHIQ_BULK_QUEUE_T *queue)
2246{
2247 queue->local_insert = 0;
2248 queue->remote_insert = 0;
2249 queue->process = 0;
2250 queue->remote_notify = 0;
2251 queue->remove = 0;
2252}
2253
2254
2255inline const char *
2256get_conn_state_name(VCHIQ_CONNSTATE_T conn_state)
2257{
2258 return conn_state_names[conn_state];
2259}
2260
2261
2262VCHIQ_SLOT_ZERO_T *
2263vchiq_init_slots(void *mem_base, int mem_size)
2264{
2265 int mem_align = (VCHIQ_SLOT_SIZE - (int)mem_base) & VCHIQ_SLOT_MASK;
2266 VCHIQ_SLOT_ZERO_T *slot_zero =
2267 (VCHIQ_SLOT_ZERO_T *)((char *)mem_base + mem_align);
2268 int num_slots = (mem_size - mem_align)/VCHIQ_SLOT_SIZE;
2269 int first_data_slot = VCHIQ_SLOT_ZERO_SLOTS;
2270
2271 /* Ensure there is enough memory to run an absolutely minimum system */
2272 num_slots -= first_data_slot;
2273
2274 if (num_slots < 4) {
2275 vchiq_log_error(vchiq_core_log_level,
2276 "vchiq_init_slots - insufficient memory %x bytes",
2277 mem_size);
2278 return NULL;
2279 }
2280
2281 memset(slot_zero, 0, sizeof(VCHIQ_SLOT_ZERO_T));
2282
2283 slot_zero->magic = VCHIQ_MAGIC;
2284 slot_zero->version = VCHIQ_VERSION;
2285 slot_zero->version_min = VCHIQ_VERSION_MIN;
2286 slot_zero->slot_zero_size = sizeof(VCHIQ_SLOT_ZERO_T);
2287 slot_zero->slot_size = VCHIQ_SLOT_SIZE;
2288 slot_zero->max_slots = VCHIQ_MAX_SLOTS;
2289 slot_zero->max_slots_per_side = VCHIQ_MAX_SLOTS_PER_SIDE;
2290
2291 slot_zero->master.slot_sync = first_data_slot;
2292 slot_zero->master.slot_first = first_data_slot + 1;
2293 slot_zero->master.slot_last = first_data_slot + (num_slots/2) - 1;
2294 slot_zero->slave.slot_sync = first_data_slot + (num_slots/2);
2295 slot_zero->slave.slot_first = first_data_slot + (num_slots/2) + 1;
2296 slot_zero->slave.slot_last = first_data_slot + num_slots - 1;
2297
2298 return slot_zero;
2299}
2300
2301VCHIQ_STATUS_T
2302vchiq_init_state(VCHIQ_STATE_T *state, VCHIQ_SLOT_ZERO_T *slot_zero,
2303 int is_master)
2304{
2305 VCHIQ_SHARED_STATE_T *local;
2306 VCHIQ_SHARED_STATE_T *remote;
2307 VCHIQ_STATUS_T status;
2308 char threadname[10];
2309 static int id;
2310 int i;
2311
2312 vchiq_log_warning(vchiq_core_log_level,
2313 "%s: slot_zero = 0x%08lx, is_master = %d",
2314 __func__, (unsigned long)slot_zero, is_master);
2315
2316 /* Check the input configuration */
2317
2318 if (slot_zero->magic != VCHIQ_MAGIC) {
2319 vchiq_loud_error_header();
2320 vchiq_loud_error("Invalid VCHIQ magic value found.");
2321 vchiq_loud_error("slot_zero=%x: magic=%x (expected %x)",
2322 (unsigned int)slot_zero, slot_zero->magic, VCHIQ_MAGIC);
2323 vchiq_loud_error_footer();
2324 return VCHIQ_ERROR;
2325 }
2326
2327 if (slot_zero->version < VCHIQ_VERSION_MIN) {
2328 vchiq_loud_error_header();
2329 vchiq_loud_error("Incompatible VCHIQ versions found.");
2330 vchiq_loud_error("slot_zero=%x: VideoCore version=%d "
2331 "(minimum %d)",
2332 (unsigned int)slot_zero, slot_zero->version,
2333 VCHIQ_VERSION_MIN);
2334 vchiq_loud_error("Restart with a newer VideoCore image.");
2335 vchiq_loud_error_footer();
2336 return VCHIQ_ERROR;
2337 }
2338
2339 if (VCHIQ_VERSION < slot_zero->version_min) {
2340 vchiq_loud_error_header();
2341 vchiq_loud_error("Incompatible VCHIQ versions found.");
2342 vchiq_loud_error("slot_zero=%x: version=%d (VideoCore "
2343 "minimum %d)",
2344 (unsigned int)slot_zero, VCHIQ_VERSION,
2345 slot_zero->version_min);
2346 vchiq_loud_error("Restart with a newer kernel.");
2347 vchiq_loud_error_footer();
2348 return VCHIQ_ERROR;
2349 }
2350
2351 if ((slot_zero->slot_zero_size != sizeof(VCHIQ_SLOT_ZERO_T)) ||
2352 (slot_zero->slot_size != VCHIQ_SLOT_SIZE) ||
2353 (slot_zero->max_slots != VCHIQ_MAX_SLOTS) ||
2354 (slot_zero->max_slots_per_side != VCHIQ_MAX_SLOTS_PER_SIDE)) {
2355 vchiq_loud_error_header();
2356 if (slot_zero->slot_zero_size != sizeof(VCHIQ_SLOT_ZERO_T))
2357 vchiq_loud_error("slot_zero=%x: slot_zero_size=%x "
2358 "(expected %x)",
2359 (unsigned int)slot_zero,
2360 slot_zero->slot_zero_size,
2361 sizeof(VCHIQ_SLOT_ZERO_T));
2362 if (slot_zero->slot_size != VCHIQ_SLOT_SIZE)
2363 vchiq_loud_error("slot_zero=%x: slot_size=%d "
2364 "(expected %d",
2365 (unsigned int)slot_zero, slot_zero->slot_size,
2366 VCHIQ_SLOT_SIZE);
2367 if (slot_zero->max_slots != VCHIQ_MAX_SLOTS)
2368 vchiq_loud_error("slot_zero=%x: max_slots=%d "
2369 "(expected %d)",
2370 (unsigned int)slot_zero, slot_zero->max_slots,
2371 VCHIQ_MAX_SLOTS);
2372 if (slot_zero->max_slots_per_side != VCHIQ_MAX_SLOTS_PER_SIDE)
2373 vchiq_loud_error("slot_zero=%x: max_slots_per_side=%d "
2374 "(expected %d)",
2375 (unsigned int)slot_zero,
2376 slot_zero->max_slots_per_side,
2377 VCHIQ_MAX_SLOTS_PER_SIDE);
2378 vchiq_loud_error_footer();
2379 return VCHIQ_ERROR;
2380 }
2381
2382 if (VCHIQ_VERSION < slot_zero->version)
2383 slot_zero->version = VCHIQ_VERSION;
2384
2385 if (is_master) {
2386 local = &slot_zero->master;
2387 remote = &slot_zero->slave;
2388 } else {
2389 local = &slot_zero->slave;
2390 remote = &slot_zero->master;
2391 }
2392
2393 if (local->initialised) {
2394 vchiq_loud_error_header();
2395 if (remote->initialised)
2396 vchiq_loud_error("local state has already been "
2397 "initialised");
2398 else
2399 vchiq_loud_error("master/slave mismatch - two %ss",
2400 is_master ? "master" : "slave");
2401 vchiq_loud_error_footer();
2402 return VCHIQ_ERROR;
2403 }
2404
2405 memset(state, 0, sizeof(VCHIQ_STATE_T));
2406
2407 state->id = id++;
2408 state->is_master = is_master;
2409
2410 /*
2411 initialize shared state pointers
2412 */
2413
2414 state->local = local;
2415 state->remote = remote;
2416 state->slot_data = (VCHIQ_SLOT_T *)slot_zero;
2417
2418 /*
2419 initialize events and mutexes
2420 */
2421
2422 sema_init(&state->connect, 0);
2423 mutex_init(&state->mutex);
2424 sema_init(&state->trigger_event, 0);
2425 sema_init(&state->recycle_event, 0);
2426 sema_init(&state->sync_trigger_event, 0);
2427 sema_init(&state->sync_release_event, 0);
2428
2429 mutex_init(&state->slot_mutex);
2430 mutex_init(&state->recycle_mutex);
2431 mutex_init(&state->sync_mutex);
2432 mutex_init(&state->bulk_transfer_mutex);
2433
2434 sema_init(&state->slot_available_event, 0);
2435 sema_init(&state->slot_remove_event, 0);
2436 sema_init(&state->data_quota_event, 0);
2437
2438 state->slot_queue_available = 0;
2439
2440 for (i = 0; i < VCHIQ_MAX_SERVICES; i++) {
2441 VCHIQ_SERVICE_QUOTA_T *service_quota =
2442 &state->service_quotas[i];
2443 sema_init(&service_quota->quota_event, 0);
2444 }
2445
2446 for (i = local->slot_first; i <= local->slot_last; i++) {
2447 local->slot_queue[state->slot_queue_available++] = i;
2448 up(&state->slot_available_event);
2449 }
2450
2451 state->default_slot_quota = state->slot_queue_available/2;
2452 state->default_message_quota =
2453 min((unsigned short)(state->default_slot_quota * 256),
2454 (unsigned short)~0);
2455
2456 state->previous_data_index = -1;
2457 state->data_use_count = 0;
2458 state->data_quota = state->slot_queue_available - 1;
2459
2460 local->trigger.event = &state->trigger_event;
2461 remote_event_create(&local->trigger);
2462 local->tx_pos = 0;
2463
2464 local->recycle.event = &state->recycle_event;
2465 remote_event_create(&local->recycle);
2466 local->slot_queue_recycle = state->slot_queue_available;
2467
2468 local->sync_trigger.event = &state->sync_trigger_event;
2469 remote_event_create(&local->sync_trigger);
2470
2471 local->sync_release.event = &state->sync_release_event;
2472 remote_event_create(&local->sync_release);
2473
2474 /* At start-of-day, the slot is empty and available */
2475 ((VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state, local->slot_sync))->msgid
2476 = VCHIQ_MSGID_PADDING;
2477 remote_event_signal_local(&local->sync_release);
2478
2479 local->debug[DEBUG_ENTRIES] = DEBUG_MAX;
2480
2481 status = vchiq_platform_init_state(state);
2482
2483 /*
2484 bring up slot handler thread
2485 */
2486 snprintf(threadname, sizeof(threadname), "VCHIQ-%d", state->id);
2487 state->slot_handler_thread = kthread_create(&slot_handler_func,
2488 (void *)state,
2489 threadname);
2490
2491 if (state->slot_handler_thread == NULL) {
2492 vchiq_loud_error_header();
2493 vchiq_loud_error("couldn't create thread %s", threadname);
2494 vchiq_loud_error_footer();
2495 return VCHIQ_ERROR;
2496 }
2497 set_user_nice(state->slot_handler_thread, -19);
2498 wake_up_process(state->slot_handler_thread);
2499
2500 snprintf(threadname, sizeof(threadname), "VCHIQr-%d", state->id);
2501 state->recycle_thread = kthread_create(&recycle_func,
2502 (void *)state,
2503 threadname);
2504 if (state->recycle_thread == NULL) {
2505 vchiq_loud_error_header();
2506 vchiq_loud_error("couldn't create thread %s", threadname);
2507 vchiq_loud_error_footer();
2508 return VCHIQ_ERROR;
2509 }
2510 set_user_nice(state->recycle_thread, -19);
2511 wake_up_process(state->recycle_thread);
2512
2513 snprintf(threadname, sizeof(threadname), "VCHIQs-%d", state->id);
2514 state->sync_thread = kthread_create(&sync_func,
2515 (void *)state,
2516 threadname);
2517 if (state->sync_thread == NULL) {
2518 vchiq_loud_error_header();
2519 vchiq_loud_error("couldn't create thread %s", threadname);
2520 vchiq_loud_error_footer();
2521 return VCHIQ_ERROR;
2522 }
2523 set_user_nice(state->sync_thread, -20);
2524 wake_up_process(state->sync_thread);
2525
2526 BUG_ON(state->id >= VCHIQ_MAX_STATES);
2527 vchiq_states[state->id] = state;
2528
2529 /* Indicate readiness to the other side */
2530 local->initialised = 1;
2531
2532 return status;
2533}
2534
2535/* Called from application thread when a client or server service is created. */
2536VCHIQ_SERVICE_T *
2537vchiq_add_service_internal(VCHIQ_STATE_T *state,
2538 const VCHIQ_SERVICE_PARAMS_T *params, int srvstate,
2539 VCHIQ_INSTANCE_T instance, VCHIQ_USERDATA_TERM_T userdata_term)
2540{
2541 VCHIQ_SERVICE_T *service;
2542
2543 service = kmalloc(sizeof(VCHIQ_SERVICE_T), GFP_KERNEL);
2544 if (service) {
2545 service->base.fourcc = params->fourcc;
2546 service->base.callback = params->callback;
2547 service->base.userdata = params->userdata;
2548 service->handle = VCHIQ_SERVICE_HANDLE_INVALID;
2549 service->ref_count = 1;
2550 service->srvstate = VCHIQ_SRVSTATE_FREE;
2551 service->userdata_term = userdata_term;
2552 service->localport = VCHIQ_PORT_FREE;
2553 service->remoteport = VCHIQ_PORT_FREE;
2554
2555 service->public_fourcc = (srvstate == VCHIQ_SRVSTATE_OPENING) ?
2556 VCHIQ_FOURCC_INVALID : params->fourcc;
2557 service->client_id = 0;
2558 service->auto_close = 1;
2559 service->sync = 0;
2560 service->closing = 0;
2561 service->trace = 0;
2562 atomic_set(&service->poll_flags, 0);
2563 service->version = params->version;
2564 service->version_min = params->version_min;
2565 service->state = state;
2566 service->instance = instance;
2567 service->service_use_count = 0;
2568 init_bulk_queue(&service->bulk_tx);
2569 init_bulk_queue(&service->bulk_rx);
2570 sema_init(&service->remove_event, 0);
2571 sema_init(&service->bulk_remove_event, 0);
2572 mutex_init(&service->bulk_mutex);
2573 memset(&service->stats, 0, sizeof(service->stats));
2574 } else {
2575 vchiq_log_error(vchiq_core_log_level,
2576 "Out of memory");
2577 }
2578
2579 if (service) {
2580 VCHIQ_SERVICE_T **pservice = NULL;
2581 int i;
2582
2583 /* Although it is perfectly possible to use service_spinlock
2584 ** to protect the creation of services, it is overkill as it
2585 ** disables interrupts while the array is searched.
2586 ** The only danger is of another thread trying to create a
2587 ** service - service deletion is safe.
2588 ** Therefore it is preferable to use state->mutex which,
2589 ** although slower to claim, doesn't block interrupts while
2590 ** it is held.
2591 */
2592
2593 mutex_lock(&state->mutex);
2594
2595 /* Prepare to use a previously unused service */
2596 if (state->unused_service < VCHIQ_MAX_SERVICES)
2597 pservice = &state->services[state->unused_service];
2598
2599 if (srvstate == VCHIQ_SRVSTATE_OPENING) {
2600 for (i = 0; i < state->unused_service; i++) {
2601 VCHIQ_SERVICE_T *srv = state->services[i];
2602 if (!srv) {
2603 pservice = &state->services[i];
2604 break;
2605 }
2606 }
2607 } else {
2608 for (i = (state->unused_service - 1); i >= 0; i--) {
2609 VCHIQ_SERVICE_T *srv = state->services[i];
2610 if (!srv)
2611 pservice = &state->services[i];
2612 else if ((srv->public_fourcc == params->fourcc)
2613 && ((srv->instance != instance) ||
2614 (srv->base.callback !=
2615 params->callback))) {
2616 /* There is another server using this
2617 ** fourcc which doesn't match. */
2618 pservice = NULL;
2619 break;
2620 }
2621 }
2622 }
2623
2624 if (pservice) {
2625 service->localport = (pservice - state->services);
2626 if (!handle_seq)
2627 handle_seq = VCHIQ_MAX_STATES *
2628 VCHIQ_MAX_SERVICES;
2629 service->handle = handle_seq |
2630 (state->id * VCHIQ_MAX_SERVICES) |
2631 service->localport;
2632 handle_seq += VCHIQ_MAX_STATES * VCHIQ_MAX_SERVICES;
2633 *pservice = service;
2634 if (pservice == &state->services[state->unused_service])
2635 state->unused_service++;
2636 }
2637
2638 mutex_unlock(&state->mutex);
2639
2640 if (!pservice) {
2641 kfree(service);
2642 service = NULL;
2643 }
2644 }
2645
2646 if (service) {
2647 VCHIQ_SERVICE_QUOTA_T *service_quota =
2648 &state->service_quotas[service->localport];
2649 service_quota->slot_quota = state->default_slot_quota;
2650 service_quota->message_quota = state->default_message_quota;
2651 if (service_quota->slot_use_count == 0)
2652 service_quota->previous_tx_index =
2653 SLOT_QUEUE_INDEX_FROM_POS(state->local_tx_pos)
2654 - 1;
2655
2656 /* Bring this service online */
2657 vchiq_set_service_state(service, srvstate);
2658
2659 vchiq_log_info(vchiq_core_msg_log_level,
2660 "%s Service %c%c%c%c SrcPort:%d",
2661 (srvstate == VCHIQ_SRVSTATE_OPENING)
2662 ? "Open" : "Add",
2663 VCHIQ_FOURCC_AS_4CHARS(params->fourcc),
2664 service->localport);
2665 }
2666
2667 /* Don't unlock the service - leave it with a ref_count of 1. */
2668
2669 return service;
2670}
2671
2672VCHIQ_STATUS_T
2673vchiq_open_service_internal(VCHIQ_SERVICE_T *service, int client_id)
2674{
2675 struct vchiq_open_payload payload = {
2676 service->base.fourcc,
2677 client_id,
2678 service->version,
2679 service->version_min
2680 };
2681 VCHIQ_ELEMENT_T body = { &payload, sizeof(payload) };
2682 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
2683
2684 service->client_id = client_id;
2685 vchiq_use_service_internal(service);
2686 status = queue_message(service->state, NULL,
2687 VCHIQ_MAKE_MSG(VCHIQ_MSG_OPEN, service->localport, 0),
2688 &body, 1, sizeof(payload), QMFLAGS_IS_BLOCKING);
2689 if (status == VCHIQ_SUCCESS) {
2690 /* Wait for the ACK/NAK */
2691 if (down_interruptible(&service->remove_event) != 0) {
2692 status = VCHIQ_RETRY;
2693 vchiq_release_service_internal(service);
2694 } else if ((service->srvstate != VCHIQ_SRVSTATE_OPEN) &&
2695 (service->srvstate != VCHIQ_SRVSTATE_OPENSYNC)) {
2696 if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT)
2697 vchiq_log_error(vchiq_core_log_level,
2698 "%d: osi - srvstate = %s (ref %d)",
2699 service->state->id,
2700 srvstate_names[service->srvstate],
2701 service->ref_count);
2702 status = VCHIQ_ERROR;
2703 VCHIQ_SERVICE_STATS_INC(service, error_count);
2704 vchiq_release_service_internal(service);
2705 }
2706 }
2707 return status;
2708}
2709
2710static void
2711release_service_messages(VCHIQ_SERVICE_T *service)
2712{
2713 VCHIQ_STATE_T *state = service->state;
2714 int slot_last = state->remote->slot_last;
2715 int i;
2716
2717 /* Release any claimed messages aimed at this service */
2718
2719 if (service->sync) {
2720 VCHIQ_HEADER_T *header =
2721 (VCHIQ_HEADER_T *)SLOT_DATA_FROM_INDEX(state,
2722 state->remote->slot_sync);
2723 if (VCHIQ_MSG_DSTPORT(header->msgid) == service->localport)
2724 release_message_sync(state, header);
2725
2726 return;
2727 }
2728
2729 for (i = state->remote->slot_first; i <= slot_last; i++) {
2730 VCHIQ_SLOT_INFO_T *slot_info =
2731 SLOT_INFO_FROM_INDEX(state, i);
2732 if (slot_info->release_count != slot_info->use_count) {
2733 char *data =
2734 (char *)SLOT_DATA_FROM_INDEX(state, i);
2735 unsigned int pos, end;
2736
2737 end = VCHIQ_SLOT_SIZE;
2738 if (data == state->rx_data)
2739 /* This buffer is still being read from - stop
2740 ** at the current read position */
2741 end = state->rx_pos & VCHIQ_SLOT_MASK;
2742
2743 pos = 0;
2744
2745 while (pos < end) {
2746 VCHIQ_HEADER_T *header =
2747 (VCHIQ_HEADER_T *)(data + pos);
2748 int msgid = header->msgid;
2749 int port = VCHIQ_MSG_DSTPORT(msgid);
2750 if ((port == service->localport) &&
2751 (msgid & VCHIQ_MSGID_CLAIMED)) {
2752 vchiq_log_info(vchiq_core_log_level,
2753 " fsi - hdr %x",
2754 (unsigned int)header);
2755 release_slot(state, slot_info, header,
2756 NULL);
2757 }
2758 pos += calc_stride(header->size);
2759 if (pos > VCHIQ_SLOT_SIZE) {
2760 vchiq_log_error(vchiq_core_log_level,
2761 "fsi - pos %x: header %x, "
2762 "msgid %x, header->msgid %x, "
2763 "header->size %x",
2764 pos, (unsigned int)header,
2765 msgid, header->msgid,
2766 header->size);
2767 WARN(1, "invalid slot position\n");
2768 }
2769 }
2770 }
2771 }
2772}
2773
2774static int
2775do_abort_bulks(VCHIQ_SERVICE_T *service)
2776{
2777 VCHIQ_STATUS_T status;
2778
2779 /* Abort any outstanding bulk transfers */
2780 if (mutex_lock_interruptible(&service->bulk_mutex) != 0)
2781 return 0;
2782 abort_outstanding_bulks(service, &service->bulk_tx);
2783 abort_outstanding_bulks(service, &service->bulk_rx);
2784 mutex_unlock(&service->bulk_mutex);
2785
2786 status = notify_bulks(service, &service->bulk_tx, 0/*!retry_poll*/);
2787 if (status == VCHIQ_SUCCESS)
2788 status = notify_bulks(service, &service->bulk_rx,
2789 0/*!retry_poll*/);
2790 return (status == VCHIQ_SUCCESS);
2791}
2792
2793static VCHIQ_STATUS_T
2794close_service_complete(VCHIQ_SERVICE_T *service, int failstate)
2795{
2796 VCHIQ_STATUS_T status;
2797 int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID);
2798 int newstate;
2799
2800 switch (service->srvstate) {
2801 case VCHIQ_SRVSTATE_OPEN:
2802 case VCHIQ_SRVSTATE_CLOSESENT:
2803 case VCHIQ_SRVSTATE_CLOSERECVD:
2804 if (is_server) {
2805 if (service->auto_close) {
2806 service->client_id = 0;
2807 service->remoteport = VCHIQ_PORT_FREE;
2808 newstate = VCHIQ_SRVSTATE_LISTENING;
2809 } else
2810 newstate = VCHIQ_SRVSTATE_CLOSEWAIT;
2811 } else
2812 newstate = VCHIQ_SRVSTATE_CLOSED;
2813 vchiq_set_service_state(service, newstate);
2814 break;
2815 case VCHIQ_SRVSTATE_LISTENING:
2816 break;
2817 default:
2818 vchiq_log_error(vchiq_core_log_level,
2819 "close_service_complete(%x) called in state %s",
2820 service->handle, srvstate_names[service->srvstate]);
2821 WARN(1, "close_service_complete in unexpected state\n");
2822 return VCHIQ_ERROR;
2823 }
2824
2825 status = make_service_callback(service,
2826 VCHIQ_SERVICE_CLOSED, NULL, NULL);
2827
2828 if (status != VCHIQ_RETRY) {
2829 int uc = service->service_use_count;
2830 int i;
2831 /* Complete the close process */
2832 for (i = 0; i < uc; i++)
2833 /* cater for cases where close is forced and the
2834 ** client may not close all it's handles */
2835 vchiq_release_service_internal(service);
2836
2837 service->client_id = 0;
2838 service->remoteport = VCHIQ_PORT_FREE;
2839
2840 if (service->srvstate == VCHIQ_SRVSTATE_CLOSED)
2841 vchiq_free_service_internal(service);
2842 else if (service->srvstate != VCHIQ_SRVSTATE_CLOSEWAIT) {
2843 if (is_server)
2844 service->closing = 0;
2845
2846 up(&service->remove_event);
2847 }
2848 } else
2849 vchiq_set_service_state(service, failstate);
2850
2851 return status;
2852}
2853
2854/* Called by the slot handler */
2855VCHIQ_STATUS_T
2856vchiq_close_service_internal(VCHIQ_SERVICE_T *service, int close_recvd)
2857{
2858 VCHIQ_STATE_T *state = service->state;
2859 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
2860 int is_server = (service->public_fourcc != VCHIQ_FOURCC_INVALID);
2861
2862 vchiq_log_info(vchiq_core_log_level, "%d: csi:%d,%d (%s)",
2863 service->state->id, service->localport, close_recvd,
2864 srvstate_names[service->srvstate]);
2865
2866 switch (service->srvstate) {
2867 case VCHIQ_SRVSTATE_CLOSED:
2868 case VCHIQ_SRVSTATE_HIDDEN:
2869 case VCHIQ_SRVSTATE_LISTENING:
2870 case VCHIQ_SRVSTATE_CLOSEWAIT:
2871 if (close_recvd)
2872 vchiq_log_error(vchiq_core_log_level,
2873 "vchiq_close_service_internal(1) called "
2874 "in state %s",
2875 srvstate_names[service->srvstate]);
2876 else if (is_server) {
2877 if (service->srvstate == VCHIQ_SRVSTATE_LISTENING) {
2878 status = VCHIQ_ERROR;
2879 } else {
2880 service->client_id = 0;
2881 service->remoteport = VCHIQ_PORT_FREE;
2882 if (service->srvstate ==
2883 VCHIQ_SRVSTATE_CLOSEWAIT)
2884 vchiq_set_service_state(service,
2885 VCHIQ_SRVSTATE_LISTENING);
2886 }
2887 up(&service->remove_event);
2888 } else
2889 vchiq_free_service_internal(service);
2890 break;
2891 case VCHIQ_SRVSTATE_OPENING:
2892 if (close_recvd) {
2893 /* The open was rejected - tell the user */
2894 vchiq_set_service_state(service,
2895 VCHIQ_SRVSTATE_CLOSEWAIT);
2896 up(&service->remove_event);
2897 } else {
2898 /* Shutdown mid-open - let the other side know */
2899 status = queue_message(state, service,
2900 VCHIQ_MAKE_MSG
2901 (VCHIQ_MSG_CLOSE,
2902 service->localport,
2903 VCHIQ_MSG_DSTPORT(service->remoteport)),
2904 NULL, 0, 0, 0);
2905 }
2906 break;
2907
2908 case VCHIQ_SRVSTATE_OPENSYNC:
2909 mutex_lock(&state->sync_mutex);
2910 /* Drop through */
2911
2912 case VCHIQ_SRVSTATE_OPEN:
2913 if (state->is_master || close_recvd) {
2914 if (!do_abort_bulks(service))
2915 status = VCHIQ_RETRY;
2916 }
2917
2918 release_service_messages(service);
2919
2920 if (status == VCHIQ_SUCCESS)
2921 status = queue_message(state, service,
2922 VCHIQ_MAKE_MSG
2923 (VCHIQ_MSG_CLOSE,
2924 service->localport,
2925 VCHIQ_MSG_DSTPORT(service->remoteport)),
2926 NULL, 0, 0, QMFLAGS_NO_MUTEX_UNLOCK);
2927
2928 if (status == VCHIQ_SUCCESS) {
2929 if (!close_recvd) {
2930 /* Change the state while the mutex is
2931 still held */
2932 vchiq_set_service_state(service,
2933 VCHIQ_SRVSTATE_CLOSESENT);
2934 mutex_unlock(&state->slot_mutex);
2935 if (service->sync)
2936 mutex_unlock(&state->sync_mutex);
2937 break;
2938 }
2939 } else if (service->srvstate == VCHIQ_SRVSTATE_OPENSYNC) {
2940 mutex_unlock(&state->sync_mutex);
2941 break;
2942 } else
2943 break;
2944
2945 /* Change the state while the mutex is still held */
2946 vchiq_set_service_state(service, VCHIQ_SRVSTATE_CLOSERECVD);
2947 mutex_unlock(&state->slot_mutex);
2948 if (service->sync)
2949 mutex_unlock(&state->sync_mutex);
2950
2951 status = close_service_complete(service,
2952 VCHIQ_SRVSTATE_CLOSERECVD);
2953 break;
2954
2955 case VCHIQ_SRVSTATE_CLOSESENT:
2956 if (!close_recvd)
2957 /* This happens when a process is killed mid-close */
2958 break;
2959
2960 if (!state->is_master) {
2961 if (!do_abort_bulks(service)) {
2962 status = VCHIQ_RETRY;
2963 break;
2964 }
2965 }
2966
2967 if (status == VCHIQ_SUCCESS)
2968 status = close_service_complete(service,
2969 VCHIQ_SRVSTATE_CLOSERECVD);
2970 break;
2971
2972 case VCHIQ_SRVSTATE_CLOSERECVD:
2973 if (!close_recvd && is_server)
2974 /* Force into LISTENING mode */
2975 vchiq_set_service_state(service,
2976 VCHIQ_SRVSTATE_LISTENING);
2977 status = close_service_complete(service,
2978 VCHIQ_SRVSTATE_CLOSERECVD);
2979 break;
2980
2981 default:
2982 vchiq_log_error(vchiq_core_log_level,
2983 "vchiq_close_service_internal(%d) called in state %s",
2984 close_recvd, srvstate_names[service->srvstate]);
2985 break;
2986 }
2987
2988 return status;
2989}
2990
2991/* Called from the application process upon process death */
2992void
2993vchiq_terminate_service_internal(VCHIQ_SERVICE_T *service)
2994{
2995 VCHIQ_STATE_T *state = service->state;
2996
2997 vchiq_log_info(vchiq_core_log_level, "%d: tsi - (%d<->%d)",
2998 state->id, service->localport, service->remoteport);
2999
3000 mark_service_closing(service);
3001
3002 /* Mark the service for removal by the slot handler */
3003 request_poll(state, service, VCHIQ_POLL_REMOVE);
3004}
3005
3006/* Called from the slot handler */
3007void
3008vchiq_free_service_internal(VCHIQ_SERVICE_T *service)
3009{
3010 VCHIQ_STATE_T *state = service->state;
3011
3012 vchiq_log_info(vchiq_core_log_level, "%d: fsi - (%d)",
3013 state->id, service->localport);
3014
3015 switch (service->srvstate) {
3016 case VCHIQ_SRVSTATE_OPENING:
3017 case VCHIQ_SRVSTATE_CLOSED:
3018 case VCHIQ_SRVSTATE_HIDDEN:
3019 case VCHIQ_SRVSTATE_LISTENING:
3020 case VCHIQ_SRVSTATE_CLOSEWAIT:
3021 break;
3022 default:
3023 vchiq_log_error(vchiq_core_log_level,
3024 "%d: fsi - (%d) in state %s",
3025 state->id, service->localport,
3026 srvstate_names[service->srvstate]);
3027 return;
3028 }
3029
3030 vchiq_set_service_state(service, VCHIQ_SRVSTATE_FREE);
3031
3032 up(&service->remove_event);
3033
3034 /* Release the initial lock */
3035 unlock_service(service);
3036}
3037
3038VCHIQ_STATUS_T
3039vchiq_connect_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
3040{
3041 VCHIQ_SERVICE_T *service;
3042 int i;
3043
3044 /* Find all services registered to this client and enable them. */
3045 i = 0;
3046 while ((service = next_service_by_instance(state, instance,
3047 &i)) != NULL) {
3048 if (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)
3049 vchiq_set_service_state(service,
3050 VCHIQ_SRVSTATE_LISTENING);
3051 unlock_service(service);
3052 }
3053
3054 if (state->conn_state == VCHIQ_CONNSTATE_DISCONNECTED) {
3055 if (queue_message(state, NULL,
3056 VCHIQ_MAKE_MSG(VCHIQ_MSG_CONNECT, 0, 0), NULL, 0,
3057 0, QMFLAGS_IS_BLOCKING) == VCHIQ_RETRY)
3058 return VCHIQ_RETRY;
3059
3060 vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTING);
3061 }
3062
3063 if (state->conn_state == VCHIQ_CONNSTATE_CONNECTING) {
3064 if (down_interruptible(&state->connect) != 0)
3065 return VCHIQ_RETRY;
3066
3067 vchiq_set_conn_state(state, VCHIQ_CONNSTATE_CONNECTED);
3068 up(&state->connect);
3069 }
3070
3071 return VCHIQ_SUCCESS;
3072}
3073
3074VCHIQ_STATUS_T
3075vchiq_shutdown_internal(VCHIQ_STATE_T *state, VCHIQ_INSTANCE_T instance)
3076{
3077 VCHIQ_SERVICE_T *service;
3078 int i;
3079
3080 /* Find all services registered to this client and enable them. */
3081 i = 0;
3082 while ((service = next_service_by_instance(state, instance,
3083 &i)) != NULL) {
3084 (void)vchiq_remove_service(service->handle);
3085 unlock_service(service);
3086 }
3087
3088 return VCHIQ_SUCCESS;
3089}
3090
3091VCHIQ_STATUS_T
3092vchiq_pause_internal(VCHIQ_STATE_T *state)
3093{
3094 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
3095
3096 switch (state->conn_state) {
3097 case VCHIQ_CONNSTATE_CONNECTED:
3098 /* Request a pause */
3099 vchiq_set_conn_state(state, VCHIQ_CONNSTATE_PAUSING);
3100 request_poll(state, NULL, 0);
3101 break;
3102 default:
3103 vchiq_log_error(vchiq_core_log_level,
3104 "vchiq_pause_internal in state %s\n",
3105 conn_state_names[state->conn_state]);
3106 status = VCHIQ_ERROR;
3107 VCHIQ_STATS_INC(state, error_count);
3108 break;
3109 }
3110
3111 return status;
3112}
3113
3114VCHIQ_STATUS_T
3115vchiq_resume_internal(VCHIQ_STATE_T *state)
3116{
3117 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
3118
3119 if (state->conn_state == VCHIQ_CONNSTATE_PAUSED) {
3120 vchiq_set_conn_state(state, VCHIQ_CONNSTATE_RESUMING);
3121 request_poll(state, NULL, 0);
3122 } else {
3123 status = VCHIQ_ERROR;
3124 VCHIQ_STATS_INC(state, error_count);
3125 }
3126
3127 return status;
3128}
3129
3130VCHIQ_STATUS_T
3131vchiq_close_service(VCHIQ_SERVICE_HANDLE_T handle)
3132{
3133 /* Unregister the service */
3134 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3135 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
3136
3137 if (!service)
3138 return VCHIQ_ERROR;
3139
3140 vchiq_log_info(vchiq_core_log_level,
3141 "%d: close_service:%d",
3142 service->state->id, service->localport);
3143
3144 if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
3145 (service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
3146 (service->srvstate == VCHIQ_SRVSTATE_HIDDEN)) {
3147 unlock_service(service);
3148 return VCHIQ_ERROR;
3149 }
3150
3151 mark_service_closing(service);
3152
3153 if (current == service->state->slot_handler_thread) {
3154 status = vchiq_close_service_internal(service,
3155 0/*!close_recvd*/);
3156 BUG_ON(status == VCHIQ_RETRY);
3157 } else {
3158 /* Mark the service for termination by the slot handler */
3159 request_poll(service->state, service, VCHIQ_POLL_TERMINATE);
3160 }
3161
3162 while (1) {
3163 if (down_interruptible(&service->remove_event) != 0) {
3164 status = VCHIQ_RETRY;
3165 break;
3166 }
3167
3168 if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
3169 (service->srvstate == VCHIQ_SRVSTATE_LISTENING) ||
3170 (service->srvstate == VCHIQ_SRVSTATE_OPEN))
3171 break;
3172
3173 vchiq_log_warning(vchiq_core_log_level,
3174 "%d: close_service:%d - waiting in state %s",
3175 service->state->id, service->localport,
3176 srvstate_names[service->srvstate]);
3177 }
3178
3179 if ((status == VCHIQ_SUCCESS) &&
3180 (service->srvstate != VCHIQ_SRVSTATE_FREE) &&
3181 (service->srvstate != VCHIQ_SRVSTATE_LISTENING))
3182 status = VCHIQ_ERROR;
3183
3184 unlock_service(service);
3185
3186 return status;
3187}
3188
3189VCHIQ_STATUS_T
3190vchiq_remove_service(VCHIQ_SERVICE_HANDLE_T handle)
3191{
3192 /* Unregister the service */
3193 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3194 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
3195
3196 if (!service)
3197 return VCHIQ_ERROR;
3198
3199 vchiq_log_info(vchiq_core_log_level,
3200 "%d: remove_service:%d",
3201 service->state->id, service->localport);
3202
3203 if (service->srvstate == VCHIQ_SRVSTATE_FREE) {
3204 unlock_service(service);
3205 return VCHIQ_ERROR;
3206 }
3207
3208 mark_service_closing(service);
3209
3210 if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) ||
3211 (current == service->state->slot_handler_thread)) {
3212 /* Make it look like a client, because it must be removed and
3213 not left in the LISTENING state. */
3214 service->public_fourcc = VCHIQ_FOURCC_INVALID;
3215
3216 status = vchiq_close_service_internal(service,
3217 0/*!close_recvd*/);
3218 BUG_ON(status == VCHIQ_RETRY);
3219 } else {
3220 /* Mark the service for removal by the slot handler */
3221 request_poll(service->state, service, VCHIQ_POLL_REMOVE);
3222 }
3223 while (1) {
3224 if (down_interruptible(&service->remove_event) != 0) {
3225 status = VCHIQ_RETRY;
3226 break;
3227 }
3228
3229 if ((service->srvstate == VCHIQ_SRVSTATE_FREE) ||
3230 (service->srvstate == VCHIQ_SRVSTATE_OPEN))
3231 break;
3232
3233 vchiq_log_warning(vchiq_core_log_level,
3234 "%d: remove_service:%d - waiting in state %s",
3235 service->state->id, service->localport,
3236 srvstate_names[service->srvstate]);
3237 }
3238
3239 if ((status == VCHIQ_SUCCESS) &&
3240 (service->srvstate != VCHIQ_SRVSTATE_FREE))
3241 status = VCHIQ_ERROR;
3242
3243 unlock_service(service);
3244
3245 return status;
3246}
3247
3248
3249/* This function may be called by kernel threads or user threads.
3250 * User threads may receive VCHIQ_RETRY to indicate that a signal has been
3251 * received and the call should be retried after being returned to user
3252 * context.
3253 * When called in blocking mode, the userdata field points to a bulk_waiter
3254 * structure.
3255 */
3256VCHIQ_STATUS_T
3257vchiq_bulk_transfer(VCHIQ_SERVICE_HANDLE_T handle,
3258 VCHI_MEM_HANDLE_T memhandle, void *offset, int size, void *userdata,
3259 VCHIQ_BULK_MODE_T mode, VCHIQ_BULK_DIR_T dir)
3260{
3261 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3262 VCHIQ_BULK_QUEUE_T *queue;
3263 VCHIQ_BULK_T *bulk;
3264 VCHIQ_STATE_T *state;
3265 struct bulk_waiter *bulk_waiter = NULL;
3266 const char dir_char = (dir == VCHIQ_BULK_TRANSMIT) ? 't' : 'r';
3267 const int dir_msgtype = (dir == VCHIQ_BULK_TRANSMIT) ?
3268 VCHIQ_MSG_BULK_TX : VCHIQ_MSG_BULK_RX;
3269 VCHIQ_STATUS_T status = VCHIQ_ERROR;
3270
3271 if (!service ||
3272 (service->srvstate != VCHIQ_SRVSTATE_OPEN) ||
3273 ((memhandle == VCHI_MEM_HANDLE_INVALID) && (offset == NULL)) ||
3274 (vchiq_check_service(service) != VCHIQ_SUCCESS))
3275 goto error_exit;
3276
3277 switch (mode) {
3278 case VCHIQ_BULK_MODE_NOCALLBACK:
3279 case VCHIQ_BULK_MODE_CALLBACK:
3280 break;
3281 case VCHIQ_BULK_MODE_BLOCKING:
3282 bulk_waiter = (struct bulk_waiter *)userdata;
3283 sema_init(&bulk_waiter->event, 0);
3284 bulk_waiter->actual = 0;
3285 bulk_waiter->bulk = NULL;
3286 break;
3287 case VCHIQ_BULK_MODE_WAITING:
3288 bulk_waiter = (struct bulk_waiter *)userdata;
3289 bulk = bulk_waiter->bulk;
3290 goto waiting;
3291 default:
3292 goto error_exit;
3293 }
3294
3295 state = service->state;
3296
3297 queue = (dir == VCHIQ_BULK_TRANSMIT) ?
3298 &service->bulk_tx : &service->bulk_rx;
3299
3300 if (mutex_lock_interruptible(&service->bulk_mutex) != 0) {
3301 status = VCHIQ_RETRY;
3302 goto error_exit;
3303 }
3304
3305 if (queue->local_insert == queue->remove + VCHIQ_NUM_SERVICE_BULKS) {
3306 VCHIQ_SERVICE_STATS_INC(service, bulk_stalls);
3307 do {
3308 mutex_unlock(&service->bulk_mutex);
3309 if (down_interruptible(&service->bulk_remove_event)
3310 != 0) {
3311 status = VCHIQ_RETRY;
3312 goto error_exit;
3313 }
3314 if (mutex_lock_interruptible(&service->bulk_mutex)
3315 != 0) {
3316 status = VCHIQ_RETRY;
3317 goto error_exit;
3318 }
3319 } while (queue->local_insert == queue->remove +
3320 VCHIQ_NUM_SERVICE_BULKS);
3321 }
3322
3323 bulk = &queue->bulks[BULK_INDEX(queue->local_insert)];
3324
3325 bulk->mode = mode;
3326 bulk->dir = dir;
3327 bulk->userdata = userdata;
3328 bulk->size = size;
3329 bulk->actual = VCHIQ_BULK_ACTUAL_ABORTED;
3330
3331 if (vchiq_prepare_bulk_data(bulk, memhandle, offset, size, dir) !=
3332 VCHIQ_SUCCESS)
3333 goto unlock_error_exit;
3334
3335 wmb();
3336
3337 vchiq_log_info(vchiq_core_log_level,
3338 "%d: bt (%d->%d) %cx %x@%x %x",
3339 state->id,
3340 service->localport, service->remoteport, dir_char,
3341 size, (unsigned int)bulk->data, (unsigned int)userdata);
3342
3343 /* The slot mutex must be held when the service is being closed, so
3344 claim it here to ensure that isn't happening */
3345 if (mutex_lock_interruptible(&state->slot_mutex) != 0) {
3346 status = VCHIQ_RETRY;
3347 goto cancel_bulk_error_exit;
3348 }
3349
3350 if (service->srvstate != VCHIQ_SRVSTATE_OPEN)
3351 goto unlock_both_error_exit;
3352
3353 if (state->is_master) {
3354 queue->local_insert++;
3355 if (resolve_bulks(service, queue))
3356 request_poll(state, service,
3357 (dir == VCHIQ_BULK_TRANSMIT) ?
3358 VCHIQ_POLL_TXNOTIFY : VCHIQ_POLL_RXNOTIFY);
3359 } else {
3360 int payload[2] = { (int)bulk->data, bulk->size };
3361 VCHIQ_ELEMENT_T element = { payload, sizeof(payload) };
3362
3363 status = queue_message(state, NULL,
3364 VCHIQ_MAKE_MSG(dir_msgtype,
3365 service->localport, service->remoteport),
3366 &element, 1, sizeof(payload),
3367 QMFLAGS_IS_BLOCKING |
3368 QMFLAGS_NO_MUTEX_LOCK |
3369 QMFLAGS_NO_MUTEX_UNLOCK);
3370 if (status != VCHIQ_SUCCESS) {
3371 goto unlock_both_error_exit;
3372 }
3373 queue->local_insert++;
3374 }
3375
3376 mutex_unlock(&state->slot_mutex);
3377 mutex_unlock(&service->bulk_mutex);
3378
3379 vchiq_log_trace(vchiq_core_log_level,
3380 "%d: bt:%d %cx li=%x ri=%x p=%x",
3381 state->id,
3382 service->localport, dir_char,
3383 queue->local_insert, queue->remote_insert, queue->process);
3384
3385waiting:
3386 unlock_service(service);
3387
3388 status = VCHIQ_SUCCESS;
3389
3390 if (bulk_waiter) {
3391 bulk_waiter->bulk = bulk;
3392 if (down_interruptible(&bulk_waiter->event) != 0)
3393 status = VCHIQ_RETRY;
3394 else if (bulk_waiter->actual == VCHIQ_BULK_ACTUAL_ABORTED)
3395 status = VCHIQ_ERROR;
3396 }
3397
3398 return status;
3399
3400unlock_both_error_exit:
3401 mutex_unlock(&state->slot_mutex);
3402cancel_bulk_error_exit:
3403 vchiq_complete_bulk(bulk);
3404unlock_error_exit:
3405 mutex_unlock(&service->bulk_mutex);
3406
3407error_exit:
3408 if (service)
3409 unlock_service(service);
3410 return status;
3411}
3412
3413VCHIQ_STATUS_T
3414vchiq_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
3415 const VCHIQ_ELEMENT_T *elements, unsigned int count)
3416{
3417 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3418 VCHIQ_STATUS_T status = VCHIQ_ERROR;
3419
3420 unsigned int size = 0;
3421 unsigned int i;
3422
3423 if (!service ||
3424 (vchiq_check_service(service) != VCHIQ_SUCCESS))
3425 goto error_exit;
3426
3427 for (i = 0; i < (unsigned int)count; i++) {
3428 if (elements[i].size) {
3429 if (elements[i].data == NULL) {
3430 VCHIQ_SERVICE_STATS_INC(service, error_count);
3431 goto error_exit;
3432 }
3433 size += elements[i].size;
3434 }
3435 }
3436
3437 if (size > VCHIQ_MAX_MSG_SIZE) {
3438 VCHIQ_SERVICE_STATS_INC(service, error_count);
3439 goto error_exit;
3440 }
3441
3442 switch (service->srvstate) {
3443 case VCHIQ_SRVSTATE_OPEN:
3444 status = queue_message(service->state, service,
3445 VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA,
3446 service->localport,
3447 service->remoteport),
3448 elements, count, size, 1);
3449 break;
3450 case VCHIQ_SRVSTATE_OPENSYNC:
3451 status = queue_message_sync(service->state, service,
3452 VCHIQ_MAKE_MSG(VCHIQ_MSG_DATA,
3453 service->localport,
3454 service->remoteport),
3455 elements, count, size, 1);
3456 break;
3457 default:
3458 status = VCHIQ_ERROR;
3459 break;
3460 }
3461
3462error_exit:
3463 if (service)
3464 unlock_service(service);
3465
3466 return status;
3467}
3468
3469void
3470vchiq_release_message(VCHIQ_SERVICE_HANDLE_T handle, VCHIQ_HEADER_T *header)
3471{
3472 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3473 VCHIQ_SHARED_STATE_T *remote;
3474 VCHIQ_STATE_T *state;
3475 int slot_index;
3476
3477 if (!service)
3478 return;
3479
3480 state = service->state;
3481 remote = state->remote;
3482
3483 slot_index = SLOT_INDEX_FROM_DATA(state, (void *)header);
3484
3485 if ((slot_index >= remote->slot_first) &&
3486 (slot_index <= remote->slot_last)) {
3487 int msgid = header->msgid;
3488 if (msgid & VCHIQ_MSGID_CLAIMED) {
3489 VCHIQ_SLOT_INFO_T *slot_info =
3490 SLOT_INFO_FROM_INDEX(state, slot_index);
3491
3492 release_slot(state, slot_info, header, service);
3493 }
3494 } else if (slot_index == remote->slot_sync)
3495 release_message_sync(state, header);
3496
3497 unlock_service(service);
3498}
3499
3500static void
3501release_message_sync(VCHIQ_STATE_T *state, VCHIQ_HEADER_T *header)
3502{
3503 header->msgid = VCHIQ_MSGID_PADDING;
3504 wmb();
3505 remote_event_signal(&state->remote->sync_release);
3506}
3507
3508VCHIQ_STATUS_T
3509vchiq_get_peer_version(VCHIQ_SERVICE_HANDLE_T handle, short *peer_version)
3510{
3511 VCHIQ_STATUS_T status = VCHIQ_ERROR;
3512 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3513
3514 if (!service ||
3515 (vchiq_check_service(service) != VCHIQ_SUCCESS) ||
3516 !peer_version)
3517 goto exit;
3518 *peer_version = service->peer_version;
3519 status = VCHIQ_SUCCESS;
3520
3521exit:
3522 if (service)
3523 unlock_service(service);
3524 return status;
3525}
3526
3527VCHIQ_STATUS_T
3528vchiq_get_config(VCHIQ_INSTANCE_T instance,
3529 int config_size, VCHIQ_CONFIG_T *pconfig)
3530{
3531 VCHIQ_CONFIG_T config;
3532
3533 (void)instance;
3534
3535 config.max_msg_size = VCHIQ_MAX_MSG_SIZE;
3536 config.bulk_threshold = VCHIQ_MAX_MSG_SIZE;
3537 config.max_outstanding_bulks = VCHIQ_NUM_SERVICE_BULKS;
3538 config.max_services = VCHIQ_MAX_SERVICES;
3539 config.version = VCHIQ_VERSION;
3540 config.version_min = VCHIQ_VERSION_MIN;
3541
3542 if (config_size > sizeof(VCHIQ_CONFIG_T))
3543 return VCHIQ_ERROR;
3544
3545 memcpy(pconfig, &config,
3546 min(config_size, (int)(sizeof(VCHIQ_CONFIG_T))));
3547
3548 return VCHIQ_SUCCESS;
3549}
3550
3551VCHIQ_STATUS_T
3552vchiq_set_service_option(VCHIQ_SERVICE_HANDLE_T handle,
3553 VCHIQ_SERVICE_OPTION_T option, int value)
3554{
3555 VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3556 VCHIQ_STATUS_T status = VCHIQ_ERROR;
3557
3558 if (service) {
3559 switch (option) {
3560 case VCHIQ_SERVICE_OPTION_AUTOCLOSE:
3561 service->auto_close = value;
3562 status = VCHIQ_SUCCESS;
3563 break;
3564
3565 case VCHIQ_SERVICE_OPTION_SLOT_QUOTA: {
3566 VCHIQ_SERVICE_QUOTA_T *service_quota =
3567 &service->state->service_quotas[
3568 service->localport];
3569 if (value == 0)
3570 value = service->state->default_slot_quota;
3571 if ((value >= service_quota->slot_use_count) &&
3572 (value < (unsigned short)~0)) {
3573 service_quota->slot_quota = value;
3574 if ((value >= service_quota->slot_use_count) &&
3575 (service_quota->message_quota >=
3576 service_quota->message_use_count)) {
3577 /* Signal the service that it may have
3578 ** dropped below its quota */
3579 up(&service_quota->quota_event);
3580 }
3581 status = VCHIQ_SUCCESS;
3582 }
3583 } break;
3584
3585 case VCHIQ_SERVICE_OPTION_MESSAGE_QUOTA: {
3586 VCHIQ_SERVICE_QUOTA_T *service_quota =
3587 &service->state->service_quotas[
3588 service->localport];
3589 if (value == 0)
3590 value = service->state->default_message_quota;
3591 if ((value >= service_quota->message_use_count) &&
3592 (value < (unsigned short)~0)) {
3593 service_quota->message_quota = value;
3594 if ((value >=
3595 service_quota->message_use_count) &&
3596 (service_quota->slot_quota >=
3597 service_quota->slot_use_count))
3598 /* Signal the service that it may have
3599 ** dropped below its quota */
3600 up(&service_quota->quota_event);
3601 status = VCHIQ_SUCCESS;
3602 }
3603 } break;
3604
3605 case VCHIQ_SERVICE_OPTION_SYNCHRONOUS:
3606 if ((service->srvstate == VCHIQ_SRVSTATE_HIDDEN) ||
3607 (service->srvstate ==
3608 VCHIQ_SRVSTATE_LISTENING)) {
3609 service->sync = value;
3610 status = VCHIQ_SUCCESS;
3611 }
3612 break;
3613
3614 case VCHIQ_SERVICE_OPTION_TRACE:
3615 service->trace = value;
3616 status = VCHIQ_SUCCESS;
3617 break;
3618
3619 default:
3620 break;
3621 }
3622 unlock_service(service);
3623 }
3624
3625 return status;
3626}
3627
3628void
3629vchiq_dump_shared_state(void *dump_context, VCHIQ_STATE_T *state,
3630 VCHIQ_SHARED_STATE_T *shared, const char *label)
3631{
3632 static const char *const debug_names[] = {
3633 "<entries>",
3634 "SLOT_HANDLER_COUNT",
3635 "SLOT_HANDLER_LINE",
3636 "PARSE_LINE",
3637 "PARSE_HEADER",
3638 "PARSE_MSGID",
3639 "AWAIT_COMPLETION_LINE",
3640 "DEQUEUE_MESSAGE_LINE",
3641 "SERVICE_CALLBACK_LINE",
3642 "MSG_QUEUE_FULL_COUNT",
3643 "COMPLETION_QUEUE_FULL_COUNT"
3644 };
3645 int i;
3646
3647 char buf[80];
3648 int len;
3649 len = snprintf(buf, sizeof(buf),
3650 " %s: slots %d-%d tx_pos=%x recycle=%x",
3651 label, shared->slot_first, shared->slot_last,
3652 shared->tx_pos, shared->slot_queue_recycle);
3653 vchiq_dump(dump_context, buf, len + 1);
3654
3655 len = snprintf(buf, sizeof(buf),
3656 " Slots claimed:");
3657 vchiq_dump(dump_context, buf, len + 1);
3658
3659 for (i = shared->slot_first; i <= shared->slot_last; i++) {
3660 VCHIQ_SLOT_INFO_T slot_info = *SLOT_INFO_FROM_INDEX(state, i);
3661 if (slot_info.use_count != slot_info.release_count) {
3662 len = snprintf(buf, sizeof(buf),
3663 " %d: %d/%d", i, slot_info.use_count,
3664 slot_info.release_count);
3665 vchiq_dump(dump_context, buf, len + 1);
3666 }
3667 }
3668
3669 for (i = 1; i < shared->debug[DEBUG_ENTRIES]; i++) {
3670 len = snprintf(buf, sizeof(buf), " DEBUG: %s = %d(%x)",
3671 debug_names[i], shared->debug[i], shared->debug[i]);
3672 vchiq_dump(dump_context, buf, len + 1);
3673 }
3674}
3675
3676void
3677vchiq_dump_state(void *dump_context, VCHIQ_STATE_T *state)
3678{
3679 char buf[80];
3680 int len;
3681 int i;
3682
3683 len = snprintf(buf, sizeof(buf), "State %d: %s", state->id,
3684 conn_state_names[state->conn_state]);
3685 vchiq_dump(dump_context, buf, len + 1);
3686
3687 len = snprintf(buf, sizeof(buf),
3688 " tx_pos=%x(@%x), rx_pos=%x(@%x)",
3689 state->local->tx_pos,
3690 (uint32_t)state->tx_data +
3691 (state->local_tx_pos & VCHIQ_SLOT_MASK),
3692 state->rx_pos,
3693 (uint32_t)state->rx_data +
3694 (state->rx_pos & VCHIQ_SLOT_MASK));
3695 vchiq_dump(dump_context, buf, len + 1);
3696
3697 len = snprintf(buf, sizeof(buf),
3698 " Version: %d (min %d)",
3699 VCHIQ_VERSION, VCHIQ_VERSION_MIN);
3700 vchiq_dump(dump_context, buf, len + 1);
3701
3702 if (VCHIQ_ENABLE_STATS) {
3703 len = snprintf(buf, sizeof(buf),
3704 " Stats: ctrl_tx_count=%d, ctrl_rx_count=%d, "
3705 "error_count=%d",
3706 state->stats.ctrl_tx_count, state->stats.ctrl_rx_count,
3707 state->stats.error_count);
3708 vchiq_dump(dump_context, buf, len + 1);
3709 }
3710
3711 len = snprintf(buf, sizeof(buf),
3712 " Slots: %d available (%d data), %d recyclable, %d stalls "
3713 "(%d data)",
3714 ((state->slot_queue_available * VCHIQ_SLOT_SIZE) -
3715 state->local_tx_pos) / VCHIQ_SLOT_SIZE,
3716 state->data_quota - state->data_use_count,
3717 state->local->slot_queue_recycle - state->slot_queue_available,
3718 state->stats.slot_stalls, state->stats.data_stalls);
3719 vchiq_dump(dump_context, buf, len + 1);
3720
3721 vchiq_dump_platform_state(dump_context);
3722
3723 vchiq_dump_shared_state(dump_context, state, state->local, "Local");
3724 vchiq_dump_shared_state(dump_context, state, state->remote, "Remote");
3725
3726 vchiq_dump_platform_instances(dump_context);
3727
3728 for (i = 0; i < state->unused_service; i++) {
3729 VCHIQ_SERVICE_T *service = find_service_by_port(state, i);
3730
3731 if (service) {
3732 vchiq_dump_service_state(dump_context, service);
3733 unlock_service(service);
3734 }
3735 }
3736}
3737
3738void
3739vchiq_dump_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
3740{
3741 char buf[80];
3742 int len;
3743
3744 len = snprintf(buf, sizeof(buf), "Service %d: %s (ref %u)",
3745 service->localport, srvstate_names[service->srvstate],
3746 service->ref_count - 1); /*Don't include the lock just taken*/
3747
3748 if (service->srvstate != VCHIQ_SRVSTATE_FREE) {
3749 char remoteport[30];
3750 VCHIQ_SERVICE_QUOTA_T *service_quota =
3751 &service->state->service_quotas[service->localport];
3752 int fourcc = service->base.fourcc;
3753 int tx_pending, rx_pending;
3754 if (service->remoteport != VCHIQ_PORT_FREE) {
3755 int len2 = snprintf(remoteport, sizeof(remoteport),
3756 "%d", service->remoteport);
3757 if (service->public_fourcc != VCHIQ_FOURCC_INVALID)
3758 snprintf(remoteport + len2,
3759 sizeof(remoteport) - len2,
3760 " (client %x)", service->client_id);
3761 } else
3762 strcpy(remoteport, "n/a");
3763
3764 len += snprintf(buf + len, sizeof(buf) - len,
3765 " '%c%c%c%c' remote %s (msg use %d/%d, slot use %d/%d)",
3766 VCHIQ_FOURCC_AS_4CHARS(fourcc),
3767 remoteport,
3768 service_quota->message_use_count,
3769 service_quota->message_quota,
3770 service_quota->slot_use_count,
3771 service_quota->slot_quota);
3772
3773 vchiq_dump(dump_context, buf, len + 1);
3774
3775 tx_pending = service->bulk_tx.local_insert -
3776 service->bulk_tx.remote_insert;
3777
3778 rx_pending = service->bulk_rx.local_insert -
3779 service->bulk_rx.remote_insert;
3780
3781 len = snprintf(buf, sizeof(buf),
3782 " Bulk: tx_pending=%d (size %d),"
3783 " rx_pending=%d (size %d)",
3784 tx_pending,
3785 tx_pending ? service->bulk_tx.bulks[
3786 BULK_INDEX(service->bulk_tx.remove)].size : 0,
3787 rx_pending,
3788 rx_pending ? service->bulk_rx.bulks[
3789 BULK_INDEX(service->bulk_rx.remove)].size : 0);
3790
3791 if (VCHIQ_ENABLE_STATS) {
3792 vchiq_dump(dump_context, buf, len + 1);
3793
3794 len = snprintf(buf, sizeof(buf),
3795 " Ctrl: tx_count=%d, tx_bytes=%llu, "
3796 "rx_count=%d, rx_bytes=%llu",
3797 service->stats.ctrl_tx_count,
3798 service->stats.ctrl_tx_bytes,
3799 service->stats.ctrl_rx_count,
3800 service->stats.ctrl_rx_bytes);
3801 vchiq_dump(dump_context, buf, len + 1);
3802
3803 len = snprintf(buf, sizeof(buf),
3804 " Bulk: tx_count=%d, tx_bytes=%llu, "
3805 "rx_count=%d, rx_bytes=%llu",
3806 service->stats.bulk_tx_count,
3807 service->stats.bulk_tx_bytes,
3808 service->stats.bulk_rx_count,
3809 service->stats.bulk_rx_bytes);
3810 vchiq_dump(dump_context, buf, len + 1);
3811
3812 len = snprintf(buf, sizeof(buf),
3813 " %d quota stalls, %d slot stalls, "
3814 "%d bulk stalls, %d aborted, %d errors",
3815 service->stats.quota_stalls,
3816 service->stats.slot_stalls,
3817 service->stats.bulk_stalls,
3818 service->stats.bulk_aborted_count,
3819 service->stats.error_count);
3820 }
3821 }
3822
3823 vchiq_dump(dump_context, buf, len + 1);
3824
3825 if (service->srvstate != VCHIQ_SRVSTATE_FREE)
3826 vchiq_dump_platform_service_state(dump_context, service);
3827}
3828
3829
3830void
3831vchiq_loud_error_header(void)
3832{
3833 vchiq_log_error(vchiq_core_log_level,
3834 "============================================================"
3835 "================");
3836 vchiq_log_error(vchiq_core_log_level,
3837 "============================================================"
3838 "================");
3839 vchiq_log_error(vchiq_core_log_level, "=====");
3840}
3841
3842void
3843vchiq_loud_error_footer(void)
3844{
3845 vchiq_log_error(vchiq_core_log_level, "=====");
3846 vchiq_log_error(vchiq_core_log_level,
3847 "============================================================"
3848 "================");
3849 vchiq_log_error(vchiq_core_log_level,
3850 "============================================================"
3851 "================");
3852}
3853
3854
3855VCHIQ_STATUS_T vchiq_send_remote_use(VCHIQ_STATE_T *state)
3856{
3857 VCHIQ_STATUS_T status = VCHIQ_RETRY;
3858 if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
3859 status = queue_message(state, NULL,
3860 VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE, 0, 0),
3861 NULL, 0, 0, 0);
3862 return status;
3863}
3864
3865VCHIQ_STATUS_T vchiq_send_remote_release(VCHIQ_STATE_T *state)
3866{
3867 VCHIQ_STATUS_T status = VCHIQ_RETRY;
3868 if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
3869 status = queue_message(state, NULL,
3870 VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_RELEASE, 0, 0),
3871 NULL, 0, 0, 0);
3872 return status;
3873}
3874
3875VCHIQ_STATUS_T vchiq_send_remote_use_active(VCHIQ_STATE_T *state)
3876{
3877 VCHIQ_STATUS_T status = VCHIQ_RETRY;
3878 if (state->conn_state != VCHIQ_CONNSTATE_DISCONNECTED)
3879 status = queue_message(state, NULL,
3880 VCHIQ_MAKE_MSG(VCHIQ_MSG_REMOTE_USE_ACTIVE, 0, 0),
3881 NULL, 0, 0, 0);
3882 return status;
3883}
3884
3885void vchiq_log_dump_mem(const char *label, uint32_t addr, const void *voidMem,
3886 size_t numBytes)
3887{
3888 const uint8_t *mem = (const uint8_t *)voidMem;
3889 size_t offset;
3890 char lineBuf[100];
3891 char *s;
3892
3893 while (numBytes > 0) {
3894 s = lineBuf;
3895
3896 for (offset = 0; offset < 16; offset++) {
3897 if (offset < numBytes)
3898 s += snprintf(s, 4, "%02x ", mem[offset]);
3899 else
3900 s += snprintf(s, 4, " ");
3901 }
3902
3903 for (offset = 0; offset < 16; offset++) {
3904 if (offset < numBytes) {
3905 uint8_t ch = mem[offset];
3906
3907 if ((ch < ' ') || (ch > '~'))
3908 ch = '.';
3909 *s++ = (char)ch;
3910 }
3911 }
3912 *s++ = '\0';
3913
3914 if ((label != NULL) && (*label != '\0'))
3915 vchiq_log_trace(VCHIQ_LOG_TRACE,
3916 "%s: %08x: %s", label, addr, lineBuf);
3917 else
3918 vchiq_log_trace(VCHIQ_LOG_TRACE,
3919 "%08x: %s", addr, lineBuf);
3920
3921 addr += 16;
3922 mem += 16;
3923 if (numBytes > 16)
3924 numBytes -= 16;
3925 else
3926 numBytes = 0;
3927 }
3928}
ubuntu-zesty-kernel mirror