@@ -0,0 +1,582 @@
+/*
+ * Pawel Osciak, <p.osciak@samsung.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
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+#include <errno.h>
+#include <time.h>
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/mman.h>
+#include <stdint.h>
+
+#include <linux/fb.h>
+#include <linux/videodev2.h>
+
+#define VIDEO_DEV_NAME "/dev/video"
+#define FB_DEV_NAME "/dev/fb0"
+
+#define NUM_DST_BUFS 4
+
+enum io_method {
+ IO_METHOD_MMAP,
+ IO_METHOD_USERPTR,
+};
+
+#define perror_exit(cond, func)\
+ if (cond) {\
+ fprintf(stderr, "%s:%d: ", __func__, __LINE__);\
+ perror(func);\
+ exit(EXIT_FAILURE);\
+ }
+
+#define error_exit(cond, msg, ...)\
+ if (cond) {\
+ fprintf(stderr, "%s:%d: " msg "\n",\
+ __func__, __LINE__, ##__VA_ARGS__);\
+ exit(EXIT_FAILURE);\
+ }
+
+#define perror_ret(cond, func)\
+ if (cond) {\
+ fprintf(stderr, "%s:%d: ", __func__, __LINE__);\
+ perror(func);\
+ return ret;\
+ }
+
+#define memzero(x)\
+ memset(&(x), 0, sizeof (x));
+
+#define error(msg) fprintf(stderr, "%s:%d: " msg "\n", __func__, __LINE__);
+
+#define _DEBUG
+#ifdef _DEBUG
+#define debug(msg, ...)\
+ fprintf(stderr, "%s: " msg, __func__, ##__VA_ARGS__);
+#else
+#define debug(msg, ...)
+#endif
+
+#define PAGE_ALIGN(addr) (((addr) + page_size - 1) & ~(page_size -1))
+
+enum v4l2_buf_type type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+
+enum format {
+ FMT_422,
+ FMT_565,
+ FMT_422P,
+};
+
+struct buffer {
+ char *addr[VIDEO_MAX_PLANES];
+ unsigned long size[VIDEO_MAX_PLANES];
+ unsigned int num_planes;
+ unsigned int index;
+ enum format fmt;
+ unsigned int width;
+ unsigned int height;
+ enum v4l2_buf_type type;
+};
+
+static int vid_fd, fb_fd;
+static void *fb_addr, *fb_alloc_ptr;
+static int width = 200, height = 200;
+static off_t fb_line_w, fb_buf_w, fb_size, fb_pix_dist;
+static struct fb_var_screeninfo fbinfo;
+static int vid_node;
+static int framesize;
+static enum format format;
+static int num_planes;
+static long page_size;
+enum io_method io_method;
+enum v4l2_memory memory;
+
+static void set_fmt(enum format format)
+{
+ struct v4l2_format fmt;
+ int ret;
+
+ memzero(fmt);
+ switch (format) {
+ case FMT_422:
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV;
+ fb_buf_w = 2;
+ break;
+ case FMT_565:
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB565X;
+ fb_buf_w = 2;
+ break;
+ case FMT_422P:
+ if (2 == num_planes)
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_NV16M;
+ else if (3 == num_planes)
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUV422PM;
+ else
+ error_exit(1, "Invalid format/planes combination\n");
+ fb_buf_w = 1;
+ break;
+ default:
+ error_exit(1, "Invalid params\n");
+ break;
+ }
+
+ debug("Selected fourcc: %d\n", fmt.fmt.pix.pixelformat);
+
+ /* Set format for capture */
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.width = width;
+ fmt.fmt.pix.height = height;
+ fmt.fmt.pix.field = V4L2_FIELD_ANY;
+
+ ret = ioctl(vid_fd, VIDIOC_S_FMT, &fmt);
+ perror_exit(ret != 0, "ioctl");
+
+ width = fmt.fmt.pix.width;
+ height = fmt.fmt.pix.height;
+ fb_buf_w *= width;
+ framesize = PAGE_ALIGN(fmt.fmt.pix.sizeimage);
+ debug("Framesize: %d\n", framesize);
+
+ switch (io_method) {
+ case IO_METHOD_MMAP:
+ if (num_planes == 1)
+ memory = V4L2_MEMORY_MMAP;
+ else
+ memory = V4L2_MEMORY_MULTI_MMAP;
+ break;
+ case IO_METHOD_USERPTR:
+ if (num_planes == 1)
+ memory = V4L2_MEMORY_USERPTR;
+ else
+ memory = V4L2_MEMORY_MULTI_USERPTR;
+ break;
+ default:
+ error_exit(1, "Invalid io method\n");
+ }
+}
+
+
+static void verify_caps(void)
+{
+ struct v4l2_capability cap;
+ int ret;
+
+ memzero(cap);
+ ret = ioctl(vid_fd, VIDIOC_QUERYCAP, &cap);
+ perror_exit(ret != 0, "ioctl");
+
+ if (!(cap.capabilities & V4L2_CAP_VIDEO_CAPTURE))
+ error_exit(1, "Device does not support capture\n");
+
+ if (!(cap.capabilities & V4L2_CAP_STREAMING))
+ error_exit(1, "Device does not support streaming\n");
+}
+
+static void init_video_dev(void)
+{
+ char devname[64];
+
+ snprintf(devname, 64, "%s%d", VIDEO_DEV_NAME, vid_node);
+ vid_fd = open(devname, O_RDWR | O_NONBLOCK, 0);
+ perror_exit(vid_fd < 0, "open");
+
+ verify_caps();
+}
+
+static void init_fb(void)
+{
+ int ret;
+
+ fb_fd = open(FB_DEV_NAME, O_RDWR);
+ perror_exit(fb_fd < 0, "open");
+
+ ret = ioctl(fb_fd, FBIOGET_VSCREENINFO, &fbinfo);
+ perror_exit(ret != 0, "ioctl");
+ debug("fbinfo: xres: %d, xres_virt: %d, yres: %d, yres_virt: %d\n",
+ fbinfo.xres, fbinfo.xres_virtual,
+ fbinfo.yres, fbinfo.yres_virtual);
+
+ fb_pix_dist = fbinfo.bits_per_pixel >> 3;
+ fb_line_w = fbinfo.xres_virtual * fb_pix_dist;
+ debug("fb_buf_w: %ld, fb_line_w: %ld\n", fb_buf_w, fb_line_w);
+ fb_size = fb_line_w * fbinfo.yres_virtual;
+
+ fb_addr = fb_alloc_ptr = mmap(0, fb_size, PROT_WRITE | PROT_READ,
+ MAP_SHARED, fb_fd, 0);
+ perror_exit(fb_addr == MAP_FAILED, "mmap");
+}
+
+static void parse_args(int argc, char *argv[])
+{
+ if (argc != 5) {
+ fprintf(stderr, "Usage: "
+ "%s video_node io_method format num_planes \n"
+ "io methods: mmap, userptr \n"
+ "formats: 0: 422, 1: 565, 2: 422P\n",
+ argv[0]);
+ error_exit(1, "Invalid number of arguments\n");
+ }
+
+ vid_node = atoi(argv[1]);
+ io_method = atoi(argv[2]);
+ format = atoi(argv[3]);
+ num_planes = atoi(argv[4]);
+
+ debug("vid_node: %d, format: %d, num_planes: %d\n",
+ vid_node, format, num_planes);
+}
+
+static void alloc_buffer(struct buffer *buf)
+{
+ buf->addr[0] = fb_alloc_ptr;
+
+ switch(num_planes) {
+ case 1:
+ buf->size[0] = framesize;
+ break;
+ case 2:
+ if (FMT_422 != format)
+ error_exit(1, "Unsupported format for this number "
+ "of planes\n");
+ buf->size[0] = PAGE_ALIGN(width * height);
+ buf->addr[1] = buf->addr[0] + buf->size[0];
+ buf->size[1] = PAGE_ALIGN(width * height);
+ break;
+ case 3:
+ if (FMT_422 != format)
+ error_exit(1, "Unsupported format for this number "
+ "of planes\n");
+ buf->size[0] = PAGE_ALIGN(width * height);
+ buf->addr[1] = buf->addr[0] + buf->size[0];
+ buf->size[1] = PAGE_ALIGN(width * height / 2);
+ buf->addr[2] = buf->addr[1] + buf->size[1];
+ buf->size[2] = buf->size[1];
+ break;
+ default:
+ error_exit(1, "Unsupported number of planes\n");
+ break;
+ }
+
+ buf->fmt = format;
+ buf->width = width;
+ buf->height = height;
+ buf->num_planes = num_planes;
+ buf->type = type;
+
+ fb_alloc_ptr = buf->addr[num_planes - 1] + buf->size[num_planes - 1];
+ if (fb_alloc_ptr > fb_addr + fb_size)
+ error_exit(1, "Out of fb memory\n");
+}
+
+static void request_buffers(unsigned int *num_bufs, enum v4l2_buf_type type)
+{
+ struct v4l2_requestbuffers reqbuf;
+ int ret;
+
+ memzero(reqbuf);
+
+ reqbuf.memory = memory;
+
+ reqbuf.count = *num_bufs;
+ reqbuf.type = type;
+ ret = ioctl(vid_fd, VIDIOC_REQBUFS, &reqbuf);
+ perror_exit(ret != 0, "ioctl");
+ *num_bufs = reqbuf.count;
+}
+
+static void display(struct buffer *buf,
+ unsigned int start_x, unsigned int start_y)
+{
+ char *p_buf, *p_fb;
+ int curr_y;
+ unsigned int i, pl_h, pl_w;
+
+ p_fb = fb_addr + start_y * fb_line_w + start_x;// * fb_pix_dist;
+ p_buf = buf->addr[0];
+
+ debug("fb_buf_w: %ld, fb_line_w: %ld\n", fb_buf_w, fb_line_w);
+
+ if (1 == buf->num_planes) {
+ for (curr_y = 0; curr_y < buf->height; ++curr_y) {
+ memcpy(p_fb, p_buf, fb_buf_w);
+ p_fb += fb_line_w;
+ p_buf += fb_buf_w;
+ }
+ } else {
+ /* 0th plane */
+ for (curr_y = 0; curr_y < buf->height; ++curr_y) {
+ memcpy(p_fb, p_buf, buf->width);
+ p_fb += fb_line_w;
+ p_buf += buf->width;
+ }
+
+ if (buf->num_planes == 2) {
+ pl_w = buf->width;
+ pl_h = buf->height;
+ } else if (buf->num_planes == 3) {
+ pl_w = buf->width / 2;
+ pl_h = buf->height;
+ } else {
+ debug("Cannot display more than 3 planes\n");
+ return;
+ }
+
+ for (i = 1; i < buf->num_planes; ++i) {
+ p_buf = buf->addr[i];
+ for (curr_y = 0; curr_y < pl_h; ++curr_y) {
+ memcpy(p_fb, p_buf, pl_w);
+ p_fb += fb_line_w;
+ p_buf += pl_w;
+ }
+ }
+ }
+}
+
+static void setup_userptr(struct buffer *buffers, int num_buffers)
+{
+ int i;
+
+ for (i = 0; i < num_buffers; ++i) {
+ alloc_buffer(&buffers[i]);
+
+ buffers[i].index = i;
+ buffers[i].type = type;
+ buffers[i].num_planes = num_planes;
+ buffers[i].width = width;
+ buffers[i].height = height;
+ }
+}
+
+static void setup_mmap(struct buffer *buffers, int num_buffers)
+{
+ struct v4l2_buffer buf;
+ struct v4l2_plane planes[VIDEO_MAX_PLANES];
+ int ret;
+ int i, j;
+
+ for (i = 0; i < num_buffers; ++i) {
+ memzero(buf);
+
+ if (V4L2_MEMORY_MULTI_MMAP == memory) {
+ buf.memory = memory;
+ buf.m.planes = planes;
+ buf.length = num_planes;
+ }
+ buffers[i].index = buf.index = i;
+ buffers[i].type = buf.type = type;
+ buffers[i].num_planes = num_planes;
+ buffers[i].width = width;
+ buffers[i].height = height;
+
+ ret = ioctl(vid_fd, VIDIOC_QUERYBUF, &buf);
+ perror_exit(ret != 0, "ioctl");
+
+ if (V4L2_MEMORY_MULTI_MMAP == memory) {
+ if (buf.length != num_planes)
+ error_exit(1, "Driver requires %d planes, "
+ "expected %d\n",
+ buf.length, num_planes);
+
+ for (j = 0; j < buf.length; ++j) {
+ buffers[i].size[j] = buf.m.planes[j].length;
+ buffers[i].addr[j] = mmap(NULL, buffers[i].size[j],
+ PROT_READ | PROT_WRITE,
+ MAP_SHARED, vid_fd,
+ buf.m.planes[j].m.offset);
+ perror_exit(MAP_FAILED == buffers[i].addr[j], "mmap");
+ }
+ } else if (V4L2_MEMORY_MMAP == memory) {
+ buffers[i].size[0] = buf.length;
+ buffers[i].addr[0] = mmap(NULL, buffers[i].size[0],
+ PROT_READ | PROT_WRITE,
+ MAP_SHARED, vid_fd,
+ buf.m.offset);
+ perror_exit(MAP_FAILED == buffers[i].addr[0], "mmap");
+ }
+
+ }
+}
+
+static int get_frame(void)
+{
+ struct v4l2_buffer v4l2_buf;
+ struct v4l2_plane planes[VIDEO_MAX_PLANES];
+
+ memzero(v4l2_buf);
+
+ switch (io_method) {
+ case IO_METHOD_MMAP:
+ v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ v4l2_buf.memory = memory;
+ if (V4L2_MEMORY_MULTI_MMAP == memory) {
+ v4l2_buf.m.planes = planes;
+ v4l2_buf.length = num_planes;
+ }
+
+ if (-1 == ioctl (vid_fd, VIDIOC_DQBUF, &v4l2_buf)) {
+ switch (errno) {
+ case EAGAIN:
+ return 0;
+ case EIO:
+ /* Could ignore EIO, see spec. */
+ /* fall through */
+ default:
+ perror_exit(1, "ioctl");
+ }
+ }
+ return v4l2_buf.index;
+
+ case IO_METHOD_USERPTR:
+ v4l2_buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ v4l2_buf.memory = memory;
+ if (V4L2_MEMORY_MULTI_USERPTR == memory) {
+ v4l2_buf.m.planes = planes;
+ v4l2_buf.length = num_planes;
+ }
+
+ if (-1 == ioctl (vid_fd, VIDIOC_DQBUF, &v4l2_buf)) {
+ switch (errno) {
+ case EAGAIN:
+ return 0;
+ case EIO:
+ /* Could ignore EIO, see spec. */
+ /* fall through */
+ default:
+ perror_exit(1, "ioctl");
+ }
+ }
+ return v4l2_buf.index;
+ default:
+ return -1;
+ }
+}
+
+void put_frame(struct buffer *buf)
+{
+ struct v4l2_buffer v4l2_buf;
+ struct v4l2_plane planes[VIDEO_MAX_PLANES];
+ int ret, i;
+
+ memzero(v4l2_buf);
+
+ switch (io_method) {
+ case IO_METHOD_MMAP:
+ v4l2_buf.type = buf->type;
+ v4l2_buf.memory = memory;
+ v4l2_buf.index = buf->index;
+
+ if (V4L2_MEMORY_MULTI_MMAP == memory) {
+ v4l2_buf.m.planes = planes;
+ v4l2_buf.length = num_planes;
+ }
+
+ ret = ioctl(vid_fd, VIDIOC_QBUF, &v4l2_buf);
+ perror_exit(ret != 0, "ioctl");
+ break;
+
+ case IO_METHOD_USERPTR:
+ v4l2_buf.type = buf->type;
+ v4l2_buf.memory = memory;
+ v4l2_buf.index = buf->index;
+
+ if (V4L2_MEMORY_USERPTR == memory) {
+ v4l2_buf.m.userptr = (unsigned long)buf->addr[0];
+ v4l2_buf.length = buf->size[0];
+ } else if (V4L2_MEMORY_MULTI_USERPTR == memory) {
+ v4l2_buf.m.planes = planes;
+ v4l2_buf.length = num_planes;
+
+ for (i = 0; i < num_planes; ++i) {
+ v4l2_buf.m.planes[i].m.userptr =
+ (unsigned long)buf->addr[i];
+ v4l2_buf.m.planes[i].length = buf->size[i];
+ }
+ }
+
+ ret = ioctl(vid_fd, VIDIOC_QBUF, &v4l2_buf);
+ perror_exit(ret != 0, "ioctl");
+ break;
+ }
+}
+
+int main(int argc, char *argv[])
+{
+ struct buffer dst_buffers[VIDEO_MAX_FRAME];
+ int ret = 0;
+ unsigned int num_dst_buffers = NUM_DST_BUFS;
+ unsigned int count = 10;
+ unsigned int i;
+ int index;
+
+ parse_args(argc, argv);
+
+ page_size = sysconf(_SC_PAGESIZE);
+
+ init_fb();
+
+ init_video_dev();
+ set_fmt(format);
+
+ request_buffers(&num_dst_buffers, V4L2_BUF_TYPE_VIDEO_CAPTURE);
+ if (IO_METHOD_MMAP == io_method) {
+ setup_mmap(dst_buffers, num_dst_buffers);
+ } else if (IO_METHOD_USERPTR == io_method) {
+ setup_userptr(dst_buffers, num_dst_buffers);
+ }
+
+ for (i = 0; i < num_dst_buffers; ++i)
+ put_frame(&dst_buffers[i]);
+
+ ret = ioctl(vid_fd, VIDIOC_STREAMON, &type);
+ perror_exit(0 != ret, "ioctl");
+
+ while (count-- > 0) {
+ for (;;) {
+ fd_set fds;
+ struct timeval tv;
+ int r;
+
+ FD_ZERO(&fds);
+ FD_SET(vid_fd, &fds);
+
+ tv.tv_sec = 2;
+ tv.tv_usec = 0;
+
+ r = select(vid_fd + 1, &fds, NULL, NULL, &tv);
+
+ if (-1 == r) {
+ if (EINTR == errno)
+ continue;
+ else
+ perror_exit(1, "select");
+ }
+
+ if (0 == r) {
+ error_exit(1, "timeout!\n");
+ }
+
+ break;
+ }
+
+ index = get_frame();
+ if (index < 0 || index >= num_dst_buffers)
+ error_exit(1, "Invalid index %d\n", index);
+ display(&dst_buffers[index], 0, 0);
+ put_frame(&dst_buffers[index]);
+ }
+ if (ret)
+ return ret;
+
+ return 0;
+}