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