]> 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:


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