uvcvideo.c

linux video4Linux 驱动

static void uvc_uninit_isoc(struct uvc_video_device *video)
{
	struct urb *urb;
	unsigned int i;

	for (i = 0; i < UVC_URBS; ++i) {
		if ((urb = video->urb[i]) == NULL)
			continue;

		usb_kill_urb(urb);
		/* urb->transfer_buffer_length is not touched by USB core, so
		 * we can use it here as the buffer length.
		 */
		if (video->iso_buffer[i]) {
			usb_buffer_free(video->dev->udev,
				urb->transfer_buffer_length,
				video->iso_buffer[i], urb->transfer_dma);
			video->iso_buffer[i] = NULL;
		}

		usb_free_urb(urb);
		video->urb[i] = NULL;
	}
}

/*
 * Initialize isochronous URBs and allocate transfer buffers. The packet size
 * is given by the endpoint.
 */
static int uvc_init_isoc(struct uvc_video_device *video, struct usb_host_endpoint *ep)
{
	struct urb *urb;
	unsigned int npackets, i, j;
	__u16 psize;
	__u32 size;
	int ret;

	psize = le16_to_cpu(ep->desc.wMaxPacketSize);
	psize = (psize & 0x07ff) * (1 + ((psize >> 11) & 3));

	size = video->streaming->ctrl.dwMaxVideoFrameSize;
	if (size > UVC_MAX_FRAME_SIZE)
		return -EINVAL;

	npackets = (size + psize - 1) / psize;
	if (npackets > UVC_MAX_ISO_PACKETS)
		npackets = UVC_MAX_ISO_PACKETS;

	size = npackets * psize;

	for (i = 0; i < UVC_URBS; ++i) {
		urb = usb_alloc_urb(npackets, GFP_KERNEL);
		if (urb == NULL) {
			uvc_uninit_isoc(video);
			return -ENOMEM;
		}

		video->iso_buffer[i] = usb_buffer_alloc(video->dev->udev,
			size, GFP_KERNEL, &urb->transfer_dma);
		if (video->iso_buffer[i] == NULL) {
			usb_free_urb(urb);
			uvc_uninit_isoc(video);
			return -ENOMEM;
		}

		urb->dev = video->dev->udev;
		urb->context = video;
		urb->pipe = usb_rcvisocpipe(video->dev->udev,
				ep->desc.bEndpointAddress);
		urb->transfer_flags = URB_ISO_ASAP | URB_NO_TRANSFER_DMA_MAP;
		urb->interval = ep->desc.bInterval;
		urb->transfer_buffer = video->iso_buffer[i];
		urb->complete = uvc_video_complete;
		urb->number_of_packets = npackets;
		urb->transfer_buffer_length = size;

		for (j = 0; j < npackets; ++j) {
			urb->iso_frame_desc[j].offset = j * psize;
			urb->iso_frame_desc[j].length = psize;
		}

		video->urb[i] = urb;
	}

	/* Submit the URBs. */
	for (i = 0; i < UVC_URBS; ++i) {
		if ((ret = usb_submit_urb(video->urb[i], GFP_KERNEL)) < 0) {
			uvc_printk(KERN_ERR, "Failed to submit isoc URB %u "
					"(%d).\n", i, ret);
			uvc_uninit_isoc(video);
			return ret;
		}
	}

	return 0;
}

/* ------------------------------------------------------------------------
 * UVC Controls
 */

static int uvc_query_ctrl(struct uvc_device *dev, __u8 query, __u8 unit,
			  __u8 intfnum, __u8 cs, void *data, __u16 size)
{
	__u8 type = USB_TYPE_CLASS | USB_RECIP_INTERFACE;
	unsigned int pipe;
	int ret;

	pipe = (query & 0x80) ? usb_rcvctrlpipe(dev->udev, 0)
			      : usb_sndctrlpipe(dev->udev, 0);
	type |= (query & 0x80) ? USB_DIR_IN : USB_DIR_OUT;

	ret = usb_control_msg(dev->udev, pipe, query, type, cs << 8,
			unit << 8 | intfnum, data, size, UVC_CTRL_TIMEOUT);

	if (ret != size) {
		uvc_printk(KERN_ERR, "Failed to query (%u) UVC control %u "
			"(unit %u) : %d.\n", query, cs, unit, ret);
		return -EIO;
	}

	return 0;
}

static struct uvc_control_desc *uvc_find_control(struct uvc_video_device *video,
	__u32 v4l2_id, __u8 *unit_id)
{
	struct uvc_unit *unit = NULL;
	struct uvc_control_desc *ctrl = uvc_ctrls;
	struct uvc_control_desc *end = ctrl + ARRAY_SIZE(uvc_ctrls);
	__u16 size;
	__u8 *bitmask;
	__u8 id = 0;

	/* Find the control. */
	for (; ctrl < end; ++ctrl) {
		if (ctrl->v4l2 != v4l2_id)
			continue;

		if (memcmp(ctrl->unit, uvc_processing_guid, 16) == 0)
		{
			if (video->processing == NULL)
				continue;

			bitmask = video->processing->processing.bmControls;
			size = video->processing->processing.bControlSize << 3;
			id = video->processing->id;
		}
		else if (memcmp(ctrl->unit, uvc_camera_guid, 16) == 0)
		{
			if (video->iterm->type != ITT_CAMERA)
				continue;

			bitmask = video->iterm->camera.bmControls;
			size = video->iterm->camera.bControlSize << 3;
			id = video->iterm->id;
		}
		else
		{
			unit = uvc_unit_by_guid(video->dev, ctrl->unit);
			if (unit == NULL)
				continue;

			bitmask = unit->extension.bmControls;
			size = unit->extension.bControlSize << 3;
			id = unit->id;
		}

		if (ctrl->bit < size && uvc_get_bit(bitmask, ctrl->bit))
			break;
	}

	if (ctrl == end) {
		uvc_trace(UVC_TRACE_CONTROL, "Control 0x%08x not found.\n",
			v4l2_id);
		return NULL;
	}

	if (unit_id != NULL)
		*unit_id = id;

	return ctrl;
}

static int uvc_get_v4l2_ctrl(struct uvc_video_device *video, struct v4l2_control *v4l2_ctrl)
{
	struct uvc_control_desc *ctrl;
	__u8 unit_id;
	__u8 data[4];
	int ret;

	ctrl = uvc_find_control(video, v4l2_ctrl->id, &unit_id);
	if (ctrl == NULL)
		return -EINVAL;

	ret = uvc_query_ctrl(video->dev, GET_CUR, unit_id, video->dev->intfnum,
				ctrl->id, &data, ctrl->size);
	if (ret < 0)
		return ret;

	v4l2_ctrl->value = uvc_get_le_value(data, ctrl->size);
	return 0;
}

static int uvc_set_v4l2_ctrl(struct uvc_video_device *video, struct v4l2_control *v4l2_ctrl)
{
	struct uvc_control_desc *ctrl;
	__u8 unit_id;
	__u8 data[4];

	ctrl = uvc_find_control(video, v4l2_ctrl->id, &unit_id);
	if (ctrl == NULL)
		return -EINVAL;

	uvc_set_le_value(v4l2_ctrl->value, data, ctrl->size);

	return uvc_query_ctrl(video->dev, SET_CUR, unit_id, video->dev->intfnum,
				ctrl->id, &data, ctrl->size);
}

static int uvc_query_v4l2_ctrl(struct uvc_video_device *video, struct v4l2_queryctrl *v4l2_ctrl)
{
	struct uvc_control_desc *ctrl;
	__u8 unit_id;
	__u8 data[4];
	int ret = 0;

	ctrl = uvc_find_control(video, v4l2_ctrl->id, &unit_id);
	if (ctrl == NULL) {
		/* If the V4L2 control ID is in the private control ID range,
		 * there's a chance the application is enumerating our private
		 * controls, so we can't return EINVAL because (according to the
		 * V4L2 spec) it will think that this was the last one. However,
		 * it might just be this particular control that is not
		 * supported and we want the enumeration to continue.
		 */
		if (v4l2_ctrl->id < V4L2_CID_PRIVATE_BASE ||
		    v4l2_ctrl->id > V4L2_CID_PRIVATE_LAST) {
			return -EINVAL;
		} else {
			v4l2_ctrl->name[0] = '\0';
			v4l2_ctrl->flags = V4L2_CTRL_FLAG_DISABLED;
			return 0;
		}
	}

	if (ctrl->flags & UVC_CONTROL_GET_DEF) {
		ret |= uvc_query_ctrl(video->dev, GET_DEF, unit_id,
					video->dev->intfnum, ctrl->id, &data,
					ctrl->size);
		v4l2_ctrl->default_value = uvc_get_le_value(data, ctrl->size);
	}
	if (ctrl->flags & UVC_CONTROL_GET_MIN) {
		ret |= uvc_query_ctrl(video->dev, GET_MIN, unit_id,
					video->dev->intfnum, ctrl->id, &data,
					ctrl->size);
		v4l2_ctrl->minimum = uvc_get_le_value(data, ctrl->size);
	}
	if (ctrl->flags & UVC_CONTROL_GET_MAX) {
		ret |= uvc_query_ctrl(video->dev, GET_MAX, unit_id,
					video->dev->intfnum, ctrl->id, &data,
					ctrl->size);
		v4l2_ctrl->maximum = uvc_get_le_value(data, ctrl->size);
	}
	if (ctrl->flags & UVC_CONTROL_GET_RES) {
		ret |= uvc_query_ctrl(video->dev, GET_RES, unit_id,
					video->dev->intfnum, ctrl->id, &data,
					ctrl->size);
		v4l2_ctrl->step = uvc_get_le_value(data, ctrl->size);
	}

	if (ret != 0)
		return -EIO;

	v4l2_ctrl->type = ctrl->type;
	strncpy(v4l2_ctrl->name, ctrl->name, sizeof v4l2_ctrl->name);
	v4l2_ctrl->flags = 0;

	return 0;
}

static int uvc_get_video_ctrl(struct uvc_video_device *video,
	struct uvc_streaming_control *ctrl, int probe, __u8 query)
{
	__u8 data[34];
	__u8 size;
	int ret;

	size = video->dev->uvc_version > 0x0100 ? 34 : 26;
	ret = uvc_query_ctrl(video->dev, query, 0, video->streaming->intfnum,
		probe ? VS_PROBE_CONTROL : VS_COMMIT_CONTROL, &data, size);

	if (ret < 0)
		return ret;

	ctrl->bmHint = le16_to_cpup((__le16*)&data[0]);
	ctrl->bFormatIndex = data[2];
	ctrl->bFrameIndex = data[3];
	ctrl->dwFrameInterval = le32_to_cpup((__le32*)&data[4]);
	ctrl->wKeyFrameRate = le16_to_cpup((__le16*)&data[8]);
	ctrl->wPFrameRate = le16_to_cpup((__le16*)&data[10]);
	ctrl->wCompQuality = le16_to_cpup((__le16*)&data[12]);
	ctrl->wCompWindowSize = le16_to_cpup((__le16*)&data[14]);
	ctrl->wDelay = le16_to_cpup((__le16*)&data[16]);
	ctrl->dwMaxVideoFrameSize = le32_to_cpup((__le32*)&data[18]);
	ctrl->dwMaxPayloadTransferSize = le32_to_cpup((__le32*)&data[22]);

	if (size == 34) {
		ctrl->dwClockFrequency = le32_to_cpup((__le32*)&data[26]);
		ctrl->bmFramingInfo = data[30];
		ctrl->bPreferedVersion = data[31];
		ctrl->bMinVersion = data[32];
		ctrl->bMaxVersion = data[33];
	}
	else {
		ctrl->dwClockFrequency = video->dev->clock_frequency;
		ctrl->bmFramingInfo = 0;
		ctrl->bPreferedVersion = 0;
		ctrl->bMinVersion = 0;
		ctrl->bMaxVersion = 0;
	}

	if (ctrl->dwMaxVideoFrameSize == 0 && video->dev->uvc_version <= 0x0100) {
		/* Some broken UVC 1.0 devices return a null
		 * dwMaxVideoFrameSize. Try to get the value from the format
		 * and frame descriptor.
		 */
		struct uvc_format *format = NULL;
		struct uvc_frame *frame = NULL;

		if (ctrl->bFormatIndex <= video->streaming->nformats)
			format = &video->streaming->format[ctrl->bFormatIndex - 1];
		if (format && ctrl->bFrameIndex <= format->nframes) {
			frame = &format->frame[ctrl->bFrameIndex - 1];
			ctrl->dwMaxVideoFrameSize = frame->dwMaxVideoFrameBufferSize;
		}
	}

	return 0;
}

static int uvc_set_video_ctrl(struct uvc_video_device *video,
	struct uvc_streaming_control *ctrl, int probe)
{
	__u8 data[34];
	__u8 size;

	size = video->dev->uvc_version > 0x0100 ? 34 : 26;
	memset(data, 0, sizeof data);

	*(__le16*)&data[0] = cpu_to_le16(ctrl->bmHint);
	data[2] = ctrl->bFormatIndex;
	data[3] = ctrl->bFrameIndex;
	*(__le32*)&data[4] = cpu_to_le32(ctrl->dwFrameInterval);
	*(__le16*)&data[8] = cpu_to_le16(ctrl->wKeyFrameRate);
	*(__le16*)&data[10] = cpu_to_le16(ctrl->wPFrameRate);
	*(__le16*)&data[12] = cpu_to_le16(ctrl->wCompQuality);
	*(__le16*)&data[14] = cpu_to_le16(ctrl->wCompWindowSize);
	*(__le16*)&data[16] = cpu_to_le16(ctrl->wDelay);
	/* Note: Some of the fields below are not required for IN devices (see
	 * UVC spec, 4.3.1.1), but we still copy them in case support for OUT
	 * devices is added in the future. */
	*(__le32*)&data[18] = cpu_to_le32(ctrl->dwMaxVideoFrameSize);
	*(__le32*)&data[22] = cpu_to_le32(ctrl->dwMaxPayloadTransferSize);

	if (size == 34) {
		*(__le32*)&data[26] = cpu_to_le32(ctrl->dwClockFrequency);
		data[30] = ctrl->bmFramingInfo;
		data[31] = ctrl->bPreferedVersion;
		data[32] = ctrl->bMinVersion;
		data[33] = ctrl->bMaxVersion;
	}

	return uvc_query_ctrl(video->dev, SET_CUR, 0,
		video->streaming->intfnum,
		probe ? VS_PROBE_CONTROL : VS_COMMIT_CONTROL, &data, size);
}

/*
 * Find the frame interval closest to the requested frame interval for the
 * given frame format and size. This should be done by the device as part of
 * the Video Probe and Commit negotiation, but some hardware don't implement
 * that feature.
 */
static __u32 uvc_try_frame_interval(struct uvc_frame *frame, __u32 interval)
{
	unsigned int i;

	if (frame->bFrameIntervalType) {
		__u32 best = -1, dist;

		for (i = 0; i < frame->bFrameIntervalType; ++i) {
			dist = interval > frame->dwFrameInterval[i]
			     ? interval - frame->dwFrameInterval[i]
			     : frame->dwFrameInterval[i] - interval;

			if (dist > best)
				break;

			best = dist;
		}

		interval = frame->dwFrameInterval[i-1];
	}
	else {
		const __u32 min = frame->dwFrameInterval[0];
		const __u32 max = frame->dwFrameInterval[1];
		const __u32 step = frame->dwFrameInterval[2];

		interval = min + (interval - min + step/2) / step * step;
		if (interval > max)
			interval = max;
	}

	return interval;
}

static int uvc_probe_video(struct uvc_video_device *video, struct uvc_streaming_control *probe)
{
	struct uvc_streaming_control probe_min, probe_max;
	__u16 bandwidth;
	unsigned int i;
	int ret;

	down(&video->streaming->lock);

	/* Perform probing. The device should adjust the requested values
	 * according to its capabilities. However, some devices, namely the
	 * first generation UVC Logitech webcams, don't implement the Video
	 * Probe control properly, and just return the needed bandwidth. For
	 * that reason, if the needed bandwidth exceeds the maximum available
	 * bandwidth, try to lower the quality.
	 */
	if ((ret = uvc_set_video_ctrl(video, probe, 1)) < 0)
		goto done;

	/* Get the minimum and maximum values for compression settings. */
	if ((ret = uvc_get_video_ctrl(video, &probe_min, 1, GET_MIN)) < 0 ||
	    (ret = uvc_get_video_ctrl(video, &probe_max, 1, GET_MAX)) < 0)
		goto done;

	probe->wCompQuality = probe_max.wCompQuality;

	for (i = 0; i < 2; ++i) {
		if ((ret = uvc_set_video_ctrl(video, probe, 1)) < 0 ||
		    (ret = uvc_get_video_ctrl(video, probe, 1, GET_CUR)) < 0)
			goto done;

		bandwidth = probe->dwMaxPayloadTransferSize;
		if (bandwidth <= video->streaming->maxpsize)
			break;

		/* TODO: negotiate compression parameters */
		probe->wKeyFrameRate = probe_min.wKeyFrameRate;
		probe->wPFrameRate = probe_min.wPFrameRate;
		probe->wCompQuality = probe_max.wCompQuality;
		probe->wCompWindowSize = probe_min.wCompWindowSize;
	}

done:
	up(&video->streaming->lock);
	return ret;
}

/* ------------------------------------------------------------------------
 * Video device
 */

/*
 * Initialize the UVC video device.
 *
 * This function is called before registering the device with V4L.
 */
static int uvc_video_init(struct uvc_video_device *video)
{
	struct uvc_streaming_control *probe = &video->streaming->ctrl;
	struct uvc_format *format = &video->streaming->format[0];
	struct uvc_frame *frame = &format->frame[0];
	int ret;

	if (format->nframes == 0) {
		uvc_printk(KERN_WARNING, "Device initialization failed: the "
			"default format has no frame descriptors.\n");
		return -EINVAL;
	}

	/* Set the format index, frame index and frame interval. */
	memset(probe, 0, sizeof *probe);
	probe->bmHint = 0;
	probe->bFormatIndex = format->index;
	probe->bFrameIndex = frame->bFrameIndex;
	probe->dwFrameInterval = frame->dwDefaultFrameInterval;

	if ((ret = uvc_set_video_ctrl(video, probe, 1)) < 0)