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