zc0301_core.c

linux内核源码

		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_g_input(struct zc0301_device* cam, void __user * arg)
{
	int index = 0;

	if (copy_to_user(arg, &index, sizeof(index)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_s_input(struct zc0301_device* cam, void __user * arg)
{
	int index;

	if (copy_from_user(&index, arg, sizeof(index)))
		return -EFAULT;

	if (index != 0)
		return -EINVAL;

	return 0;
}


static int
zc0301_vidioc_query_ctrl(struct zc0301_device* cam, void __user * arg)
{
	struct zc0301_sensor* s = &cam->sensor;
	struct v4l2_queryctrl qc;
	u8 i;

	if (copy_from_user(&qc, arg, sizeof(qc)))
		return -EFAULT;

	for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
		if (qc.id && qc.id == s->qctrl[i].id) {
			memcpy(&qc, &(s->qctrl[i]), sizeof(qc));
			if (copy_to_user(arg, &qc, sizeof(qc)))
				return -EFAULT;
			return 0;
		}

	return -EINVAL;
}


static int
zc0301_vidioc_g_ctrl(struct zc0301_device* cam, void __user * arg)
{
	struct zc0301_sensor* s = &cam->sensor;
	struct v4l2_control ctrl;
	int err = 0;
	u8 i;

	if (!s->get_ctrl && !s->set_ctrl)
		return -EINVAL;

	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
		return -EFAULT;

	if (!s->get_ctrl) {
		for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
			if (ctrl.id == s->qctrl[i].id) {
				ctrl.value = s->_qctrl[i].default_value;
				goto exit;
			}
		return -EINVAL;
	} else
		err = s->get_ctrl(cam, &ctrl);

exit:
	if (copy_to_user(arg, &ctrl, sizeof(ctrl)))
		return -EFAULT;

	return err;
}


static int
zc0301_vidioc_s_ctrl(struct zc0301_device* cam, void __user * arg)
{
	struct zc0301_sensor* s = &cam->sensor;
	struct v4l2_control ctrl;
	u8 i;
	int err = 0;

	if (!s->set_ctrl)
		return -EINVAL;

	if (copy_from_user(&ctrl, arg, sizeof(ctrl)))
		return -EFAULT;

	for (i = 0; i < ARRAY_SIZE(s->qctrl); i++)
		if (ctrl.id == s->qctrl[i].id) {
			if (s->qctrl[i].flags & V4L2_CTRL_FLAG_DISABLED)
				return -EINVAL;
			if (ctrl.value < s->qctrl[i].minimum ||
			    ctrl.value > s->qctrl[i].maximum)
				return -ERANGE;
			ctrl.value -= ctrl.value % s->qctrl[i].step;
			break;
		}

	if ((err = s->set_ctrl(cam, &ctrl)))
		return err;

	s->_qctrl[i].default_value = ctrl.value;

	return 0;
}


static int
zc0301_vidioc_cropcap(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_cropcap* cc = &(cam->sensor.cropcap);

	cc->type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	cc->pixelaspect.numerator = 1;
	cc->pixelaspect.denominator = 1;

	if (copy_to_user(arg, cc, sizeof(*cc)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_g_crop(struct zc0301_device* cam, void __user * arg)
{
	struct zc0301_sensor* s = &cam->sensor;
	struct v4l2_crop crop = {
		.type = V4L2_BUF_TYPE_VIDEO_CAPTURE,
	};

	memcpy(&(crop.c), &(s->_rect), sizeof(struct v4l2_rect));

	if (copy_to_user(arg, &crop, sizeof(crop)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_s_crop(struct zc0301_device* cam, void __user * arg)
{
	struct zc0301_sensor* s = &cam->sensor;
	struct v4l2_crop crop;
	struct v4l2_rect* rect;
	struct v4l2_rect* bounds = &(s->cropcap.bounds);
	const enum zc0301_stream_state stream = cam->stream;
	const u32 nbuffers = cam->nbuffers;
	u32 i;
	int err = 0;

	if (copy_from_user(&crop, arg, sizeof(crop)))
		return -EFAULT;

	rect = &(crop.c);

	if (crop.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
		return -EINVAL;

	if (cam->module_param.force_munmap)
		for (i = 0; i < cam->nbuffers; i++)
			if (cam->frame[i].vma_use_count) {
				DBG(3, "VIDIOC_S_CROP failed. "
				       "Unmap the buffers first.");
				return -EBUSY;
			}

	if (!s->set_crop) {
		memcpy(rect, &(s->_rect), sizeof(*rect));
		if (copy_to_user(arg, &crop, sizeof(crop)))
			return -EFAULT;
		return 0;
	}

	rect->left &= ~7L;
	rect->top &= ~7L;
	if (rect->width < 8)
		rect->width = 8;
	if (rect->height < 8)
		rect->height = 8;
	if (rect->width > bounds->width)
		rect->width = bounds->width;
	if (rect->height > bounds->height)
		rect->height = bounds->height;
	if (rect->left < bounds->left)
		rect->left = bounds->left;
	if (rect->top < bounds->top)
		rect->top = bounds->top;
	if (rect->left + rect->width > bounds->left + bounds->width)
		rect->left = bounds->left+bounds->width - rect->width;
	if (rect->top + rect->height > bounds->top + bounds->height)
		rect->top = bounds->top+bounds->height - rect->height;
	rect->width &= ~7L;
	rect->height &= ~7L;

	if (cam->stream == STREAM_ON)
		if ((err = zc0301_stream_interrupt(cam)))
			return err;

	if (copy_to_user(arg, &crop, sizeof(crop))) {
		cam->stream = stream;
		return -EFAULT;
	}

	if (cam->module_param.force_munmap || cam->io == IO_READ)
		zc0301_release_buffers(cam);

	if (s->set_crop)
		err += s->set_crop(cam, rect);

	if (err) { /* atomic, no rollback in ioctl() */
		cam->state |= DEV_MISCONFIGURED;
		DBG(1, "VIDIOC_S_CROP failed because of hardware problems. To "
		       "use the camera, close and open /dev/video%d again.",
		    cam->v4ldev->minor);
		return -EIO;
	}

	s->pix_format.width = rect->width;
	s->pix_format.height = rect->height;
	memcpy(&(s->_rect), rect, sizeof(*rect));

	if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
	    nbuffers != zc0301_request_buffers(cam, nbuffers, cam->io)) {
		cam->state |= DEV_MISCONFIGURED;
		DBG(1, "VIDIOC_S_CROP failed because of not enough memory. To "
		       "use the camera, close and open /dev/video%d again.",
		    cam->v4ldev->minor);
		return -ENOMEM;
	}

	if (cam->io == IO_READ)
		zc0301_empty_framequeues(cam);
	else if (cam->module_param.force_munmap)
		zc0301_requeue_outqueue(cam);

	cam->stream = stream;

	return 0;
}


static int
zc0301_vidioc_enum_framesizes(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_frmsizeenum frmsize;

	if (copy_from_user(&frmsize, arg, sizeof(frmsize)))
		return -EFAULT;

	if (frmsize.index != 0 && frmsize.index != 1)
		return -EINVAL;

	if (frmsize.pixel_format != V4L2_PIX_FMT_JPEG)
		return -EINVAL;

	frmsize.type = V4L2_FRMSIZE_TYPE_DISCRETE;

	if (frmsize.index == 1) {
		frmsize.discrete.width = cam->sensor.cropcap.defrect.width;
		frmsize.discrete.height = cam->sensor.cropcap.defrect.height;
	}
	memset(&frmsize.reserved, 0, sizeof(frmsize.reserved));

	if (copy_to_user(arg, &frmsize, sizeof(frmsize)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_enum_fmt(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_fmtdesc fmtd;

	if (copy_from_user(&fmtd, arg, sizeof(fmtd)))
		return -EFAULT;

	if (fmtd.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
		return -EINVAL;

	if (fmtd.index == 0) {
		strcpy(fmtd.description, "JPEG");
		fmtd.pixelformat = V4L2_PIX_FMT_JPEG;
		fmtd.flags = V4L2_FMT_FLAG_COMPRESSED;
	} else
		return -EINVAL;

	fmtd.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
	memset(&fmtd.reserved, 0, sizeof(fmtd.reserved));

	if (copy_to_user(arg, &fmtd, sizeof(fmtd)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_g_fmt(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_format format;
	struct v4l2_pix_format* pfmt = &(cam->sensor.pix_format);

	if (copy_from_user(&format, arg, sizeof(format)))
		return -EFAULT;

	if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
		return -EINVAL;

	pfmt->bytesperline = 0;
	pfmt->sizeimage = pfmt->height * ((pfmt->width*pfmt->priv)/8);
	pfmt->field = V4L2_FIELD_NONE;
	memcpy(&(format.fmt.pix), pfmt, sizeof(*pfmt));

	if (copy_to_user(arg, &format, sizeof(format)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_try_s_fmt(struct zc0301_device* cam, unsigned int cmd,
			void __user * arg)
{
	struct zc0301_sensor* s = &cam->sensor;
	struct v4l2_format format;
	struct v4l2_pix_format* pix;
	struct v4l2_pix_format* pfmt = &(s->pix_format);
	struct v4l2_rect* bounds = &(s->cropcap.bounds);
	struct v4l2_rect rect;
	const enum zc0301_stream_state stream = cam->stream;
	const u32 nbuffers = cam->nbuffers;
	u32 i;
	int err = 0;

	if (copy_from_user(&format, arg, sizeof(format)))
		return -EFAULT;

	pix = &(format.fmt.pix);

	if (format.type != V4L2_BUF_TYPE_VIDEO_CAPTURE)
		return -EINVAL;

	memcpy(&rect, &(s->_rect), sizeof(rect));

	if (!s->set_crop) {
		pix->width = rect.width;
		pix->height = rect.height;
	} else {
		rect.width = pix->width;
		rect.height = pix->height;
	}

	if (rect.width < 8)
		rect.width = 8;
	if (rect.height < 8)
		rect.height = 8;
	if (rect.width > bounds->left + bounds->width - rect.left)
		rect.width = bounds->left + bounds->width - rect.left;
	if (rect.height > bounds->top + bounds->height - rect.top)
		rect.height = bounds->top + bounds->height - rect.top;
	rect.width &= ~7L;
	rect.height &= ~7L;

	pix->width = rect.width;
	pix->height = rect.height;
	pix->pixelformat = pfmt->pixelformat;
	pix->priv = pfmt->priv;
	pix->colorspace = pfmt->colorspace;
	pix->bytesperline = 0;
	pix->sizeimage = pix->height * ((pix->width * pix->priv) / 8);
	pix->field = V4L2_FIELD_NONE;

	if (cmd == VIDIOC_TRY_FMT) {
		if (copy_to_user(arg, &format, sizeof(format)))
			return -EFAULT;
		return 0;
	}

	if (cam->module_param.force_munmap)
		for (i = 0; i < cam->nbuffers; i++)
			if (cam->frame[i].vma_use_count) {
				DBG(3, "VIDIOC_S_FMT failed. "
				       "Unmap the buffers first.");
				return -EBUSY;
			}

	if (cam->stream == STREAM_ON)
		if ((err = zc0301_stream_interrupt(cam)))
			return err;

	if (copy_to_user(arg, &format, sizeof(format))) {
		cam->stream = stream;
		return -EFAULT;
	}

	if (cam->module_param.force_munmap || cam->io == IO_READ)
		zc0301_release_buffers(cam);

	if (s->set_crop)
		err += s->set_crop(cam, &rect);

	if (err) { /* atomic, no rollback in ioctl() */
		cam->state |= DEV_MISCONFIGURED;
		DBG(1, "VIDIOC_S_FMT failed because of hardware problems. To "
		       "use the camera, close and open /dev/video%d again.",
		    cam->v4ldev->minor);
		return -EIO;
	}

	memcpy(pfmt, pix, sizeof(*pix));
	memcpy(&(s->_rect), &rect, sizeof(rect));

	if ((cam->module_param.force_munmap  || cam->io == IO_READ) &&
	    nbuffers != zc0301_request_buffers(cam, nbuffers, cam->io)) {
		cam->state |= DEV_MISCONFIGURED;
		DBG(1, "VIDIOC_S_FMT failed because of not enough memory. To "
		       "use the camera, close and open /dev/video%d again.",
		    cam->v4ldev->minor);
		return -ENOMEM;
	}

	if (cam->io == IO_READ)
		zc0301_empty_framequeues(cam);
	else if (cam->module_param.force_munmap)
		zc0301_requeue_outqueue(cam);

	cam->stream = stream;

	return 0;
}


static int
zc0301_vidioc_g_jpegcomp(struct zc0301_device* cam, void __user * arg)
{
	if (copy_to_user(arg, &cam->compression, sizeof(cam->compression)))
		return -EFAULT;

	return 0;
}


static int
zc0301_vidioc_s_jpegcomp(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_jpegcompression jc;
	const enum zc0301_stream_state stream = cam->stream;
	int err = 0;

	if (copy_from_user(&jc, arg, sizeof(jc)))
		return -EFAULT;

	if (jc.quality != 0)
		return -EINVAL;

	if (cam->stream == STREAM_ON)
		if ((err = zc0301_stream_interrupt(cam)))
			return err;

	err += zc0301_set_compression(cam, &jc);
	if (err) { /* atomic, no rollback in ioctl() */
		cam->state |= DEV_MISCONFIGURED;
		DBG(1, "VIDIOC_S_JPEGCOMP failed because of hardware "
		       "problems. To use the camera, close and open "
		       "/dev/video%d again.", cam->v4ldev->minor);
		return -EIO;
	}

	cam->compression.quality = jc.quality;

	cam->stream = stream;

	return 0;
}


static int
zc0301_vidioc_reqbufs(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_requestbuffers rb;
	u32 i;
	int err;

	if (copy_from_user(&rb, arg, sizeof(rb)))
		return -EFAULT;

	if (rb.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
	    rb.memory != V4L2_MEMORY_MMAP)
		return -EINVAL;

	if (cam->io == IO_READ) {
		DBG(3, "Close and open the device again to choose the mmap "
		       "I/O method");
		return -EBUSY;
	}

	for (i = 0; i < cam->nbuffers; i++)
		if (cam->frame[i].vma_use_count) {
			DBG(3, "VIDIOC_REQBUFS failed. "
			       "Previous buffers are still mapped.");
			return -EBUSY;
		}

	if (cam->stream == STREAM_ON)
		if ((err = zc0301_stream_interrupt(cam)))