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);
 }
