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