support autofocus / autogain in libv4l2

Pavel Machek
2017-04-24T09:30:59Z

[-- Attachment #1: Type: text/plain, Size: 42076 bytes --]

Hi!

For focus to be useful, we need autofocus implmented
somewhere. Unfortunately, v4l framework does not seem to provide good
place where to put autofocus. I believe, long-term, we'll need some
kind of "video server" providing this kind of services.

Anyway, we probably don't want autofocus in kernel (even through some
cameras do it in hardware), and we probably don't want autofocus in
each and every user application.

So what remains is libv4l2. Now, this is in no way clean or complete,
and functionality provided by sdl.c and asciicam.c probably _should_
be in application, but... I'd like to get the code out there.

Oh and yes, I've canibalized decode_tm6000.c application instead of
introducing my own. Autotools scare me, sorry.

Regards,
Pavel

diff --git a/lib/libv4l2/asciicam.c b/lib/libv4l2/asciicam.c
new file mode 100644
index 0000000..5388967
--- /dev/null
+++ b/lib/libv4l2/asciicam.c
@@ -0,0 +1,63 @@
+/* gcc asciicam.c /usr/lib/i386-linux-gnu/libv4l2.so.0.0.0 -o asciicam
+ gcc asciicam.c /usr/lib/arm-linux-gnueabi/libv4l2.so.0 -o asciicam
+
+gcc -std=gnu99 -DHAVE_CONFIG_H -I. -I../.. -fvisibility=hidden -I../../lib/include -Wall -Wpointer-arith -D_GNU_SOURCE -I../../include -g -O2 asciicam.c libv4l2.c /usr/lib/arm-linux-gnueabi/libv4lconvert.so.0 log.c v4l2convert.c v4l2-plugin.c -o asciicam
+ */
+#include <stdio.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+#include <linux/videodev2.h>
+
+#define SIZE 10*1024*1024
+
+char buf[SIZE];
+
+void main(void)
+{
+ int fd = v4l2_open("/dev/video2", O_RDWR);
+ int i;
+ static struct v4l2_format fmt;
+
+#if 1
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
+ fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ fmt.fmt.pix.width = 640;
+ fmt.fmt.pix.height = 480;
+ if (fmt.fmt.pix.pixelformat != 'RGB3') /* v4l2_fourcc('R', 'G', 'B', '3'); */
+ printf("hmm. strange format?\n");
+
+ printf("ioctl = %d\n", v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt));
+#endif
+
+ for (i=0; i<500; i++) {
+ int num = v4l2_read(fd, buf, SIZE);
+ int y,x;
+
+ printf("%d\n", num);
+#if 0
+ for (y = 0; y < 25; y++) {
+ for (x = 0; x < 80; x++) {
+ int y1 = y * 480/25;
+ int x1 = x * 640/80;
+ int c = buf[fmt.fmt.pix.width*3*y1 + 3*x1] +
+ buf[fmt.fmt.pix.width*3*y1 + 3*x1 + 1] +
+ buf[fmt.fmt.pix.width*3*y1 + 3*x1 + 2];
+
+ if (c < 30) c = ' ';
+ else if (c < 60) c = '.';
+ else if (c < 120) c = '_';
+ else if (c < 180) c = 'o';
+ else if (c < 300) c = 'x';
+ else if (c < 400) c = 'X';
+ else c = '#';
+
+ printf("%c", c);
+ }
+ printf("\n");
+ }
+#endif
+ }
+}
diff --git a/lib/libv4l2/libv4l2-priv.h b/lib/libv4l2/libv4l2-priv.h
index 343db5e..af740a7 100644
--- a/lib/libv4l2/libv4l2-priv.h
+++ b/lib/libv4l2/libv4l2-priv.h
@@ -1,3 +1,4 @@
+/* -*- c-file-style: "linux" -*- */
/*
# (C) 2008 Hans de Goede <hdegoede@redhat.com>

@@ -70,6 +71,14 @@
} while (0)

#define MIN(a, b) (((a) < (b)) ? (a) : (b))
+#define V4L2_MAX_SUBDEVS 16
+
+#define V4L2_MAX_FOCUS 10
+struct v4l2_focus_step {
+ int num;
+ int sharpness[V4L2_MAX_FOCUS];
+ int position[V4L2_MAX_FOCUS];
+};

struct v4l2_dev_info {
int fd;
@@ -104,6 +113,14 @@ struct v4l2_dev_info {
void *plugin_library;
void *dev_ops_priv;
const struct libv4l_dev_ops *dev_ops;
+ int subdev_fds[V4L2_MAX_SUBDEVS];
+ int exposure;
+ int frame;
+ int focus;
+
+ /* Autofocus parameters */
+ int focus_frame, focus_exposure, focus_step, sh_prev;
+ struct v4l2_focus_step focus_step_params;
};

/* From v4l2-plugin.c */
diff --git a/lib/libv4l2/libv4l2.c b/lib/libv4l2/libv4l2.c
index 0ba0a88..3b84437 100644
--- a/lib/libv4l2/libv4l2.c
+++ b/lib/libv4l2/libv4l2.c
@@ -1,3 +1,4 @@
+/* -*- c-file-style: "linux" -*- */
/*
# (C) 2008 Hans de Goede <hdegoede@redhat.com>

@@ -100,6 +101,13 @@ static struct v4l2_dev_info devices[V4L2_MAX_DEVICES] = {
};
static int devices_used;

+#include "sdl.c"
+
+static struct sdl sdl;
+
+int v4l2_get_index(int fd);
+void my_main(void);
+
static int v4l2_ensure_convert_mmap_buf(int index)
{
if (devices[index].convert_mmap_buf != MAP_FAILED) {
@@ -311,7 +319,409 @@ static int v4l2_queue_read_buffer(int index, int buffer_index)
return 0;
}

-static int v4l2_dequeue_and_convert(int index, struct v4l2_buffer *buf,
+static void v4l2_paint(char *buf, int x1, int y1, const struct v4l2_format *fmt)
+{
+ int y, x;
+ int width = fmt->fmt.pix.width;
+ printf("width = %d\n", width);
+
+ for (y = 0; y < 25; y++) {
+ for (x = 0; x < 80; x++) {
+ int c = buf[width*4*y1 + 4*x1] +
+ buf[width*4*y1 + 4*x1 + 1] +
+ buf[width*4*y1 + 4*x1 + 2];
+
+ if (c < 30) c = ' ';
+ else if (c < 60) c = '.';
+ else if (c < 120) c = '_';
+ else if (c < 180) c = 'o';
+ else if (c < 300) c = 'x';
+ else if (c < 400) c = 'X';
+ else c = '#';
+
+ printf("%c", c);
+ }
+ printf("\n");
+ }
+}
+
+#define SX 80
+#define SY 25
+#define BUCKETS 20
+
+static void v4l2_histogram(unsigned char *buf, int cdf[], struct v4l2_format *fmt)
+{
+ for (int y = 0; y < fmt->fmt.pix.height; y+=19)
+ for (int x = 0; x < fmt->fmt.pix.width; x+=19) {
+ pixel p = buf_pixel(fmt, buf, x, y);
+
+ int b;
+ /* HACK: we divide green by 2 to have nice picture, undo it here. */
+ b = p.r + 2*p.g + p.b;
+ b = (b * BUCKETS)/(256);
+ cdf[b]++;
+ }
+}
+
+static long v4l2_sharpness(unsigned char *buf, struct v4l2_format *fmt)
+{
+ int h = fmt->fmt.pix.height;
+ int w = fmt->fmt.pix.width;
+ long r = 0;
+
+ for (int y = h/3; y < h-h/3; y+=h/9)
+ for (int x = w/3; x < w-w/3; x++) {
+ pixel p1 = buf_pixel(fmt, buf, x, y);
+ pixel p2 = buf_pixel(fmt, buf, x+2, y);
+
+ int b1, b2;
+ /* HACK: we divide green by 2 to have nice picture, undo it here. */
+ b1 = p1.r + 2*p1.g + p1.b;
+ b2 = p2.r + 2*p2.g + p2.b;
+
+ int v;
+ v = (b1-b2)*(b1-b2);
+ if (v > 36)
+ r+=v;
+ }
+
+ return r;
+}
+
+int v4l2_set_exposure(int fd, int exposure)
+{
+ int index = v4l2_get_index(fd);
+
+ if (index == -1 || devices[index].convert == NULL) {
+ V4L2_LOG_ERR("v4l2_set_exposure called with invalid fd: %d\n", fd);
+ errno = EBADF;
+ return -1;
+ }
+
+ struct v4l2_control ctrl;
+ ctrl.id = V4L2_CID_EXPOSURE;
+ ctrl.value = exposure;
+ if (ioctl(devices[index].subdev_fds[0], VIDIOC_S_CTRL, &ctrl) < 0) {
+ printf("Could not set exposure\n");
+ }
+ return 0;
+}
+
+int v4l2_set_gain(int fd, int gain)
+{
+ int index = v4l2_get_index(fd);
+
+ if (index == -1 || devices[index].convert == NULL) {
+ V4L2_LOG_ERR("v4l2_set_exposure called with invalid fd: %d\n", fd);
+ errno = EBADF;
+ return -1;
+ }
+
+ struct v4l2_control ctrl;
+ ctrl.id = 0x00980913;
+ ctrl.value = gain;
+ if (ioctl(devices[index].subdev_fds[0], VIDIOC_S_CTRL, &ctrl) < 0) {
+ printf("Could not set exposure\n");
+ }
+ return 0;
+}
+
+int v4l2_set_focus(int fd, int diopt)
+{
+ int index = v4l2_get_index(fd);
+
+ if (index == -1 || devices[index].convert == NULL) {
+ V4L2_LOG_ERR("v4l2_set_focus called with invalid fd: %d\n", fd);
+ errno = EBADF;
+ return -1;
+ }
+
+ struct v4l2_control ctrl;
+ ctrl.id = V4L2_CID_FOCUS_ABSOLUTE;
+ ctrl.value = diopt;
+ if (ioctl(devices[index].subdev_fds[1], VIDIOC_S_CTRL, &ctrl) < 0) {
+ printf("Could not set focus\n");
+ }
+ return 0;
+}
+
+#define LESS 1
+#define MID 0
+#define MORE 2
+
+static void v4l2_start_focus_step(struct v4l2_focus_step *fs, int focus, int step)
+{
+ int i;
+
+ fs->num = 3;
+ for (i=0; i<V4L2_MAX_FOCUS; i++) {
+ fs->sharpness[i] = -1;
+ fs->position[i] = -1;
+ }
+
+ fs->position[LESS] = focus - step;
+ fs->position[MID] = focus;
+ fs->position[MORE] = focus + step;
+}
+
+static int v4l2_focus_get_best(struct v4l2_focus_step *fs)
+{
+ int i, max = -1, maxi = -1;
+
+ for (i=0; i<fs->num; i++)
+ if (max < fs->sharpness[i]) {
+ max = fs->sharpness[i];
+ maxi = i;
+ }
+
+ return maxi;
+}
+
+static void v4l2_auto_focus_continuous(struct v4l2_dev_info *m, int sh)
+{
+ int f = m->frame - m->focus_frame;
+ int step;
+ const int max_step = 300, min_step = 20;
+ struct v4l2_focus_step *fs = &m->focus_step_params;
+
+ if (m->focus_step == 0 || m->focus_step > max_step) {
+ printf("step reset -- max\n");
+ m->focus_step = max_step;
+ }
+ if (m->focus_step < min_step) {
+ printf("step reset -- 10 (%d)\n", m->focus_step);
+ m->focus_step = min_step;
+ /* It takes cca 5.7 seconds to achieve the focus:
+ 0.76user 0.30system 5.66 (0m5.661s) elapsed 18.72%CPU
+ */
+ printf("Focused at %d\n", m->focus);
+ exit(0);
+ }
+
+ step = m->focus_step;
+
+ if (m->exposure != m->focus_exposure) {
+ m->focus_frame = m->frame;
+ m->focus_exposure = m->exposure;
+ v4l2_start_focus_step(fs, m->focus, m->focus_step);
+ return;
+ }
+ if (m->focus < step) {
+ m->focus = step;
+ }
+
+ const int every = 3;
+ if (f%every)
+ return;
+
+ {
+ int i = f/every;
+
+ if (i == 0) {
+ printf("Can not happen?\n");
+ return;
+ }
+ i--;
+ if (i < fs->num)
+ v4l2_set_focus(m->fd, fs->position[i]);
+ if (i > 0)
+ fs->sharpness[i-1] = sh;
+ if (i < fs->num)
+ return;
+ }
+ int i;
+ for (i=0; i<fs->num; i++) {
+ printf("%d: %d | ", fs->position[i], fs->sharpness[i]);
+ }
+ int best = v4l2_focus_get_best(fs);
+ if ((fs->sharpness[best] < m->sh_prev) && (m->focus_step < max_step)) {
+ m->focus_step *=2;
+ m->sh_prev = m->sh_prev * 0.9;
+ printf("step up %d\n", m->focus_step);
+ } else if (best == LESS) {
+ printf("less ");
+ m->focus -= step;
+ } else if (best == MORE) {
+ printf("more ");
+ m->focus += step;
+ } else {
+ m->sh_prev = fs->sharpness[MID];
+ m->focus_step = m->focus_step / 2;
+ printf("step %d ", m->focus_step);
+ }
+ m->focus_frame = m->frame;
+ v4l2_start_focus_step(fs, m->focus, m->focus_step);
+ printf("Focus now %d\n", m->focus);
+}
+
+static void v4l2_start_focus_sweep(struct v4l2_focus_step *fs, int focus, int step)
+{
+ int i;
+
+ fs->num = V4L2_MAX_FOCUS;
+ for (i=0; i<V4L2_MAX_FOCUS; i++) {
+ fs->sharpness[i] = -1;
+ fs->position[i] = -1;
+ }
+
+ int f = focus;
+ for (i=0; i<V4L2_MAX_FOCUS; i++) {
+ fs->position[i] = f;
+ f += step;
+ }
+}
+
+static void v4l2_auto_focus_single(struct v4l2_dev_info *m, int sh)
+{
+ int f = m->frame - m->focus_frame;
+ int step;
+ struct v4l2_focus_step *fs = &m->focus_step_params;
+
+ if (m->focus_step == 0) {
+ printf("step reset -- max\n");
+ m->focus_step = 1;
+ }
+
+ if (m->exposure != m->focus_exposure) {
+ m->focus_frame = m->frame;
+ m->focus_exposure = m->exposure;
+ v4l2_start_focus_sweep(fs, 0, 100);
+ return;
+ }
+
+ const int every = 3;
+ if (f%every)
+ return;
+
+ {
+ int i = f/every;
+
+ if (i == 0) {
+ printf("Can not happen?\n");
+ return;
+ }
+ i--;
+ if (i < fs->num)
+ v4l2_set_focus(m->fd, fs->position[i]);
+ if (i > 0)
+ fs->sharpness[i-1] = sh;
+ if (i < fs->num)
+ return;
+ }
+#if 0
+ int i;
+ for (i=0; i<fs->num; i++) {
+ printf("%d: %d | ", fs->position[i], fs->sharpness[i]);
+ }
+#endif
+ int best = v4l2_focus_get_best(fs);
+ m->focus_frame = m->frame;
+ switch (m->focus_step) {
+ case 1:
+ printf("Best now %d / %d\n", fs->position[best], fs->sharpness[best]);
+ v4l2_start_focus_sweep(fs, fs->position[best] - 50, 10);
+ m->focus_step = 2;
+ break;
+ case 2:
+ printf("Best now %d / %d\n", fs->position[best], fs->sharpness[best]);
+ v4l2_start_focus_sweep(fs, fs->position[best] - 10, 2);
+ m->focus_step = 3;
+ break;
+ case 3:
+ printf("Best now %d / %d\n", fs->position[best], fs->sharpness[best]);
+ printf("done.\n");
+ exit(0);
+ }
+}
+
+
+static void v4l2_auto_exposure(int index, struct v4l2_buffer *buf)
+{
+ struct v4l2_format *fmt;
+ int cdf[BUCKETS] = { 0, };
+ int i;
+
+ fmt = &devices[index].src_fmt;
+
+ v4l2_histogram(devices[index].frame_pointers[buf->index], cdf, fmt);
+
+#if 0
+ printf("hist: ");
+ for (i = 0; i<BUCKETS; i++)
+ printf("%d ", cdf[i]);
+ printf("\n");
+#endif
+ for (i=1; i<BUCKETS; i++)
+ cdf[i] += cdf[i-1];
+
+ int b = BUCKETS;
+ int brightPixels = cdf[b-1] - cdf[b-8];
+ int targetBrightPixels = cdf[b-1]/50;
+ int maxSaturatedPixels = cdf[b-1]/200;
+ int saturatedPixels = cdf[b-1] - cdf[b-2];
+ // how much should I change brightness by
+ float adjustment = 1.0f;
+#if 0
+ printf( "AutoExposure: totalPixels: %d,"
+ "brightPixels: %d, targetBrightPixels: %d,"
+ "saturatedPixels: %d, maxSaturatedPixels: %d\n",
+ cdf[b-1], brightPixels, targetBrightPixels,
+ saturatedPixels, maxSaturatedPixels);
+#endif
+
+ if (saturatedPixels > maxSaturatedPixels) {
+ // first don't let things saturate too much
+ adjustment = 1.0f - ((float)(saturatedPixels - maxSaturatedPixels))/cdf[b-1];
+ } else if (brightPixels < (targetBrightPixels - (saturatedPixels * 4))) {
+ // increase brightness to try and hit the desired number of well exposed pixels
+ int l = b-6;
+ while (brightPixels < targetBrightPixels && l > 0) {
+ brightPixels += cdf[l];
+ brightPixels -= cdf[l-1];
+ l--;
+ }
+
+ // that level is supposed to be at b-11;
+ adjustment = ((float) (b-6+1))/(l+1);
+ } else {
+ // we're not oversaturated, and we have enough bright pixels. Do nothing.
+ }
+
+ {
+ float limit = 4;
+ if (adjustment > limit) { adjustment = limit; }
+ if (adjustment < 1/limit) { adjustment = 1/limit; }
+ }
+
+ if (!devices[index].exposure)
+ devices[index].exposure = 1;
+ devices[index].exposure *= adjustment;
+ if (adjustment != 1.)
+ printf( "AutoExposure: adjustment: %f exposure %d\n", adjustment, devices[index].exposure);
+
+ v4l2_set_exposure(devices[index].fd, devices[index].exposure);
+}
+
+static void v4l2_statistics(int index, struct v4l2_buffer *buf)
+{
+ unsigned char *b;
+ struct v4l2_format *fmt;
+
+ fmt = &devices[index].src_fmt;
+ b = devices[index].frame_pointers[buf->index];
+
+ if (!(devices[index].frame%3))
+ v4l2_auto_exposure(index, buf);
+
+ int sh = v4l2_sharpness(b, fmt);
+ v4l2_auto_focus_single(&devices[index], sh);
+
+ devices[index].frame++;
+ if (!(devices[index].frame%4))
+ sdl_render(&sdl, b, fmt);
+}
+
+int v4l2_dequeue_and_convert(int index, struct v4l2_buffer *buf,
unsigned char *dest, int dest_size)
{
const int max_tries = V4L2_IGNORE_FIRST_FRAME_ERRORS + 1;
@@ -345,6 +755,13 @@ static int v4l2_dequeue_and_convert(int index, struct v4l2_buffer *buf,
errno = -EINVAL;
return -1;
}
+
+#if 1
+ v4l2_statistics(index, buf);
+#endif
+#if 0
+ /* This is rather major eater of CPU time. CPU time goes from 80% to 4%
+ when conversion is disabled. */

result = v4lconvert_convert(devices[index].convert,
&devices[index].src_fmt, &devices[index].dest_fmt,
@@ -352,7 +769,7 @@ static int v4l2_dequeue_and_convert(int index, struct v4l2_buffer *buf,
buf->bytesused, dest ? dest : (devices[index].convert_mmap_buf +
buf->index * devices[index].convert_mmap_frame_size),
dest_size);
-
+#endif
if (devices[index].first_frame) {
/* Always treat convert errors as EAGAIN during the first few frames, as
some cams produce bad frames at the start of the stream
@@ -789,18 +1206,24 @@ no_capture:

/* Note we always tell v4lconvert to optimize src fmt selection for
our default fps, the only exception is the app explicitly selecting
- a fram erate using the S_PARM ioctl after a S_FMT */
+ a frame rate using the S_PARM ioctl after a S_FMT */
if (devices[index].convert)
v4lconvert_set_fps(devices[index].convert, V4L2_DEFAULT_FPS);
v4l2_update_fps(index, &parm);

+ devices[index].subdev_fds[0] = SYS_OPEN("/dev/video_sensor", O_RDWR, 0);
+ devices[index].subdev_fds[1] = SYS_OPEN("/dev/video_focus", O_RDWR, 0);
+
+ printf("Sensor: %d, focus: %d\n", devices[index].subdev_fds[0],
+ devices[index].subdev_fds[1]);
+
V4L2_LOG("open: %d\n", fd);

return fd;
}

/* Is this an fd for which we are emulating v4l1 ? */
-static int v4l2_get_index(int fd)
+int v4l2_get_index(int fd)
{
int index;

@@ -823,6 +1246,10 @@ int v4l2_close(int fd)
{
int index, result;

+ if (fd == -2) {
+ my_main();
+ }
+
index = v4l2_get_index(fd);
if (index == -1)
return SYS_CLOSE(fd);
@@ -1782,3 +2209,65 @@ int v4l2_get_control(int fd, int cid)
(qctrl.maximum - qctrl.minimum) / 2) /
(qctrl.maximum - qctrl.minimum);
}
+
+void v4l2_debug(void)
+{
+ printf("debug\n");
+}
+
+/* ------------------------------------------------------------------ */
+
+#define SIZE 10*1024*1024
+
+char buf[SIZE];
+
+void my_main(void)
+{
+ int fd = v4l2_open("/dev/video2", O_RDWR);
+ int i;
+ static struct v4l2_format fmt;
+
+ fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_RGB24;
+ fmt.fmt.pix.field = V4L2_FIELD_INTERLACED;
+ fmt.fmt.pix.width = 640;
+ fmt.fmt.pix.height = 480;
+
+ v4l2_set_gain(fd, 300);
+ v4l2_set_exposure(fd, 10000);
+ v4l2_set_focus(fd, 0);
+
+
+ printf("ioctl = %d\n", v4l2_ioctl(fd, VIDIOC_S_FMT, &fmt));
+
+ printf("capture is %d, %d\n", fmt.fmt.pix.width, fmt.fmt.pix.height);
+ /* factor == 2 still fits window, but very slow. factor == 6 .. */
+ /* factor needs to be odd, otherwise ... fun with BA10 format. */
+ sdl_init(&sdl, fmt.fmt.pix.width, fmt.fmt.pix.height, 5);
+
+ v4l2_debug();
+
+ /* In 800x600 "raw" mode, this should take cca 17.8 seconds (without
+ sdl output. CPU usage should be cca 5% without conversion). That's 28 fps.
+ (benchmark with i<500)
+ */
+ for (i=0; i<50000; i++) {
+ int num = v4l2_read(fd, buf, SIZE);
+
+ if (i==490) {
+ printf("Focus to closest.... -------------------\n");
+ v4l2_set_focus(fd, 99999);
+ }
+
+#if 0
+ v4l2_paint(buf, 640/80, 480/25, &fmt);
+#endif
+ /* Over USB connection, rendering every single frame slows
+ execution down from 23 seconds to 36 seconds. */
+#if 0
+ if (!(i%4))
+ sdl_render(&sdl, buf, &fmt);
+#endif
+ }
+
+}
diff --git a/lib/libv4l2/sdl.c b/lib/libv4l2/sdl.c
new file mode 100644
index 0000000..17a8d24
--- /dev/null
+++ b/lib/libv4l2/sdl.c
@@ -0,0 +1,530 @@
+/* -*- c-file-style: "linux" -*- */
+/* SDL support.
+
+ Copyright 2017 Pavel Machek, LGPL
+*/
+
+#include <SDL2/SDL.h>
+#include <SDL2/SDL_image.h>
+
+struct sdl {
+ SDL_Window *window;
+ SDL_Surface *liveview, *screen;
+
+ int wx, wy;
+ int sx, sy;
+ int bx, by;
+ int factor;
+ float focus, gain, exposure, do_focus, do_exposure;
+};
+
+#if 0
+void loop(void) {
+ int done;
+ SDL_Event event;
+
+ while(!done){ //While program isn't done
+ while(SDL_PollEvent(&event)){ //Poll events
+ switch(event.type){ //Check event type
+ case SDL_QUIT: //User hit the X (or equivelent)
+ done = true; //Make the loop end
+ break; //We handled the event
+ } //Finished with current event
+ } //Done with all events for now
+ } //Program done, exited
+}
+#endif
+
+typedef struct {
+ Uint8 r;
+ Uint8 g;
+ Uint8 b;
+ Uint8 alpha;
+} pixel;
+
+#define d_raw 1
+
+void sfc_put_pixel(SDL_Surface* liveview, int x, int y, pixel *p)
+{
+ Uint32* p_liveview = (Uint32*)liveview->pixels;
+ p_liveview += y*liveview->w+x;
+ *p_liveview = SDL_MapRGBA(liveview->format,p->r,p->g,p->b,p->alpha);
+}
+
+#if 0
+int pix_exposure(float x)
+{
+ return sy*x / 199410.0;
+}
+
+int pix_gain(float x)
+{
+ return sy*x / 16.0;
+}
+
+int render_statistics(SDL_Surface* liveview)
+{
+ pixel white;
+ white.r = (Uint8)0xff;
+ white.g = (Uint8)0xff;
+ white.b = (Uint8)0xff;
+ white.alpha = (Uint8)128;
+
+ //printf("Stats: focus %d, gain %d, exposure %d\n", focus, gain, exposure);
+
+ for (int x=0; x<sx && x<sx*focus; x++)
+ put_pixel(liveview, x, 0, &white);
+
+ for (int y=0; y<sy && y<pix_gain(gain); y++)
+ put_pixel(liveview, 0, y, &white);
+
+ for (int y=0; y<sy && y<pix_exposure(exposure); y++)
+ put_pixel(liveview, sx-1, y, &white);
+
+ for (int x=0; x<sx; x++)
+ put_pixel(liveview, x, sy-1, &white);
+
+ return 0;
+}
+#endif
+
+void sdl_begin_paint(struct sdl *m) {
+ //Fill the surface white
+ SDL_FillRect(m->liveview, NULL, SDL_MapRGB( m->liveview->format, 0, 0, 0 ));
+
+ SDL_LockSurface(m->liveview);
+}
+
+void sdl_finish_paint(struct sdl *m) {
+ SDL_UnlockSurface(m->liveview);
+ SDL_Rect rcDest = { m->bx, m->by, m->sx, m->sy };
+
+ SDL_BlitSurface(m->liveview, NULL, m->screen, &rcDest);
+ //Update the surface
+ SDL_UpdateWindowSurfaceRects(m->window, &rcDest, 1);
+}
+
+void sdl_paint_image(struct sdl *m, char **xpm, int x, int y) {
+ SDL_Surface *image = IMG_ReadXPMFromArray(xpm);
+ if (!image) {
+ printf("IMG_Load: %s\n", IMG_GetError());
+ exit(1);
+ }
+
+ int x_pos = x - image->w/2, y_pos = y - image->h/2;
+
+ SDL_Rect rcDest = { x_pos, y_pos, image->w, image->h };
+ int r = SDL_BlitSurface ( image, NULL, m->screen, &rcDest );
+
+ if (r) {
+ printf("Error blitting: %s\n", SDL_GetError());
+ exit(1);
+ }
+ SDL_FreeSurface ( image );
+}
+
+void sdl_paint_ui(struct sdl *m) {
+ static char *wait_xpm[] = {
+ "16 9 2 1",
+ "# c #ffffff",
+ ". c #000000",
+ "....########....",
+ ".....#....#.....",
+ ".....#....#.....",
+ "......#..#......",
+ ".......##.......",
+ "......#..#......",
+ ".....#....#.....",
+ ".....#....#.....",
+ "....########....",
+ };
+
+ static char *ok_xpm[] = {
+ "16 9 2 1",
+ "# c #ffffff",
+ ". c #000000",
+ "...............#",
+ "............###.",
+ "..........##....",
+ "#.......##......",
+ ".#.....#........",
+ "..#...#.........",
+ "..#..#..........",
+ "...##...........",
+ "...#............",
+ };
+
+ static char *exit_xpm[] = {
+ "16 9 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ "....x......x....",
+ ".....x....x.....",
+ "......x..x......",
+ ".......xx.......",
+ ".......xx.......",
+ "......x..x......",
+ ".....x....x.....",
+ "....x......x....",
+ "................",
+ };
+
+ static char *f1m_xpm[] = {
+ "16 9 2 1",
+ "# c #ffffff",
+ ". c #000000",
+ "....##..........",
+ "...#.#..........",
+ "..#..#..........",
+ ".....#...#.#.##.",
+ ".....#...##.#..#",
+ ".....#...#..#..#",
+ ".....#...#..#..#",
+ ".....#...#..#..#",
+ "................",
+ };
+
+ static char *f25cm_xpm[] = {
+ "16 9 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ ".xxx..xxxx......",
+ "x...x.x.........",
+ "...x..xxx.......",
+ "..x......x..xx.x",
+ ".x.......x.x.xxx",
+ "xxxxx.xxx...xxxx",
+ "................",
+ "................",
+ "................",
+ };
+
+ static char *iso400_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ "x..x.xxxx.xxxx..",
+ "x..x.x..x.x..x..",
+ "xxxx.x..x.x..x..",
+ "...x.x..x.x..x..",
+ "...x.xxxx.xxxx..",
+ "................",
+ ".x..xx..x.......",
+ ".x.x...x.x......",
+ ".x..x..x.x......",
+ ".x...x.x.x......",
+ ".x.xx...x.......",
+ "................",
+ };
+
+ static char *time_1_100_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ ".x....x.........",
+ ".x...x..........",
+ ".x..x...........",
+ ".x.x............",
+ "................",
+ "..x.xxxx.xxxx...",
+ "..x.x..x.x..x...",
+ "..x.x..x.x..x...",
+ "..x.x..x.x..x...",
+ "..x.x..x.x..x...",
+ "..x.xxxx.xxxx...",
+ "................",
+ };
+
+ static char *time_1_10_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ ".x....x.........",
+ ".x...x..........",
+ ".x..x...........",
+ ".x.x............",
+ "................",
+ "..x.xxxx........",
+ "..x.x..x........",
+ "..x.x..x........",
+ "..x.x..x........",
+ "..x.x..x........",
+ "..x.xxxx........",
+ "................",
+ };
+
+ static char *af_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ ".....xxxxxxx....",
+ ".....x..........",
+ ".....x..........",
+ ".x...xxxxx......",
+ "x.x..x..........",
+ "xxx..x..........",
+ "x.x..x..........",
+ "x.x..x..........",
+ "................",
+ "................",
+ "................",
+ "................",
+ };
+
+ static char *ae_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ ".....xxxxxxx....",
+ ".....x..........",
+ ".....x..........",
+ ".x...xxxxx......",
+ "x.x..x..........",
+ "xxx..x..........",
+ "x.x..x..........",
+ "x.x..xxxxxxx....",
+ "................",
+ "................",
+ "................",
+ "................",
+ };
+
+ static char *not_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ "......xxxxx.....",
+ "....xx.....xx...",
+ "...x.........x..",
+ "..x........xx.x.",
+ "..x......xx...x.",
+ ".x.....xx......x",
+ ".x...xx........x",
+ "..xxx.........x.",
+ "..x...........x.",
+ "...x.........x..",
+ "....xx.....xx...",
+ "......xxxxx.....",
+ };
+
+ static char *empty_xpm[] = {
+ "16 12 2 1",
+ "x c #ffffff",
+ ". c #000000",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ "................",
+ };
+
+ SDL_FillRect(m->screen, NULL, SDL_MapRGB( m->liveview->format, 0, 0, 0 ));
+ sdl_paint_image(m, wait_xpm, m->wx/2, m->wy/2);
+ sdl_paint_image(m, ok_xpm, m->wx-m->bx/2, m->wy-m->by/2);
+ sdl_paint_image(m, exit_xpm, m->bx/2, m->wy-m->by/2);
+ sdl_paint_image(m, f1m_xpm, m->bx+m->sx/20, m->by/2);
+ sdl_paint_image(m, f25cm_xpm, m->bx+m->sx/5, m->by/2);
+
+ sdl_paint_image(m, af_xpm, m->bx/2, m->by/2);
+ if (!m->do_focus) {
+ sdl_paint_image(m, not_xpm, 16+m->bx/2, m->by/2);
+ }
+ sdl_paint_image(m, ae_xpm, m->wx-m->bx/2, m->by/2);
+ if (!m->do_exposure) {
+ sdl_paint_image(m, not_xpm, 16+m->bx/2, m->by/2);
+ }
+
+#if 0
+ sdl_paint_image(m, time_1_100_xpm, m->wx-m->bx/2, m->by+pix_exposure(10000));
+ sdl_paint_image(m, time_1_10_xpm, m->wx-m->bx/2, m->by+pix_exposure(100000));
+ sdl_paint_image(m, iso400_xpm, m->bx/2, m->by+pix_gain(4.0));
+#endif
+
+ SDL_UpdateWindowSurface(m->window);
+}
+
+void fmt_print(struct v4l2_format *fmt)
+{
+ int f;
+ printf("Format: %dx%d. ", fmt->fmt.pix.width, fmt->fmt.pix.height);
+ printf("%x ", fmt->fmt.pix.pixelformat);
+ f = fmt->fmt.pix.pixelformat;
+ for (int i=0; i<4; i++) {
+ printf("%c", f & 0xff);
+ f >>= 8;
+ }
+ printf("\n");
+}
+
+pixel buf_pixel(struct v4l2_format *fmt, unsigned char *buf, int x, int y)
+{
+ pixel p = { 0, 0, 0, 0 };
+ int pos = x + y*fmt->fmt.pix.width;
+ int b;
+
+ p.alpha = 128;
+
+ switch (fmt->fmt.pix.pixelformat) {
+ case '01AB': /* BA10, aka GRBG10,
+ https://www.linuxtv.org/downloads/v4l-dvb-apis-new/uapi/v4l/pixfmt-srggb10.html?highlight=ba10
+ */
+ b = buf[pos*2];
+ b += buf[pos*2+1] << 8;
+ b /= 4;
+ if ((y % 2) == 0) {
+ switch (x % 2) {
+ case 0: p.g = b/2; break;
+ case 1: p.r = b; break;
+ }
+ } else {
+ switch (x % 2) {
+ case 0: p.b = b; break;
+ case 1: p.g = b/2; break;
+ }
+ }
+ break;
+
+ case V4L2_PIX_FMT_RGB24:
+ pos *= 4;
+ p.r = buf[pos];
+ p.g = buf[pos+1];
+ p.b = buf[pos+2];
+ break;
+
+ default:
+ printf("Wrong pixel format!\n");
+ fmt_print(fmt);
+ exit(1);
+ }
+
+ return p;
+}
+
+void sdl_render(struct sdl *m, unsigned char *buf, struct v4l2_format *fmt)
+{
+ if (!m->window)
+ return;
+ sdl_begin_paint(m);
+ /* do your rendering here */
+
+ for (int y = 0; y < m->sy; y++)
+ for (int x = 0; x < m->sx; x++) {
+ pixel p = buf_pixel(fmt, buf, x*m->factor, y*m->factor);
+ p.alpha = 128;
+ sfc_put_pixel(m->liveview, x, y, &p);
+ }
+
+ //render_statistics(liveview);
+
+ sdl_finish_paint(m);
+}
+
+#if 0
+void imageStatistics(FCam::Image img)
+{
+ int sx, sy;
+
+ sx = img.size().width;
+ sy = img.size().height;
+ printf("Image is %d x %d, ", sx, sy);
+
+ printf("(%d) ", img.brightness(sx/2, sy/2));
+
+ int dark = 0;
+ int bright = 0;
+ int total = 0;
+#define RATIO 10
+ for (int y = sy/(2*RATIO); y < sy; y += sy/RATIO)
+ for (int x = sx/(2*RATIO); x < sx; x += sx/RATIO) {
+ int br = img.brightness(x, y);
+
+ if (!d_raw) {
+ /* It seems real range is 60 to 218 */
+ if (br > 200)
+ bright++;
+ if (br < 80)
+ dark++;
+ } else {
+ /* there's a lot of noise, it seems black is commonly 65..71,
+ bright is cca 1023 */
+ if (br > 1000)
+ bright++;
+ if (br < 75)
+ dark++;
+ }
+ total++;
+ }
+
+ printf("%d dark %d bri,", dark, bright);
+
+ long sharp = 0;
+ for (int y = sy/3; y < 2*(sy/3); y+=sy/12) {
+ int b = -1;
+ for (int x = sx/3; x < 2*(sx/3); x++) {
+ int b2;
+ b2 = img.brightness(x, y/2);
+ if (b != -1)
+ sharp += (b-b2) * (b-b2);
+ b = b2;
+ }
+ }
+ printf("sh %d\n", sharp);
+}
+#endif
+
+
+void sdl_init(struct sdl *m, int _sx, int _sy, int _factor)
+{
+ printf("Initing SDL\n");
+
+ if (SDL_Init(SDL_INIT_VIDEO) < 0) {
+ printf("Could not init SDL\n");
+ exit(1);
+ }
+
+ atexit(SDL_Quit);
+
+ m->wx = 800;
+ m->wy = 400;
+
+ m->window = SDL_CreateWindow( "Camera", SDL_WINDOWPOS_UNDEFINED,
+ SDL_WINDOWPOS_UNDEFINED, m->wx, m->wy,
+ SDL_WINDOW_SHOWN );
+ if (m->window == NULL) {
+ printf( "Window could not be created! SDL_Error: %s\n", SDL_GetError() );
+ }
+
+ m->screen = SDL_GetWindowSurface(m->window);
+ if (!m->screen) {
+ printf("Couldn't create liveview\n");
+ exit(1);
+ }
+
+ m->sx = _sx;
+ m->sy = _sy;
+ m->factor = _factor;
+
+ m->sx /= m->factor;
+ m->sy /= m->factor;
+
+ m->focus = 0;
+ m->gain = 0;
+ m->exposure = 0;
+
+ m->bx = (m->wx-m->sx)/2;
+ m->by = (m->wy-m->sy)/2;
+
+ m->liveview = SDL_CreateRGBSurface(0,m->sx,m->sy,32,0,0,0,0);
+ if (!m->liveview) {
+ printf("Couldn't create liveview\n");
+ exit(1);
+ }
+ sdl_paint_ui(m);
+}
diff --git a/lib/libv4lconvert/libv4lconvert.c b/lib/libv4lconvert/libv4lconvert.c
index d3d8936..7521ec8 100644
--- a/lib/libv4lconvert/libv4lconvert.c
+++ b/lib/libv4lconvert/libv4lconvert.c
@@ -1205,6 +1205,8 @@ static int v4lconvert_convert_pixfmt(struct v4lconvert_data *data,
v4lconvert_swap_uv(src, dest, fmt);
break;
}
+ default:
+ /* This is bad, fixme? */
break;

case V4L2_PIX_FMT_YVU420:
@@ -1228,6 +1230,8 @@ static int v4lconvert_convert_pixfmt(struct v4lconvert_data *data,
case V4L2_PIX_FMT_YVU420:
memcpy(dest, src, width * height * 3 / 2);
break;
+ default:
+ /* This is bad, fixme? */
}
break;

diff --git a/utils/decode_tm6000/Makefile.am b/utils/decode_tm6000/Makefile.am
index ac4e85e..f059e3c 100644
--- a/utils/decode_tm6000/Makefile.am
+++ b/utils/decode_tm6000/Makefile.am
@@ -1,4 +1,4 @@
bin_PROGRAMS = decode_tm6000
decode_tm6000_SOURCES = decode_tm6000.c
-decode_tm6000_LDADD = ../libv4l2util/libv4l2util.la
+decode_tm6000_LDADD = ../libv4l2/libv4l2.la
decode_tm6000_LDFLAGS = $(ARGP_LIBS)
diff --git a/utils/decode_tm6000/decode_tm6000.c b/utils/decode_tm6000/decode_tm6000.c
index 4bffbdd..fda7e3b 100644
--- a/utils/decode_tm6000/decode_tm6000.c
+++ b/utils/decode_tm6000/decode_tm6000.c
@@ -1,365 +1,16 @@
-/*
- decode_tm6000.c - decode multiplexed format from TM5600/TM6000 USB
+/* gcc asciicam.c /usr/lib/i386-linux-gnu/libv4l2.so.0.0.0 -o asciicam
+ gcc asciicam.c /usr/lib/arm-linux-gnueabi/libv4l2.so.0 -o asciicam

- Copyright (C) 2007 Mauro Carvalho Chehab <mchehab@infradead.org>
-
- 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 version 2.
-
- 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., 51 Franklin Street, Suite 500, Boston, MA 02110-1335 USA.
+gcc -std=gnu99 -DHAVE_CONFIG_H -I. -I../.. -fvisibility=hidden -I../../lib/include -Wall -Wpointer-arith -D_GNU_SOURCE -I../../include -g -O2 asciicam.c libv4l2.c /usr/lib/arm-linux-gnueabi/libv4lconvert.so.0 log.c v4l2convert.c v4l2-plugin.c -o asciicam
*/
-#include "../libv4l2util/v4l2_driver.h"
#include <stdio.h>
-#include <string.h>
-#include <argp.h>
-#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <fcntl.h>
-#include <stdlib.h>
-#include <errno.h>
-
-const char *argp_program_version="decode_tm6000 version 0.0.1";
-const char *argp_program_bug_address="Mauro Carvalho Chehab <mchehab@infradead.org>";
-const char doc[]="Decodes tm6000 proprietary format streams";
-const struct argp_option options[] = {
- {"verbose", 'v', 0, 0, "enables debug messages", 0},
- {"device", 'd', "DEV", 0, "uses device for reading", 0},
- {"output", 'o', "FILE", 0, "outputs raw stream to a file", 0},
- {"input", 'i', "FILE", 0, "parses a file, instead of a device", 0},
- {"freq", 'f', "Freq", 0, "station frequency, in MHz (default is 193.25)", 0},
- {"nbufs", 'n', "quant",0, "number of video buffers", 0},
- {"audio", 'a', 0, 0, "outputs audio on stdout", 0},
- {"read", 'r', 0, 0, "use read() instead of mmap method", 0},
- { 0, 0, 0, 0, 0, 0 }
-};
-
-static char outbuf[692224];
-static int debug=0, audio=0, use_mmap=1, nbufs=4;
-static float freq_mhz=193.25;
-static char *devicename="/dev/video0";
-static char *filename=NULL;
-static enum {
- NORMAL,
- INPUT,
- OUTPUT
-} mode = NORMAL;
-
-static FILE *fout;
-
-//const char args_doc[]="ARG1 ARG2";
-
-static error_t parse_opt (int key, char *arg, struct argp_state *state)
-{
- switch (key) {
- case 'a':
- audio++;
- break;
- case 'r':
- use_mmap=0;
- break;
- case 'v':
- debug++;
- break;
- case 'd':
- devicename=arg;
- break;
- case 'i':
- case 'o':
- if (mode!=NORMAL) {
- argp_error(state,"You can't use input/output options simultaneously.\n");
- break;
- }
- if (key=='i')
- mode=INPUT;
- else
- mode=OUTPUT;
-
- filename=arg;
- break;
- case 'f':
- freq_mhz=atof(arg);
- break;
- case 'n':
- nbufs=atoi(arg);
- if (nbufs<2)
- nbufs=2;
- break;
- default:
- return ARGP_ERR_UNKNOWN;
- }
- return 0;
-}
-
-static struct argp argp = {
- .options=options,
- .parser=parse_opt,
- .args_doc=NULL,
- .doc=doc,
-};

-#define TM6000_URB_MSG_LEN 180
-enum {
- TM6000_URB_MSG_VIDEO=1,
- TM6000_URB_MSG_AUDIO,
- TM6000_URB_MSG_VBI,
- TM6000_URB_MSG_PTS,
- TM6000_URB_MSG_ERR,
-};
-
-const char *tm6000_msg_type[]= {
- "unknown(0)", //0
- "video", //1
- "audio", //2
- "vbi", //3
- "pts", //4
- "err", //5
- "unknown(6)", //6
- "unknown(7)", //7
-};
-
-#define dprintf(fmt,arg...) \
- if (debug) fprintf(stderr, fmt, ##arg)
-
-static int recebe_buffer (struct v4l2_buffer *v4l2_buf, struct v4l2_t_buf *buf)
-{
- dprintf("Received %zd bytes\n", buf->length);
-fflush(stdout);
- memcpy (outbuf,buf->start,buf->length);
- return buf->length;
-}
-
-
-static int prepare_read (struct v4l2_driver *drv)
-{
- struct v4l2_format fmt;
- double freq;
- int rc;
-
- memset (drv,0,sizeof(*drv));
-
- if (v4l2_open (devicename, 1,drv)<0) {
- perror ("Error opening dev");
- return -1;
- }
-
- memset (&fmt,0,sizeof(fmt));
-
- uint32_t pixelformat=V4L2_PIX_FMT_TM6000;
-
- if (v4l2_gettryset_fmt_cap (drv,V4L2_SET,&fmt, 720, 480,
- pixelformat,V4L2_FIELD_ANY)) {
- perror("set_input to tm6000 raw format");
- return -1;
- }
-
- if (freq_mhz) {
- freq=freq_mhz * 1000 * 1000;
- rc=v4l2_getset_freq (drv,V4L2_SET, &freq);
- if (rc<0)
- printf ("Cannot set freq to %.3f MHz\n",freq_mhz);
- }
-
- if (use_mmap) {
- printf("Preparing for receiving frames on %d buffers...\n",nbufs);
- fflush (stdout);
-
- rc=v4l2_mmap_bufs(drv, nbufs);
- if (rc<0) {
- printf ("Cannot mmap %d buffers\n",nbufs);
- return -1;
- }
-
-// v4l2_stop_streaming(&drv);
- rc=v4l2_start_streaming(drv);
- if (rc<0) {
- printf ("Cannot start streaming\n");
- return -1;
- }
- }
- printf("Waiting for frames...\n");
-
- return 0;
-}
+#include <linux/videodev2.h>

-static int read_stream (struct v4l2_driver *drv, int fd)
+void main(void)
{
- if (use_mmap) {
- fd_set fds;
- struct timeval tv;
- int r;
-
- FD_ZERO (&fds);
- FD_SET (fd, &fds);
-
- /* Timeout. */
- tv.tv_sec = 2;
- tv.tv_usec = 0;
-
- r = select (fd + 1, &fds, NULL, NULL, &tv);
- if (-1 == r) {
- if (EINTR == errno) {
- perror ("select");
- return -errno;
- }
- }
-
- if (0 == r) {
- fprintf (stderr, "select timeout\n");
- return -errno;
- }
-
- return v4l2_rcvbuf(drv, recebe_buffer);
- } else {
- int size=read(fd, outbuf, sizeof(outbuf));
- return size;
- }
-
- return 0;
-}
-
-static int read_char (struct v4l2_driver *drv, int fd)
-{
- static int sizebuf=0;
- static unsigned char *p=NULL;
- unsigned char c;
-
- if (sizebuf<=0) {
- sizebuf=read_stream(drv,fd);
- if (sizebuf<=0)
- return -1;
- p=(unsigned char *)outbuf;
- }
- c=*p;
- p++;
- sizebuf--;
-
- return c;
-}
-
-
-int main (int argc, char*argv[])
-{
- int fd;
- unsigned int i;
- unsigned char buf[TM6000_URB_MSG_LEN], img[720*2*480];
- unsigned int cmd, size, field, block, line, pos=0;
- unsigned long header=0;
- int linesize=720*2,skip=0;
- struct v4l2_driver drv;
-
- argp_parse (&argp, argc, argv, 0, 0, 0);
-
- if (mode!=INPUT) {
- if (prepare_read (&drv)<0)
- return -1;
- fd=drv.fd;
- } else {
- /*mode == INPUT */
-
- fd=open(filename,O_RDONLY);
- if (fd<0) {
- perror ("error opening a file for parsing");
- return -1;
- }
- dprintf("file %s opened for parsing\n",filename);
- use_mmap=0;
- }
-
- if (mode==OUTPUT) {
- fout=fopen(filename,"w");
- if (!fout) {
- perror ("error opening a file to write");
- return -1;
- }
- dprintf("file %s opened for output\n",filename);
-
- do {
- size=read_stream (&drv,fd);
-
- if (size<=0) {
- close (fd);
- return -1;
- }
- dprintf("writing %d bytes\n",size);
- fwrite(outbuf,1, size,fout);
-// fflush(fout);
- } while (1);
- }
-
-
- while (1) {
- skip=0;
- header=0;
- do {
- int c;
- c=read_char (&drv,fd);
- if (c<0) {
- perror("read");
- return -1;
- }
-
- header=(header>>8)&0xffffff;
- header=header|(c<<24);
- skip++;
- } while ( (((header>>24)&0xff) != 0x47) );
-
- /* split the header fields */
- size = (((header & 0x7e)<<1) -1) * 4;
- block = (header>>7) & 0xf;
- field = (header>>11) & 0x1;
- line = (header>>12) & 0x1ff;
- cmd = (header>>21) & 0x7;
-
- /* Read the remaining buffer */
- for (i=0;i<sizeof(buf);i++) {
- int c;
- c=read_char (&drv,fd);
- if (c<0) {
- perror("read");
- return -1;
- }
- buf[i]=c;
- }
-
- /* FIXME: Mounts the image as field0+field1
- * It should, instead, check if the user selected
- * entrelaced or non-entrelaced mode
- */
- pos=((line<<1)+field)*linesize+
- block*TM6000_URB_MSG_LEN;
-
- /* Prints debug info */
- dprintf("0x%08x (skip %d), %s size=%d, line=%d, field=%d, block=%d\n",
- (unsigned int)header, skip,
- tm6000_msg_type[cmd],
- size, line, field, block);
-
- /* Don't allow to write out of the buffer */
- if (pos+sizeof(buf) > sizeof(img))
- cmd = TM6000_URB_MSG_ERR;
-
- /* handles each different URB message */
- switch(cmd) {
- case TM6000_URB_MSG_VIDEO:
- /* Fills video buffer */
- memcpy(buf,&img[pos],sizeof(buf));
- case TM6000_URB_MSG_AUDIO:
- if (audio)
- fwrite(buf,sizeof(buf),1,stdout);
-// case TM6000_URB_MSG_VBI:
-// case TM6000_URB_MSG_PTS:
- break;
- }
- }
- close(fd);
- return 0;
+ v4l2_close(-2);
}