LCOV - code coverage report
Current view: top level - drivers/staging/vc04_services/bcm2835-camera - bcm2835-camera.c (source / functions) Hit Total Coverage
Test: gcov_data_raspi2_real_modules_combined.info Lines: 19 746 2.5 %
Date: 2020-09-30 20:25:40 Functions: 3 39 7.7 %
Branches: 5 418 1.2 %

           Branch data     Line data    Source code
       1                 :            : // SPDX-License-Identifier: GPL-2.0
       2                 :            : /*
       3                 :            :  * Broadcom BM2835 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                 :            : 
      14                 :            : #include <linux/errno.h>
      15                 :            : #include <linux/kernel.h>
      16                 :            : #include <linux/module.h>
      17                 :            : #include <linux/slab.h>
      18                 :            : #include <media/videobuf2-vmalloc.h>
      19                 :            : #include <media/videobuf2-dma-contig.h>
      20                 :            : #include <media/v4l2-device.h>
      21                 :            : #include <media/v4l2-ioctl.h>
      22                 :            : #include <media/v4l2-ctrls.h>
      23                 :            : #include <media/v4l2-fh.h>
      24                 :            : #include <media/v4l2-event.h>
      25                 :            : #include <media/v4l2-common.h>
      26                 :            : #include <linux/delay.h>
      27                 :            : #include <linux/platform_device.h>
      28                 :            : 
      29                 :            : #include "mmal-common.h"
      30                 :            : #include "mmal-encodings.h"
      31                 :            : #include "mmal-vchiq.h"
      32                 :            : #include "mmal-msg.h"
      33                 :            : #include "mmal-parameters.h"
      34                 :            : #include "bcm2835-camera.h"
      35                 :            : 
      36                 :            : #define BM2835_MMAL_VERSION "0.0.2"
      37                 :            : #define BM2835_MMAL_MODULE_NAME "bcm2835-v4l2"
      38                 :            : #define MIN_WIDTH 32
      39                 :            : #define MIN_HEIGHT 32
      40                 :            : #define MIN_BUFFER_SIZE (80 * 1024)
      41                 :            : 
      42                 :            : #define MAX_VIDEO_MODE_WIDTH 1280
      43                 :            : #define MAX_VIDEO_MODE_HEIGHT 720
      44                 :            : 
      45                 :            : #define MAX_BCM2835_CAMERAS 2
      46                 :            : 
      47                 :            : int bcm2835_v4l2_debug;
      48                 :            : module_param_named(debug, bcm2835_v4l2_debug, int, 0644);
      49                 :            : MODULE_PARM_DESC(bcm2835_v4l2_debug, "Debug level 0-2");
      50                 :            : 
      51                 :            : #define UNSET (-1)
      52                 :            : static int video_nr[] = {[0 ... (MAX_BCM2835_CAMERAS - 1)] = UNSET };
      53                 :            : module_param_array(video_nr, int, NULL, 0644);
      54                 :            : MODULE_PARM_DESC(video_nr, "videoX start numbers, -1 is autodetect");
      55                 :            : 
      56                 :            : static int max_video_width = MAX_VIDEO_MODE_WIDTH;
      57                 :            : static int max_video_height = MAX_VIDEO_MODE_HEIGHT;
      58                 :            : module_param(max_video_width, int, 0644);
      59                 :            : MODULE_PARM_DESC(max_video_width, "Threshold for video mode");
      60                 :            : module_param(max_video_height, int, 0644);
      61                 :            : MODULE_PARM_DESC(max_video_height, "Threshold for video mode");
      62                 :            : 
      63                 :            : /* global device data array */
      64                 :            : static struct bm2835_mmal_dev *gdev[MAX_BCM2835_CAMERAS];
      65                 :            : 
      66                 :            : #define FPS_MIN 1
      67                 :            : #define FPS_MAX 90
      68                 :            : 
      69                 :            : /* timeperframe: min/max and default */
      70                 :            : static const struct v4l2_fract
      71                 :            :         tpf_min     = {.numerator = 1,          .denominator = FPS_MAX},
      72                 :            :         tpf_max     = {.numerator = 1,          .denominator = FPS_MIN},
      73                 :            :         tpf_default = {.numerator = 1000,       .denominator = 30000};
      74                 :            : 
      75                 :            : /* Container for MMAL and VB2 buffers*/
      76                 :            : struct vb2_mmal_buffer {
      77                 :            :         struct vb2_v4l2_buffer  vb;
      78                 :            :         struct mmal_buffer      mmal;
      79                 :            : };
      80                 :            : 
      81                 :            : /* video formats */
      82                 :            : static struct mmal_fmt formats[] = {
      83                 :            :         {
      84                 :            :                 .fourcc = V4L2_PIX_FMT_YUV420,
      85                 :            :                 .mmal = MMAL_ENCODING_I420,
      86                 :            :                 .depth = 12,
      87                 :            :                 .mmal_component = COMP_CAMERA,
      88                 :            :                 .ybbp = 1,
      89                 :            :                 .remove_padding = 1,
      90                 :            :         }, {
      91                 :            :                 .fourcc = V4L2_PIX_FMT_YUYV,
      92                 :            :                 .mmal = MMAL_ENCODING_YUYV,
      93                 :            :                 .depth = 16,
      94                 :            :                 .mmal_component = COMP_CAMERA,
      95                 :            :                 .ybbp = 2,
      96                 :            :                 .remove_padding = 0,
      97                 :            :         }, {
      98                 :            :                 .fourcc = V4L2_PIX_FMT_RGB24,
      99                 :            :                 .mmal = MMAL_ENCODING_RGB24,
     100                 :            :                 .depth = 24,
     101                 :            :                 .mmal_component = COMP_CAMERA,
     102                 :            :                 .ybbp = 3,
     103                 :            :                 .remove_padding = 0,
     104                 :            :         }, {
     105                 :            :                 .fourcc = V4L2_PIX_FMT_JPEG,
     106                 :            :                 .flags = V4L2_FMT_FLAG_COMPRESSED,
     107                 :            :                 .mmal = MMAL_ENCODING_JPEG,
     108                 :            :                 .depth = 8,
     109                 :            :                 .mmal_component = COMP_IMAGE_ENCODE,
     110                 :            :                 .ybbp = 0,
     111                 :            :                 .remove_padding = 0,
     112                 :            :         }, {
     113                 :            :                 .fourcc = V4L2_PIX_FMT_H264,
     114                 :            :                 .flags = V4L2_FMT_FLAG_COMPRESSED,
     115                 :            :                 .mmal = MMAL_ENCODING_H264,
     116                 :            :                 .depth = 8,
     117                 :            :                 .mmal_component = COMP_VIDEO_ENCODE,
     118                 :            :                 .ybbp = 0,
     119                 :            :                 .remove_padding = 0,
     120                 :            :         }, {
     121                 :            :                 .fourcc = V4L2_PIX_FMT_MJPEG,
     122                 :            :                 .flags = V4L2_FMT_FLAG_COMPRESSED,
     123                 :            :                 .mmal = MMAL_ENCODING_MJPEG,
     124                 :            :                 .depth = 8,
     125                 :            :                 .mmal_component = COMP_VIDEO_ENCODE,
     126                 :            :                 .ybbp = 0,
     127                 :            :                 .remove_padding = 0,
     128                 :            :         }, {
     129                 :            :                 .fourcc = V4L2_PIX_FMT_YVYU,
     130                 :            :                 .mmal = MMAL_ENCODING_YVYU,
     131                 :            :                 .depth = 16,
     132                 :            :                 .mmal_component = COMP_CAMERA,
     133                 :            :                 .ybbp = 2,
     134                 :            :                 .remove_padding = 0,
     135                 :            :         }, {
     136                 :            :                 .fourcc = V4L2_PIX_FMT_VYUY,
     137                 :            :                 .mmal = MMAL_ENCODING_VYUY,
     138                 :            :                 .depth = 16,
     139                 :            :                 .mmal_component = COMP_CAMERA,
     140                 :            :                 .ybbp = 2,
     141                 :            :                 .remove_padding = 0,
     142                 :            :         }, {
     143                 :            :                 .fourcc = V4L2_PIX_FMT_UYVY,
     144                 :            :                 .mmal = MMAL_ENCODING_UYVY,
     145                 :            :                 .depth = 16,
     146                 :            :                 .mmal_component = COMP_CAMERA,
     147                 :            :                 .ybbp = 2,
     148                 :            :                 .remove_padding = 0,
     149                 :            :         }, {
     150                 :            :                 .fourcc = V4L2_PIX_FMT_NV12,
     151                 :            :                 .mmal = MMAL_ENCODING_NV12,
     152                 :            :                 .depth = 12,
     153                 :            :                 .mmal_component = COMP_CAMERA,
     154                 :            :                 .ybbp = 1,
     155                 :            :                 .remove_padding = 1,
     156                 :            :         }, {
     157                 :            :                 .fourcc = V4L2_PIX_FMT_BGR24,
     158                 :            :                 .mmal = MMAL_ENCODING_BGR24,
     159                 :            :                 .depth = 24,
     160                 :            :                 .mmal_component = COMP_CAMERA,
     161                 :            :                 .ybbp = 3,
     162                 :            :                 .remove_padding = 0,
     163                 :            :         }, {
     164                 :            :                 .fourcc = V4L2_PIX_FMT_YVU420,
     165                 :            :                 .mmal = MMAL_ENCODING_YV12,
     166                 :            :                 .depth = 12,
     167                 :            :                 .mmal_component = COMP_CAMERA,
     168                 :            :                 .ybbp = 1,
     169                 :            :                 .remove_padding = 1,
     170                 :            :         }, {
     171                 :            :                 .fourcc = V4L2_PIX_FMT_NV21,
     172                 :            :                 .mmal = MMAL_ENCODING_NV21,
     173                 :            :                 .depth = 12,
     174                 :            :                 .mmal_component = COMP_CAMERA,
     175                 :            :                 .ybbp = 1,
     176                 :            :                 .remove_padding = 1,
     177                 :            :         }, {
     178                 :            :                 .fourcc = V4L2_PIX_FMT_BGR32,
     179                 :            :                 .mmal = MMAL_ENCODING_BGRA,
     180                 :            :                 .depth = 32,
     181                 :            :                 .mmal_component = COMP_CAMERA,
     182                 :            :                 .ybbp = 4,
     183                 :            :                 .remove_padding = 0,
     184                 :            :         },
     185                 :            : };
     186                 :            : 
     187                 :            : static struct mmal_fmt *get_format(struct v4l2_format *f)
     188                 :            : {
     189                 :            :         struct mmal_fmt *fmt;
     190                 :            :         unsigned int k;
     191                 :            : 
     192   [ #  #  #  #  :          0 :         for (k = 0; k < ARRAY_SIZE(formats); k++) {
             #  #  #  # ]
     193                 :          0 :                 fmt = &formats[k];
     194   [ #  #  #  #  :          0 :                 if (fmt->fourcc == f->fmt.pix.pixelformat)
             #  #  #  # ]
     195                 :          0 :                         return fmt;
     196                 :            :         }
     197                 :            : 
     198                 :            :         return NULL;
     199                 :            : }
     200                 :            : 
     201                 :            : /* ------------------------------------------------------------------
     202                 :            :  *      Videobuf queue operations
     203                 :            :  * ------------------------------------------------------------------
     204                 :            :  */
     205                 :            : 
     206                 :          0 : static int queue_setup(struct vb2_queue *vq,
     207                 :            :                        unsigned int *nbuffers, unsigned int *nplanes,
     208                 :            :                        unsigned int sizes[], struct device *alloc_ctxs[])
     209                 :            : {
     210                 :            :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
     211                 :            :         unsigned long size;
     212                 :            : 
     213                 :            :         /* refuse queue setup if port is not configured */
     214         [ #  # ]:          0 :         if (!dev->capture.port) {
     215                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     216                 :            :                          "%s: capture port not configured\n", __func__);
     217                 :          0 :                 return -EINVAL;
     218                 :            :         }
     219                 :            : 
     220                 :            :         /* Handle CREATE_BUFS situation - *nplanes != 0 */
     221         [ #  # ]:          0 :         if (*nplanes) {
     222   [ #  #  #  # ]:          0 :                 if (*nplanes != 1 ||
     223                 :          0 :                     sizes[0] < dev->capture.port->current_buffer.size) {
     224         [ #  # ]:          0 :                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     225                 :            :                                  "%s: dev:%p Invalid buffer request from CREATE_BUFS, size %u < %u, nplanes %u != 1\n",
     226                 :            :                                  __func__, dev, sizes[0],
     227                 :            :                                  dev->capture.port->current_buffer.size,
     228                 :            :                                  *nplanes);
     229                 :            :                         return -EINVAL;
     230                 :            :                 } else {
     231                 :            :                         return 0;
     232                 :            :                 }
     233                 :            :         }
     234                 :            : 
     235                 :            :         /* Handle REQBUFS situation */
     236                 :          0 :         size = dev->capture.port->current_buffer.size;
     237         [ #  # ]:          0 :         if (size == 0) {
     238                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     239                 :            :                          "%s: capture port buffer size is zero\n", __func__);
     240                 :          0 :                 return -EINVAL;
     241                 :            :         }
     242                 :            : 
     243         [ #  # ]:          0 :         if (*nbuffers < dev->capture.port->minimum_buffer.num)
     244                 :          0 :                 *nbuffers = dev->capture.port->minimum_buffer.num;
     245                 :            : 
     246                 :          0 :         dev->capture.port->current_buffer.num = *nbuffers;
     247                 :            : 
     248                 :          0 :         *nplanes = 1;
     249                 :            : 
     250                 :          0 :         sizes[0] = size;
     251                 :            : 
     252                 :            :         /*
     253                 :            :          * videobuf2-vmalloc allocator is context-less so no need to set
     254                 :            :          * alloc_ctxs array.
     255                 :            :          */
     256                 :            : 
     257         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
     258                 :            :                  __func__, dev);
     259                 :            : 
     260                 :            :         return 0;
     261                 :            : }
     262                 :            : 
     263                 :          0 : static int buffer_init(struct vb2_buffer *vb)
     264                 :            : {
     265                 :          0 :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
     266                 :            :         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
     267                 :            :         struct vb2_mmal_buffer *buf =
     268                 :            :                                 container_of(vb2, struct vb2_mmal_buffer, vb);
     269                 :            : 
     270         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
     271                 :            :                  __func__, dev, vb);
     272                 :          0 :         buf->mmal.buffer = vb2_plane_vaddr(&buf->vb.vb2_buf, 0);
     273                 :          0 :         buf->mmal.buffer_size = vb2_plane_size(&buf->vb.vb2_buf, 0);
     274                 :            : 
     275                 :          0 :         return mmal_vchi_buffer_init(dev->instance, &buf->mmal);
     276                 :            : }
     277                 :            : 
     278                 :          0 : static int buffer_prepare(struct vb2_buffer *vb)
     279                 :            : {
     280                 :          0 :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
     281                 :            :         unsigned long size;
     282                 :            : 
     283         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
     284                 :            :                  __func__, dev, vb);
     285                 :            : 
     286   [ #  #  #  # ]:          0 :         if (!dev->capture.port || !dev->capture.fmt)
     287                 :            :                 return -ENODEV;
     288                 :            : 
     289                 :          0 :         size = dev->capture.stride * dev->capture.height;
     290         [ #  # ]:          0 :         if (vb2_plane_size(vb, 0) < size) {
     291                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     292                 :            :                          "%s data will not fit into plane (%lu < %lu)\n",
     293                 :            :                          __func__, vb2_plane_size(vb, 0), size);
     294                 :          0 :                 return -EINVAL;
     295                 :            :         }
     296                 :            : 
     297                 :            :         return 0;
     298                 :            : }
     299                 :            : 
     300                 :          0 : static void buffer_cleanup(struct vb2_buffer *vb)
     301                 :            : {
     302                 :          0 :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
     303                 :            :         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
     304                 :            :         struct vb2_mmal_buffer *buf =
     305                 :            :                                 container_of(vb2, struct vb2_mmal_buffer, vb);
     306                 :            : 
     307         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p, vb %p\n",
     308                 :            :                  __func__, dev, vb);
     309                 :            : 
     310                 :          0 :         mmal_vchi_buffer_cleanup(&buf->mmal);
     311                 :          0 : }
     312                 :            : 
     313                 :            : static inline bool is_capturing(struct bm2835_mmal_dev *dev)
     314                 :            : {
     315                 :          0 :         return dev->capture.camera_port ==
     316                 :          0 :             &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
     317                 :            : }
     318                 :            : 
     319                 :          0 : static void buffer_cb(struct vchiq_mmal_instance *instance,
     320                 :            :                       struct vchiq_mmal_port *port,
     321                 :            :                       int status,
     322                 :            :                       struct mmal_buffer *mmal_buf)
     323                 :            : {
     324                 :          0 :         struct bm2835_mmal_dev *dev = port->cb_ctx;
     325                 :            :         struct vb2_mmal_buffer *buf =
     326                 :          0 :                         container_of(mmal_buf, struct vb2_mmal_buffer, mmal);
     327                 :            : 
     328         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     329                 :            :                  "%s: status:%d, buf:%p, length:%lu, flags %u, pts %lld\n",
     330                 :            :                  __func__, status, buf, mmal_buf->length, mmal_buf->mmal_flags,
     331                 :            :                  mmal_buf->pts);
     332                 :            : 
     333         [ #  # ]:          0 :         if (status) {
     334                 :            :                 /* error in transfer */
     335         [ #  # ]:          0 :                 if (buf) {
     336                 :            :                         /* there was a buffer with the error so return it */
     337                 :          0 :                         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
     338                 :            :                 }
     339                 :            :                 return;
     340                 :            :         }
     341                 :            : 
     342         [ #  # ]:          0 :         if (mmal_buf->length == 0) {
     343                 :            :                 /* stream ended */
     344         [ #  # ]:          0 :                 if (dev->capture.frame_count) {
     345                 :            :                         /* empty buffer whilst capturing - expected to be an
     346                 :            :                          * EOS, so grab another frame
     347                 :            :                          */
     348         [ #  # ]:          0 :                         if (is_capturing(dev)) {
     349         [ #  # ]:          0 :                                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     350                 :            :                                          "Grab another frame");
     351                 :          0 :                                 vchiq_mmal_port_parameter_set(
     352                 :            :                                         instance,
     353                 :            :                                         dev->capture.camera_port,
     354                 :            :                                         MMAL_PARAMETER_CAPTURE,
     355                 :          0 :                                         &dev->capture.frame_count,
     356                 :            :                                         sizeof(dev->capture.frame_count));
     357                 :            :                         }
     358         [ #  # ]:          0 :                         if (vchiq_mmal_submit_buffer(instance, port,
     359                 :            :                                                      &buf->mmal))
     360         [ #  # ]:          0 :                                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     361                 :            :                                          "Failed to return EOS buffer");
     362                 :            :                 } else {
     363                 :            :                         /* stopping streaming.
     364                 :            :                          * return buffer, and signal frame completion
     365                 :            :                          */
     366                 :          0 :                         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
     367                 :          0 :                         complete(&dev->capture.frame_cmplt);
     368                 :            :                 }
     369                 :            :                 return;
     370                 :            :         }
     371                 :            : 
     372         [ #  # ]:          0 :         if (!dev->capture.frame_count) {
     373                 :            :                 /* signal frame completion */
     374                 :          0 :                 vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_ERROR);
     375                 :          0 :                 complete(&dev->capture.frame_cmplt);
     376                 :          0 :                 return;
     377                 :            :         }
     378                 :            : 
     379         [ #  # ]:          0 :         if (dev->capture.vc_start_timestamp == -1) {
     380                 :          0 :                 buf->vb.vb2_buf.timestamp = ktime_get_ns();
     381         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     382                 :            :                         "Buffer time set as current time - %lld",
     383                 :            :                         buf->vb.vb2_buf.timestamp);
     384         [ #  # ]:          0 :         } else if (mmal_buf->pts != 0) {
     385                 :            :                 ktime_t timestamp;
     386                 :          0 :                 s64 runtime_us = mmal_buf->pts -
     387                 :            :                     dev->capture.vc_start_timestamp;
     388                 :          0 :                 timestamp = ktime_add_us(dev->capture.kernel_start_ts,
     389                 :            :                                          runtime_us);
     390         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     391                 :            :                          "Convert start time %llu and %llu with offset %llu to %llu\n",
     392                 :            :                          ktime_to_ns(dev->capture.kernel_start_ts),
     393                 :            :                          dev->capture.vc_start_timestamp, mmal_buf->pts,
     394                 :            :                          ktime_to_ns(timestamp));
     395         [ #  # ]:          0 :                 if (timestamp < dev->capture.last_timestamp) {
     396         [ #  # ]:          0 :                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     397                 :            :                                  "Negative delta - using last time\n");
     398                 :          0 :                         timestamp = dev->capture.last_timestamp;
     399                 :            :                 }
     400                 :          0 :                 buf->vb.vb2_buf.timestamp = ktime_to_ns(timestamp);
     401                 :            :         } else {
     402         [ #  # ]:          0 :                 if (dev->capture.last_timestamp) {
     403                 :          0 :                         buf->vb.vb2_buf.timestamp =
     404                 :            :                                 dev->capture.last_timestamp;
     405         [ #  # ]:          0 :                         v4l2_dbg(1, bcm2835_v4l2_debug,
     406                 :            :                                 &dev->v4l2_dev,
     407                 :            :                                 "Buffer time set as last timestamp - %lld",
     408                 :            :                                 buf->vb.vb2_buf.timestamp);
     409                 :            :                 } else {
     410                 :          0 :                         buf->vb.vb2_buf.timestamp =
     411                 :          0 :                                 ktime_to_ns(dev->capture.kernel_start_ts);
     412         [ #  # ]:          0 :                         v4l2_dbg(1, bcm2835_v4l2_debug,
     413                 :            :                                 &dev->v4l2_dev,
     414                 :            :                                 "Buffer time set as start timestamp - %lld",
     415                 :            :                                 buf->vb.vb2_buf.timestamp);
     416                 :            :                 }
     417                 :            :         }
     418                 :          0 :         dev->capture.last_timestamp = buf->vb.vb2_buf.timestamp;
     419                 :          0 :         buf->vb.sequence = dev->capture.sequence++;
     420                 :          0 :         buf->vb.field = V4L2_FIELD_NONE;
     421                 :            : 
     422                 :          0 :         vb2_set_plane_payload(&buf->vb.vb2_buf, 0, mmal_buf->length);
     423         [ #  # ]:          0 :         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_KEYFRAME)
     424                 :          0 :                 buf->vb.flags |= V4L2_BUF_FLAG_KEYFRAME;
     425                 :            : 
     426         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     427                 :            :                 "Buffer has ts %llu",
     428                 :            :                 dev->capture.last_timestamp);
     429                 :          0 :         vb2_buffer_done(&buf->vb.vb2_buf, VB2_BUF_STATE_DONE);
     430                 :            : 
     431   [ #  #  #  # ]:          0 :         if (mmal_buf->mmal_flags & MMAL_BUFFER_HEADER_FLAG_EOS &&
     432                 :            :             is_capturing(dev)) {
     433         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     434                 :            :                          "Grab another frame as buffer has EOS");
     435                 :          0 :                 vchiq_mmal_port_parameter_set(
     436                 :            :                         instance,
     437                 :            :                         dev->capture.camera_port,
     438                 :            :                         MMAL_PARAMETER_CAPTURE,
     439                 :          0 :                         &dev->capture.frame_count,
     440                 :            :                         sizeof(dev->capture.frame_count));
     441                 :            :         }
     442                 :            : }
     443                 :            : 
     444                 :          0 : static int enable_camera(struct bm2835_mmal_dev *dev)
     445                 :            : {
     446                 :            :         int ret;
     447                 :            : 
     448         [ #  # ]:          0 :         if (!dev->camera_use_count) {
     449                 :          0 :                 ret = vchiq_mmal_port_parameter_set(
     450                 :            :                         dev->instance,
     451                 :          0 :                         &dev->component[COMP_CAMERA]->control,
     452                 :          0 :                         MMAL_PARAMETER_CAMERA_NUM, &dev->camera_num,
     453                 :            :                         sizeof(dev->camera_num));
     454         [ #  # ]:          0 :                 if (ret < 0) {
     455                 :          0 :                         v4l2_err(&dev->v4l2_dev,
     456                 :            :                                  "Failed setting camera num, ret %d\n", ret);
     457                 :          0 :                         return -EINVAL;
     458                 :            :                 }
     459                 :            : 
     460                 :          0 :                 ret = vchiq_mmal_component_enable(
     461                 :            :                                 dev->instance,
     462                 :            :                                 dev->component[COMP_CAMERA]);
     463         [ #  # ]:          0 :                 if (ret < 0) {
     464                 :          0 :                         v4l2_err(&dev->v4l2_dev,
     465                 :            :                                  "Failed enabling camera, ret %d\n", ret);
     466                 :          0 :                         return -EINVAL;
     467                 :            :                 }
     468                 :            :         }
     469                 :          0 :         dev->camera_use_count++;
     470         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug,
     471                 :            :                  &dev->v4l2_dev, "enabled camera (refcount %d)\n",
     472                 :            :                         dev->camera_use_count);
     473                 :            :         return 0;
     474                 :            : }
     475                 :            : 
     476                 :          0 : static int disable_camera(struct bm2835_mmal_dev *dev)
     477                 :            : {
     478                 :            :         int ret;
     479                 :            : 
     480         [ #  # ]:          0 :         if (!dev->camera_use_count) {
     481                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     482                 :            :                          "Disabled the camera when already disabled\n");
     483                 :          0 :                 return -EINVAL;
     484                 :            :         }
     485                 :          0 :         dev->camera_use_count--;
     486         [ #  # ]:          0 :         if (!dev->camera_use_count) {
     487                 :          0 :                 unsigned int i = 0xFFFFFFFF;
     488                 :            : 
     489         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     490                 :            :                          "Disabling camera\n");
     491                 :          0 :                 ret =
     492                 :          0 :                     vchiq_mmal_component_disable(
     493                 :            :                                 dev->instance,
     494                 :            :                                 dev->component[COMP_CAMERA]);
     495         [ #  # ]:          0 :                 if (ret < 0) {
     496                 :          0 :                         v4l2_err(&dev->v4l2_dev,
     497                 :            :                                  "Failed disabling camera, ret %d\n", ret);
     498                 :          0 :                         return -EINVAL;
     499                 :            :                 }
     500                 :          0 :                 vchiq_mmal_port_parameter_set(
     501                 :            :                         dev->instance,
     502                 :          0 :                         &dev->component[COMP_CAMERA]->control,
     503                 :            :                         MMAL_PARAMETER_CAMERA_NUM, &i,
     504                 :            :                         sizeof(i));
     505                 :            :         }
     506         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     507                 :            :                  "Camera refcount now %d\n", dev->camera_use_count);
     508                 :            :         return 0;
     509                 :            : }
     510                 :            : 
     511                 :          0 : static void buffer_queue(struct vb2_buffer *vb)
     512                 :            : {
     513                 :          0 :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vb->vb2_queue);
     514                 :            :         struct vb2_v4l2_buffer *vb2 = to_vb2_v4l2_buffer(vb);
     515                 :            :         struct vb2_mmal_buffer *buf =
     516                 :            :                                 container_of(vb2, struct vb2_mmal_buffer, vb);
     517                 :            :         int ret;
     518                 :            : 
     519         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     520                 :            :                  "%s: dev:%p buf:%p, idx %u\n",
     521                 :            :                  __func__, dev, buf, vb2->vb2_buf.index);
     522                 :            : 
     523                 :          0 :         ret = vchiq_mmal_submit_buffer(dev->instance, dev->capture.port,
     524                 :            :                                        &buf->mmal);
     525         [ #  # ]:          0 :         if (ret < 0)
     526                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: error submitting buffer\n",
     527                 :            :                          __func__);
     528                 :          0 : }
     529                 :            : 
     530                 :          0 : static int start_streaming(struct vb2_queue *vq, unsigned int count)
     531                 :            : {
     532                 :            :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
     533                 :            :         int ret;
     534                 :            :         u32 parameter_size;
     535                 :            : 
     536         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
     537                 :            :                  __func__, dev);
     538                 :            : 
     539                 :            :         /* ensure a format has actually been set */
     540         [ #  # ]:          0 :         if (!dev->capture.port)
     541                 :            :                 return -EINVAL;
     542                 :            : 
     543         [ #  # ]:          0 :         if (enable_camera(dev) < 0) {
     544                 :          0 :                 v4l2_err(&dev->v4l2_dev, "Failed to enable camera\n");
     545                 :          0 :                 return -EINVAL;
     546                 :            :         }
     547                 :            : 
     548                 :            :         /*init_completion(&dev->capture.frame_cmplt); */
     549                 :            : 
     550                 :            :         /* enable frame capture */
     551                 :          0 :         dev->capture.frame_count = 1;
     552                 :            : 
     553                 :            :         /* reset sequence number */
     554                 :          0 :         dev->capture.sequence = 0;
     555                 :            : 
     556                 :            :         /* if the preview is not already running, wait for a few frames for AGC
     557                 :            :          * to settle down.
     558                 :            :          */
     559         [ #  # ]:          0 :         if (!dev->component[COMP_PREVIEW]->enabled)
     560                 :          0 :                 msleep(300);
     561                 :            : 
     562                 :            :         /* enable the connection from camera to encoder (if applicable) */
     563   [ #  #  #  # ]:          0 :         if (dev->capture.camera_port != dev->capture.port &&
     564                 :            :             dev->capture.camera_port) {
     565                 :          0 :                 ret = vchiq_mmal_port_enable(dev->instance,
     566                 :            :                                              dev->capture.camera_port, NULL);
     567         [ #  # ]:          0 :                 if (ret) {
     568                 :          0 :                         v4l2_err(&dev->v4l2_dev,
     569                 :            :                                  "Failed to enable encode tunnel - error %d\n",
     570                 :            :                                  ret);
     571                 :          0 :                         return -1;
     572                 :            :                 }
     573                 :            :         }
     574                 :            : 
     575                 :            :         /* Get VC timestamp at this point in time */
     576                 :          0 :         parameter_size = sizeof(dev->capture.vc_start_timestamp);
     577         [ #  # ]:          0 :         if (vchiq_mmal_port_parameter_get(dev->instance,
     578                 :            :                                           dev->capture.camera_port,
     579                 :            :                                           MMAL_PARAMETER_SYSTEM_TIME,
     580                 :          0 :                                           &dev->capture.vc_start_timestamp,
     581                 :            :                                           &parameter_size)) {
     582                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     583                 :            :                          "Failed to get VC start time - update your VC f/w\n");
     584                 :            : 
     585                 :            :                 /* Flag to indicate just to rely on kernel timestamps */
     586                 :          0 :                 dev->capture.vc_start_timestamp = -1;
     587                 :            :         } else {
     588         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     589                 :            :                          "Start time %lld size %d\n",
     590                 :            :                          dev->capture.vc_start_timestamp, parameter_size);
     591                 :            :         }
     592                 :            : 
     593                 :          0 :         dev->capture.kernel_start_ts = ktime_get();
     594                 :          0 :         dev->capture.last_timestamp = 0;
     595                 :            : 
     596                 :            :         /* enable the camera port */
     597                 :          0 :         dev->capture.port->cb_ctx = dev;
     598                 :          0 :         ret =
     599                 :          0 :             vchiq_mmal_port_enable(dev->instance, dev->capture.port, buffer_cb);
     600         [ #  # ]:          0 :         if (ret) {
     601                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     602                 :            :                          "Failed to enable capture port - error %d. Disabling camera port again\n",
     603                 :            :                          ret);
     604                 :            : 
     605                 :          0 :                 vchiq_mmal_port_disable(dev->instance,
     606                 :            :                                         dev->capture.camera_port);
     607         [ #  # ]:          0 :                 if (disable_camera(dev) < 0) {
     608                 :          0 :                         v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
     609                 :          0 :                         return -EINVAL;
     610                 :            :                 }
     611                 :            :                 return -1;
     612                 :            :         }
     613                 :            : 
     614                 :            :         /* capture the first frame */
     615                 :          0 :         vchiq_mmal_port_parameter_set(dev->instance,
     616                 :            :                                       dev->capture.camera_port,
     617                 :            :                                       MMAL_PARAMETER_CAPTURE,
     618                 :          0 :                                       &dev->capture.frame_count,
     619                 :            :                                       sizeof(dev->capture.frame_count));
     620                 :          0 :         return 0;
     621                 :            : }
     622                 :            : 
     623                 :            : /* abort streaming and wait for last buffer */
     624                 :          0 : static void stop_streaming(struct vb2_queue *vq)
     625                 :            : {
     626                 :            :         int ret;
     627                 :            :         unsigned long timeout;
     628                 :            :         struct bm2835_mmal_dev *dev = vb2_get_drv_priv(vq);
     629                 :          0 :         struct vchiq_mmal_port *port = dev->capture.port;
     630                 :            : 
     631         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "%s: dev:%p\n",
     632                 :            :                  __func__, dev);
     633                 :            : 
     634                 :            :         init_completion(&dev->capture.frame_cmplt);
     635                 :          0 :         dev->capture.frame_count = 0;
     636                 :            : 
     637                 :            :         /* ensure a format has actually been set */
     638         [ #  # ]:          0 :         if (!port) {
     639                 :          0 :                 v4l2_err(&dev->v4l2_dev,
     640                 :            :                          "no capture port - stream not started?\n");
     641                 :          0 :                 return;
     642                 :            :         }
     643                 :            : 
     644         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "stopping capturing\n");
     645                 :            : 
     646                 :            :         /* stop capturing frames */
     647                 :          0 :         vchiq_mmal_port_parameter_set(dev->instance,
     648                 :            :                                       dev->capture.camera_port,
     649                 :            :                                       MMAL_PARAMETER_CAPTURE,
     650                 :          0 :                                       &dev->capture.frame_count,
     651                 :            :                                       sizeof(dev->capture.frame_count));
     652                 :            : 
     653         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     654                 :            :                  "disabling connection\n");
     655                 :            : 
     656                 :            :         /* disable the connection from camera to encoder */
     657                 :          0 :         ret = vchiq_mmal_port_disable(dev->instance, dev->capture.camera_port);
     658   [ #  #  #  # ]:          0 :         if (!ret && dev->capture.camera_port != port) {
     659         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     660                 :            :                          "disabling port\n");
     661                 :          0 :                 ret = vchiq_mmal_port_disable(dev->instance, port);
     662         [ #  # ]:          0 :         } else if (dev->capture.camera_port != port) {
     663                 :          0 :                 v4l2_err(&dev->v4l2_dev, "port_disable failed, error %d\n",
     664                 :            :                          ret);
     665                 :            :         }
     666                 :            : 
     667                 :            :         /* wait for all buffers to be returned */
     668         [ #  # ]:          0 :         while (atomic_read(&port->buffers_with_vpu)) {
     669         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     670                 :            :                          "%s: Waiting for buffers to be returned - %d outstanding\n",
     671                 :            :                          __func__, atomic_read(&port->buffers_with_vpu));
     672                 :          0 :                 timeout = wait_for_completion_timeout(&dev->capture.frame_cmplt,
     673                 :            :                                                       HZ);
     674         [ #  # ]:          0 :                 if (timeout == 0) {
     675                 :          0 :                         v4l2_err(&dev->v4l2_dev, "%s: Timeout waiting for buffers to be returned - %d outstanding\n",
     676                 :            :                                  __func__,
     677                 :            :                                  atomic_read(&port->buffers_with_vpu));
     678                 :          0 :                         break;
     679                 :            :                 }
     680                 :            :         }
     681                 :            : 
     682         [ #  # ]:          0 :         if (disable_camera(dev) < 0)
     683                 :          0 :                 v4l2_err(&dev->v4l2_dev, "Failed to disable camera\n");
     684                 :            : }
     685                 :            : 
     686                 :            : static const struct vb2_ops bm2835_mmal_video_qops = {
     687                 :            :         .queue_setup = queue_setup,
     688                 :            :         .buf_init = buffer_init,
     689                 :            :         .buf_prepare = buffer_prepare,
     690                 :            :         .buf_cleanup = buffer_cleanup,
     691                 :            :         .buf_queue = buffer_queue,
     692                 :            :         .start_streaming = start_streaming,
     693                 :            :         .stop_streaming = stop_streaming,
     694                 :            :         .wait_prepare = vb2_ops_wait_prepare,
     695                 :            :         .wait_finish = vb2_ops_wait_finish,
     696                 :            : };
     697                 :            : 
     698                 :            : /* ------------------------------------------------------------------
     699                 :            :  *      IOCTL operations
     700                 :            :  * ------------------------------------------------------------------
     701                 :            :  */
     702                 :            : 
     703                 :          0 : static int set_overlay_params(struct bm2835_mmal_dev *dev,
     704                 :            :                               struct vchiq_mmal_port *port)
     705                 :            : {
     706                 :          0 :         struct mmal_parameter_displayregion prev_config = {
     707                 :            :                 .set =  MMAL_DISPLAY_SET_LAYER |
     708                 :            :                         MMAL_DISPLAY_SET_ALPHA |
     709                 :            :                         MMAL_DISPLAY_SET_DEST_RECT |
     710                 :            :                         MMAL_DISPLAY_SET_FULLSCREEN,
     711                 :            :                 .layer = PREVIEW_LAYER,
     712                 :          0 :                 .alpha = dev->overlay.global_alpha,
     713                 :            :                 .fullscreen = 0,
     714                 :            :                 .dest_rect = {
     715                 :          0 :                         .x = dev->overlay.w.left,
     716                 :          0 :                         .y = dev->overlay.w.top,
     717                 :          0 :                         .width = dev->overlay.w.width,
     718                 :          0 :                         .height = dev->overlay.w.height,
     719                 :            :                 },
     720                 :            :         };
     721                 :          0 :         return vchiq_mmal_port_parameter_set(dev->instance, port,
     722                 :            :                                              MMAL_PARAMETER_DISPLAYREGION,
     723                 :            :                                              &prev_config, sizeof(prev_config));
     724                 :            : }
     725                 :            : 
     726                 :            : /* overlay ioctl */
     727                 :          0 : static int vidioc_enum_fmt_vid_overlay(struct file *file, void *priv,
     728                 :            :                                        struct v4l2_fmtdesc *f)
     729                 :            : {
     730                 :            :         struct mmal_fmt *fmt;
     731                 :            : 
     732         [ #  # ]:          0 :         if (f->index >= ARRAY_SIZE(formats))
     733                 :            :                 return -EINVAL;
     734                 :            : 
     735                 :            :         fmt = &formats[f->index];
     736                 :            : 
     737                 :          0 :         f->pixelformat = fmt->fourcc;
     738                 :            : 
     739                 :          0 :         return 0;
     740                 :            : }
     741                 :            : 
     742                 :          0 : static int vidioc_g_fmt_vid_overlay(struct file *file, void *priv,
     743                 :            :                                     struct v4l2_format *f)
     744                 :            : {
     745                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     746                 :            : 
     747                 :          0 :         f->fmt.win = dev->overlay;
     748                 :            : 
     749                 :          0 :         return 0;
     750                 :            : }
     751                 :            : 
     752                 :          0 : static int vidioc_try_fmt_vid_overlay(struct file *file, void *priv,
     753                 :            :                                       struct v4l2_format *f)
     754                 :            : {
     755                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     756                 :            : 
     757                 :          0 :         f->fmt.win.field = V4L2_FIELD_NONE;
     758                 :          0 :         f->fmt.win.chromakey = 0;
     759                 :          0 :         f->fmt.win.clips = NULL;
     760                 :          0 :         f->fmt.win.clipcount = 0;
     761                 :          0 :         f->fmt.win.bitmap = NULL;
     762                 :            : 
     763                 :          0 :         v4l_bound_align_image(&f->fmt.win.w.width, MIN_WIDTH, dev->max_width, 1,
     764                 :          0 :                               &f->fmt.win.w.height, MIN_HEIGHT, dev->max_height,
     765                 :            :                               1, 0);
     766                 :          0 :         v4l_bound_align_image(&f->fmt.win.w.left, MIN_WIDTH, dev->max_width, 1,
     767                 :          0 :                               &f->fmt.win.w.top, MIN_HEIGHT, dev->max_height,
     768                 :            :                               1, 0);
     769                 :            : 
     770         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     771                 :            :                  "Overlay: Now w/h %dx%d l/t %dx%d\n",
     772                 :            :                 f->fmt.win.w.width, f->fmt.win.w.height,
     773                 :            :                 f->fmt.win.w.left, f->fmt.win.w.top);
     774                 :            : 
     775         [ #  # ]:          0 :         v4l2_dump_win_format(1,
     776                 :            :                              bcm2835_v4l2_debug,
     777                 :            :                              &dev->v4l2_dev,
     778                 :            :                              &f->fmt.win,
     779                 :            :                              __func__);
     780                 :          0 :         return 0;
     781                 :            : }
     782                 :            : 
     783                 :          0 : static int vidioc_s_fmt_vid_overlay(struct file *file, void *priv,
     784                 :            :                                     struct v4l2_format *f)
     785                 :            : {
     786                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     787                 :            : 
     788                 :          0 :         vidioc_try_fmt_vid_overlay(file, priv, f);
     789                 :            : 
     790                 :          0 :         dev->overlay = f->fmt.win;
     791         [ #  # ]:          0 :         if (dev->component[COMP_PREVIEW]->enabled) {
     792                 :          0 :                 set_overlay_params(dev,
     793                 :            :                                    &dev->component[COMP_PREVIEW]->input[0]);
     794                 :            :         }
     795                 :            : 
     796                 :          0 :         return 0;
     797                 :            : }
     798                 :            : 
     799                 :          0 : static int vidioc_overlay(struct file *file, void *f, unsigned int on)
     800                 :            : {
     801                 :            :         int ret;
     802                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     803                 :            :         struct vchiq_mmal_port *src;
     804                 :            :         struct vchiq_mmal_port *dst;
     805                 :            : 
     806   [ #  #  #  #  :          0 :         if ((on && dev->component[COMP_PREVIEW]->enabled) ||
                   #  # ]
     807         [ #  # ]:          0 :             (!on && !dev->component[COMP_PREVIEW]->enabled))
     808                 :            :                 return 0;       /* already in requested state */
     809                 :            : 
     810                 :          0 :         src =
     811                 :          0 :             &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
     812                 :            : 
     813         [ #  # ]:          0 :         if (!on) {
     814                 :            :                 /* disconnect preview ports and disable component */
     815                 :          0 :                 ret = vchiq_mmal_port_disable(dev->instance, src);
     816         [ #  # ]:          0 :                 if (!ret)
     817                 :          0 :                         ret =
     818                 :          0 :                             vchiq_mmal_port_connect_tunnel(dev->instance, src,
     819                 :            :                                                            NULL);
     820         [ #  # ]:          0 :                 if (ret >= 0)
     821                 :          0 :                         ret = vchiq_mmal_component_disable(
     822                 :            :                                         dev->instance,
     823                 :            :                                         dev->component[COMP_PREVIEW]);
     824                 :            : 
     825                 :          0 :                 disable_camera(dev);
     826                 :          0 :                 return ret;
     827                 :            :         }
     828                 :            : 
     829                 :            :         /* set preview port format and connect it to output */
     830                 :          0 :         dst = &dev->component[COMP_PREVIEW]->input[0];
     831                 :            : 
     832                 :          0 :         ret = vchiq_mmal_port_set_format(dev->instance, src);
     833         [ #  # ]:          0 :         if (ret < 0)
     834                 :            :                 return ret;
     835                 :            : 
     836                 :          0 :         ret = set_overlay_params(dev, dst);
     837         [ #  # ]:          0 :         if (ret < 0)
     838                 :            :                 return ret;
     839                 :            : 
     840         [ #  # ]:          0 :         if (enable_camera(dev) < 0)
     841                 :            :                 return -EINVAL;
     842                 :            : 
     843                 :          0 :         ret = vchiq_mmal_component_enable(
     844                 :            :                         dev->instance,
     845                 :            :                         dev->component[COMP_PREVIEW]);
     846         [ #  # ]:          0 :         if (ret < 0)
     847                 :            :                 return ret;
     848                 :            : 
     849         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev, "connecting %p to %p\n",
     850                 :            :                  src, dst);
     851                 :          0 :         ret = vchiq_mmal_port_connect_tunnel(dev->instance, src, dst);
     852         [ #  # ]:          0 :         if (ret)
     853                 :            :                 return ret;
     854                 :            : 
     855                 :          0 :         return vchiq_mmal_port_enable(dev->instance, src, NULL);
     856                 :            : }
     857                 :            : 
     858                 :          0 : static int vidioc_g_fbuf(struct file *file, void *fh,
     859                 :            :                          struct v4l2_framebuffer *a)
     860                 :            : {
     861                 :            :         /* The video overlay must stay within the framebuffer and can't be
     862                 :            :          * positioned independently.
     863                 :            :          */
     864                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     865                 :            :         struct vchiq_mmal_port *preview_port =
     866                 :          0 :                 &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
     867                 :            : 
     868                 :          0 :         a->capability = V4L2_FBUF_CAP_EXTERNOVERLAY |
     869                 :            :                         V4L2_FBUF_CAP_GLOBAL_ALPHA;
     870                 :          0 :         a->flags = V4L2_FBUF_FLAG_OVERLAY;
     871                 :          0 :         a->fmt.width = preview_port->es.video.width;
     872                 :          0 :         a->fmt.height = preview_port->es.video.height;
     873                 :          0 :         a->fmt.pixelformat = V4L2_PIX_FMT_YUV420;
     874                 :          0 :         a->fmt.bytesperline = preview_port->es.video.width;
     875                 :          0 :         a->fmt.sizeimage = (preview_port->es.video.width *
     876                 :          0 :                                preview_port->es.video.height * 3) >> 1;
     877                 :          0 :         a->fmt.colorspace = V4L2_COLORSPACE_SMPTE170M;
     878                 :            : 
     879                 :          0 :         return 0;
     880                 :            : }
     881                 :            : 
     882                 :            : /* input ioctls */
     883                 :          0 : static int vidioc_enum_input(struct file *file, void *priv,
     884                 :            :                              struct v4l2_input *inp)
     885                 :            : {
     886                 :            :         /* only a single camera input */
     887         [ #  # ]:          0 :         if (inp->index)
     888                 :            :                 return -EINVAL;
     889                 :            : 
     890                 :          0 :         inp->type = V4L2_INPUT_TYPE_CAMERA;
     891                 :          0 :         sprintf((char *)inp->name, "Camera %u", inp->index);
     892                 :          0 :         return 0;
     893                 :            : }
     894                 :            : 
     895                 :          0 : static int vidioc_g_input(struct file *file, void *priv, unsigned int *i)
     896                 :            : {
     897                 :          0 :         *i = 0;
     898                 :          0 :         return 0;
     899                 :            : }
     900                 :            : 
     901                 :          0 : static int vidioc_s_input(struct file *file, void *priv, unsigned int i)
     902                 :            : {
     903         [ #  # ]:          0 :         if (i)
     904                 :            :                 return -EINVAL;
     905                 :            : 
     906                 :          0 :         return 0;
     907                 :            : }
     908                 :            : 
     909                 :            : /* capture ioctls */
     910                 :          0 : static int vidioc_querycap(struct file *file, void *priv,
     911                 :            :                            struct v4l2_capability *cap)
     912                 :            : {
     913                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     914                 :            :         u32 major;
     915                 :            :         u32 minor;
     916                 :            : 
     917                 :          0 :         vchiq_mmal_version(dev->instance, &major, &minor);
     918                 :            : 
     919                 :          0 :         strcpy((char *)cap->driver, "bm2835 mmal");
     920                 :          0 :         snprintf((char *)cap->card, sizeof(cap->card), "mmal service %d.%d",
     921                 :            :                  major, minor);
     922                 :            : 
     923                 :          0 :         snprintf((char *)cap->bus_info, sizeof(cap->bus_info),
     924                 :          0 :                  "platform:%s", dev->v4l2_dev.name);
     925                 :          0 :         return 0;
     926                 :            : }
     927                 :            : 
     928                 :          0 : static int vidioc_enum_fmt_vid_cap(struct file *file, void *priv,
     929                 :            :                                    struct v4l2_fmtdesc *f)
     930                 :            : {
     931                 :            :         struct mmal_fmt *fmt;
     932                 :            : 
     933         [ #  # ]:          0 :         if (f->index >= ARRAY_SIZE(formats))
     934                 :            :                 return -EINVAL;
     935                 :            : 
     936                 :            :         fmt = &formats[f->index];
     937                 :            : 
     938                 :          0 :         f->pixelformat = fmt->fourcc;
     939                 :            : 
     940                 :          0 :         return 0;
     941                 :            : }
     942                 :            : 
     943                 :          0 : static int vidioc_g_fmt_vid_cap(struct file *file, void *priv,
     944                 :            :                                 struct v4l2_format *f)
     945                 :            : {
     946                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     947                 :            : 
     948                 :          0 :         f->fmt.pix.width = dev->capture.width;
     949                 :          0 :         f->fmt.pix.height = dev->capture.height;
     950                 :          0 :         f->fmt.pix.field = V4L2_FIELD_NONE;
     951                 :          0 :         f->fmt.pix.pixelformat = dev->capture.fmt->fourcc;
     952                 :          0 :         f->fmt.pix.bytesperline = dev->capture.stride;
     953                 :          0 :         f->fmt.pix.sizeimage = dev->capture.buffersize;
     954                 :            : 
     955         [ #  # ]:          0 :         if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_RGB24)
     956                 :          0 :                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
     957         [ #  # ]:          0 :         else if (dev->capture.fmt->fourcc == V4L2_PIX_FMT_JPEG)
     958                 :          0 :                 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
     959                 :            :         else
     960                 :          0 :                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
     961                 :          0 :         f->fmt.pix.priv = 0;
     962                 :            : 
     963         [ #  # ]:          0 :         v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
     964                 :            :                              __func__);
     965                 :          0 :         return 0;
     966                 :            : }
     967                 :            : 
     968                 :          0 : static int vidioc_try_fmt_vid_cap(struct file *file, void *priv,
     969                 :            :                                   struct v4l2_format *f)
     970                 :            : {
     971                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
     972                 :            :         struct mmal_fmt *mfmt;
     973                 :            : 
     974                 :            :         mfmt = get_format(f);
     975         [ #  # ]:          0 :         if (!mfmt) {
     976         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     977                 :            :                          "Fourcc format (0x%08x) unknown.\n",
     978                 :            :                          f->fmt.pix.pixelformat);
     979                 :          0 :                 f->fmt.pix.pixelformat = formats[0].fourcc;
     980                 :            :                 mfmt = get_format(f);
     981                 :            :         }
     982                 :            : 
     983                 :          0 :         f->fmt.pix.field = V4L2_FIELD_NONE;
     984                 :            : 
     985         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
     986                 :            :                  "Clipping/aligning %dx%d format %08X\n",
     987                 :            :                  f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
     988                 :            : 
     989                 :          0 :         v4l_bound_align_image(&f->fmt.pix.width, MIN_WIDTH, dev->max_width, 1,
     990                 :          0 :                               &f->fmt.pix.height, MIN_HEIGHT, dev->max_height,
     991                 :            :                               1, 0);
     992                 :          0 :         f->fmt.pix.bytesperline = f->fmt.pix.width * mfmt->ybbp;
     993         [ #  # ]:          0 :         if (!mfmt->remove_padding) {
     994         [ #  # ]:          0 :                 if (mfmt->depth == 24) {
     995                 :            :                         /*
     996                 :            :                          * 24bpp is a pain as we can't use simple masking.
     997                 :            :                          * Min stride is width aligned to 16, times 24bpp.
     998                 :            :                          */
     999                 :          0 :                         f->fmt.pix.bytesperline =
    1000                 :          0 :                                 ((f->fmt.pix.width + 15) & ~15) * 3;
    1001                 :            :                 } else {
    1002                 :            :                         /*
    1003                 :            :                          * GPU isn't removing padding, so stride is aligned to
    1004                 :            :                          * 32
    1005                 :            :                          */
    1006                 :          0 :                         int align_mask = ((32 * mfmt->depth) >> 3) - 1;
    1007                 :            : 
    1008                 :          0 :                         f->fmt.pix.bytesperline =
    1009                 :          0 :                                 (f->fmt.pix.bytesperline + align_mask) &
    1010                 :          0 :                                                         ~align_mask;
    1011                 :            :                 }
    1012         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1013                 :            :                          "Not removing padding, so bytes/line = %d\n",
    1014                 :            :                          f->fmt.pix.bytesperline);
    1015                 :            :         }
    1016                 :            : 
    1017                 :            :         /* Image buffer has to be padded to allow for alignment, even though
    1018                 :            :          * we sometimes then remove that padding before delivering the buffer.
    1019                 :            :          */
    1020                 :          0 :         f->fmt.pix.sizeimage = ((f->fmt.pix.height + 15) & ~15) *
    1021                 :          0 :                         (((f->fmt.pix.width + 31) & ~31) * mfmt->depth) >> 3;
    1022                 :            : 
    1023   [ #  #  #  # ]:          0 :         if ((mfmt->flags & V4L2_FMT_FLAG_COMPRESSED) &&
    1024                 :            :             f->fmt.pix.sizeimage < MIN_BUFFER_SIZE)
    1025                 :          0 :                 f->fmt.pix.sizeimage = MIN_BUFFER_SIZE;
    1026                 :            : 
    1027         [ #  # ]:          0 :         if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_RGB24)
    1028                 :          0 :                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SRGB;
    1029         [ #  # ]:          0 :         else if (f->fmt.pix.pixelformat == V4L2_PIX_FMT_JPEG)
    1030                 :          0 :                 f->fmt.pix.colorspace = V4L2_COLORSPACE_JPEG;
    1031                 :            :         else
    1032                 :          0 :                 f->fmt.pix.colorspace = V4L2_COLORSPACE_SMPTE170M;
    1033                 :          0 :         f->fmt.pix.priv = 0;
    1034                 :            : 
    1035         [ #  # ]:          0 :         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1036                 :            :                  "Now %dx%d format %08X\n",
    1037                 :            :                 f->fmt.pix.width, f->fmt.pix.height, f->fmt.pix.pixelformat);
    1038                 :            : 
    1039         [ #  # ]:          0 :         v4l2_dump_pix_format(1, bcm2835_v4l2_debug, &dev->v4l2_dev, &f->fmt.pix,
    1040                 :            :                              __func__);
    1041                 :          0 :         return 0;
    1042                 :            : }
    1043                 :            : 
    1044                 :          0 : static int mmal_setup_components(struct bm2835_mmal_dev *dev,
    1045                 :            :                                  struct v4l2_format *f)
    1046                 :            : {
    1047                 :            :         int ret;
    1048                 :            :         struct vchiq_mmal_port *port = NULL, *camera_port = NULL;
    1049                 :            :         struct vchiq_mmal_component *encode_component = NULL;
    1050                 :            :         struct mmal_fmt *mfmt = get_format(f);
    1051                 :            :         u32 remove_padding;
    1052                 :            : 
    1053         [ #  # ]:          0 :         if (!mfmt)
    1054                 :            :                 return -EINVAL;
    1055                 :            : 
    1056         [ #  # ]:          0 :         if (dev->capture.encode_component) {
    1057         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1058                 :            :                          "vid_cap - disconnect previous tunnel\n");
    1059                 :            : 
    1060                 :            :                 /* Disconnect any previous connection */
    1061                 :          0 :                 vchiq_mmal_port_connect_tunnel(dev->instance,
    1062                 :            :                                                dev->capture.camera_port, NULL);
    1063                 :          0 :                 dev->capture.camera_port = NULL;
    1064                 :          0 :                 ret = vchiq_mmal_component_disable(dev->instance,
    1065                 :            :                                                    dev->capture.encode_component);
    1066         [ #  # ]:          0 :                 if (ret)
    1067                 :          0 :                         v4l2_err(&dev->v4l2_dev,
    1068                 :            :                                  "Failed to disable encode component %d\n",
    1069                 :            :                                  ret);
    1070                 :            : 
    1071                 :          0 :                 dev->capture.encode_component = NULL;
    1072                 :            :         }
    1073                 :            :         /* format dependent port setup */
    1074   [ #  #  #  # ]:          0 :         switch (mfmt->mmal_component) {
    1075                 :            :         case COMP_CAMERA:
    1076                 :            :                 /* Make a further decision on port based on resolution */
    1077   [ #  #  #  # ]:          0 :                 if (f->fmt.pix.width <= max_video_width &&
    1078                 :          0 :                     f->fmt.pix.height <= max_video_height)
    1079                 :          0 :                         camera_port =
    1080                 :          0 :                             &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
    1081                 :            :                 else
    1082                 :          0 :                         camera_port =
    1083                 :          0 :                             &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
    1084                 :            :                 port = camera_port;
    1085                 :          0 :                 break;
    1086                 :            :         case COMP_IMAGE_ENCODE:
    1087                 :          0 :                 encode_component = dev->component[COMP_IMAGE_ENCODE];
    1088                 :          0 :                 port = &dev->component[COMP_IMAGE_ENCODE]->output[0];
    1089                 :          0 :                 camera_port =
    1090                 :          0 :                     &dev->component[COMP_CAMERA]->output[CAM_PORT_CAPTURE];
    1091                 :          0 :                 break;
    1092                 :            :         case COMP_VIDEO_ENCODE:
    1093                 :          0 :                 encode_component = dev->component[COMP_VIDEO_ENCODE];
    1094                 :          0 :                 port = &dev->component[COMP_VIDEO_ENCODE]->output[0];
    1095                 :          0 :                 camera_port =
    1096                 :          0 :                     &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO];
    1097                 :          0 :                 break;
    1098                 :            :         default:
    1099                 :            :                 break;
    1100                 :            :         }
    1101                 :            : 
    1102         [ #  # ]:          0 :         if (!port)
    1103                 :            :                 return -EINVAL;
    1104                 :            : 
    1105         [ #  # ]:          0 :         if (encode_component)
    1106                 :          0 :                 camera_port->format.encoding = MMAL_ENCODING_OPAQUE;
    1107                 :            :         else
    1108                 :          0 :                 camera_port->format.encoding = mfmt->mmal;
    1109                 :            : 
    1110         [ #  # ]:          0 :         if (dev->rgb_bgr_swapped) {
    1111         [ #  # ]:          0 :                 if (camera_port->format.encoding == MMAL_ENCODING_RGB24)
    1112                 :          0 :                         camera_port->format.encoding = MMAL_ENCODING_BGR24;
    1113         [ #  # ]:          0 :                 else if (camera_port->format.encoding == MMAL_ENCODING_BGR24)
    1114                 :          0 :                         camera_port->format.encoding = MMAL_ENCODING_RGB24;
    1115                 :            :         }
    1116                 :            : 
    1117                 :          0 :         remove_padding = mfmt->remove_padding;
    1118                 :          0 :         vchiq_mmal_port_parameter_set(dev->instance,
    1119                 :            :                                       camera_port,
    1120                 :            :                                       MMAL_PARAMETER_NO_IMAGE_PADDING,
    1121                 :            :                                       &remove_padding, sizeof(remove_padding));
    1122                 :            : 
    1123                 :          0 :         camera_port->format.encoding_variant = 0;
    1124                 :          0 :         camera_port->es.video.width = f->fmt.pix.width;
    1125                 :          0 :         camera_port->es.video.height = f->fmt.pix.height;
    1126                 :          0 :         camera_port->es.video.crop.x = 0;
    1127                 :          0 :         camera_port->es.video.crop.y = 0;
    1128                 :          0 :         camera_port->es.video.crop.width = f->fmt.pix.width;
    1129                 :          0 :         camera_port->es.video.crop.height = f->fmt.pix.height;
    1130                 :          0 :         camera_port->es.video.frame_rate.num = 0;
    1131                 :          0 :         camera_port->es.video.frame_rate.den = 1;
    1132                 :          0 :         camera_port->es.video.color_space = MMAL_COLOR_SPACE_JPEG_JFIF;
    1133                 :            : 
    1134                 :          0 :         ret = vchiq_mmal_port_set_format(dev->instance, camera_port);
    1135                 :            : 
    1136   [ #  #  #  # ]:          0 :         if (!ret &&
    1137                 :            :             camera_port ==
    1138                 :          0 :             &dev->component[COMP_CAMERA]->output[CAM_PORT_VIDEO]) {
    1139                 :          0 :                 bool overlay_enabled =
    1140                 :          0 :                     !!dev->component[COMP_PREVIEW]->enabled;
    1141                 :          0 :                 struct vchiq_mmal_port *preview_port =
    1142                 :            :                     &dev->component[COMP_CAMERA]->output[CAM_PORT_PREVIEW];
    1143                 :            :                 /* Preview and encode ports need to match on resolution */
    1144         [ #  # ]:          0 :                 if (overlay_enabled) {
    1145                 :            :                         /* Need to disable the overlay before we can update
    1146                 :            :                          * the resolution
    1147                 :            :                          */
    1148                 :          0 :                         ret =
    1149                 :          0 :                             vchiq_mmal_port_disable(dev->instance,
    1150                 :            :                                                     preview_port);
    1151         [ #  # ]:          0 :                         if (!ret)
    1152                 :          0 :                                 ret =
    1153                 :          0 :                                     vchiq_mmal_port_connect_tunnel(
    1154                 :            :                                                 dev->instance,
    1155                 :            :                                                 preview_port,
    1156                 :            :                                                 NULL);
    1157                 :            :                 }
    1158                 :          0 :                 preview_port->es.video.width = f->fmt.pix.width;
    1159                 :          0 :                 preview_port->es.video.height = f->fmt.pix.height;
    1160                 :          0 :                 preview_port->es.video.crop.x = 0;
    1161                 :          0 :                 preview_port->es.video.crop.y = 0;
    1162                 :          0 :                 preview_port->es.video.crop.width = f->fmt.pix.width;
    1163                 :          0 :                 preview_port->es.video.crop.height = f->fmt.pix.height;
    1164                 :          0 :                 preview_port->es.video.frame_rate.num =
    1165                 :          0 :                                           dev->capture.timeperframe.denominator;
    1166                 :          0 :                 preview_port->es.video.frame_rate.den =
    1167                 :          0 :                                           dev->capture.timeperframe.numerator;
    1168                 :          0 :                 ret = vchiq_mmal_port_set_format(dev->instance, preview_port);
    1169         [ #  # ]:          0 :                 if (overlay_enabled) {
    1170                 :          0 :                         ret = vchiq_mmal_port_connect_tunnel(
    1171                 :            :                                 dev->instance,
    1172                 :            :                                 preview_port,
    1173                 :          0 :                                 &dev->component[COMP_PREVIEW]->input[0]);
    1174         [ #  # ]:          0 :                         if (!ret)
    1175                 :          0 :                                 ret = vchiq_mmal_port_enable(dev->instance,
    1176                 :            :                                                              preview_port,
    1177                 :            :                                                              NULL);
    1178                 :            :                 }
    1179                 :            :         }
    1180                 :            : 
    1181         [ #  # ]:          0 :         if (ret) {
    1182         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1183                 :            :                          "%s failed to set format %dx%d %08X\n", __func__,
    1184                 :            :                          f->fmt.pix.width, f->fmt.pix.height,
    1185                 :            :                          f->fmt.pix.pixelformat);
    1186                 :            :                 /* ensure capture is not going to be tried */
    1187                 :          0 :                 dev->capture.port = NULL;
    1188                 :            :         } else {
    1189         [ #  # ]:          0 :                 if (encode_component) {
    1190         [ #  # ]:          0 :                         v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1191                 :            :                                  "vid_cap - set up encode comp\n");
    1192                 :            : 
    1193                 :            :                         /* configure buffering */
    1194                 :          0 :                         camera_port->current_buffer.size =
    1195                 :          0 :                             camera_port->recommended_buffer.size;
    1196                 :          0 :                         camera_port->current_buffer.num =
    1197                 :          0 :                             camera_port->recommended_buffer.num;
    1198                 :            : 
    1199                 :          0 :                         ret =
    1200                 :          0 :                             vchiq_mmal_port_connect_tunnel(
    1201                 :            :                                         dev->instance,
    1202                 :            :                                         camera_port,
    1203                 :            :                                         &encode_component->input[0]);
    1204         [ #  # ]:          0 :                         if (ret) {
    1205         [ #  # ]:          0 :                                 v4l2_dbg(1, bcm2835_v4l2_debug,
    1206                 :            :                                          &dev->v4l2_dev,
    1207                 :            :                                          "%s failed to create connection\n",
    1208                 :            :                                          __func__);
    1209                 :            :                                 /* ensure capture is not going to be tried */
    1210                 :          0 :                                 dev->capture.port = NULL;
    1211                 :            :                         } else {
    1212                 :          0 :                                 port->es.video.width = f->fmt.pix.width;
    1213                 :          0 :                                 port->es.video.height = f->fmt.pix.height;
    1214                 :          0 :                                 port->es.video.crop.x = 0;
    1215                 :          0 :                                 port->es.video.crop.y = 0;
    1216                 :          0 :                                 port->es.video.crop.width = f->fmt.pix.width;
    1217                 :          0 :                                 port->es.video.crop.height = f->fmt.pix.height;
    1218                 :          0 :                                 port->es.video.frame_rate.num =
    1219                 :          0 :                                           dev->capture.timeperframe.denominator;
    1220                 :          0 :                                 port->es.video.frame_rate.den =
    1221                 :          0 :                                           dev->capture.timeperframe.numerator;
    1222                 :            : 
    1223                 :          0 :                                 port->format.encoding = mfmt->mmal;
    1224                 :          0 :                                 port->format.encoding_variant = 0;
    1225                 :            :                                 /* Set any encoding specific parameters */
    1226         [ #  # ]:          0 :                                 switch (mfmt->mmal_component) {
    1227                 :            :                                 case COMP_VIDEO_ENCODE:
    1228                 :          0 :                                         port->format.bitrate =
    1229                 :          0 :                                             dev->capture.encode_bitrate;
    1230                 :          0 :                                         break;
    1231                 :            :                                 case COMP_IMAGE_ENCODE:
    1232                 :            :                                         /* Could set EXIF parameters here */
    1233                 :            :                                         break;
    1234                 :            :                                 default:
    1235                 :            :                                         break;
    1236                 :            :                                 }
    1237                 :          0 :                                 ret = vchiq_mmal_port_set_format(dev->instance,
    1238                 :            :                                                                  port);
    1239         [ #  # ]:          0 :                                 if (ret)
    1240         [ #  # ]:          0 :                                         v4l2_dbg(1, bcm2835_v4l2_debug,
    1241                 :            :                                                  &dev->v4l2_dev,
    1242                 :            :                                                  "%s failed to set format %dx%d fmt %08X\n",
    1243                 :            :                                                  __func__,
    1244                 :            :                                                  f->fmt.pix.width,
    1245                 :            :                                                  f->fmt.pix.height,
    1246                 :            :                                                  f->fmt.pix.pixelformat
    1247                 :            :                                                  );
    1248                 :            :                         }
    1249                 :            : 
    1250         [ #  # ]:          0 :                         if (!ret) {
    1251                 :          0 :                                 ret = vchiq_mmal_component_enable(
    1252                 :            :                                                 dev->instance,
    1253                 :            :                                                 encode_component);
    1254         [ #  # ]:          0 :                                 if (ret) {
    1255         [ #  # ]:          0 :                                         v4l2_dbg(1, bcm2835_v4l2_debug,
    1256                 :            :                                                  &dev->v4l2_dev,
    1257                 :            :                                                  "%s Failed to enable encode components\n",
    1258                 :            :                                                  __func__);
    1259                 :            :                                 }
    1260                 :            :                         }
    1261         [ #  # ]:          0 :                         if (!ret) {
    1262                 :            :                                 /* configure buffering */
    1263                 :          0 :                                 port->current_buffer.num = 1;
    1264                 :          0 :                                 port->current_buffer.size =
    1265                 :          0 :                                     f->fmt.pix.sizeimage;
    1266         [ #  # ]:          0 :                                 if (port->format.encoding ==
    1267                 :            :                                     MMAL_ENCODING_JPEG) {
    1268         [ #  # ]:          0 :                                         v4l2_dbg(1, bcm2835_v4l2_debug,
    1269                 :            :                                                  &dev->v4l2_dev,
    1270                 :            :                                                  "JPG - buf size now %d was %d\n",
    1271                 :            :                                                  f->fmt.pix.sizeimage,
    1272                 :            :                                                  port->current_buffer.size);
    1273                 :          0 :                                         port->current_buffer.size =
    1274                 :          0 :                                             (f->fmt.pix.sizeimage <
    1275                 :            :                                              (100 << 10)) ?
    1276                 :          0 :                                             (100 << 10) : f->fmt.pix.sizeimage;
    1277                 :            :                                 }
    1278         [ #  # ]:          0 :                                 v4l2_dbg(1, bcm2835_v4l2_debug,
    1279                 :            :                                          &dev->v4l2_dev,
    1280                 :            :                                          "vid_cap - cur_buf.size set to %d\n",
    1281                 :            :                                          f->fmt.pix.sizeimage);
    1282                 :          0 :                                 port->current_buffer.alignment = 0;
    1283                 :            :                         }
    1284                 :            :                 } else {
    1285                 :            :                         /* configure buffering */
    1286                 :          0 :                         camera_port->current_buffer.num = 1;
    1287                 :          0 :                         camera_port->current_buffer.size = f->fmt.pix.sizeimage;
    1288                 :          0 :                         camera_port->current_buffer.alignment = 0;
    1289                 :            :                 }
    1290                 :            : 
    1291         [ #  # ]:          0 :                 if (!ret) {
    1292                 :          0 :                         dev->capture.fmt = mfmt;
    1293                 :          0 :                         dev->capture.stride = f->fmt.pix.bytesperline;
    1294                 :          0 :                         dev->capture.width = camera_port->es.video.crop.width;
    1295                 :          0 :                         dev->capture.height = camera_port->es.video.crop.height;
    1296                 :          0 :                         dev->capture.buffersize = port->current_buffer.size;
    1297                 :            : 
    1298                 :            :                         /* select port for capture */
    1299                 :          0 :                         dev->capture.port = port;
    1300                 :          0 :                         dev->capture.camera_port = camera_port;
    1301                 :          0 :                         dev->capture.encode_component = encode_component;
    1302         [ #  # ]:          0 :                         v4l2_dbg(1, bcm2835_v4l2_debug,
    1303                 :            :                                  &dev->v4l2_dev,
    1304                 :            :                                 "Set dev->capture.fmt %08X, %dx%d, stride %d, size %d",
    1305                 :            :                                 port->format.encoding,
    1306                 :            :                                 dev->capture.width, dev->capture.height,
    1307                 :            :                                 dev->capture.stride, dev->capture.buffersize);
    1308                 :            :                 }
    1309                 :            :         }
    1310                 :            : 
    1311                 :            :         /* todo: Need to convert the vchiq/mmal error into a v4l2 error. */
    1312                 :          0 :         return ret;
    1313                 :            : }
    1314                 :            : 
    1315                 :          0 : static int vidioc_s_fmt_vid_cap(struct file *file, void *priv,
    1316                 :            :                                 struct v4l2_format *f)
    1317                 :            : {
    1318                 :            :         int ret;
    1319                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
    1320                 :            :         struct mmal_fmt *mfmt;
    1321                 :            : 
    1322                 :            :         /* try the format to set valid parameters */
    1323                 :          0 :         ret = vidioc_try_fmt_vid_cap(file, priv, f);
    1324         [ #  # ]:          0 :         if (ret) {
    1325                 :          0 :                 v4l2_err(&dev->v4l2_dev,
    1326                 :            :                          "vid_cap - vidioc_try_fmt_vid_cap failed\n");
    1327                 :          0 :                 return ret;
    1328                 :            :         }
    1329                 :            : 
    1330                 :            :         /* if a capture is running refuse to set format */
    1331         [ #  # ]:          0 :         if (vb2_is_busy(&dev->capture.vb_vidq)) {
    1332                 :          0 :                 v4l2_info(&dev->v4l2_dev, "%s device busy\n", __func__);
    1333                 :          0 :                 return -EBUSY;
    1334                 :            :         }
    1335                 :            : 
    1336                 :            :         /* If the format is unsupported v4l2 says we should switch to
    1337                 :            :          * a supported one and not return an error.
    1338                 :            :          */
    1339                 :            :         mfmt = get_format(f);
    1340         [ #  # ]:          0 :         if (!mfmt) {
    1341         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1342                 :            :                          "Fourcc format (0x%08x) unknown.\n",
    1343                 :            :                          f->fmt.pix.pixelformat);
    1344                 :          0 :                 f->fmt.pix.pixelformat = formats[0].fourcc;
    1345                 :            :                 mfmt = get_format(f);
    1346                 :            :         }
    1347                 :            : 
    1348                 :          0 :         ret = mmal_setup_components(dev, f);
    1349         [ #  # ]:          0 :         if (ret) {
    1350                 :          0 :                 v4l2_err(&dev->v4l2_dev,
    1351                 :            :                          "%s: failed to setup mmal components: %d\n",
    1352                 :            :                          __func__, ret);
    1353                 :            :                 ret = -EINVAL;
    1354                 :            :         }
    1355                 :            : 
    1356                 :          0 :         return ret;
    1357                 :            : }
    1358                 :            : 
    1359                 :          0 : static int vidioc_enum_framesizes(struct file *file, void *fh,
    1360                 :            :                                   struct v4l2_frmsizeenum *fsize)
    1361                 :            : {
    1362                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
    1363                 :            :         static const struct v4l2_frmsize_stepwise sizes = {
    1364                 :            :                 MIN_WIDTH, 0, 2,
    1365                 :            :                 MIN_HEIGHT, 0, 2
    1366                 :            :         };
    1367                 :            :         int i;
    1368                 :            : 
    1369         [ #  # ]:          0 :         if (fsize->index)
    1370                 :            :                 return -EINVAL;
    1371         [ #  # ]:          0 :         for (i = 0; i < ARRAY_SIZE(formats); i++)
    1372         [ #  # ]:          0 :                 if (formats[i].fourcc == fsize->pixel_format)
    1373                 :            :                         break;
    1374         [ #  # ]:          0 :         if (i == ARRAY_SIZE(formats))
    1375                 :            :                 return -EINVAL;
    1376                 :          0 :         fsize->type = V4L2_FRMSIZE_TYPE_STEPWISE;
    1377                 :          0 :         fsize->stepwise = sizes;
    1378                 :          0 :         fsize->stepwise.max_width = dev->max_width;
    1379                 :          0 :         fsize->stepwise.max_height = dev->max_height;
    1380                 :          0 :         return 0;
    1381                 :            : }
    1382                 :            : 
    1383                 :            : /* timeperframe is arbitrary and continuous */
    1384                 :          0 : static int vidioc_enum_frameintervals(struct file *file, void *priv,
    1385                 :            :                                       struct v4l2_frmivalenum *fival)
    1386                 :            : {
    1387                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
    1388                 :            :         int i;
    1389                 :            : 
    1390         [ #  # ]:          0 :         if (fival->index)
    1391                 :            :                 return -EINVAL;
    1392                 :            : 
    1393         [ #  # ]:          0 :         for (i = 0; i < ARRAY_SIZE(formats); i++)
    1394         [ #  # ]:          0 :                 if (formats[i].fourcc == fival->pixel_format)
    1395                 :            :                         break;
    1396         [ #  # ]:          0 :         if (i == ARRAY_SIZE(formats))
    1397                 :            :                 return -EINVAL;
    1398                 :            : 
    1399                 :            :         /* regarding width & height - we support any within range */
    1400   [ #  #  #  #  :          0 :         if (fival->width < MIN_WIDTH || fival->width > dev->max_width ||
                   #  # ]
    1401         [ #  # ]:          0 :             fival->height < MIN_HEIGHT || fival->height > dev->max_height)
    1402                 :            :                 return -EINVAL;
    1403                 :            : 
    1404                 :          0 :         fival->type = V4L2_FRMIVAL_TYPE_CONTINUOUS;
    1405                 :            : 
    1406                 :            :         /* fill in stepwise (step=1.0 is required by V4L2 spec) */
    1407                 :          0 :         fival->stepwise.min  = tpf_min;
    1408                 :          0 :         fival->stepwise.max  = tpf_max;
    1409                 :          0 :         fival->stepwise.step = (struct v4l2_fract) {1, 1};
    1410                 :            : 
    1411                 :          0 :         return 0;
    1412                 :            : }
    1413                 :            : 
    1414                 :          0 : static int vidioc_g_parm(struct file *file, void *priv,
    1415                 :            :                          struct v4l2_streamparm *parm)
    1416                 :            : {
    1417                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
    1418                 :            : 
    1419         [ #  # ]:          0 :         if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
    1420                 :            :                 return -EINVAL;
    1421                 :            : 
    1422                 :          0 :         parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
    1423                 :          0 :         parm->parm.capture.timeperframe = dev->capture.timeperframe;
    1424                 :          0 :         parm->parm.capture.readbuffers  = 1;
    1425                 :          0 :         return 0;
    1426                 :            : }
    1427                 :            : 
    1428                 :          0 : static int vidioc_s_parm(struct file *file, void *priv,
    1429                 :            :                          struct v4l2_streamparm *parm)
    1430                 :            : {
    1431                 :            :         struct bm2835_mmal_dev *dev = video_drvdata(file);
    1432                 :            :         struct v4l2_fract tpf;
    1433                 :            : 
    1434         [ #  # ]:          0 :         if (parm->type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
    1435                 :            :                 return -EINVAL;
    1436                 :            : 
    1437                 :          0 :         tpf = parm->parm.capture.timeperframe;
    1438                 :            : 
    1439                 :            :         /* tpf: {*, 0} resets timing; clip to [min, max]*/
    1440         [ #  # ]:          0 :         tpf = tpf.denominator ? tpf : tpf_default;
    1441         [ #  # ]:          0 :         tpf = V4L2_FRACT_COMPARE(tpf, <, tpf_min) ? tpf_min : tpf;
    1442         [ #  # ]:          0 :         tpf = V4L2_FRACT_COMPARE(tpf, >, tpf_max) ? tpf_max : tpf;
    1443                 :            : 
    1444                 :          0 :         dev->capture.timeperframe = tpf;
    1445                 :          0 :         parm->parm.capture.timeperframe = tpf;
    1446                 :          0 :         parm->parm.capture.readbuffers  = 1;
    1447                 :          0 :         parm->parm.capture.capability   = V4L2_CAP_TIMEPERFRAME;
    1448                 :            : 
    1449                 :          0 :         set_framerate_params(dev);
    1450                 :            : 
    1451                 :          0 :         return 0;
    1452                 :            : }
    1453                 :            : 
    1454                 :            : static const struct v4l2_ioctl_ops camera0_ioctl_ops = {
    1455                 :            :         /* overlay */
    1456                 :            :         .vidioc_enum_fmt_vid_overlay = vidioc_enum_fmt_vid_overlay,
    1457                 :            :         .vidioc_g_fmt_vid_overlay = vidioc_g_fmt_vid_overlay,
    1458                 :            :         .vidioc_try_fmt_vid_overlay = vidioc_try_fmt_vid_overlay,
    1459                 :            :         .vidioc_s_fmt_vid_overlay = vidioc_s_fmt_vid_overlay,
    1460                 :            :         .vidioc_overlay = vidioc_overlay,
    1461                 :            :         .vidioc_g_fbuf = vidioc_g_fbuf,
    1462                 :            : 
    1463                 :            :         /* inputs */
    1464                 :            :         .vidioc_enum_input = vidioc_enum_input,
    1465                 :            :         .vidioc_g_input = vidioc_g_input,
    1466                 :            :         .vidioc_s_input = vidioc_s_input,
    1467                 :            : 
    1468                 :            :         /* capture */
    1469                 :            :         .vidioc_querycap = vidioc_querycap,
    1470                 :            :         .vidioc_enum_fmt_vid_cap = vidioc_enum_fmt_vid_cap,
    1471                 :            :         .vidioc_g_fmt_vid_cap = vidioc_g_fmt_vid_cap,
    1472                 :            :         .vidioc_try_fmt_vid_cap = vidioc_try_fmt_vid_cap,
    1473                 :            :         .vidioc_s_fmt_vid_cap = vidioc_s_fmt_vid_cap,
    1474                 :            : 
    1475                 :            :         /* buffer management */
    1476                 :            :         .vidioc_reqbufs = vb2_ioctl_reqbufs,
    1477                 :            :         .vidioc_create_bufs = vb2_ioctl_create_bufs,
    1478                 :            :         .vidioc_prepare_buf = vb2_ioctl_prepare_buf,
    1479                 :            :         .vidioc_querybuf = vb2_ioctl_querybuf,
    1480                 :            :         .vidioc_qbuf = vb2_ioctl_qbuf,
    1481                 :            :         .vidioc_dqbuf = vb2_ioctl_dqbuf,
    1482                 :            :         .vidioc_enum_framesizes = vidioc_enum_framesizes,
    1483                 :            :         .vidioc_enum_frameintervals = vidioc_enum_frameintervals,
    1484                 :            :         .vidioc_g_parm        = vidioc_g_parm,
    1485                 :            :         .vidioc_s_parm        = vidioc_s_parm,
    1486                 :            :         .vidioc_streamon = vb2_ioctl_streamon,
    1487                 :            :         .vidioc_streamoff = vb2_ioctl_streamoff,
    1488                 :            : 
    1489                 :            :         .vidioc_log_status = v4l2_ctrl_log_status,
    1490                 :            :         .vidioc_subscribe_event = v4l2_ctrl_subscribe_event,
    1491                 :            :         .vidioc_unsubscribe_event = v4l2_event_unsubscribe,
    1492                 :            : };
    1493                 :            : 
    1494                 :            : /* ------------------------------------------------------------------
    1495                 :            :  *      Driver init/finalise
    1496                 :            :  * ------------------------------------------------------------------
    1497                 :            :  */
    1498                 :            : 
    1499                 :            : static const struct v4l2_file_operations camera0_fops = {
    1500                 :            :         .owner = THIS_MODULE,
    1501                 :            :         .open = v4l2_fh_open,
    1502                 :            :         .release = vb2_fop_release,
    1503                 :            :         .read = vb2_fop_read,
    1504                 :            :         .poll = vb2_fop_poll,
    1505                 :            :         .unlocked_ioctl = video_ioctl2, /* V4L2 ioctl handler */
    1506                 :            :         .mmap = vb2_fop_mmap,
    1507                 :            : };
    1508                 :            : 
    1509                 :            : static const struct video_device vdev_template = {
    1510                 :            :         .name = "camera0",
    1511                 :            :         .fops = &camera0_fops,
    1512                 :            :         .ioctl_ops = &camera0_ioctl_ops,
    1513                 :            :         .release = video_device_release_empty,
    1514                 :            :         .device_caps = V4L2_CAP_VIDEO_CAPTURE | V4L2_CAP_VIDEO_OVERLAY |
    1515                 :            :                        V4L2_CAP_STREAMING | V4L2_CAP_READWRITE,
    1516                 :            : };
    1517                 :            : 
    1518                 :            : /* Returns the number of cameras, and also the max resolution supported
    1519                 :            :  * by those cameras.
    1520                 :            :  */
    1521                 :        207 : static int get_num_cameras(struct vchiq_mmal_instance *instance,
    1522                 :            :                            unsigned int resolutions[][2], int num_resolutions)
    1523                 :            : {
    1524                 :            :         int ret;
    1525                 :            :         struct vchiq_mmal_component  *cam_info_component;
    1526                 :        207 :         struct mmal_parameter_camera_info cam_info = {0};
    1527                 :        207 :         u32 param_size = sizeof(cam_info);
    1528                 :            :         int i;
    1529                 :            : 
    1530                 :            :         /* create a camera_info component */
    1531                 :        207 :         ret = vchiq_mmal_component_init(instance, "camera_info",
    1532                 :            :                                         &cam_info_component);
    1533         [ +  - ]:        207 :         if (ret < 0)
    1534                 :            :                 /* Unusual failure - let's guess one camera. */
    1535                 :            :                 return 1;
    1536                 :            : 
    1537         [ -  + ]:        207 :         if (vchiq_mmal_port_parameter_get(instance,
    1538                 :        207 :                                           &cam_info_component->control,
    1539                 :            :                                           MMAL_PARAMETER_CAMERA_INFO,
    1540                 :            :                                           &cam_info,
    1541                 :            :                                           &param_size)) {
    1542                 :          0 :                 pr_info("Failed to get camera info\n");
    1543                 :            :         }
    1544         [ -  + ]:        207 :         for (i = 0;
    1545                 :        207 :              i < min_t(unsigned int, cam_info.num_cameras, num_resolutions);
    1546                 :          0 :              i++) {
    1547                 :          0 :                 resolutions[i][0] = cam_info.cameras[i].max_width;
    1548                 :          0 :                 resolutions[i][1] = cam_info.cameras[i].max_height;
    1549                 :            :         }
    1550                 :            : 
    1551                 :        207 :         vchiq_mmal_component_finalise(instance,
    1552                 :            :                                       cam_info_component);
    1553                 :            : 
    1554                 :        207 :         return cam_info.num_cameras;
    1555                 :            : }
    1556                 :            : 
    1557                 :          0 : static int set_camera_parameters(struct vchiq_mmal_instance *instance,
    1558                 :            :                                  struct vchiq_mmal_component *camera,
    1559                 :            :                                  struct bm2835_mmal_dev *dev)
    1560                 :            : {
    1561                 :          0 :         struct mmal_parameter_camera_config cam_config = {
    1562                 :          0 :                 .max_stills_w = dev->max_width,
    1563                 :          0 :                 .max_stills_h = dev->max_height,
    1564                 :            :                 .stills_yuv422 = 1,
    1565                 :            :                 .one_shot_stills = 1,
    1566                 :            :                 .max_preview_video_w = (max_video_width > 1920) ?
    1567                 :          0 :                                                 max_video_width : 1920,
    1568                 :            :                 .max_preview_video_h = (max_video_height > 1088) ?
    1569                 :          0 :                                                 max_video_height : 1088,
    1570                 :            :                 .num_preview_video_frames = 3,
    1571                 :            :                 .stills_capture_circular_buffer_height = 0,
    1572                 :            :                 .fast_preview_resume = 0,
    1573                 :            :                 .use_stc_timestamp = MMAL_PARAM_TIMESTAMP_MODE_RAW_STC
    1574                 :            :         };
    1575                 :            : 
    1576                 :          0 :         return vchiq_mmal_port_parameter_set(instance, &camera->control,
    1577                 :            :                                             MMAL_PARAMETER_CAMERA_CONFIG,
    1578                 :            :                                             &cam_config, sizeof(cam_config));
    1579                 :            : }
    1580                 :            : 
    1581                 :            : #define MAX_SUPPORTED_ENCODINGS 20
    1582                 :            : 
    1583                 :            : /* MMAL instance and component init */
    1584                 :          0 : static int mmal_init(struct bm2835_mmal_dev *dev)
    1585                 :            : {
    1586                 :            :         int ret;
    1587                 :            :         struct mmal_es_format_local *format;
    1588                 :            :         u32 supported_encodings[MAX_SUPPORTED_ENCODINGS];
    1589                 :            :         u32 param_size;
    1590                 :            :         struct vchiq_mmal_component  *camera;
    1591                 :            : 
    1592                 :          0 :         ret = vchiq_mmal_init(&dev->instance);
    1593         [ #  # ]:          0 :         if (ret < 0) {
    1594                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: vchiq mmal init failed %d\n",
    1595                 :            :                          __func__, ret);
    1596                 :          0 :                 return ret;
    1597                 :            :         }
    1598                 :            : 
    1599                 :            :         /* get the camera component ready */
    1600                 :          0 :         ret = vchiq_mmal_component_init(dev->instance, "ril.camera",
    1601                 :            :                                         &dev->component[COMP_CAMERA]);
    1602         [ #  # ]:          0 :         if (ret < 0)
    1603                 :            :                 goto unreg_mmal;
    1604                 :            : 
    1605                 :          0 :         camera = dev->component[COMP_CAMERA];
    1606         [ #  # ]:          0 :         if (camera->outputs < CAM_PORT_COUNT) {
    1607                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: too few camera outputs %d needed %d\n",
    1608                 :            :                          __func__, camera->outputs, CAM_PORT_COUNT);
    1609                 :            :                 ret = -EINVAL;
    1610                 :          0 :                 goto unreg_camera;
    1611                 :            :         }
    1612                 :            : 
    1613                 :          0 :         ret = set_camera_parameters(dev->instance,
    1614                 :            :                                     camera,
    1615                 :            :                                     dev);
    1616         [ #  # ]:          0 :         if (ret < 0) {
    1617                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: unable to set camera parameters: %d\n",
    1618                 :            :                          __func__, ret);
    1619                 :          0 :                 goto unreg_camera;
    1620                 :            :         }
    1621                 :            : 
    1622                 :            :         /* There was an error in the firmware that meant the camera component
    1623                 :            :          * produced BGR instead of RGB.
    1624                 :            :          * This is now fixed, but in order to support the old firmwares, we
    1625                 :            :          * have to check.
    1626                 :            :          */
    1627                 :          0 :         dev->rgb_bgr_swapped = true;
    1628                 :          0 :         param_size = sizeof(supported_encodings);
    1629                 :          0 :         ret = vchiq_mmal_port_parameter_get(dev->instance,
    1630                 :            :                                             &camera->output[CAM_PORT_CAPTURE],
    1631                 :            :                                             MMAL_PARAMETER_SUPPORTED_ENCODINGS,
    1632                 :            :                                             &supported_encodings,
    1633                 :            :                                             &param_size);
    1634         [ #  # ]:          0 :         if (ret == 0) {
    1635                 :            :                 int i;
    1636                 :            : 
    1637         [ #  # ]:          0 :                 for (i = 0; i < param_size / sizeof(u32); i++) {
    1638         [ #  # ]:          0 :                         if (supported_encodings[i] == MMAL_ENCODING_BGR24) {
    1639                 :            :                                 /* Found BGR24 first - old firmware. */
    1640                 :            :                                 break;
    1641                 :            :                         }
    1642         [ #  # ]:          0 :                         if (supported_encodings[i] == MMAL_ENCODING_RGB24) {
    1643                 :            :                                 /* Found RGB24 first
    1644                 :            :                                  * new firmware, so use RGB24.
    1645                 :            :                                  */
    1646                 :          0 :                                 dev->rgb_bgr_swapped = false;
    1647                 :          0 :                         break;
    1648                 :            :                         }
    1649                 :            :                 }
    1650                 :            :         }
    1651                 :            :         format = &camera->output[CAM_PORT_PREVIEW].format;
    1652                 :            : 
    1653                 :          0 :         format->encoding = MMAL_ENCODING_OPAQUE;
    1654                 :          0 :         format->encoding_variant = MMAL_ENCODING_I420;
    1655                 :            : 
    1656                 :          0 :         format->es->video.width = 1024;
    1657                 :          0 :         format->es->video.height = 768;
    1658                 :          0 :         format->es->video.crop.x = 0;
    1659                 :          0 :         format->es->video.crop.y = 0;
    1660                 :          0 :         format->es->video.crop.width = 1024;
    1661                 :          0 :         format->es->video.crop.height = 768;
    1662                 :          0 :         format->es->video.frame_rate.num = 0; /* Rely on fps_range */
    1663                 :          0 :         format->es->video.frame_rate.den = 1;
    1664                 :            : 
    1665                 :            :         format = &camera->output[CAM_PORT_VIDEO].format;
    1666                 :            : 
    1667                 :          0 :         format->encoding = MMAL_ENCODING_OPAQUE;
    1668                 :          0 :         format->encoding_variant = MMAL_ENCODING_I420;
    1669                 :            : 
    1670                 :          0 :         format->es->video.width = 1024;
    1671                 :          0 :         format->es->video.height = 768;
    1672                 :          0 :         format->es->video.crop.x = 0;
    1673                 :          0 :         format->es->video.crop.y = 0;
    1674                 :          0 :         format->es->video.crop.width = 1024;
    1675                 :          0 :         format->es->video.crop.height = 768;
    1676                 :          0 :         format->es->video.frame_rate.num = 0; /* Rely on fps_range */
    1677                 :          0 :         format->es->video.frame_rate.den = 1;
    1678                 :            : 
    1679                 :            :         format = &camera->output[CAM_PORT_CAPTURE].format;
    1680                 :            : 
    1681                 :          0 :         format->encoding = MMAL_ENCODING_OPAQUE;
    1682                 :            : 
    1683                 :          0 :         format->es->video.width = 2592;
    1684                 :          0 :         format->es->video.height = 1944;
    1685                 :          0 :         format->es->video.crop.x = 0;
    1686                 :          0 :         format->es->video.crop.y = 0;
    1687                 :          0 :         format->es->video.crop.width = 2592;
    1688                 :          0 :         format->es->video.crop.height = 1944;
    1689                 :          0 :         format->es->video.frame_rate.num = 0; /* Rely on fps_range */
    1690                 :          0 :         format->es->video.frame_rate.den = 1;
    1691                 :            : 
    1692                 :          0 :         dev->capture.width = format->es->video.width;
    1693                 :          0 :         dev->capture.height = format->es->video.height;
    1694                 :          0 :         dev->capture.fmt = &formats[0];
    1695                 :          0 :         dev->capture.encode_component = NULL;
    1696                 :          0 :         dev->capture.timeperframe = tpf_default;
    1697                 :          0 :         dev->capture.enc_profile = V4L2_MPEG_VIDEO_H264_PROFILE_HIGH;
    1698                 :          0 :         dev->capture.enc_level = V4L2_MPEG_VIDEO_H264_LEVEL_4_0;
    1699                 :            : 
    1700                 :            :         /* get the preview component ready */
    1701                 :          0 :         ret = vchiq_mmal_component_init(
    1702                 :            :                         dev->instance, "ril.video_render",
    1703                 :            :                         &dev->component[COMP_PREVIEW]);
    1704         [ #  # ]:          0 :         if (ret < 0)
    1705                 :            :                 goto unreg_camera;
    1706                 :            : 
    1707         [ #  # ]:          0 :         if (dev->component[COMP_PREVIEW]->inputs < 1) {
    1708                 :            :                 ret = -EINVAL;
    1709                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
    1710                 :            :                          __func__, dev->component[COMP_PREVIEW]->inputs, 1);
    1711                 :          0 :                 goto unreg_preview;
    1712                 :            :         }
    1713                 :            : 
    1714                 :            :         /* get the image encoder component ready */
    1715                 :          0 :         ret = vchiq_mmal_component_init(
    1716                 :            :                 dev->instance, "ril.image_encode",
    1717                 :            :                 &dev->component[COMP_IMAGE_ENCODE]);
    1718         [ #  # ]:          0 :         if (ret < 0)
    1719                 :            :                 goto unreg_preview;
    1720                 :            : 
    1721         [ #  # ]:          0 :         if (dev->component[COMP_IMAGE_ENCODE]->inputs < 1) {
    1722                 :            :                 ret = -EINVAL;
    1723                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
    1724                 :            :                          __func__, dev->component[COMP_IMAGE_ENCODE]->inputs,
    1725                 :            :                          1);
    1726                 :          0 :                 goto unreg_image_encoder;
    1727                 :            :         }
    1728                 :            : 
    1729                 :            :         /* get the video encoder component ready */
    1730                 :          0 :         ret = vchiq_mmal_component_init(dev->instance, "ril.video_encode",
    1731                 :            :                                         &dev->component[COMP_VIDEO_ENCODE]);
    1732         [ #  # ]:          0 :         if (ret < 0)
    1733                 :            :                 goto unreg_image_encoder;
    1734                 :            : 
    1735         [ #  # ]:          0 :         if (dev->component[COMP_VIDEO_ENCODE]->inputs < 1) {
    1736                 :            :                 ret = -EINVAL;
    1737                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: too few input ports %d needed %d\n",
    1738                 :            :                          __func__, dev->component[COMP_VIDEO_ENCODE]->inputs,
    1739                 :            :                          1);
    1740                 :          0 :                 goto unreg_vid_encoder;
    1741                 :            :         }
    1742                 :            : 
    1743                 :            :         {
    1744                 :          0 :                 struct vchiq_mmal_port *encoder_port =
    1745                 :            :                         &dev->component[COMP_VIDEO_ENCODE]->output[0];
    1746                 :          0 :                 encoder_port->format.encoding = MMAL_ENCODING_H264;
    1747                 :          0 :                 ret = vchiq_mmal_port_set_format(dev->instance,
    1748                 :            :                                                  encoder_port);
    1749                 :            :         }
    1750                 :            : 
    1751                 :            :         {
    1752                 :          0 :                 unsigned int enable = 1;
    1753                 :            : 
    1754                 :          0 :                 vchiq_mmal_port_parameter_set(
    1755                 :            :                         dev->instance,
    1756                 :          0 :                         &dev->component[COMP_VIDEO_ENCODE]->control,
    1757                 :            :                         MMAL_PARAMETER_VIDEO_IMMUTABLE_INPUT,
    1758                 :            :                         &enable, sizeof(enable));
    1759                 :            : 
    1760                 :          0 :                 vchiq_mmal_port_parameter_set(dev->instance,
    1761                 :          0 :                                               &dev->component[COMP_VIDEO_ENCODE]->control,
    1762                 :            :                                               MMAL_PARAMETER_MINIMISE_FRAGMENTATION,
    1763                 :            :                                               &enable,
    1764                 :            :                                               sizeof(enable));
    1765                 :            : 
    1766                 :            :                 /* Enable inserting headers into the first frame */
    1767                 :          0 :                 vchiq_mmal_port_parameter_set(dev->instance,
    1768                 :          0 :                                               &dev->component[COMP_VIDEO_ENCODE]->control,
    1769                 :            :                                               MMAL_PARAMETER_VIDEO_ENCODE_HEADERS_WITH_FRAME,
    1770                 :            :                                               &enable, sizeof(enable));
    1771                 :            :         }
    1772                 :          0 :         ret = bm2835_mmal_set_all_camera_controls(dev);
    1773         [ #  # ]:          0 :         if (ret < 0) {
    1774                 :          0 :                 v4l2_err(&dev->v4l2_dev, "%s: failed to set all camera controls: %d\n",
    1775                 :            :                          __func__, ret);
    1776                 :          0 :                 goto unreg_vid_encoder;
    1777                 :            :         }
    1778                 :            : 
    1779                 :            :         return 0;
    1780                 :            : 
    1781                 :            : unreg_vid_encoder:
    1782                 :          0 :         pr_err("Cleanup: Destroy video encoder\n");
    1783                 :          0 :         vchiq_mmal_component_finalise(
    1784                 :            :                 dev->instance,
    1785                 :            :                 dev->component[COMP_VIDEO_ENCODE]);
    1786                 :            : 
    1787                 :            : unreg_image_encoder:
    1788                 :          0 :         pr_err("Cleanup: Destroy image encoder\n");
    1789                 :          0 :         vchiq_mmal_component_finalise(
    1790                 :            :                 dev->instance,
    1791                 :            :                 dev->component[COMP_IMAGE_ENCODE]);
    1792                 :            : 
    1793                 :            : unreg_preview:
    1794                 :          0 :         pr_err("Cleanup: Destroy video render\n");
    1795                 :          0 :         vchiq_mmal_component_finalise(dev->instance,
    1796                 :            :                                       dev->component[COMP_PREVIEW]);
    1797                 :            : 
    1798                 :            : unreg_camera:
    1799                 :          0 :         pr_err("Cleanup: Destroy camera\n");
    1800                 :          0 :         vchiq_mmal_component_finalise(dev->instance,
    1801                 :            :                                       dev->component[COMP_CAMERA]);
    1802                 :            : 
    1803                 :            : unreg_mmal:
    1804                 :          0 :         vchiq_mmal_finalise(dev->instance);
    1805                 :          0 :         return ret;
    1806                 :            : }
    1807                 :            : 
    1808                 :          0 : static int bm2835_mmal_init_device(struct bm2835_mmal_dev *dev,
    1809                 :            :                                    struct video_device *vfd)
    1810                 :            : {
    1811                 :            :         int ret;
    1812                 :            : 
    1813                 :          0 :         *vfd = vdev_template;
    1814                 :            : 
    1815                 :          0 :         vfd->v4l2_dev = &dev->v4l2_dev;
    1816                 :            : 
    1817                 :          0 :         vfd->lock = &dev->mutex;
    1818                 :            : 
    1819                 :          0 :         vfd->queue = &dev->capture.vb_vidq;
    1820                 :            : 
    1821                 :            :         /* video device needs to be able to access instance data */
    1822                 :            :         video_set_drvdata(vfd, dev);
    1823                 :            : 
    1824                 :          0 :         ret = video_register_device(vfd,
    1825                 :            :                                     VFL_TYPE_GRABBER,
    1826                 :          0 :                                     video_nr[dev->camera_num]);
    1827         [ #  # ]:          0 :         if (ret < 0)
    1828                 :            :                 return ret;
    1829                 :            : 
    1830                 :          0 :         v4l2_info(vfd->v4l2_dev,
    1831                 :            :                   "V4L2 device registered as %s - stills mode > %dx%d\n",
    1832                 :            :                   video_device_node_name(vfd),
    1833                 :            :                   max_video_width, max_video_height);
    1834                 :            : 
    1835                 :          0 :         return 0;
    1836                 :            : }
    1837                 :            : 
    1838                 :          0 : static void bcm2835_cleanup_instance(struct bm2835_mmal_dev *dev)
    1839                 :            : {
    1840         [ #  # ]:          0 :         if (!dev)
    1841                 :          0 :                 return;
    1842                 :            : 
    1843                 :          0 :         v4l2_info(&dev->v4l2_dev, "unregistering %s\n",
    1844                 :            :                   video_device_node_name(&dev->vdev));
    1845                 :            : 
    1846                 :          0 :         video_unregister_device(&dev->vdev);
    1847                 :            : 
    1848         [ #  # ]:          0 :         if (dev->capture.encode_component) {
    1849         [ #  # ]:          0 :                 v4l2_dbg(1, bcm2835_v4l2_debug, &dev->v4l2_dev,
    1850                 :            :                          "mmal_exit - disconnect tunnel\n");
    1851                 :          0 :                 vchiq_mmal_port_connect_tunnel(dev->instance,
    1852                 :            :                                                dev->capture.camera_port, NULL);
    1853                 :          0 :                 vchiq_mmal_component_disable(dev->instance,
    1854                 :            :                                              dev->capture.encode_component);
    1855                 :            :         }
    1856                 :          0 :         vchiq_mmal_component_disable(dev->instance,
    1857                 :            :                                      dev->component[COMP_CAMERA]);
    1858                 :            : 
    1859                 :          0 :         vchiq_mmal_component_finalise(dev->instance,
    1860                 :            :                                       dev->component[COMP_VIDEO_ENCODE]);
    1861                 :            : 
    1862                 :          0 :         vchiq_mmal_component_finalise(dev->instance,
    1863                 :            :                                       dev->component[COMP_IMAGE_ENCODE]);
    1864                 :            : 
    1865                 :          0 :         vchiq_mmal_component_finalise(dev->instance,
    1866                 :            :                                       dev->component[COMP_PREVIEW]);
    1867                 :            : 
    1868                 :          0 :         vchiq_mmal_component_finalise(dev->instance,
    1869                 :            :                                       dev->component[COMP_CAMERA]);
    1870                 :            : 
    1871                 :          0 :         v4l2_ctrl_handler_free(&dev->ctrl_handler);
    1872                 :            : 
    1873                 :          0 :         v4l2_device_unregister(&dev->v4l2_dev);
    1874                 :            : 
    1875                 :          0 :         kfree(dev);
    1876                 :            : }
    1877                 :            : 
    1878                 :            : static struct v4l2_format default_v4l2_format = {
    1879                 :            :         .fmt.pix.pixelformat = V4L2_PIX_FMT_JPEG,
    1880                 :            :         .fmt.pix.width = 1024,
    1881                 :            :         .fmt.pix.bytesperline = 0,
    1882                 :            :         .fmt.pix.height = 768,
    1883                 :            :         .fmt.pix.sizeimage = 1024 * 768,
    1884                 :            : };
    1885                 :            : 
    1886                 :        207 : static int bcm2835_mmal_probe(struct platform_device *pdev)
    1887                 :            : {
    1888                 :            :         int ret;
    1889                 :            :         struct bm2835_mmal_dev *dev;
    1890                 :            :         struct vb2_queue *q;
    1891                 :            :         int camera;
    1892                 :            :         unsigned int num_cameras;
    1893                 :            :         struct vchiq_mmal_instance *instance;
    1894                 :            :         unsigned int resolutions[MAX_BCM2835_CAMERAS][2];
    1895                 :            :         int i;
    1896                 :            : 
    1897                 :        207 :         ret = vchiq_mmal_init(&instance);
    1898         [ +  - ]:        207 :         if (ret < 0)
    1899                 :            :                 return ret;
    1900                 :            : 
    1901                 :        207 :         num_cameras = get_num_cameras(instance,
    1902                 :            :                                       resolutions,
    1903                 :            :                                       MAX_BCM2835_CAMERAS);
    1904                 :            : 
    1905         [ -  + ]:        207 :         if (num_cameras < 1) {
    1906                 :            :                 ret = -ENODEV;
    1907                 :            :                 goto cleanup_mmal;
    1908                 :            :         }
    1909                 :            : 
    1910         [ #  # ]:          0 :         if (num_cameras > MAX_BCM2835_CAMERAS)
    1911                 :            :                 num_cameras = MAX_BCM2835_CAMERAS;
    1912                 :            : 
    1913         [ #  # ]:          0 :         for (camera = 0; camera < num_cameras; camera++) {
    1914                 :          0 :                 dev = kzalloc(sizeof(*dev), GFP_KERNEL);
    1915         [ #  # ]:          0 :                 if (!dev) {
    1916                 :            :                         ret = -ENOMEM;
    1917                 :            :                         goto cleanup_gdev;
    1918                 :            :                 }
    1919                 :            : 
    1920                 :            :                 /* v4l2 core mutex used to protect all fops and v4l2 ioctls. */
    1921                 :          0 :                 mutex_init(&dev->mutex);
    1922                 :          0 :                 dev->camera_num = camera;
    1923                 :          0 :                 dev->max_width = resolutions[camera][0];
    1924                 :          0 :                 dev->max_height = resolutions[camera][1];
    1925                 :            : 
    1926                 :            :                 /* setup device defaults */
    1927                 :          0 :                 dev->overlay.w.left = 150;
    1928                 :          0 :                 dev->overlay.w.top = 50;
    1929                 :          0 :                 dev->overlay.w.width = 1024;
    1930                 :          0 :                 dev->overlay.w.height = 768;
    1931                 :          0 :                 dev->overlay.clipcount = 0;
    1932                 :          0 :                 dev->overlay.field = V4L2_FIELD_NONE;
    1933                 :          0 :                 dev->overlay.global_alpha = 255;
    1934                 :            : 
    1935                 :          0 :                 dev->capture.fmt = &formats[3]; /* JPEG */
    1936                 :            : 
    1937                 :            :                 /* v4l device registration */
    1938                 :          0 :                 snprintf(dev->v4l2_dev.name, sizeof(dev->v4l2_dev.name),
    1939                 :            :                          "%s", BM2835_MMAL_MODULE_NAME);
    1940                 :          0 :                 ret = v4l2_device_register(NULL, &dev->v4l2_dev);
    1941         [ #  # ]:          0 :                 if (ret) {
    1942                 :          0 :                         dev_err(&pdev->dev, "%s: could not register V4L2 device: %d\n",
    1943                 :            :                                 __func__, ret);
    1944                 :          0 :                         goto free_dev;
    1945                 :            :                 }
    1946                 :            : 
    1947                 :            :                 /* setup v4l controls */
    1948                 :          0 :                 ret = bm2835_mmal_init_controls(dev, &dev->ctrl_handler);
    1949         [ #  # ]:          0 :                 if (ret < 0) {
    1950                 :          0 :                         v4l2_err(&dev->v4l2_dev, "%s: could not init controls: %d\n",
    1951                 :            :                                  __func__, ret);
    1952                 :          0 :                         goto unreg_dev;
    1953                 :            :                 }
    1954                 :          0 :                 dev->v4l2_dev.ctrl_handler = &dev->ctrl_handler;
    1955                 :            : 
    1956                 :            :                 /* mmal init */
    1957                 :          0 :                 dev->instance = instance;
    1958                 :          0 :                 ret = mmal_init(dev);
    1959         [ #  # ]:          0 :                 if (ret < 0) {
    1960                 :          0 :                         v4l2_err(&dev->v4l2_dev, "%s: mmal init failed: %d\n",
    1961                 :            :                                  __func__, ret);
    1962                 :          0 :                         goto unreg_dev;
    1963                 :            :                 }
    1964                 :            :                 /* initialize queue */
    1965                 :          0 :                 q = &dev->capture.vb_vidq;
    1966                 :          0 :                 memset(q, 0, sizeof(*q));
    1967                 :          0 :                 q->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    1968                 :          0 :                 q->io_modes = VB2_MMAP | VB2_USERPTR | VB2_READ;
    1969                 :          0 :                 q->drv_priv = dev;
    1970                 :          0 :                 q->buf_struct_size = sizeof(struct vb2_mmal_buffer);
    1971                 :          0 :                 q->ops = &bm2835_mmal_video_qops;
    1972                 :          0 :                 q->mem_ops = &vb2_vmalloc_memops;
    1973                 :          0 :                 q->timestamp_flags = V4L2_BUF_FLAG_TIMESTAMP_MONOTONIC;
    1974                 :          0 :                 q->lock = &dev->mutex;
    1975                 :          0 :                 ret = vb2_queue_init(q);
    1976         [ #  # ]:          0 :                 if (ret < 0)
    1977                 :            :                         goto unreg_dev;
    1978                 :            : 
    1979                 :            :                 /* initialise video devices */
    1980                 :          0 :                 ret = bm2835_mmal_init_device(dev, &dev->vdev);
    1981         [ #  # ]:          0 :                 if (ret < 0) {
    1982                 :          0 :                         v4l2_err(&dev->v4l2_dev, "%s: could not init device: %d\n",
    1983                 :            :                                  __func__, ret);
    1984                 :          0 :                         goto unreg_dev;
    1985                 :            :                 }
    1986                 :            : 
    1987                 :            :                 /* Really want to call vidioc_s_fmt_vid_cap with the default
    1988                 :            :                  * format, but currently the APIs don't join up.
    1989                 :            :                  */
    1990                 :          0 :                 ret = mmal_setup_components(dev, &default_v4l2_format);
    1991         [ #  # ]:          0 :                 if (ret < 0) {
    1992                 :          0 :                         v4l2_err(&dev->v4l2_dev, "%s: could not setup components: %d\n",
    1993                 :            :                                  __func__, ret);
    1994                 :          0 :                         goto unreg_dev;
    1995                 :            :                 }
    1996                 :            : 
    1997                 :          0 :                 v4l2_info(&dev->v4l2_dev,
    1998                 :            :                           "Broadcom 2835 MMAL video capture ver %s loaded.\n",
    1999                 :            :                           BM2835_MMAL_VERSION);
    2000                 :            : 
    2001                 :          0 :                 gdev[camera] = dev;
    2002                 :            :         }
    2003                 :            :         return 0;
    2004                 :            : 
    2005                 :            : unreg_dev:
    2006                 :          0 :         v4l2_ctrl_handler_free(&dev->ctrl_handler);
    2007                 :          0 :         v4l2_device_unregister(&dev->v4l2_dev);
    2008                 :            : 
    2009                 :            : free_dev:
    2010                 :          0 :         kfree(dev);
    2011                 :            : 
    2012                 :            : cleanup_gdev:
    2013         [ #  # ]:          0 :         for (i = 0; i < camera; i++) {
    2014                 :          0 :                 bcm2835_cleanup_instance(gdev[i]);
    2015                 :          0 :                 gdev[i] = NULL;
    2016                 :            :         }
    2017                 :            : 
    2018                 :            : cleanup_mmal:
    2019                 :        207 :         vchiq_mmal_finalise(instance);
    2020                 :            : 
    2021                 :        207 :         return ret;
    2022                 :            : }
    2023                 :            : 
    2024                 :          0 : static int bcm2835_mmal_remove(struct platform_device *pdev)
    2025                 :            : {
    2026                 :            :         int camera;
    2027                 :          0 :         struct vchiq_mmal_instance *instance = gdev[0]->instance;
    2028                 :            : 
    2029         [ #  # ]:          0 :         for (camera = 0; camera < MAX_BCM2835_CAMERAS; camera++) {
    2030                 :          0 :                 bcm2835_cleanup_instance(gdev[camera]);
    2031                 :          0 :                 gdev[camera] = NULL;
    2032                 :            :         }
    2033                 :          0 :         vchiq_mmal_finalise(instance);
    2034                 :            : 
    2035                 :          0 :         return 0;
    2036                 :            : }
    2037                 :            : 
    2038                 :            : static struct platform_driver bcm2835_camera_driver = {
    2039                 :            :         .probe          = bcm2835_mmal_probe,
    2040                 :            :         .remove         = bcm2835_mmal_remove,
    2041                 :            :         .driver         = {
    2042                 :            :                 .name   = "bcm2835-camera",
    2043                 :            :         },
    2044                 :            : };
    2045                 :            : 
    2046                 :        207 : module_platform_driver(bcm2835_camera_driver)
    2047                 :            : 
    2048                 :            : MODULE_DESCRIPTION("Broadcom 2835 MMAL video capture");
    2049                 :            : MODULE_AUTHOR("Vincent Sanders");
    2050                 :            : MODULE_LICENSE("GPL");
    2051                 :            : MODULE_VERSION(BM2835_MMAL_VERSION);
    2052                 :            : MODULE_ALIAS("platform:bcm2835-camera");

Generated by: LCOV version 1.14