uvc_ctrl.c

linux camera下的uvc driver驱动源码

 */

int uvc_xu_ctrl_query(struct uvc_video_device *video,
	struct uvc_xu_control *xctrl, int set)
{
	struct uvc_entity *entity;
	struct uvc_control *ctrl = NULL;
	unsigned int i, found = 0;
	__u8 *data;
	int ret;

	/* Find the extension unit. */
	list_for_each_entry(entity, &video->extensions, chain) {
		if (entity->id == xctrl->unit)
			break;
	}

	if (entity->id != xctrl->unit) {
		uvc_trace(UVC_TRACE_CONTROL, "Extension unit %u not found.\n",
			xctrl->unit);
		return -EINVAL;
	}

	/* Find the control. */
	for (i = 0; i < entity->ncontrols; ++i) {
		ctrl = &entity->controls[i];
		if (ctrl->info == NULL)
			continue;

		if (ctrl->info->selector == xctrl->selector) {
			found = 1;
			break;
		}
	}

	if (!found) {
		uvc_trace(UVC_TRACE_CONTROL,
			"Control " UVC_GUID_FORMAT "/%u not found.\n",
			UVC_GUID_ARGS(entity->extension.guidExtensionCode),
			xctrl->selector);
		return -EINVAL;
	}

	/* Validate control data size. */
	if (ctrl->info->size != xctrl->size)
		return -EINVAL;

	if ((set && !(ctrl->info->flags & UVC_CONTROL_SET_CUR)) ||
	    (!set && !(ctrl->info->flags & UVC_CONTROL_GET_CUR)))
	    	return -EINVAL;

	if (mutex_lock_interruptible(&video->ctrl_mutex))
		return -ERESTARTSYS;

	memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_BACKUP),
	       uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
	       xctrl->size);
	data = uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT);

	if (set && copy_from_user(data, xctrl->data, xctrl->size)) {
		ret = -EFAULT;
		goto out;
	}

	ret = uvc_query_ctrl(video->dev, set ? SET_CUR : GET_CUR, xctrl->unit,
			     video->dev->intfnum, xctrl->selector, data,
			     xctrl->size);
	if (ret < 0)
		goto out;

	if (!set && copy_to_user(xctrl->data, data, xctrl->size)) {
		ret = -EFAULT;
		goto out;
	}

out:
	if (ret)
		memcpy(uvc_ctrl_data(ctrl, UVC_CTRL_DATA_CURRENT),
		       uvc_ctrl_data(ctrl, UVC_CTRL_DATA_BACKUP),
		       xctrl->size);

	mutex_unlock(&video->ctrl_mutex);
	return ret;
}

/* --------------------------------------------------------------------------
 * Suspend/resume
 */

/*
 * Restore control values after resume, skipping controls that haven't been
 * changed.
 *
 * TODO
 * - Don't restore modified controls that are back to their default value.
 * - Handle restore order (Auto-Exposure Mode should be restored before
 *   Exposure Time).
 */
int uvc_ctrl_resume_device(struct uvc_device *dev)
{
	struct uvc_control *ctrl;
	struct uvc_entity *entity;
	unsigned int i;
	int ret;

	/* Walk the entities list and restore controls when possible. */
	list_for_each_entry(entity, &dev->entities, list) {

		for (i = 0; i < entity->ncontrols; ++i) {
			ctrl = &entity->controls[i];

			if (ctrl->info == NULL || !ctrl->modified ||
			    (ctrl->info->flags & UVC_CONTROL_RESTORE) == 0)
				continue;

			printk(KERN_INFO "restoring control " UVC_GUID_FORMAT
				"/%u/%u\n", UVC_GUID_ARGS(ctrl->info->entity),
				ctrl->info->index, ctrl->info->selector);
			ctrl->dirty = 1;
		}

		ret = uvc_ctrl_commit_entity(dev, entity, 0);
		if (ret < 0)
			return ret;
	}

	return 0;
}

/* --------------------------------------------------------------------------
 * Control and mapping handling
 */

static void uvc_ctrl_add_ctrl(struct uvc_device *dev,
	struct uvc_control_info *info)
{
	struct uvc_entity *entity;
	struct uvc_control *ctrl = NULL;
	int ret, found = 0;
	unsigned int i;

	list_for_each_entry(entity, &dev->entities, list) {
		if (!uvc_entity_match_guid(entity, info->entity))
			continue;

		for (i = 0; i < entity->ncontrols; ++i) {
			ctrl = &entity->controls[i];
			if (ctrl->index == info->index) {
				found = 1;
				break;
			}
		}

		if (found)
			break;
	}

	if (!found)
		return;

	if (UVC_ENTITY_TYPE(entity) == VC_EXTENSION_UNIT) {
		/* Check if the device control information and length match
		 * the user supplied information.
		 */
		__le16 size;
		__u8 inf;

		if ((ret = uvc_query_ctrl(dev, GET_LEN, ctrl->entity->id,
			dev->intfnum, info->selector, (__u8*)&size, 2)) < 0) {
			uvc_trace(UVC_TRACE_CONTROL, "GET_LEN failed on "
				"control " UVC_GUID_FORMAT "/%u (%d).\n",
				UVC_GUID_ARGS(info->entity), info->selector,
				ret);
			return;
		}

		if (info->size != le16_to_cpu(size)) {
			uvc_trace(UVC_TRACE_CONTROL, "Control "
				UVC_GUID_FORMAT "/%u size doesn't match user "
				"supplied value.\n", UVC_GUID_ARGS(info->entity),
				info->selector);
			return;
		}

		if ((ret = uvc_query_ctrl(dev, GET_INFO, ctrl->entity->id,
			dev->intfnum, info->selector, &inf, 1)) < 0) {
			uvc_trace(UVC_TRACE_CONTROL, "GET_INFO failed on "
				"control " UVC_GUID_FORMAT "/%u (%d).\n",
				UVC_GUID_ARGS(info->entity), info->selector,
				ret);
			return;
		}

		if (((info->flags & UVC_CONTROL_GET_CUR) && !(inf & (1 << 0))) ||
		    ((info->flags & UVC_CONTROL_SET_CUR) && !(inf & (1 << 1)))) {
			uvc_trace(UVC_TRACE_CONTROL, "Control "
				UVC_GUID_FORMAT "/%u flags don't match "
				"supported operations.\n",
				UVC_GUID_ARGS(info->entity), info->selector);
			return;
		}
	}

	ctrl->info = info;
	ctrl->data = kmalloc(ctrl->info->size * UVC_CTRL_NDATA, GFP_KERNEL);
	uvc_trace(UVC_TRACE_CONTROL, "Added control " UVC_GUID_FORMAT "/%u "
		"to device %s entity %u\n", UVC_GUID_ARGS(ctrl->info->entity),
		ctrl->info->selector, dev->udev->devpath, entity->id);
}

/*
 * Add an item to the UVC control information list, and instantiate a control
 * structure for each device that supports the control.
 */
int uvc_ctrl_add_info(struct uvc_control_info *info)
{
	struct uvc_control_info *ctrl;
	struct uvc_device *dev;
	int ret = 0;

	/* Find matching controls by walking the devices, entities and
	 * controls list.
	 */
	mutex_lock(&uvc_driver.ctrl_mutex);

	/* First check if the list contains a control matching the new one.
	 * Bail out if it does.
	 */
	list_for_each_entry(ctrl, &uvc_driver.controls, list) {
		if (memcmp(ctrl->entity, info->entity, 16))
			continue;

		if (ctrl->selector == info->selector) {
			uvc_trace(UVC_TRACE_CONTROL, "Control "
				UVC_GUID_FORMAT "/%u is already defined.\n",
				UVC_GUID_ARGS(info->entity), info->selector);
			ret = -EEXIST;
			goto end;
		}
		if (ctrl->index == info->index) {
			uvc_trace(UVC_TRACE_CONTROL, "Control "
				UVC_GUID_FORMAT "/%u would overwrite index "
				"%d.\n", UVC_GUID_ARGS(info->entity),
				info->selector, info->index);
			ret = -EEXIST;
			goto end;
		}
	}

	list_for_each_entry(dev, &uvc_driver.devices, list)
		uvc_ctrl_add_ctrl(dev, info);

	INIT_LIST_HEAD(&info->mappings);
	list_add_tail(&info->list, &uvc_driver.controls);
end:
	mutex_unlock(&uvc_driver.ctrl_mutex);
	return ret;
}

int uvc_ctrl_add_mapping(struct uvc_control_mapping *mapping)
{
	struct uvc_control_info *info;
	struct uvc_control_mapping *map;
	int ret = -EINVAL;

	if (mapping->id & ~V4L2_CTRL_ID_MASK) {
		uvc_trace(UVC_TRACE_CONTROL, "Can't add mapping '%s' with "
			"invalid control id 0x%08x\n", mapping->name,
			mapping->id);
		return -EINVAL;
	}

	mutex_lock(&uvc_driver.ctrl_mutex);
	list_for_each_entry(info, &uvc_driver.controls, list) {
		if (memcmp(info->entity, mapping->entity, 16) ||
			info->selector != mapping->selector)
			continue;

		if (info->size * 8 < mapping->size + mapping->offset) {
			uvc_trace(UVC_TRACE_CONTROL, "Mapping '%s' would "
				"overflow control " UVC_GUID_FORMAT "/%u\n",
				mapping->name, UVC_GUID_ARGS(info->entity),
				info->selector);
			ret = -EOVERFLOW;
			goto end;
		}

		/* Check if the list contains a mapping matching the new one.
		 * Bail out if it does.
		 */
		list_for_each_entry(map, &info->mappings, list) {
			if (map->id == mapping->id) {
				uvc_trace(UVC_TRACE_CONTROL, "Mapping '%s' is already "
					"defined.\n", mapping->name);
				ret = -EEXIST;
				goto end;
			}
		}

		mapping->ctrl = info;
		list_add_tail(&mapping->list, &info->mappings);
		uvc_trace(UVC_TRACE_CONTROL, "Adding mapping %s to control "
			UVC_GUID_FORMAT "/%u.\n", mapping->name,
			UVC_GUID_ARGS(info->entity), info->selector);

		ret = 0;
		break;
	}
end:
	mutex_unlock(&uvc_driver.ctrl_mutex);
	return ret;
}

/*
 * Initialize device controls.
 */
int uvc_ctrl_init_device(struct uvc_device *dev)
{
	struct uvc_control_info *info;
	struct uvc_control *ctrl;
	struct uvc_entity *entity;
	unsigned int i;

	/* Walk the entities list and instantiate controls */
	list_for_each_entry(entity, &dev->entities, list) {
		unsigned int bControlSize = 0, ncontrols = 0;
		__u8 *bmControls = NULL;

		if (UVC_ENTITY_TYPE(entity) == VC_EXTENSION_UNIT) {
			bmControls = entity->extension.bmControls;
			bControlSize = entity->extension.bControlSize;
		} else if (UVC_ENTITY_TYPE(entity) == VC_PROCESSING_UNIT) {
			bmControls = entity->processing.bmControls;
			bControlSize = entity->processing.bControlSize;
		} else if (UVC_ENTITY_TYPE(entity) == ITT_CAMERA) {
			bmControls = entity->camera.bmControls;
			bControlSize = entity->camera.bControlSize;
		}

		for (i = 0; i < bControlSize; ++i)
			ncontrols += hweight8(bmControls[i]);

		if (ncontrols == 0)
			continue;

		entity->controls = kzalloc(ncontrols*sizeof *ctrl, GFP_KERNEL);
		if (entity->controls == NULL)
			return -ENOMEM;

		entity->ncontrols = ncontrols;

		ctrl = entity->controls;
		for (i = 0; i < bControlSize * 8; ++i) {
			if (uvc_get_bit(bmControls, i) == 0)
				continue;

			ctrl->entity = entity;
			ctrl->index = i;
			ctrl++;
		}
	}

	/* Walk the controls info list and associate them with the device
	 * controls, then add the device to the global device list. This has
	 * to be done while holding the controls lock, to make sure
	 * uvc_ctrl_add_info() will not get called in-between.
	 */
	mutex_lock(&uvc_driver.ctrl_mutex);
	list_for_each_entry(info, &uvc_driver.controls, list)
		uvc_ctrl_add_ctrl(dev, info);

	list_add_tail(&dev->list, &uvc_driver.devices);
	mutex_unlock(&uvc_driver.ctrl_mutex);

	return 0;
}

/*
 * Cleanup device controls.
 */
void uvc_ctrl_cleanup_device(struct uvc_device *dev)
{
	struct uvc_entity *entity;
	unsigned int i;

	/* Remove the device from the global devices list */
	mutex_lock(&uvc_driver.ctrl_mutex);
	if (dev->list.next != NULL)
		list_del(&dev->list);
	mutex_unlock(&uvc_driver.ctrl_mutex);

	list_for_each_entry(entity, &dev->entities, list) {
		for (i = 0; i < entity->ncontrols; ++i)
			kfree(entity->controls[i].data);

		kfree(entity->controls);
	}
}

void uvc_ctrl_init(void)
{
	struct uvc_control_info *ctrl = uvc_ctrls;
	struct uvc_control_info *cend = ctrl + ARRAY_SIZE(uvc_ctrls);
	struct uvc_control_mapping *mapping = uvc_ctrl_mappings;
	struct uvc_control_mapping *mend = mapping + ARRAY_SIZE(uvc_ctrl_mappings);

	for (; ctrl < cend; ++ctrl)
		uvc_ctrl_add_info(ctrl);

	for (; mapping < mend; ++mapping)
		uvc_ctrl_add_mapping(mapping);
}