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