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