zc030x_mm.c

AVR单片机视频采集,主要是为了在自主导航的微型机器人里使用。 获取图像后

/* Implementation file for memory management */
/* Zc030x        -- Driver --         Zc030x */
/* This file is under GPL                    */
/* Copyright :
         Tommy
         Martin     Braun
         All others from other drivers
         Cyril      Russo
---------------------------------------*/

/* Include kernel headers */
#include <linux/slab.h>
#include <linux/vmalloc.h>
/* Include declaration */
#include "zc030x_mm.h"


/* Given PGD from the address space's page table, return the kernel
   virtual mapping of the physical memory mapped at ADR.           */
unsigned long uvirt_to_kva(pgd_t *pgd, unsigned long adr)
{
	unsigned long ret = 0UL;
	pmd_t *pmd;
	pte_t *ptep, pte;

	if (!pgd_none(*pgd)) {
		pmd = pmd_offset(pgd, adr);
		if (!pmd_none(*pmd)) {
			/* WOLT kernels appear to have changed pte_offset to pte_offset_kernel */
#ifdef pte_offset		
			ptep = pte_offset(pmd, adr);
#else /* pte_offset */
			ptep = pte_offset_kernel(pmd, adr);
#endif /* pte_offset */
			pte = *ptep;
			if (pte_present(pte)) {
				ret = (unsigned long) page_address(pte_page(pte));
				ret |= (adr & (PAGE_SIZE - 1));
			}
		}
	}

	return ret;
}

/* Return the physical address of a kernel virtual mapping */
#if LINUX_VERSION_CODE > KERNEL_VERSION(2,5,0)
unsigned long kvirt_to_pa(unsigned long adr)
{
	unsigned long kva, ret;

	kva = (unsigned long) page_address(vmalloc_to_page((void *) adr));
	kva |= adr & (PAGE_SIZE - 1);
	ret = __pa(kva);
	return ret;
}
#else 
unsigned long kvirt_to_pa(unsigned long adr)
{
	unsigned long va, kva, ret;

	va = VMALLOC_VMADDR(adr);
	kva = uvirt_to_kva(pgd_offset_k(va), va);
	ret = __pa(kva);
	return ret;
}
#endif


/* Alloc memory */ 
void * rvmalloc(unsigned long size)
{
    /* Return object */ 
	void *        mem;
    /* Temporary address */
	unsigned long adr;

    /* Need to align the size to the page size */
	size = PAGE_ALIGN(size);
    /* And alloc memory now */
	mem = vmalloc_32(size);
    /* Check allocation */
	if (!mem)	return NULL;

    /* Clear the allocated area, to avoid user's ghost of kernel space */
	memset(mem, 0, size); 
    /* Now reserve the page in the kernel table */
	adr = (unsigned long) mem;
    /* Debug this to track memory leak (if any) */
    PDEBUG(3, "[ZCMEM] Allocated %ld bytes at 0x%p", size, mem);
    /* Need to reserve it, page by page */
	while ((long) size > 0) 
    {
		SetPageReserved(vmalloc_to_page((void *)adr));
		adr += PAGE_SIZE;
		size -= PAGE_SIZE;
	}
	/* Okay, return */
	return mem;
}


/* Free memory */
void rvfree(void *mem, unsigned long size)
{
	/* Temporary address */
    unsigned long adr;

    /* Check argument */
	if (!mem)	return;

    /* Cancel kernel page reservation */
	adr = (unsigned long) mem;
    /* Debug this to track memory leaks */
    PDEBUG(3, "[ZCMEM] Freed %ld bytes at 0x%p", size, mem);
    /* Need to cancel it, page by page */
	while ((long) size > 0) 
    {
		ClearPageReserved(vmalloc_to_page((void *)adr));
		adr += PAGE_SIZE;
		size -= PAGE_SIZE;
	}
    /* Free the memory now */
	vfree(mem);
}

/* Allocate isochronous urbs */
int zc030x_allocate_urbs(struct usb_zc030x *dev)
{
    /* Iterator */
    int i;
    /* Temporary pointer */
    void *kbuf;
    /* URB pointer */
    struct urb *urb = NULL;
    /* Check argument */
    if (dev == NULL)    return -ENXIO;

    /* Lock the device */
    down(&dev->buf_lock);
    
    /* Allocate Isochronuous pipe buffers */
    for (i = 0; i < MAX_ISO_BUFS; i++) 
    {
        PDEBUG(4, "ALLOCURB Submitted sbuf[%d].data = %p .urb = %p", i, dev->sbuf[i].data, dev->sbuf[i].urb);
        if (dev->sbuf[i].data == NULL)
        {
            /* Allocate data buffer */	
            kbuf = kmalloc (ISO_BUFFER_SIZE, GFP_KERNEL);
            if (kbuf == NULL)
            {
                PDEBUG (1, "Failed to allocate data for iso buffer %d.", i);
                up(&dev->buf_lock);
                return -ENOMEM;
            }
            
            PDEBUG (4, "Allocated iso buffer at %p.", kbuf);
            /* Clear it */
            memset (kbuf, 0, ISO_BUFFER_SIZE);
            /* Store it */
            dev->sbuf[i].data = kbuf;
            
        } 
        
        if (dev->sbuf[i].urb == NULL)
        {
            /* Allocate urb */
        #if LINUX_VERSION_CODE < KERNEL_VERSION(2,6,0) 
              urb = usb_alloc_urb(ISO_FRAMES_PER_DESC);
        #else
              urb = usb_alloc_urb(ISO_FRAMES_PER_DESC, GFP_KERNEL);	  
        #endif
            if (urb == NULL)
            {
                PDEBUG (1, "Failed to allocate urb for iso buffer %d", i);
                up(&dev->buf_lock);
                return -ENOMEM;
            }
            
            PDEBUG (4, "Allocated URB at %p.", urb);
            /* Store it */
            dev->sbuf[i].urb = urb;
        }
    }

    /* Unlock the buffer */
    up(&dev->buf_lock);
    
    /* Okay return */
    PDEBUG (4, "<< zc030x_allocate_urbs.\n");
    return 0;
}


/* Allocate needed buffers */
int zc030x_allocate_buffers (struct usb_zc030x *dev)
{
    /* Iterator */
    int i = 0;
    /* Check argument */
    if (dev == NULL)    return -ENXIO;

    /*  Debug this function */
    PDEBUG (4, ">> zc030x_allocate_buffers(dev = 0x%p)\n", dev);
    
    /* Zero this object */
    memset(dev->sbuf, 0, sizeof(dev->sbuf[0]) * MAX_ISO_BUFS);
  
    /* Allocate Isochronuous pipe buffers */
    if (zc030x_allocate_urbs(dev) < 0)
        return -ENXIO;

    /* Lock the device */
    down(&dev->buf_lock);
    
    /* Allocate video buffer space */
    dev->fbuf = rvmalloc(ZC030X_NUMFRAMES * MAX_VIDEODATA_SIZE);
    if (!dev->fbuf) 
    {
        PDEBUG (1, "Failed to allocate video buffer %d.\n", i);
        goto error; //NO MEMORY
    }
    
    /* Allocate picture buffer */
    dev->mmbuf = rvmalloc(MAX_VIDEODATA_SIZE);
    if (!dev->mmbuf) 
    {
        PDEBUG (1, "Failed to allocate mmap buffer %d.\n", i);
        goto error; //NO MEMORY
    }
    
    /* Allocate temporary buffer for jpeg decoder */
    dev->tmpBuffer = rvmalloc(MAX_VIDEODATA_SIZE);
    if(!dev->tmpBuffer)
        goto error; //NO MEMORY
    
    /* Init frame structure */
    for (i = 0; i < ZC030X_NUMFRAMES; i++) 
    {
        init_MUTEX (&dev->frame[i].Lock);

        /* Lock the frame now */
        down(&dev->frame[i].Lock);
        dev->frame[i].GrabState = FRAME_UNUSED;
        dev->frame[i].ScanState = STATE_SCANNING;
        dev->frame[i].Data      = dev->fbuf + i * MAX_VIDEODATA_SIZE; 
        dev->frame[i].CompData  = dev->tmpBuffer + (i * MAX_VIDEODATA_SIZE) / ZC030X_NUMFRAMES;
        dev->frame[i].CompLen   = 0;
        /* Unlock it */
        up(&dev->frame[i].Lock);
        PDEBUG(4, "frame[%d] @ (%p,%p)", i, dev->frame[i].Data, dev->frame[i].CompData);
    }
    
    /* Set the state */
    dev->buf_state = BUF_ALLOCATED;
    /* Release this object */
    up(&dev->buf_lock);
    /* And return */
    PDEBUG (4, "<< zc030x_allocate_buffers.\n");
    return 0;

/* Error handling */    
error:
    
    /* Free created buffer if any */
    for (i = 0; i < MAX_ISO_BUFS; i++) 
    {
        if (dev->sbuf[i].data) 
        {
          kfree(dev->sbuf[i].data);
          dev->sbuf[i].data = NULL;
        }
        if (dev->sbuf[i].urb) 
        {
          usb_free_urb(dev->sbuf[i].urb);
          dev->sbuf[i].urb = NULL;
        }
    }
    
    /* Free all other buffers */
    if (dev->fbuf) 
    {
        rvfree(dev->fbuf, ZC030X_NUMFRAMES * MAX_VIDEODATA_SIZE);
        dev->fbuf = NULL;
    }
    if (dev->tmpBuffer) 
    {
        rvfree(dev->tmpBuffer,MAX_VIDEODATA_SIZE);
        dev->tmpBuffer = NULL;
    }
    
    /* Set the state */
    dev->buf_state = BUF_NOT_ALLOCATED;
    /* And release the lock */
    up(&dev->buf_lock);
    PDEBUG(1, "Error : Not enough memory");
    return -ENOMEM;
}

/* Free used buffers */
int zc030x_unallocate_buffers(struct usb_zc030x *dev)
{
    /* Iterator */
    int i;
    
    PDEBUG(3,"zc030x_unallocate_buffers()");

    /* Check arguments */
    if (dev == NULL)
        return -ENXIO;

    /* Lock the device */
    down(&dev->buf_lock);
    
    /* Free isochronous buffers */
    for (i = 0; i < MAX_ISO_BUFS; i++) 
    {
        if (dev->sbuf[i].data != NULL) 
        {
            kfree(dev->sbuf[i].data);
            dev->sbuf[i].data = NULL;
        }
        if (dev->sbuf[i].urb != NULL) 
        {
            usb_free_urb(dev->sbuf[i].urb);
            dev->sbuf[i].urb = NULL;
        }
    }
    
    /* Free V4L buffer */
    if (dev->fbuf) 
    {
        rvfree(dev->fbuf, ZC030X_NUMFRAMES * MAX_VIDEODATA_SIZE);
        dev->fbuf = NULL;
    }

    /* Free tmp buffer */
    if (dev->tmpBuffer) 
    {
        rvfree(dev->tmpBuffer,MAX_VIDEODATA_SIZE);
        dev->tmpBuffer = NULL;
    }
    
    /* Change the state */
    dev->buf_state = BUF_NOT_ALLOCATED;

    /* Okay, quit */
    up(&dev->buf_lock);
    return 0;
}