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