From: Stelian Pop Signed-off-by: Stelian Pop Signed-off-by: Andrew Morton --- 25-akpm/drivers/media/video/meye.c | 201 ++++++++++++++++--------------------- 25-akpm/drivers/media/video/meye.h | 18 +-- 2 files changed, 99 insertions(+), 120 deletions(-) diff -puN drivers/media/video/meye.c~meye-replace-homebrew-queue-with-kfifo drivers/media/video/meye.c --- 25/drivers/media/video/meye.c~meye-replace-homebrew-queue-with-kfifo Thu Nov 4 15:04:32 2004 +++ 25-akpm/drivers/media/video/meye.c Thu Nov 4 15:04:32 2004 @@ -66,68 +66,6 @@ MODULE_PARM_DESC(video_nr, "video device static struct meye meye; /****************************************************************************/ -/* Queue routines */ -/****************************************************************************/ - -/* Inits the queue */ -static inline void meye_initq(struct meye_queue *queue) { - queue->head = queue->tail = 0; - queue->len = 0; - queue->s_lock = SPIN_LOCK_UNLOCKED; - init_waitqueue_head(&queue->proc_list); -} - -/* Pulls an element from the queue */ -static inline int meye_pullq(struct meye_queue *queue) { - int result; - unsigned long flags; - - spin_lock_irqsave(&queue->s_lock, flags); - if (!queue->len) { - spin_unlock_irqrestore(&queue->s_lock, flags); - return -1; - } - result = queue->buf[queue->head]; - queue->head++; - queue->head &= (MEYE_QUEUE_SIZE - 1); - queue->len--; - spin_unlock_irqrestore(&queue->s_lock, flags); - return result; -} - -/* Pushes an element into the queue */ -static inline void meye_pushq(struct meye_queue *queue, int element) { - unsigned long flags; - - spin_lock_irqsave(&queue->s_lock, flags); - if (queue->len == MEYE_QUEUE_SIZE) { - /* remove the first element */ - queue->head++; - queue->head &= (MEYE_QUEUE_SIZE - 1); - queue->len--; - } - queue->buf[queue->tail] = element; - queue->tail++; - queue->tail &= (MEYE_QUEUE_SIZE - 1); - queue->len++; - - spin_unlock_irqrestore(&queue->s_lock, flags); -} - -/* Tests if the queue is empty */ -static inline int meye_emptyq(struct meye_queue *queue, int *elem) { - int result; - unsigned long flags; - - spin_lock_irqsave(&queue->s_lock, flags); - result = (queue->len == 0); - if (!result && elem) - *elem = queue->buf[queue->head]; - spin_unlock_irqrestore(&queue->s_lock, flags); - return result; -} - -/****************************************************************************/ /* Memory allocation routines (stolen from bttv-driver.c) */ /****************************************************************************/ static void *rvmalloc(unsigned long size) { @@ -839,54 +777,54 @@ static void mchip_cont_compression_start /****************************************************************************/ /* Interrupt handling */ /****************************************************************************/ - -static irqreturn_t meye_irq(int irq, void *dev_id, struct pt_regs *regs) { +static irqreturn_t meye_irq(int irq, void *dev_id, struct pt_regs *regs) +{ u32 v; int reqnr; v = mchip_read(MCHIP_MM_INTA); - while (1) { - v = mchip_get_frame(); - if (!(v & MCHIP_MM_FIR_RDY)) - return IRQ_NONE; - switch (meye.mchip_mode) { - - case MCHIP_HIC_MODE_CONT_OUT: - if (!meye_emptyq(&meye.grabq, NULL)) { - int nr = meye_pullq(&meye.grabq); - mchip_cont_read_frame( - v, - meye.grab_fbuffer + gbufsize * nr, - mchip_hsize() * mchip_vsize() * 2); - meye.grab_buffer[nr].state = MEYE_BUF_DONE; - wake_up_interruptible(&meye.grabq.proc_list); - } - break; - - case MCHIP_HIC_MODE_CONT_COMP: - if (!meye_emptyq(&meye.grabq, &reqnr)) { - int size; - size = mchip_comp_read_frame( - v, - meye.grab_fbuffer + gbufsize * reqnr, - gbufsize); - if (size == -1) - break; - reqnr = meye_pullq(&meye.grabq); - meye.grab_buffer[reqnr].size = size; - meye.grab_buffer[reqnr].state = MEYE_BUF_DONE; - wake_up_interruptible(&meye.grabq.proc_list); - } - break; - - default: - /* do not free frame, since it can be a snap */ - return IRQ_NONE; - } /* switch */ - - mchip_free_frame(); + if (meye.mchip_mode != MCHIP_HIC_MODE_CONT_OUT && + meye.mchip_mode != MCHIP_HIC_MODE_CONT_COMP) + return IRQ_NONE; + +again: + v = mchip_get_frame(); + if (!(v & MCHIP_MM_FIR_RDY)) + return IRQ_HANDLED; + + if (meye.mchip_mode == MCHIP_HIC_MODE_CONT_OUT) { + if (kfifo_get(meye.grabq, (unsigned char *)&reqnr, + sizeof(int)) != sizeof(int)) { + mchip_free_frame(); + return IRQ_HANDLED; + } + mchip_cont_read_frame(v, meye.grab_fbuffer + gbufsize * reqnr, + mchip_hsize() * mchip_vsize() * 2); + meye.grab_buffer[reqnr].size = mchip_hsize() * mchip_vsize() * 2; + meye.grab_buffer[reqnr].state = MEYE_BUF_DONE; + kfifo_put(meye.doneq, (unsigned char *)&reqnr, sizeof(int)); + wake_up_interruptible(&meye.proc_list); + } else { + int size; + size = mchip_comp_read_frame(v, meye.grab_temp, gbufsize); + if (size == -1) { + mchip_free_frame(); + goto again; + } + if (kfifo_get(meye.grabq, (unsigned char *)&reqnr, + sizeof(int)) != sizeof(int)) { + mchip_free_frame(); + goto again; + } + memcpy(meye.grab_fbuffer + gbufsize * reqnr, meye.grab_temp, + size); + meye.grab_buffer[reqnr].size = size; + meye.grab_buffer[reqnr].state = MEYE_BUF_DONE; + kfifo_put(meye.doneq, (unsigned char *)&reqnr, sizeof(int)); + wake_up_interruptible(&meye.proc_list); } - return IRQ_HANDLED; + mchip_free_frame(); + goto again; } /****************************************************************************/ @@ -906,9 +844,12 @@ static int meye_open(struct inode *inode return -ENOBUFS; } mchip_hic_stop(); - meye_initq(&meye.grabq); + for (i = 0; i < MEYE_MAX_BUFNBRS; i++) meye.grab_buffer[i].state = MEYE_BUF_UNUSED; + kfifo_reset(meye.grabq); + kfifo_reset(meye.doneq); + return 0; } @@ -983,6 +924,7 @@ static int meye_do_ioctl(struct inode *i case VIDIOCSYNC: { int *i = arg; + int unused; if (*i < 0 || *i >= gbuffers) return -EINVAL; @@ -992,12 +934,13 @@ static int meye_do_ioctl(struct inode *i case MEYE_BUF_UNUSED: return -EINVAL; case MEYE_BUF_USING: - if (wait_event_interruptible(meye.grabq.proc_list, + if (wait_event_interruptible(meye.proc_list, (meye.grab_buffer[*i].state != MEYE_BUF_USING))) return -EINTR; /* fall through */ case MEYE_BUF_DONE: meye.grab_buffer[*i].state = MEYE_BUF_UNUSED; + kfifo_get(meye.doneq, (unsigned char *)&unused, sizeof(int)); } break; } @@ -1038,7 +981,7 @@ static int meye_do_ioctl(struct inode *i if (restart || meye.mchip_mode != MCHIP_HIC_MODE_CONT_OUT) mchip_continuous_start(); meye.grab_buffer[vm->frame].state = MEYE_BUF_USING; - meye_pushq(&meye.grabq, vm->frame); + kfifo_put(meye.grabq, (unsigned char *)&vm->frame, sizeof(int)); up(&meye.lock); break; } @@ -1104,13 +1047,14 @@ static int meye_do_ioctl(struct inode *i if (meye.mchip_mode != MCHIP_HIC_MODE_CONT_COMP) mchip_cont_compression_start(); meye.grab_buffer[*nb].state = MEYE_BUF_USING; - meye_pushq(&meye.grabq, *nb); + kfifo_put(meye.grabq, (unsigned char *)nb, sizeof(int)); up(&meye.lock); break; } case MEYEIOC_SYNC: { int *i = arg; + int unused; if (*i < 0 || *i >= gbuffers) return -EINVAL; @@ -1120,12 +1064,13 @@ static int meye_do_ioctl(struct inode *i case MEYE_BUF_UNUSED: return -EINVAL; case MEYE_BUF_USING: - if (wait_event_interruptible(meye.grabq.proc_list, + if (wait_event_interruptible(meye.proc_list, (meye.grab_buffer[*i].state != MEYE_BUF_USING))) return -EINTR; /* fall through */ case MEYE_BUF_DONE: meye.grab_buffer[*i].state = MEYE_BUF_UNUSED; + kfifo_get(meye.doneq, (unsigned char *)&unused, sizeof(int)); } *i = meye.grab_buffer[*i].size; break; @@ -1290,6 +1235,29 @@ static int __devinit meye_probe(struct p ret = -EBUSY; goto out1; } + + ret = -ENOMEM; + meye.grab_temp = vmalloc(MCHIP_NB_PAGES_MJPEG * PAGE_SIZE); + if (!meye.grab_temp) { + printk(KERN_ERR "meye: grab buffer allocation failed\n"); + goto outvmalloc; + } + + meye.grabq_lock = SPIN_LOCK_UNLOCKED; + meye.grabq = kfifo_alloc(sizeof(int) * MEYE_MAX_BUFNBRS, GFP_KERNEL, + &meye.grabq_lock); + if (IS_ERR(meye.grabq)) { + printk(KERN_ERR "meye: fifo allocation failed\n"); + goto outkfifoalloc1; + } + meye.doneq_lock = SPIN_LOCK_UNLOCKED; + meye.doneq = kfifo_alloc(sizeof(int) * MEYE_MAX_BUFNBRS, GFP_KERNEL, + &meye.doneq_lock); + if (IS_ERR(meye.doneq)) { + printk(KERN_ERR "meye: fifo allocation failed\n"); + goto outkfifoalloc2; + } + memcpy(meye.video_dev, &meye_template, sizeof(meye_template)); meye.video_dev->dev = &meye.mchip_dev->dev; @@ -1365,6 +1333,7 @@ static int __devinit meye_probe(struct p /* init all fields */ init_MUTEX(&meye.lock); + init_waitqueue_head(&meye.proc_list); meye.picture.depth = 2; meye.picture.palette = VIDEO_PALETTE_YUV422; @@ -1402,6 +1371,13 @@ out2: meye.video_dev = NULL; sonypi_camera_command(SONYPI_COMMAND_SETCAMERA, 0); + kfifo_free(meye.doneq); +outkfifoalloc2: + kfifo_free(meye.grabq); +outkfifoalloc1: + vfree(meye.grab_temp); +outvmalloc: + video_device_release(meye.video_dev); out1: return ret; } @@ -1431,6 +1407,11 @@ static void __devexit meye_remove(struct sonypi_camera_command(SONYPI_COMMAND_SETCAMERA, 0); + kfifo_free(meye.doneq); + kfifo_free(meye.grabq); + + vfree(meye.grab_temp); + printk(KERN_INFO "meye: removed\n"); } diff -puN drivers/media/video/meye.h~meye-replace-homebrew-queue-with-kfifo drivers/media/video/meye.h --- 25/drivers/media/video/meye.h~meye-replace-homebrew-queue-with-kfifo Thu Nov 4 15:04:32 2004 +++ 25-akpm/drivers/media/video/meye.h Thu Nov 4 15:04:32 2004 @@ -39,6 +39,7 @@ #include #include #include +#include /****************************************************************************/ /* Motion JPEG chip registers */ @@ -280,16 +281,8 @@ struct meye_grab_buffer { unsigned long size; /* size of jpg frame */ }; -/* queues containing the buffer indices */ +/* size of kfifos containings buffer indices */ #define MEYE_QUEUE_SIZE MEYE_MAX_BUFNBRS -struct meye_queue { - unsigned int head; /* queue head */ - unsigned int tail; /* queue tail */ - unsigned int len; /* queue length */ - spinlock_t s_lock; /* spinlock protecting the queue */ - wait_queue_head_t proc_list; /* wait queue */ - int buf[MEYE_QUEUE_SIZE]; /* queue contents */ -}; /* Motion Eye device structure */ struct meye { @@ -306,13 +299,18 @@ struct meye { dma_addr_t mchip_dmahandle; /* mchip: dma handle to ptable toc */ unsigned char *grab_fbuffer; /* capture framebuffer */ + unsigned char *grab_temp; /* temporary buffer */ /* list of buffers */ struct meye_grab_buffer grab_buffer[MEYE_MAX_BUFNBRS]; /* other */ struct semaphore lock; /* semaphore for open/mmap... */ - struct meye_queue grabq; /* queue for buffers to be grabbed */ + struct kfifo *grabq; /* queue for buffers to be grabbed */ + spinlock_t grabq_lock; /* lock protecting the queue */ + struct kfifo *doneq; /* queue for grabbed buffers */ + spinlock_t doneq_lock; /* lock protecting the queue */ + wait_queue_head_t proc_list; /* wait queue */ struct video_device *video_dev; /* video device parameters */ struct video_picture picture; /* video picture parameters */ _