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