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