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