[ilsensine]

Device Model: revisione del driver di acquisizione video

L'esempio seguente, molto importante, mostra l'aspetto finale di un driver classico di linux.
Non ho introdotto altri concetti oltre un esempio di creazione di un attributo modificabile runtime: per ogni device v4l, ho definito l'attributo "frame_inc"; un attributo numerico, che ci consentirà di variare l'incremento dei frame del filmato. Visto che il device v4l è in effetti un class_device, ho dichiarato un attributo per questo tipo di device. L'attributo sarà accessibile e modificabile, tramite comuni echo e cat, dal file /sys/class/video4linux/video<n>/frame_inc; potete inserire qualsiasi valore compreso tra -16 e 16. Ognuno dei due device creati ne possiederà uno, indipendente dall'altro. Come esercizio utile, potete cercare di definire ed implementare l'attributo "frame_rate".

Driver testato su un kernel 2.6.8.1:

#include <linux/kernel.h>
#include <linux/module.h>
#include <linux/init.h>
#include <linux/videodev.h>
#include <linux/vmalloc.h>
#include <linux/wait.h>
#include <linux/workqueue.h>
#include <linux/mm.h>
#include <asm/atomic.h>

/* Identificativo fittizzio per i nostri device video */
#define VID_HARDWARE_VSAMPLE 255

/* Limiti alla dimensione dell'immagine */
#define MAX_X 352
#define MAX_Y 288
#define MIN_X 16
#define MIN_Y 16

/* Dimensione massima di una immagine, in yuv420, arrotondata per eccesso alla pagina */
#define IMAGE_AREA PAGE_ALIGN((MAX_X*MAX_Y*3)/2)

/* Stato di acquisizione dell'immagine */
enum istate {
	img_ready,	/* acquisibile */
	img_grabbing,	/* in acquisizione */
	img_done	/* pronta per essere letta in user space */
};

struct v_data;

/* Teniamo un buffer classico di due immagini, per il double buffering. */
/* La struct v_image descrive un singolo buffer. */
struct v_image {
	unsigned char *map;
	int x;
	int y;
	enum istate state;
	wait_queue_head_t queue;
	struct v_data *vdata;
	spinlock_t lock; /* Protegge la delicata transizione ready->grabbing */
};

struct v_work_data {
	struct work_struct work;
	struct v_image *image;
};

/* Struttura descrittiva per ogni istanza del nostro device v4l */
struct v_data {
	struct video_device *vdev;
	struct video_picture vpict;
	atomic_t frame_index;
	int frame_inc;
	unsigned char *vma;
	struct v_image image[2];
};

/* Crea una nuova immagine.
   In un driver reale questa funzione
   probabilmente sara' una irq service routine. */
static void v_work_image(void *data)
{
	struct v_work_data *wdata = data;
	struct v_image *image = wdata->image;
	int i, px, py;
	unsigned char *y, *u, *v;

	kfree(wdata);
	WARN_ON(image->state!=img_grabbing);
	y = image->map;
	u = y+(image->x*image->y);
	v = u+(image->x*image->y)/4;
	i = atomic_read(&image->vdata->frame_index);
	atomic_add(image->vdata->frame_inc, &image->vdata->frame_index);

	for (py=0; py<image->y; py++) {
		for (px=0; px<image->x; px++) {
			y[py*image->x+px] = px + py + i * 3;
		}
	}
	for (py=0; py<image->y/2; py++) {
		for (px=0; px<image->x/2; px++) {
			u[py*image->x/2+px] = 128 + py + i * 2;
			v[py*image->x/2+px] = 64 + px + i * 5;
		}
	}

	smp_wmb();
	image->state = img_done;
	wake_up_interruptible_all(&image->queue);
}

/* Schedula l'acquisizione di una immagine.
   Un driver reale preparerà qui una transazione dma. */
static int v_create_work(struct v_data *vdata, struct video_mmap *map)
{
	struct v_work_data *work_data;
	struct v_image *image;
	if (map->frame<0 || map->frame>1 ||
			map->format!=VIDEO_PALETTE_YUV420P ||
			map->width<MIN_X || map->width>MAX_X ||
			map->height<MIN_Y || map->height>MAX_Y ||
			map->width&1 || map->height&1) {
		return -EINVAL;
	}
	image = vdata->image+map->frame;
	work_data = kmalloc(sizeof(*work_data), GFP_KERNEL);
	if (!work_data)
		return -ENOMEM;
	INIT_WORK(&work_data->work, v_work_image, work_data);
	work_data->image = image;
/* Lo spinlock non e' realmente necessario, in quanto la ioctl viene eseguita sotto BKL.
   A breve pero' le ioctl verranno liberate da questo lock, quindi teniamoci pronti.
   Notate che le altre parti che modificano image->state non e' necessario che siano
   protette da questo lock, in quanto effettuate su uno stato noto che non
   puo' venire modificato altrove. */
	spin_lock(&image->lock);
	if (image->state!=img_ready) {
/* Giungiamo qui solo se lo user space sta facendo qualcosa di sbagliato.
   Non dobbiamo mai fidarci delle applicazioni user space. */
		spin_unlock(&image->lock);
		kfree(work_data);
		return -EBUSY;
	}
	image->state = img_grabbing;
	spin_unlock(&image->lock);
	image->x = map->width;
	image->y = map->height;
	schedule_delayed_work(&work_data->work, msecs_to_jiffies(25));
	return 0;
}

/* Interfaccia v4l per la ioctl. arg punta a un buffer interno del kernel,
   non occorre usare copy_*_user. */
static int v_ioctl(struct inode *inode, struct file *file,
	unsigned int cmd, void *arg)
{
	struct video_device *vdev = video_devdata(file);
	struct v_data *vdata = video_get_drvdata(vdev);
	int ret = 0;

	switch (cmd) {
	case VIDIOCGCAP: { /* get capability */
		struct video_capability *vcap = arg;
		memset(vcap, 0, sizeof(*vcap));
		snprintf(vcap->name, sizeof(vcap->name), "v4l_dummy");
		vcap->type = VID_TYPE_CAPTURE | VID_TYPE_SUBCAPTURE;
		vcap->channels = 1;
		vcap->audios = 0;
		vcap->maxwidth = MAX_X;
		vcap->maxheight = MAX_Y;
		vcap->minwidth = MIN_X;
		vcap->minheight = MIN_Y;
		break;
	}
	case VIDIOCGCHAN: { /* get channel format */
		struct video_channel *vc = arg;
		if(vc->channel!=0)
			return -EINVAL;
		vc->norm = VIDEO_MODE_PAL;
		vc->type = VIDEO_TYPE_CAMERA;
		vc->flags = 0;
		vc->tuners = 0;
		snprintf(vc->name, sizeof(vc->name),
			"channel %d", vc->channel);
		break;
	}
	case VIDIOCSCHAN: { /* set channel format */
		struct video_channel *vc = arg;
		if (vc->channel!=0)
			return -EINVAL;
		if (vc->norm != VIDEO_MODE_PAL &&
				vc->norm != VIDEO_MODE_NTSC &&
				vc->norm != VIDEO_MODE_SECAM &&
				vc->norm != VIDEO_MODE_AUTO)
			return -EINVAL;
		break;
	}
	case VIDIOCGPICT: { /* get picture type */
		struct video_picture *pict = arg;
		memcpy(pict, &vdata->vpict, sizeof(*pict));
		break;
	}
	case VIDIOCSPICT: { /* set picture type */
		struct video_picture *pict = arg;
		if (pict->palette!=VIDEO_PALETTE_YUV420P ||
				pict->depth!=12)
			return -EINVAL;
		memcpy(&vdata->vpict, pict, sizeof(*pict));
		break;
	}
	case VIDIOCGMBUF: { /* get memory buffer info */
		struct video_mbuf *vm = arg;
		memset(vm, 0, sizeof(*vm));
		vm->frames = 2;
		vm->size = vm->frames*IMAGE_AREA;
		vm->offsets[0] = 0;
		vm->offsets[1] = IMAGE_AREA;
		break;
	}
	case VIDIOCGFBUF: {
		struct video_buffer *vb = arg;
		memset(vb, 0, sizeof(*vb));
		break;
	}
	case VIDIOCMCAPTURE: { /* schedula l'acquisizione del buffer indicato */
		struct video_mmap *map = arg;
		ret = v_create_work(vdata, map);
		break;
	}
	case VIDIOCSYNC: { /* sincronizza l'acquisizione del buffer indicato */
		int ipos = *(int *) arg;
		struct v_image *image;
		if (ipos<0 || ipos>1)
			return -EINVAL;
		image = &vdata->image[ipos];
		if (image->state==img_ready)
			return -EINVAL;
		if (wait_event_interruptible(image->queue, image->state==img_done))
			ret = -EINTR;
		else
			image->state = img_ready;
		break;
	}
	default:
/* Il framework v4l richiede il ritorno di -ENOIOCTLCMD, non di -ENOTTY. */
		ret = -ENOIOCTLCMD;
	}
  return ret;
}

static int v_ioctl_if(struct inode *inode, struct file *file,
	unsigned int cmd, unsigned long arg)
{
	return video_usercopy(inode, file, cmd, arg, v_ioctl);
}

static struct page *v_vm_nopage(struct vm_area_struct *vma,
	unsigned long address, int *type)
{
	unsigned long ofs = (vma->vm_pgoff<<PAGE_SHIFT)+(address-vma->vm_start);
	struct page *ret;
	struct video_device *vdev = vma->vm_private_data;
	struct v_data *vdata = video_get_drvdata(vdev);

	ofs &= PAGE_MASK;
	if (ofs>=(IMAGE_AREA*2))
		return NOPAGE_SIGBUS;
	ret = vmalloc_to_page(vdata->vma+ofs);
	if (ret)
		get_page(ret);
	if (type)
		*type = VM_FAULT_MINOR;
	return ret;
}

static struct vm_operations_struct v_vm_operations = {
	.nopage = v_vm_nopage,
};

static int v_mmap(struct file *file, struct vm_area_struct *vma)
{
	struct video_device *vdev = video_devdata(file);
	unsigned long size  = vma->vm_end - vma->vm_start;
	unsigned long pg_base = vma->vm_pgoff<<PAGE_SHIFT;

	if ((pg_base+size)>(IMAGE_AREA*2))
		return -EINVAL;
	if (!(vma->vm_flags&VM_SHARED))
		return -EINVAL;
	vma->vm_private_data = vdev;
	vma->vm_flags |= VM_RESERVED;
	vma->vm_ops = &v_vm_operations;
	return 0;
}

static int v_open(struct inode *inode, struct file *file)
{
	struct video_device *vdev = video_devdata(file);
	struct v_data *vdata = video_get_drvdata(vdev);
	int ret = video_exclusive_open(inode, file);
	if (ret)
		return ret;

	vdata->vma = vmalloc(IMAGE_AREA*2);
	if (!vdata->vma) {
		video_exclusive_release(inode, file);
		return -ENOMEM;
	}
	vdata->image[0].map = vdata->vma;
	vdata->image[1].map = vdata->image[0].map+IMAGE_AREA;
	vdata->image[0].state = img_ready;
	vdata->image[1].state = img_ready;
	return 0;
}

static int v_release(struct inode *inode, struct file *file)
{
	struct video_device *vdev = video_devdata(file);
	struct v_data *vdata = video_get_drvdata(vdev);
/* Potrebbero esserci ancora work all'opera senza che l'utente abbia
   effettuato una VIDIOCSYNC. Occorre attenderne la terminazione prima del vfree! */
	flush_scheduled_work();
	vfree(vdata->vma);
	video_exclusive_release(inode, file);
	return 0;
}

static void v_dev_release(struct video_device *vdev)
{
	struct v_data *vdata = video_get_drvdata(vdev);
	video_device_release(vdev);
	kfree(vdata);
}

static struct file_operations v_fops = {
	.owner		= THIS_MODULE,
	.open		= v_open,
	.release	= v_release,
	.ioctl		= v_ioctl_if,
	.mmap		= v_mmap,
	.llseek		= no_llseek,
};

static struct video_device vdev_skull = {
	.owner =	THIS_MODULE,
	.name =		"v4l_sample",
	.type =		VID_TYPE_CAPTURE,
	.hardware =	VID_HARDWARE_VSAMPLE,
	.fops =		&v_fops,
	.release =	v_dev_release,
};

/* Alloca e inizializza una struttura v_data */
static __devinit struct v_data *v_dev_alloc(struct device *dev)
{
	struct v_data *vdata = kmalloc(sizeof(*vdata), GFP_KERNEL);
	struct platform_device *pdev = to_platform_device(dev);
	if (!vdata)
		return NULL;
	vdata->vdev = video_device_alloc();
	if (!vdata->vdev) {
		kfree(vdata);
		return NULL;
	}
	vdata->frame_inc = 1;
	*vdata->vdev = vdev_skull;
	video_set_drvdata(vdata->vdev, vdata);
	snprintf(vdata->vdev->name, sizeof(vdata->vdev->name), "%s:%d",
		pdev->name, pdev->id);
	vdata->vpict.palette = VIDEO_PALETTE_YUV420P;
	vdata->vpict.depth = 12;
	atomic_set(&vdata->frame_index, 0);
	vdata->image[0].vdata = vdata;
	vdata->image[1].vdata = vdata;
	init_waitqueue_head(&vdata->image[0].queue);
	init_waitqueue_head(&vdata->image[1].queue);
	spin_lock_init(&vdata->image[0].lock);
	spin_lock_init(&vdata->image[1].lock);
	vdata->vdev->dev = dev;
	return vdata;
}

/* Mostra il contenuto di frame_inc */
static ssize_t show_frame_inc(struct class_device *cd, char *buf)
{
	struct video_device *vdev = to_video_device(cd);
	struct v_data *vdata = video_get_drvdata(vdev);
/* buf e' sempre di una pagina */
	return sprintf(buf, "%d\n", vdata->frame_inc);
}

/* Memorizza un nuovo valore per frame_inc */
static ssize_t store_frame_inc(struct class_device *cd, const char * buf, size_t count)
{
	struct video_device *vdev = to_video_device(cd);
	struct v_data *vdata = video_get_drvdata(vdev);
	int frame_inc;
/* buf proviene direttamente da user space, quindi è inaffidabile per definizione.
   Aggiungiamo un paio di controlli, almeno per essere certi che la strtol legga
   entro il buffer fornito. */
	if (count<=0)
		return -EINVAL;
	if (buf[count-1]!='\0' && buf[count-1]!='\n')
		return -EINVAL;
	frame_inc = simple_strtol(buf, NULL, 0);
	if (frame_inc<-16 || frame_inc>16)
		return -EINVAL;
	vdata->frame_inc = frame_inc;
	return count;
}

/* Questa macro crea l'attributo con il nome class_device_attr_frame_inc.
   Possiamo modificare runtime /sys/class/video4linux/videoX/frame_inc. */
static CLASS_DEVICE_ATTR(frame_inc, 0644, show_frame_inc, store_frame_inc);

/* Il kernel ha trovato un nuovo dispositivo e ci chiede di gestirlo */
static int __devinit v_probe(struct device *dev)
{
	struct v_data *vdata = v_dev_alloc(dev);
	int ret;
	if (!vdata)
		return -ENOMEM;
	dev_set_drvdata(dev, vdata);
	ret = video_register_device(vdata->vdev, VFL_TYPE_GRABBER, -1);
	if (ret) {
		dev_set_drvdata(dev, NULL);
		video_device_release(vdata->vdev);
		kfree(vdata);
		return ret;
	}
	video_device_create_file(vdata->vdev, &class_device_attr_frame_inc);
	printk(KERN_INFO "%s: Using device /dev/video%d\n", __func__, vdata->vdev->minor);
	return 0;
}

/* Questa funzione e' semplice in quanto sappiamo che il device non ci scomparira'
   sotto i piedi runtime. Se non fossimo certi di questo, dovremmo prendere
   altre accortezze. */
static int __devexit v_remove(struct device *dev)
{
	struct v_data *vdata = dev_get_drvdata(dev);
	dev_set_drvdata(dev, NULL);
	video_unregister_device(vdata->vdev);
	return 0;
}

static struct device_driver drv = {
	.name		= "dummy_dev",
	.bus		= &platform_bus_type,
	.probe		= v_probe,
/* I puntatori __devexit devono essere inseriti tramite __devexit_p */
	.remove		= __devexit_p(v_remove),
};

static void dummy_dev_release(struct device *dev)
{
}

/* I nostri device fittizzi */
static struct platform_device pdev0 = {
	.name =	"dummy_dev",
	.id =	0,
	.dev =	{
		.release = dummy_dev_release
	}
};

static struct platform_device pdev1 = {
	.name =	"dummy_dev",
	.id =	1,
	.dev =	{
		.release = dummy_dev_release
	}
};

/* Regitra i device fittizzi. In un driver reale (ad es. un driver usb), saranno
   altre parti del kernel ad occuparsene. */
static int __init register_dummy_devices(void)
{
	int ret = platform_device_register(&pdev0);
	if (ret)
		return ret;
	ret = platform_device_register(&pdev1);
	if(ret)
		platform_device_unregister(&pdev0);
	return ret;
}

/* Non puo' essere __exit in quanto e' chiamata anche da v_init,
   che e' dichiarata __init. */
static int unregister_dummy_devices(void)
{
	platform_device_unregister(&pdev0);
	platform_device_unregister(&pdev1);
	return 0;
}

static int __init v_init(void)
{
	int ret = register_dummy_devices();
	if (ret)
		return ret;
	ret = driver_register(&drv);
	if (ret)
		unregister_dummy_devices();
	return ret;
}

static void __exit v_exit(void)
{
	driver_unregister(&drv);
	unregister_dummy_devices();
}

MODULE_AUTHOR("ilsensine");
MODULE_DESCRIPTION("sample v4l driver");
MODULE_LICENSE("GPL");

module_init(v_init);
module_exit(v_exit);