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