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