1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47#include <linux/module.h>
48#include <linux/types.h>
49#include <linux/kernel.h>
50#include <linux/mm.h>
51#include <linux/string.h>
52#include <linux/errno.h>
53#include <linux/i2c.h>
54#if defined(CONFIG_SPI)
55#include <linux/spi/spi.h>
56#endif
57#include <asm/uaccess.h>
58#include <asm/pgtable.h>
59#include <asm/io.h>
60#include <asm/div64.h>
61#include <media/v4l2-common.h>
62#include <media/v4l2-device.h>
63#include <media/v4l2-ctrls.h>
64
65#include <linux/videodev2.h>
66
67MODULE_AUTHOR("Bill Dirks, Justin Schoeman, Gerd Knorr");
68MODULE_DESCRIPTION("misc helper functions for v4l2 device drivers");
69MODULE_LICENSE("GPL");
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86int v4l2_ctrl_check(struct v4l2_ext_control *ctrl, struct v4l2_queryctrl *qctrl,
87 const char * const *menu_items)
88{
89 if (qctrl->flags & V4L2_CTRL_FLAG_DISABLED)
90 return -EINVAL;
91 if (qctrl->flags & V4L2_CTRL_FLAG_GRABBED)
92 return -EBUSY;
93 if (qctrl->type == V4L2_CTRL_TYPE_STRING)
94 return 0;
95 if (qctrl->type == V4L2_CTRL_TYPE_BUTTON ||
96 qctrl->type == V4L2_CTRL_TYPE_INTEGER64 ||
97 qctrl->type == V4L2_CTRL_TYPE_CTRL_CLASS)
98 return 0;
99 if (ctrl->value < qctrl->minimum || ctrl->value > qctrl->maximum)
100 return -ERANGE;
101 if (qctrl->type == V4L2_CTRL_TYPE_MENU && menu_items != NULL) {
102 if (menu_items[ctrl->value] == NULL ||
103 menu_items[ctrl->value][0] == '\0')
104 return -EINVAL;
105 }
106 if (qctrl->type == V4L2_CTRL_TYPE_BITMASK &&
107 (ctrl->value & ~qctrl->maximum))
108 return -ERANGE;
109 return 0;
110}
111EXPORT_SYMBOL(v4l2_ctrl_check);
112
113
114int v4l2_ctrl_query_fill(struct v4l2_queryctrl *qctrl, s32 _min, s32 _max, s32 _step, s32 _def)
115{
116 const char *name;
117 s64 min = _min;
118 s64 max = _max;
119 u64 step = _step;
120 s64 def = _def;
121
122 v4l2_ctrl_fill(qctrl->id, &name, &qctrl->type,
123 &min, &max, &step, &def, &qctrl->flags);
124
125 if (name == NULL)
126 return -EINVAL;
127
128 qctrl->minimum = min;
129 qctrl->maximum = max;
130 qctrl->step = step;
131 qctrl->default_value = def;
132 qctrl->reserved[0] = qctrl->reserved[1] = 0;
133 strlcpy(qctrl->name, name, sizeof(qctrl->name));
134 return 0;
135}
136EXPORT_SYMBOL(v4l2_ctrl_query_fill);
137
138
139
140
141
142int v4l2_ctrl_query_menu(struct v4l2_querymenu *qmenu, struct v4l2_queryctrl *qctrl,
143 const char * const *menu_items)
144{
145 int i;
146
147 qmenu->reserved = 0;
148 if (menu_items == NULL)
149 menu_items = v4l2_ctrl_get_menu(qmenu->id);
150 if (menu_items == NULL ||
151 (qctrl && (qmenu->index < qctrl->minimum || qmenu->index > qctrl->maximum)))
152 return -EINVAL;
153 for (i = 0; i < qmenu->index && menu_items[i]; i++) ;
154 if (menu_items[i] == NULL || menu_items[i][0] == '\0')
155 return -EINVAL;
156 strlcpy(qmenu->name, menu_items[qmenu->index], sizeof(qmenu->name));
157 return 0;
158}
159EXPORT_SYMBOL(v4l2_ctrl_query_menu);
160
161
162
163
164int v4l2_ctrl_query_menu_valid_items(struct v4l2_querymenu *qmenu, const u32 *ids)
165{
166 const char * const *menu_items = v4l2_ctrl_get_menu(qmenu->id);
167
168 qmenu->reserved = 0;
169 if (menu_items == NULL || ids == NULL)
170 return -EINVAL;
171 while (*ids != V4L2_CTRL_MENU_IDS_END) {
172 if (*ids++ == qmenu->index) {
173 strlcpy(qmenu->name, menu_items[qmenu->index],
174 sizeof(qmenu->name));
175 return 0;
176 }
177 }
178 return -EINVAL;
179}
180EXPORT_SYMBOL(v4l2_ctrl_query_menu_valid_items);
181
182
183
184
185
186
187
188
189
190u32 v4l2_ctrl_next(const u32 * const * ctrl_classes, u32 id)
191{
192 u32 ctrl_class = V4L2_CTRL_ID2CLASS(id);
193 const u32 *pctrl;
194
195 if (ctrl_classes == NULL)
196 return 0;
197
198
199 if ((id & V4L2_CTRL_FLAG_NEXT_CTRL) == 0) {
200
201 while (*ctrl_classes && V4L2_CTRL_ID2CLASS(**ctrl_classes) != ctrl_class)
202 ctrl_classes++;
203 if (*ctrl_classes == NULL)
204 return 0;
205 pctrl = *ctrl_classes;
206
207 while (*pctrl && *pctrl != id) pctrl++;
208 return *pctrl ? id : 0;
209 }
210 id &= V4L2_CTRL_ID_MASK;
211 id++;
212
213
214 while (*ctrl_classes && V4L2_CTRL_ID2CLASS(**ctrl_classes) < ctrl_class)
215 ctrl_classes++;
216
217 if (*ctrl_classes == NULL)
218 return 0;
219 pctrl = *ctrl_classes;
220
221 while (*pctrl && *pctrl < id) pctrl++;
222 if (*pctrl)
223 return *pctrl;
224
225
226 ctrl_classes++;
227 if (*ctrl_classes == NULL)
228 return 0;
229 return **ctrl_classes;
230}
231EXPORT_SYMBOL(v4l2_ctrl_next);
232
233
234
235#if IS_ENABLED(CONFIG_I2C)
236
237void v4l2_i2c_subdev_init(struct v4l2_subdev *sd, struct i2c_client *client,
238 const struct v4l2_subdev_ops *ops)
239{
240 v4l2_subdev_init(sd, ops);
241 sd->flags |= V4L2_SUBDEV_FL_IS_I2C;
242
243 sd->owner = client->dev.driver->owner;
244 sd->dev = &client->dev;
245
246 v4l2_set_subdevdata(sd, client);
247 i2c_set_clientdata(client, sd);
248
249 snprintf(sd->name, sizeof(sd->name), "%s %d-%04x",
250 client->dev.driver->name, i2c_adapter_id(client->adapter),
251 client->addr);
252}
253EXPORT_SYMBOL_GPL(v4l2_i2c_subdev_init);
254
255
256struct v4l2_subdev *v4l2_i2c_new_subdev_board(struct v4l2_device *v4l2_dev,
257 struct i2c_adapter *adapter, struct i2c_board_info *info,
258 const unsigned short *probe_addrs)
259{
260 struct v4l2_subdev *sd = NULL;
261 struct i2c_client *client;
262
263 BUG_ON(!v4l2_dev);
264
265 request_module(I2C_MODULE_PREFIX "%s", info->type);
266
267
268 if (info->addr == 0 && probe_addrs)
269 client = i2c_new_probed_device(adapter, info, probe_addrs,
270 NULL);
271 else
272 client = i2c_new_device(adapter, info);
273
274
275
276
277
278
279
280
281 if (client == NULL || client->dev.driver == NULL)
282 goto error;
283
284
285 if (!try_module_get(client->dev.driver->owner))
286 goto error;
287 sd = i2c_get_clientdata(client);
288
289
290
291 if (v4l2_device_register_subdev(v4l2_dev, sd))
292 sd = NULL;
293
294 module_put(client->dev.driver->owner);
295
296error:
297
298
299 if (client && sd == NULL)
300 i2c_unregister_device(client);
301 return sd;
302}
303EXPORT_SYMBOL_GPL(v4l2_i2c_new_subdev_board);
304
305struct v4l2_subdev *v4l2_i2c_new_subdev(struct v4l2_device *v4l2_dev,
306 struct i2c_adapter *adapter, const char *client_type,
307 u8 addr, const unsigned short *probe_addrs)
308{
309 struct i2c_board_info info;
310
311
312
313 memset(&info, 0, sizeof(info));
314 strlcpy(info.type, client_type, sizeof(info.type));
315 info.addr = addr;
316
317 return v4l2_i2c_new_subdev_board(v4l2_dev, adapter, &info, probe_addrs);
318}
319EXPORT_SYMBOL_GPL(v4l2_i2c_new_subdev);
320
321
322unsigned short v4l2_i2c_subdev_addr(struct v4l2_subdev *sd)
323{
324 struct i2c_client *client = v4l2_get_subdevdata(sd);
325
326 return client ? client->addr : I2C_CLIENT_END;
327}
328EXPORT_SYMBOL_GPL(v4l2_i2c_subdev_addr);
329
330
331
332const unsigned short *v4l2_i2c_tuner_addrs(enum v4l2_i2c_tuner_type type)
333{
334 static const unsigned short radio_addrs[] = {
335#if IS_ENABLED(CONFIG_MEDIA_TUNER_TEA5761)
336 0x10,
337#endif
338 0x60,
339 I2C_CLIENT_END
340 };
341 static const unsigned short demod_addrs[] = {
342 0x42, 0x43, 0x4a, 0x4b,
343 I2C_CLIENT_END
344 };
345 static const unsigned short tv_addrs[] = {
346 0x42, 0x43, 0x4a, 0x4b,
347 0x60, 0x61, 0x62, 0x63, 0x64,
348 I2C_CLIENT_END
349 };
350
351 switch (type) {
352 case ADDRS_RADIO:
353 return radio_addrs;
354 case ADDRS_DEMOD:
355 return demod_addrs;
356 case ADDRS_TV:
357 return tv_addrs;
358 case ADDRS_TV_WITH_DEMOD:
359 return tv_addrs + 4;
360 }
361 return NULL;
362}
363EXPORT_SYMBOL_GPL(v4l2_i2c_tuner_addrs);
364
365#endif
366
367#if defined(CONFIG_SPI)
368
369
370
371void v4l2_spi_subdev_init(struct v4l2_subdev *sd, struct spi_device *spi,
372 const struct v4l2_subdev_ops *ops)
373{
374 v4l2_subdev_init(sd, ops);
375 sd->flags |= V4L2_SUBDEV_FL_IS_SPI;
376
377 sd->owner = spi->dev.driver->owner;
378 sd->dev = &spi->dev;
379
380 v4l2_set_subdevdata(sd, spi);
381 spi_set_drvdata(spi, sd);
382
383 strlcpy(sd->name, spi->dev.driver->name, sizeof(sd->name));
384}
385EXPORT_SYMBOL_GPL(v4l2_spi_subdev_init);
386
387struct v4l2_subdev *v4l2_spi_new_subdev(struct v4l2_device *v4l2_dev,
388 struct spi_master *master, struct spi_board_info *info)
389{
390 struct v4l2_subdev *sd = NULL;
391 struct spi_device *spi = NULL;
392
393 BUG_ON(!v4l2_dev);
394
395 if (info->modalias[0])
396 request_module(info->modalias);
397
398 spi = spi_new_device(master, info);
399
400 if (spi == NULL || spi->dev.driver == NULL)
401 goto error;
402
403 if (!try_module_get(spi->dev.driver->owner))
404 goto error;
405
406 sd = spi_get_drvdata(spi);
407
408
409
410 if (v4l2_device_register_subdev(v4l2_dev, sd))
411 sd = NULL;
412
413
414 module_put(spi->dev.driver->owner);
415
416error:
417
418
419 if (spi && sd == NULL)
420 spi_unregister_device(spi);
421
422 return sd;
423}
424EXPORT_SYMBOL_GPL(v4l2_spi_new_subdev);
425
426#endif
427
428
429
430
431
432static unsigned int clamp_align(unsigned int x, unsigned int min,
433 unsigned int max, unsigned int align)
434{
435
436 unsigned int mask = ~((1 << align) - 1);
437
438
439 if (align)
440 x = (x + (1 << (align - 1))) & mask;
441
442
443 if (x < min)
444 x = (min + ~mask) & mask;
445 else if (x > max)
446 x = max & mask;
447
448 return x;
449}
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465void v4l_bound_align_image(u32 *w, unsigned int wmin, unsigned int wmax,
466 unsigned int walign,
467 u32 *h, unsigned int hmin, unsigned int hmax,
468 unsigned int halign, unsigned int salign)
469{
470 *w = clamp_align(*w, wmin, wmax, walign);
471 *h = clamp_align(*h, hmin, hmax, halign);
472
473
474 if (!salign)
475 return;
476
477
478 walign = __ffs(*w);
479 halign = __ffs(*h);
480
481 if (walign + halign < salign) {
482
483 unsigned int wmaxa = __fls(wmax ^ (wmin - 1));
484
485 unsigned int hmaxa = __fls(hmax ^ (hmin - 1));
486
487
488 do {
489 if (halign >= hmaxa ||
490 (walign <= halign && walign < wmaxa)) {
491 *w = clamp_align(*w, wmin, wmax, walign + 1);
492 walign = __ffs(*w);
493 } else {
494 *h = clamp_align(*h, hmin, hmax, halign + 1);
495 halign = __ffs(*h);
496 }
497 } while (halign + walign < salign);
498 }
499}
500EXPORT_SYMBOL_GPL(v4l_bound_align_image);
501
502const struct v4l2_frmsize_discrete *v4l2_find_nearest_format(
503 const struct v4l2_discrete_probe *probe,
504 s32 width, s32 height)
505{
506 int i;
507 u32 error, min_error = UINT_MAX;
508 const struct v4l2_frmsize_discrete *size, *best = NULL;
509
510 if (!probe)
511 return best;
512
513 for (i = 0, size = probe->sizes; i < probe->num_sizes; i++, size++) {
514 error = abs(size->width - width) + abs(size->height - height);
515 if (error < min_error) {
516 min_error = error;
517 best = size;
518 }
519 if (!error)
520 break;
521 }
522
523 return best;
524}
525EXPORT_SYMBOL_GPL(v4l2_find_nearest_format);
526
527void v4l2_get_timestamp(struct timeval *tv)
528{
529 struct timespec ts;
530
531 ktime_get_ts(&ts);
532 tv->tv_sec = ts.tv_sec;
533 tv->tv_usec = ts.tv_nsec / NSEC_PER_USEC;
534}
535EXPORT_SYMBOL_GPL(v4l2_get_timestamp);
536