int init_module (void)
{
RTIME tick_period;
RTIME now;
#ifdef SHM_DEBUG
int size = SHMSIZ ;
unsigned long vaddr, paddr;
#endif
shm = (int *)rtai_kmalloc(nam2num(SHMNAM), SHMSIZ);
shm = (int *)rtai_kmalloc(nam2num(SHMNAM), SHMSIZ);
shm = (int *)rtai_kmalloc(nam2num(SHMNAM), SHMSIZ);
shm = (int *)rtai_kmalloc(nam2num(SHMNAM), SHMSIZ);
if (shm == NULL)
return -ENOMEM;
memset(shm, 0, SHMSIZ);
#ifdef SHM_DEBUG
/* Show physical addresses of */
vaddr = (unsigned long)shm;
while (size > 0)
{
paddr = kvirt_to_pa(vaddr);
printk("vaddr=0x%lx paddr=0x%lx\n", vaddr, paddr);
vaddr += PAGE_SIZE;
size -= PAGE_SIZE;
}
#endif
rt_task_init(&thread, fun, 0, STACK_SIZE, 0, 0, 0);
tick_period = start_rt_timer(nano2count(TICK_PERIOD));
now = rt_get_time();
rt_task_make_periodic(&thread, now + tick_period, tick_period*PERIOD_COUNT);
return 0;
}
/* allocate user space mmapable block of memory in the kernel space */
void * rvmalloc(unsigned long size)
{
void * mem;
unsigned long adr, page;
#if LINUX_VERSION_CODE < 0x020300
mem=vmalloc(size);
#else
mem=vmalloc_32(size);
#endif
if (mem)
{
memset(mem, 0, size); /* Clear the ram out, no junk to the user */
adr=(unsigned long) mem;
while (size > 0)
{
#if LINUX_VERSION_CODE < 0x020300
page = kvirt_to_phys(adr);
mem_map_reserve(MAP_NR(phys_to_virt(page)));
#else
page = kvirt_to_pa(adr);
mem_map_reserve(virt_to_page(__va(page)));
#endif
adr+=PAGE_SIZE;
size-=PAGE_SIZE;
}
}
return mem;
}
static void rvunnail(char *mem, unsigned long size)
{
unsigned long adr, page;
if (mem)
{
adr=(unsigned long) mem;
while (size > 0)
{
page = kvirt_to_pa(adr);
ClearPageReserved(virt_to_page(__va(page)));
adr+=PAGE_SIZE;
size-=PAGE_SIZE;
}
printk(" pages have been unnailed \n");
}
}
void rvfree(void * mem, signed long size)
{
unsigned long adr, page;
if (!mem)
return;
adr=(unsigned long) mem;
while (size > 0) {
page = kvirt_to_pa(adr);
mem_map_unreserve(virt_to_page((unsigned long)__va(page)));
adr += PAGE_SIZE;
size -= PAGE_SIZE;
}
vfree(mem);
}
static void rvfree(void * mem, signed long size)
{
unsigned long adr, page;
if (mem)
{
adr=(unsigned long) mem;
while (size > 0)
{
page = kvirt_to_pa(adr);
ClearPageReserved(virt_to_page(__va(page)));
adr+=PAGE_SIZE;
size-=PAGE_SIZE;
}
vfree(mem);
}
}
static int do_rv_mmap(struct vm_area_struct *vma,const char *adr, unsigned long size)
{
unsigned long start=(unsigned long) adr;
unsigned long page,pos;
if (!buf_start_2) {
printk(KERN_INFO " Data was not initialized");
return -EINVAL;
}
pos=(unsigned long)buf_start_2;
while (size > 0) {
page = kvirt_to_pa(pos);
if (remap_page_range(vma,start, page, PAGE_SIZE, PAGE_SHARED))
return -EAGAIN;
start+=PAGE_SIZE;
pos+=PAGE_SIZE;
size-=PAGE_SIZE;
}
return 0;
}
static void rvnail(char *mem,unsigned long size)
{
unsigned long adr, page;
printk(KERN_INFO " before vrnail %ld \n",size);
if (mem)
{
memset(mem, 0, size);
adr=(unsigned long) mem;
while (size > 0)
{
page = kvirt_to_pa(adr);
SetPageReserved(virt_to_page(__va(page)));
adr+=PAGE_SIZE;
size-=PAGE_SIZE;
}
printk(KERN_INFO " pages have been nailed \n");
}
return ;
}
int oprof_hash_map_mmap(struct file * file, struct vm_area_struct * vma)
{
ulong start = (ulong)vma->vm_start;
ulong page, pos;
ulong size = (ulong)(vma->vm_end-vma->vm_start);
if (size > PAGE_ALIGN(OP_HASH_MAP_SIZE) || (vma->vm_flags & VM_WRITE) || GET_VM_OFFSET(vma))
return -EINVAL;
pos = (ulong)hash_map;
while (size > 0) {
page = kvirt_to_pa(pos);
if (remap_page_range(start, page, PAGE_SIZE, PAGE_SHARED))
return -EAGAIN;
start += PAGE_SIZE;
pos += PAGE_SIZE;
size -= PAGE_SIZE;
}
return 0;
}
void * rvmalloc(signed long size)
{
void * mem;
unsigned long adr, page;
mem = VMALLOC_32(size);
if (!mem)
return NULL;
memset(mem, 0, size);
adr=(unsigned long) mem;
while (size > 0) {
page = kvirt_to_pa(adr);
mem_map_reserve(virt_to_page((unsigned long)__va(page)));
adr += PAGE_SIZE;
size -= PAGE_SIZE;
}
return mem;
}
static void * rvmalloc(signed long size)
{
void * mem;
unsigned long adr, page;
mem=vmalloc_32(size);
if (mem)
{
memset(mem, 0, size); /* Clear the ram out, no junk to the user */
adr=(unsigned long) mem;
while (size > 0)
{
page = kvirt_to_pa(adr);
SetPageReserved(virt_to_page(__va(page)));
adr+=PAGE_SIZE;
size-=PAGE_SIZE;
}
}
return mem;
}
void rvfree(void * mem, unsigned long size)
{
unsigned long adr, page;
if (mem)
{
adr=(unsigned long) mem;
while (size > 0)
{
#if LINUX_VERSION_CODE < 0x020300
page = kvirt_to_phys(adr);
mem_map_unreserve(MAP_NR(phys_to_virt(page)));
#else
page = kvirt_to_pa(adr);
mem_map_unreserve(virt_to_page(__va(page)));
#endif
adr+=PAGE_SIZE;
size-=PAGE_SIZE;
}
vfree(mem);
}
}
/* this function will map (fragment of) rvmalloc'ed memory area to user space */
int rvmmap(void *mem, unsigned memsize, struct vm_area_struct *vma) {
unsigned long pos, size, start=vma->vm_start;
#if LINUX_VERSION_CODE < 0x20300
unsigned long offset = vma->vm_offset;
#else
unsigned long offset = vma->vm_pgoff << PAGE_SHIFT;
#endif
/* this is not time critical code, so we check the arguments */
/* vma->vm_offset HAS to be checked (and is checked)*/
if (offset<0)
return -EFAULT;
size = vma->vm_end - vma->vm_start;
if (size + offset > memsize)
return -EFAULT;
pos = (unsigned long) mem + offset;
if (pos%PAGE_SIZE || start%PAGE_SIZE || size%PAGE_SIZE)
return -EFAULT;
while (size>0) {
#if LINUX_VERSION_CODE < 0x020300
if (remap_page_range(start,kvirt_to_phys(pos), PAGE_SIZE,
vma->vm_page_prot )) {
/* FIXME: what should we do here to unmap previous pages ?*/
printk(KERN_ERR "rvmmap failed: vm_start=0x%lx, vm_end=0x%lx, size=0x%lx, pos=0x%lx; please report to [email protected]\n",vma->vm_start,vma->vm_end,size,pos);
return -EFAULT;
}
#else
if (remap_page_range(start, kvirt_to_pa(pos),
PAGE_SIZE, PAGE_SHARED))
return -EAGAIN;
#endif
pos+=PAGE_SIZE;
start+=PAGE_SIZE;
size-=PAGE_SIZE;
}
return 0;
}
static int vloopback_mmap(struct file *f, struct vm_area_struct *vma)
{
struct video_device *loopdev=video_devdata(f);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
priv_ptr ptr=(priv_ptr)video_get_drvdata(loopdev);
#else
priv_ptr ptr=(priv_ptr)loopdev->priv;
#endif
int nr=ptr->pipenr;
unsigned long start = (unsigned long)vma->vm_start;
long size = vma->vm_end - vma->vm_start;
unsigned long page, pos;
down(&loops[nr]->lock);
if (ptr->in) {
loops[nr]->zerocopy=1;
if (loops[nr]->ropen) {
info("Can't change size while opened for read");
up(&loops[nr]->lock);
return -EINVAL;
}
if (!size) {
up(&loops[nr]->lock);
return -EINVAL;
}
if (loops[nr]->buffer)
rvfree(loops[nr]->buffer, loops[nr]->buflength*N_BUFFS);
loops[nr]->buflength=size;
loops[nr]->buffer=rvmalloc(loops[nr]->buflength*N_BUFFS);
}
if (loops[nr]->buffer == NULL) {
up(&loops[nr]->lock);
return -EINVAL;
}
if (size > (((N_BUFFS * loops[nr]->buflength) + PAGE_SIZE - 1) & ~(PAGE_SIZE - 1))) {
up(&loops[nr]->lock);
return -EINVAL;
}
pos = (unsigned long)loops[nr]->buffer;
while (size > 0) {
#if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,9)
page = kvirt_to_pa(pos);
if (remap_page_range(vma,start, page, PAGE_SIZE, PAGE_SHARED)) {
#else
page = vmalloc_to_pfn((void *)pos);
if (remap_pfn_range(vma, start, page, PAGE_SIZE, PAGE_SHARED)) {
#endif
up(&loops[nr]->lock);
return -EAGAIN;
}
start += PAGE_SIZE;
pos += PAGE_SIZE;
size -= PAGE_SIZE;
}
up(&loops[nr]->lock);
return 0;
}
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,31)
static long vloopback_ioctl(struct file *f, unsigned int cmd, unsigned long arg)
#else
static int vloopback_ioctl(struct inode *inod, struct file *f, unsigned int cmd, unsigned long arg)
#endif
{
struct video_device *loopdev=video_devdata(f);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
priv_ptr ptr=(priv_ptr)video_get_drvdata(loopdev);
#else
priv_ptr ptr=(priv_ptr)loopdev->priv;
#endif
int nr=ptr->pipenr;
int i;
if (loops[nr]->zerocopy) {
if (!ptr->in) {
loops[nr]->ioctlnr=cmd;
loops[nr]->ioctllength=_IOC_SIZE(cmd);
/* info("DEBUG: vl_ioctl: !loop->in"); */
/* info("DEBUG: vl_ioctl: cmd %lu", cmd); */
/* info("DEBUG: vl_ioctl: len %lu", loops[nr]->ioctllength); */
if(copy_from_user(loops[nr]->ioctldata, (void*)arg, _IOC_SIZE(cmd)))
return -EFAULT;
kill_proc(loops[nr]->pid, SIGIO, 1);
#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
wait_event_interruptible(loops[nr]->wait, loops[nr]->ioctlnr==-1);
#else
interruptible_sleep_on(&loops[nr]->wait);
#endif
if (loops[nr]->invalid_ioctl) {
//info ("DEBUG: There was an invalid ioctl");
loops[nr]->invalid_ioctl = 0;
return -ENOTTY;
}
if (cmd & IOC_IN && !(cmd & IOC_OUT)) {
//info("DEBUG: vl_ioctl: cmd & IOC_IN 1");
if (memcmp(loops[nr]->ioctlretdata, loops[nr]->ioctldata, _IOC_SIZE(cmd))) {
return -EINVAL;
}
//info("DEBUG: vl_ioctl: cmd & IOC_IN 2");
return 0;
} else {
if (copy_to_user((void*)arg, loops[nr]->ioctlretdata, _IOC_SIZE(cmd)))
return -EFAULT;
//info("DEBUG: vl_ioctl: !(cmd & IOC_IN) 1");
return 0;
}
} else {
if ( (loops[nr]->ioctlnr!=cmd) && (cmd != (VIDIOCSINVALID))) {
/* wrong ioctl */
info("DEBUG: vo_ioctl: Wrong IOCTL");
return 0;
}
if (cmd == VIDIOCSINVALID) {
loops[nr]->invalid_ioctl = 1;
} else {
if (copy_from_user(loops[nr]->ioctlretdata, (void*)arg, loops[nr]->ioctllength))
return -EFAULT;
}
loops[nr]->ioctlnr=-1;
if (waitqueue_active(&loops[nr]->wait))
wake_up(&loops[nr]->wait);
return 0;
}
}
switch(cmd)
{
/* Get capabilities */
case VIDIOCGCAP:
{
struct video_capability b;
if (ptr->in) {
sprintf(b.name, "Video loopback %d input",
ptr->pipenr);
b.type = 0;
} else {
sprintf(b.name, "Video loopback %d output",
ptr->pipenr);
b.type = VID_TYPE_CAPTURE;
}
b.channels=1;
b.audios=0;
b.maxwidth=loops[nr]->width;
b.maxheight=loops[nr]->height;
b.minwidth=20;
b.minheight=20;
if(copy_to_user((void*)arg, &b, sizeof(b)))
return -EFAULT;
return 0;
}
/* Get channel info (sources) */
case VIDIOCGCHAN:
{
struct video_channel v;
if(copy_from_user(&v, (void*)arg, sizeof(v)))
return -EFAULT;
if(v.channel!=0) {
info("VIDIOCGCHAN: Invalid Channel, was %d", v.channel);
v.channel=0;
//return -EINVAL;
}
v.flags=0;
v.tuners=0;
v.norm=0;
v.type = VIDEO_TYPE_CAMERA;
/*strcpy(v.name, "Loopback"); -- tibit */
strcpy(v.name, "Composite1");
if(copy_to_user((void*)arg, &v, sizeof(v)))
return -EFAULT;
return 0;
}
/* Set channel */
case VIDIOCSCHAN:
{
int v;
if(copy_from_user(&v, (void*)arg, sizeof(v)))
return -EFAULT;
if(v!=0) {
info("VIDIOCSCHAN: Invalid Channel, was %d", v);
return -EINVAL;
}
return 0;
}
/* Get tuner abilities */
case VIDIOCGTUNER:
{
struct video_tuner v;
if(copy_from_user(&v, (void*)arg, sizeof(v))!=0)
return -EFAULT;
if(v.tuner) {
info("VIDIOCGTUNER: Invalid Tuner, was %d", v.tuner);
return -EINVAL;
}
strcpy(v.name, "Format");
v.rangelow=0;
v.rangehigh=0;
v.flags=0;
v.mode=VIDEO_MODE_AUTO;
if(copy_to_user((void*)arg,&v, sizeof(v))!=0)
return -EFAULT;
return 0;
}
/* Get picture properties */
case VIDIOCGPICT:
{
struct video_picture p;
p.colour=0x8000;
p.hue=0x8000;
p.brightness=0x8000;
p.contrast=0x8000;
p.whiteness=0x8000;
p.depth=0x8000;
p.palette=loops[nr]->palette;
if(copy_to_user((void*)arg, &p, sizeof(p)))
return -EFAULT;
return 0;
}
/* Set picture properties */
case VIDIOCSPICT:
{
struct video_picture p;
if(copy_from_user(&p, (void*)arg, sizeof(p)))
return -EFAULT;
if (!ptr->in) {
if (p.palette!=loops[nr]->palette)
return -EINVAL;
} else
loops[nr]->palette=p.palette;
return 0;
}
/* Get the video overlay window */
case VIDIOCGWIN:
{
struct video_window vw;
vw.x=0;
vw.y=0;
vw.width=loops[nr]->width;
vw.height=loops[nr]->height;
vw.chromakey=0;
vw.flags=0;
vw.clipcount=0;
if(copy_to_user((void*)arg, &vw, sizeof(vw)))
return -EFAULT;
return 0;
}
/* Set the video overlay window - passes clip list for hardware smarts , chromakey etc */
case VIDIOCSWIN:
{
struct video_window vw;
if(copy_from_user(&vw, (void*)arg, sizeof(vw)))
return -EFAULT;
if(vw.flags)
return -EINVAL;
if(vw.clipcount)
return -EINVAL;
if (loops[nr]->height==vw.height &&
loops[nr]->width==vw.width)
return 0;
if(!ptr->in) {
return -EINVAL;
} else {
loops[nr]->height=vw.height;
loops[nr]->width=vw.width;
/* Make sure nobody is using the buffer while we
fool around with it.
We are also not allowing changes while
somebody using mmap has the output open.
*/
down(&loops[nr]->lock);
if (loops[nr]->ropen) {
info("Can't change size while opened for read");
up(&loops[nr]->lock);
return -EINVAL;
}
if (loops[nr]->buffer)
rvfree(loops[nr]->buffer, loops[nr]->buflength*N_BUFFS);
loops[nr]->buflength=vw.width*vw.height*4;
loops[nr]->buffer=rvmalloc(loops[nr]->buflength*N_BUFFS);
up(&loops[nr]->lock);
}
return 0;
}
/* Memory map buffer info */
case VIDIOCGMBUF:
{
struct video_mbuf vm;
vm.size=loops[nr]->buflength*N_BUFFS;
vm.frames=N_BUFFS;
for (i=0; i<vm.frames; i++)
vm.offsets[i]=i*loops[nr]->buflength;
if(copy_to_user((void*)arg, &vm, sizeof(vm)))
return -EFAULT;
return 0;
}
/* Grab frames */
case VIDIOCMCAPTURE:
{
struct video_mmap vm;
if (ptr->in)
return -EINVAL;
if (!loops[nr]->buffer)
return -EINVAL;
if (copy_from_user(&vm, (void*)arg, sizeof(vm)))
return -EFAULT;
if (vm.format!=loops[nr]->palette)
return -EINVAL;
if (vm.frame > N_BUFFS)
return -EINVAL;
return 0;
}
/* Sync with mmap grabbing */
case VIDIOCSYNC:
{
int frame;
unsigned long fw;
if (copy_from_user((void *)&frame, (void*)arg, sizeof(int)))
return -EFAULT;
if (ptr->in)
return -EINVAL;
if (!loops[nr]->buffer)
return -EINVAL;
/* Ok, everything should be alright since the program
should have called VIDIOMCAPTURE and we are ready to
do the 'capturing' */
if (frame > 1)
return -EINVAL;
loops[nr]->frame=frame;
#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
fw = loops[nr]->frameswrite;
wait_event_interruptible(loops[nr]->wait, fw!=loops[nr]->frameswrite);
#else
interruptible_sleep_on(&loops[nr]->wait);
#endif
if (!loops[nr]->buffer) /* possibly released during sleep */
return -EINVAL;
loops[nr]->framesread++;
return 0;
}
/* Get attached units */
case VIDIOCGUNIT:
{
struct video_unit vu;
if (ptr->in)
vu.video=loops[nr]->vloopout->minor;
else
vu.video=loops[nr]->vloopin->minor;
vu.vbi=VIDEO_NO_UNIT;
vu.radio=VIDEO_NO_UNIT;
vu.audio=VIDEO_NO_UNIT;
vu.teletext=VIDEO_NO_UNIT;
if (copy_to_user((void*)arg, &vu, sizeof(vu)))
return -EFAULT;
return 0;
}
/* Get frame buffer */
case VIDIOCGFBUF:
{
struct video_buffer vb;
memset(&vb, 0, sizeof(vb));
vb.base=NULL;
if(copy_to_user((void *)arg, (void *)&vb, sizeof(vb)))
return -EFAULT;
return 0;
}
/* Start, end capture */
case VIDIOCCAPTURE:
{
int start;
if (copy_from_user(&start, (void*)arg, sizeof(int)))
return -EFAULT;
/*
if (start) info ("Capture started");
else info ("Capture stopped");
*/
return 0;
}
case VIDIOCGFREQ:
case VIDIOCSFREQ:
case VIDIOCGAUDIO:
case VIDIOCSAUDIO:
return -EINVAL;
case VIDIOCKEY:
return 0;
default:
return -ENOTTY;
//return -ENOIOCTLCMD;
}
return 0;
}
static unsigned int vloopback_poll(struct file *f, struct poll_table_struct *wait)
{
struct video_device *loopdev=video_devdata(f);
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,27)
priv_ptr ptr=(priv_ptr)video_get_drvdata(loopdev);
#else
priv_ptr ptr=(priv_ptr)loopdev->priv;
#endif
int nr=ptr->pipenr;
if (loopdev==NULL)
return -EFAULT;
if (!ptr->in)
return 0;
if (loops[nr]->ioctlnr!=-1) {
if (loops[nr]->zerocopy) {
return (POLLIN | POLLPRI | POLLOUT | POLLRDNORM);
} else {
return (POLLOUT);
}
}
return 0;
}
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,31)
static struct v4l2_file_operations fileops_template=
#else
static struct file_operations fileops_template=
#endif
{
owner: THIS_MODULE,
open: vloopback_open,
release: vloopback_release,
read: vloopback_read,
write: vloopback_write,
poll: vloopback_poll,
ioctl: vloopback_ioctl,
#if LINUX_VERSION_CODE > KERNEL_VERSION(2,6,15) && defined(CONFIG_COMPAT)
compat_ioctl: v4l_compat_ioctl32,
#endif
mmap: vloopback_mmap,
};