drivers/staging/vc04_services/vchiq-mmal/mmal-vchiq.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * Broadcom BCM2835 V4L2 driver
   4  *
   5  * Copyright © 2013 Raspberry Pi (Trading) Ltd.
   6  *
   7  * Authors: Vincent Sanders @ Collabora
   8  *          Dave Stevenson @ Broadcom
   9  *              (now dave.stevenson@raspberrypi.org)
  10  *          Simon Mellor @ Broadcom
  11  *          Luke Diamand @ Broadcom
  12  *
  13  * V4L2 driver MMAL vchiq interface code
  14  */
  15
  16 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  17
  18 #include <linux/errno.h>
  19 #include <linux/kernel.h>
  20 #include <linux/module.h>
  21 #include <linux/mutex.h>
  22 #include <linux/mm.h>
  23 #include <linux/slab.h>
  24 #include <linux/completion.h>
  25 #include <linux/vmalloc.h>
  26 #include <media/videobuf2-vmalloc.h>
  27
  28 #include "../include/linux/raspberrypi/vchiq.h"
  29 #include "mmal-common.h"
  30 #include "mmal-vchiq.h"
  31 #include "mmal-msg.h"
  32
  33 /*
  34  * maximum number of components supported.
  35  * This matches the maximum permitted by default on the VPU
  36  */
  37 #define VCHIQ_MMAL_MAX_COMPONENTS 64
  38
  39 /*
  40  * Timeout for synchronous msg responses in seconds.
  41  * Helpful to increase this if stopping in the VPU debugger.
  42  */
  43 #define SYNC_MSG_TIMEOUT       3
  44
  45 /*#define FULL_MSG_DUMP 1*/
  46
  47 #ifdef DEBUG
  48 static const char *const msg_type_names[] = {
  49         "UNKNOWN",
  50         "QUIT",
  51         "SERVICE_CLOSED",
  52         "GET_VERSION",
  53         "COMPONENT_CREATE",
  54         "COMPONENT_DESTROY",
  55         "COMPONENT_ENABLE",
  56         "COMPONENT_DISABLE",
  57         "PORT_INFO_GET",
  58         "PORT_INFO_SET",
  59         "PORT_ACTION",
  60         "BUFFER_FROM_HOST",
  61         "BUFFER_TO_HOST",
  62         "GET_STATS",
  63         "PORT_PARAMETER_SET",
  64         "PORT_PARAMETER_GET",
  65         "EVENT_TO_HOST",
  66         "GET_CORE_STATS_FOR_PORT",
  67         "OPAQUE_ALLOCATOR",
  68         "CONSUME_MEM",
  69         "LMK",
  70         "OPAQUE_ALLOCATOR_DESC",
  71         "DRM_GET_LHS32",
  72         "DRM_GET_TIME",
  73         "BUFFER_FROM_HOST_ZEROLEN",
  74         "PORT_FLUSH",
  75         "HOST_LOG",
  76 };
  77 #endif
  78
  79 static const char *const port_action_type_names[] = {
  80         "UNKNOWN",
  81         "ENABLE",
  82         "DISABLE",
  83         "FLUSH",
  84         "CONNECT",
  85         "DISCONNECT",
  86         "SET_REQUIREMENTS",
  87 };
  88
  89 #if defined(DEBUG)
  90 #if defined(FULL_MSG_DUMP)
  91 #define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)                               \
  92         do {                                                            \
  93                 pr_debug(TITLE" type:%s(%d) length:%d\n",               \
  94                          msg_type_names[(MSG)->h.type],                 \
  95                          (MSG)->h.type, (MSG_LEN));                     \
  96                 print_hex_dump(KERN_DEBUG, "<<h: ", DUMP_PREFIX_OFFSET, \
  97                                16, 4, (MSG),                            \
  98                                sizeof(struct mmal_msg_header), 1);      \
  99                 print_hex_dump(KERN_DEBUG, "<<p: ", DUMP_PREFIX_OFFSET, \
 100                                16, 4,                                   \
 101                                ((u8 *)(MSG)) + sizeof(struct mmal_msg_header),\
 102                                (MSG_LEN) - sizeof(struct mmal_msg_header), 1); \
 103         } while (0)
 104 #else
 105 #define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)                               \
 106         {                                                               \
 107                 pr_debug(TITLE" type:%s(%d) length:%d\n",               \
 108                          msg_type_names[(MSG)->h.type],                 \
 109                          (MSG)->h.type, (MSG_LEN));                     \
 110         }
 111 #endif
 112 #else
 113 #define DBG_DUMP_MSG(MSG, MSG_LEN, TITLE)
 114 #endif
 115
 116 struct vchiq_mmal_instance;
 117
 118 /* normal message context */
 119 struct mmal_msg_context {
 120         struct vchiq_mmal_instance *instance;
 121
 122         /* Index in the context_map idr so that we can find the
 123          * mmal_msg_context again when servicing the VCHI reply.
 124          */
 125         int handle;
 126
 127         union {
 128                 struct {
 129                         /* work struct for buffer_cb callback */
 130                         struct work_struct work;
 131                         /* work struct for deferred callback */
 132                         struct work_struct buffer_to_host_work;
 133                         /* mmal instance */
 134                         struct vchiq_mmal_instance *instance;
 135                         /* mmal port */
 136                         struct vchiq_mmal_port *port;
 137                         /* actual buffer used to store bulk reply */
 138                         struct mmal_buffer *buffer;
 139                         /* amount of buffer used */
 140                         unsigned long buffer_used;
 141                         /* MMAL buffer flags */
 142                         u32 mmal_flags;
 143                         /* Presentation and Decode timestamps */
 144                         s64 pts;
 145                         s64 dts;
 146
 147                         int status;     /* context status */
 148
 149                 } bulk;         /* bulk data */
 150
 151                 struct {
 152                         /* message handle to release */
 153                         struct vchiq_header *msg_handle;
 154                         /* pointer to received message */
 155                         struct mmal_msg *msg;
 156                         /* received message length */
 157                         u32 msg_len;
 158                         /* completion upon reply */
 159                         struct completion cmplt;
 160                 } sync;         /* synchronous response */
 161         } u;
 162
 163 };
 164
 165 struct vchiq_mmal_instance {
 166         unsigned int service_handle;
 167
 168         /* ensure serialised access to service */
 169         struct mutex vchiq_mutex;
 170
 171         struct idr context_map;
 172         /* protect accesses to context_map */
 173         struct mutex context_map_lock;
 174
 175         struct vchiq_mmal_component component[VCHIQ_MMAL_MAX_COMPONENTS];
 176
 177         /* ordered workqueue to process all bulk operations */
 178         struct workqueue_struct *bulk_wq;
 179
 180         /* handle for a vchiq instance */
 181         struct vchiq_instance *vchiq_instance;
 182 };
 183
 184 static struct mmal_msg_context *
 185 get_msg_context(struct vchiq_mmal_instance *instance)
 186 {
 187         struct mmal_msg_context *msg_context;
 188         int handle;
 189
 190         /* todo: should this be allocated from a pool to avoid kzalloc */
 191         msg_context = kzalloc(sizeof(*msg_context), GFP_KERNEL);
 192
 193         if (!msg_context)
 194                 return ERR_PTR(-ENOMEM);
 195
 196         /* Create an ID that will be passed along with our message so
 197          * that when we service the VCHI reply, we can look up what
 198          * message is being replied to.
 199          */
 200         mutex_lock(&instance->context_map_lock);
 201         handle = idr_alloc(&instance->context_map, msg_context,
 202                            0, 0, GFP_KERNEL);
 203         mutex_unlock(&instance->context_map_lock);
 204
 205         if (handle < 0) {
 206                 kfree(msg_context);
 207                 return ERR_PTR(handle);
 208         }
 209
 210         msg_context->instance = instance;
 211         msg_context->handle = handle;
 212
 213         return msg_context;
 214 }
 215
 216 static struct mmal_msg_context *
 217 lookup_msg_context(struct vchiq_mmal_instance *instance, int handle)
 218 {
 219         return idr_find(&instance->context_map, handle);
 220 }
 221
 222 static void
 223 release_msg_context(struct mmal_msg_context *msg_context)
 224 {
 225         struct vchiq_mmal_instance *instance = msg_context->instance;
 226
 227         mutex_lock(&instance->context_map_lock);
 228         idr_remove(&instance->context_map, msg_context->handle);
 229         mutex_unlock(&instance->context_map_lock);
 230         kfree(msg_context);
 231 }
 232
 233 /* deals with receipt of event to host message */
 234 static void event_to_host_cb(struct vchiq_mmal_instance *instance,
 235                              struct mmal_msg *msg, u32 msg_len)
 236 {
 237         pr_debug("unhandled event\n");
 238         pr_debug("component:%u port type:%d num:%d cmd:0x%x length:%d\n",
 239                  msg->u.event_to_host.client_component,
 240                  msg->u.event_to_host.port_type,
 241                  msg->u.event_to_host.port_num,
 242                  msg->u.event_to_host.cmd, msg->u.event_to_host.length);
 243 }
 244
 245 /* workqueue scheduled callback
 246  *
 247  * we do this because it is important we do not call any other vchiq
 248  * sync calls from within the message delivery thread
 249  */
 250 static void buffer_work_cb(struct work_struct *work)
 251 {
 252         struct mmal_msg_context *msg_context =
 253                 container_of(work, struct mmal_msg_context, u.bulk.work);
 254         struct mmal_buffer *buffer = msg_context->u.bulk.buffer;
 255
 256         if (!buffer) {
 257                 pr_err("%s: ctx: %p, No mmal buffer to pass details\n",
 258                        __func__, msg_context);
 259                 return;
 260         }
 261
 262         buffer->length = msg_context->u.bulk.buffer_used;
 263         buffer->mmal_flags = msg_context->u.bulk.mmal_flags;
 264         buffer->dts = msg_context->u.bulk.dts;
 265         buffer->pts = msg_context->u.bulk.pts;
 266
 267         atomic_dec(&msg_context->u.bulk.port->buffers_with_vpu);
 268
 269         msg_context->u.bulk.port->buffer_cb(msg_context->u.bulk.instance,
 270                                             msg_context->u.bulk.port,
 271                                             msg_context->u.bulk.status,
 272                                             msg_context->u.bulk.buffer);
 273 }
 274
 275 /* workqueue scheduled callback to handle receiving buffers
 276  *
 277  * VCHI will allow up to 4 bulk receives to be scheduled before blocking.
 278  * If we block in the service_callback context then we can't process the
 279  * VCHI_CALLBACK_BULK_RECEIVED message that would otherwise allow the blocked
 280  * vchiq_bulk_receive() call to complete.
 281  */
 282 static void buffer_to_host_work_cb(struct work_struct *work)
 283 {
 284         struct mmal_msg_context *msg_context =
 285                 container_of(work, struct mmal_msg_context,
 286                              u.bulk.buffer_to_host_work);
 287         struct vchiq_mmal_instance *instance = msg_context->instance;
 288         unsigned long len = msg_context->u.bulk.buffer_used;
 289         int ret;
 290
 291         if (!len)
 292                 /* Dummy receive to ensure the buffers remain in order */
 293                 len = 8;
 294         /* queue the bulk submission */
 295         vchiq_use_service(instance->vchiq_instance, instance->service_handle);
 296         ret = vchiq_bulk_receive(instance->vchiq_instance, instance->service_handle,
 297                                  msg_context->u.bulk.buffer->buffer,
 298                                  /* Actual receive needs to be a multiple
 299                                   * of 4 bytes
 300                                   */
 301                                 (len + 3) & ~3,
 302                                 msg_context,
 303                                 VCHIQ_BULK_MODE_CALLBACK);
 304
 305         vchiq_release_service(instance->vchiq_instance, instance->service_handle);
 306
 307         if (ret != 0)
 308                 pr_err("%s: ctx: %p, vchiq_bulk_receive failed %d\n",
 309                        __func__, msg_context, ret);
 310 }
 311
 312 /* enqueue a bulk receive for a given message context */
 313 static int bulk_receive(struct vchiq_mmal_instance *instance,
 314                         struct mmal_msg *msg,
 315                         struct mmal_msg_context *msg_context)
 316 {
 317         unsigned long rd_len;
 318
 319         rd_len = msg->u.buffer_from_host.buffer_header.length;
 320
 321         if (!msg_context->u.bulk.buffer) {
 322                 pr_err("bulk.buffer not configured - error in buffer_from_host\n");
 323
 324                 /* todo: this is a serious error, we should never have
 325                  * committed a buffer_to_host operation to the mmal
 326                  * port without the buffer to back it up (underflow
 327                  * handling) and there is no obvious way to deal with
 328                  * this - how is the mmal servie going to react when
 329                  * we fail to do the xfer and reschedule a buffer when
 330                  * it arrives? perhaps a starved flag to indicate a
 331                  * waiting bulk receive?
 332                  */
 333
 334                 return -EINVAL;
 335         }
 336
 337         /* ensure we do not overrun the available buffer */
 338         if (rd_len > msg_context->u.bulk.buffer->buffer_size) {
 339                 rd_len = msg_context->u.bulk.buffer->buffer_size;
 340                 pr_warn("short read as not enough receive buffer space\n");
 341                 /* todo: is this the correct response, what happens to
 342                  * the rest of the message data?
 343                  */
 344         }
 345
 346         /* store length */
 347         msg_context->u.bulk.buffer_used = rd_len;
 348         msg_context->u.bulk.dts = msg->u.buffer_from_host.buffer_header.dts;
 349         msg_context->u.bulk.pts = msg->u.buffer_from_host.buffer_header.pts;
 350
 351         queue_work(msg_context->instance->bulk_wq,
 352                    &msg_context->u.bulk.buffer_to_host_work);
 353
 354         return 0;
 355 }
 356
 357 /* data in message, memcpy from packet into output buffer */
 358 static int inline_receive(struct vchiq_mmal_instance *instance,
 359                           struct mmal_msg *msg,
 360                           struct mmal_msg_context *msg_context)
 361 {
 362         memcpy(msg_context->u.bulk.buffer->buffer,
 363                msg->u.buffer_from_host.short_data,
 364                msg->u.buffer_from_host.payload_in_message);
 365
 366         msg_context->u.bulk.buffer_used =
 367             msg->u.buffer_from_host.payload_in_message;
 368
 369         return 0;
 370 }
 371
 372 /* queue the buffer availability with MMAL_MSG_TYPE_BUFFER_FROM_HOST */
 373 static int
 374 buffer_from_host(struct vchiq_mmal_instance *instance,
 375                  struct vchiq_mmal_port *port, struct mmal_buffer *buf)
 376 {
 377         struct mmal_msg_context *msg_context;
 378         struct mmal_msg m;
 379         int ret;
 380
 381         if (!port->enabled)
 382                 return -EINVAL;
 383
 384         pr_debug("instance:%u buffer:%p\n", instance->service_handle, buf);
 385
 386         /* get context */
 387         if (!buf->msg_context) {
 388                 pr_err("%s: msg_context not allocated, buf %p\n", __func__,
 389                        buf);
 390                 return -EINVAL;
 391         }
 392         msg_context = buf->msg_context;
 393
 394         /* store bulk message context for when data arrives */
 395         msg_context->u.bulk.instance = instance;
 396         msg_context->u.bulk.port = port;
 397         msg_context->u.bulk.buffer = buf;
 398         msg_context->u.bulk.buffer_used = 0;
 399
 400         /* initialise work structure ready to schedule callback */
 401         INIT_WORK(&msg_context->u.bulk.work, buffer_work_cb);
 402         INIT_WORK(&msg_context->u.bulk.buffer_to_host_work,
 403                   buffer_to_host_work_cb);
 404
 405         atomic_inc(&port->buffers_with_vpu);
 406
 407         /* prep the buffer from host message */
 408         memset(&m, 0xbc, sizeof(m));    /* just to make debug clearer */
 409
 410         m.h.type = MMAL_MSG_TYPE_BUFFER_FROM_HOST;
 411         m.h.magic = MMAL_MAGIC;
 412         m.h.context = msg_context->handle;
 413         m.h.status = 0;
 414
 415         /* drvbuf is our private data passed back */
 416         m.u.buffer_from_host.drvbuf.magic = MMAL_MAGIC;
 417         m.u.buffer_from_host.drvbuf.component_handle = port->component->handle;
 418         m.u.buffer_from_host.drvbuf.port_handle = port->handle;
 419         m.u.buffer_from_host.drvbuf.client_context = msg_context->handle;
 420
 421         /* buffer header */
 422         m.u.buffer_from_host.buffer_header.cmd = 0;
 423         m.u.buffer_from_host.buffer_header.data =
 424                 (u32)(unsigned long)buf->buffer;
 425         m.u.buffer_from_host.buffer_header.alloc_size = buf->buffer_size;
 426         m.u.buffer_from_host.buffer_header.length = 0;  /* nothing used yet */
 427         m.u.buffer_from_host.buffer_header.offset = 0;  /* no offset */
 428         m.u.buffer_from_host.buffer_header.flags = 0;   /* no flags */
 429         m.u.buffer_from_host.buffer_header.pts = MMAL_TIME_UNKNOWN;
 430         m.u.buffer_from_host.buffer_header.dts = MMAL_TIME_UNKNOWN;
 431
 432         /* clear buffer type specific data */
 433         memset(&m.u.buffer_from_host.buffer_header_type_specific, 0,
 434                sizeof(m.u.buffer_from_host.buffer_header_type_specific));
 435
 436         /* no payload in message */
 437         m.u.buffer_from_host.payload_in_message = 0;
 438
 439         vchiq_use_service(instance->vchiq_instance, instance->service_handle);
 440
 441         ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, &m,
 442                                          sizeof(struct mmal_msg_header) +
 443                                          sizeof(m.u.buffer_from_host));
 444         if (ret)
 445                 atomic_dec(&port->buffers_with_vpu);
 446
 447         vchiq_release_service(instance->vchiq_instance, instance->service_handle);
 448
 449         return ret;
 450 }
 451
 452 /* deals with receipt of buffer to host message */
 453 static void buffer_to_host_cb(struct vchiq_mmal_instance *instance,
 454                               struct mmal_msg *msg, u32 msg_len)
 455 {
 456         struct mmal_msg_context *msg_context;
 457         u32 handle;
 458
 459         pr_debug("%s: instance:%p msg:%p msg_len:%d\n",
 460                  __func__, instance, msg, msg_len);
 461
 462         if (msg->u.buffer_from_host.drvbuf.magic == MMAL_MAGIC) {
 463                 handle = msg->u.buffer_from_host.drvbuf.client_context;
 464                 msg_context = lookup_msg_context(instance, handle);
 465
 466                 if (!msg_context) {
 467                         pr_err("drvbuf.client_context(%u) is invalid\n",
 468                                handle);
 469                         return;
 470                 }
 471         } else {
 472                 pr_err("MMAL_MSG_TYPE_BUFFER_TO_HOST with bad magic\n");
 473                 return;
 474         }
 475
 476         msg_context->u.bulk.mmal_flags =
 477                                 msg->u.buffer_from_host.buffer_header.flags;
 478
 479         if (msg->h.status != MMAL_MSG_STATUS_SUCCESS) {
 480                 /* message reception had an error */
 481                 pr_warn("error %d in reply\n", msg->h.status);
 482
 483                 msg_context->u.bulk.status = msg->h.status;
 484
 485         } else if (msg->u.buffer_from_host.buffer_header.length == 0) {
 486                 /* empty buffer */
 487                 if (msg->u.buffer_from_host.buffer_header.flags &
 488                     MMAL_BUFFER_HEADER_FLAG_EOS) {
 489                         msg_context->u.bulk.status =
 490                             bulk_receive(instance, msg, msg_context);
 491                         if (msg_context->u.bulk.status == 0)
 492                                 return; /* successful bulk submission, bulk
 493                                          * completion will trigger callback
 494                                          */
 495                 } else {
 496                         /* do callback with empty buffer - not EOS though */
 497                         msg_context->u.bulk.status = 0;
 498                         msg_context->u.bulk.buffer_used = 0;
 499                 }
 500         } else if (msg->u.buffer_from_host.payload_in_message == 0) {
 501                 /* data is not in message, queue a bulk receive */
 502                 msg_context->u.bulk.status =
 503                     bulk_receive(instance, msg, msg_context);
 504                 if (msg_context->u.bulk.status == 0)
 505                         return; /* successful bulk submission, bulk
 506                                  * completion will trigger callback
 507                                  */
 508
 509                 /* failed to submit buffer, this will end badly */
 510                 pr_err("error %d on bulk submission\n",
 511                        msg_context->u.bulk.status);
 512
 513         } else if (msg->u.buffer_from_host.payload_in_message <=
 514                    MMAL_VC_SHORT_DATA) {
 515                 /* data payload within message */
 516                 msg_context->u.bulk.status = inline_receive(instance, msg,
 517                                                             msg_context);
 518         } else {
 519                 pr_err("message with invalid short payload\n");
 520
 521                 /* signal error */
 522                 msg_context->u.bulk.status = -EINVAL;
 523                 msg_context->u.bulk.buffer_used =
 524                     msg->u.buffer_from_host.payload_in_message;
 525         }
 526
 527         /* schedule the port callback */
 528         schedule_work(&msg_context->u.bulk.work);
 529 }
 530
 531 static void bulk_receive_cb(struct vchiq_mmal_instance *instance,
 532                             struct mmal_msg_context *msg_context)
 533 {
 534         msg_context->u.bulk.status = 0;
 535
 536         /* schedule the port callback */
 537         schedule_work(&msg_context->u.bulk.work);
 538 }
 539
 540 static void bulk_abort_cb(struct vchiq_mmal_instance *instance,
 541                           struct mmal_msg_context *msg_context)
 542 {
 543         pr_err("%s: bulk ABORTED msg_context:%p\n", __func__, msg_context);
 544
 545         msg_context->u.bulk.status = -EINTR;
 546
 547         schedule_work(&msg_context->u.bulk.work);
 548 }
 549
 550 /* incoming event service callback */
 551 static int service_callback(struct vchiq_instance *vchiq_instance,
 552                             enum vchiq_reason reason, struct vchiq_header *header,
 553                             unsigned int handle, void *bulk_ctx)
 554 {
 555         struct vchiq_mmal_instance *instance = vchiq_get_service_userdata(vchiq_instance, handle);
 556         u32 msg_len;
 557         struct mmal_msg *msg;
 558         struct mmal_msg_context *msg_context;
 559
 560         if (!instance) {
 561                 pr_err("Message callback passed NULL instance\n");
 562                 return 0;
 563         }
 564
 565         switch (reason) {
 566         case VCHIQ_MESSAGE_AVAILABLE:
 567                 msg = (void *)header->data;
 568                 msg_len = header->size;
 569
 570                 DBG_DUMP_MSG(msg, msg_len, "<<< reply message");
 571
 572                 /* handling is different for buffer messages */
 573                 switch (msg->h.type) {
 574                 case MMAL_MSG_TYPE_BUFFER_FROM_HOST:
 575                         vchiq_release_message(vchiq_instance, handle, header);
 576                         break;
 577
 578                 case MMAL_MSG_TYPE_EVENT_TO_HOST:
 579                         event_to_host_cb(instance, msg, msg_len);
 580                         vchiq_release_message(vchiq_instance, handle, header);
 581
 582                         break;
 583
 584                 case MMAL_MSG_TYPE_BUFFER_TO_HOST:
 585                         buffer_to_host_cb(instance, msg, msg_len);
 586                         vchiq_release_message(vchiq_instance, handle, header);
 587                         break;
 588
 589                 default:
 590                         /* messages dependent on header context to complete */
 591                         if (!msg->h.context) {
 592                                 pr_err("received message context was null!\n");
 593                                 vchiq_release_message(vchiq_instance, handle, header);
 594                                 break;
 595                         }
 596
 597                         msg_context = lookup_msg_context(instance,
 598                                                          msg->h.context);
 599                         if (!msg_context) {
 600                                 pr_err("received invalid message context %u!\n",
 601                                        msg->h.context);
 602                                 vchiq_release_message(vchiq_instance, handle, header);
 603                                 break;
 604                         }
 605
 606                         /* fill in context values */
 607                         msg_context->u.sync.msg_handle = header;
 608                         msg_context->u.sync.msg = msg;
 609                         msg_context->u.sync.msg_len = msg_len;
 610
 611                         /* todo: should this check (completion_done()
 612                          * == 1) for no one waiting? or do we need a
 613                          * flag to tell us the completion has been
 614                          * interrupted so we can free the message and
 615                          * its context. This probably also solves the
 616                          * message arriving after interruption todo
 617                          * below
 618                          */
 619
 620                         /* complete message so caller knows it happened */
 621                         complete(&msg_context->u.sync.cmplt);
 622                         break;
 623                 }
 624
 625                 break;
 626
 627         case VCHIQ_BULK_RECEIVE_DONE:
 628                 bulk_receive_cb(instance, bulk_ctx);
 629                 break;
 630
 631         case VCHIQ_BULK_RECEIVE_ABORTED:
 632                 bulk_abort_cb(instance, bulk_ctx);
 633                 break;
 634
 635         case VCHIQ_SERVICE_CLOSED:
 636                 /* TODO: consider if this requires action if received when
 637                  * driver is not explicitly closing the service
 638                  */
 639                 break;
 640
 641         default:
 642                 pr_err("Received unhandled message reason %d\n", reason);
 643                 break;
 644         }
 645
 646         return 0;
 647 }
 648
 649 static int send_synchronous_mmal_msg(struct vchiq_mmal_instance *instance,
 650                                      struct mmal_msg *msg,
 651                                      unsigned int payload_len,
 652                                      struct mmal_msg **msg_out,
 653                                      struct vchiq_header **msg_handle)
 654 {
 655         struct mmal_msg_context *msg_context;
 656         int ret;
 657         unsigned long timeout;
 658
 659         /* payload size must not cause message to exceed max size */
 660         if (payload_len >
 661             (MMAL_MSG_MAX_SIZE - sizeof(struct mmal_msg_header))) {
 662                 pr_err("payload length %d exceeds max:%d\n", payload_len,
 663                        (int)(MMAL_MSG_MAX_SIZE -
 664                             sizeof(struct mmal_msg_header)));
 665                 return -EINVAL;
 666         }
 667
 668         msg_context = get_msg_context(instance);
 669         if (IS_ERR(msg_context))
 670                 return PTR_ERR(msg_context);
 671
 672         init_completion(&msg_context->u.sync.cmplt);
 673
 674         msg->h.magic = MMAL_MAGIC;
 675         msg->h.context = msg_context->handle;
 676         msg->h.status = 0;
 677
 678         DBG_DUMP_MSG(msg, (sizeof(struct mmal_msg_header) + payload_len),
 679                      ">>> sync message");
 680
 681         vchiq_use_service(instance->vchiq_instance, instance->service_handle);
 682
 683         ret = vchiq_queue_kernel_message(instance->vchiq_instance, instance->service_handle, msg,
 684                                          sizeof(struct mmal_msg_header) +
 685                                          payload_len);
 686
 687         vchiq_release_service(instance->vchiq_instance, instance->service_handle);
 688
 689         if (ret) {
 690                 pr_err("error %d queuing message\n", ret);
 691                 release_msg_context(msg_context);
 692                 return ret;
 693         }
 694
 695         timeout = wait_for_completion_timeout(&msg_context->u.sync.cmplt,
 696                                               SYNC_MSG_TIMEOUT * HZ);
 697         if (timeout == 0) {
 698                 pr_err("timed out waiting for sync completion\n");
 699                 ret = -ETIME;
 700                 /* todo: what happens if the message arrives after aborting */
 701                 release_msg_context(msg_context);
 702                 return ret;
 703         }
 704
 705         *msg_out = msg_context->u.sync.msg;
 706         *msg_handle = msg_context->u.sync.msg_handle;
 707         release_msg_context(msg_context);
 708
 709         return 0;
 710 }
 711
 712 static void dump_port_info(struct vchiq_mmal_port *port)
 713 {
 714         pr_debug("port handle:0x%x enabled:%d\n", port->handle, port->enabled);
 715
 716         pr_debug("buffer minimum num:%d size:%d align:%d\n",
 717                  port->minimum_buffer.num,
 718                  port->minimum_buffer.size, port->minimum_buffer.alignment);
 719
 720         pr_debug("buffer recommended num:%d size:%d align:%d\n",
 721                  port->recommended_buffer.num,
 722                  port->recommended_buffer.size,
 723                  port->recommended_buffer.alignment);
 724
 725         pr_debug("buffer current values num:%d size:%d align:%d\n",
 726                  port->current_buffer.num,
 727                  port->current_buffer.size, port->current_buffer.alignment);
 728
 729         pr_debug("elementary stream: type:%d encoding:0x%x variant:0x%x\n",
 730                  port->format.type,
 731                  port->format.encoding, port->format.encoding_variant);
 732
 733         pr_debug("                  bitrate:%d flags:0x%x\n",
 734                  port->format.bitrate, port->format.flags);
 735
 736         if (port->format.type == MMAL_ES_TYPE_VIDEO) {
 737                 pr_debug
 738                     ("es video format: width:%d height:%d colourspace:0x%x\n",
 739                      port->es.video.width, port->es.video.height,
 740                      port->es.video.color_space);
 741
 742                 pr_debug("               : crop xywh %d,%d,%d,%d\n",
 743                          port->es.video.crop.x,
 744                          port->es.video.crop.y,
 745                          port->es.video.crop.width, port->es.video.crop.height);
 746                 pr_debug("               : framerate %d/%d  aspect %d/%d\n",
 747                          port->es.video.frame_rate.numerator,
 748                          port->es.video.frame_rate.denominator,
 749                          port->es.video.par.numerator, port->es.video.par.denominator);
 750         }
 751 }
 752
 753 static void port_to_mmal_msg(struct vchiq_mmal_port *port, struct mmal_port *p)
 754 {
 755         /* todo do readonly fields need setting at all? */
 756         p->type = port->type;
 757         p->index = port->index;
 758         p->index_all = 0;
 759         p->is_enabled = port->enabled;
 760         p->buffer_num_min = port->minimum_buffer.num;
 761         p->buffer_size_min = port->minimum_buffer.size;
 762         p->buffer_alignment_min = port->minimum_buffer.alignment;
 763         p->buffer_num_recommended = port->recommended_buffer.num;
 764         p->buffer_size_recommended = port->recommended_buffer.size;
 765
 766         /* only three writable fields in a port */
 767         p->buffer_num = port->current_buffer.num;
 768         p->buffer_size = port->current_buffer.size;
 769         p->userdata = (u32)(unsigned long)port;
 770 }
 771
 772 static int port_info_set(struct vchiq_mmal_instance *instance,
 773                          struct vchiq_mmal_port *port)
 774 {
 775         int ret;
 776         struct mmal_msg m;
 777         struct mmal_msg *rmsg;
 778         struct vchiq_header *rmsg_handle;
 779
 780         pr_debug("setting port info port %p\n", port);
 781         if (!port)
 782                 return -1;
 783         dump_port_info(port);
 784
 785         m.h.type = MMAL_MSG_TYPE_PORT_INFO_SET;
 786
 787         m.u.port_info_set.component_handle = port->component->handle;
 788         m.u.port_info_set.port_type = port->type;
 789         m.u.port_info_set.port_index = port->index;
 790
 791         port_to_mmal_msg(port, &m.u.port_info_set.port);
 792
 793         /* elementary stream format setup */
 794         m.u.port_info_set.format.type = port->format.type;
 795         m.u.port_info_set.format.encoding = port->format.encoding;
 796         m.u.port_info_set.format.encoding_variant =
 797             port->format.encoding_variant;
 798         m.u.port_info_set.format.bitrate = port->format.bitrate;
 799         m.u.port_info_set.format.flags = port->format.flags;
 800
 801         memcpy(&m.u.port_info_set.es, &port->es,
 802                sizeof(union mmal_es_specific_format));
 803
 804         m.u.port_info_set.format.extradata_size = port->format.extradata_size;
 805         memcpy(&m.u.port_info_set.extradata, port->format.extradata,
 806                port->format.extradata_size);
 807
 808         ret = send_synchronous_mmal_msg(instance, &m,
 809                                         sizeof(m.u.port_info_set),
 810                                         &rmsg, &rmsg_handle);
 811         if (ret)
 812                 return ret;
 813
 814         if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_SET) {
 815                 /* got an unexpected message type in reply */
 816                 ret = -EINVAL;
 817                 goto release_msg;
 818         }
 819
 820         /* return operation status */
 821         ret = -rmsg->u.port_info_get_reply.status;
 822
 823         pr_debug("%s:result:%d component:0x%x port:%d\n", __func__, ret,
 824                  port->component->handle, port->handle);
 825
 826 release_msg:
 827         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
 828
 829         return ret;
 830 }
 831
 832 /* use port info get message to retrieve port information */
 833 static int port_info_get(struct vchiq_mmal_instance *instance,
 834                          struct vchiq_mmal_port *port)
 835 {
 836         int ret;
 837         struct mmal_msg m;
 838         struct mmal_msg *rmsg;
 839         struct vchiq_header *rmsg_handle;
 840
 841         /* port info time */
 842         m.h.type = MMAL_MSG_TYPE_PORT_INFO_GET;
 843         m.u.port_info_get.component_handle = port->component->handle;
 844         m.u.port_info_get.port_type = port->type;
 845         m.u.port_info_get.index = port->index;
 846
 847         ret = send_synchronous_mmal_msg(instance, &m,
 848                                         sizeof(m.u.port_info_get),
 849                                         &rmsg, &rmsg_handle);
 850         if (ret)
 851                 return ret;
 852
 853         if (rmsg->h.type != MMAL_MSG_TYPE_PORT_INFO_GET) {
 854                 /* got an unexpected message type in reply */
 855                 ret = -EINVAL;
 856                 goto release_msg;
 857         }
 858
 859         /* return operation status */
 860         ret = -rmsg->u.port_info_get_reply.status;
 861         if (ret != MMAL_MSG_STATUS_SUCCESS)
 862                 goto release_msg;
 863
 864         if (rmsg->u.port_info_get_reply.port.is_enabled == 0)
 865                 port->enabled = false;
 866         else
 867                 port->enabled = true;
 868
 869         /* copy the values out of the message */
 870         port->handle = rmsg->u.port_info_get_reply.port_handle;
 871
 872         /* port type and index cached to use on port info set because
 873          * it does not use a port handle
 874          */
 875         port->type = rmsg->u.port_info_get_reply.port_type;
 876         port->index = rmsg->u.port_info_get_reply.port_index;
 877
 878         port->minimum_buffer.num =
 879             rmsg->u.port_info_get_reply.port.buffer_num_min;
 880         port->minimum_buffer.size =
 881             rmsg->u.port_info_get_reply.port.buffer_size_min;
 882         port->minimum_buffer.alignment =
 883             rmsg->u.port_info_get_reply.port.buffer_alignment_min;
 884
 885         port->recommended_buffer.alignment =
 886             rmsg->u.port_info_get_reply.port.buffer_alignment_min;
 887         port->recommended_buffer.num =
 888             rmsg->u.port_info_get_reply.port.buffer_num_recommended;
 889
 890         port->current_buffer.num = rmsg->u.port_info_get_reply.port.buffer_num;
 891         port->current_buffer.size =
 892             rmsg->u.port_info_get_reply.port.buffer_size;
 893
 894         /* stream format */
 895         port->format.type = rmsg->u.port_info_get_reply.format.type;
 896         port->format.encoding = rmsg->u.port_info_get_reply.format.encoding;
 897         port->format.encoding_variant =
 898             rmsg->u.port_info_get_reply.format.encoding_variant;
 899         port->format.bitrate = rmsg->u.port_info_get_reply.format.bitrate;
 900         port->format.flags = rmsg->u.port_info_get_reply.format.flags;
 901
 902         /* elementary stream format */
 903         memcpy(&port->es,
 904                &rmsg->u.port_info_get_reply.es,
 905                sizeof(union mmal_es_specific_format));
 906         port->format.es = &port->es;
 907
 908         port->format.extradata_size =
 909             rmsg->u.port_info_get_reply.format.extradata_size;
 910         memcpy(port->format.extradata,
 911                rmsg->u.port_info_get_reply.extradata,
 912                port->format.extradata_size);
 913
 914         pr_debug("received port info\n");
 915         dump_port_info(port);
 916
 917 release_msg:
 918
 919         pr_debug("%s:result:%d component:0x%x port:%d\n",
 920                  __func__, ret, port->component->handle, port->handle);
 921
 922         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
 923
 924         return ret;
 925 }
 926
 927 /* create component on vc */
 928 static int create_component(struct vchiq_mmal_instance *instance,
 929                             struct vchiq_mmal_component *component,
 930                             const char *name)
 931 {
 932         int ret;
 933         struct mmal_msg m;
 934         struct mmal_msg *rmsg;
 935         struct vchiq_header *rmsg_handle;
 936
 937         /* build component create message */
 938         m.h.type = MMAL_MSG_TYPE_COMPONENT_CREATE;
 939         m.u.component_create.client_component = component->client_component;
 940         strscpy_pad(m.u.component_create.name, name,
 941                     sizeof(m.u.component_create.name));
 942         m.u.component_create.pid = 0;
 943
 944         ret = send_synchronous_mmal_msg(instance, &m,
 945                                         sizeof(m.u.component_create),
 946                                         &rmsg, &rmsg_handle);
 947         if (ret)
 948                 return ret;
 949
 950         if (rmsg->h.type != m.h.type) {
 951                 /* got an unexpected message type in reply */
 952                 ret = -EINVAL;
 953                 goto release_msg;
 954         }
 955
 956         ret = -rmsg->u.component_create_reply.status;
 957         if (ret != MMAL_MSG_STATUS_SUCCESS)
 958                 goto release_msg;
 959
 960         /* a valid component response received */
 961         component->handle = rmsg->u.component_create_reply.component_handle;
 962         component->inputs = rmsg->u.component_create_reply.input_num;
 963         component->outputs = rmsg->u.component_create_reply.output_num;
 964         component->clocks = rmsg->u.component_create_reply.clock_num;
 965
 966         pr_debug("Component handle:0x%x in:%d out:%d clock:%d\n",
 967                  component->handle,
 968                  component->inputs, component->outputs, component->clocks);
 969
 970 release_msg:
 971         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
 972
 973         return ret;
 974 }
 975
 976 /* destroys a component on vc */
 977 static int destroy_component(struct vchiq_mmal_instance *instance,
 978                              struct vchiq_mmal_component *component)
 979 {
 980         int ret;
 981         struct mmal_msg m;
 982         struct mmal_msg *rmsg;
 983         struct vchiq_header *rmsg_handle;
 984
 985         m.h.type = MMAL_MSG_TYPE_COMPONENT_DESTROY;
 986         m.u.component_destroy.component_handle = component->handle;
 987
 988         ret = send_synchronous_mmal_msg(instance, &m,
 989                                         sizeof(m.u.component_destroy),
 990                                         &rmsg, &rmsg_handle);
 991         if (ret)
 992                 return ret;
 993
 994         if (rmsg->h.type != m.h.type) {
 995                 /* got an unexpected message type in reply */
 996                 ret = -EINVAL;
 997                 goto release_msg;
 998         }
 999
1000         ret = -rmsg->u.component_destroy_reply.status;
1001
1002 release_msg:
1003
1004         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1005
1006         return ret;
1007 }
1008
1009 /* enable a component on vc */
1010 static int enable_component(struct vchiq_mmal_instance *instance,
1011                             struct vchiq_mmal_component *component)
1012 {
1013         int ret;
1014         struct mmal_msg m;
1015         struct mmal_msg *rmsg;
1016         struct vchiq_header *rmsg_handle;
1017
1018         m.h.type = MMAL_MSG_TYPE_COMPONENT_ENABLE;
1019         m.u.component_enable.component_handle = component->handle;
1020
1021         ret = send_synchronous_mmal_msg(instance, &m,
1022                                         sizeof(m.u.component_enable),
1023                                         &rmsg, &rmsg_handle);
1024         if (ret)
1025                 return ret;
1026
1027         if (rmsg->h.type != m.h.type) {
1028                 /* got an unexpected message type in reply */
1029                 ret = -EINVAL;
1030                 goto release_msg;
1031         }
1032
1033         ret = -rmsg->u.component_enable_reply.status;
1034
1035 release_msg:
1036         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1037
1038         return ret;
1039 }
1040
1041 /* disable a component on vc */
1042 static int disable_component(struct vchiq_mmal_instance *instance,
1043                              struct vchiq_mmal_component *component)
1044 {
1045         int ret;
1046         struct mmal_msg m;
1047         struct mmal_msg *rmsg;
1048         struct vchiq_header *rmsg_handle;
1049
1050         m.h.type = MMAL_MSG_TYPE_COMPONENT_DISABLE;
1051         m.u.component_disable.component_handle = component->handle;
1052
1053         ret = send_synchronous_mmal_msg(instance, &m,
1054                                         sizeof(m.u.component_disable),
1055                                         &rmsg, &rmsg_handle);
1056         if (ret)
1057                 return ret;
1058
1059         if (rmsg->h.type != m.h.type) {
1060                 /* got an unexpected message type in reply */
1061                 ret = -EINVAL;
1062                 goto release_msg;
1063         }
1064
1065         ret = -rmsg->u.component_disable_reply.status;
1066
1067 release_msg:
1068
1069         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1070
1071         return ret;
1072 }
1073
1074 /* get version of mmal implementation */
1075 static int get_version(struct vchiq_mmal_instance *instance,
1076                        u32 *major_out, u32 *minor_out)
1077 {
1078         int ret;
1079         struct mmal_msg m;
1080         struct mmal_msg *rmsg;
1081         struct vchiq_header *rmsg_handle;
1082
1083         m.h.type = MMAL_MSG_TYPE_GET_VERSION;
1084
1085         ret = send_synchronous_mmal_msg(instance, &m,
1086                                         sizeof(m.u.version),
1087                                         &rmsg, &rmsg_handle);
1088         if (ret)
1089                 return ret;
1090
1091         if (rmsg->h.type != m.h.type) {
1092                 /* got an unexpected message type in reply */
1093                 ret = -EINVAL;
1094                 goto release_msg;
1095         }
1096
1097         *major_out = rmsg->u.version.major;
1098         *minor_out = rmsg->u.version.minor;
1099
1100 release_msg:
1101         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1102
1103         return ret;
1104 }
1105
1106 /* do a port action with a port as a parameter */
1107 static int port_action_port(struct vchiq_mmal_instance *instance,
1108                             struct vchiq_mmal_port *port,
1109                             enum mmal_msg_port_action_type action_type)
1110 {
1111         int ret;
1112         struct mmal_msg m;
1113         struct mmal_msg *rmsg;
1114         struct vchiq_header *rmsg_handle;
1115
1116         m.h.type = MMAL_MSG_TYPE_PORT_ACTION;
1117         m.u.port_action_port.component_handle = port->component->handle;
1118         m.u.port_action_port.port_handle = port->handle;
1119         m.u.port_action_port.action = action_type;
1120
1121         port_to_mmal_msg(port, &m.u.port_action_port.port);
1122
1123         ret = send_synchronous_mmal_msg(instance, &m,
1124                                         sizeof(m.u.port_action_port),
1125                                         &rmsg, &rmsg_handle);
1126         if (ret)
1127                 return ret;
1128
1129         if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) {
1130                 /* got an unexpected message type in reply */
1131                 ret = -EINVAL;
1132                 goto release_msg;
1133         }
1134
1135         ret = -rmsg->u.port_action_reply.status;
1136
1137         pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d)\n",
1138                  __func__,
1139                  ret, port->component->handle, port->handle,
1140                  port_action_type_names[action_type], action_type);
1141
1142 release_msg:
1143         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1144
1145         return ret;
1146 }
1147
1148 /* do a port action with handles as parameters */
1149 static int port_action_handle(struct vchiq_mmal_instance *instance,
1150                               struct vchiq_mmal_port *port,
1151                               enum mmal_msg_port_action_type action_type,
1152                               u32 connect_component_handle,
1153                               u32 connect_port_handle)
1154 {
1155         int ret;
1156         struct mmal_msg m;
1157         struct mmal_msg *rmsg;
1158         struct vchiq_header *rmsg_handle;
1159
1160         m.h.type = MMAL_MSG_TYPE_PORT_ACTION;
1161
1162         m.u.port_action_handle.component_handle = port->component->handle;
1163         m.u.port_action_handle.port_handle = port->handle;
1164         m.u.port_action_handle.action = action_type;
1165
1166         m.u.port_action_handle.connect_component_handle =
1167             connect_component_handle;
1168         m.u.port_action_handle.connect_port_handle = connect_port_handle;
1169
1170         ret = send_synchronous_mmal_msg(instance, &m,
1171                                         sizeof(m.u.port_action_handle),
1172                                         &rmsg, &rmsg_handle);
1173         if (ret)
1174                 return ret;
1175
1176         if (rmsg->h.type != MMAL_MSG_TYPE_PORT_ACTION) {
1177                 /* got an unexpected message type in reply */
1178                 ret = -EINVAL;
1179                 goto release_msg;
1180         }
1181
1182         ret = -rmsg->u.port_action_reply.status;
1183
1184         pr_debug("%s:result:%d component:0x%x port:%d action:%s(%d) connect component:0x%x connect port:%d\n",
1185                  __func__,
1186                  ret, port->component->handle, port->handle,
1187                  port_action_type_names[action_type],
1188                  action_type, connect_component_handle, connect_port_handle);
1189
1190 release_msg:
1191         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1192
1193         return ret;
1194 }
1195
1196 static int port_parameter_set(struct vchiq_mmal_instance *instance,
1197                               struct vchiq_mmal_port *port,
1198                               u32 parameter_id, void *value, u32 value_size)
1199 {
1200         int ret;
1201         struct mmal_msg m;
1202         struct mmal_msg *rmsg;
1203         struct vchiq_header *rmsg_handle;
1204
1205         m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_SET;
1206
1207         m.u.port_parameter_set.component_handle = port->component->handle;
1208         m.u.port_parameter_set.port_handle = port->handle;
1209         m.u.port_parameter_set.id = parameter_id;
1210         m.u.port_parameter_set.size = (2 * sizeof(u32)) + value_size;
1211         memcpy(&m.u.port_parameter_set.value, value, value_size);
1212
1213         ret = send_synchronous_mmal_msg(instance, &m,
1214                                         (4 * sizeof(u32)) + value_size,
1215                                         &rmsg, &rmsg_handle);
1216         if (ret)
1217                 return ret;
1218
1219         if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_SET) {
1220                 /* got an unexpected message type in reply */
1221                 ret = -EINVAL;
1222                 goto release_msg;
1223         }
1224
1225         ret = -rmsg->u.port_parameter_set_reply.status;
1226
1227         pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n",
1228                  __func__,
1229                  ret, port->component->handle, port->handle, parameter_id);
1230
1231 release_msg:
1232         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1233
1234         return ret;
1235 }
1236
1237 static int port_parameter_get(struct vchiq_mmal_instance *instance,
1238                               struct vchiq_mmal_port *port,
1239                               u32 parameter_id, void *value, u32 *value_size)
1240 {
1241         int ret;
1242         struct mmal_msg m;
1243         struct mmal_msg *rmsg;
1244         struct vchiq_header *rmsg_handle;
1245
1246         m.h.type = MMAL_MSG_TYPE_PORT_PARAMETER_GET;
1247
1248         m.u.port_parameter_get.component_handle = port->component->handle;
1249         m.u.port_parameter_get.port_handle = port->handle;
1250         m.u.port_parameter_get.id = parameter_id;
1251         m.u.port_parameter_get.size = (2 * sizeof(u32)) + *value_size;
1252
1253         ret = send_synchronous_mmal_msg(instance, &m,
1254                                         sizeof(struct
1255                                                mmal_msg_port_parameter_get),
1256                                         &rmsg, &rmsg_handle);
1257         if (ret)
1258                 return ret;
1259
1260         if (rmsg->h.type != MMAL_MSG_TYPE_PORT_PARAMETER_GET) {
1261                 /* got an unexpected message type in reply */
1262                 pr_err("Incorrect reply type %d\n", rmsg->h.type);
1263                 ret = -EINVAL;
1264                 goto release_msg;
1265         }
1266
1267         ret = rmsg->u.port_parameter_get_reply.status;
1268
1269         /* port_parameter_get_reply.size includes the header,
1270          * whilst *value_size doesn't.
1271          */
1272         rmsg->u.port_parameter_get_reply.size -= (2 * sizeof(u32));
1273
1274         if (ret || rmsg->u.port_parameter_get_reply.size > *value_size) {
1275                 /* Copy only as much as we have space for
1276                  * but report true size of parameter
1277                  */
1278                 memcpy(value, &rmsg->u.port_parameter_get_reply.value,
1279                        *value_size);
1280         } else {
1281                 memcpy(value, &rmsg->u.port_parameter_get_reply.value,
1282                        rmsg->u.port_parameter_get_reply.size);
1283         }
1284         /* Always report the size of the returned parameter to the caller */
1285         *value_size = rmsg->u.port_parameter_get_reply.size;
1286
1287         pr_debug("%s:result:%d component:0x%x port:%d parameter:%d\n", __func__,
1288                  ret, port->component->handle, port->handle, parameter_id);
1289
1290 release_msg:
1291         vchiq_release_message(instance->vchiq_instance, instance->service_handle, rmsg_handle);
1292
1293         return ret;
1294 }
1295
1296 /* disables a port and drains buffers from it */
1297 static int port_disable(struct vchiq_mmal_instance *instance,
1298                         struct vchiq_mmal_port *port)
1299 {
1300         int ret;
1301         struct list_head *q, *buf_head;
1302         unsigned long flags = 0;
1303
1304         if (!port->enabled)
1305                 return 0;
1306
1307         port->enabled = false;
1308
1309         ret = port_action_port(instance, port,
1310                                MMAL_MSG_PORT_ACTION_TYPE_DISABLE);
1311         if (ret == 0) {
1312                 /*
1313                  * Drain all queued buffers on port. This should only
1314                  * apply to buffers that have been queued before the port
1315                  * has been enabled. If the port has been enabled and buffers
1316                  * passed, then the buffers should have been removed from this
1317                  * list, and we should get the relevant callbacks via VCHIQ
1318                  * to release the buffers.
1319                  */
1320                 spin_lock_irqsave(&port->slock, flags);
1321
1322                 list_for_each_safe(buf_head, q, &port->buffers) {
1323                         struct mmal_buffer *mmalbuf;
1324
1325                         mmalbuf = list_entry(buf_head, struct mmal_buffer,
1326                                              list);
1327                         list_del(buf_head);
1328                         if (port->buffer_cb) {
1329                                 mmalbuf->length = 0;
1330                                 mmalbuf->mmal_flags = 0;
1331                                 mmalbuf->dts = MMAL_TIME_UNKNOWN;
1332                                 mmalbuf->pts = MMAL_TIME_UNKNOWN;
1333                                 port->buffer_cb(instance,
1334                                                 port, 0, mmalbuf);
1335                         }
1336                 }
1337
1338                 spin_unlock_irqrestore(&port->slock, flags);
1339
1340                 ret = port_info_get(instance, port);
1341         }
1342
1343         return ret;
1344 }
1345
1346 /* enable a port */
1347 static int port_enable(struct vchiq_mmal_instance *instance,
1348                        struct vchiq_mmal_port *port)
1349 {
1350         unsigned int hdr_count;
1351         struct list_head *q, *buf_head;
1352         int ret;
1353
1354         if (port->enabled)
1355                 return 0;
1356
1357         ret = port_action_port(instance, port,
1358                                MMAL_MSG_PORT_ACTION_TYPE_ENABLE);
1359         if (ret)
1360                 goto done;
1361
1362         port->enabled = true;
1363
1364         if (port->buffer_cb) {
1365                 /* send buffer headers to videocore */
1366                 hdr_count = 1;
1367                 list_for_each_safe(buf_head, q, &port->buffers) {
1368                         struct mmal_buffer *mmalbuf;
1369
1370                         mmalbuf = list_entry(buf_head, struct mmal_buffer,
1371                                              list);
1372                         ret = buffer_from_host(instance, port, mmalbuf);
1373                         if (ret)
1374                                 goto done;
1375
1376                         list_del(buf_head);
1377                         hdr_count++;
1378                         if (hdr_count > port->current_buffer.num)
1379                                 break;
1380                 }
1381         }
1382
1383         ret = port_info_get(instance, port);
1384
1385 done:
1386         return ret;
1387 }
1388
1389 /* ------------------------------------------------------------------
1390  * Exported API
1391  *------------------------------------------------------------------
1392  */
1393
1394 int vchiq_mmal_port_set_format(struct vchiq_mmal_instance *instance,
1395                                struct vchiq_mmal_port *port)
1396 {
1397         int ret;
1398
1399         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1400                 return -EINTR;
1401
1402         ret = port_info_set(instance, port);
1403         if (ret)
1404                 goto release_unlock;
1405
1406         /* read what has actually been set */
1407         ret = port_info_get(instance, port);
1408
1409 release_unlock:
1410         mutex_unlock(&instance->vchiq_mutex);
1411
1412         return ret;
1413 }
1414 EXPORT_SYMBOL_GPL(vchiq_mmal_port_set_format);
1415
1416 int vchiq_mmal_port_parameter_set(struct vchiq_mmal_instance *instance,
1417                                   struct vchiq_mmal_port *port,
1418                                   u32 parameter, void *value, u32 value_size)
1419 {
1420         int ret;
1421
1422         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1423                 return -EINTR;
1424
1425         ret = port_parameter_set(instance, port, parameter, value, value_size);
1426
1427         mutex_unlock(&instance->vchiq_mutex);
1428
1429         return ret;
1430 }
1431 EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_set);
1432
1433 int vchiq_mmal_port_parameter_get(struct vchiq_mmal_instance *instance,
1434                                   struct vchiq_mmal_port *port,
1435                                   u32 parameter, void *value, u32 *value_size)
1436 {
1437         int ret;
1438
1439         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1440                 return -EINTR;
1441
1442         ret = port_parameter_get(instance, port, parameter, value, value_size);
1443
1444         mutex_unlock(&instance->vchiq_mutex);
1445
1446         return ret;
1447 }
1448 EXPORT_SYMBOL_GPL(vchiq_mmal_port_parameter_get);
1449
1450 /* enable a port
1451  *
1452  * enables a port and queues buffers for satisfying callbacks if we
1453  * provide a callback handler
1454  */
1455 int vchiq_mmal_port_enable(struct vchiq_mmal_instance *instance,
1456                            struct vchiq_mmal_port *port,
1457                            vchiq_mmal_buffer_cb buffer_cb)
1458 {
1459         int ret;
1460
1461         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1462                 return -EINTR;
1463
1464         /* already enabled - noop */
1465         if (port->enabled) {
1466                 ret = 0;
1467                 goto unlock;
1468         }
1469
1470         port->buffer_cb = buffer_cb;
1471
1472         ret = port_enable(instance, port);
1473
1474 unlock:
1475         mutex_unlock(&instance->vchiq_mutex);
1476
1477         return ret;
1478 }
1479 EXPORT_SYMBOL_GPL(vchiq_mmal_port_enable);
1480
1481 int vchiq_mmal_port_disable(struct vchiq_mmal_instance *instance,
1482                             struct vchiq_mmal_port *port)
1483 {
1484         int ret;
1485
1486         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1487                 return -EINTR;
1488
1489         if (!port->enabled) {
1490                 mutex_unlock(&instance->vchiq_mutex);
1491                 return 0;
1492         }
1493
1494         ret = port_disable(instance, port);
1495
1496         mutex_unlock(&instance->vchiq_mutex);
1497
1498         return ret;
1499 }
1500 EXPORT_SYMBOL_GPL(vchiq_mmal_port_disable);
1501
1502 /* ports will be connected in a tunneled manner so data buffers
1503  * are not handled by client.
1504  */
1505 int vchiq_mmal_port_connect_tunnel(struct vchiq_mmal_instance *instance,
1506                                    struct vchiq_mmal_port *src,
1507                                    struct vchiq_mmal_port *dst)
1508 {
1509         int ret;
1510
1511         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1512                 return -EINTR;
1513
1514         /* disconnect ports if connected */
1515         if (src->connected) {
1516                 ret = port_disable(instance, src);
1517                 if (ret) {
1518                         pr_err("failed disabling src port(%d)\n", ret);
1519                         goto release_unlock;
1520                 }
1521
1522                 /* do not need to disable the destination port as they
1523                  * are connected and it is done automatically
1524                  */
1525
1526                 ret = port_action_handle(instance, src,
1527                                          MMAL_MSG_PORT_ACTION_TYPE_DISCONNECT,
1528                                          src->connected->component->handle,
1529                                          src->connected->handle);
1530                 if (ret < 0) {
1531                         pr_err("failed disconnecting src port\n");
1532                         goto release_unlock;
1533                 }
1534                 src->connected->enabled = false;
1535                 src->connected = NULL;
1536         }
1537
1538         if (!dst) {
1539                 /* do not make new connection */
1540                 ret = 0;
1541                 pr_debug("not making new connection\n");
1542                 goto release_unlock;
1543         }
1544
1545         /* copy src port format to dst */
1546         dst->format.encoding = src->format.encoding;
1547         dst->es.video.width = src->es.video.width;
1548         dst->es.video.height = src->es.video.height;
1549         dst->es.video.crop.x = src->es.video.crop.x;
1550         dst->es.video.crop.y = src->es.video.crop.y;
1551         dst->es.video.crop.width = src->es.video.crop.width;
1552         dst->es.video.crop.height = src->es.video.crop.height;
1553         dst->es.video.frame_rate.numerator = src->es.video.frame_rate.numerator;
1554         dst->es.video.frame_rate.denominator = src->es.video.frame_rate.denominator;
1555
1556         /* set new format */
1557         ret = port_info_set(instance, dst);
1558         if (ret) {
1559                 pr_debug("setting port info failed\n");
1560                 goto release_unlock;
1561         }
1562
1563         /* read what has actually been set */
1564         ret = port_info_get(instance, dst);
1565         if (ret) {
1566                 pr_debug("read back port info failed\n");
1567                 goto release_unlock;
1568         }
1569
1570         /* connect two ports together */
1571         ret = port_action_handle(instance, src,
1572                                  MMAL_MSG_PORT_ACTION_TYPE_CONNECT,
1573                                  dst->component->handle, dst->handle);
1574         if (ret < 0) {
1575                 pr_debug("connecting port %d:%d to %d:%d failed\n",
1576                          src->component->handle, src->handle,
1577                          dst->component->handle, dst->handle);
1578                 goto release_unlock;
1579         }
1580         src->connected = dst;
1581
1582 release_unlock:
1583
1584         mutex_unlock(&instance->vchiq_mutex);
1585
1586         return ret;
1587 }
1588 EXPORT_SYMBOL_GPL(vchiq_mmal_port_connect_tunnel);
1589
1590 int vchiq_mmal_submit_buffer(struct vchiq_mmal_instance *instance,
1591                              struct vchiq_mmal_port *port,
1592                              struct mmal_buffer *buffer)
1593 {
1594         unsigned long flags = 0;
1595         int ret;
1596
1597         ret = buffer_from_host(instance, port, buffer);
1598         if (ret == -EINVAL) {
1599                 /* Port is disabled. Queue for when it is enabled. */
1600                 spin_lock_irqsave(&port->slock, flags);
1601                 list_add_tail(&buffer->list, &port->buffers);
1602                 spin_unlock_irqrestore(&port->slock, flags);
1603         }
1604
1605         return 0;
1606 }
1607 EXPORT_SYMBOL_GPL(vchiq_mmal_submit_buffer);
1608
1609 int mmal_vchi_buffer_init(struct vchiq_mmal_instance *instance,
1610                           struct mmal_buffer *buf)
1611 {
1612         struct mmal_msg_context *msg_context = get_msg_context(instance);
1613
1614         if (IS_ERR(msg_context))
1615                 return (PTR_ERR(msg_context));
1616
1617         buf->msg_context = msg_context;
1618         return 0;
1619 }
1620 EXPORT_SYMBOL_GPL(mmal_vchi_buffer_init);
1621
1622 int mmal_vchi_buffer_cleanup(struct mmal_buffer *buf)
1623 {
1624         struct mmal_msg_context *msg_context = buf->msg_context;
1625
1626         if (msg_context)
1627                 release_msg_context(msg_context);
1628         buf->msg_context = NULL;
1629
1630         return 0;
1631 }
1632 EXPORT_SYMBOL_GPL(mmal_vchi_buffer_cleanup);
1633
1634 /* Initialise a mmal component and its ports
1635  *
1636  */
1637 int vchiq_mmal_component_init(struct vchiq_mmal_instance *instance,
1638                               const char *name,
1639                               struct vchiq_mmal_component **component_out)
1640 {
1641         int ret;
1642         int idx;                /* port index */
1643         struct vchiq_mmal_component *component = NULL;
1644
1645         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1646                 return -EINTR;
1647
1648         for (idx = 0; idx < VCHIQ_MMAL_MAX_COMPONENTS; idx++) {
1649                 if (!instance->component[idx].in_use) {
1650                         component = &instance->component[idx];
1651                         component->in_use = true;
1652                         break;
1653                 }
1654         }
1655
1656         if (!component) {
1657                 ret = -EINVAL;  /* todo is this correct error? */
1658                 goto unlock;
1659         }
1660
1661         /* We need a handle to reference back to our component structure.
1662          * Use the array index in instance->component rather than rolling
1663          * another IDR.
1664          */
1665         component->client_component = idx;
1666
1667         ret = create_component(instance, component, name);
1668         if (ret < 0) {
1669                 pr_err("%s: failed to create component %d (Not enough GPU mem?)\n",
1670                        __func__, ret);
1671                 goto unlock;
1672         }
1673
1674         /* ports info needs gathering */
1675         component->control.type = MMAL_PORT_TYPE_CONTROL;
1676         component->control.index = 0;
1677         component->control.component = component;
1678         spin_lock_init(&component->control.slock);
1679         INIT_LIST_HEAD(&component->control.buffers);
1680         ret = port_info_get(instance, &component->control);
1681         if (ret < 0)
1682                 goto release_component;
1683
1684         for (idx = 0; idx < component->inputs; idx++) {
1685                 component->input[idx].type = MMAL_PORT_TYPE_INPUT;
1686                 component->input[idx].index = idx;
1687                 component->input[idx].component = component;
1688                 spin_lock_init(&component->input[idx].slock);
1689                 INIT_LIST_HEAD(&component->input[idx].buffers);
1690                 ret = port_info_get(instance, &component->input[idx]);
1691                 if (ret < 0)
1692                         goto release_component;
1693         }
1694
1695         for (idx = 0; idx < component->outputs; idx++) {
1696                 component->output[idx].type = MMAL_PORT_TYPE_OUTPUT;
1697                 component->output[idx].index = idx;
1698                 component->output[idx].component = component;
1699                 spin_lock_init(&component->output[idx].slock);
1700                 INIT_LIST_HEAD(&component->output[idx].buffers);
1701                 ret = port_info_get(instance, &component->output[idx]);
1702                 if (ret < 0)
1703                         goto release_component;
1704         }
1705
1706         for (idx = 0; idx < component->clocks; idx++) {
1707                 component->clock[idx].type = MMAL_PORT_TYPE_CLOCK;
1708                 component->clock[idx].index = idx;
1709                 component->clock[idx].component = component;
1710                 spin_lock_init(&component->clock[idx].slock);
1711                 INIT_LIST_HEAD(&component->clock[idx].buffers);
1712                 ret = port_info_get(instance, &component->clock[idx]);
1713                 if (ret < 0)
1714                         goto release_component;
1715         }
1716
1717         *component_out = component;
1718
1719         mutex_unlock(&instance->vchiq_mutex);
1720
1721         return 0;
1722
1723 release_component:
1724         destroy_component(instance, component);
1725 unlock:
1726         if (component)
1727                 component->in_use = false;
1728         mutex_unlock(&instance->vchiq_mutex);
1729
1730         return ret;
1731 }
1732 EXPORT_SYMBOL_GPL(vchiq_mmal_component_init);
1733
1734 /*
1735  * cause a mmal component to be destroyed
1736  */
1737 int vchiq_mmal_component_finalise(struct vchiq_mmal_instance *instance,
1738                                   struct vchiq_mmal_component *component)
1739 {
1740         int ret;
1741
1742         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1743                 return -EINTR;
1744
1745         if (component->enabled)
1746                 ret = disable_component(instance, component);
1747
1748         ret = destroy_component(instance, component);
1749
1750         component->in_use = false;
1751
1752         mutex_unlock(&instance->vchiq_mutex);
1753
1754         return ret;
1755 }
1756 EXPORT_SYMBOL_GPL(vchiq_mmal_component_finalise);
1757
1758 /*
1759  * cause a mmal component to be enabled
1760  */
1761 int vchiq_mmal_component_enable(struct vchiq_mmal_instance *instance,
1762                                 struct vchiq_mmal_component *component)
1763 {
1764         int ret;
1765
1766         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1767                 return -EINTR;
1768
1769         if (component->enabled) {
1770                 mutex_unlock(&instance->vchiq_mutex);
1771                 return 0;
1772         }
1773
1774         ret = enable_component(instance, component);
1775         if (ret == 0)
1776                 component->enabled = true;
1777
1778         mutex_unlock(&instance->vchiq_mutex);
1779
1780         return ret;
1781 }
1782 EXPORT_SYMBOL_GPL(vchiq_mmal_component_enable);
1783
1784 /*
1785  * cause a mmal component to be enabled
1786  */
1787 int vchiq_mmal_component_disable(struct vchiq_mmal_instance *instance,
1788                                  struct vchiq_mmal_component *component)
1789 {
1790         int ret;
1791
1792         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1793                 return -EINTR;
1794
1795         if (!component->enabled) {
1796                 mutex_unlock(&instance->vchiq_mutex);
1797                 return 0;
1798         }
1799
1800         ret = disable_component(instance, component);
1801         if (ret == 0)
1802                 component->enabled = false;
1803
1804         mutex_unlock(&instance->vchiq_mutex);
1805
1806         return ret;
1807 }
1808 EXPORT_SYMBOL_GPL(vchiq_mmal_component_disable);
1809
1810 int vchiq_mmal_version(struct vchiq_mmal_instance *instance,
1811                        u32 *major_out, u32 *minor_out)
1812 {
1813         int ret;
1814
1815         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1816                 return -EINTR;
1817
1818         ret = get_version(instance, major_out, minor_out);
1819
1820         mutex_unlock(&instance->vchiq_mutex);
1821
1822         return ret;
1823 }
1824 EXPORT_SYMBOL_GPL(vchiq_mmal_version);
1825
1826 int vchiq_mmal_finalise(struct vchiq_mmal_instance *instance)
1827 {
1828         int status = 0;
1829
1830         if (!instance)
1831                 return -EINVAL;
1832
1833         if (mutex_lock_interruptible(&instance->vchiq_mutex))
1834                 return -EINTR;
1835
1836         vchiq_use_service(instance->vchiq_instance, instance->service_handle);
1837
1838         status = vchiq_close_service(instance->vchiq_instance, instance->service_handle);
1839         if (status != 0)
1840                 pr_err("mmal-vchiq: VCHIQ close failed\n");
1841
1842         mutex_unlock(&instance->vchiq_mutex);
1843
1844         vchiq_shutdown(instance->vchiq_instance);
1845         destroy_workqueue(instance->bulk_wq);
1846
1847         idr_destroy(&instance->context_map);
1848
1849         kfree(instance);
1850
1851         return status;
1852 }
1853 EXPORT_SYMBOL_GPL(vchiq_mmal_finalise);
1854
1855 int vchiq_mmal_init(struct vchiq_mmal_instance **out_instance)
1856 {
1857         int status;
1858         int err = -ENODEV;
1859         struct vchiq_mmal_instance *instance;
1860         struct vchiq_instance *vchiq_instance;
1861         struct vchiq_service_params_kernel params = {
1862                 .version                = VC_MMAL_VER,
1863                 .version_min            = VC_MMAL_MIN_VER,
1864                 .fourcc                 = VCHIQ_MAKE_FOURCC('m', 'm', 'a', 'l'),
1865                 .callback               = service_callback,
1866                 .userdata               = NULL,
1867         };
1868
1869         /* compile time checks to ensure structure size as they are
1870          * directly (de)serialised from memory.
1871          */
1872
1873         /* ensure the header structure has packed to the correct size */
1874         BUILD_BUG_ON(sizeof(struct mmal_msg_header) != 24);
1875
1876         /* ensure message structure does not exceed maximum length */
1877         BUILD_BUG_ON(sizeof(struct mmal_msg) > MMAL_MSG_MAX_SIZE);
1878
1879         /* mmal port struct is correct size */
1880         BUILD_BUG_ON(sizeof(struct mmal_port) != 64);
1881
1882         /* create a vchi instance */
1883         status = vchiq_initialise(&vchiq_instance);
1884         if (status) {
1885                 pr_err("Failed to initialise VCHI instance (status=%d)\n",
1886                        status);
1887                 return -EIO;
1888         }
1889
1890         status = vchiq_connect(vchiq_instance);
1891         if (status) {
1892                 pr_err("Failed to connect VCHI instance (status=%d)\n", status);
1893                 err = -EIO;
1894                 goto err_shutdown_vchiq;
1895         }
1896
1897         instance = kzalloc(sizeof(*instance), GFP_KERNEL);
1898
1899         if (!instance) {
1900                 err = -ENOMEM;
1901                 goto err_shutdown_vchiq;
1902         }
1903
1904         mutex_init(&instance->vchiq_mutex);
1905
1906         instance->vchiq_instance = vchiq_instance;
1907
1908         mutex_init(&instance->context_map_lock);
1909         idr_init_base(&instance->context_map, 1);
1910
1911         params.userdata = instance;
1912
1913         instance->bulk_wq = alloc_ordered_workqueue("mmal-vchiq",
1914                                                     WQ_MEM_RECLAIM);
1915         if (!instance->bulk_wq)
1916                 goto err_free;
1917
1918         status = vchiq_open_service(vchiq_instance, &params,
1919                                     &instance->service_handle);
1920         if (status) {
1921                 pr_err("Failed to open VCHI service connection (status=%d)\n",
1922                        status);
1923                 goto err_close_services;
1924         }
1925
1926         vchiq_release_service(instance->vchiq_instance, instance->service_handle);
1927
1928         *out_instance = instance;
1929
1930         return 0;
1931
1932 err_close_services:
1933         vchiq_close_service(instance->vchiq_instance, instance->service_handle);
1934         destroy_workqueue(instance->bulk_wq);
1935 err_free:
1936         kfree(instance);
1937 err_shutdown_vchiq:
1938         vchiq_shutdown(vchiq_instance);
1939         return err;
1940 }
1941 EXPORT_SYMBOL_GPL(vchiq_mmal_init);
1942
1943 MODULE_DESCRIPTION("BCM2835 MMAL VCHIQ interface");
1944 MODULE_AUTHOR("Dave Stevenson, <dave.stevenson@raspberrypi.org>");
1945 MODULE_LICENSE("GPL");