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