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