Merge branch 'x86/unify-cpu-detect' into x86-v28-for-linus-phase4-D
[linux-2.6] / drivers / media / video / uvc / uvc_ctrl.c
1 /*
2  *      uvc_ctrl.c  --  USB Video Class driver - Controls
3  *
4  *      Copyright (C) 2005-2008
5  *          Laurent Pinchart (laurent.pinchart@skynet.be)
6  *
7  *      This program is free software; you can redistribute it and/or modify
8  *      it under the terms of the GNU General Public License as published by
9  *      the Free Software Foundation; either version 2 of the License, or
10  *      (at your option) any later version.
11  *
12  */
13
14 #include <linux/kernel.h>
15 #include <linux/version.h>
16 #include <linux/list.h>
17 #include <linux/module.h>
18 #include <linux/uaccess.h>
19 #include <linux/usb.h>
20 #include <linux/videodev2.h>
21 #include <linux/vmalloc.h>
22 #include <linux/wait.h>
23 #include <asm/atomic.h>
24
25 #include "uvcvideo.h"
26
27 #define UVC_CTRL_NDATA          2
28 #define UVC_CTRL_DATA_CURRENT   0
29 #define UVC_CTRL_DATA_BACKUP    1
30
31 /* ------------------------------------------------------------------------
32  * Control, formats, ...
33  */
34
35 static struct uvc_control_info uvc_ctrls[] = {
36         {
37                 .entity         = UVC_GUID_UVC_PROCESSING,
38                 .selector       = PU_BRIGHTNESS_CONTROL,
39                 .index          = 0,
40                 .size           = 2,
41                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
42                                 | UVC_CONTROL_RESTORE,
43         },
44         {
45                 .entity         = UVC_GUID_UVC_PROCESSING,
46                 .selector       = PU_CONTRAST_CONTROL,
47                 .index          = 1,
48                 .size           = 2,
49                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
50                                 | UVC_CONTROL_RESTORE,
51         },
52         {
53                 .entity         = UVC_GUID_UVC_PROCESSING,
54                 .selector       = PU_HUE_CONTROL,
55                 .index          = 2,
56                 .size           = 2,
57                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
58                                 | UVC_CONTROL_RESTORE | UVC_CONTROL_AUTO_UPDATE,
59         },
60         {
61                 .entity         = UVC_GUID_UVC_PROCESSING,
62                 .selector       = PU_SATURATION_CONTROL,
63                 .index          = 3,
64                 .size           = 2,
65                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
66                                 | UVC_CONTROL_RESTORE,
67         },
68         {
69                 .entity         = UVC_GUID_UVC_PROCESSING,
70                 .selector       = PU_SHARPNESS_CONTROL,
71                 .index          = 4,
72                 .size           = 2,
73                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
74                                 | UVC_CONTROL_RESTORE,
75         },
76         {
77                 .entity         = UVC_GUID_UVC_PROCESSING,
78                 .selector       = PU_GAMMA_CONTROL,
79                 .index          = 5,
80                 .size           = 2,
81                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
82                                 | UVC_CONTROL_RESTORE,
83         },
84         {
85                 .entity         = UVC_GUID_UVC_PROCESSING,
86                 .selector       = PU_BACKLIGHT_COMPENSATION_CONTROL,
87                 .index          = 8,
88                 .size           = 2,
89                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
90                                 | UVC_CONTROL_RESTORE,
91         },
92         {
93                 .entity         = UVC_GUID_UVC_PROCESSING,
94                 .selector       = PU_GAIN_CONTROL,
95                 .index          = 9,
96                 .size           = 2,
97                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
98                                 | UVC_CONTROL_RESTORE,
99         },
100         {
101                 .entity         = UVC_GUID_UVC_PROCESSING,
102                 .selector       = PU_POWER_LINE_FREQUENCY_CONTROL,
103                 .index          = 10,
104                 .size           = 1,
105                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
106                                 | UVC_CONTROL_RESTORE,
107         },
108         {
109                 .entity         = UVC_GUID_UVC_PROCESSING,
110                 .selector       = PU_HUE_AUTO_CONTROL,
111                 .index          = 11,
112                 .size           = 1,
113                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_CUR
114                                 | UVC_CONTROL_GET_DEF | UVC_CONTROL_RESTORE,
115         },
116         {
117                 .entity         = UVC_GUID_UVC_CAMERA,
118                 .selector       = CT_AE_MODE_CONTROL,
119                 .index          = 1,
120                 .size           = 1,
121                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_CUR
122                                 | UVC_CONTROL_GET_DEF | UVC_CONTROL_GET_RES
123                                 | UVC_CONTROL_RESTORE,
124         },
125         {
126                 .entity         = UVC_GUID_UVC_CAMERA,
127                 .selector       = CT_AE_PRIORITY_CONTROL,
128                 .index          = 2,
129                 .size           = 1,
130                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_CUR
131                                 | UVC_CONTROL_RESTORE,
132         },
133         {
134                 .entity         = UVC_GUID_UVC_CAMERA,
135                 .selector       = CT_EXPOSURE_TIME_ABSOLUTE_CONTROL,
136                 .index          = 3,
137                 .size           = 4,
138                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
139                                 | UVC_CONTROL_RESTORE,
140         },
141         {
142                 .entity         = UVC_GUID_UVC_CAMERA,
143                 .selector       = CT_FOCUS_ABSOLUTE_CONTROL,
144                 .index          = 5,
145                 .size           = 2,
146                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
147                                 | UVC_CONTROL_RESTORE | UVC_CONTROL_AUTO_UPDATE,
148         },
149         {
150                 .entity         = UVC_GUID_UVC_CAMERA,
151                 .selector       = CT_FOCUS_AUTO_CONTROL,
152                 .index          = 17,
153                 .size           = 1,
154                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_CUR
155                                 | UVC_CONTROL_GET_DEF | UVC_CONTROL_RESTORE,
156         },
157         {
158                 .entity         = UVC_GUID_UVC_PROCESSING,
159                 .selector       = PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL,
160                 .index          = 12,
161                 .size           = 1,
162                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_CUR
163                                 | UVC_CONTROL_GET_DEF | UVC_CONTROL_RESTORE,
164         },
165         {
166                 .entity         = UVC_GUID_UVC_PROCESSING,
167                 .selector       = PU_WHITE_BALANCE_TEMPERATURE_CONTROL,
168                 .index          = 6,
169                 .size           = 2,
170                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
171                                 | UVC_CONTROL_RESTORE | UVC_CONTROL_AUTO_UPDATE,
172         },
173         {
174                 .entity         = UVC_GUID_UVC_PROCESSING,
175                 .selector       = PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL,
176                 .index          = 13,
177                 .size           = 1,
178                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_CUR
179                                 | UVC_CONTROL_GET_DEF | UVC_CONTROL_RESTORE,
180         },
181         {
182                 .entity         = UVC_GUID_UVC_PROCESSING,
183                 .selector       = PU_WHITE_BALANCE_COMPONENT_CONTROL,
184                 .index          = 7,
185                 .size           = 4,
186                 .flags          = UVC_CONTROL_SET_CUR | UVC_CONTROL_GET_RANGE
187                                 | UVC_CONTROL_RESTORE | UVC_CONTROL_AUTO_UPDATE,
188         },
189 };
190
191 static struct uvc_menu_info power_line_frequency_controls[] = {
192         { 0, "Disabled" },
193         { 1, "50 Hz" },
194         { 2, "60 Hz" },
195 };
196
197 static struct uvc_menu_info exposure_auto_controls[] = {
198         { 2, "Auto Mode" },
199         { 1, "Manual Mode" },
200         { 4, "Shutter Priority Mode" },
201         { 8, "Aperture Priority Mode" },
202 };
203
204 static struct uvc_control_mapping uvc_ctrl_mappings[] = {
205         {
206                 .id             = V4L2_CID_BRIGHTNESS,
207                 .name           = "Brightness",
208                 .entity         = UVC_GUID_UVC_PROCESSING,
209                 .selector       = PU_BRIGHTNESS_CONTROL,
210                 .size           = 16,
211                 .offset         = 0,
212                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
213                 .data_type      = UVC_CTRL_DATA_TYPE_SIGNED,
214         },
215         {
216                 .id             = V4L2_CID_CONTRAST,
217                 .name           = "Contrast",
218                 .entity         = UVC_GUID_UVC_PROCESSING,
219                 .selector       = PU_CONTRAST_CONTROL,
220                 .size           = 16,
221                 .offset         = 0,
222                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
223                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
224         },
225         {
226                 .id             = V4L2_CID_HUE,
227                 .name           = "Hue",
228                 .entity         = UVC_GUID_UVC_PROCESSING,
229                 .selector       = PU_HUE_CONTROL,
230                 .size           = 16,
231                 .offset         = 0,
232                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
233                 .data_type      = UVC_CTRL_DATA_TYPE_SIGNED,
234         },
235         {
236                 .id             = V4L2_CID_SATURATION,
237                 .name           = "Saturation",
238                 .entity         = UVC_GUID_UVC_PROCESSING,
239                 .selector       = PU_SATURATION_CONTROL,
240                 .size           = 16,
241                 .offset         = 0,
242                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
243                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
244         },
245         {
246                 .id             = V4L2_CID_SHARPNESS,
247                 .name           = "Sharpness",
248                 .entity         = UVC_GUID_UVC_PROCESSING,
249                 .selector       = PU_SHARPNESS_CONTROL,
250                 .size           = 16,
251                 .offset         = 0,
252                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
253                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
254         },
255         {
256                 .id             = V4L2_CID_GAMMA,
257                 .name           = "Gamma",
258                 .entity         = UVC_GUID_UVC_PROCESSING,
259                 .selector       = PU_GAMMA_CONTROL,
260                 .size           = 16,
261                 .offset         = 0,
262                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
263                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
264         },
265         {
266                 .id             = V4L2_CID_BACKLIGHT_COMPENSATION,
267                 .name           = "Backlight Compensation",
268                 .entity         = UVC_GUID_UVC_PROCESSING,
269                 .selector       = PU_BACKLIGHT_COMPENSATION_CONTROL,
270                 .size           = 16,
271                 .offset         = 0,
272                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
273                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
274         },
275         {
276                 .id             = V4L2_CID_GAIN,
277                 .name           = "Gain",
278                 .entity         = UVC_GUID_UVC_PROCESSING,
279                 .selector       = PU_GAIN_CONTROL,
280                 .size           = 16,
281                 .offset         = 0,
282                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
283                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
284         },
285         {
286                 .id             = V4L2_CID_POWER_LINE_FREQUENCY,
287                 .name           = "Power Line Frequency",
288                 .entity         = UVC_GUID_UVC_PROCESSING,
289                 .selector       = PU_POWER_LINE_FREQUENCY_CONTROL,
290                 .size           = 2,
291                 .offset         = 0,
292                 .v4l2_type      = V4L2_CTRL_TYPE_MENU,
293                 .data_type      = UVC_CTRL_DATA_TYPE_ENUM,
294                 .menu_info      = power_line_frequency_controls,
295                 .menu_count     = ARRAY_SIZE(power_line_frequency_controls),
296         },
297         {
298                 .id             = V4L2_CID_HUE_AUTO,
299                 .name           = "Hue, Auto",
300                 .entity         = UVC_GUID_UVC_PROCESSING,
301                 .selector       = PU_HUE_AUTO_CONTROL,
302                 .size           = 1,
303                 .offset         = 0,
304                 .v4l2_type      = V4L2_CTRL_TYPE_BOOLEAN,
305                 .data_type      = UVC_CTRL_DATA_TYPE_BOOLEAN,
306         },
307         {
308                 .id             = V4L2_CID_EXPOSURE_AUTO,
309                 .name           = "Exposure, Auto",
310                 .entity         = UVC_GUID_UVC_CAMERA,
311                 .selector       = CT_AE_MODE_CONTROL,
312                 .size           = 4,
313                 .offset         = 0,
314                 .v4l2_type      = V4L2_CTRL_TYPE_MENU,
315                 .data_type      = UVC_CTRL_DATA_TYPE_BITMASK,
316                 .menu_info      = exposure_auto_controls,
317                 .menu_count     = ARRAY_SIZE(exposure_auto_controls),
318         },
319         {
320                 .id             = V4L2_CID_EXPOSURE_AUTO_PRIORITY,
321                 .name           = "Exposure, Auto Priority",
322                 .entity         = UVC_GUID_UVC_CAMERA,
323                 .selector       = CT_AE_PRIORITY_CONTROL,
324                 .size           = 1,
325                 .offset         = 0,
326                 .v4l2_type      = V4L2_CTRL_TYPE_BOOLEAN,
327                 .data_type      = UVC_CTRL_DATA_TYPE_BOOLEAN,
328         },
329         {
330                 .id             = V4L2_CID_EXPOSURE_ABSOLUTE,
331                 .name           = "Exposure (Absolute)",
332                 .entity         = UVC_GUID_UVC_CAMERA,
333                 .selector       = CT_EXPOSURE_TIME_ABSOLUTE_CONTROL,
334                 .size           = 32,
335                 .offset         = 0,
336                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
337                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
338         },
339         {
340                 .id             = V4L2_CID_AUTO_WHITE_BALANCE,
341                 .name           = "White Balance Temperature, Auto",
342                 .entity         = UVC_GUID_UVC_PROCESSING,
343                 .selector       = PU_WHITE_BALANCE_TEMPERATURE_AUTO_CONTROL,
344                 .size           = 1,
345                 .offset         = 0,
346                 .v4l2_type      = V4L2_CTRL_TYPE_BOOLEAN,
347                 .data_type      = UVC_CTRL_DATA_TYPE_BOOLEAN,
348         },
349         {
350                 .id             = V4L2_CID_WHITE_BALANCE_TEMPERATURE,
351                 .name           = "White Balance Temperature",
352                 .entity         = UVC_GUID_UVC_PROCESSING,
353                 .selector       = PU_WHITE_BALANCE_TEMPERATURE_CONTROL,
354                 .size           = 16,
355                 .offset         = 0,
356                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
357                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
358         },
359         {
360                 .id             = V4L2_CID_AUTO_WHITE_BALANCE,
361                 .name           = "White Balance Component, Auto",
362                 .entity         = UVC_GUID_UVC_PROCESSING,
363                 .selector       = PU_WHITE_BALANCE_COMPONENT_AUTO_CONTROL,
364                 .size           = 1,
365                 .offset         = 0,
366                 .v4l2_type      = V4L2_CTRL_TYPE_BOOLEAN,
367                 .data_type      = UVC_CTRL_DATA_TYPE_BOOLEAN,
368         },
369         {
370                 .id             = V4L2_CID_BLUE_BALANCE,
371                 .name           = "White Balance Blue Component",
372                 .entity         = UVC_GUID_UVC_PROCESSING,
373                 .selector       = PU_WHITE_BALANCE_COMPONENT_CONTROL,
374                 .size           = 16,
375                 .offset         = 0,
376                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
377                 .data_type      = UVC_CTRL_DATA_TYPE_SIGNED,
378         },
379         {
380                 .id             = V4L2_CID_RED_BALANCE,
381                 .name           = "White Balance Red Component",
382                 .entity         = UVC_GUID_UVC_PROCESSING,
383                 .selector       = PU_WHITE_BALANCE_COMPONENT_CONTROL,
384                 .size           = 16,
385                 .offset         = 16,
386                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
387                 .data_type      = UVC_CTRL_DATA_TYPE_SIGNED,
388         },
389         {
390                 .id             = V4L2_CID_FOCUS_ABSOLUTE,
391                 .name           = "Focus (absolute)",
392                 .entity         = UVC_GUID_UVC_CAMERA,
393                 .selector       = CT_FOCUS_ABSOLUTE_CONTROL,
394                 .size           = 16,
395                 .offset         = 0,
396                 .v4l2_type      = V4L2_CTRL_TYPE_INTEGER,
397                 .data_type      = UVC_CTRL_DATA_TYPE_UNSIGNED,
398         },
399         {
400                 .id             = V4L2_CID_FOCUS_AUTO,
401                 .name           = "Focus, Auto",
402                 .entity         = UVC_GUID_UVC_CAMERA,
403                 .selector       = CT_FOCUS_AUTO_CONTROL,
404                 .size           = 1,
405                 .offset         = 0,
406                 .v4l2_type      = V4L2_CTRL_TYPE_BOOLEAN,
407                 .data_type      = UVC_CTRL_DATA_TYPE_BOOLEAN,
408         },
409 };
410
411 /* ------------------------------------------------------------------------
412  * Utility functions
413  */
414
415 static inline __u8 *uvc_ctrl_data(struct uvc_control *ctrl, int id)
416 {
417         return ctrl->data + id * ctrl->info->size;
418 }
419
420 static inline int uvc_get_bit(const __u8 *data, int bit)
421 {
422         return (data[bit >> 3] >> (bit & 7)) & 1;
423 }
424
425 /* Extract the bit string specified by mapping->offset and mapping->size
426  * from the little-endian data stored at 'data' and return the result as
427  * a signed 32bit integer. Sign extension will be performed if the mapping
428  * references a signed data type.
429  */
430 static __s32 uvc_get_le_value(const __u8 *data,
431         struct uvc_control_mapping *mapping)
432 {
433         int bits = mapping->size;
434         int offset = mapping->offset;
435         __s32 value = 0;
436         __u8 mask;
437
438         data += offset / 8;
439         offset &= 7;
440         mask = ((1LL << bits) - 1) << offset;
441
442         for (; bits > 0; data++) {
443                 __u8 byte = *data & mask;
444                 value |= offset > 0 ? (byte >> offset) : (byte << (-offset));
445                 bits -= 8 - (offset > 0 ? offset : 0);
446                 offset -= 8;
447                 mask = (1 << bits) - 1;
448         }
449
450         /* Sign-extend the value if needed */
451         if (mapping->data_type == UVC_CTRL_DATA_TYPE_SIGNED)
452                 value |= -(value & (1 << (mapping->size - 1)));
453
454         return value;
455 }
456
457 /* Set the bit string specified by mapping->offset and mapping->size
458  * in the little-endian data stored at 'data' to the value 'value'.
459  */
460 static void uvc_set_le_value(__s32 value, __u8 *data,
461         struct uvc_control_mapping *mapping)
462 {
463         int bits = mapping->size;
464         int offset = mapping->offset;
465         __u8 mask;
466
467         data += offset / 8;
468         offset &= 7;
469
470         for (; bits > 0; data++) {
471                 mask = ((1LL << bits) - 1) << offset;
472                 *data = (*data & ~mask) | ((value << offset) & mask);
473                 value >>= offset ? offset : 8;
474                 bits -= 8 - offset;
475                 offset = 0;
476         }
477 }
478
479 /* ------------------------------------------------------------------------
480  * Terminal and unit management
481  */
482
483 static const __u8 uvc_processing_guid[16] = UVC_GUID_UVC_PROCESSING;
484 static const __u8 uvc_camera_guid[16] = UVC_GUID_UVC_CAMERA;
485 static const __u8 uvc_media_transport_input_guid[16] =
486         UVC_GUID_UVC_MEDIA_TRANSPORT_INPUT;
487
488 static int uvc_entity_match_guid(struct uvc_entity *entity, __u8 guid[16])
489 {
490         switch (UVC_ENTITY_TYPE(entity)) {
491         case ITT_CAMERA:
492                 return memcmp(uvc_camera_guid, guid, 16) == 0;
493
494         case ITT_MEDIA_TRANSPORT_INPUT:
495                 return memcmp(uvc_media_transport_input_guid, guid, 16) == 0;
496
497         case VC_PROCESSING_UNIT:
498                 return memcmp(uvc_processing_guid, guid, 16) == 0;
499
500         case VC_EXTENSION_UNIT:
501                 return memcmp(entity->extension.guidExtensionCode,
502                               guid, 16) == 0;
503
504         default:
505                 return 0;
506         }
507 }
508
509 /* ------------------------------------------------------------------------
510  * UVC Controls
511  */
512
513 static void __uvc_find_control(struct uvc_entity *entity, __u32 v4l2_id,
514         struct uvc_control_mapping **mapping, struct uvc_control **control,
515         int next)
516 {
517         struct uvc_control *ctrl;
518         struct uvc_control_mapping *map;
519         unsigned int i;
520
521         if (entity == NULL)
522                 return;
523
524         for (i = 0; i < entity->ncontrols; ++i) {
525                 ctrl = &entity->controls[i];
526                 if (ctrl->info == NULL)
527                         continue;
528
529                 list_for_each_entry(map, &ctrl->info->mappings, list) {
530                         if ((map->id == v4l2_id) && !next) {
531                                 *control = ctrl;
532                                 *mapping = map;
533                                 return;
534                         }
535
536                         if ((*mapping == NULL || (*mapping)->id > map->id) &&
537                             (map->id > v4l2_id) && next) {
538                                 *control = ctrl;
539                                 *mapping = map;
540                         }
541                 }
542         }
543 }
544
545 struct uvc_control *uvc_find_control(struct uvc_video_device *video,
546         __u32 v4l2_id, struct uvc_control_mapping **mapping)
547 {
548         struct uvc_control *ctrl = NULL;
549         struct uvc_entity *entity;
550         int next = v4l2_id & V4L2_CTRL_FLAG_NEXT_CTRL;
551
552         *mapping = NULL;
553
554         /* Mask the query flags. */
555         v4l2_id &= V4L2_CTRL_ID_MASK;
556
557         /* Find the control. */
558         __uvc_find_control(video->processing, v4l2_id, mapping, &ctrl, next);
559         if (ctrl && !next)
560                 return ctrl;
561
562         list_for_each_entry(entity, &video->iterms, chain) {
563                 __uvc_find_control(entity, v4l2_id, mapping, &ctrl, next);
564                 if (ctrl && !next)
565                         return ctrl;
566         }
567
568         list_for_each_entry(entity, &video->extensions, chain) {
569                 __uvc_find_control(entity, v4l2_id, mapping, &ctrl, next);
570                 if (ctrl && !next)
571                         return ctrl;
572         }
573
574         if (ctrl == NULL && !next)
575                 uvc_trace(UVC_TRACE_CONTROL, "Control 0x%08x not found.\n",
576                                 v4l2_id);
577
578         return ctrl;
579 }
580
581 int uvc_query_v4l2_ctrl(struct uvc_video_device *video,
582         struct v4l2_queryctrl *v4l2_ctrl)
583 {
584         struct uvc_control *ctrl;
585         struct uvc_control_mapping *mapping;
586         struct uvc_menu_info *menu;
587         unsigned int i;
588         __u8 *data;
589         int ret;
590
591         ctrl = uvc_find_control(video, v4l2_ctrl->id, &mapping);
592         if (ctrl == NULL)
593                 return -EINVAL;
594
595         data = kmalloc(ctrl->info->size, GFP_KERNEL);
596         if (data == NULL)
597                 return -ENOMEM;
598
599         memset(v4l2_ctrl, 0, sizeof *v4l2_ctrl);
600         v4l2_ctrl->id = mapping->id;
601         v4l2_ctrl->type = mapping->v4l2_type;
602         strncpy(v4l2_ctrl->name, mapping->name, sizeof v4l2_ctrl->name);
603         v4l2_ctrl->flags = 0;
604
605         if (!(ctrl->info->flags & UVC_CONTROL_SET_CUR))
606                 v4l2_ctrl->flags |= V4L2_CTRL_FLAG_READ_ONLY;
607
608         if (ctrl->info->flags & UVC_CONTROL_GET_DEF) {
609                 if ((ret = uvc_query_ctrl(video->dev, GET_DEF, ctrl->entity->id,
610                                 video->dev->intfnum, ctrl->info->selector,
611                                 data, ctrl->info->size)) < 0)
612                         goto out;
613                 v4l2_ctrl->default_value = uvc_get_le_value(data, mapping);
614         }
615
616         switch (mapping->v4l2_type) {
617         case V4L2_CTRL_TYPE_MENU:
618                 v4l2_ctrl->minimum = 0;
619                 v4l2_ctrl->maximum = mapping->menu_count - 1;
620                 v4l2_ctrl->step = 1;
621
622                 menu = mapping->menu_info;
623                 for (i = 0; i < mapping->menu_count; ++i, ++menu) {
624                         if (menu->value == v4l2_ctrl->default_value) {
625                                 v4l2_ctrl->default_value = i;
626                                 break;
627                         }
628                 }
629
630                 ret = 0;
631                 goto out;
632
633         case V4L2_CTRL_TYPE_BOOLEAN:
634                 v4l2_ctrl->minimum = 0;
635                 v4l2_ctrl->maximum = 1;
636                 v4l2_ctrl->step = 1;
637                 ret = 0;
638                 goto out;
639
640         default:
641                 break;
642         }
643
644         if (ctrl->info->flags & UVC_CONTROL_GET_MIN) {
645                 if ((ret = uvc_query_ctrl(video->dev, GET_MIN, ctrl->entity->id,
646                                 video->dev->intfnum, ctrl->info->selector,
647                                 data, ctrl->info->size)) < 0)
648                         goto out;
649                 v4l2_ctrl->minimum = uvc_get_le_value(data, mapping);
650         }
651         if (ctrl->info->flags & UVC_CONTROL_GET_MAX) {
652                 if ((ret = uvc_query_ctrl(video->dev, GET_MAX, ctrl->entity->id,
653                                 video->dev->intfnum, ctrl->info->selector,
654                                 data, ctrl->info->size)) < 0)
655                         goto out;
656                 v4l2_ctrl->maximum = uvc_get_le_value(data, mapping);
657         }
658         if (ctrl->info->flags & UVC_CONTROL_GET_RES) {
659                 if ((ret = uvc_query_ctrl(video->dev, GET_RES, ctrl->entity->id,
660                                 video->dev->intfnum, ctrl->info->selector,
661                                 data, ctrl->info->size)) < 0)
662                         goto out;
663                 v4l2_ctrl->step = uvc_get_le_value(data, mapping);
664         }
665
666         ret = 0;
667 out:
668         kfree(data);
669         return ret;
670 }
671
672
673 /* --------------------------------------------------------------------------
674  * Control transactions
675  *
676  * To make extended set operations as atomic as the hardware allows, controls
677  * are handled using begin/commit/rollback operations.
678  *
679  * At the beginning of a set request, uvc_ctrl_begin should be called to
680  * initialize the request. This function acquires the control lock.
681  *
682  * When setting a control, the new value is stored in the control data field
683  * at position UVC_CTRL_DATA_CURRENT. The control is then marked as dirty for
684  * later processing. If the UVC and V4L2 control sizes differ, the current
685  * value is loaded from the hardware before storing the new value in the data
686  * field.
687  *
688  * After processing all controls in the transaction, uvc_ctrl_commit or
689  * uvc_ctrl_rollback must be called to apply the pending changes to the
690  * hardware or revert them. When applying changes, all controls marked as
691  * dirty will be modified in the UVC device, and the dirty flag will be
692  * cleared. When reverting controls, the control data field
693  * UVC_CTRL_DATA_CURRENT is reverted to its previous value
694  * (UVC_CTRL_DATA_BACKUP) for all dirty controls. Both functions release the
695  * control lock.
696  */
697 int uvc_ctrl_begin(struct uvc_video_device *video)
698 {
699         return mutex_lock_interruptible(&video->ctrl_mutex) ? -ERESTARTSYS : 0;
700 }
701
702 static int uvc_ctrl_commit_entity(struct uvc_device *dev,
703         struct uvc_entity *entity, int rollback)
704 {
705         struct uvc_control *ctrl;
706         unsigned int i;
707         int ret;
708
709         if (entity == NULL)
710                 return 0;
711
712         for (i = 0; i < entity->ncontrols; ++i) {
713                 ctrl = &entity->controls[i];
714                 if (ctrl->info == NULL || !ctrl->dirty)
715                         continue;
716
717                 if (!rollback)
718                         ret = uvc_query_ctrl(dev, SET_CUR, ctrl->entity->id,
719                                 dev->intfnum, ctrl->info->selector,
720                                 uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
721                                 ctrl->info->size);
722                 else
723                         ret = 0;
724
725                 if (rollback || ret < 0)
726                         memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
727                                uvc_ctrl_data(ctrl, UVC_CTRL_DATA_BACKUP),
728                                ctrl->info->size);
729
730                 if ((ctrl->info->flags & UVC_CONTROL_GET_CUR) == 0)
731                         ctrl->loaded = 0;
732
733                 ctrl->dirty = 0;
734
735                 if (ret < 0)
736                         return ret;
737         }
738
739         return 0;
740 }
741
742 int __uvc_ctrl_commit(struct uvc_video_device *video, int rollback)
743 {
744         struct uvc_entity *entity;
745         int ret = 0;
746
747         /* Find the control. */
748         ret = uvc_ctrl_commit_entity(video->dev, video->processing, rollback);
749         if (ret < 0)
750                 goto done;
751
752         list_for_each_entry(entity, &video->iterms, chain) {
753                 ret = uvc_ctrl_commit_entity(video->dev, entity, rollback);
754                 if (ret < 0)
755                         goto done;
756         }
757
758         list_for_each_entry(entity, &video->extensions, chain) {
759                 ret = uvc_ctrl_commit_entity(video->dev, entity, rollback);
760                 if (ret < 0)
761                         goto done;
762         }
763
764 done:
765         mutex_unlock(&video->ctrl_mutex);
766         return ret;
767 }
768
769 int uvc_ctrl_get(struct uvc_video_device *video,
770         struct v4l2_ext_control *xctrl)
771 {
772         struct uvc_control *ctrl;
773         struct uvc_control_mapping *mapping;
774         struct uvc_menu_info *menu;
775         unsigned int i;
776         int ret;
777
778         ctrl = uvc_find_control(video, xctrl->id, &mapping);
779         if (ctrl == NULL || (ctrl->info->flags & UVC_CONTROL_GET_CUR) == 0)
780                 return -EINVAL;
781
782         if (!ctrl->loaded) {
783                 ret = uvc_query_ctrl(video->dev, GET_CUR, ctrl->entity->id,
784                                 video->dev->intfnum, ctrl->info->selector,
785                                 uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
786                                 ctrl->info->size);
787                 if (ret < 0)
788                         return ret;
789
790                 if ((ctrl->info->flags & UVC_CONTROL_AUTO_UPDATE) == 0)
791                         ctrl->loaded = 1;
792         }
793
794         xctrl->value = uvc_get_le_value(
795                 uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT), mapping);
796
797         if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
798                 menu = mapping->menu_info;
799                 for (i = 0; i < mapping->menu_count; ++i, ++menu) {
800                         if (menu->value == xctrl->value) {
801                                 xctrl->value = i;
802                                 break;
803                         }
804                 }
805         }
806
807         return 0;
808 }
809
810 int uvc_ctrl_set(struct uvc_video_device *video,
811         struct v4l2_ext_control *xctrl)
812 {
813         struct uvc_control *ctrl;
814         struct uvc_control_mapping *mapping;
815         s32 value = xctrl->value;
816         int ret;
817
818         ctrl = uvc_find_control(video, xctrl->id, &mapping);
819         if (ctrl == NULL || (ctrl->info->flags & UVC_CONTROL_SET_CUR) == 0)
820                 return -EINVAL;
821
822         if (mapping->v4l2_type == V4L2_CTRL_TYPE_MENU) {
823                 if (value < 0 || value >= mapping->menu_count)
824                         return -EINVAL;
825                 value = mapping->menu_info[value].value;
826         }
827
828         if (!ctrl->loaded && (ctrl->info->size * 8) != mapping->size) {
829                 if ((ctrl->info->flags & UVC_CONTROL_GET_CUR) == 0) {
830                         memset(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
831                                 0, ctrl->info->size);
832                 } else {
833                         ret = uvc_query_ctrl(video->dev, GET_CUR,
834                                 ctrl->entity->id, video->dev->intfnum,
835                                 ctrl->info->selector,
836                                 uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
837                                 ctrl->info->size);
838                         if (ret < 0)
839                                 return ret;
840                 }
841
842                 if ((ctrl->info->flags & UVC_CONTROL_AUTO_UPDATE) == 0)
843                         ctrl->loaded = 1;
844         }
845
846         if (!ctrl->dirty) {
847                 memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_BACKUP),
848                        uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
849                        ctrl->info->size);
850         }
851
852         uvc_set_le_value(value,
853                 uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT), mapping);
854
855         ctrl->dirty = 1;
856         ctrl->modified = 1;
857         return 0;
858 }
859
860 /* --------------------------------------------------------------------------
861  * Dynamic controls
862  */
863
864 int uvc_xu_ctrl_query(struct uvc_video_device *video,
865         struct uvc_xu_control *xctrl, int set)
866 {
867         struct uvc_entity *entity;
868         struct uvc_control *ctrl = NULL;
869         unsigned int i, found = 0;
870         __u8 *data;
871         int ret;
872
873         /* Find the extension unit. */
874         list_for_each_entry(entity, &video->extensions, chain) {
875                 if (entity->id == xctrl->unit)
876                         break;
877         }
878
879         if (entity->id != xctrl->unit) {
880                 uvc_trace(UVC_TRACE_CONTROL, "Extension unit %u not found.\n",
881                         xctrl->unit);
882                 return -EINVAL;
883         }
884
885         /* Find the control. */
886         for (i = 0; i < entity->ncontrols; ++i) {
887                 ctrl = &entity->controls[i];
888                 if (ctrl->info == NULL)
889                         continue;
890
891                 if (ctrl->info->selector == xctrl->selector) {
892                         found = 1;
893                         break;
894                 }
895         }
896
897         if (!found) {
898                 uvc_trace(UVC_TRACE_CONTROL,
899                         "Control " UVC_GUID_FORMAT "/%u not found.\n",
900                         UVC_GUID_ARGS(entity->extension.guidExtensionCode),
901                         xctrl->selector);
902                 return -EINVAL;
903         }
904
905         /* Validate control data size. */
906         if (ctrl->info->size != xctrl->size)
907                 return -EINVAL;
908
909         if ((set && !(ctrl->info->flags & UVC_CONTROL_SET_CUR)) ||
910             (!set && !(ctrl->info->flags & UVC_CONTROL_GET_CUR)))
911                 return -EINVAL;
912
913         if (mutex_lock_interruptible(&video->ctrl_mutex))
914                 return -ERESTARTSYS;
915
916         memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_BACKUP),
917                uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
918                xctrl->size);
919         data = uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT);
920
921         if (set && copy_from_user(data, xctrl->data, xctrl->size)) {
922                 ret = -EFAULT;
923                 goto out;
924         }
925
926         ret = uvc_query_ctrl(video->dev, set ? SET_CUR : GET_CUR, xctrl->unit,
927                              video->dev->intfnum, xctrl->selector, data,
928                              xctrl->size);
929         if (ret < 0)
930                 goto out;
931
932         if (!set && copy_to_user(xctrl->data, data, xctrl->size)) {
933                 ret = -EFAULT;
934                 goto out;
935         }
936
937 out:
938         if (ret)
939                 memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
940                        uvc_ctrl_data(ctrl, UVC_CTRL_DATA_BACKUP),
941                        xctrl->size);
942
943         mutex_unlock(&video->ctrl_mutex);
944         return ret;
945 }
946
947 /* --------------------------------------------------------------------------
948  * Suspend/resume
949  */
950
951 /*
952  * Restore control values after resume, skipping controls that haven't been
953  * changed.
954  *
955  * TODO
956  * - Don't restore modified controls that are back to their default value.
957  * - Handle restore order (Auto-Exposure Mode should be restored before
958  *   Exposure Time).
959  */
960 int uvc_ctrl_resume_device(struct uvc_device *dev)
961 {
962         struct uvc_control *ctrl;
963         struct uvc_entity *entity;
964         unsigned int i;
965         int ret;
966
967         /* Walk the entities list and restore controls when possible. */
968         list_for_each_entry(entity, &dev->entities, list) {
969
970                 for (i = 0; i < entity->ncontrols; ++i) {
971                         ctrl = &entity->controls[i];
972
973                         if (ctrl->info == NULL || !ctrl->modified ||
974                             (ctrl->info->flags & UVC_CONTROL_RESTORE) == 0)
975                                 continue;
976
977                         printk(KERN_INFO "restoring control " UVC_GUID_FORMAT
978                                 "/%u/%u\n", UVC_GUID_ARGS(ctrl->info->entity),
979                                 ctrl->info->index, ctrl->info->selector);
980                         ctrl->dirty = 1;
981                 }
982
983                 ret = uvc_ctrl_commit_entity(dev, entity, 0);
984                 if (ret < 0)
985                         return ret;
986         }
987
988         return 0;
989 }
990
991 /* --------------------------------------------------------------------------
992  * Control and mapping handling
993  */
994
995 static void uvc_ctrl_add_ctrl(struct uvc_device *dev,
996         struct uvc_control_info *info)
997 {
998         struct uvc_entity *entity;
999         struct uvc_control *ctrl = NULL;
1000         int ret, found = 0;
1001         unsigned int i;
1002
1003         list_for_each_entry(entity, &dev->entities, list) {
1004                 if (!uvc_entity_match_guid(entity, info->entity))
1005                         continue;
1006
1007                 for (i = 0; i < entity->ncontrols; ++i) {
1008                         ctrl = &entity->controls[i];
1009                         if (ctrl->index == info->index) {
1010                                 found = 1;
1011                                 break;
1012                         }
1013                 }
1014
1015                 if (found)
1016                         break;
1017         }
1018
1019         if (!found)
1020                 return;
1021
1022         if (UVC_ENTITY_TYPE(entity) == VC_EXTENSION_UNIT) {
1023                 /* Check if the device control information and length match
1024                  * the user supplied information.
1025                  */
1026                 __u32 flags;
1027                 __le16 size;
1028                 __u8 inf;
1029
1030                 if ((ret = uvc_query_ctrl(dev, GET_LEN, ctrl->entity->id,
1031                         dev->intfnum, info->selector, (__u8 *)&size, 2)) < 0) {
1032                         uvc_trace(UVC_TRACE_CONTROL, "GET_LEN failed on "
1033                                 "control " UVC_GUID_FORMAT "/%u (%d).\n",
1034                                 UVC_GUID_ARGS(info->entity), info->selector,
1035                                 ret);
1036                         return;
1037                 }
1038
1039                 if (info->size != le16_to_cpu(size)) {
1040                         uvc_trace(UVC_TRACE_CONTROL, "Control " UVC_GUID_FORMAT
1041                                 "/%u size doesn't match user supplied "
1042                                 "value.\n", UVC_GUID_ARGS(info->entity),
1043                                 info->selector);
1044                         return;
1045                 }
1046
1047                 if ((ret = uvc_query_ctrl(dev, GET_INFO, ctrl->entity->id,
1048                         dev->intfnum, info->selector, &inf, 1)) < 0) {
1049                         uvc_trace(UVC_TRACE_CONTROL, "GET_INFO failed on "
1050                                 "control " UVC_GUID_FORMAT "/%u (%d).\n",
1051                                 UVC_GUID_ARGS(info->entity), info->selector,
1052                                 ret);
1053                         return;
1054                 }
1055
1056                 flags = info->flags;
1057                 if (((flags & UVC_CONTROL_GET_CUR) && !(inf & (1 << 0))) ||
1058                     ((flags & UVC_CONTROL_SET_CUR) && !(inf & (1 << 1)))) {
1059                         uvc_trace(UVC_TRACE_CONTROL, "Control "
1060                                 UVC_GUID_FORMAT "/%u flags don't match "
1061                                 "supported operations.\n",
1062                                 UVC_GUID_ARGS(info->entity), info->selector);
1063                         return;
1064                 }
1065         }
1066
1067         ctrl->info = info;
1068         ctrl->data = kmalloc(ctrl->info->size * UVC_CTRL_NDATA, GFP_KERNEL);
1069         uvc_trace(UVC_TRACE_CONTROL, "Added control " UVC_GUID_FORMAT "/%u "
1070                 "to device %s entity %u\n", UVC_GUID_ARGS(ctrl->info->entity),
1071                 ctrl->info->selector, dev->udev->devpath, entity->id);
1072 }
1073
1074 /*
1075  * Add an item to the UVC control information list, and instantiate a control
1076  * structure for each device that supports the control.
1077  */
1078 int uvc_ctrl_add_info(struct uvc_control_info *info)
1079 {
1080         struct uvc_control_info *ctrl;
1081         struct uvc_device *dev;
1082         int ret = 0;
1083
1084         /* Find matching controls by walking the devices, entities and
1085          * controls list.
1086          */
1087         mutex_lock(&uvc_driver.ctrl_mutex);
1088
1089         /* First check if the list contains a control matching the new one.
1090          * Bail out if it does.
1091          */
1092         list_for_each_entry(ctrl, &uvc_driver.controls, list) {
1093                 if (memcmp(ctrl->entity, info->entity, 16))
1094                         continue;
1095
1096                 if (ctrl->selector == info->selector) {
1097                         uvc_trace(UVC_TRACE_CONTROL, "Control "
1098                                 UVC_GUID_FORMAT "/%u is already defined.\n",
1099                                 UVC_GUID_ARGS(info->entity), info->selector);
1100                         ret = -EEXIST;
1101                         goto end;
1102                 }
1103                 if (ctrl->index == info->index) {
1104                         uvc_trace(UVC_TRACE_CONTROL, "Control "
1105                                 UVC_GUID_FORMAT "/%u would overwrite index "
1106                                 "%d.\n", UVC_GUID_ARGS(info->entity),
1107                                 info->selector, info->index);
1108                         ret = -EEXIST;
1109                         goto end;
1110                 }
1111         }
1112
1113         list_for_each_entry(dev, &uvc_driver.devices, list)
1114                 uvc_ctrl_add_ctrl(dev, info);
1115
1116         INIT_LIST_HEAD(&info->mappings);
1117         list_add_tail(&info->list, &uvc_driver.controls);
1118 end:
1119         mutex_unlock(&uvc_driver.ctrl_mutex);
1120         return ret;
1121 }
1122
1123 int uvc_ctrl_add_mapping(struct uvc_control_mapping *mapping)
1124 {
1125         struct uvc_control_info *info;
1126         struct uvc_control_mapping *map;
1127         int ret = -EINVAL;
1128
1129         if (mapping->id & ~V4L2_CTRL_ID_MASK) {
1130                 uvc_trace(UVC_TRACE_CONTROL, "Can't add mapping '%s' with "
1131                         "invalid control id 0x%08x\n", mapping->name,
1132                         mapping->id);
1133                 return -EINVAL;
1134         }
1135
1136         mutex_lock(&uvc_driver.ctrl_mutex);
1137         list_for_each_entry(info, &uvc_driver.controls, list) {
1138                 if (memcmp(info->entity, mapping->entity, 16) ||
1139                         info->selector != mapping->selector)
1140                         continue;
1141
1142                 if (info->size * 8 < mapping->size + mapping->offset) {
1143                         uvc_trace(UVC_TRACE_CONTROL, "Mapping '%s' would "
1144                                 "overflow control " UVC_GUID_FORMAT "/%u\n",
1145                                 mapping->name, UVC_GUID_ARGS(info->entity),
1146                                 info->selector);
1147                         ret = -EOVERFLOW;
1148                         goto end;
1149                 }
1150
1151                 /* Check if the list contains a mapping matching the new one.
1152                  * Bail out if it does.
1153                  */
1154                 list_for_each_entry(map, &info->mappings, list) {
1155                         if (map->id == mapping->id) {
1156                                 uvc_trace(UVC_TRACE_CONTROL, "Mapping '%s' is "
1157                                         "already defined.\n", mapping->name);
1158                                 ret = -EEXIST;
1159                                 goto end;
1160                         }
1161                 }
1162
1163                 mapping->ctrl = info;
1164                 list_add_tail(&mapping->list, &info->mappings);
1165                 uvc_trace(UVC_TRACE_CONTROL, "Adding mapping %s to control "
1166                         UVC_GUID_FORMAT "/%u.\n", mapping->name,
1167                         UVC_GUID_ARGS(info->entity), info->selector);
1168
1169                 ret = 0;
1170                 break;
1171         }
1172 end:
1173         mutex_unlock(&uvc_driver.ctrl_mutex);
1174         return ret;
1175 }
1176
1177 /*
1178  * Initialize device controls.
1179  */
1180 int uvc_ctrl_init_device(struct uvc_device *dev)
1181 {
1182         struct uvc_control_info *info;
1183         struct uvc_control *ctrl;
1184         struct uvc_entity *entity;
1185         unsigned int i;
1186
1187         /* Walk the entities list and instantiate controls */
1188         list_for_each_entry(entity, &dev->entities, list) {
1189                 unsigned int bControlSize = 0, ncontrols = 0;
1190                 __u8 *bmControls = NULL;
1191
1192                 if (UVC_ENTITY_TYPE(entity) == VC_EXTENSION_UNIT) {
1193                         bmControls = entity->extension.bmControls;
1194                         bControlSize = entity->extension.bControlSize;
1195                 } else if (UVC_ENTITY_TYPE(entity) == VC_PROCESSING_UNIT) {
1196                         bmControls = entity->processing.bmControls;
1197                         bControlSize = entity->processing.bControlSize;
1198                 } else if (UVC_ENTITY_TYPE(entity) == ITT_CAMERA) {
1199                         bmControls = entity->camera.bmControls;
1200                         bControlSize = entity->camera.bControlSize;
1201                 }
1202
1203                 for (i = 0; i < bControlSize; ++i)
1204                         ncontrols += hweight8(bmControls[i]);
1205
1206                 if (ncontrols == 0)
1207                         continue;
1208
1209                 entity->controls = kzalloc(ncontrols*sizeof *ctrl, GFP_KERNEL);
1210                 if (entity->controls == NULL)
1211                         return -ENOMEM;
1212
1213                 entity->ncontrols = ncontrols;
1214
1215                 ctrl = entity->controls;
1216                 for (i = 0; i < bControlSize * 8; ++i) {
1217                         if (uvc_get_bit(bmControls, i) == 0)
1218                                 continue;
1219
1220                         ctrl->entity = entity;
1221                         ctrl->index = i;
1222                         ctrl++;
1223                 }
1224         }
1225
1226         /* Walk the controls info list and associate them with the device
1227          * controls, then add the device to the global device list. This has
1228          * to be done while holding the controls lock, to make sure
1229          * uvc_ctrl_add_info() will not get called in-between.
1230          */
1231         mutex_lock(&uvc_driver.ctrl_mutex);
1232         list_for_each_entry(info, &uvc_driver.controls, list)
1233                 uvc_ctrl_add_ctrl(dev, info);
1234
1235         list_add_tail(&dev->list, &uvc_driver.devices);
1236         mutex_unlock(&uvc_driver.ctrl_mutex);
1237
1238         return 0;
1239 }
1240
1241 /*
1242  * Cleanup device controls.
1243  */
1244 void uvc_ctrl_cleanup_device(struct uvc_device *dev)
1245 {
1246         struct uvc_entity *entity;
1247         unsigned int i;
1248
1249         /* Remove the device from the global devices list */
1250         mutex_lock(&uvc_driver.ctrl_mutex);
1251         if (dev->list.next != NULL)
1252                 list_del(&dev->list);
1253         mutex_unlock(&uvc_driver.ctrl_mutex);
1254
1255         list_for_each_entry(entity, &dev->entities, list) {
1256                 for (i = 0; i < entity->ncontrols; ++i)
1257                         kfree(entity->controls[i].data);
1258
1259                 kfree(entity->controls);
1260         }
1261 }
1262
1263 void uvc_ctrl_init(void)
1264 {
1265         struct uvc_control_info *ctrl = uvc_ctrls;
1266         struct uvc_control_info *cend = ctrl + ARRAY_SIZE(uvc_ctrls);
1267         struct uvc_control_mapping *mapping = uvc_ctrl_mappings;
1268         struct uvc_control_mapping *mend =
1269                 mapping + ARRAY_SIZE(uvc_ctrl_mappings);
1270
1271         for (; ctrl < cend; ++ctrl)
1272                 uvc_ctrl_add_info(ctrl);
1273
1274         for (; mapping < mend; ++mapping)
1275                 uvc_ctrl_add_mapping(mapping);
1276 }
1277