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