Skip to content
Permalink
master
Switch branches/tags
Go to file
 
 
Cannot retrieve contributors at this time
executable file 684 lines (530 sloc) 15.4 KB
/**
* @file linect-buf.c
* @author Arturo Casal
* @date 2010
* @version v0.1
*
* @brief Driver for MS Kinect
*
* @note Copyright (C) Arturo Casal
*
* @par Licences
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* any later version.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with this program; if not, write to the Free Software
* Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
*
*/
#include <linux/module.h>
#include <linux/init.h>
#include <linux/kernel.h>
#include <linux/version.h>
#include <linux/errno.h>
#include <linux/slab.h>
#include <linux/kref.h>
#include <linux/vmalloc.h>
#include <linux/mm.h>
#include <linux/usb.h>
#include <media/v4l2-common.h>
#include <media/v4l2-ioctl.h>
#include "linect.h"
/**
* @var default_nbrframebuf
* Number of frame buffer by default
*/
static int default_nbrframebuf = 3;
/**
* @param size Size of memory
*
* @returns Address on the allocated memory
*
* @brief Allocate a buffer.
*
* This function permits to allocate a buffer in memory.
*/
void * linect_rvmalloc(unsigned long size)
{
void *mem;
unsigned long addr;
size = PAGE_ALIGN(size);
mem = vmalloc(size);
if (!mem)
return NULL;
memset(mem, 0, size);
addr = (unsigned long) mem;
while (size > 0) {
SetPageReserved(vmalloc_to_page((void *) addr));
addr += PAGE_SIZE;
size -= PAGE_SIZE;
}
return mem;
}
/**
* @param mem Memory address
* @param size Size of allocated memory
*
* @brief Free a buffer
*
* This function permits to free a buffer.
*/
void linect_rvfree(void *mem, unsigned long size)
{
unsigned long addr;
if (!mem)
return;
addr = (unsigned long) mem;
while ((long) size > 0) {
ClearPageReserved(vmalloc_to_page((void *) addr));
addr += PAGE_SIZE;
size -= PAGE_SIZE;
}
vfree(mem);
}
/**
* @param dev Device structure
*
* @returns 0 if all is OK
*
* @brief Allocate all ISOC buffers.
*
* This function permits to reserved the memory for each ISOC buffer.
*/
int linect_allocate_rgb_buffers(struct usb_linect *dev)
{
int i;
void *kbuf;
LNT_DEBUG("Allocate video buffers\n");
if (dev == NULL)
return -ENXIO;
// Allocate frame buffer structure
if (dev->cam->framebuf == NULL) {
kbuf = kzalloc(default_nbrframebuf * sizeof(struct linect_frame_buf), GFP_KERNEL);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate frame buffer structure\n");
return -ENOMEM;
}
dev->cam->framebuf = kbuf;
}
// Create frame buffers and make circular ring
for (i=0; i<default_nbrframebuf; i++) {
if (dev->cam->framebuf[i].data == NULL) {
kbuf = vmalloc(LNT_FRAME_SIZE);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate frame buffer %d\n", i);
return -ENOMEM;
}
dev->cam->framebuf[i].data = kbuf;
memset(kbuf, 0, LNT_FRAME_SIZE);
}
}
// Allocate image buffer; double buffer for mmap()
kbuf = linect_rvmalloc(dev->cam->nbuffers * dev->cam->len_per_image);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate image buffer(s). needed (%d)\n",
dev->cam->nbuffers * dev->cam->len_per_image);
return -ENOMEM;
}
dev->cam->image_data = kbuf;
for (i = 0; i < dev->cam->nbuffers; i++) {
dev->cam->images[i].offset = i * dev->cam->len_per_image;
dev->cam->images[i].vma_use_count = 0;
}
for (; i < LNT_MAX_IMAGES; i++)
dev->cam->images[i].offset = 0;
kbuf = NULL;
return 0;
}
int linect_allocate_depth_buffers(struct usb_linect *dev)
{
int i;
void *kbuf;
LNT_DEBUG("Allocate video buffers\n");
if (dev == NULL)
return -ENXIO;
// Allocate frame buffer structure
if (dev->cam->framebuf_depth == NULL) {
kbuf = kzalloc(default_nbrframebuf * sizeof(struct linect_frame_buf), GFP_KERNEL);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate frame buffer structure\n");
return -ENOMEM;
}
dev->cam->framebuf_depth = kbuf;
}
// Create frame buffers and make circular ring
for (i=0; i<default_nbrframebuf; i++) {
if (dev->cam->framebuf_depth[i].data == NULL) {
kbuf = vmalloc(LNT_FRAME_SIZE);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate frame buffer %d\n", i);
return -ENOMEM;
}
dev->cam->framebuf_depth[i].data = kbuf;
memset(kbuf, 0, LNT_FRAME_SIZE);
}
}
// Allocate image buffer; double buffer for mmap()
kbuf = linect_rvmalloc(dev->cam->nbuffers_depth * dev->cam->len_per_image_depth);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate image buffer(s). needed (%d)\n",
dev->cam->nbuffers_depth * dev->cam->len_per_image_depth);
return -ENOMEM;
}
dev->cam->image_data_depth = kbuf;
for (i = 0; i < dev->cam->nbuffers_depth; i++) {
dev->cam->images_depth[i].offset = i * dev->cam->len_per_image_depth;
dev->cam->images_depth[i].vma_use_count = 0;
}
for (; i < LNT_MAX_IMAGES; i++)
dev->cam->images_depth[i].offset = 0;
kbuf = NULL;
kbuf = linect_rvmalloc(640*480*2);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate image temp buffer. needed (%d)\n",
640*480*2);
return -ENOMEM;
}
dev->cam->image_tmp = kbuf;
/*kbuf = linect_rvmalloc(4096);
if (kbuf == NULL) {
LNT_ERROR("Failed to allocate depth gamma buffer. needed (%d)\n",
4096);
return -ENOMEM;
}
dev->cam->depth_gamma = kbuf;
// Init gamma
for (i=0; i<2048; i++) {
v = i/2048.0;
v = v*v*v* 6;
dev->cam->depth_gamma[i] = v*6*256;
}*/
return 0;
}
/**
* @param dev Device structure
*
* @returns 0 if all is OK
*
* @brief Reset all ISOC buffers.
*
* This function permits to reset all ISOC buffers.
*/
int linect_reset_rgb_buffers(struct usb_linect *dev)
{
int i;
unsigned long flags;
LNT_DEBUG("Reset all buffers\n");
spin_lock_irqsave(&dev->cam->spinlock_rgb, flags);
dev->cam->full_frames = NULL;
dev->cam->full_frames_tail = NULL;
for (i=0; i<dev->cam->nbuffers; i++) {
dev->cam->framebuf[i].filled = 0;
dev->cam->framebuf[i].errors = 0;
if (i > 0)
dev->cam->framebuf[i].next = &dev->cam->framebuf[i - 1];
else
dev->cam->framebuf->next = NULL;
}
dev->cam->empty_frames = &dev->cam->framebuf[dev->cam->nbuffers - 1];
dev->cam->empty_frames_tail = dev->cam->framebuf;
dev->cam->read_frame = NULL;
dev->cam->fill_frame = dev->cam->empty_frames;
dev->cam->empty_frames = dev->cam->empty_frames->next;
dev->cam->image_read_pos = 0;
dev->cam->fill_image = 0;
spin_unlock_irqrestore(&dev->cam->spinlock_rgb, flags);
for (i=0; i<dev->cam->nbuffers; i++)
dev->cam->image_used[i] = 0;
return 0;
}
int linect_reset_depth_buffers(struct usb_linect *dev)
{
int i;
unsigned long flags;
LNT_DEBUG("Reset all buffers\n");
spin_lock_irqsave(&dev->cam->spinlock_depth, flags);
dev->cam->full_frames_depth = NULL;
dev->cam->full_frames_tail_depth = NULL;
for (i=0; i<dev->cam->nbuffers_depth; i++) {
dev->cam->framebuf_depth[i].filled = 0;
dev->cam->framebuf_depth[i].errors = 0;
if (i > 0)
dev->cam->framebuf_depth[i].next = &dev->cam->framebuf_depth[i - 1];
else
dev->cam->framebuf_depth->next = NULL;
}
dev->cam->empty_frames_depth = &dev->cam->framebuf_depth[dev->cam->nbuffers_depth - 1];
dev->cam->empty_frames_tail_depth = dev->cam->framebuf_depth;
dev->cam->read_frame_depth = NULL;
dev->cam->fill_frame_depth = dev->cam->empty_frames_depth;
dev->cam->empty_frames_depth = dev->cam->empty_frames_depth->next;
dev->cam->image_read_pos_depth = 0;
dev->cam->fill_image_depth = 0;
spin_unlock_irqrestore(&dev->cam->spinlock_depth, flags);
for (i=0; i<dev->cam->nbuffers_depth; i++)
dev->cam->image_used_depth[i] = 0;
return 0;
}
/**
* @param dev Device structure
*
* @returns 0 if all is OK
*
* @brief Clear current buffers.
*
* This function permits to clear the memory.
*/
int linect_clear_rgb_buffers(struct usb_linect *dev)
{
memset(dev->cam->image_data, 0x00, dev->cam->nbuffers * dev->cam->len_per_image);
return 0;
}
int linect_clear_depth_buffers(struct usb_linect *dev)
{
memset(dev->cam->image_data_depth, 0x00, dev->cam->nbuffers_depth * dev->cam->len_per_image_depth);
return 0;
}
/**
* @param dev Device structure
*
* @returns 0 if all is OK
*
* @brief Release all buffers.
*
* This function permits to release and free the memory for each ISOC buffer.
*/
int linect_free_rgb_buffers(struct usb_linect *dev)
{
int i;
LNT_DEBUG("Free buffers\n");
if (dev == NULL)
return -1;
// Release frame buffers
if (dev->cam->framebuf != NULL) {
for (i=0; i<default_nbrframebuf; i++) {
if (dev->cam->framebuf[i].data != NULL) {
vfree(dev->cam->framebuf[i].data);
dev->cam->framebuf[i].data = NULL;
}
}
kfree(dev->cam->framebuf);
dev->cam->framebuf = NULL;
}
// Release image buffers
if (dev->cam->image_data != NULL)
linect_rvfree(dev->cam->image_data, dev->cam->nbuffers * dev->cam->len_per_image);
dev->cam->image_data = NULL;
return 0;
}
int linect_free_depth_buffers(struct usb_linect *dev)
{
int i;
LNT_DEBUG("Free buffers\n");
if (dev == NULL)
return -1;
// Release frame buffers
if (dev->cam->framebuf_depth != NULL) {
for (i=0; i<default_nbrframebuf; i++) {
if (dev->cam->framebuf_depth[i].data != NULL) {
vfree(dev->cam->framebuf_depth[i].data);
dev->cam->framebuf_depth[i].data = NULL;
}
}
kfree(dev->cam->framebuf_depth);
dev->cam->framebuf_depth = NULL;
}
// Release image buffers
if (dev->cam->image_data_depth != NULL)
linect_rvfree(dev->cam->image_data_depth, dev->cam->nbuffers_depth * dev->cam->len_per_image_depth);
dev->cam->image_data_depth = NULL;
if (dev->cam->image_tmp != NULL)
linect_rvfree(dev->cam->image_tmp, 640*480*2);
dev->cam->image_tmp = NULL;
/*if (dev->cam->depth_gamma != NULL)
linect_rvfree(dev->cam->depth_gamma, 4096);
dev->cam->depth_gamma = NULL;*/
return 0;
}
/**
* @param dev Device structure
*
* @brief Prepare the next image.
*
* This function is called when an image is ready, so as to prepare the next image.
*/
void linect_next_rgb_image(struct usb_linect *dev)
{
dev->cam->image_used[dev->cam->fill_image] = 0;
dev->cam->fill_image = (dev->cam->fill_image + 1) % dev->cam->nbuffers;
}
void linect_next_depth_image(struct usb_linect *dev)
{
dev->cam->image_used_depth[dev->cam->fill_image_depth] = 0;
dev->cam->fill_image_depth = (dev->cam->fill_image_depth + 1) % dev->cam->nbuffers;
}
/**
* @param dev Device structure
*
* @returns 0 if all is OK
*
* @brief Prepare the next frame.
*
* This function is called when a frame is ready, so as to prepare the next frame.
*/
int linect_next_rgb_frame(struct usb_linect *dev)
{
int ret = 0;
unsigned long flags;
//LNT_DEBUG("Select next frame\n");
spin_lock_irqsave(&dev->cam->spinlock_rgb, flags);
if (dev->cam->fill_frame != NULL) {
if (dev->cam->full_frames == NULL) {
dev->cam->full_frames = dev->cam->fill_frame;
dev->cam->full_frames_tail = dev->cam->full_frames;
}
else {
dev->cam->full_frames_tail->next = dev->cam->fill_frame;
dev->cam->full_frames_tail = dev->cam->fill_frame;
}
}
if (dev->cam->empty_frames != NULL) {
dev->cam->fill_frame = dev->cam->empty_frames;
dev->cam->empty_frames = dev->cam->empty_frames->next;
}
else {
if (dev->cam->full_frames == NULL) {
LNT_ERROR("Neither empty or full frames available!\n");
spin_unlock_irqrestore(&dev->cam->spinlock_rgb, flags);
return -EINVAL;
}
dev->cam->fill_frame = dev->cam->full_frames;
dev->cam->full_frames = dev->cam->full_frames->next;
ret = 1;
}
dev->cam->fill_frame->next = NULL;
spin_unlock_irqrestore(&dev->cam->spinlock_rgb, flags);
return ret;
}
int linect_next_depth_frame(struct usb_linect *dev)
{
int ret = 0;
unsigned long flags;
//LNT_DEBUG("Select next frame\n");
spin_lock_irqsave(&dev->cam->spinlock_depth, flags);
if (dev->cam->fill_frame_depth != NULL) {
if (dev->cam->full_frames_depth == NULL) {
dev->cam->full_frames_depth = dev->cam->fill_frame_depth;
dev->cam->full_frames_tail_depth = dev->cam->full_frames_depth;
}
else {
dev->cam->full_frames_tail_depth->next = dev->cam->fill_frame_depth;
dev->cam->full_frames_tail_depth = dev->cam->fill_frame_depth;
}
}
if (dev->cam->empty_frames_depth != NULL) {
dev->cam->fill_frame_depth = dev->cam->empty_frames_depth;
dev->cam->empty_frames_depth = dev->cam->empty_frames_depth->next;
}
else {
if (dev->cam->full_frames_depth == NULL) {
LNT_ERROR("Neither empty or full frames available!\n");
spin_unlock_irqrestore(&dev->cam->spinlock_depth, flags);
return -EINVAL;
}
dev->cam->fill_frame_depth = dev->cam->full_frames_depth;
dev->cam->full_frames_depth = dev->cam->full_frames_depth->next;
ret = 1;
}
dev->cam->fill_frame_depth->next = NULL;
spin_unlock_irqrestore(&dev->cam->spinlock_depth, flags);
return ret;
}
/**
* @param dev Device structure
*
* @returns 0 if all is OK
*
* @brief Handler frame
*
* This function gets called for the isochronous pipe. This function is only called
* when a frame is ready. So we have to be fast to decompress the data.
*/
int linect_handle_rgb_frame(struct usb_linect *dev)
{
int ret = 0;
unsigned long flags;
LNT_DEBUG("Sync Handle Frame\n");
spin_lock_irqsave(&dev->cam->spinlock_rgb, flags);
if (dev->cam->read_frame != NULL) {
spin_unlock_irqrestore(&dev->cam->spinlock_rgb, flags);
return ret;
}
if (dev->cam->full_frames == NULL) {
}
else {
dev->cam->read_frame = dev->cam->full_frames;
dev->cam->full_frames = dev->cam->full_frames->next;
dev->cam->read_frame->next = NULL;
}
if (dev->cam->read_frame != NULL) {
spin_unlock_irqrestore(&dev->cam->spinlock_rgb, flags);
ret = linect_rgb_decompress(dev);
spin_lock_irqsave(&dev->cam->spinlock_rgb, flags);
if (dev->cam->empty_frames == NULL) {
dev->cam->empty_frames = dev->cam->read_frame;
dev->cam->empty_frames_tail = dev->cam->empty_frames;
}
else {
dev->cam->empty_frames_tail->next = dev->cam->read_frame;
dev->cam->empty_frames_tail = dev->cam->read_frame;
}
dev->cam->read_frame = NULL;
}
spin_unlock_irqrestore(&dev->cam->spinlock_rgb, flags);
return ret;
}
int linect_handle_depth_frame(struct usb_linect *dev)
{
int ret = 0;
unsigned long flags;
LNT_DEBUG("Sync Handle Frame\n");
spin_lock_irqsave(&dev->cam->spinlock_depth, flags);
if (dev->cam->read_frame_depth != NULL) {
spin_unlock_irqrestore(&dev->cam->spinlock_depth, flags);
return ret;
}
if (dev->cam->full_frames_depth == NULL) {
}
else {
dev->cam->read_frame_depth = dev->cam->full_frames_depth;
dev->cam->full_frames_depth = dev->cam->full_frames_depth->next;
dev->cam->read_frame_depth->next = NULL;
}
if (dev->cam->read_frame_depth != NULL) {
spin_unlock_irqrestore(&dev->cam->spinlock_depth, flags);
ret = linect_depth_decompress(dev);
//ret = linect_decompress(dev);
//LNT_DEBUG("descomprime frame depth\n");
spin_lock_irqsave(&dev->cam->spinlock_depth, flags);
if (dev->cam->empty_frames_depth == NULL) {
dev->cam->empty_frames_depth = dev->cam->read_frame_depth;
dev->cam->empty_frames_tail_depth = dev->cam->empty_frames_depth;
}
else {
dev->cam->empty_frames_tail_depth->next = dev->cam->read_frame_depth;
dev->cam->empty_frames_tail_depth = dev->cam->read_frame_depth;
}
dev->cam->read_frame_depth = NULL;
}
spin_unlock_irqrestore(&dev->cam->spinlock_depth, flags);
return ret;
}