/**

* @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;

}