vchiq_core.c

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

FreeBSD/Linux Kernel Cross Reference
sys/contrib/vchiq/interface/vchiq_arm/vchiq_core.c
