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

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