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