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 s_mbus_fmt, also updates cached client crop on success */
 215 static int client_s_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_mbus_framefmt *mf, bool host_can_scale)
 219 {
 220         struct v4l2_subdev *sd = soc_camera_to_subdev(icd);
 221         struct device *dev = icd->parent;
 222         unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h;
 223         struct v4l2_cropcap cap;
 224         bool host_1to1;
 225         int ret;
 226
 227         ret = v4l2_device_call_until_err(sd->v4l2_dev,
 228                                          soc_camera_grp_id(icd), video,
 229                                          s_mbus_fmt, mf);
 230         if (ret < 0)
 231                 return ret;
 232
 233         dev_geo(dev, "camera scaled to %ux%u\n", mf->width, mf->height);
 234
 235         if (width == mf->width && height == mf->height) {
 236                 /* Perfect! The client has done it all. */
 237                 host_1to1 = true;
 238                 goto update_cache;
 239         }
 240
 241         host_1to1 = false;
 242         if (!host_can_scale)
 243                 goto update_cache;
 244
 245         cap.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
 246
 247         ret = v4l2_subdev_call(sd, video, cropcap, &cap);
 248         if (ret < 0)
 249                 return ret;
 250
 251         if (max_width > cap.bounds.width)
 252                 max_width = cap.bounds.width;
 253         if (max_height > cap.bounds.height)
 254                 max_height = cap.bounds.height;
 255
 256         /* Camera set a format, but geometry is not precise, try to improve */
 257         tmp_w = mf->width;
 258         tmp_h = mf->height;
 259
 260         /* width <= max_width && height <= max_height - guaranteed by try_fmt */
 261         while ((width > tmp_w || height > tmp_h) &&
 262                tmp_w < max_width && tmp_h < max_height) {
 263                 tmp_w = min(2 * tmp_w, max_width);
 264                 tmp_h = min(2 * tmp_h, max_height);
 265                 mf->width = tmp_w;
 266                 mf->height = tmp_h;
 267                 ret = v4l2_device_call_until_err(sd->v4l2_dev,
 268                                         soc_camera_grp_id(icd), video,
 269                                         s_mbus_fmt, mf);
 270                 dev_geo(dev, "Camera scaled to %ux%u\n",
 271                         mf->width, mf->height);
 272                 if (ret < 0) {
 273                         /* This shouldn't happen */
 274                         dev_err(dev, "Client failed to set format: %d\n", ret);
 275                         return ret;
 276                 }
 277         }
 278
 279 update_cache:
 280         /* Update cache */
 281         ret = soc_camera_client_g_rect(sd, rect);
 282         if (ret < 0)
 283                 return ret;
 284
 285         if (host_1to1)
 286                 *subrect = *rect;
 287         else
 288                 update_subrect(rect, subrect);
 289
 290         return 0;
 291 }
 292
 293 /**
 294  * @icd         - soc-camera device
 295  * @rect        - camera cropping window
 296  * @subrect     - part of rect, sent to the user
 297  * @mf          - in- / output camera output window
 298  * @width       - on input: max host input width
 299  *                on output: user width, mapped back to input
 300  * @height      - on input: max host input height
 301  *                on output: user height, mapped back to input
 302  * @host_can_scale - host can scale this pixel format
 303  * @shift       - shift, used for scaling
 304  */
 305 int soc_camera_client_scale(struct soc_camera_device *icd,
 306                         struct v4l2_rect *rect, struct v4l2_rect *subrect,
 307                         struct v4l2_mbus_framefmt *mf,
 308                         unsigned int *width, unsigned int *height,
 309                         bool host_can_scale, unsigned int shift)
 310 {
 311         struct device *dev = icd->parent;
 312         struct v4l2_mbus_framefmt mf_tmp = *mf;
 313         unsigned int scale_h, scale_v;
 314         int ret;
 315
 316         /*
 317          * 5. Apply iterative camera S_FMT for camera user window (also updates
 318          *    client crop cache and the imaginary sub-rectangle).
 319          */
 320         ret = client_s_fmt(icd, rect, subrect, *width, *height,
 321                            &mf_tmp, host_can_scale);
 322         if (ret < 0)
 323                 return ret;
 324
 325         dev_geo(dev, "5: camera scaled to %ux%u\n",
 326                 mf_tmp.width, mf_tmp.height);
 327
 328         /* 6. Retrieve camera output window (g_fmt) */
 329
 330         /* unneeded - it is already in "mf_tmp" */
 331
 332         /* 7. Calculate new client scales. */
 333         scale_h = soc_camera_calc_scale(rect->width, shift, mf_tmp.width);
 334         scale_v = soc_camera_calc_scale(rect->height, shift, mf_tmp.height);
 335
 336         mf->width       = mf_tmp.width;
 337         mf->height      = mf_tmp.height;
 338         mf->colorspace  = mf_tmp.colorspace;
 339
 340         /*
 341          * 8. Calculate new host crop - apply camera scales to previously
 342          *    updated "effective" crop.
 343          */
 344         *width = soc_camera_shift_scale(subrect->width, shift, scale_h);
 345         *height = soc_camera_shift_scale(subrect->height, shift, scale_v);
 346
 347         dev_geo(dev, "8: new client sub-window %ux%u\n", *width, *height);
 348
 349         return 0;
 350 }
 351 EXPORT_SYMBOL(soc_camera_client_scale);
 352
 353 /*
 354  * Calculate real client output window by applying new scales to the current
 355  * client crop. New scales are calculated from the requested output format and
 356  * host crop, mapped backed onto the client input (subrect).
 357  */
 358 void soc_camera_calc_client_output(struct soc_camera_device *icd,
 359                 struct v4l2_rect *rect, struct v4l2_rect *subrect,
 360                 const struct v4l2_pix_format *pix, struct v4l2_mbus_framefmt *mf,
 361                 unsigned int shift)
 362 {
 363         struct device *dev = icd->parent;
 364         unsigned int scale_v, scale_h;
 365
 366         if (subrect->width == rect->width &&
 367             subrect->height == rect->height) {
 368                 /* No sub-cropping */
 369                 mf->width       = pix->width;
 370                 mf->height      = pix->height;
 371                 return;
 372         }
 373
 374         /* 1.-2. Current camera scales and subwin - cached. */
 375
 376         dev_geo(dev, "2: subwin %ux%u@%u:%u\n",
 377                 subrect->width, subrect->height,
 378                 subrect->left, subrect->top);
 379
 380         /*
 381          * 3. Calculate new combined scales from input sub-window to requested
 382          *    user window.
 383          */
 384
 385         /*
 386          * TODO: CEU cannot scale images larger than VGA to smaller than SubQCIF
 387          * (128x96) or larger than VGA. This and similar limitations have to be
 388          * taken into account here.
 389          */
 390         scale_h = soc_camera_calc_scale(subrect->width, shift, pix->width);
 391         scale_v = soc_camera_calc_scale(subrect->height, shift, pix->height);
 392
 393         dev_geo(dev, "3: scales %u:%u\n", scale_h, scale_v);
 394
 395         /*
 396          * 4. Calculate desired client output window by applying combined scales
 397          *    to client (real) input window.
 398          */
 399         mf->width = soc_camera_shift_scale(rect->width, shift, scale_h);
 400         mf->height = soc_camera_shift_scale(rect->height, shift, scale_v);
 401 }
 402 EXPORT_SYMBOL(soc_camera_calc_client_output);