drivers/staging/vc04_services/interface/vchiq_arm/vchiq_arm.c

   1 // SPDX-License-Identifier: GPL-2.0 OR BSD-3-Clause
   2 /*
   3  * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved.
   4  * Copyright (c) 2010-2012 Broadcom. All rights reserved.
   5  */
   6
   7 #include <linux/kernel.h>
   8 #include <linux/module.h>
   9 #include <linux/sched/signal.h>
  10 #include <linux/types.h>
  11 #include <linux/errno.h>
  12 #include <linux/cdev.h>
  13 #include <linux/fs.h>
  14 #include <linux/device.h>
  15 #include <linux/mm.h>
  16 #include <linux/highmem.h>
  17 #include <linux/pagemap.h>
  18 #include <linux/bug.h>
  19 #include <linux/completion.h>
  20 #include <linux/list.h>
  21 #include <linux/of.h>
  22 #include <linux/platform_device.h>
  23 #include <linux/compat.h>
  24 #include <linux/dma-mapping.h>
  25 #include <linux/rcupdate.h>
  26 #include <soc/bcm2835/raspberrypi-firmware.h>
  27
  28 #include "vchiq_core.h"
  29 #include "vchiq_ioctl.h"
  30 #include "vchiq_arm.h"
  31 #include "vchiq_debugfs.h"
  32
  33 #define DEVICE_NAME "vchiq"
  34
  35 /* Override the default prefix, which would be vchiq_arm (from the filename) */
  36 #undef MODULE_PARAM_PREFIX
  37 #define MODULE_PARAM_PREFIX DEVICE_NAME "."
  38
  39 /* Some per-instance constants */
  40 #define MAX_COMPLETIONS 128
  41 #define MAX_SERVICES 64
  42 #define MAX_ELEMENTS 8
  43 #define MSG_QUEUE_SIZE 128
  44
  45 #define KEEPALIVE_VER 1
  46 #define KEEPALIVE_VER_MIN KEEPALIVE_VER
  47
  48 /* Run time control of log level, based on KERN_XXX level. */
  49 int vchiq_arm_log_level = VCHIQ_LOG_DEFAULT;
  50 int vchiq_susp_log_level = VCHIQ_LOG_ERROR;
  51
  52 struct user_service {
  53         struct vchiq_service *service;
  54         void *userdata;
  55         struct vchiq_instance *instance;
  56         char is_vchi;
  57         char dequeue_pending;
  58         char close_pending;
  59         int message_available_pos;
  60         int msg_insert;
  61         int msg_remove;
  62         struct completion insert_event;
  63         struct completion remove_event;
  64         struct completion close_event;
  65         struct vchiq_header *msg_queue[MSG_QUEUE_SIZE];
  66 };
  67
  68 struct bulk_waiter_node {
  69         struct bulk_waiter bulk_waiter;
  70         int pid;
  71         struct list_head list;
  72 };
  73
  74 struct vchiq_instance {
  75         struct vchiq_state *state;
  76         struct vchiq_completion_data completions[MAX_COMPLETIONS];
  77         int completion_insert;
  78         int completion_remove;
  79         struct completion insert_event;
  80         struct completion remove_event;
  81         struct mutex completion_mutex;
  82
  83         int connected;
  84         int closing;
  85         int pid;
  86         int mark;
  87         int use_close_delivered;
  88         int trace;
  89
  90         struct list_head bulk_waiter_list;
  91         struct mutex bulk_waiter_list_mutex;
  92
  93         struct vchiq_debugfs_node debugfs_node;
  94 };
  95
  96 struct dump_context {
  97         char __user *buf;
  98         size_t actual;
  99         size_t space;
 100         loff_t offset;
 101 };
 102
 103 static struct cdev    vchiq_cdev;
 104 static dev_t          vchiq_devid;
 105 static struct vchiq_state g_state;
 106 static struct class  *vchiq_class;
 107 static DEFINE_SPINLOCK(msg_queue_spinlock);
 108 static struct platform_device *bcm2835_camera;
 109 static struct platform_device *bcm2835_audio;
 110
 111 static struct vchiq_drvdata bcm2835_drvdata = {
 112         .cache_line_size = 32,
 113 };
 114
 115 static struct vchiq_drvdata bcm2836_drvdata = {
 116         .cache_line_size = 64,
 117 };
 118
 119 static const char *const ioctl_names[] = {
 120         "CONNECT",
 121         "SHUTDOWN",
 122         "CREATE_SERVICE",
 123         "REMOVE_SERVICE",
 124         "QUEUE_MESSAGE",
 125         "QUEUE_BULK_TRANSMIT",
 126         "QUEUE_BULK_RECEIVE",
 127         "AWAIT_COMPLETION",
 128         "DEQUEUE_MESSAGE",
 129         "GET_CLIENT_ID",
 130         "GET_CONFIG",
 131         "CLOSE_SERVICE",
 132         "USE_SERVICE",
 133         "RELEASE_SERVICE",
 134         "SET_SERVICE_OPTION",
 135         "DUMP_PHYS_MEM",
 136         "LIB_VERSION",
 137         "CLOSE_DELIVERED"
 138 };
 139
 140 vchiq_static_assert(ARRAY_SIZE(ioctl_names) ==
 141                     (VCHIQ_IOC_MAX + 1));
 142
 143 static enum vchiq_status
 144 vchiq_blocking_bulk_transfer(unsigned int handle, void *data,
 145         unsigned int size, enum vchiq_bulk_dir dir);
 146
 147 #define VCHIQ_INIT_RETRIES 10
 148 enum vchiq_status vchiq_initialise(struct vchiq_instance **instance_out)
 149 {
 150         enum vchiq_status status = VCHIQ_ERROR;
 151         struct vchiq_state *state;
 152         struct vchiq_instance *instance = NULL;
 153         int i;
 154
 155         vchiq_log_trace(vchiq_core_log_level, "%s called", __func__);
 156
 157         /* VideoCore may not be ready due to boot up timing.
 158          * It may never be ready if kernel and firmware are mismatched,so don't
 159          * block forever.
 160          */
 161         for (i = 0; i < VCHIQ_INIT_RETRIES; i++) {
 162                 state = vchiq_get_state();
 163                 if (state)
 164                         break;
 165                 usleep_range(500, 600);
 166         }
 167         if (i == VCHIQ_INIT_RETRIES) {
 168                 vchiq_log_error(vchiq_core_log_level,
 169                         "%s: videocore not initialized\n", __func__);
 170                 goto failed;
 171         } else if (i > 0) {
 172                 vchiq_log_warning(vchiq_core_log_level,
 173                         "%s: videocore initialized after %d retries\n",
 174                         __func__, i);
 175         }
 176
 177         instance = kzalloc(sizeof(*instance), GFP_KERNEL);
 178         if (!instance) {
 179                 vchiq_log_error(vchiq_core_log_level,
 180                         "%s: error allocating vchiq instance\n", __func__);
 181                 goto failed;
 182         }
 183
 184         instance->connected = 0;
 185         instance->state = state;
 186         mutex_init(&instance->bulk_waiter_list_mutex);
 187         INIT_LIST_HEAD(&instance->bulk_waiter_list);
 188
 189         *instance_out = instance;
 190
 191         status = VCHIQ_SUCCESS;
 192
 193 failed:
 194         vchiq_log_trace(vchiq_core_log_level,
 195                 "%s(%p): returning %d", __func__, instance, status);
 196
 197         return status;
 198 }
 199 EXPORT_SYMBOL(vchiq_initialise);
 200
 201 enum vchiq_status vchiq_shutdown(struct vchiq_instance *instance)
 202 {
 203         enum vchiq_status status;
 204         struct vchiq_state *state = instance->state;
 205
 206         vchiq_log_trace(vchiq_core_log_level,
 207                 "%s(%p) called", __func__, instance);
 208
 209         if (mutex_lock_killable(&state->mutex))
 210                 return VCHIQ_RETRY;
 211
 212         /* Remove all services */
 213         status = vchiq_shutdown_internal(state, instance);
 214
 215         mutex_unlock(&state->mutex);
 216
 217         vchiq_log_trace(vchiq_core_log_level,
 218                 "%s(%p): returning %d", __func__, instance, status);
 219
 220         if (status == VCHIQ_SUCCESS) {
 221                 struct bulk_waiter_node *waiter, *next;
 222
 223                 list_for_each_entry_safe(waiter, next,
 224                                          &instance->bulk_waiter_list, list) {
 225                         list_del(&waiter->list);
 226                         vchiq_log_info(vchiq_arm_log_level,
 227                                         "bulk_waiter - cleaned up %pK for pid %d",
 228                                         waiter, waiter->pid);
 229                         kfree(waiter);
 230                 }
 231                 kfree(instance);
 232         }
 233
 234         return status;
 235 }
 236 EXPORT_SYMBOL(vchiq_shutdown);
 237
 238 static int vchiq_is_connected(struct vchiq_instance *instance)
 239 {
 240         return instance->connected;
 241 }
 242
 243 enum vchiq_status vchiq_connect(struct vchiq_instance *instance)
 244 {
 245         enum vchiq_status status;
 246         struct vchiq_state *state = instance->state;
 247
 248         vchiq_log_trace(vchiq_core_log_level,
 249                 "%s(%p) called", __func__, instance);
 250
 251         if (mutex_lock_killable(&state->mutex)) {
 252                 vchiq_log_trace(vchiq_core_log_level,
 253                         "%s: call to mutex_lock failed", __func__);
 254                 status = VCHIQ_RETRY;
 255                 goto failed;
 256         }
 257         status = vchiq_connect_internal(state, instance);
 258
 259         if (status == VCHIQ_SUCCESS)
 260                 instance->connected = 1;
 261
 262         mutex_unlock(&state->mutex);
 263
 264 failed:
 265         vchiq_log_trace(vchiq_core_log_level,
 266                 "%s(%p): returning %d", __func__, instance, status);
 267
 268         return status;
 269 }
 270 EXPORT_SYMBOL(vchiq_connect);
 271
 272 enum vchiq_status vchiq_add_service(
 273         struct vchiq_instance             *instance,
 274         const struct vchiq_service_params *params,
 275         unsigned int       *phandle)
 276 {
 277         enum vchiq_status status;
 278         struct vchiq_state *state = instance->state;
 279         struct vchiq_service *service = NULL;
 280         int srvstate;
 281
 282         vchiq_log_trace(vchiq_core_log_level,
 283                 "%s(%p) called", __func__, instance);
 284
 285         *phandle = VCHIQ_SERVICE_HANDLE_INVALID;
 286
 287         srvstate = vchiq_is_connected(instance)
 288                 ? VCHIQ_SRVSTATE_LISTENING
 289                 : VCHIQ_SRVSTATE_HIDDEN;
 290
 291         service = vchiq_add_service_internal(
 292                 state,
 293                 params,
 294                 srvstate,
 295                 instance,
 296                 NULL);
 297
 298         if (service) {
 299                 *phandle = service->handle;
 300                 status = VCHIQ_SUCCESS;
 301         } else
 302                 status = VCHIQ_ERROR;
 303
 304         vchiq_log_trace(vchiq_core_log_level,
 305                 "%s(%p): returning %d", __func__, instance, status);
 306
 307         return status;
 308 }
 309 EXPORT_SYMBOL(vchiq_add_service);
 310
 311 enum vchiq_status vchiq_open_service(
 312         struct vchiq_instance             *instance,
 313         const struct vchiq_service_params *params,
 314         unsigned int       *phandle)
 315 {
 316         enum vchiq_status   status = VCHIQ_ERROR;
 317         struct vchiq_state   *state = instance->state;
 318         struct vchiq_service *service = NULL;
 319
 320         vchiq_log_trace(vchiq_core_log_level,
 321                 "%s(%p) called", __func__, instance);
 322
 323         *phandle = VCHIQ_SERVICE_HANDLE_INVALID;
 324
 325         if (!vchiq_is_connected(instance))
 326                 goto failed;
 327
 328         service = vchiq_add_service_internal(state,
 329                 params,
 330                 VCHIQ_SRVSTATE_OPENING,
 331                 instance,
 332                 NULL);
 333
 334         if (service) {
 335                 *phandle = service->handle;
 336                 status = vchiq_open_service_internal(service, current->pid);
 337                 if (status != VCHIQ_SUCCESS) {
 338                         vchiq_remove_service(service->handle);
 339                         *phandle = VCHIQ_SERVICE_HANDLE_INVALID;
 340                 }
 341         }
 342
 343 failed:
 344         vchiq_log_trace(vchiq_core_log_level,
 345                 "%s(%p): returning %d", __func__, instance, status);
 346
 347         return status;
 348 }
 349 EXPORT_SYMBOL(vchiq_open_service);
 350
 351 enum vchiq_status
 352 vchiq_bulk_transmit(unsigned int handle, const void *data,
 353         unsigned int size, void *userdata, enum vchiq_bulk_mode mode)
 354 {
 355         enum vchiq_status status;
 356
 357         switch (mode) {
 358         case VCHIQ_BULK_MODE_NOCALLBACK:
 359         case VCHIQ_BULK_MODE_CALLBACK:
 360                 status = vchiq_bulk_transfer(handle, (void *)data, size,
 361                                              userdata, mode,
 362                                              VCHIQ_BULK_TRANSMIT);
 363                 break;
 364         case VCHIQ_BULK_MODE_BLOCKING:
 365                 status = vchiq_blocking_bulk_transfer(handle,
 366                         (void *)data, size, VCHIQ_BULK_TRANSMIT);
 367                 break;
 368         default:
 369                 return VCHIQ_ERROR;
 370         }
 371
 372         return status;
 373 }
 374 EXPORT_SYMBOL(vchiq_bulk_transmit);
 375
 376 enum vchiq_status
 377 vchiq_bulk_receive(unsigned int handle, void *data,
 378         unsigned int size, void *userdata, enum vchiq_bulk_mode mode)
 379 {
 380         enum vchiq_status status;
 381
 382         switch (mode) {
 383         case VCHIQ_BULK_MODE_NOCALLBACK:
 384         case VCHIQ_BULK_MODE_CALLBACK:
 385                 status = vchiq_bulk_transfer(handle, data, size, userdata,
 386                                              mode, VCHIQ_BULK_RECEIVE);
 387                 break;
 388         case VCHIQ_BULK_MODE_BLOCKING:
 389                 status = vchiq_blocking_bulk_transfer(handle,
 390                         (void *)data, size, VCHIQ_BULK_RECEIVE);
 391                 break;
 392         default:
 393                 return VCHIQ_ERROR;
 394         }
 395
 396         return status;
 397 }
 398 EXPORT_SYMBOL(vchiq_bulk_receive);
 399
 400 static enum vchiq_status
 401 vchiq_blocking_bulk_transfer(unsigned int handle, void *data,
 402         unsigned int size, enum vchiq_bulk_dir dir)
 403 {
 404         struct vchiq_instance *instance;
 405         struct vchiq_service *service;
 406         enum vchiq_status status;
 407         struct bulk_waiter_node *waiter = NULL;
 408
 409         service = find_service_by_handle(handle);
 410         if (!service)
 411                 return VCHIQ_ERROR;
 412
 413         instance = service->instance;
 414
 415         unlock_service(service);
 416
 417         mutex_lock(&instance->bulk_waiter_list_mutex);
 418         list_for_each_entry(waiter, &instance->bulk_waiter_list, list) {
 419                 if (waiter->pid == current->pid) {
 420                         list_del(&waiter->list);
 421                         break;
 422                 }
 423         }
 424         mutex_unlock(&instance->bulk_waiter_list_mutex);
 425
 426         if (waiter) {
 427                 struct vchiq_bulk *bulk = waiter->bulk_waiter.bulk;
 428
 429                 if (bulk) {
 430                         /* This thread has an outstanding bulk transfer. */
 431                         if ((bulk->data != data) ||
 432                                 (bulk->size != size)) {
 433                                 /* This is not a retry of the previous one.
 434                                  * Cancel the signal when the transfer
 435                                  * completes.
 436                                  */
 437                                 spin_lock(&bulk_waiter_spinlock);
 438                                 bulk->userdata = NULL;
 439                                 spin_unlock(&bulk_waiter_spinlock);
 440                         }
 441                 }
 442         }
 443
 444         if (!waiter) {
 445                 waiter = kzalloc(sizeof(struct bulk_waiter_node), GFP_KERNEL);
 446                 if (!waiter) {
 447                         vchiq_log_error(vchiq_core_log_level,
 448                                 "%s - out of memory", __func__);
 449                         return VCHIQ_ERROR;
 450                 }
 451         }
 452
 453         status = vchiq_bulk_transfer(handle, data, size, &waiter->bulk_waiter,
 454                                      VCHIQ_BULK_MODE_BLOCKING, dir);
 455         if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) ||
 456                 !waiter->bulk_waiter.bulk) {
 457                 struct vchiq_bulk *bulk = waiter->bulk_waiter.bulk;
 458
 459                 if (bulk) {
 460                         /* Cancel the signal when the transfer
 461                          * completes.
 462                          */
 463                         spin_lock(&bulk_waiter_spinlock);
 464                         bulk->userdata = NULL;
 465                         spin_unlock(&bulk_waiter_spinlock);
 466                 }
 467                 kfree(waiter);
 468         } else {
 469                 waiter->pid = current->pid;
 470                 mutex_lock(&instance->bulk_waiter_list_mutex);
 471                 list_add(&waiter->list, &instance->bulk_waiter_list);
 472                 mutex_unlock(&instance->bulk_waiter_list_mutex);
 473                 vchiq_log_info(vchiq_arm_log_level,
 474                                 "saved bulk_waiter %pK for pid %d",
 475                                 waiter, current->pid);
 476         }
 477
 478         return status;
 479 }
 480 /****************************************************************************
 481 *
 482 *   add_completion
 483 *
 484 ***************************************************************************/
 485
 486 static enum vchiq_status
 487 add_completion(struct vchiq_instance *instance, enum vchiq_reason reason,
 488                struct vchiq_header *header, struct user_service *user_service,
 489                void *bulk_userdata)
 490 {
 491         struct vchiq_completion_data *completion;
 492         int insert;
 493
 494         DEBUG_INITIALISE(g_state.local)
 495
 496         insert = instance->completion_insert;
 497         while ((insert - instance->completion_remove) >= MAX_COMPLETIONS) {
 498                 /* Out of space - wait for the client */
 499                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 500                 vchiq_log_trace(vchiq_arm_log_level,
 501                         "%s - completion queue full", __func__);
 502                 DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT);
 503                 if (wait_for_completion_interruptible(
 504                                         &instance->remove_event)) {
 505                         vchiq_log_info(vchiq_arm_log_level,
 506                                 "service_callback interrupted");
 507                         return VCHIQ_RETRY;
 508                 } else if (instance->closing) {
 509                         vchiq_log_info(vchiq_arm_log_level,
 510                                 "service_callback closing");
 511                         return VCHIQ_SUCCESS;
 512                 }
 513                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 514         }
 515
 516         completion = &instance->completions[insert & (MAX_COMPLETIONS - 1)];
 517
 518         completion->header = header;
 519         completion->reason = reason;
 520         /* N.B. service_userdata is updated while processing AWAIT_COMPLETION */
 521         completion->service_userdata = user_service->service;
 522         completion->bulk_userdata = bulk_userdata;
 523
 524         if (reason == VCHIQ_SERVICE_CLOSED) {
 525                 /* Take an extra reference, to be held until
 526                    this CLOSED notification is delivered. */
 527                 lock_service(user_service->service);
 528                 if (instance->use_close_delivered)
 529                         user_service->close_pending = 1;
 530         }
 531
 532         /* A write barrier is needed here to ensure that the entire completion
 533                 record is written out before the insert point. */
 534         wmb();
 535
 536         if (reason == VCHIQ_MESSAGE_AVAILABLE)
 537                 user_service->message_available_pos = insert;
 538
 539         insert++;
 540         instance->completion_insert = insert;
 541
 542         complete(&instance->insert_event);
 543
 544         return VCHIQ_SUCCESS;
 545 }
 546
 547 /****************************************************************************
 548 *
 549 *   service_callback
 550 *
 551 ***************************************************************************/
 552
 553 static enum vchiq_status
 554 service_callback(enum vchiq_reason reason, struct vchiq_header *header,
 555                  unsigned int handle, void *bulk_userdata)
 556 {
 557         /* How do we ensure the callback goes to the right client?
 558         ** The service_user data points to a user_service record
 559         ** containing the original callback and the user state structure, which
 560         ** contains a circular buffer for completion records.
 561         */
 562         struct user_service *user_service;
 563         struct vchiq_service *service;
 564         struct vchiq_instance *instance;
 565         bool skip_completion = false;
 566
 567         DEBUG_INITIALISE(g_state.local)
 568
 569         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 570
 571         service = handle_to_service(handle);
 572         BUG_ON(!service);
 573         user_service = (struct user_service *)service->base.userdata;
 574         instance = user_service->instance;
 575
 576         if (!instance || instance->closing)
 577                 return VCHIQ_SUCCESS;
 578
 579         vchiq_log_trace(vchiq_arm_log_level,
 580                 "%s - service %lx(%d,%p), reason %d, header %lx, "
 581                 "instance %lx, bulk_userdata %lx",
 582                 __func__, (unsigned long)user_service,
 583                 service->localport, user_service->userdata,
 584                 reason, (unsigned long)header,
 585                 (unsigned long)instance, (unsigned long)bulk_userdata);
 586
 587         if (header && user_service->is_vchi) {
 588                 spin_lock(&msg_queue_spinlock);
 589                 while (user_service->msg_insert ==
 590                         (user_service->msg_remove + MSG_QUEUE_SIZE)) {
 591                         spin_unlock(&msg_queue_spinlock);
 592                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 593                         DEBUG_COUNT(MSG_QUEUE_FULL_COUNT);
 594                         vchiq_log_trace(vchiq_arm_log_level,
 595                                 "service_callback - msg queue full");
 596                         /* If there is no MESSAGE_AVAILABLE in the completion
 597                         ** queue, add one
 598                         */
 599                         if ((user_service->message_available_pos -
 600                                 instance->completion_remove) < 0) {
 601                                 enum vchiq_status status;
 602
 603                                 vchiq_log_info(vchiq_arm_log_level,
 604                                         "Inserting extra MESSAGE_AVAILABLE");
 605                                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 606                                 status = add_completion(instance, reason,
 607                                         NULL, user_service, bulk_userdata);
 608                                 if (status != VCHIQ_SUCCESS) {
 609                                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 610                                         return status;
 611                                 }
 612                         }
 613
 614                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 615                         if (wait_for_completion_interruptible(
 616                                                 &user_service->remove_event)) {
 617                                 vchiq_log_info(vchiq_arm_log_level,
 618                                         "%s interrupted", __func__);
 619                                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 620                                 return VCHIQ_RETRY;
 621                         } else if (instance->closing) {
 622                                 vchiq_log_info(vchiq_arm_log_level,
 623                                         "%s closing", __func__);
 624                                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 625                                 return VCHIQ_ERROR;
 626                         }
 627                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 628                         spin_lock(&msg_queue_spinlock);
 629                 }
 630
 631                 user_service->msg_queue[user_service->msg_insert &
 632                         (MSG_QUEUE_SIZE - 1)] = header;
 633                 user_service->msg_insert++;
 634
 635                 /* If there is a thread waiting in DEQUEUE_MESSAGE, or if
 636                 ** there is a MESSAGE_AVAILABLE in the completion queue then
 637                 ** bypass the completion queue.
 638                 */
 639                 if (((user_service->message_available_pos -
 640                         instance->completion_remove) >= 0) ||
 641                         user_service->dequeue_pending) {
 642                         user_service->dequeue_pending = 0;
 643                         skip_completion = true;
 644                 }
 645
 646                 spin_unlock(&msg_queue_spinlock);
 647                 complete(&user_service->insert_event);
 648
 649                 header = NULL;
 650         }
 651         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 652
 653         if (skip_completion)
 654                 return VCHIQ_SUCCESS;
 655
 656         return add_completion(instance, reason, header, user_service,
 657                 bulk_userdata);
 658 }
 659
 660 /****************************************************************************
 661 *
 662 *   user_service_free
 663 *
 664 ***************************************************************************/
 665 static void
 666 user_service_free(void *userdata)
 667 {
 668         kfree(userdata);
 669 }
 670
 671 /****************************************************************************
 672 *
 673 *   close_delivered
 674 *
 675 ***************************************************************************/
 676 static void close_delivered(struct user_service *user_service)
 677 {
 678         vchiq_log_info(vchiq_arm_log_level,
 679                 "%s(handle=%x)",
 680                 __func__, user_service->service->handle);
 681
 682         if (user_service->close_pending) {
 683                 /* Allow the underlying service to be culled */
 684                 unlock_service(user_service->service);
 685
 686                 /* Wake the user-thread blocked in close_ or remove_service */
 687                 complete(&user_service->close_event);
 688
 689                 user_service->close_pending = 0;
 690         }
 691 }
 692
 693 struct vchiq_io_copy_callback_context {
 694         struct vchiq_element *element;
 695         size_t element_offset;
 696         unsigned long elements_to_go;
 697 };
 698
 699 static ssize_t vchiq_ioc_copy_element_data(void *context, void *dest,
 700                                            size_t offset, size_t maxsize)
 701 {
 702         struct vchiq_io_copy_callback_context *cc = context;
 703         size_t total_bytes_copied = 0;
 704         size_t bytes_this_round;
 705
 706         while (total_bytes_copied < maxsize) {
 707                 if (!cc->elements_to_go)
 708                         return total_bytes_copied;
 709
 710                 if (!cc->element->size) {
 711                         cc->elements_to_go--;
 712                         cc->element++;
 713                         cc->element_offset = 0;
 714                         continue;
 715                 }
 716
 717                 bytes_this_round = min(cc->element->size - cc->element_offset,
 718                                        maxsize - total_bytes_copied);
 719
 720                 if (copy_from_user(dest + total_bytes_copied,
 721                                   cc->element->data + cc->element_offset,
 722                                   bytes_this_round))
 723                         return -EFAULT;
 724
 725                 cc->element_offset += bytes_this_round;
 726                 total_bytes_copied += bytes_this_round;
 727
 728                 if (cc->element_offset == cc->element->size) {
 729                         cc->elements_to_go--;
 730                         cc->element++;
 731                         cc->element_offset = 0;
 732                 }
 733         }
 734
 735         return maxsize;
 736 }
 737
 738 /**************************************************************************
 739  *
 740  *   vchiq_ioc_queue_message
 741  *
 742  **************************************************************************/
 743 static enum vchiq_status
 744 vchiq_ioc_queue_message(unsigned int handle,
 745                         struct vchiq_element *elements,
 746                         unsigned long count)
 747 {
 748         struct vchiq_io_copy_callback_context context;
 749         unsigned long i;
 750         size_t total_size = 0;
 751
 752         context.element = elements;
 753         context.element_offset = 0;
 754         context.elements_to_go = count;
 755
 756         for (i = 0; i < count; i++) {
 757                 if (!elements[i].data && elements[i].size != 0)
 758                         return -EFAULT;
 759
 760                 total_size += elements[i].size;
 761         }
 762
 763         return vchiq_queue_message(handle, vchiq_ioc_copy_element_data,
 764                                    &context, total_size);
 765 }
 766
 767 /****************************************************************************
 768 *
 769 *   vchiq_ioctl
 770 *
 771 ***************************************************************************/
 772 static long
 773 vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 774 {
 775         struct vchiq_instance *instance = file->private_data;
 776         enum vchiq_status status = VCHIQ_SUCCESS;
 777         struct vchiq_service *service = NULL;
 778         long ret = 0;
 779         int i, rc;
 780
 781         DEBUG_INITIALISE(g_state.local)
 782
 783         vchiq_log_trace(vchiq_arm_log_level,
 784                 "%s - instance %pK, cmd %s, arg %lx",
 785                 __func__, instance,
 786                 ((_IOC_TYPE(cmd) == VCHIQ_IOC_MAGIC) &&
 787                 (_IOC_NR(cmd) <= VCHIQ_IOC_MAX)) ?
 788                 ioctl_names[_IOC_NR(cmd)] : "<invalid>", arg);
 789
 790         switch (cmd) {
 791         case VCHIQ_IOC_SHUTDOWN:
 792                 if (!instance->connected)
 793                         break;
 794
 795                 /* Remove all services */
 796                 i = 0;
 797                 while ((service = next_service_by_instance(instance->state,
 798                         instance, &i))) {
 799                         status = vchiq_remove_service(service->handle);
 800                         unlock_service(service);
 801                         if (status != VCHIQ_SUCCESS)
 802                                 break;
 803                 }
 804                 service = NULL;
 805
 806                 if (status == VCHIQ_SUCCESS) {
 807                         /* Wake the completion thread and ask it to exit */
 808                         instance->closing = 1;
 809                         complete(&instance->insert_event);
 810                 }
 811
 812                 break;
 813
 814         case VCHIQ_IOC_CONNECT:
 815                 if (instance->connected) {
 816                         ret = -EINVAL;
 817                         break;
 818                 }
 819                 rc = mutex_lock_killable(&instance->state->mutex);
 820                 if (rc) {
 821                         vchiq_log_error(vchiq_arm_log_level,
 822                                 "vchiq: connect: could not lock mutex for "
 823                                 "state %d: %d",
 824                                 instance->state->id, rc);
 825                         ret = -EINTR;
 826                         break;
 827                 }
 828                 status = vchiq_connect_internal(instance->state, instance);
 829                 mutex_unlock(&instance->state->mutex);
 830
 831                 if (status == VCHIQ_SUCCESS)
 832                         instance->connected = 1;
 833                 else
 834                         vchiq_log_error(vchiq_arm_log_level,
 835                                 "vchiq: could not connect: %d", status);
 836                 break;
 837
 838         case VCHIQ_IOC_CREATE_SERVICE: {
 839                 struct vchiq_create_service args;
 840                 struct user_service *user_service = NULL;
 841                 void *userdata;
 842                 int srvstate;
 843
 844                 if (copy_from_user(&args, (const void __user *)arg,
 845                                    sizeof(args))) {
 846                         ret = -EFAULT;
 847                         break;
 848                 }
 849
 850                 user_service = kmalloc(sizeof(*user_service), GFP_KERNEL);
 851                 if (!user_service) {
 852                         ret = -ENOMEM;
 853                         break;
 854                 }
 855
 856                 if (args.is_open) {
 857                         if (!instance->connected) {
 858                                 ret = -ENOTCONN;
 859                                 kfree(user_service);
 860                                 break;
 861                         }
 862                         srvstate = VCHIQ_SRVSTATE_OPENING;
 863                 } else {
 864                         srvstate =
 865                                  instance->connected ?
 866                                  VCHIQ_SRVSTATE_LISTENING :
 867                                  VCHIQ_SRVSTATE_HIDDEN;
 868                 }
 869
 870                 userdata = args.params.userdata;
 871                 args.params.callback = service_callback;
 872                 args.params.userdata = user_service;
 873                 service = vchiq_add_service_internal(
 874                                 instance->state,
 875                                 &args.params, srvstate,
 876                                 instance, user_service_free);
 877
 878                 if (service) {
 879                         user_service->service = service;
 880                         user_service->userdata = userdata;
 881                         user_service->instance = instance;
 882                         user_service->is_vchi = (args.is_vchi != 0);
 883                         user_service->dequeue_pending = 0;
 884                         user_service->close_pending = 0;
 885                         user_service->message_available_pos =
 886                                 instance->completion_remove - 1;
 887                         user_service->msg_insert = 0;
 888                         user_service->msg_remove = 0;
 889                         init_completion(&user_service->insert_event);
 890                         init_completion(&user_service->remove_event);
 891                         init_completion(&user_service->close_event);
 892
 893                         if (args.is_open) {
 894                                 status = vchiq_open_service_internal
 895                                         (service, instance->pid);
 896                                 if (status != VCHIQ_SUCCESS) {
 897                                         vchiq_remove_service(service->handle);
 898                                         service = NULL;
 899                                         ret = (status == VCHIQ_RETRY) ?
 900                                                 -EINTR : -EIO;
 901                                         break;
 902                                 }
 903                         }
 904
 905                         if (copy_to_user((void __user *)
 906                                 &(((struct vchiq_create_service __user *)
 907                                         arg)->handle),
 908                                 (const void *)&service->handle,
 909                                 sizeof(service->handle))) {
 910                                 ret = -EFAULT;
 911                                 vchiq_remove_service(service->handle);
 912                         }
 913
 914                         service = NULL;
 915                 } else {
 916                         ret = -EEXIST;
 917                         kfree(user_service);
 918                 }
 919         } break;
 920
 921         case VCHIQ_IOC_CLOSE_SERVICE:
 922         case VCHIQ_IOC_REMOVE_SERVICE: {
 923                 unsigned int handle = (unsigned int)arg;
 924                 struct user_service *user_service;
 925
 926                 service = find_service_for_instance(instance, handle);
 927                 if (!service) {
 928                         ret = -EINVAL;
 929                         break;
 930                 }
 931
 932                 user_service = service->base.userdata;
 933
 934                 /* close_pending is false on first entry, and when the
 935                    wait in vchiq_close_service has been interrupted. */
 936                 if (!user_service->close_pending) {
 937                         status = (cmd == VCHIQ_IOC_CLOSE_SERVICE) ?
 938                                  vchiq_close_service(service->handle) :
 939                                  vchiq_remove_service(service->handle);
 940                         if (status != VCHIQ_SUCCESS)
 941                                 break;
 942                 }
 943
 944                 /* close_pending is true once the underlying service
 945                    has been closed until the client library calls the
 946                    CLOSE_DELIVERED ioctl, signalling close_event. */
 947                 if (user_service->close_pending &&
 948                         wait_for_completion_interruptible(
 949                                 &user_service->close_event))
 950                         status = VCHIQ_RETRY;
 951                 break;
 952         }
 953
 954         case VCHIQ_IOC_USE_SERVICE:
 955         case VCHIQ_IOC_RELEASE_SERVICE: {
 956                 unsigned int handle = (unsigned int)arg;
 957
 958                 service = find_service_for_instance(instance, handle);
 959                 if (service) {
 960                         status = (cmd == VCHIQ_IOC_USE_SERVICE) ?
 961                                 vchiq_use_service_internal(service) :
 962                                 vchiq_release_service_internal(service);
 963                         if (status != VCHIQ_SUCCESS) {
 964                                 vchiq_log_error(vchiq_susp_log_level,
 965                                         "%s: cmd %s returned error %d for "
 966                                         "service %c%c%c%c:%03d",
 967                                         __func__,
 968                                         (cmd == VCHIQ_IOC_USE_SERVICE) ?
 969                                                 "VCHIQ_IOC_USE_SERVICE" :
 970                                                 "VCHIQ_IOC_RELEASE_SERVICE",
 971                                         status,
 972                                         VCHIQ_FOURCC_AS_4CHARS(
 973                                                 service->base.fourcc),
 974                                         service->client_id);
 975                                 ret = -EINVAL;
 976                         }
 977                 } else
 978                         ret = -EINVAL;
 979         } break;
 980
 981         case VCHIQ_IOC_QUEUE_MESSAGE: {
 982                 struct vchiq_queue_message args;
 983
 984                 if (copy_from_user(&args, (const void __user *)arg,
 985                                    sizeof(args))) {
 986                         ret = -EFAULT;
 987                         break;
 988                 }
 989
 990                 service = find_service_for_instance(instance, args.handle);
 991
 992                 if (service && (args.count <= MAX_ELEMENTS)) {
 993                         /* Copy elements into kernel space */
 994                         struct vchiq_element elements[MAX_ELEMENTS];
 995
 996                         if (copy_from_user(elements, args.elements,
 997                                 args.count * sizeof(struct vchiq_element)) == 0)
 998                                 status = vchiq_ioc_queue_message
 999                                         (args.handle,
1000                                         elements, args.count);
1001                         else
1002                                 ret = -EFAULT;
1003                 } else {
1004                         ret = -EINVAL;
1005                 }
1006         } break;
1007
1008         case VCHIQ_IOC_QUEUE_BULK_TRANSMIT:
1009         case VCHIQ_IOC_QUEUE_BULK_RECEIVE: {
1010                 struct vchiq_queue_bulk_transfer args;
1011                 struct bulk_waiter_node *waiter = NULL;
1012
1013                 enum vchiq_bulk_dir dir =
1014                         (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ?
1015                         VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
1016
1017                 if (copy_from_user(&args, (const void __user *)arg,
1018                                    sizeof(args))) {
1019                         ret = -EFAULT;
1020                         break;
1021                 }
1022
1023                 service = find_service_for_instance(instance, args.handle);
1024                 if (!service) {
1025                         ret = -EINVAL;
1026                         break;
1027                 }
1028
1029                 if (args.mode == VCHIQ_BULK_MODE_BLOCKING) {
1030                         waiter = kzalloc(sizeof(struct bulk_waiter_node),
1031                                 GFP_KERNEL);
1032                         if (!waiter) {
1033                                 ret = -ENOMEM;
1034                                 break;
1035                         }
1036
1037                         args.userdata = &waiter->bulk_waiter;
1038                 } else if (args.mode == VCHIQ_BULK_MODE_WAITING) {
1039                         mutex_lock(&instance->bulk_waiter_list_mutex);
1040                         list_for_each_entry(waiter, &instance->bulk_waiter_list,
1041                                             list) {
1042                                 if (waiter->pid == current->pid) {
1043                                         list_del(&waiter->list);
1044                                         break;
1045                                 }
1046                         }
1047                         mutex_unlock(&instance->bulk_waiter_list_mutex);
1048                         if (!waiter) {
1049                                 vchiq_log_error(vchiq_arm_log_level,
1050                                         "no bulk_waiter found for pid %d",
1051                                         current->pid);
1052                                 ret = -ESRCH;
1053                                 break;
1054                         }
1055                         vchiq_log_info(vchiq_arm_log_level,
1056                                 "found bulk_waiter %pK for pid %d", waiter,
1057                                 current->pid);
1058                         args.userdata = &waiter->bulk_waiter;
1059                 }
1060
1061                 status = vchiq_bulk_transfer(args.handle, args.data, args.size,
1062                                              args.userdata, args.mode, dir);
1063
1064                 if (!waiter)
1065                         break;
1066
1067                 if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) ||
1068                         !waiter->bulk_waiter.bulk) {
1069                         if (waiter->bulk_waiter.bulk) {
1070                                 /* Cancel the signal when the transfer
1071                                 ** completes. */
1072                                 spin_lock(&bulk_waiter_spinlock);
1073                                 waiter->bulk_waiter.bulk->userdata = NULL;
1074                                 spin_unlock(&bulk_waiter_spinlock);
1075                         }
1076                         kfree(waiter);
1077                 } else {
1078                         const enum vchiq_bulk_mode mode_waiting =
1079                                 VCHIQ_BULK_MODE_WAITING;
1080                         waiter->pid = current->pid;
1081                         mutex_lock(&instance->bulk_waiter_list_mutex);
1082                         list_add(&waiter->list, &instance->bulk_waiter_list);
1083                         mutex_unlock(&instance->bulk_waiter_list_mutex);
1084                         vchiq_log_info(vchiq_arm_log_level,
1085                                 "saved bulk_waiter %pK for pid %d",
1086                                 waiter, current->pid);
1087
1088                         if (copy_to_user((void __user *)
1089                                 &(((struct vchiq_queue_bulk_transfer __user *)
1090                                         arg)->mode),
1091                                 (const void *)&mode_waiting,
1092                                 sizeof(mode_waiting)))
1093                                 ret = -EFAULT;
1094                 }
1095         } break;
1096
1097         case VCHIQ_IOC_AWAIT_COMPLETION: {
1098                 struct vchiq_await_completion args;
1099
1100                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1101                 if (!instance->connected) {
1102                         ret = -ENOTCONN;
1103                         break;
1104                 }
1105
1106                 if (copy_from_user(&args, (const void __user *)arg,
1107                         sizeof(args))) {
1108                         ret = -EFAULT;
1109                         break;
1110                 }
1111
1112                 mutex_lock(&instance->completion_mutex);
1113
1114                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1115                 while ((instance->completion_remove ==
1116                         instance->completion_insert)
1117                         && !instance->closing) {
1118                         int rc;
1119
1120                         DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1121                         mutex_unlock(&instance->completion_mutex);
1122                         rc = wait_for_completion_interruptible(
1123                                                 &instance->insert_event);
1124                         mutex_lock(&instance->completion_mutex);
1125                         if (rc) {
1126                                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1127                                 vchiq_log_info(vchiq_arm_log_level,
1128                                         "AWAIT_COMPLETION interrupted");
1129                                 ret = -EINTR;
1130                                 break;
1131                         }
1132                 }
1133                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1134
1135                 if (ret == 0) {
1136                         int msgbufcount = args.msgbufcount;
1137                         int remove = instance->completion_remove;
1138
1139                         for (ret = 0; ret < args.count; ret++) {
1140                                 struct vchiq_completion_data *completion;
1141                                 struct vchiq_service *service;
1142                                 struct user_service *user_service;
1143                                 struct vchiq_header *header;
1144
1145                                 if (remove == instance->completion_insert)
1146                                         break;
1147
1148                                 completion = &instance->completions[
1149                                         remove & (MAX_COMPLETIONS - 1)];
1150
1151                                 /*
1152                                  * A read memory barrier is needed to stop
1153                                  * prefetch of a stale completion record
1154                                  */
1155                                 rmb();
1156
1157                                 service = completion->service_userdata;
1158                                 user_service = service->base.userdata;
1159                                 completion->service_userdata =
1160                                         user_service->userdata;
1161
1162                                 header = completion->header;
1163                                 if (header) {
1164                                         void __user *msgbuf;
1165                                         int msglen;
1166
1167                                         msglen = header->size +
1168                                                 sizeof(struct vchiq_header);
1169                                         /* This must be a VCHIQ-style service */
1170                                         if (args.msgbufsize < msglen) {
1171                                                 vchiq_log_error(
1172                                                         vchiq_arm_log_level,
1173                                                         "header %pK: msgbufsize %x < msglen %x",
1174                                                         header, args.msgbufsize,
1175                                                         msglen);
1176                                                 WARN(1, "invalid message "
1177                                                         "size\n");
1178                                                 if (ret == 0)
1179                                                         ret = -EMSGSIZE;
1180                                                 break;
1181                                         }
1182                                         if (msgbufcount <= 0)
1183                                                 /* Stall here for lack of a
1184                                                 ** buffer for the message. */
1185                                                 break;
1186                                         /* Get the pointer from user space */
1187                                         msgbufcount--;
1188                                         if (copy_from_user(&msgbuf,
1189                                                 (const void __user *)
1190                                                 &args.msgbufs[msgbufcount],
1191                                                 sizeof(msgbuf))) {
1192                                                 if (ret == 0)
1193                                                         ret = -EFAULT;
1194                                                 break;
1195                                         }
1196
1197                                         /* Copy the message to user space */
1198                                         if (copy_to_user(msgbuf, header,
1199                                                 msglen)) {
1200                                                 if (ret == 0)
1201                                                         ret = -EFAULT;
1202                                                 break;
1203                                         }
1204
1205                                         /* Now it has been copied, the message
1206                                         ** can be released. */
1207                                         vchiq_release_message(service->handle,
1208                                                 header);
1209
1210                                         /* The completion must point to the
1211                                         ** msgbuf. */
1212                                         completion->header = msgbuf;
1213                                 }
1214
1215                                 if ((completion->reason ==
1216                                         VCHIQ_SERVICE_CLOSED) &&
1217                                         !instance->use_close_delivered)
1218                                         unlock_service(service);
1219
1220                                 if (copy_to_user((void __user *)(
1221                                         (size_t)args.buf + ret *
1222                                         sizeof(struct vchiq_completion_data)),
1223                                         completion,
1224                                         sizeof(struct vchiq_completion_data))) {
1225                                                 if (ret == 0)
1226                                                         ret = -EFAULT;
1227                                         break;
1228                                 }
1229
1230                                 /*
1231                                  * Ensure that the above copy has completed
1232                                  * before advancing the remove pointer.
1233                                  */
1234                                 mb();
1235                                 remove++;
1236                                 instance->completion_remove = remove;
1237                         }
1238
1239                         if (msgbufcount != args.msgbufcount) {
1240                                 if (copy_to_user((void __user *)
1241                                         &((struct vchiq_await_completion *)arg)
1242                                                 ->msgbufcount,
1243                                         &msgbufcount,
1244                                         sizeof(msgbufcount))) {
1245                                         ret = -EFAULT;
1246                                 }
1247                         }
1248                 }
1249
1250                 if (ret)
1251                         complete(&instance->remove_event);
1252                 mutex_unlock(&instance->completion_mutex);
1253                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1254         } break;
1255
1256         case VCHIQ_IOC_DEQUEUE_MESSAGE: {
1257                 struct vchiq_dequeue_message args;
1258                 struct user_service *user_service;
1259                 struct vchiq_header *header;
1260
1261                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1262                 if (copy_from_user(&args, (const void __user *)arg,
1263                                    sizeof(args))) {
1264                         ret = -EFAULT;
1265                         break;
1266                 }
1267                 service = find_service_for_instance(instance, args.handle);
1268                 if (!service) {
1269                         ret = -EINVAL;
1270                         break;
1271                 }
1272                 user_service = (struct user_service *)service->base.userdata;
1273                 if (user_service->is_vchi == 0) {
1274                         ret = -EINVAL;
1275                         break;
1276                 }
1277
1278                 spin_lock(&msg_queue_spinlock);
1279                 if (user_service->msg_remove == user_service->msg_insert) {
1280                         if (!args.blocking) {
1281                                 spin_unlock(&msg_queue_spinlock);
1282                                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1283                                 ret = -EWOULDBLOCK;
1284                                 break;
1285                         }
1286                         user_service->dequeue_pending = 1;
1287                         do {
1288                                 spin_unlock(&msg_queue_spinlock);
1289                                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1290                                 if (wait_for_completion_interruptible(
1291                                         &user_service->insert_event)) {
1292                                         vchiq_log_info(vchiq_arm_log_level,
1293                                                 "DEQUEUE_MESSAGE interrupted");
1294                                         ret = -EINTR;
1295                                         break;
1296                                 }
1297                                 spin_lock(&msg_queue_spinlock);
1298                         } while (user_service->msg_remove ==
1299                                 user_service->msg_insert);
1300
1301                         if (ret)
1302                                 break;
1303                 }
1304
1305                 BUG_ON((int)(user_service->msg_insert -
1306                         user_service->msg_remove) < 0);
1307
1308                 header = user_service->msg_queue[user_service->msg_remove &
1309                         (MSG_QUEUE_SIZE - 1)];
1310                 user_service->msg_remove++;
1311                 spin_unlock(&msg_queue_spinlock);
1312
1313                 complete(&user_service->remove_event);
1314                 if (!header)
1315                         ret = -ENOTCONN;
1316                 else if (header->size <= args.bufsize) {
1317                         /* Copy to user space if msgbuf is not NULL */
1318                         if (!args.buf ||
1319                                 (copy_to_user((void __user *)args.buf,
1320                                 header->data,
1321                                 header->size) == 0)) {
1322                                 ret = header->size;
1323                                 vchiq_release_message(
1324                                         service->handle,
1325                                         header);
1326                         } else
1327                                 ret = -EFAULT;
1328                 } else {
1329                         vchiq_log_error(vchiq_arm_log_level,
1330                                 "header %pK: bufsize %x < size %x",
1331                                 header, args.bufsize, header->size);
1332                         WARN(1, "invalid size\n");
1333                         ret = -EMSGSIZE;
1334                 }
1335                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1336         } break;
1337
1338         case VCHIQ_IOC_GET_CLIENT_ID: {
1339                 unsigned int handle = (unsigned int)arg;
1340
1341                 ret = vchiq_get_client_id(handle);
1342         } break;
1343
1344         case VCHIQ_IOC_GET_CONFIG: {
1345                 struct vchiq_get_config args;
1346                 struct vchiq_config config;
1347
1348                 if (copy_from_user(&args, (const void __user *)arg,
1349                                    sizeof(args))) {
1350                         ret = -EFAULT;
1351                         break;
1352                 }
1353                 if (args.config_size > sizeof(config)) {
1354                         ret = -EINVAL;
1355                         break;
1356                 }
1357
1358                 vchiq_get_config(&config);
1359                 if (copy_to_user(args.pconfig, &config, args.config_size)) {
1360                         ret = -EFAULT;
1361                         break;
1362                 }
1363         } break;
1364
1365         case VCHIQ_IOC_SET_SERVICE_OPTION: {
1366                 struct vchiq_set_service_option args;
1367
1368                 if (copy_from_user(&args, (const void __user *)arg,
1369                                    sizeof(args))) {
1370                         ret = -EFAULT;
1371                         break;
1372                 }
1373
1374                 service = find_service_for_instance(instance, args.handle);
1375                 if (!service) {
1376                         ret = -EINVAL;
1377                         break;
1378                 }
1379
1380                 status = vchiq_set_service_option(
1381                                 args.handle, args.option, args.value);
1382         } break;
1383
1384         case VCHIQ_IOC_LIB_VERSION: {
1385                 unsigned int lib_version = (unsigned int)arg;
1386
1387                 if (lib_version < VCHIQ_VERSION_MIN)
1388                         ret = -EINVAL;
1389                 else if (lib_version >= VCHIQ_VERSION_CLOSE_DELIVERED)
1390                         instance->use_close_delivered = 1;
1391         } break;
1392
1393         case VCHIQ_IOC_CLOSE_DELIVERED: {
1394                 unsigned int handle = (unsigned int)arg;
1395
1396                 service = find_closed_service_for_instance(instance, handle);
1397                 if (service) {
1398                         struct user_service *user_service =
1399                                 (struct user_service *)service->base.userdata;
1400                         close_delivered(user_service);
1401                 } else
1402                         ret = -EINVAL;
1403         } break;
1404
1405         default:
1406                 ret = -ENOTTY;
1407                 break;
1408         }
1409
1410         if (service)
1411                 unlock_service(service);
1412
1413         if (ret == 0) {
1414                 if (status == VCHIQ_ERROR)
1415                         ret = -EIO;
1416                 else if (status == VCHIQ_RETRY)
1417                         ret = -EINTR;
1418         }
1419
1420         if ((status == VCHIQ_SUCCESS) && (ret < 0) && (ret != -EINTR) &&
1421                 (ret != -EWOULDBLOCK))
1422                 vchiq_log_info(vchiq_arm_log_level,
1423                         "  ioctl instance %pK, cmd %s -> status %d, %ld",
1424                         instance,
1425                         (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
1426                                 ioctl_names[_IOC_NR(cmd)] :
1427                                 "<invalid>",
1428                         status, ret);
1429         else
1430                 vchiq_log_trace(vchiq_arm_log_level,
1431                         "  ioctl instance %pK, cmd %s -> status %d, %ld",
1432                         instance,
1433                         (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
1434                                 ioctl_names[_IOC_NR(cmd)] :
1435                                 "<invalid>",
1436                         status, ret);
1437
1438         return ret;
1439 }
1440
1441 #if defined(CONFIG_COMPAT)
1442
1443 struct vchiq_service_params32 {
1444         int fourcc;
1445         compat_uptr_t callback;
1446         compat_uptr_t userdata;
1447         short version; /* Increment for non-trivial changes */
1448         short version_min; /* Update for incompatible changes */
1449 };
1450
1451 struct vchiq_create_service32 {
1452         struct vchiq_service_params32 params;
1453         int is_open;
1454         int is_vchi;
1455         unsigned int handle; /* OUT */
1456 };
1457
1458 #define VCHIQ_IOC_CREATE_SERVICE32 \
1459         _IOWR(VCHIQ_IOC_MAGIC, 2, struct vchiq_create_service32)
1460
1461 static long
1462 vchiq_compat_ioctl_create_service(
1463         struct file *file,
1464         unsigned int cmd,
1465         unsigned long arg)
1466 {
1467         struct vchiq_create_service __user *args;
1468         struct vchiq_create_service32 __user *ptrargs32 =
1469                 (struct vchiq_create_service32 __user *)arg;
1470         struct vchiq_create_service32 args32;
1471         long ret;
1472
1473         args = compat_alloc_user_space(sizeof(*args));
1474         if (!args)
1475                 return -EFAULT;
1476
1477         if (copy_from_user(&args32, ptrargs32, sizeof(args32)))
1478                 return -EFAULT;
1479
1480         if (put_user(args32.params.fourcc, &args->params.fourcc) ||
1481             put_user(compat_ptr(args32.params.callback),
1482                      &args->params.callback) ||
1483             put_user(compat_ptr(args32.params.userdata),
1484                      &args->params.userdata) ||
1485             put_user(args32.params.version, &args->params.version) ||
1486             put_user(args32.params.version_min,
1487                      &args->params.version_min) ||
1488             put_user(args32.is_open, &args->is_open) ||
1489             put_user(args32.is_vchi, &args->is_vchi) ||
1490             put_user(args32.handle, &args->handle))
1491                 return -EFAULT;
1492
1493         ret = vchiq_ioctl(file, VCHIQ_IOC_CREATE_SERVICE, (unsigned long)args);
1494
1495         if (ret < 0)
1496                 return ret;
1497
1498         if (get_user(args32.handle, &args->handle))
1499                 return -EFAULT;
1500
1501         if (copy_to_user(&ptrargs32->handle,
1502                          &args32.handle,
1503                          sizeof(args32.handle)))
1504                 return -EFAULT;
1505
1506         return 0;
1507 }
1508
1509 struct vchiq_element32 {
1510         compat_uptr_t data;
1511         unsigned int size;
1512 };
1513
1514 struct vchiq_queue_message32 {
1515         unsigned int handle;
1516         unsigned int count;
1517         compat_uptr_t elements;
1518 };
1519
1520 #define VCHIQ_IOC_QUEUE_MESSAGE32 \
1521         _IOW(VCHIQ_IOC_MAGIC,  4, struct vchiq_queue_message32)
1522
1523 static long
1524 vchiq_compat_ioctl_queue_message(struct file *file,
1525                                  unsigned int cmd,
1526                                  unsigned long arg)
1527 {
1528         struct vchiq_queue_message __user *args;
1529         struct vchiq_element __user *elements;
1530         struct vchiq_queue_message32 args32;
1531         unsigned int count;
1532
1533         if (copy_from_user(&args32,
1534                            (struct vchiq_queue_message32 __user *)arg,
1535                            sizeof(args32)))
1536                 return -EFAULT;
1537
1538         args = compat_alloc_user_space(sizeof(*args) +
1539                                        (sizeof(*elements) * MAX_ELEMENTS));
1540
1541         if (!args)
1542                 return -EFAULT;
1543
1544         if (put_user(args32.handle, &args->handle) ||
1545             put_user(args32.count, &args->count) ||
1546             put_user(compat_ptr(args32.elements), &args->elements))
1547                 return -EFAULT;
1548
1549         if (args32.count > MAX_ELEMENTS)
1550                 return -EINVAL;
1551
1552         if (args32.elements && args32.count) {
1553                 struct vchiq_element32 tempelement32[MAX_ELEMENTS];
1554
1555                 elements = (struct vchiq_element __user *)(args + 1);
1556
1557                 if (copy_from_user(&tempelement32,
1558                                    compat_ptr(args32.elements),
1559                                    sizeof(tempelement32)))
1560                         return -EFAULT;
1561
1562                 for (count = 0; count < args32.count; count++) {
1563                         if (put_user(compat_ptr(tempelement32[count].data),
1564                                      &elements[count].data) ||
1565                             put_user(tempelement32[count].size,
1566                                      &elements[count].size))
1567                                 return -EFAULT;
1568                 }
1569
1570                 if (put_user(elements, &args->elements))
1571                         return -EFAULT;
1572         }
1573
1574         return vchiq_ioctl(file, VCHIQ_IOC_QUEUE_MESSAGE, (unsigned long)args);
1575 }
1576
1577 struct vchiq_queue_bulk_transfer32 {
1578         unsigned int handle;
1579         compat_uptr_t data;
1580         unsigned int size;
1581         compat_uptr_t userdata;
1582         enum vchiq_bulk_mode mode;
1583 };
1584
1585 #define VCHIQ_IOC_QUEUE_BULK_TRANSMIT32 \
1586         _IOWR(VCHIQ_IOC_MAGIC, 5, struct vchiq_queue_bulk_transfer32)
1587 #define VCHIQ_IOC_QUEUE_BULK_RECEIVE32 \
1588         _IOWR(VCHIQ_IOC_MAGIC, 6, struct vchiq_queue_bulk_transfer32)
1589
1590 static long
1591 vchiq_compat_ioctl_queue_bulk(struct file *file,
1592                               unsigned int cmd,
1593                               unsigned long arg)
1594 {
1595         struct vchiq_queue_bulk_transfer __user *args;
1596         struct vchiq_queue_bulk_transfer32 args32;
1597         struct vchiq_queue_bulk_transfer32 __user *ptrargs32 =
1598                 (struct vchiq_queue_bulk_transfer32 __user *)arg;
1599         long ret;
1600
1601         args = compat_alloc_user_space(sizeof(*args));
1602         if (!args)
1603                 return -EFAULT;
1604
1605         if (copy_from_user(&args32, ptrargs32, sizeof(args32)))
1606                 return -EFAULT;
1607
1608         if (put_user(args32.handle, &args->handle) ||
1609             put_user(compat_ptr(args32.data), &args->data) ||
1610             put_user(args32.size, &args->size) ||
1611             put_user(compat_ptr(args32.userdata), &args->userdata) ||
1612             put_user(args32.mode, &args->mode))
1613                 return -EFAULT;
1614
1615         if (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT32)
1616                 cmd = VCHIQ_IOC_QUEUE_BULK_TRANSMIT;
1617         else
1618                 cmd = VCHIQ_IOC_QUEUE_BULK_RECEIVE;
1619
1620         ret = vchiq_ioctl(file, cmd, (unsigned long)args);
1621
1622         if (ret < 0)
1623                 return ret;
1624
1625         if (get_user(args32.mode, &args->mode))
1626                 return -EFAULT;
1627
1628         if (copy_to_user(&ptrargs32->mode,
1629                          &args32.mode,
1630                          sizeof(args32.mode)))
1631                 return -EFAULT;
1632
1633         return 0;
1634 }
1635
1636 struct vchiq_completion_data32 {
1637         enum vchiq_reason reason;
1638         compat_uptr_t header;
1639         compat_uptr_t service_userdata;
1640         compat_uptr_t bulk_userdata;
1641 };
1642
1643 struct vchiq_await_completion32 {
1644         unsigned int count;
1645         compat_uptr_t buf;
1646         unsigned int msgbufsize;
1647         unsigned int msgbufcount; /* IN/OUT */
1648         compat_uptr_t msgbufs;
1649 };
1650
1651 #define VCHIQ_IOC_AWAIT_COMPLETION32 \
1652         _IOWR(VCHIQ_IOC_MAGIC, 7, struct vchiq_await_completion32)
1653
1654 static long
1655 vchiq_compat_ioctl_await_completion(struct file *file,
1656                                     unsigned int cmd,
1657                                     unsigned long arg)
1658 {
1659         struct vchiq_await_completion __user *args;
1660         struct vchiq_completion_data __user *completion;
1661         struct vchiq_completion_data completiontemp;
1662         struct vchiq_await_completion32 args32;
1663         struct vchiq_completion_data32 completion32;
1664         unsigned int __user *msgbufcount32;
1665         unsigned int msgbufcount_native;
1666         compat_uptr_t msgbuf32;
1667         void __user *msgbuf;
1668         void * __user *msgbufptr;
1669         long ret;
1670
1671         args = compat_alloc_user_space(sizeof(*args) +
1672                                        sizeof(*completion) +
1673                                        sizeof(*msgbufptr));
1674         if (!args)
1675                 return -EFAULT;
1676
1677         completion = (struct vchiq_completion_data __user *)(args + 1);
1678         msgbufptr = (void * __user *)(completion + 1);
1679
1680         if (copy_from_user(&args32,
1681                            (struct vchiq_completion_data32 __user *)arg,
1682                            sizeof(args32)))
1683                 return -EFAULT;
1684
1685         if (put_user(args32.count, &args->count) ||
1686             put_user(compat_ptr(args32.buf), &args->buf) ||
1687             put_user(args32.msgbufsize, &args->msgbufsize) ||
1688             put_user(args32.msgbufcount, &args->msgbufcount) ||
1689             put_user(compat_ptr(args32.msgbufs), &args->msgbufs))
1690                 return -EFAULT;
1691
1692         /* These are simple cases, so just fall into the native handler */
1693         if (!args32.count || !args32.buf || !args32.msgbufcount)
1694                 return vchiq_ioctl(file,
1695                                    VCHIQ_IOC_AWAIT_COMPLETION,
1696                                    (unsigned long)args);
1697
1698         /*
1699          * These are the more complex cases.  Typical applications of this
1700          * ioctl will use a very large count, with a very large msgbufcount.
1701          * Since the native ioctl can asynchronously fill in the returned
1702          * buffers and the application can in theory begin processing messages
1703          * even before the ioctl returns, a bit of a trick is used here.
1704          *
1705          * By forcing both count and msgbufcount to be 1, it forces the native
1706          * ioctl to only claim at most 1 message is available.   This tricks
1707          * the calling application into thinking only 1 message was actually
1708          * available in the queue so like all good applications it will retry
1709          * waiting until all the required messages are received.
1710          *
1711          * This trick has been tested and proven to work with vchiq_test,
1712          * Minecraft_PI, the "hello pi" examples, and various other
1713          * applications that are included in Raspbian.
1714          */
1715
1716         if (copy_from_user(&msgbuf32,
1717                            compat_ptr(args32.msgbufs) +
1718                            (sizeof(compat_uptr_t) *
1719                            (args32.msgbufcount - 1)),
1720                            sizeof(msgbuf32)))
1721                 return -EFAULT;
1722
1723         msgbuf = compat_ptr(msgbuf32);
1724
1725         if (copy_to_user(msgbufptr,
1726                          &msgbuf,
1727                          sizeof(msgbuf)))
1728                 return -EFAULT;
1729
1730         if (copy_to_user(&args->msgbufs,
1731                          &msgbufptr,
1732                          sizeof(msgbufptr)))
1733                 return -EFAULT;
1734
1735         if (put_user(1U, &args->count) ||
1736             put_user(completion, &args->buf) ||
1737             put_user(1U, &args->msgbufcount))
1738                 return -EFAULT;
1739
1740         ret = vchiq_ioctl(file,
1741                           VCHIQ_IOC_AWAIT_COMPLETION,
1742                           (unsigned long)args);
1743
1744         /*
1745          * An return value of 0 here means that no messages where available
1746          * in the message queue.  In this case the native ioctl does not
1747          * return any data to the application at all.  Not even to update
1748          * msgbufcount.  This functionality needs to be kept here for
1749          * compatibility.
1750          *
1751          * Of course, < 0 means that an error occurred and no data is being
1752          * returned.
1753          *
1754          * Since count and msgbufcount was forced to 1, that means
1755          * the only other possible return value is 1. Meaning that 1 message
1756          * was available, so that multiple message case does not need to be
1757          * handled here.
1758          */
1759         if (ret <= 0)
1760                 return ret;
1761
1762         if (copy_from_user(&completiontemp, completion, sizeof(*completion)))
1763                 return -EFAULT;
1764
1765         completion32.reason = completiontemp.reason;
1766         completion32.header = ptr_to_compat(completiontemp.header);
1767         completion32.service_userdata =
1768                 ptr_to_compat(completiontemp.service_userdata);
1769         completion32.bulk_userdata =
1770                 ptr_to_compat(completiontemp.bulk_userdata);
1771
1772         if (copy_to_user(compat_ptr(args32.buf),
1773                          &completion32,
1774                          sizeof(completion32)))
1775                 return -EFAULT;
1776
1777         if (get_user(msgbufcount_native, &args->msgbufcount))
1778                 return -EFAULT;
1779
1780         if (!msgbufcount_native)
1781                 args32.msgbufcount--;
1782
1783         msgbufcount32 =
1784                 &((struct vchiq_await_completion32 __user *)arg)->msgbufcount;
1785
1786         if (copy_to_user(msgbufcount32,
1787                          &args32.msgbufcount,
1788                          sizeof(args32.msgbufcount)))
1789                 return -EFAULT;
1790
1791         return 1;
1792 }
1793
1794 struct vchiq_dequeue_message32 {
1795         unsigned int handle;
1796         int blocking;
1797         unsigned int bufsize;
1798         compat_uptr_t buf;
1799 };
1800
1801 #define VCHIQ_IOC_DEQUEUE_MESSAGE32 \
1802         _IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message32)
1803
1804 static long
1805 vchiq_compat_ioctl_dequeue_message(struct file *file,
1806                                    unsigned int cmd,
1807                                    unsigned long arg)
1808 {
1809         struct vchiq_dequeue_message __user *args;
1810         struct vchiq_dequeue_message32 args32;
1811
1812         args = compat_alloc_user_space(sizeof(*args));
1813         if (!args)
1814                 return -EFAULT;
1815
1816         if (copy_from_user(&args32,
1817                            (struct vchiq_dequeue_message32 __user *)arg,
1818                            sizeof(args32)))
1819                 return -EFAULT;
1820
1821         if (put_user(args32.handle, &args->handle) ||
1822             put_user(args32.blocking, &args->blocking) ||
1823             put_user(args32.bufsize, &args->bufsize) ||
1824             put_user(compat_ptr(args32.buf), &args->buf))
1825                 return -EFAULT;
1826
1827         return vchiq_ioctl(file, VCHIQ_IOC_DEQUEUE_MESSAGE,
1828                            (unsigned long)args);
1829 }
1830
1831 struct vchiq_get_config32 {
1832         unsigned int config_size;
1833         compat_uptr_t pconfig;
1834 };
1835
1836 #define VCHIQ_IOC_GET_CONFIG32 \
1837         _IOWR(VCHIQ_IOC_MAGIC, 10, struct vchiq_get_config32)
1838
1839 static long
1840 vchiq_compat_ioctl_get_config(struct file *file,
1841                               unsigned int cmd,
1842                               unsigned long arg)
1843 {
1844         struct vchiq_get_config __user *args;
1845         struct vchiq_get_config32 args32;
1846
1847         args = compat_alloc_user_space(sizeof(*args));
1848         if (!args)
1849                 return -EFAULT;
1850
1851         if (copy_from_user(&args32,
1852                            (struct vchiq_get_config32 __user *)arg,
1853                            sizeof(args32)))
1854                 return -EFAULT;
1855
1856         if (put_user(args32.config_size, &args->config_size) ||
1857             put_user(compat_ptr(args32.pconfig), &args->pconfig))
1858                 return -EFAULT;
1859
1860         return vchiq_ioctl(file, VCHIQ_IOC_GET_CONFIG, (unsigned long)args);
1861 }
1862
1863 static long
1864 vchiq_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
1865 {
1866         switch (cmd) {
1867         case VCHIQ_IOC_CREATE_SERVICE32:
1868                 return vchiq_compat_ioctl_create_service(file, cmd, arg);
1869         case VCHIQ_IOC_QUEUE_MESSAGE32:
1870                 return vchiq_compat_ioctl_queue_message(file, cmd, arg);
1871         case VCHIQ_IOC_QUEUE_BULK_TRANSMIT32:
1872         case VCHIQ_IOC_QUEUE_BULK_RECEIVE32:
1873                 return vchiq_compat_ioctl_queue_bulk(file, cmd, arg);
1874         case VCHIQ_IOC_AWAIT_COMPLETION32:
1875                 return vchiq_compat_ioctl_await_completion(file, cmd, arg);
1876         case VCHIQ_IOC_DEQUEUE_MESSAGE32:
1877                 return vchiq_compat_ioctl_dequeue_message(file, cmd, arg);
1878         case VCHIQ_IOC_GET_CONFIG32:
1879                 return vchiq_compat_ioctl_get_config(file, cmd, arg);
1880         default:
1881                 return vchiq_ioctl(file, cmd, arg);
1882         }
1883 }
1884
1885 #endif
1886
1887 static int vchiq_open(struct inode *inode, struct file *file)
1888 {
1889         struct vchiq_state *state = vchiq_get_state();
1890         struct vchiq_instance *instance;
1891
1892         vchiq_log_info(vchiq_arm_log_level, "vchiq_open");
1893
1894         if (!state) {
1895                 vchiq_log_error(vchiq_arm_log_level,
1896                                 "vchiq has no connection to VideoCore");
1897                 return -ENOTCONN;
1898         }
1899
1900         instance = kzalloc(sizeof(*instance), GFP_KERNEL);
1901         if (!instance)
1902                 return -ENOMEM;
1903
1904         instance->state = state;
1905         instance->pid = current->tgid;
1906
1907         vchiq_debugfs_add_instance(instance);
1908
1909         init_completion(&instance->insert_event);
1910         init_completion(&instance->remove_event);
1911         mutex_init(&instance->completion_mutex);
1912         mutex_init(&instance->bulk_waiter_list_mutex);
1913         INIT_LIST_HEAD(&instance->bulk_waiter_list);
1914
1915         file->private_data = instance;
1916
1917         return 0;
1918 }
1919
1920 static int vchiq_release(struct inode *inode, struct file *file)
1921 {
1922         struct vchiq_instance *instance = file->private_data;
1923         struct vchiq_state *state = vchiq_get_state();
1924         struct vchiq_service *service;
1925         int ret = 0;
1926         int i;
1927
1928         vchiq_log_info(vchiq_arm_log_level, "%s: instance=%lx", __func__,
1929                        (unsigned long)instance);
1930
1931         if (!state) {
1932                 ret = -EPERM;
1933                 goto out;
1934         }
1935
1936         /* Ensure videocore is awake to allow termination. */
1937         vchiq_use_internal(instance->state, NULL, USE_TYPE_VCHIQ);
1938
1939         mutex_lock(&instance->completion_mutex);
1940
1941         /* Wake the completion thread and ask it to exit */
1942         instance->closing = 1;
1943         complete(&instance->insert_event);
1944
1945         mutex_unlock(&instance->completion_mutex);
1946
1947         /* Wake the slot handler if the completion queue is full. */
1948         complete(&instance->remove_event);
1949
1950         /* Mark all services for termination... */
1951         i = 0;
1952         while ((service = next_service_by_instance(state, instance, &i))) {
1953                 struct user_service *user_service = service->base.userdata;
1954
1955                 /* Wake the slot handler if the msg queue is full. */
1956                 complete(&user_service->remove_event);
1957
1958                 vchiq_terminate_service_internal(service);
1959                 unlock_service(service);
1960         }
1961
1962         /* ...and wait for them to die */
1963         i = 0;
1964         while ((service = next_service_by_instance(state, instance, &i))) {
1965                 struct user_service *user_service = service->base.userdata;
1966
1967                 wait_for_completion(&service->remove_event);
1968
1969                 BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
1970
1971                 spin_lock(&msg_queue_spinlock);
1972
1973                 while (user_service->msg_remove != user_service->msg_insert) {
1974                         struct vchiq_header *header;
1975                         int m = user_service->msg_remove & (MSG_QUEUE_SIZE - 1);
1976
1977                         header = user_service->msg_queue[m];
1978                         user_service->msg_remove++;
1979                         spin_unlock(&msg_queue_spinlock);
1980
1981                         if (header)
1982                                 vchiq_release_message(service->handle, header);
1983                         spin_lock(&msg_queue_spinlock);
1984                 }
1985
1986                 spin_unlock(&msg_queue_spinlock);
1987
1988                 unlock_service(service);
1989         }
1990
1991         /* Release any closed services */
1992         while (instance->completion_remove !=
1993                 instance->completion_insert) {
1994                 struct vchiq_completion_data *completion;
1995                 struct vchiq_service *service;
1996
1997                 completion = &instance->completions[
1998                         instance->completion_remove & (MAX_COMPLETIONS - 1)];
1999                 service = completion->service_userdata;
2000                 if (completion->reason == VCHIQ_SERVICE_CLOSED) {
2001                         struct user_service *user_service =
2002                                                         service->base.userdata;
2003
2004                         /* Wake any blocked user-thread */
2005                         if (instance->use_close_delivered)
2006                                 complete(&user_service->close_event);
2007                         unlock_service(service);
2008                 }
2009                 instance->completion_remove++;
2010         }
2011
2012         /* Release the PEER service count. */
2013         vchiq_release_internal(instance->state, NULL);
2014
2015         {
2016                 struct bulk_waiter_node *waiter, *next;
2017
2018                 list_for_each_entry_safe(waiter, next,
2019                                          &instance->bulk_waiter_list, list) {
2020                         list_del(&waiter->list);
2021                         vchiq_log_info(vchiq_arm_log_level,
2022                                 "bulk_waiter - cleaned up %pK for pid %d",
2023                                 waiter, waiter->pid);
2024                         kfree(waiter);
2025                 }
2026         }
2027
2028         vchiq_debugfs_remove_instance(instance);
2029
2030         kfree(instance);
2031         file->private_data = NULL;
2032
2033 out:
2034         return ret;
2035 }
2036
2037 /****************************************************************************
2038 *
2039 *   vchiq_dump
2040 *
2041 ***************************************************************************/
2042
2043 int vchiq_dump(void *dump_context, const char *str, int len)
2044 {
2045         struct dump_context *context = (struct dump_context *)dump_context;
2046         int copy_bytes;
2047
2048         if (context->actual >= context->space)
2049                 return 0;
2050
2051         if (context->offset > 0) {
2052                 int skip_bytes = min_t(int, len, context->offset);
2053
2054                 str += skip_bytes;
2055                 len -= skip_bytes;
2056                 context->offset -= skip_bytes;
2057                 if (context->offset > 0)
2058                         return 0;
2059         }
2060         copy_bytes = min_t(int, len, context->space - context->actual);
2061         if (copy_bytes == 0)
2062                 return 0;
2063         if (copy_to_user(context->buf + context->actual, str,
2064                          copy_bytes))
2065                 return -EFAULT;
2066         context->actual += copy_bytes;
2067         len -= copy_bytes;
2068
2069         /*
2070          * If the terminating NUL is included in the length, then it
2071          * marks the end of a line and should be replaced with a
2072          * carriage return.
2073          */
2074         if ((len == 0) && (str[copy_bytes - 1] == '\0')) {
2075                 char cr = '\n';
2076
2077                 if (copy_to_user(context->buf + context->actual - 1,
2078                                  &cr, 1))
2079                         return -EFAULT;
2080         }
2081         return 0;
2082 }
2083
2084 /****************************************************************************
2085 *
2086 *   vchiq_dump_platform_instance_state
2087 *
2088 ***************************************************************************/
2089
2090 int vchiq_dump_platform_instances(void *dump_context)
2091 {
2092         struct vchiq_state *state = vchiq_get_state();
2093         char buf[80];
2094         int len;
2095         int i;
2096
2097         /* There is no list of instances, so instead scan all services,
2098                 marking those that have been dumped. */
2099
2100         rcu_read_lock();
2101         for (i = 0; i < state->unused_service; i++) {
2102                 struct vchiq_service *service;
2103                 struct vchiq_instance *instance;
2104
2105                 service = rcu_dereference(state->services[i]);
2106                 if (!service || service->base.callback != service_callback)
2107                         continue;
2108
2109                 instance = service->instance;
2110                 if (instance)
2111                         instance->mark = 0;
2112         }
2113         rcu_read_unlock();
2114
2115         for (i = 0; i < state->unused_service; i++) {
2116                 struct vchiq_service *service;
2117                 struct vchiq_instance *instance;
2118                 int err;
2119
2120                 rcu_read_lock();
2121                 service = rcu_dereference(state->services[i]);
2122                 if (!service || service->base.callback != service_callback) {
2123                         rcu_read_unlock();
2124                         continue;
2125                 }
2126
2127                 instance = service->instance;
2128                 if (!instance || instance->mark) {
2129                         rcu_read_unlock();
2130                         continue;
2131                 }
2132                 rcu_read_unlock();
2133
2134                 len = snprintf(buf, sizeof(buf),
2135                                "Instance %pK: pid %d,%s completions %d/%d",
2136                                instance, instance->pid,
2137                                instance->connected ? " connected, " :
2138                                "",
2139                                instance->completion_insert -
2140                                instance->completion_remove,
2141                                MAX_COMPLETIONS);
2142                 err = vchiq_dump(dump_context, buf, len + 1);
2143                 if (err)
2144                         return err;
2145                 instance->mark = 1;
2146         }
2147         return 0;
2148 }
2149
2150 /****************************************************************************
2151 *
2152 *   vchiq_dump_platform_service_state
2153 *
2154 ***************************************************************************/
2155
2156 int vchiq_dump_platform_service_state(void *dump_context,
2157                                       struct vchiq_service *service)
2158 {
2159         struct user_service *user_service =
2160                         (struct user_service *)service->base.userdata;
2161         char buf[80];
2162         int len;
2163
2164         len = scnprintf(buf, sizeof(buf), "  instance %pK", service->instance);
2165
2166         if ((service->base.callback == service_callback) &&
2167                 user_service->is_vchi) {
2168                 len += scnprintf(buf + len, sizeof(buf) - len,
2169                         ", %d/%d messages",
2170                         user_service->msg_insert - user_service->msg_remove,
2171                         MSG_QUEUE_SIZE);
2172
2173                 if (user_service->dequeue_pending)
2174                         len += scnprintf(buf + len, sizeof(buf) - len,
2175                                 " (dequeue pending)");
2176         }
2177
2178         return vchiq_dump(dump_context, buf, len + 1);
2179 }
2180
2181 /****************************************************************************
2182 *
2183 *   vchiq_read
2184 *
2185 ***************************************************************************/
2186
2187 static ssize_t
2188 vchiq_read(struct file *file, char __user *buf,
2189         size_t count, loff_t *ppos)
2190 {
2191         struct dump_context context;
2192         int err;
2193
2194         context.buf = buf;
2195         context.actual = 0;
2196         context.space = count;
2197         context.offset = *ppos;
2198
2199         err = vchiq_dump_state(&context, &g_state);
2200         if (err)
2201                 return err;
2202
2203         *ppos += context.actual;
2204
2205         return context.actual;
2206 }
2207
2208 struct vchiq_state *
2209 vchiq_get_state(void)
2210 {
2211
2212         if (!g_state.remote)
2213                 printk(KERN_ERR "%s: g_state.remote == NULL\n", __func__);
2214         else if (g_state.remote->initialised != 1)
2215                 printk(KERN_NOTICE "%s: g_state.remote->initialised != 1 (%d)\n",
2216                         __func__, g_state.remote->initialised);
2217
2218         return (g_state.remote &&
2219                 (g_state.remote->initialised == 1)) ? &g_state : NULL;
2220 }
2221
2222 static const struct file_operations
2223 vchiq_fops = {
2224         .owner = THIS_MODULE,
2225         .unlocked_ioctl = vchiq_ioctl,
2226 #if defined(CONFIG_COMPAT)
2227         .compat_ioctl = vchiq_compat_ioctl,
2228 #endif
2229         .open = vchiq_open,
2230         .release = vchiq_release,
2231         .read = vchiq_read
2232 };
2233
2234 /*
2235  * Autosuspend related functionality
2236  */
2237
2238 static enum vchiq_status
2239 vchiq_keepalive_vchiq_callback(enum vchiq_reason reason,
2240         struct vchiq_header *header,
2241         unsigned int service_user,
2242         void *bulk_user)
2243 {
2244         vchiq_log_error(vchiq_susp_log_level,
2245                 "%s callback reason %d", __func__, reason);
2246         return 0;
2247 }
2248
2249 static int
2250 vchiq_keepalive_thread_func(void *v)
2251 {
2252         struct vchiq_state *state = (struct vchiq_state *)v;
2253         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2254
2255         enum vchiq_status status;
2256         struct vchiq_instance *instance;
2257         unsigned int ka_handle;
2258
2259         struct vchiq_service_params params = {
2260                 .fourcc      = VCHIQ_MAKE_FOURCC('K', 'E', 'E', 'P'),
2261                 .callback    = vchiq_keepalive_vchiq_callback,
2262                 .version     = KEEPALIVE_VER,
2263                 .version_min = KEEPALIVE_VER_MIN
2264         };
2265
2266         status = vchiq_initialise(&instance);
2267         if (status != VCHIQ_SUCCESS) {
2268                 vchiq_log_error(vchiq_susp_log_level,
2269                         "%s vchiq_initialise failed %d", __func__, status);
2270                 goto exit;
2271         }
2272
2273         status = vchiq_connect(instance);
2274         if (status != VCHIQ_SUCCESS) {
2275                 vchiq_log_error(vchiq_susp_log_level,
2276                         "%s vchiq_connect failed %d", __func__, status);
2277                 goto shutdown;
2278         }
2279
2280         status = vchiq_add_service(instance, &params, &ka_handle);
2281         if (status != VCHIQ_SUCCESS) {
2282                 vchiq_log_error(vchiq_susp_log_level,
2283                         "%s vchiq_open_service failed %d", __func__, status);
2284                 goto shutdown;
2285         }
2286
2287         while (1) {
2288                 long rc = 0, uc = 0;
2289
2290                 if (wait_for_completion_interruptible(&arm_state->ka_evt)) {
2291                         vchiq_log_error(vchiq_susp_log_level,
2292                                 "%s interrupted", __func__);
2293                         flush_signals(current);
2294                         continue;
2295                 }
2296
2297                 /* read and clear counters.  Do release_count then use_count to
2298                  * prevent getting more releases than uses */
2299                 rc = atomic_xchg(&arm_state->ka_release_count, 0);
2300                 uc = atomic_xchg(&arm_state->ka_use_count, 0);
2301
2302                 /* Call use/release service the requisite number of times.
2303                  * Process use before release so use counts don't go negative */
2304                 while (uc--) {
2305                         atomic_inc(&arm_state->ka_use_ack_count);
2306                         status = vchiq_use_service(ka_handle);
2307                         if (status != VCHIQ_SUCCESS) {
2308                                 vchiq_log_error(vchiq_susp_log_level,
2309                                         "%s vchiq_use_service error %d",
2310                                         __func__, status);
2311                         }
2312                 }
2313                 while (rc--) {
2314                         status = vchiq_release_service(ka_handle);
2315                         if (status != VCHIQ_SUCCESS) {
2316                                 vchiq_log_error(vchiq_susp_log_level,
2317                                         "%s vchiq_release_service error %d",
2318                                         __func__, status);
2319                         }
2320                 }
2321         }
2322
2323 shutdown:
2324         vchiq_shutdown(instance);
2325 exit:
2326         return 0;
2327 }
2328
2329 enum vchiq_status
2330 vchiq_arm_init_state(struct vchiq_state *state,
2331                      struct vchiq_arm_state *arm_state)
2332 {
2333         if (arm_state) {
2334                 rwlock_init(&arm_state->susp_res_lock);
2335
2336                 init_completion(&arm_state->ka_evt);
2337                 atomic_set(&arm_state->ka_use_count, 0);
2338                 atomic_set(&arm_state->ka_use_ack_count, 0);
2339                 atomic_set(&arm_state->ka_release_count, 0);
2340
2341                 arm_state->state = state;
2342                 arm_state->first_connect = 0;
2343
2344         }
2345         return VCHIQ_SUCCESS;
2346 }
2347
2348 enum vchiq_status
2349 vchiq_use_internal(struct vchiq_state *state, struct vchiq_service *service,
2350                    enum USE_TYPE_E use_type)
2351 {
2352         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2353         enum vchiq_status ret = VCHIQ_SUCCESS;
2354         char entity[16];
2355         int *entity_uc;
2356         int local_uc, local_entity_uc;
2357
2358         if (!arm_state)
2359                 goto out;
2360
2361         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2362
2363         if (use_type == USE_TYPE_VCHIQ) {
2364                 sprintf(entity, "VCHIQ:   ");
2365                 entity_uc = &arm_state->peer_use_count;
2366         } else if (service) {
2367                 sprintf(entity, "%c%c%c%c:%03d",
2368                         VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2369                         service->client_id);
2370                 entity_uc = &service->service_use_count;
2371         } else {
2372                 vchiq_log_error(vchiq_susp_log_level, "%s null service "
2373                                 "ptr", __func__);
2374                 ret = VCHIQ_ERROR;
2375                 goto out;
2376         }
2377
2378         write_lock_bh(&arm_state->susp_res_lock);
2379         local_uc = ++arm_state->videocore_use_count;
2380         local_entity_uc = ++(*entity_uc);
2381
2382         vchiq_log_trace(vchiq_susp_log_level,
2383                 "%s %s count %d, state count %d",
2384                 __func__, entity, *entity_uc, local_uc);
2385
2386         write_unlock_bh(&arm_state->susp_res_lock);
2387
2388         if (ret == VCHIQ_SUCCESS) {
2389                 enum vchiq_status status = VCHIQ_SUCCESS;
2390                 long ack_cnt = atomic_xchg(&arm_state->ka_use_ack_count, 0);
2391
2392                 while (ack_cnt && (status == VCHIQ_SUCCESS)) {
2393                         /* Send the use notify to videocore */
2394                         status = vchiq_send_remote_use_active(state);
2395                         if (status == VCHIQ_SUCCESS)
2396                                 ack_cnt--;
2397                         else
2398                                 atomic_add(ack_cnt,
2399                                         &arm_state->ka_use_ack_count);
2400                 }
2401         }
2402
2403 out:
2404         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
2405         return ret;
2406 }
2407
2408 enum vchiq_status
2409 vchiq_release_internal(struct vchiq_state *state, struct vchiq_service *service)
2410 {
2411         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2412         enum vchiq_status ret = VCHIQ_SUCCESS;
2413         char entity[16];
2414         int *entity_uc;
2415
2416         if (!arm_state)
2417                 goto out;
2418
2419         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2420
2421         if (service) {
2422                 sprintf(entity, "%c%c%c%c:%03d",
2423                         VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2424                         service->client_id);
2425                 entity_uc = &service->service_use_count;
2426         } else {
2427                 sprintf(entity, "PEER:   ");
2428                 entity_uc = &arm_state->peer_use_count;
2429         }
2430
2431         write_lock_bh(&arm_state->susp_res_lock);
2432         if (!arm_state->videocore_use_count || !(*entity_uc)) {
2433                 /* Don't use BUG_ON - don't allow user thread to crash kernel */
2434                 WARN_ON(!arm_state->videocore_use_count);
2435                 WARN_ON(!(*entity_uc));
2436                 ret = VCHIQ_ERROR;
2437                 goto unlock;
2438         }
2439         --arm_state->videocore_use_count;
2440         --(*entity_uc);
2441
2442         vchiq_log_trace(vchiq_susp_log_level,
2443                 "%s %s count %d, state count %d",
2444                 __func__, entity, *entity_uc,
2445                 arm_state->videocore_use_count);
2446
2447 unlock:
2448         write_unlock_bh(&arm_state->susp_res_lock);
2449
2450 out:
2451         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
2452         return ret;
2453 }
2454
2455 void
2456 vchiq_on_remote_use(struct vchiq_state *state)
2457 {
2458         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2459
2460         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2461         atomic_inc(&arm_state->ka_use_count);
2462         complete(&arm_state->ka_evt);
2463 }
2464
2465 void
2466 vchiq_on_remote_release(struct vchiq_state *state)
2467 {
2468         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2469
2470         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2471         atomic_inc(&arm_state->ka_release_count);
2472         complete(&arm_state->ka_evt);
2473 }
2474
2475 enum vchiq_status
2476 vchiq_use_service_internal(struct vchiq_service *service)
2477 {
2478         return vchiq_use_internal(service->state, service, USE_TYPE_SERVICE);
2479 }
2480
2481 enum vchiq_status
2482 vchiq_release_service_internal(struct vchiq_service *service)
2483 {
2484         return vchiq_release_internal(service->state, service);
2485 }
2486
2487 struct vchiq_debugfs_node *
2488 vchiq_instance_get_debugfs_node(struct vchiq_instance *instance)
2489 {
2490         return &instance->debugfs_node;
2491 }
2492
2493 int
2494 vchiq_instance_get_use_count(struct vchiq_instance *instance)
2495 {
2496         struct vchiq_service *service;
2497         int use_count = 0, i;
2498
2499         i = 0;
2500         rcu_read_lock();
2501         while ((service = __next_service_by_instance(instance->state,
2502                                                      instance, &i)))
2503                 use_count += service->service_use_count;
2504         rcu_read_unlock();
2505         return use_count;
2506 }
2507
2508 int
2509 vchiq_instance_get_pid(struct vchiq_instance *instance)
2510 {
2511         return instance->pid;
2512 }
2513
2514 int
2515 vchiq_instance_get_trace(struct vchiq_instance *instance)
2516 {
2517         return instance->trace;
2518 }
2519
2520 void
2521 vchiq_instance_set_trace(struct vchiq_instance *instance, int trace)
2522 {
2523         struct vchiq_service *service;
2524         int i;
2525
2526         i = 0;
2527         rcu_read_lock();
2528         while ((service = __next_service_by_instance(instance->state,
2529                                                      instance, &i)))
2530                 service->trace = trace;
2531         rcu_read_unlock();
2532         instance->trace = (trace != 0);
2533 }
2534
2535 enum vchiq_status
2536 vchiq_use_service(unsigned int handle)
2537 {
2538         enum vchiq_status ret = VCHIQ_ERROR;
2539         struct vchiq_service *service = find_service_by_handle(handle);
2540
2541         if (service) {
2542                 ret = vchiq_use_internal(service->state, service,
2543                                 USE_TYPE_SERVICE);
2544                 unlock_service(service);
2545         }
2546         return ret;
2547 }
2548
2549 enum vchiq_status
2550 vchiq_release_service(unsigned int handle)
2551 {
2552         enum vchiq_status ret = VCHIQ_ERROR;
2553         struct vchiq_service *service = find_service_by_handle(handle);
2554
2555         if (service) {
2556                 ret = vchiq_release_internal(service->state, service);
2557                 unlock_service(service);
2558         }
2559         return ret;
2560 }
2561
2562 struct service_data_struct {
2563         int fourcc;
2564         int clientid;
2565         int use_count;
2566 };
2567
2568 void
2569 vchiq_dump_service_use_state(struct vchiq_state *state)
2570 {
2571         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2572         struct service_data_struct *service_data;
2573         int i, found = 0;
2574         /* If there's more than 64 services, only dump ones with
2575          * non-zero counts */
2576         int only_nonzero = 0;
2577         static const char *nz = "<-- preventing suspend";
2578
2579         int peer_count;
2580         int vc_use_count;
2581         int active_services;
2582
2583         if (!arm_state)
2584                 return;
2585
2586         service_data = kmalloc_array(MAX_SERVICES, sizeof(*service_data),
2587                                      GFP_KERNEL);
2588         if (!service_data)
2589                 return;
2590
2591         read_lock_bh(&arm_state->susp_res_lock);
2592         peer_count = arm_state->peer_use_count;
2593         vc_use_count = arm_state->videocore_use_count;
2594         active_services = state->unused_service;
2595         if (active_services > MAX_SERVICES)
2596                 only_nonzero = 1;
2597
2598         rcu_read_lock();
2599         for (i = 0; i < active_services; i++) {
2600                 struct vchiq_service *service_ptr =
2601                         rcu_dereference(state->services[i]);
2602
2603                 if (!service_ptr)
2604                         continue;
2605
2606                 if (only_nonzero && !service_ptr->service_use_count)
2607                         continue;
2608
2609                 if (service_ptr->srvstate == VCHIQ_SRVSTATE_FREE)
2610                         continue;
2611
2612                 service_data[found].fourcc = service_ptr->base.fourcc;
2613                 service_data[found].clientid = service_ptr->client_id;
2614                 service_data[found].use_count = service_ptr->service_use_count;
2615                 found++;
2616                 if (found >= MAX_SERVICES)
2617                         break;
2618         }
2619         rcu_read_unlock();
2620
2621         read_unlock_bh(&arm_state->susp_res_lock);
2622
2623         if (only_nonzero)
2624                 vchiq_log_warning(vchiq_susp_log_level, "Too many active "
2625                         "services (%d).  Only dumping up to first %d services "
2626                         "with non-zero use-count", active_services, found);
2627
2628         for (i = 0; i < found; i++) {
2629                 vchiq_log_warning(vchiq_susp_log_level,
2630                         "----- %c%c%c%c:%d service count %d %s",
2631                         VCHIQ_FOURCC_AS_4CHARS(service_data[i].fourcc),
2632                         service_data[i].clientid,
2633                         service_data[i].use_count,
2634                         service_data[i].use_count ? nz : "");
2635         }
2636         vchiq_log_warning(vchiq_susp_log_level,
2637                 "----- VCHIQ use count count %d", peer_count);
2638         vchiq_log_warning(vchiq_susp_log_level,
2639                 "--- Overall vchiq instance use count %d", vc_use_count);
2640
2641         kfree(service_data);
2642 }
2643
2644 enum vchiq_status
2645 vchiq_check_service(struct vchiq_service *service)
2646 {
2647         struct vchiq_arm_state *arm_state;
2648         enum vchiq_status ret = VCHIQ_ERROR;
2649
2650         if (!service || !service->state)
2651                 goto out;
2652
2653         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2654
2655         arm_state = vchiq_platform_get_arm_state(service->state);
2656
2657         read_lock_bh(&arm_state->susp_res_lock);
2658         if (service->service_use_count)
2659                 ret = VCHIQ_SUCCESS;
2660         read_unlock_bh(&arm_state->susp_res_lock);
2661
2662         if (ret == VCHIQ_ERROR) {
2663                 vchiq_log_error(vchiq_susp_log_level,
2664                         "%s ERROR - %c%c%c%c:%d service count %d, "
2665                         "state count %d", __func__,
2666                         VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2667                         service->client_id, service->service_use_count,
2668                         arm_state->videocore_use_count);
2669                 vchiq_dump_service_use_state(service->state);
2670         }
2671 out:
2672         return ret;
2673 }
2674
2675 void vchiq_platform_conn_state_changed(struct vchiq_state *state,
2676                                        enum vchiq_connstate oldstate,
2677                                        enum vchiq_connstate newstate)
2678 {
2679         struct vchiq_arm_state *arm_state = vchiq_platform_get_arm_state(state);
2680         char threadname[16];
2681
2682         vchiq_log_info(vchiq_susp_log_level, "%d: %s->%s", state->id,
2683                 get_conn_state_name(oldstate), get_conn_state_name(newstate));
2684         if (state->conn_state != VCHIQ_CONNSTATE_CONNECTED)
2685                 return;
2686
2687         write_lock_bh(&arm_state->susp_res_lock);
2688         if (arm_state->first_connect) {
2689                 write_unlock_bh(&arm_state->susp_res_lock);
2690                 return;
2691         }
2692
2693         arm_state->first_connect = 1;
2694         write_unlock_bh(&arm_state->susp_res_lock);
2695         snprintf(threadname, sizeof(threadname), "vchiq-keep/%d",
2696                  state->id);
2697         arm_state->ka_thread = kthread_create(&vchiq_keepalive_thread_func,
2698                                               (void *)state,
2699                                               threadname);
2700         if (IS_ERR(arm_state->ka_thread)) {
2701                 vchiq_log_error(vchiq_susp_log_level,
2702                                 "vchiq: FATAL: couldn't create thread %s",
2703                                 threadname);
2704         } else {
2705                 wake_up_process(arm_state->ka_thread);
2706         }
2707 }
2708
2709 static const struct of_device_id vchiq_of_match[] = {
2710         { .compatible = "brcm,bcm2835-vchiq", .data = &bcm2835_drvdata },
2711         { .compatible = "brcm,bcm2836-vchiq", .data = &bcm2836_drvdata },
2712         {},
2713 };
2714 MODULE_DEVICE_TABLE(of, vchiq_of_match);
2715
2716 static struct platform_device *
2717 vchiq_register_child(struct platform_device *pdev, const char *name)
2718 {
2719         struct platform_device_info pdevinfo;
2720         struct platform_device *child;
2721
2722         memset(&pdevinfo, 0, sizeof(pdevinfo));
2723
2724         pdevinfo.parent = &pdev->dev;
2725         pdevinfo.name = name;
2726         pdevinfo.id = PLATFORM_DEVID_NONE;
2727         pdevinfo.dma_mask = DMA_BIT_MASK(32);
2728
2729         child = platform_device_register_full(&pdevinfo);
2730         if (IS_ERR(child)) {
2731                 dev_warn(&pdev->dev, "%s not registered\n", name);
2732                 child = NULL;
2733         }
2734
2735         return child;
2736 }
2737
2738 static int vchiq_probe(struct platform_device *pdev)
2739 {
2740         struct device_node *fw_node;
2741         const struct of_device_id *of_id;
2742         struct vchiq_drvdata *drvdata;
2743         struct device *vchiq_dev;
2744         int err;
2745
2746         of_id = of_match_node(vchiq_of_match, pdev->dev.of_node);
2747         drvdata = (struct vchiq_drvdata *)of_id->data;
2748         if (!drvdata)
2749                 return -EINVAL;
2750
2751         fw_node = of_find_compatible_node(NULL, NULL,
2752                                           "raspberrypi,bcm2835-firmware");
2753         if (!fw_node) {
2754                 dev_err(&pdev->dev, "Missing firmware node\n");
2755                 return -ENOENT;
2756         }
2757
2758         drvdata->fw = rpi_firmware_get(fw_node);
2759         of_node_put(fw_node);
2760         if (!drvdata->fw)
2761                 return -EPROBE_DEFER;
2762
2763         platform_set_drvdata(pdev, drvdata);
2764
2765         err = vchiq_platform_init(pdev, &g_state);
2766         if (err)
2767                 goto failed_platform_init;
2768
2769         cdev_init(&vchiq_cdev, &vchiq_fops);
2770         vchiq_cdev.owner = THIS_MODULE;
2771         err = cdev_add(&vchiq_cdev, vchiq_devid, 1);
2772         if (err) {
2773                 vchiq_log_error(vchiq_arm_log_level,
2774                         "Unable to register device");
2775                 goto failed_platform_init;
2776         }
2777
2778         vchiq_dev = device_create(vchiq_class, &pdev->dev, vchiq_devid, NULL,
2779                                   "vchiq");
2780         if (IS_ERR(vchiq_dev)) {
2781                 err = PTR_ERR(vchiq_dev);
2782                 goto failed_device_create;
2783         }
2784
2785         vchiq_debugfs_init();
2786
2787         vchiq_log_info(vchiq_arm_log_level,
2788                 "vchiq: initialised - version %d (min %d), device %d.%d",
2789                 VCHIQ_VERSION, VCHIQ_VERSION_MIN,
2790                 MAJOR(vchiq_devid), MINOR(vchiq_devid));
2791
2792         bcm2835_camera = vchiq_register_child(pdev, "bcm2835-camera");
2793         bcm2835_audio = vchiq_register_child(pdev, "bcm2835_audio");
2794
2795         return 0;
2796
2797 failed_device_create:
2798         cdev_del(&vchiq_cdev);
2799 failed_platform_init:
2800         vchiq_log_warning(vchiq_arm_log_level, "could not load vchiq");
2801         return err;
2802 }
2803
2804 static int vchiq_remove(struct platform_device *pdev)
2805 {
2806         platform_device_unregister(bcm2835_camera);
2807         vchiq_debugfs_deinit();
2808         device_destroy(vchiq_class, vchiq_devid);
2809         cdev_del(&vchiq_cdev);
2810
2811         return 0;
2812 }
2813
2814 static struct platform_driver vchiq_driver = {
2815         .driver = {
2816                 .name = "bcm2835_vchiq",
2817                 .of_match_table = vchiq_of_match,
2818         },
2819         .probe = vchiq_probe,
2820         .remove = vchiq_remove,
2821 };
2822
2823 static int __init vchiq_driver_init(void)
2824 {
2825         int ret;
2826
2827         vchiq_class = class_create(THIS_MODULE, DEVICE_NAME);
2828         if (IS_ERR(vchiq_class)) {
2829                 pr_err("Failed to create vchiq class\n");
2830                 return PTR_ERR(vchiq_class);
2831         }
2832
2833         ret = alloc_chrdev_region(&vchiq_devid, 0, 1, DEVICE_NAME);
2834         if (ret) {
2835                 pr_err("Failed to allocate vchiq's chrdev region\n");
2836                 goto class_destroy;
2837         }
2838
2839         ret = platform_driver_register(&vchiq_driver);
2840         if (ret) {
2841                 pr_err("Failed to register vchiq driver\n");
2842                 goto region_unregister;
2843         }
2844
2845         return 0;
2846
2847 region_unregister:
2848         unregister_chrdev_region(vchiq_devid, 1);
2849
2850 class_destroy:
2851         class_destroy(vchiq_class);
2852
2853         return ret;
2854 }
2855 module_init(vchiq_driver_init);
2856
2857 static void __exit vchiq_driver_exit(void)
2858 {
2859         platform_driver_unregister(&vchiq_driver);
2860         unregister_chrdev_region(vchiq_devid, 1);
2861         class_destroy(vchiq_class);
2862 }
2863 module_exit(vchiq_driver_exit);
2864
2865 MODULE_LICENSE("Dual BSD/GPL");
2866 MODULE_DESCRIPTION("Videocore VCHIQ driver");
2867 MODULE_AUTHOR("Broadcom Corporation");