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