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

   1 /**
   2  * Copyright (c) 2014 Raspberry Pi (Trading) Ltd. All rights reserved.
   3  * Copyright (c) 2010-2012 Broadcom. All rights reserved.
   4  *
   5  * Redistribution and use in source and binary forms, with or without
   6  * modification, are permitted provided that the following conditions
   7  * are met:
   8  * 1. Redistributions of source code must retain the above copyright
   9  *    notice, this list of conditions, and the following disclaimer,
  10  *    without modification.
  11  * 2. Redistributions in binary form must reproduce the above copyright
  12  *    notice, this list of conditions and the following disclaimer in the
  13  *    documentation and/or other materials provided with the distribution.
  14  * 3. The names of the above-listed copyright holders may not be used
  15  *    to endorse or promote products derived from this software without
  16  *    specific prior written permission.
  17  *
  18  * ALTERNATIVELY, this software may be distributed under the terms of the
  19  * GNU General Public License ("GPL") version 2, as published by the Free
  20  * Software Foundation.
  21  *
  22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
  23  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  24  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  25  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
  26  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  27  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  28  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  29  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  30  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  31  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  32  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  33  */
  34
  35 #include <linux/kernel.h>
  36 #include <linux/module.h>
  37 #include <linux/sched/signal.h>
  38 #include <linux/types.h>
  39 #include <linux/errno.h>
  40 #include <linux/cdev.h>
  41 #include <linux/fs.h>
  42 #include <linux/device.h>
  43 #include <linux/mm.h>
  44 #include <linux/highmem.h>
  45 #include <linux/pagemap.h>
  46 #include <linux/bug.h>
  47 #include <linux/semaphore.h>
  48 #include <linux/list.h>
  49 #include <linux/of.h>
  50 #include <linux/platform_device.h>
  51 #include <linux/compat.h>
  52 #include <soc/bcm2835/raspberrypi-firmware.h>
  53
  54 #include "vchiq_core.h"
  55 #include "vchiq_ioctl.h"
  56 #include "vchiq_arm.h"
  57 #include "vchiq_debugfs.h"
  58 #include "vchiq_killable.h"
  59
  60 #define DEVICE_NAME "vchiq"
  61
  62 /* Override the default prefix, which would be vchiq_arm (from the filename) */
  63 #undef MODULE_PARAM_PREFIX
  64 #define MODULE_PARAM_PREFIX DEVICE_NAME "."
  65
  66 #define VCHIQ_MINOR 0
  67
  68 /* Some per-instance constants */
  69 #define MAX_COMPLETIONS 128
  70 #define MAX_SERVICES 64
  71 #define MAX_ELEMENTS 8
  72 #define MSG_QUEUE_SIZE 128
  73
  74 #define KEEPALIVE_VER 1
  75 #define KEEPALIVE_VER_MIN KEEPALIVE_VER
  76
  77 /* Run time control of log level, based on KERN_XXX level. */
  78 int vchiq_arm_log_level = VCHIQ_LOG_DEFAULT;
  79 int vchiq_susp_log_level = VCHIQ_LOG_ERROR;
  80
  81 #define SUSPEND_TIMER_TIMEOUT_MS 100
  82 #define SUSPEND_RETRY_TIMER_TIMEOUT_MS 1000
  83
  84 #define VC_SUSPEND_NUM_OFFSET 3 /* number of values before idle which are -ve */
  85 static const char *const suspend_state_names[] = {
  86         "VC_SUSPEND_FORCE_CANCELED",
  87         "VC_SUSPEND_REJECTED",
  88         "VC_SUSPEND_FAILED",
  89         "VC_SUSPEND_IDLE",
  90         "VC_SUSPEND_REQUESTED",
  91         "VC_SUSPEND_IN_PROGRESS",
  92         "VC_SUSPEND_SUSPENDED"
  93 };
  94 #define VC_RESUME_NUM_OFFSET 1 /* number of values before idle which are -ve */
  95 static const char *const resume_state_names[] = {
  96         "VC_RESUME_FAILED",
  97         "VC_RESUME_IDLE",
  98         "VC_RESUME_REQUESTED",
  99         "VC_RESUME_IN_PROGRESS",
 100         "VC_RESUME_RESUMED"
 101 };
 102 /* The number of times we allow force suspend to timeout before actually
 103 ** _forcing_ suspend.  This is to cater for SW which fails to release vchiq
 104 ** correctly - we don't want to prevent ARM suspend indefinitely in this case.
 105 */
 106 #define FORCE_SUSPEND_FAIL_MAX 8
 107
 108 /* The time in ms allowed for videocore to go idle when force suspend has been
 109  * requested */
 110 #define FORCE_SUSPEND_TIMEOUT_MS 200
 111
 112
 113 static void suspend_timer_callback(unsigned long context);
 114
 115
 116 typedef struct user_service_struct {
 117         VCHIQ_SERVICE_T *service;
 118         void *userdata;
 119         VCHIQ_INSTANCE_T instance;
 120         char is_vchi;
 121         char dequeue_pending;
 122         char close_pending;
 123         int message_available_pos;
 124         int msg_insert;
 125         int msg_remove;
 126         struct semaphore insert_event;
 127         struct semaphore remove_event;
 128         struct semaphore close_event;
 129         VCHIQ_HEADER_T * msg_queue[MSG_QUEUE_SIZE];
 130 } USER_SERVICE_T;
 131
 132 struct bulk_waiter_node {
 133         struct bulk_waiter bulk_waiter;
 134         int pid;
 135         struct list_head list;
 136 };
 137
 138 struct vchiq_instance_struct {
 139         VCHIQ_STATE_T *state;
 140         VCHIQ_COMPLETION_DATA_T completions[MAX_COMPLETIONS];
 141         int completion_insert;
 142         int completion_remove;
 143         struct semaphore insert_event;
 144         struct semaphore remove_event;
 145         struct mutex completion_mutex;
 146
 147         int connected;
 148         int closing;
 149         int pid;
 150         int mark;
 151         int use_close_delivered;
 152         int trace;
 153
 154         struct list_head bulk_waiter_list;
 155         struct mutex bulk_waiter_list_mutex;
 156
 157         VCHIQ_DEBUGFS_NODE_T debugfs_node;
 158 };
 159
 160 typedef struct dump_context_struct {
 161         char __user *buf;
 162         size_t actual;
 163         size_t space;
 164         loff_t offset;
 165 } DUMP_CONTEXT_T;
 166
 167 static struct cdev    vchiq_cdev;
 168 static dev_t          vchiq_devid;
 169 static VCHIQ_STATE_T g_state;
 170 static struct class  *vchiq_class;
 171 static struct device *vchiq_dev;
 172 static DEFINE_SPINLOCK(msg_queue_spinlock);
 173
 174 static const char *const ioctl_names[] = {
 175         "CONNECT",
 176         "SHUTDOWN",
 177         "CREATE_SERVICE",
 178         "REMOVE_SERVICE",
 179         "QUEUE_MESSAGE",
 180         "QUEUE_BULK_TRANSMIT",
 181         "QUEUE_BULK_RECEIVE",
 182         "AWAIT_COMPLETION",
 183         "DEQUEUE_MESSAGE",
 184         "GET_CLIENT_ID",
 185         "GET_CONFIG",
 186         "CLOSE_SERVICE",
 187         "USE_SERVICE",
 188         "RELEASE_SERVICE",
 189         "SET_SERVICE_OPTION",
 190         "DUMP_PHYS_MEM",
 191         "LIB_VERSION",
 192         "CLOSE_DELIVERED"
 193 };
 194
 195 vchiq_static_assert(ARRAY_SIZE(ioctl_names) ==
 196                     (VCHIQ_IOC_MAX + 1));
 197
 198 #if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
 199 static void
 200 dump_phys_mem(void *virt_addr, u32 num_bytes);
 201 #endif
 202
 203 /****************************************************************************
 204 *
 205 *   add_completion
 206 *
 207 ***************************************************************************/
 208
 209 static VCHIQ_STATUS_T
 210 add_completion(VCHIQ_INSTANCE_T instance, VCHIQ_REASON_T reason,
 211         VCHIQ_HEADER_T *header, USER_SERVICE_T *user_service,
 212         void *bulk_userdata)
 213 {
 214         VCHIQ_COMPLETION_DATA_T *completion;
 215         int insert;
 216
 217         DEBUG_INITIALISE(g_state.local)
 218
 219         insert = instance->completion_insert;
 220         while ((insert - instance->completion_remove) >= MAX_COMPLETIONS) {
 221                 /* Out of space - wait for the client */
 222                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 223                 vchiq_log_trace(vchiq_arm_log_level,
 224                         "add_completion - completion queue full");
 225                 DEBUG_COUNT(COMPLETION_QUEUE_FULL_COUNT);
 226                 if (down_interruptible(&instance->remove_event) != 0) {
 227                         vchiq_log_info(vchiq_arm_log_level,
 228                                 "service_callback interrupted");
 229                         return VCHIQ_RETRY;
 230                 } else if (instance->closing) {
 231                         vchiq_log_info(vchiq_arm_log_level,
 232                                 "service_callback closing");
 233                         return VCHIQ_SUCCESS;
 234                 }
 235                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 236         }
 237
 238         completion = &instance->completions[insert & (MAX_COMPLETIONS - 1)];
 239
 240         completion->header = header;
 241         completion->reason = reason;
 242         /* N.B. service_userdata is updated while processing AWAIT_COMPLETION */
 243         completion->service_userdata = user_service->service;
 244         completion->bulk_userdata = bulk_userdata;
 245
 246         if (reason == VCHIQ_SERVICE_CLOSED) {
 247                 /* Take an extra reference, to be held until
 248                    this CLOSED notification is delivered. */
 249                 lock_service(user_service->service);
 250                 if (instance->use_close_delivered)
 251                         user_service->close_pending = 1;
 252         }
 253
 254         /* A write barrier is needed here to ensure that the entire completion
 255                 record is written out before the insert point. */
 256         wmb();
 257
 258         if (reason == VCHIQ_MESSAGE_AVAILABLE)
 259                 user_service->message_available_pos = insert;
 260
 261         insert++;
 262         instance->completion_insert = insert;
 263
 264         up(&instance->insert_event);
 265
 266         return VCHIQ_SUCCESS;
 267 }
 268
 269 /****************************************************************************
 270 *
 271 *   service_callback
 272 *
 273 ***************************************************************************/
 274
 275 static VCHIQ_STATUS_T
 276 service_callback(VCHIQ_REASON_T reason, VCHIQ_HEADER_T *header,
 277         VCHIQ_SERVICE_HANDLE_T handle, void *bulk_userdata)
 278 {
 279         /* How do we ensure the callback goes to the right client?
 280         ** The service_user data points to a USER_SERVICE_T record containing
 281         ** the original callback and the user state structure, which contains a
 282         ** circular buffer for completion records.
 283         */
 284         USER_SERVICE_T *user_service;
 285         VCHIQ_SERVICE_T *service;
 286         VCHIQ_INSTANCE_T instance;
 287         bool skip_completion = false;
 288
 289         DEBUG_INITIALISE(g_state.local)
 290
 291         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 292
 293         service = handle_to_service(handle);
 294         BUG_ON(!service);
 295         user_service = (USER_SERVICE_T *)service->base.userdata;
 296         instance = user_service->instance;
 297
 298         if (!instance || instance->closing)
 299                 return VCHIQ_SUCCESS;
 300
 301         vchiq_log_trace(vchiq_arm_log_level,
 302                 "service_callback - service %lx(%d,%p), reason %d, header %lx, "
 303                 "instance %lx, bulk_userdata %lx",
 304                 (unsigned long)user_service,
 305                 service->localport, user_service->userdata,
 306                 reason, (unsigned long)header,
 307                 (unsigned long)instance, (unsigned long)bulk_userdata);
 308
 309         if (header && user_service->is_vchi) {
 310                 spin_lock(&msg_queue_spinlock);
 311                 while (user_service->msg_insert ==
 312                         (user_service->msg_remove + MSG_QUEUE_SIZE)) {
 313                         spin_unlock(&msg_queue_spinlock);
 314                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 315                         DEBUG_COUNT(MSG_QUEUE_FULL_COUNT);
 316                         vchiq_log_trace(vchiq_arm_log_level,
 317                                 "service_callback - msg queue full");
 318                         /* If there is no MESSAGE_AVAILABLE in the completion
 319                         ** queue, add one
 320                         */
 321                         if ((user_service->message_available_pos -
 322                                 instance->completion_remove) < 0) {
 323                                 VCHIQ_STATUS_T status;
 324
 325                                 vchiq_log_info(vchiq_arm_log_level,
 326                                         "Inserting extra MESSAGE_AVAILABLE");
 327                                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 328                                 status = add_completion(instance, reason,
 329                                         NULL, user_service, bulk_userdata);
 330                                 if (status != VCHIQ_SUCCESS) {
 331                                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 332                                         return status;
 333                                 }
 334                         }
 335
 336                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 337                         if (down_interruptible(&user_service->remove_event)
 338                                 != 0) {
 339                                 vchiq_log_info(vchiq_arm_log_level,
 340                                         "service_callback interrupted");
 341                                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 342                                 return VCHIQ_RETRY;
 343                         } else if (instance->closing) {
 344                                 vchiq_log_info(vchiq_arm_log_level,
 345                                         "service_callback closing");
 346                                 DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 347                                 return VCHIQ_ERROR;
 348                         }
 349                         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 350                         spin_lock(&msg_queue_spinlock);
 351                 }
 352
 353                 user_service->msg_queue[user_service->msg_insert &
 354                         (MSG_QUEUE_SIZE - 1)] = header;
 355                 user_service->msg_insert++;
 356
 357                 /* If there is a thread waiting in DEQUEUE_MESSAGE, or if
 358                 ** there is a MESSAGE_AVAILABLE in the completion queue then
 359                 ** bypass the completion queue.
 360                 */
 361                 if (((user_service->message_available_pos -
 362                         instance->completion_remove) >= 0) ||
 363                         user_service->dequeue_pending) {
 364                         user_service->dequeue_pending = 0;
 365                         skip_completion = true;
 366                 }
 367
 368                 spin_unlock(&msg_queue_spinlock);
 369                 up(&user_service->insert_event);
 370
 371                 header = NULL;
 372         }
 373         DEBUG_TRACE(SERVICE_CALLBACK_LINE);
 374
 375         if (skip_completion)
 376                 return VCHIQ_SUCCESS;
 377
 378         return add_completion(instance, reason, header, user_service,
 379                 bulk_userdata);
 380 }
 381
 382 /****************************************************************************
 383 *
 384 *   user_service_free
 385 *
 386 ***************************************************************************/
 387 static void
 388 user_service_free(void *userdata)
 389 {
 390         kfree(userdata);
 391 }
 392
 393 /****************************************************************************
 394 *
 395 *   close_delivered
 396 *
 397 ***************************************************************************/
 398 static void close_delivered(USER_SERVICE_T *user_service)
 399 {
 400         vchiq_log_info(vchiq_arm_log_level,
 401                 "close_delivered(handle=%x)",
 402                 user_service->service->handle);
 403
 404         if (user_service->close_pending) {
 405                 /* Allow the underlying service to be culled */
 406                 unlock_service(user_service->service);
 407
 408                 /* Wake the user-thread blocked in close_ or remove_service */
 409                 up(&user_service->close_event);
 410
 411                 user_service->close_pending = 0;
 412         }
 413 }
 414
 415 struct vchiq_io_copy_callback_context {
 416         struct vchiq_element *current_element;
 417         size_t current_element_offset;
 418         unsigned long elements_to_go;
 419         size_t current_offset;
 420 };
 421
 422 static ssize_t
 423 vchiq_ioc_copy_element_data(
 424         void *context,
 425         void *dest,
 426         size_t offset,
 427         size_t maxsize)
 428 {
 429         long res;
 430         size_t bytes_this_round;
 431         struct vchiq_io_copy_callback_context *copy_context =
 432                 (struct vchiq_io_copy_callback_context *)context;
 433
 434         if (offset != copy_context->current_offset)
 435                 return 0;
 436
 437         if (!copy_context->elements_to_go)
 438                 return 0;
 439
 440         /*
 441          * Complex logic here to handle the case of 0 size elements
 442          * in the middle of the array of elements.
 443          *
 444          * Need to skip over these 0 size elements.
 445          */
 446         while (1) {
 447                 bytes_this_round = min(copy_context->current_element->size -
 448                                        copy_context->current_element_offset,
 449                                        maxsize);
 450
 451                 if (bytes_this_round)
 452                         break;
 453
 454                 copy_context->elements_to_go--;
 455                 copy_context->current_element++;
 456                 copy_context->current_element_offset = 0;
 457
 458                 if (!copy_context->elements_to_go)
 459                         return 0;
 460         }
 461
 462         res = copy_from_user(dest,
 463                              copy_context->current_element->data +
 464                              copy_context->current_element_offset,
 465                              bytes_this_round);
 466
 467         if (res != 0)
 468                 return -EFAULT;
 469
 470         copy_context->current_element_offset += bytes_this_round;
 471         copy_context->current_offset += bytes_this_round;
 472
 473         /*
 474          * Check if done with current element, and if so advance to the next.
 475          */
 476         if (copy_context->current_element_offset ==
 477             copy_context->current_element->size) {
 478                 copy_context->elements_to_go--;
 479                 copy_context->current_element++;
 480                 copy_context->current_element_offset = 0;
 481         }
 482
 483         return bytes_this_round;
 484 }
 485
 486 /**************************************************************************
 487  *
 488  *   vchiq_ioc_queue_message
 489  *
 490  **************************************************************************/
 491 static VCHIQ_STATUS_T
 492 vchiq_ioc_queue_message(VCHIQ_SERVICE_HANDLE_T handle,
 493                         struct vchiq_element *elements,
 494                         unsigned long count)
 495 {
 496         struct vchiq_io_copy_callback_context context;
 497         unsigned long i;
 498         size_t total_size = 0;
 499
 500         context.current_element = elements;
 501         context.current_element_offset = 0;
 502         context.elements_to_go = count;
 503         context.current_offset = 0;
 504
 505         for (i = 0; i < count; i++) {
 506                 if (!elements[i].data && elements[i].size != 0)
 507                         return -EFAULT;
 508
 509                 total_size += elements[i].size;
 510         }
 511
 512         return vchiq_queue_message(handle, vchiq_ioc_copy_element_data,
 513                                    &context, total_size);
 514 }
 515
 516 /****************************************************************************
 517 *
 518 *   vchiq_ioctl
 519 *
 520 ***************************************************************************/
 521 static long
 522 vchiq_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
 523 {
 524         VCHIQ_INSTANCE_T instance = file->private_data;
 525         VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
 526         VCHIQ_SERVICE_T *service = NULL;
 527         long ret = 0;
 528         int i, rc;
 529
 530         DEBUG_INITIALISE(g_state.local)
 531
 532         vchiq_log_trace(vchiq_arm_log_level,
 533                 "vchiq_ioctl - instance %pK, cmd %s, arg %lx",
 534                 instance,
 535                 ((_IOC_TYPE(cmd) == VCHIQ_IOC_MAGIC) &&
 536                 (_IOC_NR(cmd) <= VCHIQ_IOC_MAX)) ?
 537                 ioctl_names[_IOC_NR(cmd)] : "<invalid>", arg);
 538
 539         switch (cmd) {
 540         case VCHIQ_IOC_SHUTDOWN:
 541                 if (!instance->connected)
 542                         break;
 543
 544                 /* Remove all services */
 545                 i = 0;
 546                 while ((service = next_service_by_instance(instance->state,
 547                         instance, &i)) != NULL) {
 548                         status = vchiq_remove_service(service->handle);
 549                         unlock_service(service);
 550                         if (status != VCHIQ_SUCCESS)
 551                                 break;
 552                 }
 553                 service = NULL;
 554
 555                 if (status == VCHIQ_SUCCESS) {
 556                         /* Wake the completion thread and ask it to exit */
 557                         instance->closing = 1;
 558                         up(&instance->insert_event);
 559                 }
 560
 561                 break;
 562
 563         case VCHIQ_IOC_CONNECT:
 564                 if (instance->connected) {
 565                         ret = -EINVAL;
 566                         break;
 567                 }
 568                 rc = mutex_lock_killable(&instance->state->mutex);
 569                 if (rc != 0) {
 570                         vchiq_log_error(vchiq_arm_log_level,
 571                                 "vchiq: connect: could not lock mutex for "
 572                                 "state %d: %d",
 573                                 instance->state->id, rc);
 574                         ret = -EINTR;
 575                         break;
 576                 }
 577                 status = vchiq_connect_internal(instance->state, instance);
 578                 mutex_unlock(&instance->state->mutex);
 579
 580                 if (status == VCHIQ_SUCCESS)
 581                         instance->connected = 1;
 582                 else
 583                         vchiq_log_error(vchiq_arm_log_level,
 584                                 "vchiq: could not connect: %d", status);
 585                 break;
 586
 587         case VCHIQ_IOC_CREATE_SERVICE: {
 588                 VCHIQ_CREATE_SERVICE_T args;
 589                 USER_SERVICE_T *user_service = NULL;
 590                 void *userdata;
 591                 int srvstate;
 592
 593                 if (copy_from_user
 594                          (&args, (const void __user *)arg,
 595                           sizeof(args)) != 0) {
 596                         ret = -EFAULT;
 597                         break;
 598                 }
 599
 600                 user_service = kmalloc(sizeof(USER_SERVICE_T), GFP_KERNEL);
 601                 if (!user_service) {
 602                         ret = -ENOMEM;
 603                         break;
 604                 }
 605
 606                 if (args.is_open) {
 607                         if (!instance->connected) {
 608                                 ret = -ENOTCONN;
 609                                 kfree(user_service);
 610                                 break;
 611                         }
 612                         srvstate = VCHIQ_SRVSTATE_OPENING;
 613                 } else {
 614                         srvstate =
 615                                  instance->connected ?
 616                                  VCHIQ_SRVSTATE_LISTENING :
 617                                  VCHIQ_SRVSTATE_HIDDEN;
 618                 }
 619
 620                 userdata = args.params.userdata;
 621                 args.params.callback = service_callback;
 622                 args.params.userdata = user_service;
 623                 service = vchiq_add_service_internal(
 624                                 instance->state,
 625                                 &args.params, srvstate,
 626                                 instance, user_service_free);
 627
 628                 if (service != NULL) {
 629                         user_service->service = service;
 630                         user_service->userdata = userdata;
 631                         user_service->instance = instance;
 632                         user_service->is_vchi = (args.is_vchi != 0);
 633                         user_service->dequeue_pending = 0;
 634                         user_service->close_pending = 0;
 635                         user_service->message_available_pos =
 636                                 instance->completion_remove - 1;
 637                         user_service->msg_insert = 0;
 638                         user_service->msg_remove = 0;
 639                         sema_init(&user_service->insert_event, 0);
 640                         sema_init(&user_service->remove_event, 0);
 641                         sema_init(&user_service->close_event, 0);
 642
 643                         if (args.is_open) {
 644                                 status = vchiq_open_service_internal
 645                                         (service, instance->pid);
 646                                 if (status != VCHIQ_SUCCESS) {
 647                                         vchiq_remove_service(service->handle);
 648                                         service = NULL;
 649                                         ret = (status == VCHIQ_RETRY) ?
 650                                                 -EINTR : -EIO;
 651                                         break;
 652                                 }
 653                         }
 654
 655                         if (copy_to_user((void __user *)
 656                                 &(((VCHIQ_CREATE_SERVICE_T __user *)
 657                                         arg)->handle),
 658                                 (const void *)&service->handle,
 659                                 sizeof(service->handle)) != 0) {
 660                                 ret = -EFAULT;
 661                                 vchiq_remove_service(service->handle);
 662                         }
 663
 664                         service = NULL;
 665                 } else {
 666                         ret = -EEXIST;
 667                         kfree(user_service);
 668                 }
 669         } break;
 670
 671         case VCHIQ_IOC_CLOSE_SERVICE: {
 672                 VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg;
 673
 674                 service = find_service_for_instance(instance, handle);
 675                 if (service != NULL) {
 676                         USER_SERVICE_T *user_service =
 677                                 (USER_SERVICE_T *)service->base.userdata;
 678                         /* close_pending is false on first entry, and when the
 679                            wait in vchiq_close_service has been interrupted. */
 680                         if (!user_service->close_pending) {
 681                                 status = vchiq_close_service(service->handle);
 682                                 if (status != VCHIQ_SUCCESS)
 683                                         break;
 684                         }
 685
 686                         /* close_pending is true once the underlying service
 687                            has been closed until the client library calls the
 688                            CLOSE_DELIVERED ioctl, signalling close_event. */
 689                         if (user_service->close_pending &&
 690                                 down_interruptible(&user_service->close_event))
 691                                 status = VCHIQ_RETRY;
 692                 }
 693                 else
 694                         ret = -EINVAL;
 695         } break;
 696
 697         case VCHIQ_IOC_REMOVE_SERVICE: {
 698                 VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg;
 699
 700                 service = find_service_for_instance(instance, handle);
 701                 if (service != NULL) {
 702                         USER_SERVICE_T *user_service =
 703                                 (USER_SERVICE_T *)service->base.userdata;
 704                         /* close_pending is false on first entry, and when the
 705                            wait in vchiq_close_service has been interrupted. */
 706                         if (!user_service->close_pending) {
 707                                 status = vchiq_remove_service(service->handle);
 708                                 if (status != VCHIQ_SUCCESS)
 709                                         break;
 710                         }
 711
 712                         /* close_pending is true once the underlying service
 713                            has been closed until the client library calls the
 714                            CLOSE_DELIVERED ioctl, signalling close_event. */
 715                         if (user_service->close_pending &&
 716                                 down_interruptible(&user_service->close_event))
 717                                 status = VCHIQ_RETRY;
 718                 }
 719                 else
 720                         ret = -EINVAL;
 721         } break;
 722
 723         case VCHIQ_IOC_USE_SERVICE:
 724         case VCHIQ_IOC_RELEASE_SERVICE: {
 725                 VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg;
 726
 727                 service = find_service_for_instance(instance, handle);
 728                 if (service != NULL) {
 729                         status = (cmd == VCHIQ_IOC_USE_SERVICE) ?
 730                                 vchiq_use_service_internal(service) :
 731                                 vchiq_release_service_internal(service);
 732                         if (status != VCHIQ_SUCCESS) {
 733                                 vchiq_log_error(vchiq_susp_log_level,
 734                                         "%s: cmd %s returned error %d for "
 735                                         "service %c%c%c%c:%03d",
 736                                         __func__,
 737                                         (cmd == VCHIQ_IOC_USE_SERVICE) ?
 738                                                 "VCHIQ_IOC_USE_SERVICE" :
 739                                                 "VCHIQ_IOC_RELEASE_SERVICE",
 740                                         status,
 741                                         VCHIQ_FOURCC_AS_4CHARS(
 742                                                 service->base.fourcc),
 743                                         service->client_id);
 744                                 ret = -EINVAL;
 745                         }
 746                 } else
 747                         ret = -EINVAL;
 748         } break;
 749
 750         case VCHIQ_IOC_QUEUE_MESSAGE: {
 751                 VCHIQ_QUEUE_MESSAGE_T args;
 752
 753                 if (copy_from_user
 754                          (&args, (const void __user *)arg,
 755                           sizeof(args)) != 0) {
 756                         ret = -EFAULT;
 757                         break;
 758                 }
 759
 760                 service = find_service_for_instance(instance, args.handle);
 761
 762                 if ((service != NULL) && (args.count <= MAX_ELEMENTS)) {
 763                         /* Copy elements into kernel space */
 764                         struct vchiq_element elements[MAX_ELEMENTS];
 765
 766                         if (copy_from_user(elements, args.elements,
 767                                 args.count * sizeof(struct vchiq_element)) == 0)
 768                                 status = vchiq_ioc_queue_message
 769                                         (args.handle,
 770                                         elements, args.count);
 771                         else
 772                                 ret = -EFAULT;
 773                 } else {
 774                         ret = -EINVAL;
 775                 }
 776         } break;
 777
 778         case VCHIQ_IOC_QUEUE_BULK_TRANSMIT:
 779         case VCHIQ_IOC_QUEUE_BULK_RECEIVE: {
 780                 VCHIQ_QUEUE_BULK_TRANSFER_T args;
 781                 struct bulk_waiter_node *waiter = NULL;
 782
 783                 VCHIQ_BULK_DIR_T dir =
 784                         (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT) ?
 785                         VCHIQ_BULK_TRANSMIT : VCHIQ_BULK_RECEIVE;
 786
 787                 if (copy_from_user
 788                         (&args, (const void __user *)arg,
 789                         sizeof(args)) != 0) {
 790                         ret = -EFAULT;
 791                         break;
 792                 }
 793
 794                 service = find_service_for_instance(instance, args.handle);
 795                 if (!service) {
 796                         ret = -EINVAL;
 797                         break;
 798                 }
 799
 800                 if (args.mode == VCHIQ_BULK_MODE_BLOCKING) {
 801                         waiter = kzalloc(sizeof(struct bulk_waiter_node),
 802                                 GFP_KERNEL);
 803                         if (!waiter) {
 804                                 ret = -ENOMEM;
 805                                 break;
 806                         }
 807                         args.userdata = &waiter->bulk_waiter;
 808                 } else if (args.mode == VCHIQ_BULK_MODE_WAITING) {
 809                         struct list_head *pos;
 810
 811                         mutex_lock(&instance->bulk_waiter_list_mutex);
 812                         list_for_each(pos, &instance->bulk_waiter_list) {
 813                                 if (list_entry(pos, struct bulk_waiter_node,
 814                                         list)->pid == current->pid) {
 815                                         waiter = list_entry(pos,
 816                                                 struct bulk_waiter_node,
 817                                                 list);
 818                                         list_del(pos);
 819                                         break;
 820                                 }
 821
 822                         }
 823                         mutex_unlock(&instance->bulk_waiter_list_mutex);
 824                         if (!waiter) {
 825                                 vchiq_log_error(vchiq_arm_log_level,
 826                                         "no bulk_waiter found for pid %d",
 827                                         current->pid);
 828                                 ret = -ESRCH;
 829                                 break;
 830                         }
 831                         vchiq_log_info(vchiq_arm_log_level,
 832                                 "found bulk_waiter %pK for pid %d", waiter,
 833                                 current->pid);
 834                         args.userdata = &waiter->bulk_waiter;
 835                 }
 836                 status = vchiq_bulk_transfer
 837                         (args.handle,
 838                          VCHI_MEM_HANDLE_INVALID,
 839                          args.data, args.size,
 840                          args.userdata, args.mode,
 841                          dir);
 842                 if (!waiter)
 843                         break;
 844                 if ((status != VCHIQ_RETRY) || fatal_signal_pending(current) ||
 845                         !waiter->bulk_waiter.bulk) {
 846                         if (waiter->bulk_waiter.bulk) {
 847                                 /* Cancel the signal when the transfer
 848                                 ** completes. */
 849                                 spin_lock(&bulk_waiter_spinlock);
 850                                 waiter->bulk_waiter.bulk->userdata = NULL;
 851                                 spin_unlock(&bulk_waiter_spinlock);
 852                         }
 853                         kfree(waiter);
 854                 } else {
 855                         const VCHIQ_BULK_MODE_T mode_waiting =
 856                                 VCHIQ_BULK_MODE_WAITING;
 857                         waiter->pid = current->pid;
 858                         mutex_lock(&instance->bulk_waiter_list_mutex);
 859                         list_add(&waiter->list, &instance->bulk_waiter_list);
 860                         mutex_unlock(&instance->bulk_waiter_list_mutex);
 861                         vchiq_log_info(vchiq_arm_log_level,
 862                                 "saved bulk_waiter %pK for pid %d",
 863                                 waiter, current->pid);
 864
 865                         if (copy_to_user((void __user *)
 866                                 &(((VCHIQ_QUEUE_BULK_TRANSFER_T __user *)
 867                                         arg)->mode),
 868                                 (const void *)&mode_waiting,
 869                                 sizeof(mode_waiting)) != 0)
 870                                 ret = -EFAULT;
 871                 }
 872         } break;
 873
 874         case VCHIQ_IOC_AWAIT_COMPLETION: {
 875                 VCHIQ_AWAIT_COMPLETION_T args;
 876
 877                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 878                 if (!instance->connected) {
 879                         ret = -ENOTCONN;
 880                         break;
 881                 }
 882
 883                 if (copy_from_user(&args, (const void __user *)arg,
 884                         sizeof(args)) != 0) {
 885                         ret = -EFAULT;
 886                         break;
 887                 }
 888
 889                 mutex_lock(&instance->completion_mutex);
 890
 891                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 892                 while ((instance->completion_remove ==
 893                         instance->completion_insert)
 894                         && !instance->closing) {
 895                         int rc;
 896
 897                         DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 898                         mutex_unlock(&instance->completion_mutex);
 899                         rc = down_interruptible(&instance->insert_event);
 900                         mutex_lock(&instance->completion_mutex);
 901                         if (rc != 0) {
 902                                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 903                                 vchiq_log_info(vchiq_arm_log_level,
 904                                         "AWAIT_COMPLETION interrupted");
 905                                 ret = -EINTR;
 906                                 break;
 907                         }
 908                 }
 909                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
 910
 911                 if (ret == 0) {
 912                         int msgbufcount = args.msgbufcount;
 913                         int remove = instance->completion_remove;
 914
 915                         for (ret = 0; ret < args.count; ret++) {
 916                                 VCHIQ_COMPLETION_DATA_T *completion;
 917                                 VCHIQ_SERVICE_T *service;
 918                                 USER_SERVICE_T *user_service;
 919                                 VCHIQ_HEADER_T *header;
 920
 921                                 if (remove == instance->completion_insert)
 922                                         break;
 923
 924                                 completion = &instance->completions[
 925                                         remove & (MAX_COMPLETIONS - 1)];
 926
 927                                 /*
 928                                  * A read memory barrier is needed to stop
 929                                  * prefetch of a stale completion record
 930                                  */
 931                                 rmb();
 932
 933                                 service = completion->service_userdata;
 934                                 user_service = service->base.userdata;
 935                                 completion->service_userdata =
 936                                         user_service->userdata;
 937
 938                                 header = completion->header;
 939                                 if (header) {
 940                                         void __user *msgbuf;
 941                                         int msglen;
 942
 943                                         msglen = header->size +
 944                                                 sizeof(VCHIQ_HEADER_T);
 945                                         /* This must be a VCHIQ-style service */
 946                                         if (args.msgbufsize < msglen) {
 947                                                 vchiq_log_error(
 948                                                         vchiq_arm_log_level,
 949                                                         "header %pK: msgbufsize %x < msglen %x",
 950                                                         header, args.msgbufsize,
 951                                                         msglen);
 952                                                 WARN(1, "invalid message "
 953                                                         "size\n");
 954                                                 if (ret == 0)
 955                                                         ret = -EMSGSIZE;
 956                                                 break;
 957                                         }
 958                                         if (msgbufcount <= 0)
 959                                                 /* Stall here for lack of a
 960                                                 ** buffer for the message. */
 961                                                 break;
 962                                         /* Get the pointer from user space */
 963                                         msgbufcount--;
 964                                         if (copy_from_user(&msgbuf,
 965                                                 (const void __user *)
 966                                                 &args.msgbufs[msgbufcount],
 967                                                 sizeof(msgbuf)) != 0) {
 968                                                 if (ret == 0)
 969                                                         ret = -EFAULT;
 970                                                 break;
 971                                         }
 972
 973                                         /* Copy the message to user space */
 974                                         if (copy_to_user(msgbuf, header,
 975                                                 msglen) != 0) {
 976                                                 if (ret == 0)
 977                                                         ret = -EFAULT;
 978                                                 break;
 979                                         }
 980
 981                                         /* Now it has been copied, the message
 982                                         ** can be released. */
 983                                         vchiq_release_message(service->handle,
 984                                                 header);
 985
 986                                         /* The completion must point to the
 987                                         ** msgbuf. */
 988                                         completion->header = msgbuf;
 989                                 }
 990
 991                                 if ((completion->reason ==
 992                                         VCHIQ_SERVICE_CLOSED) &&
 993                                         !instance->use_close_delivered)
 994                                         unlock_service(service);
 995
 996                                 if (copy_to_user((void __user *)(
 997                                         (size_t)args.buf +
 998                                         ret * sizeof(VCHIQ_COMPLETION_DATA_T)),
 999                                         completion,
1000                                         sizeof(VCHIQ_COMPLETION_DATA_T)) != 0) {
1001                                                 if (ret == 0)
1002                                                         ret = -EFAULT;
1003                                         break;
1004                                 }
1005
1006                                 /*
1007                                  * Ensure that the above copy has completed
1008                                  * before advancing the remove pointer.
1009                                  */
1010                                 mb();
1011                                 remove++;
1012                                 instance->completion_remove = remove;
1013                         }
1014
1015                         if (msgbufcount != args.msgbufcount) {
1016                                 if (copy_to_user((void __user *)
1017                                         &((VCHIQ_AWAIT_COMPLETION_T *)arg)->
1018                                                 msgbufcount,
1019                                         &msgbufcount,
1020                                         sizeof(msgbufcount)) != 0) {
1021                                         ret = -EFAULT;
1022                                 }
1023                         }
1024                 }
1025
1026                 if (ret != 0)
1027                         up(&instance->remove_event);
1028                 mutex_unlock(&instance->completion_mutex);
1029                 DEBUG_TRACE(AWAIT_COMPLETION_LINE);
1030         } break;
1031
1032         case VCHIQ_IOC_DEQUEUE_MESSAGE: {
1033                 VCHIQ_DEQUEUE_MESSAGE_T args;
1034                 USER_SERVICE_T *user_service;
1035                 VCHIQ_HEADER_T *header;
1036
1037                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1038                 if (copy_from_user
1039                          (&args, (const void __user *)arg,
1040                           sizeof(args)) != 0) {
1041                         ret = -EFAULT;
1042                         break;
1043                 }
1044                 service = find_service_for_instance(instance, args.handle);
1045                 if (!service) {
1046                         ret = -EINVAL;
1047                         break;
1048                 }
1049                 user_service = (USER_SERVICE_T *)service->base.userdata;
1050                 if (user_service->is_vchi == 0) {
1051                         ret = -EINVAL;
1052                         break;
1053                 }
1054
1055                 spin_lock(&msg_queue_spinlock);
1056                 if (user_service->msg_remove == user_service->msg_insert) {
1057                         if (!args.blocking) {
1058                                 spin_unlock(&msg_queue_spinlock);
1059                                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1060                                 ret = -EWOULDBLOCK;
1061                                 break;
1062                         }
1063                         user_service->dequeue_pending = 1;
1064                         do {
1065                                 spin_unlock(&msg_queue_spinlock);
1066                                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1067                                 if (down_interruptible(
1068                                         &user_service->insert_event) != 0) {
1069                                         vchiq_log_info(vchiq_arm_log_level,
1070                                                 "DEQUEUE_MESSAGE interrupted");
1071                                         ret = -EINTR;
1072                                         break;
1073                                 }
1074                                 spin_lock(&msg_queue_spinlock);
1075                         } while (user_service->msg_remove ==
1076                                 user_service->msg_insert);
1077
1078                         if (ret)
1079                                 break;
1080                 }
1081
1082                 BUG_ON((int)(user_service->msg_insert -
1083                         user_service->msg_remove) < 0);
1084
1085                 header = user_service->msg_queue[user_service->msg_remove &
1086                         (MSG_QUEUE_SIZE - 1)];
1087                 user_service->msg_remove++;
1088                 spin_unlock(&msg_queue_spinlock);
1089
1090                 up(&user_service->remove_event);
1091                 if (header == NULL)
1092                         ret = -ENOTCONN;
1093                 else if (header->size <= args.bufsize) {
1094                         /* Copy to user space if msgbuf is not NULL */
1095                         if ((args.buf == NULL) ||
1096                                 (copy_to_user((void __user *)args.buf,
1097                                 header->data,
1098                                 header->size) == 0)) {
1099                                 ret = header->size;
1100                                 vchiq_release_message(
1101                                         service->handle,
1102                                         header);
1103                         } else
1104                                 ret = -EFAULT;
1105                 } else {
1106                         vchiq_log_error(vchiq_arm_log_level,
1107                                 "header %pK: bufsize %x < size %x",
1108                                 header, args.bufsize, header->size);
1109                         WARN(1, "invalid size\n");
1110                         ret = -EMSGSIZE;
1111                 }
1112                 DEBUG_TRACE(DEQUEUE_MESSAGE_LINE);
1113         } break;
1114
1115         case VCHIQ_IOC_GET_CLIENT_ID: {
1116                 VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg;
1117
1118                 ret = vchiq_get_client_id(handle);
1119         } break;
1120
1121         case VCHIQ_IOC_GET_CONFIG: {
1122                 VCHIQ_GET_CONFIG_T args;
1123                 VCHIQ_CONFIG_T config;
1124
1125                 if (copy_from_user(&args, (const void __user *)arg,
1126                         sizeof(args)) != 0) {
1127                         ret = -EFAULT;
1128                         break;
1129                 }
1130                 if (args.config_size > sizeof(config)) {
1131                         ret = -EINVAL;
1132                         break;
1133                 }
1134                 status = vchiq_get_config(instance, args.config_size, &config);
1135                 if (status == VCHIQ_SUCCESS) {
1136                         if (copy_to_user((void __user *)args.pconfig,
1137                                     &config, args.config_size) != 0) {
1138                                 ret = -EFAULT;
1139                                 break;
1140                         }
1141                 }
1142         } break;
1143
1144         case VCHIQ_IOC_SET_SERVICE_OPTION: {
1145                 VCHIQ_SET_SERVICE_OPTION_T args;
1146
1147                 if (copy_from_user(
1148                         &args, (const void __user *)arg,
1149                         sizeof(args)) != 0) {
1150                         ret = -EFAULT;
1151                         break;
1152                 }
1153
1154                 service = find_service_for_instance(instance, args.handle);
1155                 if (!service) {
1156                         ret = -EINVAL;
1157                         break;
1158                 }
1159
1160                 status = vchiq_set_service_option(
1161                                 args.handle, args.option, args.value);
1162         } break;
1163
1164 #if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
1165         case VCHIQ_IOC_DUMP_PHYS_MEM: {
1166                 VCHIQ_DUMP_MEM_T  args;
1167
1168                 if (copy_from_user
1169                          (&args, (const void __user *)arg,
1170                           sizeof(args)) != 0) {
1171                         ret = -EFAULT;
1172                         break;
1173                 }
1174                 dump_phys_mem(args.virt_addr, args.num_bytes);
1175         } break;
1176 #endif
1177
1178         case VCHIQ_IOC_LIB_VERSION: {
1179                 unsigned int lib_version = (unsigned int)arg;
1180
1181                 if (lib_version < VCHIQ_VERSION_MIN)
1182                         ret = -EINVAL;
1183                 else if (lib_version >= VCHIQ_VERSION_CLOSE_DELIVERED)
1184                         instance->use_close_delivered = 1;
1185         } break;
1186
1187         case VCHIQ_IOC_CLOSE_DELIVERED: {
1188                 VCHIQ_SERVICE_HANDLE_T handle = (VCHIQ_SERVICE_HANDLE_T)arg;
1189
1190                 service = find_closed_service_for_instance(instance, handle);
1191                 if (service != NULL) {
1192                         USER_SERVICE_T *user_service =
1193                                 (USER_SERVICE_T *)service->base.userdata;
1194                         close_delivered(user_service);
1195                 }
1196                 else
1197                         ret = -EINVAL;
1198         } break;
1199
1200         default:
1201                 ret = -ENOTTY;
1202                 break;
1203         }
1204
1205         if (service)
1206                 unlock_service(service);
1207
1208         if (ret == 0) {
1209                 if (status == VCHIQ_ERROR)
1210                         ret = -EIO;
1211                 else if (status == VCHIQ_RETRY)
1212                         ret = -EINTR;
1213         }
1214
1215         if ((status == VCHIQ_SUCCESS) && (ret < 0) && (ret != -EINTR) &&
1216                 (ret != -EWOULDBLOCK))
1217                 vchiq_log_info(vchiq_arm_log_level,
1218                         "  ioctl instance %lx, cmd %s -> status %d, %ld",
1219                         (unsigned long)instance,
1220                         (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
1221                                 ioctl_names[_IOC_NR(cmd)] :
1222                                 "<invalid>",
1223                         status, ret);
1224         else
1225                 vchiq_log_trace(vchiq_arm_log_level,
1226                         "  ioctl instance %lx, cmd %s -> status %d, %ld",
1227                         (unsigned long)instance,
1228                         (_IOC_NR(cmd) <= VCHIQ_IOC_MAX) ?
1229                                 ioctl_names[_IOC_NR(cmd)] :
1230                                 "<invalid>",
1231                         status, ret);
1232
1233         return ret;
1234 }
1235
1236 #if defined(CONFIG_COMPAT)
1237
1238 struct vchiq_service_params32 {
1239         int fourcc;
1240         compat_uptr_t callback;
1241         compat_uptr_t userdata;
1242         short version; /* Increment for non-trivial changes */
1243         short version_min; /* Update for incompatible changes */
1244 };
1245
1246 struct vchiq_create_service32 {
1247         struct vchiq_service_params32 params;
1248         int is_open;
1249         int is_vchi;
1250         unsigned int handle; /* OUT */
1251 };
1252
1253 #define VCHIQ_IOC_CREATE_SERVICE32 \
1254         _IOWR(VCHIQ_IOC_MAGIC, 2, struct vchiq_create_service32)
1255
1256 static long
1257 vchiq_compat_ioctl_create_service(
1258         struct file *file,
1259         unsigned int cmd,
1260         unsigned long arg)
1261 {
1262         VCHIQ_CREATE_SERVICE_T __user *args;
1263         struct vchiq_create_service32 __user *ptrargs32 =
1264                 (struct vchiq_create_service32 __user *)arg;
1265         struct vchiq_create_service32 args32;
1266         long ret;
1267
1268         args = compat_alloc_user_space(sizeof(*args));
1269         if (!args)
1270                 return -EFAULT;
1271
1272         if (copy_from_user(&args32,
1273                            (struct vchiq_create_service32 __user *)arg,
1274                            sizeof(args32)))
1275                 return -EFAULT;
1276
1277         if (put_user(args32.params.fourcc, &args->params.fourcc) ||
1278             put_user(compat_ptr(args32.params.callback),
1279                      &args->params.callback) ||
1280             put_user(compat_ptr(args32.params.userdata),
1281                      &args->params.userdata) ||
1282             put_user(args32.params.version, &args->params.version) ||
1283             put_user(args32.params.version_min,
1284                      &args->params.version_min) ||
1285             put_user(args32.is_open, &args->is_open) ||
1286             put_user(args32.is_vchi, &args->is_vchi) ||
1287             put_user(args32.handle, &args->handle))
1288                 return -EFAULT;
1289
1290         ret = vchiq_ioctl(file, VCHIQ_IOC_CREATE_SERVICE, (unsigned long)args);
1291
1292         if (ret < 0)
1293                 return ret;
1294
1295         if (get_user(args32.handle, &args->handle))
1296                 return -EFAULT;
1297
1298         if (copy_to_user(&ptrargs32->handle,
1299                          &args32.handle,
1300                          sizeof(args32.handle)))
1301                 return -EFAULT;
1302
1303         return 0;
1304 }
1305
1306 struct vchiq_element32 {
1307         compat_uptr_t data;
1308         unsigned int size;
1309 };
1310
1311 struct vchiq_queue_message32 {
1312         unsigned int handle;
1313         unsigned int count;
1314         compat_uptr_t elements;
1315 };
1316
1317 #define VCHIQ_IOC_QUEUE_MESSAGE32 \
1318         _IOW(VCHIQ_IOC_MAGIC,  4, struct vchiq_queue_message32)
1319
1320 static long
1321 vchiq_compat_ioctl_queue_message(struct file *file,
1322                                  unsigned int cmd,
1323                                  unsigned long arg)
1324 {
1325         VCHIQ_QUEUE_MESSAGE_T *args;
1326         struct vchiq_element *elements;
1327         struct vchiq_queue_message32 args32;
1328         unsigned int count;
1329
1330         if (copy_from_user(&args32,
1331                            (struct vchiq_queue_message32 __user *)arg,
1332                            sizeof(args32)))
1333                 return -EFAULT;
1334
1335         args = compat_alloc_user_space(sizeof(*args) +
1336                                        (sizeof(*elements) * MAX_ELEMENTS));
1337
1338         if (!args)
1339                 return -EFAULT;
1340
1341         if (put_user(args32.handle, &args->handle) ||
1342             put_user(args32.count, &args->count) ||
1343             put_user(compat_ptr(args32.elements), &args->elements))
1344                 return -EFAULT;
1345
1346         if (args32.count > MAX_ELEMENTS)
1347                 return -EINVAL;
1348
1349         if (args32.elements && args32.count) {
1350                 struct vchiq_element32 tempelement32[MAX_ELEMENTS];
1351
1352                 elements = (struct vchiq_element __user *)(args + 1);
1353
1354                 if (copy_from_user(&tempelement32,
1355                                    compat_ptr(args32.elements),
1356                                    sizeof(tempelement32)))
1357                         return -EFAULT;
1358
1359                 for (count = 0; count < args32.count; count++) {
1360                         if (put_user(compat_ptr(tempelement32[count].data),
1361                                      &elements[count].data) ||
1362                             put_user(tempelement32[count].size,
1363                                      &elements[count].size))
1364                                 return -EFAULT;
1365                 }
1366
1367                 if (put_user(elements, &args->elements))
1368                         return -EFAULT;
1369         }
1370
1371         return vchiq_ioctl(file, VCHIQ_IOC_QUEUE_MESSAGE, (unsigned long)args);
1372 }
1373
1374 struct vchiq_queue_bulk_transfer32 {
1375         unsigned int handle;
1376         compat_uptr_t data;
1377         unsigned int size;
1378         compat_uptr_t userdata;
1379         VCHIQ_BULK_MODE_T mode;
1380 };
1381
1382 #define VCHIQ_IOC_QUEUE_BULK_TRANSMIT32 \
1383         _IOWR(VCHIQ_IOC_MAGIC, 5, struct vchiq_queue_bulk_transfer32)
1384 #define VCHIQ_IOC_QUEUE_BULK_RECEIVE32 \
1385         _IOWR(VCHIQ_IOC_MAGIC, 6, struct vchiq_queue_bulk_transfer32)
1386
1387 static long
1388 vchiq_compat_ioctl_queue_bulk(struct file *file,
1389                               unsigned int cmd,
1390                               unsigned long arg)
1391 {
1392         VCHIQ_QUEUE_BULK_TRANSFER_T *args;
1393         struct vchiq_queue_bulk_transfer32 args32;
1394         struct vchiq_queue_bulk_transfer32 *ptrargs32 =
1395                 (struct vchiq_queue_bulk_transfer32 *)arg;
1396         long ret;
1397
1398         args = compat_alloc_user_space(sizeof(*args));
1399         if (!args)
1400                 return -EFAULT;
1401
1402         if (copy_from_user(&args32,
1403                            (struct vchiq_queue_bulk_transfer32 __user *)arg,
1404                            sizeof(args32)))
1405                 return -EFAULT;
1406
1407         if (put_user(args32.handle, &args->handle) ||
1408             put_user(compat_ptr(args32.data), &args->data) ||
1409             put_user(args32.size, &args->size) ||
1410             put_user(compat_ptr(args32.userdata), &args->userdata) ||
1411             put_user(args32.mode, &args->mode))
1412                 return -EFAULT;
1413
1414         if (cmd == VCHIQ_IOC_QUEUE_BULK_TRANSMIT32)
1415                 cmd = VCHIQ_IOC_QUEUE_BULK_TRANSMIT;
1416         else
1417                 cmd = VCHIQ_IOC_QUEUE_BULK_RECEIVE;
1418
1419         ret = vchiq_ioctl(file, cmd, (unsigned long)args);
1420
1421         if (ret < 0)
1422                 return ret;
1423
1424         if (get_user(args32.mode, &args->mode))
1425                 return -EFAULT;
1426
1427         if (copy_to_user(&ptrargs32->mode,
1428                          &args32.mode,
1429                          sizeof(args32.mode)))
1430                 return -EFAULT;
1431
1432         return 0;
1433 }
1434
1435 struct vchiq_completion_data32 {
1436         VCHIQ_REASON_T reason;
1437         compat_uptr_t header;
1438         compat_uptr_t service_userdata;
1439         compat_uptr_t bulk_userdata;
1440 };
1441
1442 struct vchiq_await_completion32 {
1443         unsigned int count;
1444         compat_uptr_t buf;
1445         unsigned int msgbufsize;
1446         unsigned int msgbufcount; /* IN/OUT */
1447         compat_uptr_t msgbufs;
1448 };
1449
1450 #define VCHIQ_IOC_AWAIT_COMPLETION32 \
1451         _IOWR(VCHIQ_IOC_MAGIC, 7, struct vchiq_await_completion32)
1452
1453 static long
1454 vchiq_compat_ioctl_await_completion(struct file *file,
1455                                     unsigned int cmd,
1456                                     unsigned long arg)
1457 {
1458         VCHIQ_AWAIT_COMPLETION_T *args;
1459         VCHIQ_COMPLETION_DATA_T *completion;
1460         VCHIQ_COMPLETION_DATA_T completiontemp;
1461         struct vchiq_await_completion32 args32;
1462         struct vchiq_completion_data32 completion32;
1463         unsigned int *msgbufcount32;
1464         unsigned int msgbufcount_native;
1465         compat_uptr_t msgbuf32;
1466         void *msgbuf;
1467         void **msgbufptr;
1468         long ret;
1469
1470         args = compat_alloc_user_space(sizeof(*args) +
1471                                        sizeof(*completion) +
1472                                        sizeof(*msgbufptr));
1473         if (!args)
1474                 return -EFAULT;
1475
1476         completion = (VCHIQ_COMPLETION_DATA_T *)(args + 1);
1477         msgbufptr = (void __user **)(completion + 1);
1478
1479         if (copy_from_user(&args32,
1480                            (struct vchiq_completion_data32 *)arg,
1481                            sizeof(args32)))
1482                 return -EFAULT;
1483
1484         if (put_user(args32.count, &args->count) ||
1485             put_user(compat_ptr(args32.buf), &args->buf) ||
1486             put_user(args32.msgbufsize, &args->msgbufsize) ||
1487             put_user(args32.msgbufcount, &args->msgbufcount) ||
1488             put_user(compat_ptr(args32.msgbufs), &args->msgbufs))
1489                 return -EFAULT;
1490
1491         /* These are simple cases, so just fall into the native handler */
1492         if (!args32.count || !args32.buf || !args32.msgbufcount)
1493                 return vchiq_ioctl(file,
1494                                    VCHIQ_IOC_AWAIT_COMPLETION,
1495                                    (unsigned long)args);
1496
1497         /*
1498          * These are the more complex cases.  Typical applications of this
1499          * ioctl will use a very large count, with a very large msgbufcount.
1500          * Since the native ioctl can asynchronously fill in the returned
1501          * buffers and the application can in theory begin processing messages
1502          * even before the ioctl returns, a bit of a trick is used here.
1503          *
1504          * By forcing both count and msgbufcount to be 1, it forces the native
1505          * ioctl to only claim at most 1 message is available.   This tricks
1506          * the calling application into thinking only 1 message was actually
1507          * available in the queue so like all good applications it will retry
1508          * waiting until all the required messages are received.
1509          *
1510          * This trick has been tested and proven to work with vchiq_test,
1511          * Minecraft_PI, the "hello pi" examples, and various other
1512          * applications that are included in Raspbian.
1513          */
1514
1515         if (copy_from_user(&msgbuf32,
1516                            compat_ptr(args32.msgbufs) +
1517                            (sizeof(compat_uptr_t) *
1518                            (args32.msgbufcount - 1)),
1519                            sizeof(msgbuf32)))
1520                 return -EFAULT;
1521
1522         msgbuf = compat_ptr(msgbuf32);
1523
1524         if (copy_to_user(msgbufptr,
1525                          &msgbuf,
1526                          sizeof(msgbuf)))
1527                 return -EFAULT;
1528
1529         if (copy_to_user(&args->msgbufs,
1530                          &msgbufptr,
1531                          sizeof(msgbufptr)))
1532                 return -EFAULT;
1533
1534         if (put_user(1U, &args->count) ||
1535             put_user(completion, &args->buf) ||
1536             put_user(1U, &args->msgbufcount))
1537                 return -EFAULT;
1538
1539         ret = vchiq_ioctl(file,
1540                           VCHIQ_IOC_AWAIT_COMPLETION,
1541                           (unsigned long)args);
1542
1543         /*
1544          * An return value of 0 here means that no messages where available
1545          * in the message queue.  In this case the native ioctl does not
1546          * return any data to the application at all.  Not even to update
1547          * msgbufcount.  This functionality needs to be kept here for
1548          * compatibility.
1549          *
1550          * Of course, < 0 means that an error occurred and no data is being
1551          * returned.
1552          *
1553          * Since count and msgbufcount was forced to 1, that means
1554          * the only other possible return value is 1. Meaning that 1 message
1555          * was available, so that multiple message case does not need to be
1556          * handled here.
1557          */
1558         if (ret <= 0)
1559                 return ret;
1560
1561         if (copy_from_user(&completiontemp, completion, sizeof(*completion)))
1562                 return -EFAULT;
1563
1564         completion32.reason = completiontemp.reason;
1565         completion32.header = ptr_to_compat(completiontemp.header);
1566         completion32.service_userdata =
1567                 ptr_to_compat(completiontemp.service_userdata);
1568         completion32.bulk_userdata =
1569                 ptr_to_compat(completiontemp.bulk_userdata);
1570
1571         if (copy_to_user(compat_ptr(args32.buf),
1572                          &completion32,
1573                          sizeof(completion32)))
1574                 return -EFAULT;
1575
1576         if (get_user(msgbufcount_native, &args->msgbufcount))
1577                 return -EFAULT;
1578
1579         if (!msgbufcount_native)
1580                 args32.msgbufcount--;
1581
1582         msgbufcount32 =
1583                 &((struct vchiq_await_completion32 __user *)arg)->msgbufcount;
1584
1585         if (copy_to_user(msgbufcount32,
1586                          &args32.msgbufcount,
1587                          sizeof(args32.msgbufcount)))
1588                 return -EFAULT;
1589
1590         return 1;
1591 }
1592
1593 struct vchiq_dequeue_message32 {
1594         unsigned int handle;
1595         int blocking;
1596         unsigned int bufsize;
1597         compat_uptr_t buf;
1598 };
1599
1600 #define VCHIQ_IOC_DEQUEUE_MESSAGE32 \
1601         _IOWR(VCHIQ_IOC_MAGIC, 8, struct vchiq_dequeue_message32)
1602
1603 static long
1604 vchiq_compat_ioctl_dequeue_message(struct file *file,
1605                                    unsigned int cmd,
1606                                    unsigned long arg)
1607 {
1608         VCHIQ_DEQUEUE_MESSAGE_T *args;
1609         struct vchiq_dequeue_message32 args32;
1610
1611         args = compat_alloc_user_space(sizeof(*args));
1612         if (!args)
1613                 return -EFAULT;
1614
1615         if (copy_from_user(&args32,
1616                            (struct vchiq_dequeue_message32 *)arg,
1617                            sizeof(args32)))
1618                 return -EFAULT;
1619
1620         if (put_user(args32.handle, &args->handle) ||
1621             put_user(args32.blocking, &args->blocking) ||
1622             put_user(args32.bufsize, &args->bufsize) ||
1623             put_user(compat_ptr(args32.buf), &args->buf))
1624                 return -EFAULT;
1625
1626         return vchiq_ioctl(file, VCHIQ_IOC_DEQUEUE_MESSAGE,
1627                            (unsigned long)args);
1628 }
1629
1630 struct vchiq_get_config32 {
1631         unsigned int config_size;
1632         compat_uptr_t pconfig;
1633 };
1634
1635 #define VCHIQ_IOC_GET_CONFIG32 \
1636         _IOWR(VCHIQ_IOC_MAGIC, 10, struct vchiq_get_config32)
1637
1638 static long
1639 vchiq_compat_ioctl_get_config(struct file *file,
1640                               unsigned int cmd,
1641                               unsigned long arg)
1642 {
1643         VCHIQ_GET_CONFIG_T *args;
1644         struct vchiq_get_config32 args32;
1645
1646         args = compat_alloc_user_space(sizeof(*args));
1647         if (!args)
1648                 return -EFAULT;
1649
1650         if (copy_from_user(&args32,
1651                            (struct vchiq_get_config32 *)arg,
1652                            sizeof(args32)))
1653                 return -EFAULT;
1654
1655         if (put_user(args32.config_size, &args->config_size) ||
1656             put_user(compat_ptr(args32.pconfig), &args->pconfig))
1657                 return -EFAULT;
1658
1659         return vchiq_ioctl(file, VCHIQ_IOC_GET_CONFIG, (unsigned long)args);
1660 }
1661
1662 #if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
1663
1664 struct vchiq_dump_mem32 {
1665         compat_uptr_t virt_addr;
1666         u32 num_bytes;
1667 };
1668
1669 #define VCHIQ_IOC_DUMP_PHYS_MEM32 \
1670         _IOW(VCHIQ_IOC_MAGIC, 15, struct vchiq_dump_mem32)
1671
1672 static long
1673 vchiq_compat_ioctl_dump_phys_mem(struct file *file,
1674                                  unsigned int cmd,
1675                                  unsigned long arg)
1676 {
1677         VCHIQ_DUMP_MEM_T *args;
1678         struct vchiq_dump_mem32 args32;
1679
1680         args = compat_alloc_user_space(sizeof(*args));
1681         if (!args)
1682                 return -EFAULT;
1683
1684         if (copy_from_user(&args32,
1685                            (struct vchiq_dump_mem32 *)arg,
1686                            sizeof(args32)))
1687                 return -EFAULT;
1688
1689         if (put_user(compat_ptr(args32.virt_addr), &args->virt_addr) ||
1690             put_user(args32.num_bytes, &args->num_bytes))
1691                 return -EFAULT;
1692
1693         return vchiq_ioctl(file, VCHIQ_IOC_DUMP_PHYS_MEM, (unsigned long)args);
1694 }
1695
1696 #endif
1697
1698 static long
1699 vchiq_compat_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
1700 {
1701         switch (cmd) {
1702         case VCHIQ_IOC_CREATE_SERVICE32:
1703                 return vchiq_compat_ioctl_create_service(file, cmd, arg);
1704         case VCHIQ_IOC_QUEUE_MESSAGE32:
1705                 return vchiq_compat_ioctl_queue_message(file, cmd, arg);
1706         case VCHIQ_IOC_QUEUE_BULK_TRANSMIT32:
1707         case VCHIQ_IOC_QUEUE_BULK_RECEIVE32:
1708                 return vchiq_compat_ioctl_queue_bulk(file, cmd, arg);
1709         case VCHIQ_IOC_AWAIT_COMPLETION32:
1710                 return vchiq_compat_ioctl_await_completion(file, cmd, arg);
1711         case VCHIQ_IOC_DEQUEUE_MESSAGE32:
1712                 return vchiq_compat_ioctl_dequeue_message(file, cmd, arg);
1713         case VCHIQ_IOC_GET_CONFIG32:
1714                 return vchiq_compat_ioctl_get_config(file, cmd, arg);
1715 #if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
1716         case VCHIQ_IOC_DUMP_PHYS_MEM32:
1717                 return vchiq_compat_ioctl_dump_phys_mem(file, cmd, arg);
1718 #endif
1719         default:
1720                 return vchiq_ioctl(file, cmd, arg);
1721         }
1722 }
1723
1724 #endif
1725
1726 /****************************************************************************
1727 *
1728 *   vchiq_open
1729 *
1730 ***************************************************************************/
1731
1732 static int
1733 vchiq_open(struct inode *inode, struct file *file)
1734 {
1735         int dev = iminor(inode) & 0x0f;
1736
1737         vchiq_log_info(vchiq_arm_log_level, "vchiq_open");
1738         switch (dev) {
1739         case VCHIQ_MINOR: {
1740                 int ret;
1741                 VCHIQ_STATE_T *state = vchiq_get_state();
1742                 VCHIQ_INSTANCE_T instance;
1743
1744                 if (!state) {
1745                         vchiq_log_error(vchiq_arm_log_level,
1746                                 "vchiq has no connection to VideoCore");
1747                         return -ENOTCONN;
1748                 }
1749
1750                 instance = kzalloc(sizeof(*instance), GFP_KERNEL);
1751                 if (!instance)
1752                         return -ENOMEM;
1753
1754                 instance->state = state;
1755                 instance->pid = current->tgid;
1756
1757                 ret = vchiq_debugfs_add_instance(instance);
1758                 if (ret != 0) {
1759                         kfree(instance);
1760                         return ret;
1761                 }
1762
1763                 sema_init(&instance->insert_event, 0);
1764                 sema_init(&instance->remove_event, 0);
1765                 mutex_init(&instance->completion_mutex);
1766                 mutex_init(&instance->bulk_waiter_list_mutex);
1767                 INIT_LIST_HEAD(&instance->bulk_waiter_list);
1768
1769                 file->private_data = instance;
1770         } break;
1771
1772         default:
1773                 vchiq_log_error(vchiq_arm_log_level,
1774                         "Unknown minor device: %d", dev);
1775                 return -ENXIO;
1776         }
1777
1778         return 0;
1779 }
1780
1781 /****************************************************************************
1782 *
1783 *   vchiq_release
1784 *
1785 ***************************************************************************/
1786
1787 static int
1788 vchiq_release(struct inode *inode, struct file *file)
1789 {
1790         int dev = iminor(inode) & 0x0f;
1791         int ret = 0;
1792
1793         switch (dev) {
1794         case VCHIQ_MINOR: {
1795                 VCHIQ_INSTANCE_T instance = file->private_data;
1796                 VCHIQ_STATE_T *state = vchiq_get_state();
1797                 VCHIQ_SERVICE_T *service;
1798                 int i;
1799
1800                 vchiq_log_info(vchiq_arm_log_level,
1801                         "vchiq_release: instance=%lx",
1802                         (unsigned long)instance);
1803
1804                 if (!state) {
1805                         ret = -EPERM;
1806                         goto out;
1807                 }
1808
1809                 /* Ensure videocore is awake to allow termination. */
1810                 vchiq_use_internal(instance->state, NULL,
1811                                 USE_TYPE_VCHIQ);
1812
1813                 mutex_lock(&instance->completion_mutex);
1814
1815                 /* Wake the completion thread and ask it to exit */
1816                 instance->closing = 1;
1817                 up(&instance->insert_event);
1818
1819                 mutex_unlock(&instance->completion_mutex);
1820
1821                 /* Wake the slot handler if the completion queue is full. */
1822                 up(&instance->remove_event);
1823
1824                 /* Mark all services for termination... */
1825                 i = 0;
1826                 while ((service = next_service_by_instance(state, instance,
1827                         &i)) != NULL) {
1828                         USER_SERVICE_T *user_service = service->base.userdata;
1829
1830                         /* Wake the slot handler if the msg queue is full. */
1831                         up(&user_service->remove_event);
1832
1833                         vchiq_terminate_service_internal(service);
1834                         unlock_service(service);
1835                 }
1836
1837                 /* ...and wait for them to die */
1838                 i = 0;
1839                 while ((service = next_service_by_instance(state, instance, &i))
1840                         != NULL) {
1841                         USER_SERVICE_T *user_service = service->base.userdata;
1842
1843                         down(&service->remove_event);
1844
1845                         BUG_ON(service->srvstate != VCHIQ_SRVSTATE_FREE);
1846
1847                         spin_lock(&msg_queue_spinlock);
1848
1849                         while (user_service->msg_remove !=
1850                                 user_service->msg_insert) {
1851                                 VCHIQ_HEADER_T *header = user_service->
1852                                         msg_queue[user_service->msg_remove &
1853                                                 (MSG_QUEUE_SIZE - 1)];
1854                                 user_service->msg_remove++;
1855                                 spin_unlock(&msg_queue_spinlock);
1856
1857                                 if (header)
1858                                         vchiq_release_message(
1859                                                 service->handle,
1860                                                 header);
1861                                 spin_lock(&msg_queue_spinlock);
1862                         }
1863
1864                         spin_unlock(&msg_queue_spinlock);
1865
1866                         unlock_service(service);
1867                 }
1868
1869                 /* Release any closed services */
1870                 while (instance->completion_remove !=
1871                         instance->completion_insert) {
1872                         VCHIQ_COMPLETION_DATA_T *completion;
1873                         VCHIQ_SERVICE_T *service;
1874
1875                         completion = &instance->completions[
1876                                 instance->completion_remove &
1877                                 (MAX_COMPLETIONS - 1)];
1878                         service = completion->service_userdata;
1879                         if (completion->reason == VCHIQ_SERVICE_CLOSED)
1880                         {
1881                                 USER_SERVICE_T *user_service =
1882                                         service->base.userdata;
1883
1884                                 /* Wake any blocked user-thread */
1885                                 if (instance->use_close_delivered)
1886                                         up(&user_service->close_event);
1887                                 unlock_service(service);
1888                         }
1889                         instance->completion_remove++;
1890                 }
1891
1892                 /* Release the PEER service count. */
1893                 vchiq_release_internal(instance->state, NULL);
1894
1895                 {
1896                         struct list_head *pos, *next;
1897
1898                         list_for_each_safe(pos, next,
1899                                 &instance->bulk_waiter_list) {
1900                                 struct bulk_waiter_node *waiter;
1901
1902                                 waiter = list_entry(pos,
1903                                         struct bulk_waiter_node,
1904                                         list);
1905                                 list_del(pos);
1906                                 vchiq_log_info(vchiq_arm_log_level,
1907                                         "bulk_waiter - cleaned up %pK for pid %d",
1908                                         waiter, waiter->pid);
1909                                 kfree(waiter);
1910                         }
1911                 }
1912
1913                 vchiq_debugfs_remove_instance(instance);
1914
1915                 kfree(instance);
1916                 file->private_data = NULL;
1917         } break;
1918
1919         default:
1920                 vchiq_log_error(vchiq_arm_log_level,
1921                         "Unknown minor device: %d", dev);
1922                 ret = -ENXIO;
1923         }
1924
1925 out:
1926         return ret;
1927 }
1928
1929 /****************************************************************************
1930 *
1931 *   vchiq_dump
1932 *
1933 ***************************************************************************/
1934
1935 void
1936 vchiq_dump(void *dump_context, const char *str, int len)
1937 {
1938         DUMP_CONTEXT_T *context = (DUMP_CONTEXT_T *)dump_context;
1939
1940         if (context->actual < context->space) {
1941                 int copy_bytes;
1942
1943                 if (context->offset > 0) {
1944                         int skip_bytes = min(len, (int)context->offset);
1945
1946                         str += skip_bytes;
1947                         len -= skip_bytes;
1948                         context->offset -= skip_bytes;
1949                         if (context->offset > 0)
1950                                 return;
1951                 }
1952                 copy_bytes = min(len, (int)(context->space - context->actual));
1953                 if (copy_bytes == 0)
1954                         return;
1955                 if (copy_to_user(context->buf + context->actual, str,
1956                         copy_bytes))
1957                         context->actual = -EFAULT;
1958                 context->actual += copy_bytes;
1959                 len -= copy_bytes;
1960
1961                 /* If tne terminating NUL is included in the length, then it
1962                 ** marks the end of a line and should be replaced with a
1963                 ** carriage return. */
1964                 if ((len == 0) && (str[copy_bytes - 1] == '\0')) {
1965                         char cr = '\n';
1966
1967                         if (copy_to_user(context->buf + context->actual - 1,
1968                                 &cr, 1))
1969                                 context->actual = -EFAULT;
1970                 }
1971         }
1972 }
1973
1974 /****************************************************************************
1975 *
1976 *   vchiq_dump_platform_instance_state
1977 *
1978 ***************************************************************************/
1979
1980 void
1981 vchiq_dump_platform_instances(void *dump_context)
1982 {
1983         VCHIQ_STATE_T *state = vchiq_get_state();
1984         char buf[80];
1985         int len;
1986         int i;
1987
1988         /* There is no list of instances, so instead scan all services,
1989                 marking those that have been dumped. */
1990
1991         for (i = 0; i < state->unused_service; i++) {
1992                 VCHIQ_SERVICE_T *service = state->services[i];
1993                 VCHIQ_INSTANCE_T instance;
1994
1995                 if (service && (service->base.callback == service_callback)) {
1996                         instance = service->instance;
1997                         if (instance)
1998                                 instance->mark = 0;
1999                 }
2000         }
2001
2002         for (i = 0; i < state->unused_service; i++) {
2003                 VCHIQ_SERVICE_T *service = state->services[i];
2004                 VCHIQ_INSTANCE_T instance;
2005
2006                 if (service && (service->base.callback == service_callback)) {
2007                         instance = service->instance;
2008                         if (instance && !instance->mark) {
2009                                 len = snprintf(buf, sizeof(buf),
2010                                         "Instance %pK: pid %d,%s completions %d/%d",
2011                                         instance, instance->pid,
2012                                         instance->connected ? " connected, " :
2013                                                 "",
2014                                         instance->completion_insert -
2015                                                 instance->completion_remove,
2016                                         MAX_COMPLETIONS);
2017
2018                                 vchiq_dump(dump_context, buf, len + 1);
2019
2020                                 instance->mark = 1;
2021                         }
2022                 }
2023         }
2024 }
2025
2026 /****************************************************************************
2027 *
2028 *   vchiq_dump_platform_service_state
2029 *
2030 ***************************************************************************/
2031
2032 void
2033 vchiq_dump_platform_service_state(void *dump_context, VCHIQ_SERVICE_T *service)
2034 {
2035         USER_SERVICE_T *user_service = (USER_SERVICE_T *)service->base.userdata;
2036         char buf[80];
2037         int len;
2038
2039         len = snprintf(buf, sizeof(buf), "  instance %pK", service->instance);
2040
2041         if ((service->base.callback == service_callback) &&
2042                 user_service->is_vchi) {
2043                 len += snprintf(buf + len, sizeof(buf) - len,
2044                         ", %d/%d messages",
2045                         user_service->msg_insert - user_service->msg_remove,
2046                         MSG_QUEUE_SIZE);
2047
2048                 if (user_service->dequeue_pending)
2049                         len += snprintf(buf + len, sizeof(buf) - len,
2050                                 " (dequeue pending)");
2051         }
2052
2053         vchiq_dump(dump_context, buf, len + 1);
2054 }
2055
2056 /****************************************************************************
2057 *
2058 *   dump_user_mem
2059 *
2060 ***************************************************************************/
2061
2062 #if defined(CONFIG_BCM2835_VCHIQ_SUPPORT_MEMDUMP)
2063
2064 static void
2065 dump_phys_mem(void *virt_addr, u32 num_bytes)
2066 {
2067         int            rc;
2068         u8            *end_virt_addr = virt_addr + num_bytes;
2069         int            num_pages;
2070         int            offset;
2071         int            end_offset;
2072         int            page_idx;
2073         int            prev_idx;
2074         struct page   *page;
2075         struct page  **pages;
2076         u8            *kmapped_virt_ptr;
2077
2078         /* Align virt_addr and end_virt_addr to 16 byte boundaries. */
2079
2080         virt_addr = (void *)((unsigned long)virt_addr & ~0x0fuL);
2081         end_virt_addr = (void *)(((unsigned long)end_virt_addr + 15uL) &
2082                 ~0x0fuL);
2083
2084         offset = (int)(long)virt_addr & (PAGE_SIZE - 1);
2085         end_offset = (int)(long)end_virt_addr & (PAGE_SIZE - 1);
2086
2087         num_pages = DIV_ROUND_UP(offset + num_bytes, PAGE_SIZE);
2088
2089         pages = kmalloc(sizeof(struct page *) * num_pages, GFP_KERNEL);
2090         if (!pages) {
2091                 vchiq_log_error(vchiq_arm_log_level,
2092                         "Unable to allocation memory for %d pages\n",
2093                         num_pages);
2094                 return;
2095         }
2096
2097         down_read(&current->mm->mmap_sem);
2098         rc = get_user_pages(
2099                 (unsigned long)virt_addr, /* start */
2100                 num_pages,                /* len */
2101                 0,                        /* gup_flags */
2102                 pages,                    /* pages (array of page pointers) */
2103                 NULL);                    /* vmas */
2104         up_read(&current->mm->mmap_sem);
2105
2106         prev_idx = -1;
2107         page = NULL;
2108
2109         if (rc < 0) {
2110                 vchiq_log_error(vchiq_arm_log_level,
2111                                 "Failed to get user pages: %d\n", rc);
2112                 goto out;
2113         }
2114
2115         while (offset < end_offset) {
2116                 int page_offset = offset % PAGE_SIZE;
2117
2118                 page_idx = offset / PAGE_SIZE;
2119                 if (page_idx != prev_idx) {
2120                         if (page != NULL)
2121                                 kunmap(page);
2122                         page = pages[page_idx];
2123                         kmapped_virt_ptr = kmap(page);
2124                         prev_idx = page_idx;
2125                 }
2126
2127                 if (vchiq_arm_log_level >= VCHIQ_LOG_TRACE)
2128                         vchiq_log_dump_mem("ph",
2129                                 (u32)(unsigned long)&kmapped_virt_ptr[
2130                                         page_offset],
2131                                 &kmapped_virt_ptr[page_offset], 16);
2132
2133                 offset += 16;
2134         }
2135
2136 out:
2137         if (page != NULL)
2138                 kunmap(page);
2139
2140         for (page_idx = 0; page_idx < num_pages; page_idx++)
2141                 put_page(pages[page_idx]);
2142
2143         kfree(pages);
2144 }
2145
2146 #endif
2147
2148 /****************************************************************************
2149 *
2150 *   vchiq_read
2151 *
2152 ***************************************************************************/
2153
2154 static ssize_t
2155 vchiq_read(struct file *file, char __user *buf,
2156         size_t count, loff_t *ppos)
2157 {
2158         DUMP_CONTEXT_T context;
2159
2160         context.buf = buf;
2161         context.actual = 0;
2162         context.space = count;
2163         context.offset = *ppos;
2164
2165         vchiq_dump_state(&context, &g_state);
2166
2167         *ppos += context.actual;
2168
2169         return context.actual;
2170 }
2171
2172 VCHIQ_STATE_T *
2173 vchiq_get_state(void)
2174 {
2175
2176         if (g_state.remote == NULL)
2177                 printk(KERN_ERR "%s: g_state.remote == NULL\n", __func__);
2178         else if (g_state.remote->initialised != 1)
2179                 printk(KERN_NOTICE "%s: g_state.remote->initialised != 1 (%d)\n",
2180                         __func__, g_state.remote->initialised);
2181
2182         return ((g_state.remote != NULL) &&
2183                 (g_state.remote->initialised == 1)) ? &g_state : NULL;
2184 }
2185
2186 static const struct file_operations
2187 vchiq_fops = {
2188         .owner = THIS_MODULE,
2189         .unlocked_ioctl = vchiq_ioctl,
2190 #if defined(CONFIG_COMPAT)
2191         .compat_ioctl = vchiq_compat_ioctl,
2192 #endif
2193         .open = vchiq_open,
2194         .release = vchiq_release,
2195         .read = vchiq_read
2196 };
2197
2198 /*
2199  * Autosuspend related functionality
2200  */
2201
2202 int
2203 vchiq_videocore_wanted(VCHIQ_STATE_T *state)
2204 {
2205         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2206
2207         if (!arm_state)
2208                 /* autosuspend not supported - always return wanted */
2209                 return 1;
2210         else if (arm_state->blocked_count)
2211                 return 1;
2212         else if (!arm_state->videocore_use_count)
2213                 /* usage count zero - check for override unless we're forcing */
2214                 if (arm_state->resume_blocked)
2215                         return 0;
2216                 else
2217                         return vchiq_platform_videocore_wanted(state);
2218         else
2219                 /* non-zero usage count - videocore still required */
2220                 return 1;
2221 }
2222
2223 static VCHIQ_STATUS_T
2224 vchiq_keepalive_vchiq_callback(VCHIQ_REASON_T reason,
2225         VCHIQ_HEADER_T *header,
2226         VCHIQ_SERVICE_HANDLE_T service_user,
2227         void *bulk_user)
2228 {
2229         vchiq_log_error(vchiq_susp_log_level,
2230                 "%s callback reason %d", __func__, reason);
2231         return 0;
2232 }
2233
2234 static int
2235 vchiq_keepalive_thread_func(void *v)
2236 {
2237         VCHIQ_STATE_T *state = (VCHIQ_STATE_T *) v;
2238         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2239
2240         VCHIQ_STATUS_T status;
2241         VCHIQ_INSTANCE_T instance;
2242         VCHIQ_SERVICE_HANDLE_T ka_handle;
2243
2244         VCHIQ_SERVICE_PARAMS_T params = {
2245                 .fourcc      = VCHIQ_MAKE_FOURCC('K', 'E', 'E', 'P'),
2246                 .callback    = vchiq_keepalive_vchiq_callback,
2247                 .version     = KEEPALIVE_VER,
2248                 .version_min = KEEPALIVE_VER_MIN
2249         };
2250
2251         status = vchiq_initialise(&instance);
2252         if (status != VCHIQ_SUCCESS) {
2253                 vchiq_log_error(vchiq_susp_log_level,
2254                         "%s vchiq_initialise failed %d", __func__, status);
2255                 goto exit;
2256         }
2257
2258         status = vchiq_connect(instance);
2259         if (status != VCHIQ_SUCCESS) {
2260                 vchiq_log_error(vchiq_susp_log_level,
2261                         "%s vchiq_connect failed %d", __func__, status);
2262                 goto shutdown;
2263         }
2264
2265         status = vchiq_add_service(instance, &params, &ka_handle);
2266         if (status != VCHIQ_SUCCESS) {
2267                 vchiq_log_error(vchiq_susp_log_level,
2268                         "%s vchiq_open_service failed %d", __func__, status);
2269                 goto shutdown;
2270         }
2271
2272         while (1) {
2273                 long rc = 0, uc = 0;
2274
2275                 if (wait_for_completion_interruptible(&arm_state->ka_evt)
2276                                 != 0) {
2277                         vchiq_log_error(vchiq_susp_log_level,
2278                                 "%s interrupted", __func__);
2279                         flush_signals(current);
2280                         continue;
2281                 }
2282
2283                 /* read and clear counters.  Do release_count then use_count to
2284                  * prevent getting more releases than uses */
2285                 rc = atomic_xchg(&arm_state->ka_release_count, 0);
2286                 uc = atomic_xchg(&arm_state->ka_use_count, 0);
2287
2288                 /* Call use/release service the requisite number of times.
2289                  * Process use before release so use counts don't go negative */
2290                 while (uc--) {
2291                         atomic_inc(&arm_state->ka_use_ack_count);
2292                         status = vchiq_use_service(ka_handle);
2293                         if (status != VCHIQ_SUCCESS) {
2294                                 vchiq_log_error(vchiq_susp_log_level,
2295                                         "%s vchiq_use_service error %d",
2296                                         __func__, status);
2297                         }
2298                 }
2299                 while (rc--) {
2300                         status = vchiq_release_service(ka_handle);
2301                         if (status != VCHIQ_SUCCESS) {
2302                                 vchiq_log_error(vchiq_susp_log_level,
2303                                         "%s vchiq_release_service error %d",
2304                                         __func__, status);
2305                         }
2306                 }
2307         }
2308
2309 shutdown:
2310         vchiq_shutdown(instance);
2311 exit:
2312         return 0;
2313 }
2314
2315
2316
2317 VCHIQ_STATUS_T
2318 vchiq_arm_init_state(VCHIQ_STATE_T *state, VCHIQ_ARM_STATE_T *arm_state)
2319 {
2320         if (arm_state) {
2321                 rwlock_init(&arm_state->susp_res_lock);
2322
2323                 init_completion(&arm_state->ka_evt);
2324                 atomic_set(&arm_state->ka_use_count, 0);
2325                 atomic_set(&arm_state->ka_use_ack_count, 0);
2326                 atomic_set(&arm_state->ka_release_count, 0);
2327
2328                 init_completion(&arm_state->vc_suspend_complete);
2329
2330                 init_completion(&arm_state->vc_resume_complete);
2331                 /* Initialise to 'done' state.  We only want to block on resume
2332                  * completion while videocore is suspended. */
2333                 set_resume_state(arm_state, VC_RESUME_RESUMED);
2334
2335                 init_completion(&arm_state->resume_blocker);
2336                 /* Initialise to 'done' state.  We only want to block on this
2337                  * completion while resume is blocked */
2338                 complete_all(&arm_state->resume_blocker);
2339
2340                 init_completion(&arm_state->blocked_blocker);
2341                 /* Initialise to 'done' state.  We only want to block on this
2342                  * completion while things are waiting on the resume blocker */
2343                 complete_all(&arm_state->blocked_blocker);
2344
2345                 arm_state->suspend_timer_timeout = SUSPEND_TIMER_TIMEOUT_MS;
2346                 arm_state->suspend_timer_running = 0;
2347                 setup_timer(&arm_state->suspend_timer, suspend_timer_callback,
2348                             (unsigned long)(state));
2349
2350                 arm_state->first_connect = 0;
2351
2352         }
2353         return VCHIQ_SUCCESS;
2354 }
2355
2356 /*
2357 ** Functions to modify the state variables;
2358 **      set_suspend_state
2359 **      set_resume_state
2360 **
2361 ** There are more state variables than we might like, so ensure they remain in
2362 ** step.  Suspend and resume state are maintained separately, since most of
2363 ** these state machines can operate independently.  However, there are a few
2364 ** states where state transitions in one state machine cause a reset to the
2365 ** other state machine.  In addition, there are some completion events which
2366 ** need to occur on state machine reset and end-state(s), so these are also
2367 ** dealt with in these functions.
2368 **
2369 ** In all states we set the state variable according to the input, but in some
2370 ** cases we perform additional steps outlined below;
2371 **
2372 ** VC_SUSPEND_IDLE - Initialise the suspend completion at the same time.
2373 **                      The suspend completion is completed after any suspend
2374 **                      attempt.  When we reset the state machine we also reset
2375 **                      the completion.  This reset occurs when videocore is
2376 **                      resumed, and also if we initiate suspend after a suspend
2377 **                      failure.
2378 **
2379 ** VC_SUSPEND_IN_PROGRESS - This state is considered the point of no return for
2380 **                      suspend - ie from this point on we must try to suspend
2381 **                      before resuming can occur.  We therefore also reset the
2382 **                      resume state machine to VC_RESUME_IDLE in this state.
2383 **
2384 ** VC_SUSPEND_SUSPENDED - Suspend has completed successfully. Also call
2385 **                      complete_all on the suspend completion to notify
2386 **                      anything waiting for suspend to happen.
2387 **
2388 ** VC_SUSPEND_REJECTED - Videocore rejected suspend. Videocore will also
2389 **                      initiate resume, so no need to alter resume state.
2390 **                      We call complete_all on the suspend completion to notify
2391 **                      of suspend rejection.
2392 **
2393 ** VC_SUSPEND_FAILED - We failed to initiate videocore suspend.  We notify the
2394 **                      suspend completion and reset the resume state machine.
2395 **
2396 ** VC_RESUME_IDLE - Initialise the resume completion at the same time.  The
2397 **                      resume completion is in it's 'done' state whenever
2398 **                      videcore is running.  Therefore, the VC_RESUME_IDLE
2399 **                      state implies that videocore is suspended.
2400 **                      Hence, any thread which needs to wait until videocore is
2401 **                      running can wait on this completion - it will only block
2402 **                      if videocore is suspended.
2403 **
2404 ** VC_RESUME_RESUMED - Resume has completed successfully.  Videocore is running.
2405 **                      Call complete_all on the resume completion to unblock
2406 **                      any threads waiting for resume.  Also reset the suspend
2407 **                      state machine to it's idle state.
2408 **
2409 ** VC_RESUME_FAILED - Currently unused - no mechanism to fail resume exists.
2410 */
2411
2412 void
2413 set_suspend_state(VCHIQ_ARM_STATE_T *arm_state,
2414         enum vc_suspend_status new_state)
2415 {
2416         /* set the state in all cases */
2417         arm_state->vc_suspend_state = new_state;
2418
2419         /* state specific additional actions */
2420         switch (new_state) {
2421         case VC_SUSPEND_FORCE_CANCELED:
2422                 complete_all(&arm_state->vc_suspend_complete);
2423                 break;
2424         case VC_SUSPEND_REJECTED:
2425                 complete_all(&arm_state->vc_suspend_complete);
2426                 break;
2427         case VC_SUSPEND_FAILED:
2428                 complete_all(&arm_state->vc_suspend_complete);
2429                 arm_state->vc_resume_state = VC_RESUME_RESUMED;
2430                 complete_all(&arm_state->vc_resume_complete);
2431                 break;
2432         case VC_SUSPEND_IDLE:
2433                 reinit_completion(&arm_state->vc_suspend_complete);
2434                 break;
2435         case VC_SUSPEND_REQUESTED:
2436                 break;
2437         case VC_SUSPEND_IN_PROGRESS:
2438                 set_resume_state(arm_state, VC_RESUME_IDLE);
2439                 break;
2440         case VC_SUSPEND_SUSPENDED:
2441                 complete_all(&arm_state->vc_suspend_complete);
2442                 break;
2443         default:
2444                 BUG();
2445                 break;
2446         }
2447 }
2448
2449 void
2450 set_resume_state(VCHIQ_ARM_STATE_T *arm_state,
2451         enum vc_resume_status new_state)
2452 {
2453         /* set the state in all cases */
2454         arm_state->vc_resume_state = new_state;
2455
2456         /* state specific additional actions */
2457         switch (new_state) {
2458         case VC_RESUME_FAILED:
2459                 break;
2460         case VC_RESUME_IDLE:
2461                 reinit_completion(&arm_state->vc_resume_complete);
2462                 break;
2463         case VC_RESUME_REQUESTED:
2464                 break;
2465         case VC_RESUME_IN_PROGRESS:
2466                 break;
2467         case VC_RESUME_RESUMED:
2468                 complete_all(&arm_state->vc_resume_complete);
2469                 set_suspend_state(arm_state, VC_SUSPEND_IDLE);
2470                 break;
2471         default:
2472                 BUG();
2473                 break;
2474         }
2475 }
2476
2477
2478 /* should be called with the write lock held */
2479 inline void
2480 start_suspend_timer(VCHIQ_ARM_STATE_T *arm_state)
2481 {
2482         del_timer(&arm_state->suspend_timer);
2483         arm_state->suspend_timer.expires = jiffies +
2484                 msecs_to_jiffies(arm_state->
2485                         suspend_timer_timeout);
2486         add_timer(&arm_state->suspend_timer);
2487         arm_state->suspend_timer_running = 1;
2488 }
2489
2490 /* should be called with the write lock held */
2491 static inline void
2492 stop_suspend_timer(VCHIQ_ARM_STATE_T *arm_state)
2493 {
2494         if (arm_state->suspend_timer_running) {
2495                 del_timer(&arm_state->suspend_timer);
2496                 arm_state->suspend_timer_running = 0;
2497         }
2498 }
2499
2500 static inline int
2501 need_resume(VCHIQ_STATE_T *state)
2502 {
2503         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2504
2505         return (arm_state->vc_suspend_state > VC_SUSPEND_IDLE) &&
2506                         (arm_state->vc_resume_state < VC_RESUME_REQUESTED) &&
2507                         vchiq_videocore_wanted(state);
2508 }
2509
2510 static int
2511 block_resume(VCHIQ_ARM_STATE_T *arm_state)
2512 {
2513         int status = VCHIQ_SUCCESS;
2514         const unsigned long timeout_val =
2515                                 msecs_to_jiffies(FORCE_SUSPEND_TIMEOUT_MS);
2516         int resume_count = 0;
2517
2518         /* Allow any threads which were blocked by the last force suspend to
2519          * complete if they haven't already.  Only give this one shot; if
2520          * blocked_count is incremented after blocked_blocker is completed
2521          * (which only happens when blocked_count hits 0) then those threads
2522          * will have to wait until next time around */
2523         if (arm_state->blocked_count) {
2524                 reinit_completion(&arm_state->blocked_blocker);
2525                 write_unlock_bh(&arm_state->susp_res_lock);
2526                 vchiq_log_info(vchiq_susp_log_level, "%s wait for previously "
2527                         "blocked clients", __func__);
2528                 if (wait_for_completion_interruptible_timeout(
2529                                 &arm_state->blocked_blocker, timeout_val)
2530                                         <= 0) {
2531                         vchiq_log_error(vchiq_susp_log_level, "%s wait for "
2532                                 "previously blocked clients failed", __func__);
2533                         status = VCHIQ_ERROR;
2534                         write_lock_bh(&arm_state->susp_res_lock);
2535                         goto out;
2536                 }
2537                 vchiq_log_info(vchiq_susp_log_level, "%s previously blocked "
2538                         "clients resumed", __func__);
2539                 write_lock_bh(&arm_state->susp_res_lock);
2540         }
2541
2542         /* We need to wait for resume to complete if it's in process */
2543         while (arm_state->vc_resume_state != VC_RESUME_RESUMED &&
2544                         arm_state->vc_resume_state > VC_RESUME_IDLE) {
2545                 if (resume_count > 1) {
2546                         status = VCHIQ_ERROR;
2547                         vchiq_log_error(vchiq_susp_log_level, "%s waited too "
2548                                 "many times for resume", __func__);
2549                         goto out;
2550                 }
2551                 write_unlock_bh(&arm_state->susp_res_lock);
2552                 vchiq_log_info(vchiq_susp_log_level, "%s wait for resume",
2553                         __func__);
2554                 if (wait_for_completion_interruptible_timeout(
2555                                 &arm_state->vc_resume_complete, timeout_val)
2556                                         <= 0) {
2557                         vchiq_log_error(vchiq_susp_log_level, "%s wait for "
2558                                 "resume failed (%s)", __func__,
2559                                 resume_state_names[arm_state->vc_resume_state +
2560                                                         VC_RESUME_NUM_OFFSET]);
2561                         status = VCHIQ_ERROR;
2562                         write_lock_bh(&arm_state->susp_res_lock);
2563                         goto out;
2564                 }
2565                 vchiq_log_info(vchiq_susp_log_level, "%s resumed", __func__);
2566                 write_lock_bh(&arm_state->susp_res_lock);
2567                 resume_count++;
2568         }
2569         reinit_completion(&arm_state->resume_blocker);
2570         arm_state->resume_blocked = 1;
2571
2572 out:
2573         return status;
2574 }
2575
2576 static inline void
2577 unblock_resume(VCHIQ_ARM_STATE_T *arm_state)
2578 {
2579         complete_all(&arm_state->resume_blocker);
2580         arm_state->resume_blocked = 0;
2581 }
2582
2583 /* Initiate suspend via slot handler. Should be called with the write lock
2584  * held */
2585 VCHIQ_STATUS_T
2586 vchiq_arm_vcsuspend(VCHIQ_STATE_T *state)
2587 {
2588         VCHIQ_STATUS_T status = VCHIQ_ERROR;
2589         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2590
2591         if (!arm_state)
2592                 goto out;
2593
2594         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2595         status = VCHIQ_SUCCESS;
2596
2597
2598         switch (arm_state->vc_suspend_state) {
2599         case VC_SUSPEND_REQUESTED:
2600                 vchiq_log_info(vchiq_susp_log_level, "%s: suspend already "
2601                         "requested", __func__);
2602                 break;
2603         case VC_SUSPEND_IN_PROGRESS:
2604                 vchiq_log_info(vchiq_susp_log_level, "%s: suspend already in "
2605                         "progress", __func__);
2606                 break;
2607
2608         default:
2609                 /* We don't expect to be in other states, so log but continue
2610                  * anyway */
2611                 vchiq_log_error(vchiq_susp_log_level,
2612                         "%s unexpected suspend state %s", __func__,
2613                         suspend_state_names[arm_state->vc_suspend_state +
2614                                                 VC_SUSPEND_NUM_OFFSET]);
2615                 /* fall through */
2616         case VC_SUSPEND_REJECTED:
2617         case VC_SUSPEND_FAILED:
2618                 /* Ensure any idle state actions have been run */
2619                 set_suspend_state(arm_state, VC_SUSPEND_IDLE);
2620                 /* fall through */
2621         case VC_SUSPEND_IDLE:
2622                 vchiq_log_info(vchiq_susp_log_level,
2623                         "%s: suspending", __func__);
2624                 set_suspend_state(arm_state, VC_SUSPEND_REQUESTED);
2625                 /* kick the slot handler thread to initiate suspend */
2626                 request_poll(state, NULL, 0);
2627                 break;
2628         }
2629
2630 out:
2631         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, status);
2632         return status;
2633 }
2634
2635 void
2636 vchiq_platform_check_suspend(VCHIQ_STATE_T *state)
2637 {
2638         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2639         int susp = 0;
2640
2641         if (!arm_state)
2642                 goto out;
2643
2644         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2645
2646         write_lock_bh(&arm_state->susp_res_lock);
2647         if (arm_state->vc_suspend_state == VC_SUSPEND_REQUESTED &&
2648                         arm_state->vc_resume_state == VC_RESUME_RESUMED) {
2649                 set_suspend_state(arm_state, VC_SUSPEND_IN_PROGRESS);
2650                 susp = 1;
2651         }
2652         write_unlock_bh(&arm_state->susp_res_lock);
2653
2654         if (susp)
2655                 vchiq_platform_suspend(state);
2656
2657 out:
2658         vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
2659         return;
2660 }
2661
2662
2663 static void
2664 output_timeout_error(VCHIQ_STATE_T *state)
2665 {
2666         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2667         char err[50] = "";
2668         int vc_use_count = arm_state->videocore_use_count;
2669         int active_services = state->unused_service;
2670         int i;
2671
2672         if (!arm_state->videocore_use_count) {
2673                 snprintf(err, sizeof(err), " Videocore usecount is 0");
2674                 goto output_msg;
2675         }
2676         for (i = 0; i < active_services; i++) {
2677                 VCHIQ_SERVICE_T *service_ptr = state->services[i];
2678
2679                 if (service_ptr && service_ptr->service_use_count &&
2680                         (service_ptr->srvstate != VCHIQ_SRVSTATE_FREE)) {
2681                         snprintf(err, sizeof(err), " %c%c%c%c(%d) service has "
2682                                 "use count %d%s", VCHIQ_FOURCC_AS_4CHARS(
2683                                         service_ptr->base.fourcc),
2684                                  service_ptr->client_id,
2685                                  service_ptr->service_use_count,
2686                                  service_ptr->service_use_count ==
2687                                          vc_use_count ? "" : " (+ more)");
2688                         break;
2689                 }
2690         }
2691
2692 output_msg:
2693         vchiq_log_error(vchiq_susp_log_level,
2694                 "timed out waiting for vc suspend (%d).%s",
2695                  arm_state->autosuspend_override, err);
2696
2697 }
2698
2699 /* Try to get videocore into suspended state, regardless of autosuspend state.
2700 ** We don't actually force suspend, since videocore may get into a bad state
2701 ** if we force suspend at a bad time.  Instead, we wait for autosuspend to
2702 ** determine a good point to suspend.  If this doesn't happen within 100ms we
2703 ** report failure.
2704 **
2705 ** Returns VCHIQ_SUCCESS if videocore suspended successfully, VCHIQ_RETRY if
2706 ** videocore failed to suspend in time or VCHIQ_ERROR if interrupted.
2707 */
2708 VCHIQ_STATUS_T
2709 vchiq_arm_force_suspend(VCHIQ_STATE_T *state)
2710 {
2711         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2712         VCHIQ_STATUS_T status = VCHIQ_ERROR;
2713         long rc = 0;
2714         int repeat = -1;
2715
2716         if (!arm_state)
2717                 goto out;
2718
2719         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2720
2721         write_lock_bh(&arm_state->susp_res_lock);
2722
2723         status = block_resume(arm_state);
2724         if (status != VCHIQ_SUCCESS)
2725                 goto unlock;
2726         if (arm_state->vc_suspend_state == VC_SUSPEND_SUSPENDED) {
2727                 /* Already suspended - just block resume and exit */
2728                 vchiq_log_info(vchiq_susp_log_level, "%s already suspended",
2729                         __func__);
2730                 status = VCHIQ_SUCCESS;
2731                 goto unlock;
2732         } else if (arm_state->vc_suspend_state <= VC_SUSPEND_IDLE) {
2733                 /* initiate suspend immediately in the case that we're waiting
2734                  * for the timeout */
2735                 stop_suspend_timer(arm_state);
2736                 if (!vchiq_videocore_wanted(state)) {
2737                         vchiq_log_info(vchiq_susp_log_level, "%s videocore "
2738                                 "idle, initiating suspend", __func__);
2739                         status = vchiq_arm_vcsuspend(state);
2740                 } else if (arm_state->autosuspend_override <
2741                                                 FORCE_SUSPEND_FAIL_MAX) {
2742                         vchiq_log_info(vchiq_susp_log_level, "%s letting "
2743                                 "videocore go idle", __func__);
2744                         status = VCHIQ_SUCCESS;
2745                 } else {
2746                         vchiq_log_warning(vchiq_susp_log_level, "%s failed too "
2747                                 "many times - attempting suspend", __func__);
2748                         status = vchiq_arm_vcsuspend(state);
2749                 }
2750         } else {
2751                 vchiq_log_info(vchiq_susp_log_level, "%s videocore suspend "
2752                         "in progress - wait for completion", __func__);
2753                 status = VCHIQ_SUCCESS;
2754         }
2755
2756         /* Wait for suspend to happen due to system idle (not forced..) */
2757         if (status != VCHIQ_SUCCESS)
2758                 goto unblock_resume;
2759
2760         do {
2761                 write_unlock_bh(&arm_state->susp_res_lock);
2762
2763                 rc = wait_for_completion_interruptible_timeout(
2764                                 &arm_state->vc_suspend_complete,
2765                                 msecs_to_jiffies(FORCE_SUSPEND_TIMEOUT_MS));
2766
2767                 write_lock_bh(&arm_state->susp_res_lock);
2768                 if (rc < 0) {
2769                         vchiq_log_warning(vchiq_susp_log_level, "%s "
2770                                 "interrupted waiting for suspend", __func__);
2771                         status = VCHIQ_ERROR;
2772                         goto unblock_resume;
2773                 } else if (rc == 0) {
2774                         if (arm_state->vc_suspend_state > VC_SUSPEND_IDLE) {
2775                                 /* Repeat timeout once if in progress */
2776                                 if (repeat < 0) {
2777                                         repeat = 1;
2778                                         continue;
2779                                 }
2780                         }
2781                         arm_state->autosuspend_override++;
2782                         output_timeout_error(state);
2783
2784                         status = VCHIQ_RETRY;
2785                         goto unblock_resume;
2786                 }
2787         } while (0 < (repeat--));
2788
2789         /* Check and report state in case we need to abort ARM suspend */
2790         if (arm_state->vc_suspend_state != VC_SUSPEND_SUSPENDED) {
2791                 status = VCHIQ_RETRY;
2792                 vchiq_log_error(vchiq_susp_log_level,
2793                         "%s videocore suspend failed (state %s)", __func__,
2794                         suspend_state_names[arm_state->vc_suspend_state +
2795                                                 VC_SUSPEND_NUM_OFFSET]);
2796                 /* Reset the state only if it's still in an error state.
2797                  * Something could have already initiated another suspend. */
2798                 if (arm_state->vc_suspend_state < VC_SUSPEND_IDLE)
2799                         set_suspend_state(arm_state, VC_SUSPEND_IDLE);
2800
2801                 goto unblock_resume;
2802         }
2803
2804         /* successfully suspended - unlock and exit */
2805         goto unlock;
2806
2807 unblock_resume:
2808         /* all error states need to unblock resume before exit */
2809         unblock_resume(arm_state);
2810
2811 unlock:
2812         write_unlock_bh(&arm_state->susp_res_lock);
2813
2814 out:
2815         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, status);
2816         return status;
2817 }
2818
2819 void
2820 vchiq_check_suspend(VCHIQ_STATE_T *state)
2821 {
2822         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2823
2824         if (!arm_state)
2825                 goto out;
2826
2827         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2828
2829         write_lock_bh(&arm_state->susp_res_lock);
2830         if (arm_state->vc_suspend_state != VC_SUSPEND_SUSPENDED &&
2831                         arm_state->first_connect &&
2832                         !vchiq_videocore_wanted(state)) {
2833                 vchiq_arm_vcsuspend(state);
2834         }
2835         write_unlock_bh(&arm_state->susp_res_lock);
2836
2837 out:
2838         vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
2839         return;
2840 }
2841
2842
2843 int
2844 vchiq_arm_allow_resume(VCHIQ_STATE_T *state)
2845 {
2846         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2847         int resume = 0;
2848         int ret = -1;
2849
2850         if (!arm_state)
2851                 goto out;
2852
2853         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2854
2855         write_lock_bh(&arm_state->susp_res_lock);
2856         unblock_resume(arm_state);
2857         resume = vchiq_check_resume(state);
2858         write_unlock_bh(&arm_state->susp_res_lock);
2859
2860         if (resume) {
2861                 if (wait_for_completion_interruptible(
2862                         &arm_state->vc_resume_complete) < 0) {
2863                         vchiq_log_error(vchiq_susp_log_level,
2864                                 "%s interrupted", __func__);
2865                         /* failed, cannot accurately derive suspend
2866                          * state, so exit early. */
2867                         goto out;
2868                 }
2869         }
2870
2871         read_lock_bh(&arm_state->susp_res_lock);
2872         if (arm_state->vc_suspend_state == VC_SUSPEND_SUSPENDED) {
2873                 vchiq_log_info(vchiq_susp_log_level,
2874                                 "%s: Videocore remains suspended", __func__);
2875         } else {
2876                 vchiq_log_info(vchiq_susp_log_level,
2877                                 "%s: Videocore resumed", __func__);
2878                 ret = 0;
2879         }
2880         read_unlock_bh(&arm_state->susp_res_lock);
2881 out:
2882         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
2883         return ret;
2884 }
2885
2886 /* This function should be called with the write lock held */
2887 int
2888 vchiq_check_resume(VCHIQ_STATE_T *state)
2889 {
2890         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2891         int resume = 0;
2892
2893         if (!arm_state)
2894                 goto out;
2895
2896         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2897
2898         if (need_resume(state)) {
2899                 set_resume_state(arm_state, VC_RESUME_REQUESTED);
2900                 request_poll(state, NULL, 0);
2901                 resume = 1;
2902         }
2903
2904 out:
2905         vchiq_log_trace(vchiq_susp_log_level, "%s exit", __func__);
2906         return resume;
2907 }
2908
2909 VCHIQ_STATUS_T
2910 vchiq_use_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service,
2911                 enum USE_TYPE_E use_type)
2912 {
2913         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
2914         VCHIQ_STATUS_T ret = VCHIQ_SUCCESS;
2915         char entity[16];
2916         int *entity_uc;
2917         int local_uc, local_entity_uc;
2918
2919         if (!arm_state)
2920                 goto out;
2921
2922         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
2923
2924         if (use_type == USE_TYPE_VCHIQ) {
2925                 sprintf(entity, "VCHIQ:   ");
2926                 entity_uc = &arm_state->peer_use_count;
2927         } else if (service) {
2928                 sprintf(entity, "%c%c%c%c:%03d",
2929                         VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
2930                         service->client_id);
2931                 entity_uc = &service->service_use_count;
2932         } else {
2933                 vchiq_log_error(vchiq_susp_log_level, "%s null service "
2934                                 "ptr", __func__);
2935                 ret = VCHIQ_ERROR;
2936                 goto out;
2937         }
2938
2939         write_lock_bh(&arm_state->susp_res_lock);
2940         while (arm_state->resume_blocked) {
2941                 /* If we call 'use' while force suspend is waiting for suspend,
2942                  * then we're about to block the thread which the force is
2943                  * waiting to complete, so we're bound to just time out. In this
2944                  * case, set the suspend state such that the wait will be
2945                  * canceled, so we can complete as quickly as possible. */
2946                 if (arm_state->resume_blocked && arm_state->vc_suspend_state ==
2947                                 VC_SUSPEND_IDLE) {
2948                         set_suspend_state(arm_state, VC_SUSPEND_FORCE_CANCELED);
2949                         break;
2950                 }
2951                 /* If suspend is already in progress then we need to block */
2952                 if (!try_wait_for_completion(&arm_state->resume_blocker)) {
2953                         /* Indicate that there are threads waiting on the resume
2954                          * blocker.  These need to be allowed to complete before
2955                          * a _second_ call to force suspend can complete,
2956                          * otherwise low priority threads might never actually
2957                          * continue */
2958                         arm_state->blocked_count++;
2959                         write_unlock_bh(&arm_state->susp_res_lock);
2960                         vchiq_log_info(vchiq_susp_log_level, "%s %s resume "
2961                                 "blocked - waiting...", __func__, entity);
2962                         if (wait_for_completion_killable(
2963                                         &arm_state->resume_blocker) != 0) {
2964                                 vchiq_log_error(vchiq_susp_log_level, "%s %s "
2965                                         "wait for resume blocker interrupted",
2966                                         __func__, entity);
2967                                 ret = VCHIQ_ERROR;
2968                                 write_lock_bh(&arm_state->susp_res_lock);
2969                                 arm_state->blocked_count--;
2970                                 write_unlock_bh(&arm_state->susp_res_lock);
2971                                 goto out;
2972                         }
2973                         vchiq_log_info(vchiq_susp_log_level, "%s %s resume "
2974                                 "unblocked", __func__, entity);
2975                         write_lock_bh(&arm_state->susp_res_lock);
2976                         if (--arm_state->blocked_count == 0)
2977                                 complete_all(&arm_state->blocked_blocker);
2978                 }
2979         }
2980
2981         stop_suspend_timer(arm_state);
2982
2983         local_uc = ++arm_state->videocore_use_count;
2984         local_entity_uc = ++(*entity_uc);
2985
2986         /* If there's a pending request which hasn't yet been serviced then
2987          * just clear it.  If we're past VC_SUSPEND_REQUESTED state then
2988          * vc_resume_complete will block until we either resume or fail to
2989          * suspend */
2990         if (arm_state->vc_suspend_state <= VC_SUSPEND_REQUESTED)
2991                 set_suspend_state(arm_state, VC_SUSPEND_IDLE);
2992
2993         if ((use_type != USE_TYPE_SERVICE_NO_RESUME) && need_resume(state)) {
2994                 set_resume_state(arm_state, VC_RESUME_REQUESTED);
2995                 vchiq_log_info(vchiq_susp_log_level,
2996                         "%s %s count %d, state count %d",
2997                         __func__, entity, local_entity_uc, local_uc);
2998                 request_poll(state, NULL, 0);
2999         } else
3000                 vchiq_log_trace(vchiq_susp_log_level,
3001                         "%s %s count %d, state count %d",
3002                         __func__, entity, *entity_uc, local_uc);
3003
3004
3005         write_unlock_bh(&arm_state->susp_res_lock);
3006
3007         /* Completion is in a done state when we're not suspended, so this won't
3008          * block for the non-suspended case. */
3009         if (!try_wait_for_completion(&arm_state->vc_resume_complete)) {
3010                 vchiq_log_info(vchiq_susp_log_level, "%s %s wait for resume",
3011                         __func__, entity);
3012                 if (wait_for_completion_killable(
3013                                 &arm_state->vc_resume_complete) != 0) {
3014                         vchiq_log_error(vchiq_susp_log_level, "%s %s wait for "
3015                                 "resume interrupted", __func__, entity);
3016                         ret = VCHIQ_ERROR;
3017                         goto out;
3018                 }
3019                 vchiq_log_info(vchiq_susp_log_level, "%s %s resumed", __func__,
3020                         entity);
3021         }
3022
3023         if (ret == VCHIQ_SUCCESS) {
3024                 VCHIQ_STATUS_T status = VCHIQ_SUCCESS;
3025                 long ack_cnt = atomic_xchg(&arm_state->ka_use_ack_count, 0);
3026
3027                 while (ack_cnt && (status == VCHIQ_SUCCESS)) {
3028                         /* Send the use notify to videocore */
3029                         status = vchiq_send_remote_use_active(state);
3030                         if (status == VCHIQ_SUCCESS)
3031                                 ack_cnt--;
3032                         else
3033                                 atomic_add(ack_cnt,
3034                                         &arm_state->ka_use_ack_count);
3035                 }
3036         }
3037
3038 out:
3039         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
3040         return ret;
3041 }
3042
3043 VCHIQ_STATUS_T
3044 vchiq_release_internal(VCHIQ_STATE_T *state, VCHIQ_SERVICE_T *service)
3045 {
3046         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
3047         VCHIQ_STATUS_T ret = VCHIQ_SUCCESS;
3048         char entity[16];
3049         int *entity_uc;
3050         int local_uc, local_entity_uc;
3051
3052         if (!arm_state)
3053                 goto out;
3054
3055         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
3056
3057         if (service) {
3058                 sprintf(entity, "%c%c%c%c:%03d",
3059                         VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
3060                         service->client_id);
3061                 entity_uc = &service->service_use_count;
3062         } else {
3063                 sprintf(entity, "PEER:   ");
3064                 entity_uc = &arm_state->peer_use_count;
3065         }
3066
3067         write_lock_bh(&arm_state->susp_res_lock);
3068         if (!arm_state->videocore_use_count || !(*entity_uc)) {
3069                 /* Don't use BUG_ON - don't allow user thread to crash kernel */
3070                 WARN_ON(!arm_state->videocore_use_count);
3071                 WARN_ON(!(*entity_uc));
3072                 ret = VCHIQ_ERROR;
3073                 goto unlock;
3074         }
3075         local_uc = --arm_state->videocore_use_count;
3076         local_entity_uc = --(*entity_uc);
3077
3078         if (!vchiq_videocore_wanted(state)) {
3079                 if (vchiq_platform_use_suspend_timer() &&
3080                                 !arm_state->resume_blocked) {
3081                         /* Only use the timer if we're not trying to force
3082                          * suspend (=> resume_blocked) */
3083                         start_suspend_timer(arm_state);
3084                 } else {
3085                         vchiq_log_info(vchiq_susp_log_level,
3086                                 "%s %s count %d, state count %d - suspending",
3087                                 __func__, entity, *entity_uc,
3088                                 arm_state->videocore_use_count);
3089                         vchiq_arm_vcsuspend(state);
3090                 }
3091         } else
3092                 vchiq_log_trace(vchiq_susp_log_level,
3093                         "%s %s count %d, state count %d",
3094                         __func__, entity, *entity_uc,
3095                         arm_state->videocore_use_count);
3096
3097 unlock:
3098         write_unlock_bh(&arm_state->susp_res_lock);
3099
3100 out:
3101         vchiq_log_trace(vchiq_susp_log_level, "%s exit %d", __func__, ret);
3102         return ret;
3103 }
3104
3105 void
3106 vchiq_on_remote_use(VCHIQ_STATE_T *state)
3107 {
3108         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
3109
3110         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
3111         atomic_inc(&arm_state->ka_use_count);
3112         complete(&arm_state->ka_evt);
3113 }
3114
3115 void
3116 vchiq_on_remote_release(VCHIQ_STATE_T *state)
3117 {
3118         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
3119
3120         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
3121         atomic_inc(&arm_state->ka_release_count);
3122         complete(&arm_state->ka_evt);
3123 }
3124
3125 VCHIQ_STATUS_T
3126 vchiq_use_service_internal(VCHIQ_SERVICE_T *service)
3127 {
3128         return vchiq_use_internal(service->state, service, USE_TYPE_SERVICE);
3129 }
3130
3131 VCHIQ_STATUS_T
3132 vchiq_release_service_internal(VCHIQ_SERVICE_T *service)
3133 {
3134         return vchiq_release_internal(service->state, service);
3135 }
3136
3137 VCHIQ_DEBUGFS_NODE_T *
3138 vchiq_instance_get_debugfs_node(VCHIQ_INSTANCE_T instance)
3139 {
3140         return &instance->debugfs_node;
3141 }
3142
3143 int
3144 vchiq_instance_get_use_count(VCHIQ_INSTANCE_T instance)
3145 {
3146         VCHIQ_SERVICE_T *service;
3147         int use_count = 0, i;
3148
3149         i = 0;
3150         while ((service = next_service_by_instance(instance->state,
3151                 instance, &i)) != NULL) {
3152                 use_count += service->service_use_count;
3153                 unlock_service(service);
3154         }
3155         return use_count;
3156 }
3157
3158 int
3159 vchiq_instance_get_pid(VCHIQ_INSTANCE_T instance)
3160 {
3161         return instance->pid;
3162 }
3163
3164 int
3165 vchiq_instance_get_trace(VCHIQ_INSTANCE_T instance)
3166 {
3167         return instance->trace;
3168 }
3169
3170 void
3171 vchiq_instance_set_trace(VCHIQ_INSTANCE_T instance, int trace)
3172 {
3173         VCHIQ_SERVICE_T *service;
3174         int i;
3175
3176         i = 0;
3177         while ((service = next_service_by_instance(instance->state,
3178                 instance, &i)) != NULL) {
3179                 service->trace = trace;
3180                 unlock_service(service);
3181         }
3182         instance->trace = (trace != 0);
3183 }
3184
3185 static void suspend_timer_callback(unsigned long context)
3186 {
3187         VCHIQ_STATE_T *state = (VCHIQ_STATE_T *)context;
3188         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
3189
3190         if (!arm_state)
3191                 goto out;
3192         vchiq_log_info(vchiq_susp_log_level,
3193                 "%s - suspend timer expired - check suspend", __func__);
3194         vchiq_check_suspend(state);
3195 out:
3196         return;
3197 }
3198
3199 VCHIQ_STATUS_T
3200 vchiq_use_service_no_resume(VCHIQ_SERVICE_HANDLE_T handle)
3201 {
3202         VCHIQ_STATUS_T ret = VCHIQ_ERROR;
3203         VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3204
3205         if (service) {
3206                 ret = vchiq_use_internal(service->state, service,
3207                                 USE_TYPE_SERVICE_NO_RESUME);
3208                 unlock_service(service);
3209         }
3210         return ret;
3211 }
3212
3213 VCHIQ_STATUS_T
3214 vchiq_use_service(VCHIQ_SERVICE_HANDLE_T handle)
3215 {
3216         VCHIQ_STATUS_T ret = VCHIQ_ERROR;
3217         VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3218
3219         if (service) {
3220                 ret = vchiq_use_internal(service->state, service,
3221                                 USE_TYPE_SERVICE);
3222                 unlock_service(service);
3223         }
3224         return ret;
3225 }
3226
3227 VCHIQ_STATUS_T
3228 vchiq_release_service(VCHIQ_SERVICE_HANDLE_T handle)
3229 {
3230         VCHIQ_STATUS_T ret = VCHIQ_ERROR;
3231         VCHIQ_SERVICE_T *service = find_service_by_handle(handle);
3232
3233         if (service) {
3234                 ret = vchiq_release_internal(service->state, service);
3235                 unlock_service(service);
3236         }
3237         return ret;
3238 }
3239
3240 void
3241 vchiq_dump_service_use_state(VCHIQ_STATE_T *state)
3242 {
3243         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
3244         int i, j = 0;
3245         /* Only dump 64 services */
3246         static const int local_max_services = 64;
3247         /* If there's more than 64 services, only dump ones with
3248          * non-zero counts */
3249         int only_nonzero = 0;
3250         static const char *nz = "<-- preventing suspend";
3251
3252         enum vc_suspend_status vc_suspend_state;
3253         enum vc_resume_status  vc_resume_state;
3254         int peer_count;
3255         int vc_use_count;
3256         int active_services;
3257         struct service_data_struct {
3258                 int fourcc;
3259                 int clientid;
3260                 int use_count;
3261         } service_data[local_max_services];
3262
3263         if (!arm_state)
3264                 return;
3265
3266         read_lock_bh(&arm_state->susp_res_lock);
3267         vc_suspend_state = arm_state->vc_suspend_state;
3268         vc_resume_state  = arm_state->vc_resume_state;
3269         peer_count = arm_state->peer_use_count;
3270         vc_use_count = arm_state->videocore_use_count;
3271         active_services = state->unused_service;
3272         if (active_services > local_max_services)
3273                 only_nonzero = 1;
3274
3275         for (i = 0; (i < active_services) && (j < local_max_services); i++) {
3276                 VCHIQ_SERVICE_T *service_ptr = state->services[i];
3277
3278                 if (!service_ptr)
3279                         continue;
3280
3281                 if (only_nonzero && !service_ptr->service_use_count)
3282                         continue;
3283
3284                 if (service_ptr->srvstate == VCHIQ_SRVSTATE_FREE)
3285                         continue;
3286
3287                 service_data[j].fourcc = service_ptr->base.fourcc;
3288                 service_data[j].clientid = service_ptr->client_id;
3289                 service_data[j++].use_count = service_ptr->service_use_count;
3290         }
3291
3292         read_unlock_bh(&arm_state->susp_res_lock);
3293
3294         vchiq_log_warning(vchiq_susp_log_level,
3295                 "-- Videcore suspend state: %s --",
3296                 suspend_state_names[vc_suspend_state + VC_SUSPEND_NUM_OFFSET]);
3297         vchiq_log_warning(vchiq_susp_log_level,
3298                 "-- Videcore resume state: %s --",
3299                 resume_state_names[vc_resume_state + VC_RESUME_NUM_OFFSET]);
3300
3301         if (only_nonzero)
3302                 vchiq_log_warning(vchiq_susp_log_level, "Too many active "
3303                         "services (%d).  Only dumping up to first %d services "
3304                         "with non-zero use-count", active_services,
3305                         local_max_services);
3306
3307         for (i = 0; i < j; i++) {
3308                 vchiq_log_warning(vchiq_susp_log_level,
3309                         "----- %c%c%c%c:%d service count %d %s",
3310                         VCHIQ_FOURCC_AS_4CHARS(service_data[i].fourcc),
3311                         service_data[i].clientid,
3312                         service_data[i].use_count,
3313                         service_data[i].use_count ? nz : "");
3314         }
3315         vchiq_log_warning(vchiq_susp_log_level,
3316                 "----- VCHIQ use count count %d", peer_count);
3317         vchiq_log_warning(vchiq_susp_log_level,
3318                 "--- Overall vchiq instance use count %d", vc_use_count);
3319
3320         vchiq_dump_platform_use_state(state);
3321 }
3322
3323 VCHIQ_STATUS_T
3324 vchiq_check_service(VCHIQ_SERVICE_T *service)
3325 {
3326         VCHIQ_ARM_STATE_T *arm_state;
3327         VCHIQ_STATUS_T ret = VCHIQ_ERROR;
3328
3329         if (!service || !service->state)
3330                 goto out;
3331
3332         vchiq_log_trace(vchiq_susp_log_level, "%s", __func__);
3333
3334         arm_state = vchiq_platform_get_arm_state(service->state);
3335
3336         read_lock_bh(&arm_state->susp_res_lock);
3337         if (service->service_use_count)
3338                 ret = VCHIQ_SUCCESS;
3339         read_unlock_bh(&arm_state->susp_res_lock);
3340
3341         if (ret == VCHIQ_ERROR) {
3342                 vchiq_log_error(vchiq_susp_log_level,
3343                         "%s ERROR - %c%c%c%c:%d service count %d, "
3344                         "state count %d, videocore suspend state %s", __func__,
3345                         VCHIQ_FOURCC_AS_4CHARS(service->base.fourcc),
3346                         service->client_id, service->service_use_count,
3347                         arm_state->videocore_use_count,
3348                         suspend_state_names[arm_state->vc_suspend_state +
3349                                                 VC_SUSPEND_NUM_OFFSET]);
3350                 vchiq_dump_service_use_state(service->state);
3351         }
3352 out:
3353         return ret;
3354 }
3355
3356 /* stub functions */
3357 void vchiq_on_remote_use_active(VCHIQ_STATE_T *state)
3358 {
3359         (void)state;
3360 }
3361
3362 void vchiq_platform_conn_state_changed(VCHIQ_STATE_T *state,
3363         VCHIQ_CONNSTATE_T oldstate, VCHIQ_CONNSTATE_T newstate)
3364 {
3365         VCHIQ_ARM_STATE_T *arm_state = vchiq_platform_get_arm_state(state);
3366
3367         vchiq_log_info(vchiq_susp_log_level, "%d: %s->%s", state->id,
3368                 get_conn_state_name(oldstate), get_conn_state_name(newstate));
3369         if (state->conn_state == VCHIQ_CONNSTATE_CONNECTED) {
3370                 write_lock_bh(&arm_state->susp_res_lock);
3371                 if (!arm_state->first_connect) {
3372                         char threadname[16];
3373
3374                         arm_state->first_connect = 1;
3375                         write_unlock_bh(&arm_state->susp_res_lock);
3376                         snprintf(threadname, sizeof(threadname), "vchiq-keep/%d",
3377                                 state->id);
3378                         arm_state->ka_thread = kthread_create(
3379                                 &vchiq_keepalive_thread_func,
3380                                 (void *)state,
3381                                 threadname);
3382                         if (IS_ERR(arm_state->ka_thread)) {
3383                                 vchiq_log_error(vchiq_susp_log_level,
3384                                         "vchiq: FATAL: couldn't create thread %s",
3385                                         threadname);
3386                         } else {
3387                                 wake_up_process(arm_state->ka_thread);
3388                         }
3389                 } else
3390                         write_unlock_bh(&arm_state->susp_res_lock);
3391         }
3392 }
3393
3394 static int vchiq_probe(struct platform_device *pdev)
3395 {
3396         struct device_node *fw_node;
3397         struct rpi_firmware *fw;
3398         int err;
3399
3400         fw_node = of_parse_phandle(pdev->dev.of_node, "firmware", 0);
3401         if (!fw_node) {
3402                 dev_err(&pdev->dev, "Missing firmware node\n");
3403                 return -ENOENT;
3404         }
3405
3406         fw = rpi_firmware_get(fw_node);
3407         of_node_put(fw_node);
3408         if (!fw)
3409                 return -EPROBE_DEFER;
3410
3411         platform_set_drvdata(pdev, fw);
3412
3413         err = vchiq_platform_init(pdev, &g_state);
3414         if (err != 0)
3415                 goto failed_platform_init;
3416
3417         err = alloc_chrdev_region(&vchiq_devid, VCHIQ_MINOR, 1, DEVICE_NAME);
3418         if (err != 0) {
3419                 vchiq_log_error(vchiq_arm_log_level,
3420                         "Unable to allocate device number");
3421                 goto failed_platform_init;
3422         }
3423         cdev_init(&vchiq_cdev, &vchiq_fops);
3424         vchiq_cdev.owner = THIS_MODULE;
3425         err = cdev_add(&vchiq_cdev, vchiq_devid, 1);
3426         if (err != 0) {
3427                 vchiq_log_error(vchiq_arm_log_level,
3428                         "Unable to register device");
3429                 goto failed_cdev_add;
3430         }
3431
3432         /* create sysfs entries */
3433         vchiq_class = class_create(THIS_MODULE, DEVICE_NAME);
3434         err = PTR_ERR(vchiq_class);
3435         if (IS_ERR(vchiq_class))
3436                 goto failed_class_create;
3437
3438         vchiq_dev = device_create(vchiq_class, NULL,
3439                 vchiq_devid, NULL, "vchiq");
3440         err = PTR_ERR(vchiq_dev);
3441         if (IS_ERR(vchiq_dev))
3442                 goto failed_device_create;
3443
3444         /* create debugfs entries */
3445         err = vchiq_debugfs_init();
3446         if (err != 0)
3447                 goto failed_debugfs_init;
3448
3449         vchiq_log_info(vchiq_arm_log_level,
3450                 "vchiq: initialised - version %d (min %d), device %d.%d",
3451                 VCHIQ_VERSION, VCHIQ_VERSION_MIN,
3452                 MAJOR(vchiq_devid), MINOR(vchiq_devid));
3453
3454         return 0;
3455
3456 failed_debugfs_init:
3457         device_destroy(vchiq_class, vchiq_devid);
3458 failed_device_create:
3459         class_destroy(vchiq_class);
3460 failed_class_create:
3461         cdev_del(&vchiq_cdev);
3462 failed_cdev_add:
3463         unregister_chrdev_region(vchiq_devid, 1);
3464 failed_platform_init:
3465         vchiq_log_warning(vchiq_arm_log_level, "could not load vchiq");
3466         return err;
3467 }
3468
3469 static int vchiq_remove(struct platform_device *pdev)
3470 {
3471         vchiq_debugfs_deinit();
3472         device_destroy(vchiq_class, vchiq_devid);
3473         class_destroy(vchiq_class);
3474         cdev_del(&vchiq_cdev);
3475         unregister_chrdev_region(vchiq_devid, 1);
3476
3477         return 0;
3478 }
3479
3480 static const struct of_device_id vchiq_of_match[] = {
3481         { .compatible = "brcm,bcm2835-vchiq", },
3482         {},
3483 };
3484 MODULE_DEVICE_TABLE(of, vchiq_of_match);
3485
3486 static struct platform_driver vchiq_driver = {
3487         .driver = {
3488                 .name = "bcm2835_vchiq",
3489                 .of_match_table = vchiq_of_match,
3490         },
3491         .probe = vchiq_probe,
3492         .remove = vchiq_remove,
3493 };
3494 module_platform_driver(vchiq_driver);
3495
3496 MODULE_LICENSE("Dual BSD/GPL");
3497 MODULE_DESCRIPTION("Videocore VCHIQ driver");
3498 MODULE_AUTHOR("Broadcom Corporation");