zc0301_core.c

linux内核源码

			return err;

	zc0301_empty_framequeues(cam);

	zc0301_release_buffers(cam);
	if (rb.count)
		rb.count = zc0301_request_buffers(cam, rb.count, IO_MMAP);

	if (copy_to_user(arg, &rb, sizeof(rb))) {
		zc0301_release_buffers(cam);
		cam->io = IO_NONE;
		return -EFAULT;
	}

	cam->io = rb.count ? IO_MMAP : IO_NONE;

	return 0;
}


static int
zc0301_vidioc_querybuf(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_buffer b;

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

	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
	    b.index >= cam->nbuffers || cam->io != IO_MMAP)
		return -EINVAL;

	memcpy(&b, &cam->frame[b.index].buf, sizeof(b));

	if (cam->frame[b.index].vma_use_count)
		b.flags |= V4L2_BUF_FLAG_MAPPED;

	if (cam->frame[b.index].state == F_DONE)
		b.flags |= V4L2_BUF_FLAG_DONE;
	else if (cam->frame[b.index].state != F_UNUSED)
		b.flags |= V4L2_BUF_FLAG_QUEUED;

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

	return 0;
}


static int
zc0301_vidioc_qbuf(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_buffer b;
	unsigned long lock_flags;

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

	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE ||
	    b.index >= cam->nbuffers || cam->io != IO_MMAP)
		return -EINVAL;

	if (cam->frame[b.index].state != F_UNUSED)
		return -EINVAL;

	cam->frame[b.index].state = F_QUEUED;

	spin_lock_irqsave(&cam->queue_lock, lock_flags);
	list_add_tail(&cam->frame[b.index].frame, &cam->inqueue);
	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);

	PDBGG("Frame #%lu queued", (unsigned long)b.index);

	return 0;
}


static int
zc0301_vidioc_dqbuf(struct zc0301_device* cam, struct file* filp,
		    void __user * arg)
{
	struct v4l2_buffer b;
	struct zc0301_frame_t *f;
	unsigned long lock_flags;
	long timeout;

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

	if (b.type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io!= IO_MMAP)
		return -EINVAL;

	if (list_empty(&cam->outqueue)) {
		if (cam->stream == STREAM_OFF)
			return -EINVAL;
		if (filp->f_flags & O_NONBLOCK)
			return -EAGAIN;
		timeout = wait_event_interruptible_timeout
			  ( cam->wait_frame,
			    (!list_empty(&cam->outqueue)) ||
			    (cam->state & DEV_DISCONNECTED) ||
			    (cam->state & DEV_MISCONFIGURED),
			    cam->module_param.frame_timeout *
			    1000 * msecs_to_jiffies(1) );
		if (timeout < 0)
			return timeout;
		if (cam->state & DEV_DISCONNECTED)
			return -ENODEV;
		if (!timeout || (cam->state & DEV_MISCONFIGURED))
			return -EIO;
	}

	spin_lock_irqsave(&cam->queue_lock, lock_flags);
	f = list_entry(cam->outqueue.next, struct zc0301_frame_t, frame);
	list_del(cam->outqueue.next);
	spin_unlock_irqrestore(&cam->queue_lock, lock_flags);

	f->state = F_UNUSED;

	memcpy(&b, &f->buf, sizeof(b));
	if (f->vma_use_count)
		b.flags |= V4L2_BUF_FLAG_MAPPED;

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

	PDBGG("Frame #%lu dequeued", (unsigned long)f->buf.index);

	return 0;
}


static int
zc0301_vidioc_streamon(struct zc0301_device* cam, void __user * arg)
{
	int type;

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

	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
		return -EINVAL;

	cam->stream = STREAM_ON;

	DBG(3, "Stream on");

	return 0;
}


static int
zc0301_vidioc_streamoff(struct zc0301_device* cam, void __user * arg)
{
	int type, err;

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

	if (type != V4L2_BUF_TYPE_VIDEO_CAPTURE || cam->io != IO_MMAP)
		return -EINVAL;

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

	zc0301_empty_framequeues(cam);

	DBG(3, "Stream off");

	return 0;
}


static int
zc0301_vidioc_g_parm(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_streamparm sp;

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

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

	sp.parm.capture.extendedmode = 0;
	sp.parm.capture.readbuffers = cam->nreadbuffers;

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

	return 0;
}


static int
zc0301_vidioc_s_parm(struct zc0301_device* cam, void __user * arg)
{
	struct v4l2_streamparm sp;

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

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

	sp.parm.capture.extendedmode = 0;

	if (sp.parm.capture.readbuffers == 0)
		sp.parm.capture.readbuffers = cam->nreadbuffers;

	if (sp.parm.capture.readbuffers > ZC0301_MAX_FRAMES)
		sp.parm.capture.readbuffers = ZC0301_MAX_FRAMES;

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

	cam->nreadbuffers = sp.parm.capture.readbuffers;

	return 0;
}


static int zc0301_ioctl_v4l2(struct inode* inode, struct file* filp,
			     unsigned int cmd, void __user * arg)
{
	struct zc0301_device* cam = video_get_drvdata(video_devdata(filp));

	switch (cmd) {

	case VIDIOC_QUERYCAP:
		return zc0301_vidioc_querycap(cam, arg);

	case VIDIOC_ENUMINPUT:
		return zc0301_vidioc_enuminput(cam, arg);

	case VIDIOC_G_INPUT:
		return zc0301_vidioc_g_input(cam, arg);

	case VIDIOC_S_INPUT:
		return zc0301_vidioc_s_input(cam, arg);

	case VIDIOC_QUERYCTRL:
		return zc0301_vidioc_query_ctrl(cam, arg);

	case VIDIOC_G_CTRL:
		return zc0301_vidioc_g_ctrl(cam, arg);

	case VIDIOC_S_CTRL:
		return zc0301_vidioc_s_ctrl(cam, arg);

	case VIDIOC_CROPCAP:
		return zc0301_vidioc_cropcap(cam, arg);

	case VIDIOC_G_CROP:
		return zc0301_vidioc_g_crop(cam, arg);

	case VIDIOC_S_CROP:
		return zc0301_vidioc_s_crop(cam, arg);

	case VIDIOC_ENUM_FMT:
		return zc0301_vidioc_enum_fmt(cam, arg);

	case VIDIOC_G_FMT:
		return zc0301_vidioc_g_fmt(cam, arg);

	case VIDIOC_TRY_FMT:
	case VIDIOC_S_FMT:
		return zc0301_vidioc_try_s_fmt(cam, cmd, arg);

	case VIDIOC_ENUM_FRAMESIZES:
		return zc0301_vidioc_enum_framesizes(cam, arg);

	case VIDIOC_G_JPEGCOMP:
		return zc0301_vidioc_g_jpegcomp(cam, arg);

	case VIDIOC_S_JPEGCOMP:
		return zc0301_vidioc_s_jpegcomp(cam, arg);

	case VIDIOC_REQBUFS:
		return zc0301_vidioc_reqbufs(cam, arg);

	case VIDIOC_QUERYBUF:
		return zc0301_vidioc_querybuf(cam, arg);

	case VIDIOC_QBUF:
		return zc0301_vidioc_qbuf(cam, arg);

	case VIDIOC_DQBUF:
		return zc0301_vidioc_dqbuf(cam, filp, arg);

	case VIDIOC_STREAMON:
		return zc0301_vidioc_streamon(cam, arg);

	case VIDIOC_STREAMOFF:
		return zc0301_vidioc_streamoff(cam, arg);

	case VIDIOC_G_PARM:
		return zc0301_vidioc_g_parm(cam, arg);

	case VIDIOC_S_PARM:
		return zc0301_vidioc_s_parm(cam, arg);

	case VIDIOC_G_STD:
	case VIDIOC_S_STD:
	case VIDIOC_QUERYSTD:
	case VIDIOC_ENUMSTD:
	case VIDIOC_QUERYMENU:
	case VIDIOC_ENUM_FRAMEINTERVALS:
		return -EINVAL;

	default:
		return -EINVAL;

	}
}


static int zc0301_ioctl(struct inode* inode, struct file* filp,
			unsigned int cmd, unsigned long arg)
{
	struct zc0301_device* cam = video_get_drvdata(video_devdata(filp));
	int err = 0;

	if (mutex_lock_interruptible(&cam->fileop_mutex))
		return -ERESTARTSYS;

	if (cam->state & DEV_DISCONNECTED) {
		DBG(1, "Device not present");
		mutex_unlock(&cam->fileop_mutex);
		return -ENODEV;
	}

	if (cam->state & DEV_MISCONFIGURED) {
		DBG(1, "The camera is misconfigured. Close and open it "
		       "again.");
		mutex_unlock(&cam->fileop_mutex);
		return -EIO;
	}

	V4LDBG(3, "zc0301", cmd);

	err = zc0301_ioctl_v4l2(inode, filp, cmd, (void __user *)arg);

	mutex_unlock(&cam->fileop_mutex);

	return err;
}


static const struct file_operations zc0301_fops = {
	.owner =   THIS_MODULE,
	.open =    zc0301_open,
	.release = zc0301_release,
	.ioctl =   zc0301_ioctl,
	.compat_ioctl = v4l_compat_ioctl32,
	.read =    zc0301_read,
	.poll =    zc0301_poll,
	.mmap =    zc0301_mmap,
	.llseek =  no_llseek,
};

/*****************************************************************************/

static int
zc0301_usb_probe(struct usb_interface* intf, const struct usb_device_id* id)
{
	struct usb_device *udev = interface_to_usbdev(intf);
	struct zc0301_device* cam;
	static unsigned int dev_nr = 0;
	unsigned int i;
	int err = 0;

	if (!(cam = kzalloc(sizeof(struct zc0301_device), GFP_KERNEL)))
		return -ENOMEM;

	cam->usbdev = udev;

	if (!(cam->control_buffer = kzalloc(4, GFP_KERNEL))) {
		DBG(1, "kmalloc() failed");
		err = -ENOMEM;
		goto fail;
	}

	if (!(cam->v4ldev = video_device_alloc())) {
		DBG(1, "video_device_alloc() failed");
		err = -ENOMEM;
		goto fail;
	}

	DBG(2, "ZC0301[P] Image Processor and Control Chip detected "
	       "(vid/pid 0x%04X:0x%04X)",id->idVendor, id->idProduct);

	for  (i = 0; zc0301_sensor_table[i]; i++) {
		err = zc0301_sensor_table[i](cam);
		if (!err)
			break;
	}

	if (!err)
		DBG(2, "%s image sensor detected", cam->sensor.name);
	else {
		DBG(1, "No supported image sensor detected");
		err = -ENODEV;
		goto fail;
	}

	if (zc0301_init(cam)) {
		DBG(1, "Initialization failed. I will retry on open().");
		cam->state |= DEV_MISCONFIGURED;
	}

	strcpy(cam->v4ldev->name, "ZC0301[P] PC Camera");
	cam->v4ldev->owner = THIS_MODULE;
	cam->v4ldev->type = VID_TYPE_CAPTURE | VID_TYPE_SCALES;
	cam->v4ldev->fops = &zc0301_fops;
	cam->v4ldev->minor = video_nr[dev_nr];
	cam->v4ldev->release = video_device_release;
	video_set_drvdata(cam->v4ldev, cam);

	init_completion(&cam->probe);

	err = video_register_device(cam->v4ldev, VFL_TYPE_GRABBER,
				    video_nr[dev_nr]);
	if (err) {
		DBG(1, "V4L2 device registration failed");
		if (err == -ENFILE && video_nr[dev_nr] == -1)
			DBG(1, "Free /dev/videoX node not found");
		video_nr[dev_nr] = -1;
		dev_nr = (dev_nr < ZC0301_MAX_DEVICES-1) ? dev_nr+1 : 0;
		complete_all(&cam->probe);
		goto fail;
	}

	DBG(2, "V4L2 device registered as /dev/video%d", cam->v4ldev->minor);

	cam->module_param.force_munmap = force_munmap[dev_nr];
	cam->module_param.frame_timeout = frame_timeout[dev_nr];

	dev_nr = (dev_nr < ZC0301_MAX_DEVICES-1) ? dev_nr+1 : 0;

	usb_set_intfdata(intf, cam);
	kref_init(&cam->kref);
	usb_get_dev(cam->usbdev);

	complete_all(&cam->probe);

	return 0;

fail:
	if (cam) {
		kfree(cam->control_buffer);
		if (cam->v4ldev)
			video_device_release(cam->v4ldev);
		kfree(cam);
	}
	return err;
}


static void zc0301_usb_disconnect(struct usb_interface* intf)
{
	struct zc0301_device* cam;

	down_write(&zc0301_dev_lock);

	cam = usb_get_intfdata(intf);

	DBG(2, "Disconnecting %s...", cam->v4ldev->name);

	if (cam->users) {
		DBG(2, "Device /dev/video%d is open! Deregistration and "
		       "memory deallocation are deferred.",
		    cam->v4ldev->minor);
		cam->state |= DEV_MISCONFIGURED;
		zc0301_stop_transfer(cam);
		cam->state |= DEV_DISCONNECTED;
		wake_up_interruptible(&cam->wait_frame);
		wake_up(&cam->wait_stream);
	} else
		cam->state |= DEV_DISCONNECTED;

	wake_up_interruptible_all(&cam->wait_open);

	kref_put(&cam->kref, zc0301_release_resources);

	up_write(&zc0301_dev_lock);
}


static struct usb_driver zc0301_usb_driver = {
	.name =       "zc0301",
	.id_table =   zc0301_id_table,
	.probe =      zc0301_usb_probe,
	.disconnect = zc0301_usb_disconnect,
};

/*****************************************************************************/

static int __init zc0301_module_init(void)
{
	int err = 0;

	KDBG(2, ZC0301_MODULE_NAME " v" ZC0301_MODULE_VERSION);
	KDBG(3, ZC0301_MODULE_AUTHOR);

	if ((err = usb_register(&zc0301_usb_driver)))
		KDBG(1, "usb_register() failed");

	return err;
}


static void __exit zc0301_module_exit(void)
{
	usb_deregister(&zc0301_usb_driver);
}


module_init(zc0301_module_init);
module_exit(zc0301_module_exit);