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