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

   1 /**
   2  * Copyright (c) 2010-2012 Broadcom. All rights reserved.
   3  *
   4  * Redistribution and use in source and binary forms, with or without
   5  * modification, are permitted provided that the following conditions
   6  * are met:
   7  * 1. Redistributions of source code must retain the above copyright
   8  *    notice, this list of conditions, and the following disclaimer,
   9  *    without modification.
  10  * 2. Redistributions in binary form must reproduce the above copyright
  11  *    notice, this list of conditions and the following disclaimer in the
  12  *    documentation and/or other materials provided with the distribution.
  13  * 3. The names of the above-listed copyright holders may not be used
  14  *    to endorse or promote products derived from this software without
  15  *    specific prior written permission.
  16  *
  17  * ALTERNATIVELY, this software may be distributed under the terms of the
  18  * GNU General Public License ("GPL") version 2, as published by the Free
  19  * Software Foundation.
  20  *
  21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
  22  * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
  23  * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
  24  * PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR
  25  * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
  26  * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
  27  * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
  28  * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
  29  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
  30  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  31  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  32  */
  33 #include <linux/module.h>
  34 #include <linux/types.h>
  35
  36 #include "interface/vchi/vchi.h"
  37 #include "vchiq.h"
  38 #include "vchiq_core.h"
  39
  40 #include "vchiq_util.h"
  41
  42 #include <stddef.h>
  43
  44 #define vchiq_status_to_vchi(status) ((int32_t)status)
  45
  46 typedef struct {
  47         VCHIQ_SERVICE_HANDLE_T handle;
  48
  49         VCHIU_QUEUE_T queue;
  50
  51         VCHI_CALLBACK_T callback;
  52         void *callback_param;
  53 } SHIM_SERVICE_T;
  54
  55 /* ----------------------------------------------------------------------
  56  * return pointer to the mphi message driver function table
  57  * -------------------------------------------------------------------- */
  58 const VCHI_MESSAGE_DRIVER_T *
  59 vchi_mphi_message_driver_func_table(void)
  60 {
  61         return NULL;
  62 }
  63
  64 /* ----------------------------------------------------------------------
  65  * return a pointer to the 'single' connection driver fops
  66  * -------------------------------------------------------------------- */
  67 const VCHI_CONNECTION_API_T *
  68 single_get_func_table(void)
  69 {
  70         return NULL;
  71 }
  72
  73 VCHI_CONNECTION_T *vchi_create_connection(
  74         const VCHI_CONNECTION_API_T *function_table,
  75         const VCHI_MESSAGE_DRIVER_T *low_level)
  76 {
  77         (void)function_table;
  78         (void)low_level;
  79         return NULL;
  80 }
  81
  82 /***********************************************************
  83  * Name: vchi_msg_peek
  84  *
  85  * Arguments:  const VCHI_SERVICE_HANDLE_T handle,
  86  *             void **data,
  87  *             uint32_t *msg_size,
  88
  89
  90  *             VCHI_FLAGS_T flags
  91  *
  92  * Description: Routine to return a pointer to the current message (to allow in
  93  *              place processing). The message can be removed using
  94  *              vchi_msg_remove when you're finished
  95  *
  96  * Returns: int32_t - success == 0
  97  *
  98  ***********************************************************/
  99 int32_t vchi_msg_peek(VCHI_SERVICE_HANDLE_T handle,
 100         void **data,
 101         uint32_t *msg_size,
 102         VCHI_FLAGS_T flags)
 103 {
 104         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 105         VCHIQ_HEADER_T *header;
 106
 107         WARN_ON((flags != VCHI_FLAGS_NONE) &&
 108                 (flags != VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE));
 109
 110         if (flags == VCHI_FLAGS_NONE)
 111                 if (vchiu_queue_is_empty(&service->queue))
 112                         return -1;
 113
 114         header = vchiu_queue_peek(&service->queue);
 115
 116         *data = header->data;
 117         *msg_size = header->size;
 118
 119         return 0;
 120 }
 121 EXPORT_SYMBOL(vchi_msg_peek);
 122
 123 /***********************************************************
 124  * Name: vchi_msg_remove
 125  *
 126  * Arguments:  const VCHI_SERVICE_HANDLE_T handle,
 127  *
 128  * Description: Routine to remove a message (after it has been read with
 129  *              vchi_msg_peek)
 130  *
 131  * Returns: int32_t - success == 0
 132  *
 133  ***********************************************************/
 134 int32_t vchi_msg_remove(VCHI_SERVICE_HANDLE_T handle)
 135 {
 136         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 137         VCHIQ_HEADER_T *header;
 138
 139         header = vchiu_queue_pop(&service->queue);
 140
 141         vchiq_release_message(service->handle, header);
 142
 143         return 0;
 144 }
 145 EXPORT_SYMBOL(vchi_msg_remove);
 146
 147 /***********************************************************
 148  * Name: vchi_msg_queue
 149  *
 150  * Arguments:  VCHI_SERVICE_HANDLE_T handle,
 151  *             ssize_t (*copy_callback)(void *context, void *dest,
 152  *                                      size_t offset, size_t maxsize),
 153  *             void *context,
 154  *             uint32_t data_size
 155  *
 156  * Description: Thin wrapper to queue a message onto a connection
 157  *
 158  * Returns: int32_t - success == 0
 159  *
 160  ***********************************************************/
 161 static
 162 int32_t vchi_msg_queue(VCHI_SERVICE_HANDLE_T handle,
 163         ssize_t (*copy_callback)(void *context, void *dest,
 164                                  size_t offset, size_t maxsize),
 165         void *context,
 166         uint32_t data_size)
 167 {
 168         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 169         VCHIQ_STATUS_T status;
 170
 171         while (1) {
 172                 status = vchiq_queue_message(service->handle,
 173                                              copy_callback,
 174                                              context,
 175                                              data_size);
 176
 177                 /*
 178                  * vchiq_queue_message() may return VCHIQ_RETRY, so we need to
 179                  * implement a retry mechanism since this function is supposed
 180                  * to block until queued
 181                  */
 182                 if (status != VCHIQ_RETRY)
 183                         break;
 184
 185                 msleep(1);
 186         }
 187
 188         return vchiq_status_to_vchi(status);
 189 }
 190
 191 static ssize_t
 192 vchi_queue_kernel_message_callback(void *context,
 193                                    void *dest,
 194                                    size_t offset,
 195                                    size_t maxsize)
 196 {
 197         memcpy(dest, context + offset, maxsize);
 198         return maxsize;
 199 }
 200
 201 int
 202 vchi_queue_kernel_message(VCHI_SERVICE_HANDLE_T handle,
 203                           void *data,
 204                           unsigned int size)
 205 {
 206         return vchi_msg_queue(handle,
 207                               vchi_queue_kernel_message_callback,
 208                               data,
 209                               size);
 210 }
 211 EXPORT_SYMBOL(vchi_queue_kernel_message);
 212
 213 struct vchi_queue_user_message_context {
 214         void __user *data;
 215 };
 216
 217 static ssize_t
 218 vchi_queue_user_message_callback(void *context,
 219                                  void *dest,
 220                                  size_t offset,
 221                                  size_t maxsize)
 222 {
 223         struct vchi_queue_user_message_context *copycontext = context;
 224
 225         if (copy_from_user(dest, copycontext->data + offset, maxsize))
 226                 return -EFAULT;
 227
 228         return maxsize;
 229 }
 230
 231 int
 232 vchi_queue_user_message(VCHI_SERVICE_HANDLE_T handle,
 233                         void __user *data,
 234                         unsigned int size)
 235 {
 236         struct vchi_queue_user_message_context copycontext = {
 237                 .data = data
 238         };
 239
 240         return vchi_msg_queue(handle,
 241                               vchi_queue_user_message_callback,
 242                               &copycontext,
 243                               size);
 244 }
 245 EXPORT_SYMBOL(vchi_queue_user_message);
 246
 247 /***********************************************************
 248  * Name: vchi_bulk_queue_receive
 249  *
 250  * Arguments:  VCHI_BULK_HANDLE_T handle,
 251  *             void *data_dst,
 252  *             const uint32_t data_size,
 253  *             VCHI_FLAGS_T flags
 254  *             void *bulk_handle
 255  *
 256  * Description: Routine to setup a rcv buffer
 257  *
 258  * Returns: int32_t - success == 0
 259  *
 260  ***********************************************************/
 261 int32_t vchi_bulk_queue_receive(VCHI_SERVICE_HANDLE_T handle,
 262         void *data_dst,
 263         uint32_t data_size,
 264         VCHI_FLAGS_T flags,
 265         void *bulk_handle)
 266 {
 267         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 268         VCHIQ_BULK_MODE_T mode;
 269         VCHIQ_STATUS_T status;
 270
 271         switch ((int)flags) {
 272         case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE
 273                 | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
 274                 WARN_ON(!service->callback);
 275                 mode = VCHIQ_BULK_MODE_CALLBACK;
 276                 break;
 277         case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
 278                 mode = VCHIQ_BULK_MODE_BLOCKING;
 279                 break;
 280         case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
 281         case VCHI_FLAGS_NONE:
 282                 mode = VCHIQ_BULK_MODE_NOCALLBACK;
 283                 break;
 284         default:
 285                 WARN(1, "unsupported message\n");
 286                 return vchiq_status_to_vchi(VCHIQ_ERROR);
 287         }
 288
 289         while (1) {
 290                 status = vchiq_bulk_receive(service->handle, data_dst,
 291                         data_size, bulk_handle, mode);
 292                 /*
 293                  * vchiq_bulk_receive() may return VCHIQ_RETRY, so we need to
 294                  * implement a retry mechanism since this function is supposed
 295                  * to block until queued
 296                  */
 297                 if (status != VCHIQ_RETRY)
 298                         break;
 299
 300                 msleep(1);
 301         }
 302
 303         return vchiq_status_to_vchi(status);
 304 }
 305 EXPORT_SYMBOL(vchi_bulk_queue_receive);
 306
 307 /***********************************************************
 308  * Name: vchi_bulk_queue_transmit
 309  *
 310  * Arguments:  VCHI_BULK_HANDLE_T handle,
 311  *             const void *data_src,
 312  *             uint32_t data_size,
 313  *             VCHI_FLAGS_T flags,
 314  *             void *bulk_handle
 315  *
 316  * Description: Routine to transmit some data
 317  *
 318  * Returns: int32_t - success == 0
 319  *
 320  ***********************************************************/
 321 int32_t vchi_bulk_queue_transmit(VCHI_SERVICE_HANDLE_T handle,
 322         const void *data_src,
 323         uint32_t data_size,
 324         VCHI_FLAGS_T flags,
 325         void *bulk_handle)
 326 {
 327         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 328         VCHIQ_BULK_MODE_T mode;
 329         VCHIQ_STATUS_T status;
 330
 331         switch ((int)flags) {
 332         case VCHI_FLAGS_CALLBACK_WHEN_OP_COMPLETE
 333                 | VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
 334                 WARN_ON(!service->callback);
 335                 mode = VCHIQ_BULK_MODE_CALLBACK;
 336                 break;
 337         case VCHI_FLAGS_BLOCK_UNTIL_DATA_READ:
 338         case VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE:
 339                 mode = VCHIQ_BULK_MODE_BLOCKING;
 340                 break;
 341         case VCHI_FLAGS_BLOCK_UNTIL_QUEUED:
 342         case VCHI_FLAGS_NONE:
 343                 mode = VCHIQ_BULK_MODE_NOCALLBACK;
 344                 break;
 345         default:
 346                 WARN(1, "unsupported message\n");
 347                 return vchiq_status_to_vchi(VCHIQ_ERROR);
 348         }
 349
 350         while (1) {
 351                 status = vchiq_bulk_transmit(service->handle, data_src,
 352                         data_size, bulk_handle, mode);
 353
 354                 /*
 355                  * vchiq_bulk_transmit() may return VCHIQ_RETRY, so we need to
 356                  * implement a retry mechanism since this function is supposed
 357                  * to block until queued
 358                  */
 359                 if (status != VCHIQ_RETRY)
 360                         break;
 361
 362                 msleep(1);
 363         }
 364
 365         return vchiq_status_to_vchi(status);
 366 }
 367 EXPORT_SYMBOL(vchi_bulk_queue_transmit);
 368
 369 /***********************************************************
 370  * Name: vchi_msg_dequeue
 371  *
 372  * Arguments:  VCHI_SERVICE_HANDLE_T handle,
 373  *             void *data,
 374  *             uint32_t max_data_size_to_read,
 375  *             uint32_t *actual_msg_size
 376  *             VCHI_FLAGS_T flags
 377  *
 378  * Description: Routine to dequeue a message into the supplied buffer
 379  *
 380  * Returns: int32_t - success == 0
 381  *
 382  ***********************************************************/
 383 int32_t vchi_msg_dequeue(VCHI_SERVICE_HANDLE_T handle,
 384         void *data,
 385         uint32_t max_data_size_to_read,
 386         uint32_t *actual_msg_size,
 387         VCHI_FLAGS_T flags)
 388 {
 389         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 390         VCHIQ_HEADER_T *header;
 391
 392         WARN_ON((flags != VCHI_FLAGS_NONE) &&
 393                 (flags != VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE));
 394
 395         if (flags == VCHI_FLAGS_NONE)
 396                 if (vchiu_queue_is_empty(&service->queue))
 397                         return -1;
 398
 399         header = vchiu_queue_pop(&service->queue);
 400
 401         memcpy(data, header->data, header->size < max_data_size_to_read ?
 402                 header->size : max_data_size_to_read);
 403
 404         *actual_msg_size = header->size;
 405
 406         vchiq_release_message(service->handle, header);
 407
 408         return 0;
 409 }
 410 EXPORT_SYMBOL(vchi_msg_dequeue);
 411
 412 /***********************************************************
 413  * Name: vchi_held_msg_release
 414  *
 415  * Arguments:  VCHI_HELD_MSG_T *message
 416  *
 417  * Description: Routine to release a held message (after it has been read with
 418  *              vchi_msg_hold)
 419  *
 420  * Returns: int32_t - success == 0
 421  *
 422  ***********************************************************/
 423 int32_t vchi_held_msg_release(VCHI_HELD_MSG_T *message)
 424 {
 425         /*
 426          * Convert the service field pointer back to an
 427          * VCHIQ_SERVICE_HANDLE_T which is an int.
 428          * This pointer is opaque to everything except
 429          * vchi_msg_hold which simply upcasted the int
 430          * to a pointer.
 431          */
 432
 433         vchiq_release_message((VCHIQ_SERVICE_HANDLE_T)(long)message->service,
 434                               (VCHIQ_HEADER_T *)message->message);
 435
 436         return 0;
 437 }
 438 EXPORT_SYMBOL(vchi_held_msg_release);
 439
 440 /***********************************************************
 441  * Name: vchi_msg_hold
 442  *
 443  * Arguments:  VCHI_SERVICE_HANDLE_T handle,
 444  *             void **data,
 445  *             uint32_t *msg_size,
 446  *             VCHI_FLAGS_T flags,
 447  *             VCHI_HELD_MSG_T *message_handle
 448  *
 449  * Description: Routine to return a pointer to the current message (to allow
 450  *              in place processing). The message is dequeued - don't forget
 451  *              to release the message using vchi_held_msg_release when you're
 452  *              finished.
 453  *
 454  * Returns: int32_t - success == 0
 455  *
 456  ***********************************************************/
 457 int32_t vchi_msg_hold(VCHI_SERVICE_HANDLE_T handle,
 458         void **data,
 459         uint32_t *msg_size,
 460         VCHI_FLAGS_T flags,
 461         VCHI_HELD_MSG_T *message_handle)
 462 {
 463         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 464         VCHIQ_HEADER_T *header;
 465
 466         WARN_ON((flags != VCHI_FLAGS_NONE) &&
 467                 (flags != VCHI_FLAGS_BLOCK_UNTIL_OP_COMPLETE));
 468
 469         if (flags == VCHI_FLAGS_NONE)
 470                 if (vchiu_queue_is_empty(&service->queue))
 471                         return -1;
 472
 473         header = vchiu_queue_pop(&service->queue);
 474
 475         *data = header->data;
 476         *msg_size = header->size;
 477
 478         /*
 479          * upcast the VCHIQ_SERVICE_HANDLE_T which is an int
 480          * to a pointer and stuff it in the held message.
 481          * This pointer is opaque to everything except
 482          * vchi_held_msg_release which simply downcasts it back
 483          * to an int.
 484          */
 485
 486         message_handle->service =
 487                 (struct opaque_vchi_service_t *)(long)service->handle;
 488         message_handle->message = header;
 489
 490         return 0;
 491 }
 492 EXPORT_SYMBOL(vchi_msg_hold);
 493
 494 /***********************************************************
 495  * Name: vchi_initialise
 496  *
 497  * Arguments: VCHI_INSTANCE_T *instance_handle
 498  *
 499  * Description: Initialises the hardware but does not transmit anything
 500  *              When run as a Host App this will be called twice hence the need
 501  *              to malloc the state information
 502  *
 503  * Returns: 0 if successful, failure otherwise
 504  *
 505  ***********************************************************/
 506
 507 int32_t vchi_initialise(VCHI_INSTANCE_T *instance_handle)
 508 {
 509         VCHIQ_INSTANCE_T instance;
 510         VCHIQ_STATUS_T status;
 511
 512         status = vchiq_initialise(&instance);
 513
 514         *instance_handle = (VCHI_INSTANCE_T)instance;
 515
 516         return vchiq_status_to_vchi(status);
 517 }
 518 EXPORT_SYMBOL(vchi_initialise);
 519
 520 /***********************************************************
 521  * Name: vchi_connect
 522  *
 523  * Arguments: VCHI_CONNECTION_T **connections
 524  *            const uint32_t num_connections
 525  *            VCHI_INSTANCE_T instance_handle)
 526  *
 527  * Description: Starts the command service on each connection,
 528  *              causing INIT messages to be pinged back and forth
 529  *
 530  * Returns: 0 if successful, failure otherwise
 531  *
 532  ***********************************************************/
 533 int32_t vchi_connect(VCHI_CONNECTION_T **connections,
 534         const uint32_t num_connections,
 535         VCHI_INSTANCE_T instance_handle)
 536 {
 537         VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
 538
 539         (void)connections;
 540         (void)num_connections;
 541
 542         return vchiq_connect(instance);
 543 }
 544 EXPORT_SYMBOL(vchi_connect);
 545
 546
 547 /***********************************************************
 548  * Name: vchi_disconnect
 549  *
 550  * Arguments: VCHI_INSTANCE_T instance_handle
 551  *
 552  * Description: Stops the command service on each connection,
 553  *              causing DE-INIT messages to be pinged back and forth
 554  *
 555  * Returns: 0 if successful, failure otherwise
 556  *
 557  ***********************************************************/
 558 int32_t vchi_disconnect(VCHI_INSTANCE_T instance_handle)
 559 {
 560         VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
 561         return vchiq_status_to_vchi(vchiq_shutdown(instance));
 562 }
 563 EXPORT_SYMBOL(vchi_disconnect);
 564
 565
 566 /***********************************************************
 567  * Name: vchi_service_open
 568  * Name: vchi_service_create
 569  *
 570  * Arguments: VCHI_INSTANCE_T *instance_handle
 571  *            SERVICE_CREATION_T *setup,
 572  *            VCHI_SERVICE_HANDLE_T *handle
 573  *
 574  * Description: Routine to open a service
 575  *
 576  * Returns: int32_t - success == 0
 577  *
 578  ***********************************************************/
 579
 580 static VCHIQ_STATUS_T shim_callback(VCHIQ_REASON_T reason,
 581         VCHIQ_HEADER_T *header, VCHIQ_SERVICE_HANDLE_T handle, void *bulk_user)
 582 {
 583         SHIM_SERVICE_T *service =
 584                 (SHIM_SERVICE_T *)VCHIQ_GET_SERVICE_USERDATA(handle);
 585
 586         if (!service->callback)
 587                 goto release;
 588
 589         switch (reason) {
 590         case VCHIQ_MESSAGE_AVAILABLE:
 591                 vchiu_queue_push(&service->queue, header);
 592
 593                 service->callback(service->callback_param,
 594                                   VCHI_CALLBACK_MSG_AVAILABLE, NULL);
 595
 596                 goto done;
 597                 break;
 598
 599         case VCHIQ_BULK_TRANSMIT_DONE:
 600                 service->callback(service->callback_param,
 601                                   VCHI_CALLBACK_BULK_SENT, bulk_user);
 602                 break;
 603
 604         case VCHIQ_BULK_RECEIVE_DONE:
 605                 service->callback(service->callback_param,
 606                                   VCHI_CALLBACK_BULK_RECEIVED, bulk_user);
 607                 break;
 608
 609         case VCHIQ_SERVICE_CLOSED:
 610                 service->callback(service->callback_param,
 611                                   VCHI_CALLBACK_SERVICE_CLOSED, NULL);
 612                 break;
 613
 614         case VCHIQ_SERVICE_OPENED:
 615                 /* No equivalent VCHI reason */
 616                 break;
 617
 618         case VCHIQ_BULK_TRANSMIT_ABORTED:
 619                 service->callback(service->callback_param,
 620                                   VCHI_CALLBACK_BULK_TRANSMIT_ABORTED,
 621                                   bulk_user);
 622                 break;
 623
 624         case VCHIQ_BULK_RECEIVE_ABORTED:
 625                 service->callback(service->callback_param,
 626                                   VCHI_CALLBACK_BULK_RECEIVE_ABORTED,
 627                                   bulk_user);
 628                 break;
 629
 630         default:
 631                 WARN(1, "not supported\n");
 632                 break;
 633         }
 634
 635 release:
 636         vchiq_release_message(service->handle, header);
 637 done:
 638         return VCHIQ_SUCCESS;
 639 }
 640
 641 static SHIM_SERVICE_T *service_alloc(VCHIQ_INSTANCE_T instance,
 642         SERVICE_CREATION_T *setup)
 643 {
 644         SHIM_SERVICE_T *service = kzalloc(sizeof(SHIM_SERVICE_T), GFP_KERNEL);
 645
 646         (void)instance;
 647
 648         if (service) {
 649                 if (vchiu_queue_init(&service->queue, 64)) {
 650                         service->callback = setup->callback;
 651                         service->callback_param = setup->callback_param;
 652                 } else {
 653                         kfree(service);
 654                         service = NULL;
 655                 }
 656         }
 657
 658         return service;
 659 }
 660
 661 static void service_free(SHIM_SERVICE_T *service)
 662 {
 663         if (service) {
 664                 vchiu_queue_delete(&service->queue);
 665                 kfree(service);
 666         }
 667 }
 668
 669 int32_t vchi_service_open(VCHI_INSTANCE_T instance_handle,
 670         SERVICE_CREATION_T *setup,
 671         VCHI_SERVICE_HANDLE_T *handle)
 672 {
 673         VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
 674         SHIM_SERVICE_T *service = service_alloc(instance, setup);
 675
 676         *handle = (VCHI_SERVICE_HANDLE_T)service;
 677
 678         if (service) {
 679                 VCHIQ_SERVICE_PARAMS_T params;
 680                 VCHIQ_STATUS_T status;
 681
 682                 memset(&params, 0, sizeof(params));
 683                 params.fourcc = setup->service_id;
 684                 params.callback = shim_callback;
 685                 params.userdata = service;
 686                 params.version = setup->version.version;
 687                 params.version_min = setup->version.version_min;
 688
 689                 status = vchiq_open_service(instance, &params,
 690                         &service->handle);
 691                 if (status != VCHIQ_SUCCESS) {
 692                         service_free(service);
 693                         service = NULL;
 694                         *handle = NULL;
 695                 }
 696         }
 697
 698         return (service != NULL) ? 0 : -1;
 699 }
 700 EXPORT_SYMBOL(vchi_service_open);
 701
 702 int32_t vchi_service_create(VCHI_INSTANCE_T instance_handle,
 703         SERVICE_CREATION_T *setup,
 704         VCHI_SERVICE_HANDLE_T *handle)
 705 {
 706         VCHIQ_INSTANCE_T instance = (VCHIQ_INSTANCE_T)instance_handle;
 707         SHIM_SERVICE_T *service = service_alloc(instance, setup);
 708
 709         *handle = (VCHI_SERVICE_HANDLE_T)service;
 710
 711         if (service) {
 712                 VCHIQ_SERVICE_PARAMS_T params;
 713                 VCHIQ_STATUS_T status;
 714
 715                 memset(&params, 0, sizeof(params));
 716                 params.fourcc = setup->service_id;
 717                 params.callback = shim_callback;
 718                 params.userdata = service;
 719                 params.version = setup->version.version;
 720                 params.version_min = setup->version.version_min;
 721                 status = vchiq_add_service(instance, &params, &service->handle);
 722
 723                 if (status != VCHIQ_SUCCESS) {
 724                         service_free(service);
 725                         service = NULL;
 726                         *handle = NULL;
 727                 }
 728         }
 729
 730         return (service != NULL) ? 0 : -1;
 731 }
 732 EXPORT_SYMBOL(vchi_service_create);
 733
 734 int32_t vchi_service_close(const VCHI_SERVICE_HANDLE_T handle)
 735 {
 736         int32_t ret = -1;
 737         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 738         if (service) {
 739                 VCHIQ_STATUS_T status = vchiq_close_service(service->handle);
 740                 if (status == VCHIQ_SUCCESS) {
 741                         service_free(service);
 742                         service = NULL;
 743                 }
 744
 745                 ret = vchiq_status_to_vchi(status);
 746         }
 747         return ret;
 748 }
 749 EXPORT_SYMBOL(vchi_service_close);
 750
 751 int32_t vchi_service_destroy(const VCHI_SERVICE_HANDLE_T handle)
 752 {
 753         int32_t ret = -1;
 754         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 755         if (service) {
 756                 VCHIQ_STATUS_T status = vchiq_remove_service(service->handle);
 757                 if (status == VCHIQ_SUCCESS) {
 758                         service_free(service);
 759                         service = NULL;
 760                 }
 761
 762                 ret = vchiq_status_to_vchi(status);
 763         }
 764         return ret;
 765 }
 766 EXPORT_SYMBOL(vchi_service_destroy);
 767
 768 int32_t vchi_service_set_option(const VCHI_SERVICE_HANDLE_T handle,
 769                                 VCHI_SERVICE_OPTION_T option,
 770                                 int value)
 771 {
 772         int32_t ret = -1;
 773         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 774         VCHIQ_SERVICE_OPTION_T vchiq_option;
 775         switch (option) {
 776         case VCHI_SERVICE_OPTION_TRACE:
 777                 vchiq_option = VCHIQ_SERVICE_OPTION_TRACE;
 778                 break;
 779         case VCHI_SERVICE_OPTION_SYNCHRONOUS:
 780                 vchiq_option = VCHIQ_SERVICE_OPTION_SYNCHRONOUS;
 781                 break;
 782         default:
 783                 service = NULL;
 784                 break;
 785         }
 786         if (service) {
 787                 VCHIQ_STATUS_T status =
 788                         vchiq_set_service_option(service->handle,
 789                                                 vchiq_option,
 790                                                 value);
 791
 792                 ret = vchiq_status_to_vchi(status);
 793         }
 794         return ret;
 795 }
 796 EXPORT_SYMBOL(vchi_service_set_option);
 797
 798 int32_t vchi_get_peer_version(const VCHI_SERVICE_HANDLE_T handle, short *peer_version)
 799 {
 800         int32_t ret = -1;
 801         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 802         if (service)
 803         {
 804                 VCHIQ_STATUS_T status;
 805
 806                 status = vchiq_get_peer_version(service->handle, peer_version);
 807                 ret = vchiq_status_to_vchi(status);
 808         }
 809         return ret;
 810 }
 811 EXPORT_SYMBOL(vchi_get_peer_version);
 812
 813 /* ----------------------------------------------------------------------
 814  * read a uint32_t from buffer.
 815  * network format is defined to be little endian
 816  * -------------------------------------------------------------------- */
 817 uint32_t
 818 vchi_readbuf_uint32(const void *_ptr)
 819 {
 820         const unsigned char *ptr = _ptr;
 821         return ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24);
 822 }
 823
 824 /* ----------------------------------------------------------------------
 825  * write a uint32_t to buffer.
 826  * network format is defined to be little endian
 827  * -------------------------------------------------------------------- */
 828 void
 829 vchi_writebuf_uint32(void *_ptr, uint32_t value)
 830 {
 831         unsigned char *ptr = _ptr;
 832         ptr[0] = (unsigned char)((value >> 0)  & 0xFF);
 833         ptr[1] = (unsigned char)((value >> 8)  & 0xFF);
 834         ptr[2] = (unsigned char)((value >> 16) & 0xFF);
 835         ptr[3] = (unsigned char)((value >> 24) & 0xFF);
 836 }
 837
 838 /* ----------------------------------------------------------------------
 839  * read a uint16_t from buffer.
 840  * network format is defined to be little endian
 841  * -------------------------------------------------------------------- */
 842 uint16_t
 843 vchi_readbuf_uint16(const void *_ptr)
 844 {
 845         const unsigned char *ptr = _ptr;
 846         return ptr[0] | (ptr[1] << 8);
 847 }
 848
 849 /* ----------------------------------------------------------------------
 850  * write a uint16_t into the buffer.
 851  * network format is defined to be little endian
 852  * -------------------------------------------------------------------- */
 853 void
 854 vchi_writebuf_uint16(void *_ptr, uint16_t value)
 855 {
 856         unsigned char *ptr = _ptr;
 857         ptr[0] = (value >> 0)  & 0xFF;
 858         ptr[1] = (value >> 8)  & 0xFF;
 859 }
 860
 861 /***********************************************************
 862  * Name: vchi_service_use
 863  *
 864  * Arguments: const VCHI_SERVICE_HANDLE_T handle
 865  *
 866  * Description: Routine to increment refcount on a service
 867  *
 868  * Returns: void
 869  *
 870  ***********************************************************/
 871 int32_t vchi_service_use(const VCHI_SERVICE_HANDLE_T handle)
 872 {
 873         int32_t ret = -1;
 874         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 875         if (service)
 876                 ret = vchiq_status_to_vchi(vchiq_use_service(service->handle));
 877         return ret;
 878 }
 879 EXPORT_SYMBOL(vchi_service_use);
 880
 881 /***********************************************************
 882  * Name: vchi_service_release
 883  *
 884  * Arguments: const VCHI_SERVICE_HANDLE_T handle
 885  *
 886  * Description: Routine to decrement refcount on a service
 887  *
 888  * Returns: void
 889  *
 890  ***********************************************************/
 891 int32_t vchi_service_release(const VCHI_SERVICE_HANDLE_T handle)
 892 {
 893         int32_t ret = -1;
 894         SHIM_SERVICE_T *service = (SHIM_SERVICE_T *)handle;
 895         if (service)
 896                 ret = vchiq_status_to_vchi(
 897                         vchiq_release_service(service->handle));
 898         return ret;
 899 }
 900 EXPORT_SYMBOL(vchi_service_release);