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