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