Commit | Line | Data |
---|---|---|
22e0099a GL |
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 | { | |
10d5509c HV |
43 | struct v4l2_subdev_selection sdsel = { |
44 | .which = V4L2_SUBDEV_FORMAT_ACTIVE, | |
45 | .target = V4L2_SEL_TGT_CROP, | |
46 | }; | |
22e0099a GL |
47 | int ret; |
48 | ||
10d5509c | 49 | ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel); |
22e0099a | 50 | if (!ret) { |
10d5509c | 51 | *rect = sdsel.r; |
22e0099a GL |
52 | return ret; |
53 | } | |
54 | ||
10d5509c HV |
55 | sdsel.target = V4L2_SEL_TGT_CROP_DEFAULT; |
56 | ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel); | |
22e0099a | 57 | if (!ret) |
10d5509c | 58 | *rect = sdsel.r; |
22e0099a GL |
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 | */ | |
10d5509c HV |
94 | int soc_camera_client_s_selection(struct v4l2_subdev *sd, |
95 | struct v4l2_selection *sel, struct v4l2_selection *cam_sel, | |
22e0099a GL |
96 | struct v4l2_rect *target_rect, struct v4l2_rect *subrect) |
97 | { | |
10d5509c HV |
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; | |
22e0099a | 109 | struct device *dev = sd->v4l2_dev->dev; |
22e0099a GL |
110 | int ret; |
111 | unsigned int width, height; | |
112 | ||
10d5509c HV |
113 | v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel); |
114 | sel->r = sdsel.r; | |
22e0099a GL |
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))) { | |
10d5509c HV |
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", | |
22e0099a GL |
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 */ | |
10d5509c | 132 | dev_geo(dev, "Fix camera S_SELECTION for %dx%d@%d:%d to %dx%d@%d:%d\n", |
22e0099a GL |
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 */ | |
10d5509c | 138 | ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &bounds); |
22e0099a GL |
139 | if (ret < 0) |
140 | return ret; | |
141 | ||
142 | /* Put user requested rectangle within sensor bounds */ | |
10d5509c HV |
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); | |
22e0099a GL |
147 | |
148 | /* | |
149 | * Popular special case - some cameras can only handle fixed sizes like | |
150 | * QVGA, VGA,... Take care to avoid infinite loop. | |
151 | */ | |
f90580ca RR |
152 | width = max_t(unsigned int, cam_rect->width, 2); |
153 | height = max_t(unsigned int, cam_rect->height, 2); | |
22e0099a GL |
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)) && | |
10d5509c | 161 | (bounds.r.width > width || bounds.r.height > height)) { |
22e0099a GL |
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) | |
10d5509c | 179 | cam_rect->left = bounds.r.left; |
22e0099a GL |
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) | |
10d5509c | 186 | cam_rect->top = bounds.r.top; |
22e0099a GL |
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 | ||
10d5509c HV |
192 | sdsel.r = *cam_rect; |
193 | v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel); | |
194 | *cam_rect = sdsel.r; | |
22e0099a | 195 | ret = soc_camera_client_g_rect(sd, cam_rect); |
10d5509c | 196 | dev_geo(dev, "Camera S_SELECTION %d for %dx%d@%d:%d\n", ret, |
22e0099a GL |
197 | cam_rect->width, cam_rect->height, |
198 | cam_rect->left, cam_rect->top); | |
199 | } | |
200 | ||
10d5509c | 201 | /* S_SELECTION must not modify the rectangle */ |
22e0099a GL |
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 | */ | |
10d5509c HV |
207 | sdsel.r = bounds.r; |
208 | v4l2_subdev_call(sd, pad, set_selection, NULL, &sdsel); | |
209 | *cam_rect = sdsel.r; | |
210 | ||
22e0099a | 211 | ret = soc_camera_client_g_rect(sd, cam_rect); |
10d5509c | 212 | dev_geo(dev, "Camera S_SELECTION %d for max %dx%d@%d:%d\n", ret, |
22e0099a GL |
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 | } | |
10d5509c | 224 | EXPORT_SYMBOL(soc_camera_client_s_selection); |
22e0099a | 225 | |
ebf984bb HV |
226 | /* Iterative set_fmt, also updates cached client crop on success */ |
227 | static int client_set_fmt(struct soc_camera_device *icd, | |
22e0099a GL |
228 | struct v4l2_rect *rect, struct v4l2_rect *subrect, |
229 | unsigned int max_width, unsigned int max_height, | |
ebf984bb | 230 | struct v4l2_subdev_format *format, bool host_can_scale) |
22e0099a GL |
231 | { |
232 | struct v4l2_subdev *sd = soc_camera_to_subdev(icd); | |
233 | struct device *dev = icd->parent; | |
ebf984bb | 234 | struct v4l2_mbus_framefmt *mf = &format->format; |
22e0099a | 235 | unsigned int width = mf->width, height = mf->height, tmp_w, tmp_h; |
10d5509c HV |
236 | struct v4l2_subdev_selection sdsel = { |
237 | .which = V4L2_SUBDEV_FORMAT_ACTIVE, | |
238 | .target = V4L2_SEL_TGT_CROP_BOUNDS, | |
239 | }; | |
79275a99 | 240 | bool host_1to1; |
22e0099a GL |
241 | int ret; |
242 | ||
243 | ret = v4l2_device_call_until_err(sd->v4l2_dev, | |
ebf984bb HV |
244 | soc_camera_grp_id(icd), pad, |
245 | set_fmt, NULL, format); | |
22e0099a GL |
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. */ | |
79275a99 | 253 | host_1to1 = true; |
22e0099a GL |
254 | goto update_cache; |
255 | } | |
256 | ||
79275a99 | 257 | host_1to1 = false; |
22e0099a GL |
258 | if (!host_can_scale) |
259 | goto update_cache; | |
260 | ||
10d5509c | 261 | ret = v4l2_subdev_call(sd, pad, get_selection, NULL, &sdsel); |
22e0099a GL |
262 | if (ret < 0) |
263 | return ret; | |
264 | ||
10d5509c HV |
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; | |
22e0099a GL |
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, | |
ebf984bb HV |
282 | soc_camera_grp_id(icd), pad, |
283 | set_fmt, NULL, format); | |
22e0099a GL |
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 | ||
79275a99 | 299 | if (host_1to1) |
22e0099a GL |
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; | |
ebf984bb HV |
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; | |
22e0099a GL |
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 | */ | |
ebf984bb HV |
338 | ret = client_set_fmt(icd, rect, subrect, *width, *height, |
339 | &fmt_tmp, host_can_scale); | |
22e0099a GL |
340 | if (ret < 0) |
341 | return ret; | |
342 | ||
343 | dev_geo(dev, "5: camera scaled to %ux%u\n", | |
ebf984bb | 344 | mf_tmp->width, mf_tmp->height); |
22e0099a GL |
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. */ | |
ebf984bb HV |
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); | |
22e0099a | 353 | |
ebf984bb HV |
354 | mf->width = mf_tmp->width; |
355 | mf->height = mf_tmp->height; | |
356 | mf->colorspace = mf_tmp->colorspace; | |
22e0099a GL |
357 | |
358 | /* | |
79275a99 | 359 | * 8. Calculate new host crop - apply camera scales to previously |
22e0099a GL |
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 | |
79275a99 | 374 | * host crop, mapped backed onto the client input (subrect). |
22e0099a GL |
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 | |
79275a99 GL |
405 | * (128x96) or larger than VGA. This and similar limitations have to be |
406 | * taken into account here. | |
22e0099a GL |
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); |