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