@@ -5,3 +5,9 @@ config FDIF
help
The FDIF is a face detection module, which can be integrated into
some SoCs to detect the location of faces in one image or video.
+
+config FDIF_OMAP4
+ depends on FDIF
+ tristate "OMAP4 Face Detection module"
+ help
+ OMAP4 face detection support
@@ -1 +1,2 @@
obj-$(CONFIG_FDIF) += fdif.o
+obj-$(CONFIG_FDIF_OMAP4) += fdif_omap4.o
new file mode 100644
@@ -0,0 +1,663 @@
+/*
+ * fdif_omap4.c -- face detection module driver
+ *
+ * Copyright (C) 2011 Ming Lei (ming.lei@canonical.com)
+ *
+ * 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
+ * (at your option) 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., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ */
+
+/*****************************************************************************/
+#include <linux/init.h>
+#include <linux/fs.h>
+#include <linux/mm.h>
+#include <linux/slab.h>
+#include <linux/signal.h>
+#include <linux/wait.h>
+#include <linux/poll.h>
+#include <linux/module.h>
+#include <linux/pm_runtime.h>
+#include <linux/delay.h>
+#include <linux/user_namespace.h>
+#include <linux/platform_device.h>
+#include <linux/interrupt.h>
+#include <linux/dma-mapping.h>
+#include "fdif.h"
+#include <asm/uaccess.h>
+#include <asm/byteorder.h>
+#include <asm/io.h>
+
+#undef DEBUG
+
+#define PICT_SIZE_X 320
+#define PICT_SIZE_Y 240
+
+#define WORK_MEM_SIZE (52*1024)
+
+/* 9.5 FDIF Register Manua of TI OMAP4 TRM */
+#define FDIF_REVISION 0x0
+#define FDIF_HWINFO 0x4
+#define FDIF_SYSCONFIG 0x10
+#define SOFTRESET (1 << 0)
+
+#define FDIF_IRQSTATUS_RAW_j (0x24 + 2*0x10)
+#define FDIF_IRQSTATUS_j (0x28 + 2*0x10)
+#define FDIF_IRQENABLE_SET_j (0x2c + 2*0x10)
+#define FDIF_IRQENABLE_CLR_j (0x30 + 2*0x10)
+#define FINISH_IRQ (1 << 8)
+#define ERR_IRQ (1 << 0)
+
+#define FDIF_PICADDR 0x60
+#define FDIF_CTRL 0x64
+#define CTRL_MAX_TAGS 0x0A
+
+#define FDIF_WKADDR 0x68
+#define FD_CTRL 0x80
+#define CTRL_FINISH (1 << 2)
+#define CTRL_RUN (1 << 1)
+#define CTRL_SRST (1 << 0)
+
+
+#define FD_DNUM 0x84
+#define FD_DCOND 0x88
+#define FD_STARTX 0x8c
+#define FD_STARTY 0x90
+#define FD_SIZEX 0x94
+#define FD_SIZEY 0x98
+#define FD_LHIT 0x9c
+#define FD_CENTERX_i 0x160
+#define FD_CENTERY_i 0x164
+#define FD_CONFSIZE_i 0x168
+#define FD_ANGLE_i 0x16c
+
+static inline void fd_writel(void __iomem *base, u32 reg, u32 val)
+{
+ __raw_writel(val, base + reg);
+}
+
+static inline u32 fd_readl(void __iomem *base, u32 reg)
+{
+ return __raw_readl(base + reg);
+}
+
+struct fdif_qvga {
+ struct fdif_dev *dev;
+
+ /*should be removed*/
+ struct platform_device *pdev;
+ int irq;
+ void __iomem *base;
+
+ void *work_mem_addr;
+ dma_addr_t work_dma;
+ dma_addr_t pict_dma;
+ unsigned long pict_mem_len;
+
+ struct fdif_buffer *pending;
+ spinlock_t lock;
+};
+
+struct fdif_fmt qvga_fmt[] = {
+ {
+ .name = "8 Greyscale",
+ .fourcc = V4L2_PIX_FMT_GREY,
+ .depth = 8,
+ .width = PICT_SIZE_X,
+ .height = PICT_SIZE_Y,
+ },
+};
+
+
+#ifdef DEBUG
+static void dump_fdif_setting(struct fdif_qvga *fdif, const char *func)
+{
+ printk("%s: %s\n", func, __func__);
+ printk("work mem addr:%8p\n", fdif->work_mem_addr);
+ printk("face size=%2d, face dir=%2d, lhit=%d\n",
+ fdif->dev->s.min_face_size, fdif->dev->s.face_dir,
+ fdif->dev->s.lhit);
+ printk("startx =%4d starty=%4d sizex=%4d sizey=%4d\n",
+ fdif->dev->s.startx, fdif->dev->s.starty,
+ fdif->dev->s.sizex, fdif->dev->s.sizey);
+}
+
+static void dump_fdif_results(struct v4l2_fdif_result *fdif, const char *func)
+{
+ int idx;
+
+ printk("%s: %s\n", func, __func__);
+
+ printk("found %d faces, but index:%d\n", fdif->face_cnt,
+ fdif->index);
+ for(idx=0; idx < fdif->face_cnt; idx++) {
+ struct v4l2_fd_detection *fr = &fdif->faces[idx];
+ printk(" No.%d x=%3d y=%2d sz=%2d ang=%3d conf=%2d\n",
+ idx, fr->face.centerx, fr->face.centery,
+ fr->face.sizex, fr->face.angle,
+ fr->face.confidence);
+ }
+}
+
+static void dump_fdif_regs(struct fdif_qvga *fdif, const char *func)
+{
+ printk("%s:%s\n", __func__, func);
+ printk("FDIF_CTRL=%08x FDIF_SYSCONFIG=%08x\n",
+ fd_readl(fdif->base, FDIF_CTRL),
+ fd_readl(fdif->base, FDIF_SYSCONFIG));
+ printk("FDIF_IRQSTATUS_RAW_j=%08x FDIF_IRQSTATUS_j=%08x\n",
+ fd_readl(fdif->base, FDIF_IRQSTATUS_RAW_j),
+ fd_readl(fdif->base, FDIF_IRQSTATUS_j));
+ printk("FDIF_PICADDR=%08x FDIF_WKADDR=%08x\n",
+ fd_readl(fdif->base, FDIF_PICADDR),
+ fd_readl(fdif->base, FDIF_WKADDR));
+ printk("FD_CTRL=%04x, FDIF_IRQENABLE_SET_j=%04x\n",
+ fd_readl(fdif->base, FD_CTRL),
+ fd_readl(fdif->base, FDIF_IRQENABLE_SET_j));
+}
+
+#else
+static inline void dump_fdif_setting(struct fdif_qvga *fdif, const char *func)
+{
+}
+static inline void dump_fdif_results(struct v4l2_fdif_result *fdif, const char *func)
+{
+}
+static inline void dump_fdif_regs(struct fdif_qvga *fdif, const char *func)
+{
+}
+#endif
+
+static void install_default_setting(struct fdif_qvga *fdif)
+{
+ fdif->dev->s.fmt = &qvga_fmt[0];
+ fdif->dev->s.field = V4L2_FIELD_NONE;
+
+ fdif->dev->s.min_face_size = FACE_SIZE_25_PIXELS;
+ fdif->dev->s.face_dir = FACE_DIR_UP;
+ fdif->dev->s.startx = 0;
+ fdif->dev->s.starty = 0;
+ fdif->dev->s.sizex = 0x140;
+ fdif->dev->s.sizey = 0xf0;
+ fdif->dev->s.lhit = 0x5;
+
+ fdif->dev->s.width = PICT_SIZE_X;
+ fdif->dev->s.height = PICT_SIZE_Y;
+}
+
+static void commit_image_setting(struct fdif_qvga *fdif)
+{
+ unsigned long conf;
+ struct vb2_buffer *vb = &fdif->pending->vb;
+ void *pict_vaddr = vb2_plane_vaddr(vb, 0);
+
+ fdif->pict_mem_len = vb2_plane_size(vb, 0);
+ fdif->pict_dma = dma_map_single(&fdif->pdev->dev,
+ pict_vaddr,
+ fdif->pict_mem_len,
+ DMA_TO_DEVICE);
+
+ fd_writel(fdif->base, FDIF_PICADDR, fdif->pict_dma);
+
+ conf = (fdif->dev->s.min_face_size & 0x3) ||
+ ((fdif->dev->s.face_dir & 0x3) << 2);
+ fd_writel(fdif->base, FD_DCOND, conf);
+
+ fd_writel(fdif->base, FD_STARTX, fdif->dev->s.startx);
+ fd_writel(fdif->base, FD_STARTY, fdif->dev->s.starty);
+ fd_writel(fdif->base, FD_SIZEX, fdif->dev->s.sizex);
+ fd_writel(fdif->base, FD_SIZEY, fdif->dev->s.sizey);
+ fd_writel(fdif->base, FD_LHIT, fdif->dev->s.lhit);
+}
+
+
+/*softreset fdif*/
+static int softreset_fdif(struct fdif_qvga *fdif)
+{
+ unsigned long conf;
+ int to = 0;
+
+ conf = fd_readl(fdif->base, FDIF_SYSCONFIG);
+ conf |= SOFTRESET;
+ fd_writel(fdif->base, FDIF_SYSCONFIG, conf);
+
+ while ((conf & SOFTRESET) && to++ < 2000) {
+ conf = fd_readl(fdif->base, FDIF_SYSCONFIG);
+ udelay(2);
+ }
+
+ if (to == 2000)
+ dev_err(&fdif->pdev->dev, "%s: reset failed\n", __func__);
+
+ return to == 2000;
+}
+
+static void __start_detect(struct fdif_qvga *fdif)
+{
+ unsigned long conf;
+
+ dump_fdif_setting(fdif, __func__);
+
+ commit_image_setting(fdif);
+
+ /*enable finish irq*/
+ conf = FINISH_IRQ;
+ fd_writel(fdif->base, FDIF_IRQENABLE_SET_j, conf);
+
+ /*set RUN flag*/
+ conf = CTRL_RUN;
+ fd_writel(fdif->base, FD_CTRL, conf);
+
+ dump_fdif_regs(fdif, __func__);
+}
+
+static void __stop_detect(struct fdif_qvga *fdif)
+{
+ unsigned long conf;
+
+ dump_fdif_regs(fdif, __func__);
+
+ dma_unmap_single(&fdif->pdev->dev, fdif->pict_dma,
+ fdif->pict_mem_len,
+ DMA_TO_DEVICE);
+ /*disable finish irq*/
+ conf = FINISH_IRQ;
+ fd_writel(fdif->base, FDIF_IRQENABLE_CLR_j, conf);
+
+ /*mark FINISH flag*/
+ conf = CTRL_FINISH;
+ fd_writel(fdif->base, FD_CTRL, conf);
+}
+
+static int read_faces(struct fdif_qvga *fdif, int is_err)
+{
+ int cnt, idx = 0;
+ struct v4l2_fdif_result *v4l2_fr;
+ struct fdif_dev *dev = fdif->dev;
+ struct fdif_buffer *buf;
+ unsigned long flags;
+
+ spin_lock_irqsave(&fdif->lock, flags);
+
+ buf = fdif->pending;
+ if (!buf) {
+ WARN_ON(1);
+ cnt = -EIO;
+ goto out;
+ }
+
+ buf->vb.v4l2_buf.sequence++;
+
+ if (!is_err)
+ cnt = fd_readl(fdif->base, FD_DNUM) & 0x3f;
+ else
+ cnt = 0;
+
+ v4l2_fr = kzalloc(sizeof(*v4l2_fr), GFP_ATOMIC);
+ if (!v4l2_fr) {
+ cnt = -ENOMEM;
+ goto out;
+ }
+ if (cnt)
+ v4l2_fr->faces =
+ kmalloc(sizeof(struct v4l2_fd_detection) * cnt,
+ GFP_ATOMIC);
+
+ if (cnt && !v4l2_fr->faces) {
+ cnt = -ENOMEM;
+ goto out_err;
+ }
+
+ v4l2_fr->face_cnt = cnt;
+ v4l2_fr->index = buf->vb.v4l2_buf.index;
+
+ while(idx < cnt) {
+ struct v4l2_fd_detection *fr = &v4l2_fr->faces[idx];
+
+ fr->flag = V4L2_FD_HAS_FACE;
+
+ fr->face.centerx = fd_readl(fdif->base,
+ FD_CENTERX_i + idx * 0x10) & 0x1ff;
+ fr->face.centery = fd_readl(fdif->base,
+ FD_CENTERY_i + idx * 0x10) & 0xff;
+ fr->face.angle = fd_readl(fdif->base,
+ FD_ANGLE_i + idx * 0x10) & 0x1ff;
+ fr->face.sizex = fd_readl(fdif->base,
+ FD_CONFSIZE_i + idx * 0x10);
+ fr->face.confidence = (fr->face.sizex >> 8) & 0xf;
+ fr->face.sizey = fr->face.sizex = fr->face.sizex & 0xff;
+
+ idx++;
+ }
+
+ __stop_detect(fdif);
+ fdif->pending = NULL;
+ spin_unlock_irqrestore(&fdif->lock, flags);
+
+ dump_fdif_results(v4l2_fr, __func__);
+
+ /*queue the detection result to complete queue*/
+ fdif_add_detection(dev, v4l2_fr);
+
+ if (is_err)
+ vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+ else
+ vb2_buffer_done(&buf->vb, VB2_BUF_STATE_DONE);
+
+ wake_up(&dev->fdif_dq.wq);
+
+ return cnt;
+
+out_err:
+ kfree(v4l2_fr);
+out:
+ spin_unlock_irqrestore(&fdif->lock, flags);
+ return cnt;
+}
+
+static int __submit_detection(struct fdif_qvga *fdif)
+{
+ struct fdif_dev *dev = fdif->dev;
+ struct fdif_buffer *buf;
+ unsigned long flags;
+ unsigned int ret = 0;
+
+ buf = fdif_get_buffer(dev);
+ if (!buf)
+ goto out;
+
+ spin_lock_irqsave(&fdif->lock, flags);
+ if (fdif->pending) {
+ spin_unlock_irqrestore(&fdif->lock, flags);
+ ret = -EBUSY;
+ goto out;
+ }
+ fdif->pending = buf;
+ __start_detect(fdif);
+ spin_unlock_irqrestore(&fdif->lock, flags);
+
+ return 0;
+
+out:
+ return ret;
+}
+
+static irqreturn_t handle_detection(int irq, void *__fdif)
+{
+ unsigned long irqsts;
+ struct fdif_qvga *fdif = __fdif;
+ irqreturn_t ret = IRQ_HANDLED;
+
+ /*clear irq status*/
+ irqsts = fd_readl(fdif->base, FDIF_IRQSTATUS_j);
+
+ if (irqsts & (FINISH_IRQ | ERR_IRQ)) {
+ int is_err = irqsts & ERR_IRQ;
+
+ fd_writel(fdif->base, FDIF_IRQSTATUS_j, irqsts);
+
+ read_faces(fdif, is_err);
+ if (is_err)
+ softreset_fdif(fdif);
+
+ __submit_detection(fdif);
+ } else {
+ ret = IRQ_NONE;
+ }
+
+ return ret;
+}
+
+static void fdif_global_init(struct fdif_qvga *fdif)
+{
+ unsigned long conf;
+ struct device *dev = &fdif->pdev->dev;
+
+ /*softreset fdif*/
+ softreset_fdif(fdif);
+
+ /*set max tags*/
+ conf = fd_readl(fdif->base, FDIF_CTRL);
+ conf &= ~0x1e;
+ conf |= (CTRL_MAX_TAGS << 1);
+ fd_writel(fdif->base, FDIF_CTRL, conf);
+
+ /*enable error irq*/
+ conf = ERR_IRQ;
+ fd_writel(fdif->base, FDIF_IRQENABLE_SET_j, conf);
+
+ fdif->work_dma = dma_map_single(dev,
+ fdif->work_mem_addr,
+ WORK_MEM_SIZE,
+ DMA_TO_DEVICE);
+ fd_writel(fdif->base, FDIF_WKADDR, fdif->work_dma);
+}
+
+static void fdif_global_deinit(struct fdif_qvga *fdif)
+{
+ unsigned long conf;
+ struct device *dev = &fdif->pdev->dev;
+
+ /*enable error irq*/
+ conf = ERR_IRQ;
+ fd_writel(fdif->base, FDIF_IRQENABLE_CLR_j, conf);
+
+ dma_unmap_single(dev, fdif->work_dma,
+ WORK_MEM_SIZE, DMA_TO_DEVICE);
+}
+
+
+static int start_detect(struct fdif_dev *dev)
+{
+ struct fdif_qvga *fdif = dev_get_drvdata(dev->dev);
+
+ pm_runtime_get_sync(dev->dev);
+ fdif_global_init(fdif);
+
+ return __submit_detection(fdif);
+}
+
+static int stop_detect(struct fdif_dev *dev)
+{
+ struct fdif_qvga *fdif = dev_get_drvdata(dev->dev);
+ unsigned long flags, irqsts;
+ struct fdif_buffer *buf;
+
+ spin_lock_irqsave(&fdif->lock, flags);
+
+ /*stop current transfer first*/
+ __stop_detect(fdif);
+
+ buf = fdif->pending;
+ fdif->pending = NULL;
+
+ /*clear irq status in case that it is set*/
+ irqsts = fd_readl(fdif->base, FDIF_IRQSTATUS_j);
+ fd_writel(fdif->base, FDIF_IRQSTATUS_j, irqsts);
+
+ spin_unlock_irqrestore(&fdif->lock, flags);
+
+ if (buf)
+ vb2_buffer_done(&buf->vb, VB2_BUF_STATE_ERROR);
+
+ fdif_global_deinit(fdif);
+ pm_runtime_put(dev->dev);
+ return 0;
+}
+
+static int submit_detect(struct fdif_dev *dev)
+{
+ struct fdif_qvga *fdif = dev_get_drvdata(dev->dev);
+
+ __submit_detection(fdif);
+
+ return 0;
+}
+
+static struct fdif_ops qvga_ops = {
+ .table = qvga_fmt,
+ .fmt_cnt = 1,
+ .start_detect = start_detect,
+ .stop_detect = stop_detect,
+ .submit_detect = submit_detect,
+};
+
+static int fdif_probe(struct platform_device *pdev)
+{
+ struct device *dev = &pdev->dev;
+ struct fdif_qvga *fdif;
+ struct fdif_dev *fdev;
+ struct resource *res;
+ int ret, order;
+
+ ret = fdif_create_instance(dev, sizeof(struct fdif_qvga),
+ &qvga_ops, &fdev);
+ if (ret) {
+ dev_err(dev, "fdif_create_instance failed:%d\n", ret);
+ goto end_probe;
+ }
+
+ fdif = (struct fdif_qvga *)fdev->priv;
+ fdif->dev = fdev;
+
+ spin_lock_init(&fdif->lock);
+ fdif->pdev = pdev;
+
+ res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+ if (!res) {
+ dev_err(dev, "fdif get resource failed\n");
+ ret = -ENODEV;
+ goto err_iomap;
+ }
+
+ fdif->base = ioremap(res->start, resource_size(res));
+ if (!fdif->base) {
+ dev_err(dev, "fdif ioremap failed\n");
+ ret = -ENOMEM;
+ goto err_iomap;
+ }
+
+ fdif->irq = platform_get_irq(pdev, 0);
+ if (fdif->irq < 0) {
+ dev_err(dev, "fdif get irq failed\n");
+ ret = -ENODEV;
+ goto err_get_irq;
+ }
+
+ ret = request_irq(fdif->irq, handle_detection, 0, "fdif-qvga",
+ fdif);
+ if (ret) {
+ dev_err(dev, "request_irq failed:%d\n", ret);
+ goto err_get_irq;
+ }
+
+ order = get_order(WORK_MEM_SIZE);
+ fdif->work_mem_addr = (void *)__get_free_pages(GFP_KERNEL, order);
+ if (!fdif->work_mem_addr) {
+ dev_err(dev, "fdif buffer allocation(%d) failed\n", order);
+ ret = -ENOMEM;
+ goto err_work_mem;
+ }
+
+ install_default_setting(fdif);
+
+ platform_set_drvdata(pdev, fdif);
+
+ pm_suspend_ignore_children(dev, true);
+ pm_runtime_get_sync(dev);
+ dev_info(dev, "fdif version=%8x hwinfo=%08x\n",
+ fd_readl(fdif->base, FDIF_REVISION),
+ fd_readl(fdif->base, FDIF_HWINFO));
+ pm_runtime_put(dev);
+
+ return 0;
+
+err_work_mem:
+ free_irq(fdif->irq, fdif);
+err_get_irq:
+ iounmap(fdif->base);
+err_iomap:
+ kref_put(&fdif->dev->ref, fdif_release);
+end_probe:
+ return ret;
+}
+
+static int fdif_remove(struct platform_device *pdev)
+{
+ struct fdif_qvga *fdif = platform_get_drvdata(pdev);
+ int order;
+
+ platform_set_drvdata(pdev, NULL);
+
+ free_irq(fdif->irq, fdif);
+
+ order = get_order(WORK_MEM_SIZE);
+ free_pages((unsigned long)fdif->work_mem_addr, order);
+
+ iounmap(fdif->base);
+
+ kref_put(&fdif->dev->ref, fdif_release);
+
+ return 0;
+}
+
+static int fdif_suspend(struct platform_device *pdev, pm_message_t msg)
+{
+ return 0;
+}
+
+static int fdif_resume(struct platform_device *pdev)
+{
+ return 0;
+}
+
+static struct platform_device_id fdif_device_ids[] = {
+ {.name = "fdif"},
+ {},
+};
+
+struct platform_driver fdif_driver = {
+ .probe = fdif_probe,
+ .remove = fdif_remove,
+ .suspend = fdif_suspend,
+ .resume = fdif_resume,
+ .driver = {
+ .name = "fdif",
+ .owner = THIS_MODULE,
+ },
+ .id_table = fdif_device_ids,
+};
+
+static int __init omap4_fdif_init(void)
+{
+ int retval;
+ retval = platform_driver_register(&fdif_driver);
+ if (retval) {
+ printk(KERN_ERR "Unable to register fdif driver\n");
+ return retval;
+ }
+ return 0;
+}
+
+static void omap4_fdif_cleanup(void)
+{
+ platform_driver_unregister(&fdif_driver);
+}
+
+module_init(omap4_fdif_init);
+module_exit(omap4_fdif_cleanup);
+
+MODULE_LICENSE("GPL");
+MODULE_ALIAS("platform:fdif");
+MODULE_AUTHOR("Ming Lei");