drivers/staging/vc04_services/interface/vchiq_arm/vchiq_core.c

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