kernel/drivers/media/platform/soc_camera/soc_scale_crop.c

   1 /*
   2  * soc-camera generic scaling-cropping manipulation functions
   3  *
   4  * Copyright (C) 2013 Guennadi Liakhovetski <g.liakhovetski@gmx.de>
   5  *
   6  * This program is free software; you can redistribute it and/or modify
   7  * it under the terms of the GNU General Public License as published by
   8  * the Free Software Foundation; either version 2 of the License, or
   9  * (at your option) any later version.
  10  */
  11
  12 #include <linux/device.h>
  13 #include <linux/module.h>
  14
  15 #include <media/soc_camera.h>
  16 #include <media/v4l2-common.h>
  17
  18 #include "soc_scale_crop.h"
  19
  20 #ifdef DEBUG_GEOMETRY
  21 #define dev_geo dev_info
  22 #else
  23 #define dev_geo dev_dbg
  24 #endif
  25
  26 /* Check if any dimension of r1 is smaller than respective one of r2 */
  27 static bool is_smaller(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
  28 {
  29         return r1->width < r2->width || r1->height < r2->height;
  30 }
  31
  32 /* Check if r1 fails to cover r2 */
  33 static bool is_inside(const struct v4l2_rect *r1, const struct v4l2_rect *r2)
  34 {
  35         return r1->left > r2->left || r1->top > r2->top ||
  36                 r1->left + r1->width < r2->left + r2->width ||
  37                 r1->top + r1->height < r2->top + r2->height;
  38 }
  39
  40 /* Get and store current client crop */
  41 int soc_camera_client_g_rect(struct v4l2_subdev *sd, struct v4l2_rect *rect)
  42 {
  43         struct v4l2_crop crop;
  44         struct v4l2_cropcap cap;
  45         int ret;
  46
  47         crop.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  48
  49         ret = v4l2_subdev_call(sd, video, g_crop, &crop);
  50         if (!ret) {
  51                 *rect = crop.c;
  52                 return ret;
  53         }
  54
  55         /* Camera driver doesn't support .g_crop(), assume default rectangle */
  56         cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
  57
  58         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
  59         if (!ret)
  60                 *rect = cap.defrect;
  61
  62         return ret;
  63 }
  64 EXPORT_SYMBOL(soc_camera_client_g_rect);
  65
  66 /* Client crop has changed, update our sub-rectangle to remain within the area */
  67 static void update_subrect(struct v4l2_rect *rect, struct v4l2_rect *subrect)
  68 {
  69         if (rect->width < subrect->width)
  70                 subrect->width = rect->width;
  71
  72         if (rect->height < subrect->height)
  73                 subrect->height = rect->height;
  74
  75         if (rect->left > subrect->left)
  76                 subrect->left = rect->left;
  77         else if (rect->left + rect->width >
  78                  subrect->left + subrect->width)
  79                 subrect->left = rect->left + rect->width -
  80                         subrect->width;
  81
  82         if (rect->top > subrect->top)
  83                 subrect->top = rect->top;
  84         else if (rect->top + rect->height >
  85                  subrect->top + subrect->height)
  86                 subrect->top = rect->top + rect->height -
  87                         subrect->height;
  88 }
  89
  90 /*
  91  * The common for both scaling and cropping iterative approach is:
  92  * 1. try if the client can produce exactly what requested by the user
  93  * 2. if (1) failed, try to double the client image until we get one big enough
  94  * 3. if (2) failed, try to request the maximum image
  95  */
  96 int soc_camera_client_s_crop(struct v4l2_subdev *sd,
  97                         struct v4l2_crop *crop, struct v4l2_crop *cam_crop,
  98                         struct v4l2_rect *target_rect, struct v4l2_rect *subrect)
  99 {
 100         struct v4l2_rect *rect = &crop->c, *cam_rect = &cam_crop->c;
 101         struct device *dev = sd->v4l2_dev->dev;
 102         struct v4l2_cropcap cap;
 103         int ret;
 104         unsigned int width, height;
 105
 106         v4l2_subdev_call(sd, video, s_crop, crop);
 107         ret = soc_camera_client_g_rect(sd, cam_rect);
 108         if (ret < 0)
 109                 return ret;
 110
 111         /*
 112          * Now cam_crop contains the current camera input rectangle, and it must
 113          * be within camera cropcap bounds
 114          */
 115         if (!memcmp(rect, cam_rect, sizeof(*rect))) {
 116                 /* Even if camera S_CROP failed, but camera rectangle matches */
 117                 dev_dbg(dev, "Camera S_CROP successful for %dx%d@%d:%d\n",
 118                         rect->width, rect->height, rect->left, rect->top);
 119                 *target_rect = *cam_rect;
 120                 return 0;
 121         }
 122
 123         /* Try to fix cropping, that camera hasn't managed to set */
 124         dev_geo(dev, "Fix camera S_CROP for %dx%d@%d:%d to %dx%d@%d:%d\n",
 125                 cam_rect->width, cam_rect->height,
 126                 cam_rect->left, cam_rect->top,
 127                 rect->width, rect->height, rect->left, rect->top);
 128
 129         /* We need sensor maximum rectangle */
 130         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 131         if (ret < 0)
 132                 return ret;
 133
 134         /* Put user requested rectangle within sensor bounds */
 135         soc_camera_limit_side(&rect->left, &rect->width, cap.bounds.left, 2,
 136                               cap.bounds.width);
 137         soc_camera_limit_side(&rect->top, &rect->height, cap.bounds.top, 4,
 138                               cap.bounds.height);
 139
 140         /*
 141          * Popular special case - some cameras can only handle fixed sizes like
 142          * QVGA, VGA,... Take care to avoid infinite loop.
 143          */
 144         width = max_t(unsigned int, cam_rect->width, 2);
 145         height = max_t(unsigned int, cam_rect->height, 2);
 146
 147         /*
 148          * Loop as long as sensor is not covering the requested rectangle and
 149          * is still within its bounds
 150          */
 151         while (!ret && (is_smaller(cam_rect, rect) ||
 152                         is_inside(cam_rect, rect)) &&
 153                (cap.bounds.width > width || cap.bounds.height > height)) {
 154
 155                 width *= 2;
 156                 height *= 2;
 157
 158                 cam_rect->width = width;
 159                 cam_rect->height = height;
 160
 161                 /*
 162                  * We do not know what capabilities the camera has to set up
 163                  * left and top borders. We could try to be smarter in iterating
 164                  * them, e.g., if camera current left is to the right of the
 165                  * target left, set it to the middle point between the current
 166                  * left and minimum left. But that would add too much
 167                  * complexity: we would have to iterate each border separately.
 168                  * Instead we just drop to the left and top bounds.
 169                  */
 170                 if (cam_rect->left > rect->left)
 171                         cam_rect->left = cap.bounds.left;
 172
 173                 if (cam_rect->left + cam_rect->width < rect->left + rect->width)
 174                         cam_rect->width = rect->left + rect->width -
 175                                 cam_rect->left;
 176
 177                 if (cam_rect->top > rect->top)
 178                         cam_rect->top = cap.bounds.top;
 179
 180                 if (cam_rect->top + cam_rect->height < rect->top + rect->height)
 181                         cam_rect->height = rect->top + rect->height -
 182                                 cam_rect->top;
 183
 184                 v4l2_subdev_call(sd, video, s_crop, cam_crop);
 185                 ret = soc_camera_client_g_rect(sd, cam_rect);
 186                 dev_geo(dev, "Camera S_CROP %d for %dx%d@%d:%d\n", ret,
 187                         cam_rect->width, cam_rect->height,
 188                         cam_rect->left, cam_rect->top);
 189         }
 190
 191         /* S_CROP must not modify the rectangle */
 192         if (is_smaller(cam_rect, rect) || is_inside(cam_rect, rect)) {
 193                 /*
 194                  * The camera failed to configure a suitable cropping,
 195                  * we cannot use the current rectangle, set to max
 196                  */
 197                 *cam_rect = cap.bounds;
 198                 v4l2_subdev_call(sd, video, s_crop, cam_crop);
 199                 ret = soc_camera_client_g_rect(sd, cam_rect);
 200                 dev_geo(dev, "Camera S_CROP %d for max %dx%d@%d:%d\n", ret,
 201                         cam_rect->width, cam_rect->height,
 202                         cam_rect->left, cam_rect->top);
 203         }
 204
 205         if (!ret) {
 206                 *target_rect = *cam_rect;
 207                 update_subrect(target_rect, subrect);
 208         }
 209
 210         return ret;
 211 }
 212 EXPORT_SYMBOL(soc_camera_client_s_crop);
 213
 214 /* Iterative set_fmt, also updates cached client crop on success */
 215 static int client_set_fmt(struct soc_camera_device *icd,
 216                         struct v4l2_rect *rect, struct v4l2_rect *subrect,
 217                         unsigned int max_width, unsigned int max_height,
 218                         struct v4l2_subdev_format *format, bool host_can_scale)
 219 {
 220         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 221         struct device *dev = icd->parent;
 222         struct v4l2_mbus_framefmt *mf = &format->format;
 223         unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
 224         struct v4l2_cropcap cap;
 225         bool host_1to1;
 226         int ret;
 227
 228         ret = v4l2_device_call_until_err(sd->v4l2_dev,
 229                                          soc_camera_grp_id(icd), pad,
 230                                          set_fmt, NULL, format);
 231         if (ret < 0)
 232                 return ret;
 233
 234         dev_geo(dev, "camera scaled to %ux%u\n", mf->width, mf->height);
 235
 236         if (width == mf->width && height == mf->height) {
 237                 /* Perfect! The client has done it all. */
 238                 host_1to1 = true;
 239                 goto update_cache;
 240         }
 241
 242         host_1to1 = false;
 243         if (!host_can_scale)
 244                 goto update_cache;
 245
 246         cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 247
 248         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 249         if (ret < 0)
 250                 return ret;
 251
 252         if (max_width > cap.bounds.width)
 253                 max_width = cap.bounds.width;
 254         if (max_height > cap.bounds.height)
 255                 max_height = cap.bounds.height;
 256
 257         /* Camera set a format, but geometry is not precise, try to improve */
 258         tmp_w = mf->width;
 259         tmp_h = mf->height;
 260
 261         /* width <= max_width && height <= max_height - guaranteed by try_fmt */
 262         while ((width > tmp_w || height > tmp_h) &&
 263                tmp_w < max_width && tmp_h < max_height) {
 264                 tmp_w = min(2 * tmp_w, max_width);
 265                 tmp_h = min(2 * tmp_h, max_height);
 266                 mf->width = tmp_w;
 267                 mf->height = tmp_h;
 268                 ret = v4l2_device_call_until_err(sd->v4l2_dev,
 269                                         soc_camera_grp_id(icd), pad,
 270                                         set_fmt, NULL, format);
 271                 dev_geo(dev, "Camera scaled to %ux%u\n",
 272                         mf->width, mf->height);
 273                 if (ret < 0) {
 274                         /* This shouldn't happen */
 275                         dev_err(dev, "Client failed to set format: %d\n", ret);
 276                         return ret;
 277                 }
 278         }
 279
 280 update_cache:
 281         /* Update cache */
 282         ret = soc_camera_client_g_rect(sd, rect);
 283         if (ret < 0)
 284                 return ret;
 285
 286         if (host_1to1)
 287                 *subrect = *rect;
 288         else
 289                 update_subrect(rect, subrect);
 290
 291         return 0;
 292 }
 293
 294 /**
 295  * @icd         - soc-camera device
 296  * @rect        - camera cropping window
 297  * @subrect     - part of rect, sent to the user
 298  * @mf          - in- / output camera output window
 299  * @width       - on input: max host input width
 300  *                on output: user width, mapped back to input
 301  * @height      - on input: max host input height
 302  *                on output: user height, mapped back to input
 303  * @host_can_scale - host can scale this pixel format
 304  * @shift       - shift, used for scaling
 305  */
 306 int soc_camera_client_scale(struct soc_camera_device *icd,
 307                         struct v4l2_rect *rect, struct v4l2_rect *subrect,
 308                         struct v4l2_mbus_framefmt *mf,
 309                         unsigned int *width, unsigned int *height,
 310                         bool host_can_scale, unsigned int shift)
 311 {
 312         struct device *dev = icd->parent;
 313         struct v4l2_subdev_format fmt_tmp = {
 314                 .which = V4L2_SUBDEV_FORMAT_ACTIVE,
 315                 .format = *mf,
 316         };
 317         struct v4l2_mbus_framefmt *mf_tmp = &fmt_tmp.format;
 318         unsigned int scale_h, scale_v;
 319         int ret;
 320
 321         /*
 322          * 5. Apply iterative camera S_FMT for camera user window (also updates
 323          *    client crop cache and the imaginary sub-rectangle).
 324          */
 325         ret = client_set_fmt(icd, rect, subrect, *width, *height,
 326                            &fmt_tmp, host_can_scale);
 327         if (ret < 0)
 328                 return ret;
 329
 330         dev_geo(dev, "5: camera scaled to %ux%u\n",
 331                 mf_tmp->width, mf_tmp->height);
 332
 333         /* 6. Retrieve camera output window (g_fmt) */
 334
 335         /* unneeded - it is already in "mf_tmp" */
 336
 337         /* 7. Calculate new client scales. */
 338         scale_h = soc_camera_calc_scale(rect->width, shift, mf_tmp->width);
 339         scale_v = soc_camera_calc_scale(rect->height, shift, mf_tmp->height);
 340
 341         mf->width       = mf_tmp->width;
 342         mf->height      = mf_tmp->height;
 343         mf->colorspace  = mf_tmp->colorspace;
 344
 345         /*
 346          * 8. Calculate new host crop - apply camera scales to previously
 347          *    updated "effective" crop.
 348          */
 349         *width = soc_camera_shift_scale(subrect->width, shift, scale_h);
 350         *height = soc_camera_shift_scale(subrect->height, shift, scale_v);
 351
 352         dev_geo(dev, "8: new client sub-window %ux%u\n", *width, *height);
 353
 354         return 0;
 355 }
 356 EXPORT_SYMBOL(soc_camera_client_scale);
 357
 358 /*
 359  * Calculate real client output window by applying new scales to the current
 360  * client crop. New scales are calculated from the requested output format and
 361  * host crop, mapped backed onto the client input (subrect).
 362  */
 363 void soc_camera_calc_client_output(struct soc_camera_device *icd,
 364                 struct v4l2_rect *rect, struct v4l2_rect *subrect,
 365                 const struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf,
 366                 unsigned int shift)
 367 {
 368         struct device *dev = icd->parent;
 369         unsigned int scale_v, scale_h;
 370
 371         if (subrect->width == rect->width &&
 372             subrect->height == rect->height) {
 373                 /* No sub-cropping */
 374                 mf->width       = pix->width;
 375                 mf->height      = pix->height;
 376                 return;
 377         }
 378
 379         /* 1.-2. Current camera scales and subwin - cached. */
 380
 381         dev_geo(dev, "2: subwin %ux%u@%u:%u\n",
 382                 subrect->width, subrect->height,
 383                 subrect->left, subrect->top);
 384
 385         /*
 386          * 3. Calculate new combined scales from input sub-window to requested
 387          *    user window.
 388          */
 389
 390         /*
 391          * TODO: CEU cannot scale images larger than VGA to smaller than SubQCIF
 392          * (128x96) or larger than VGA. This and similar limitations have to be
 393          * taken into account here.
 394          */
 395         scale_h = soc_camera_calc_scale(subrect->width, shift, pix->width);
 396         scale_v = soc_camera_calc_scale(subrect->height, shift, pix->height);
 397
 398         dev_geo(dev, "3: scales %u:%u\n", scale_h, scale_v);
 399
 400         /*
 401          * 4. Calculate desired client output window by applying combined scales
 402          *    to client (real) input window.
 403          */
 404         mf->width = soc_camera_shift_scale(rect->width, shift, scale_h);
 405         mf->height = soc_camera_shift_scale(rect->height, shift, scale_v);
 406 }
 407 EXPORT_SYMBOL(soc_camera_calc_client_output);