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