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