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