path: root/live.c

/*
 * OMAP3 live video display test application
 *
 * Copyright (C) 2010-2011 Ideas on board SPRL
 *
 * Contact: Laurent Pinchart <laurent.pinchart@ideasonboard.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.,
 * 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 */

#define _BSD_SOURCE
#include <ctype.h>
#include <dirent.h>
#include <endian.h>
#include <errno.h>
#include <fcntl.h>
#include <getopt.h>
#include <signal.h>
#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <time.h>
#include <unistd.h>

#include <sys/ioctl.h>
#include <sys/mman.h>
#include <sys/stat.h>
#include <sys/time.h>
#include <sys/types.h>

#include <linux/fb.h>
#include <linux/input.h>
#include <linux/spi/spidev.h>

#include "isp/list.h"
#include "isp/omap3isp.h"
#include "isp/stats.h"
#include "isp/tools.h"
#include "isp/v4l2.h"
#include "isp/v4l2-pool.h"

#include "events.h"
#include "iq.h"
#include "videoout.h"

#define MEDIA_DEVICE		"/dev/media0"
#define VIDEOOUT_DEVICE		"/dev/video0"

#define SNAPSHOT_WIDTH		2048
#define SNAPSHOT_HEIGHT		1536

static struct timespec fps_ts = { 0, 0 };
static unsigned int fps_last = 0;
static bool fps_show = true;

static unsigned int frame_count = 0;
static unsigned int frame_skip = 0;

static struct omap3_isp_device *isp = NULL;
static struct videoout *vo = NULL;
static unsigned int vo_bufmask = -1;
static struct iq_tuning *iq = NULL;

enum state {
	STATE_VIEWFINDER,
	STATE_SNAPSHOT,
	STATE_DISPLAY,
};

static enum state state = STATE_VIEWFINDER;

static struct events events;

struct input {
	int fd;
	float zoom;
	float x;
	float y;
};

static struct input input;

/* -----------------------------------------------------------------------------
 * Image quality
 */

static void __iq_aewb_process(struct omap3_isp_device *isp __attribute__((__unused__)),
			      const struct omap3_isp_aewb_stats *stats)
{
	iq_aewb_process(iq, stats);
}

/* -----------------------------------------------------------------------------
 * Events
 */

static void __events_watch_fd(int fd, enum omap3_isp_event_type type,
			      void(*callback)(void *), void *priv)
{
	events_watch_fd(&events, fd, type, callback, priv);
}

static void __events_unwatch_fd(int fd)
{
	events_unwatch_fd(&events, fd);
}

/* -----------------------------------------------------------------------------
 * Snapshots
 */

static void scaler_process(struct omap3_isp_device *isp,
			   struct v4l2_video_buffer *buffer)
{
	if (state != STATE_DISPLAY)
		return;

	if (buffer->error) {
		printf("%s: error in dequeued buffer, skipping\n", __func__);
		omap3_isp_scaler_put_buffer(isp, buffer);
		return;
	}

	/* Queue the buffer to the display. */
	vo_queue_buffer(vo, buffer);
	vo_bufmask &= ~(1 << buffer->index);
}

static void snapshot_process(struct omap3_isp_device *isp,
			     struct v4l2_video_buffer *buffer)
{
	int ret;


	if (buffer->error) {
		printf("%s: error in dequeued buffer, skipping\n", __func__);
		/* Requeue the buffer */
		ret = omap3_isp_snapshot_put_buffer(isp, buffer);
		if (ret < 0) {
			printf("error: unable to requeue buffer: %s (%d)\n",
				strerror(-ret), ret);
			state = STATE_VIEWFINDER;
		}

		return;
	}

	ret = omap3_isp_snapshot_done(isp);
	if (ret < 0) {
		printf("error: unable to stop snapshot: %s (%d)\n",
			strerror(-ret), ret);
		return;
	}

	printf("displaying snapshot\n");
	state = STATE_DISPLAY;
	input.zoom = 1.0;
	input.x = 0.5;
	input.y = 0.5;

	ret = omap3_isp_scaler_start(isp, buffer, vo_bufmask);
	if (ret < 0) {
		printf("error: unable to start scaler: %s (%d)\n",
			strerror(-ret), ret);
		return;
	}
}

static int snapshot_init(struct omap3_isp_device *isp,
			 const struct v4l2_rect *rect)
{
	struct v4l2_mbus_framefmt ifmt;
	struct v4l2_mbus_framefmt ofmt;
	struct v4l2_buffers_pool *pool;
	int ret;

	/* Snapshot */
	ifmt.code = V4L2_MBUS_FMT_YUYV8_1X16;
	ifmt.width = SNAPSHOT_WIDTH;
	ifmt.height = SNAPSHOT_HEIGHT;

	ret = omap3_isp_snapshot_setup(isp, NULL, &ifmt);
	if (ret < 0) {
		printf("error: unable to setup snapshot\n");
		return ret;
	}

	printf("snapshot configured for %04x %ux%u\n",
	       ifmt.code, ifmt.width, ifmt.height);

	/* Scaler */
	ofmt.code = V4L2_MBUS_FMT_YUYV8_1X16;
	ofmt.width = rect->width;
	ofmt.height = rect->height;

	ret = omap3_isp_scaler_setup(isp, &ifmt, &ofmt);
	if (ret < 0) {
		printf("error: unable to setup scaler\n");
		return ret;
	}

	/* Use the video output buffers pool. */
	pool = vo_get_pool(vo);
	if (pool == NULL) {
		printf("error: unable to get video output buffers pool\n");
		return ret;
	}

	ret = omap3_isp_scaler_set_pool(isp, pool);
	if (ret < 0) {
		printf("error: unable to set scaler buffers pool\n");
		return ret;
	}

	return 0;
}


/* -----------------------------------------------------------------------------
 * Viewfinder
 */

static void viewfinder_process(struct omap3_isp_device *isp __attribute__((__unused__)),
			       struct v4l2_video_buffer *buffer)
{
	struct timespec ts;
	float interval;
	int ret;

	frame_count++;

	if (fps_show){
		clock_gettime(CLOCK_MONOTONIC, &ts);

		interval = ts.tv_sec - fps_ts.tv_sec;
		interval += (ts.tv_nsec - fps_ts.tv_nsec) / 1000000000.0;

		if (interval >= 5) {
			if (frame_count > 1)
				printf("frame rate: %f fps\n",
				      (frame_count - fps_last) / interval);

			fps_last = frame_count;
			fps_ts = ts;
		}
	}

	if (buffer->error) {
		printf("%s: error in dequeued buffer, skipping\n", __func__);
		goto requeue;
	}

	/* Queue the buffer to the display, or requeue it to the viewfinder if
	 * it needs to be skipped.
	 */
	if ((frame_count % (frame_skip + 1)) == 0) {
		vo_queue_buffer(vo, buffer);
		vo_bufmask &= ~(1 << buffer->index);
		return;
	}

requeue:
	ret = omap3_isp_viewfinder_put_buffer(isp, buffer);
	if (ret < 0)
		printf("error: unable to requeue buffer: %s (%d)\n",
		       strerror(-ret), ret);
}

static struct omap3_isp_operations isp_ops = {
	.viewfinder_ready = viewfinder_process,
	.snapshot_ready = snapshot_process,
	.scaler_ready = scaler_process,
	.watch_fd = __events_watch_fd,
	.unwatch_fd = __events_unwatch_fd,
	.aewb_ready = __iq_aewb_process,
};

static int viewfinder_init(struct omap3_isp_device *isp,
			   const struct v4l2_rect *rect)
{
	struct v4l2_mbus_framefmt view_format;
	struct v4l2_buffers_pool *pool;
	int ret;

	memset(&view_format, 0, sizeof view_format);
	view_format.code = V4L2_MBUS_FMT_YUYV8_1X16;
	view_format.width = rect->width;
	view_format.height = rect->height;

	ret = omap3_isp_viewfinder_setup(isp, NULL, &view_format);
	if (ret < 0) {
		printf("error: unable to setup viewfinder\n");
		return ret;
	}

	printf("viewfinder configured for %04x %ux%u\n",
	       view_format.code, view_format.width, view_format.height);

	/* Use the video output buffers pool. */
	pool = vo_get_pool(vo);
	if (pool == NULL) {
		printf("error: unable to get video output buffers pool\n");
		return ret;
	}

	ret = omap3_isp_viewfinder_set_pool(isp, pool);
	if (ret < 0) {
		printf("error: unable to set viewfinder buffers pool\n");
		return ret;
	}

	return 0;
}

/* -----------------------------------------------------------------------------
 * Input device
 */

static void input_process_key(struct input_event *iev)
{
	if (iev->value != 1)
		return;

	switch (iev->code) {
	case BTN_LEFT:
		/* Capture a snapshot */
		if (state == STATE_VIEWFINDER) {
			printf("taking snapshot\n");
			omap3_isp_viewfinder_stop(isp);
			omap3_isp_snapshot_capture(isp);
			state = STATE_SNAPSHOT;
		}
		break;

	case BTN_RIGHT:
		/* Resume the viewfinder */
		if (state == STATE_DISPLAY) {
			printf("resuming viewfinder (%08x)\n", vo_bufmask);
			omap3_isp_scaler_stop(isp);
			omap3_isp_viewfinder_start(isp, vo_bufmask);
			input.zoom = 1.0;
			input.x = 0.5;
			input.y = 0.5;
			state = STATE_VIEWFINDER;
		}
		break;
	}
}

static void input_process_rel(struct input *input, struct input_event *iev)
{
	switch (iev->code) {
	case REL_X:
		input->x += iev->value * 0.01;
		input->x = clamp(input->x, 0.0, 1.0);
		break;

	case REL_Y:
		input->y += iev->value * 0.01;
		input->y = clamp(input->y, 0.0, 1.0);
		break;

	case REL_WHEEL:
		input->zoom += iev->value * 0.1;
		input->zoom = clamp(input->zoom, 1.0, 4.0);
		break;
	}
}

static void input_process(void *priv)
{
	struct input_event iev[8];
	struct input *input = priv;
	float last_x = input->x;
	float last_y = input->y;
	float last_zoom = input->zoom;
	unsigned int i;
	ssize_t nbytes;

	nbytes = read(input->fd, iev, sizeof iev);

	for (i = 0; i < nbytes / sizeof iev[0]; ++i) {
		switch (iev[i].type) {
		case EV_KEY:
			input_process_key(&iev[i]);
			break;
		case EV_REL:
			input_process_rel(input, &iev[i]);
			break;
		}
	}

	if (input->x == last_x && input->y == last_y &&
	    input->zoom == last_zoom)
		return;

	if (state == STATE_VIEWFINDER)
		omap3_isp_viewfinder_pan_zoom(isp, input->x, input->y,
					      input->zoom);
	else if (STATE_DISPLAY)
		omap3_isp_scaler_pan_zoom(isp, input->x, input->y,
					  input->zoom);
}

static const char *input_find_device(void)
{
	unsigned int rel_mask = (1 << REL_X) | (1 << REL_Y) | (1 << REL_WHEEL);
	unsigned int input_idx = 256;
	unsigned int major;
	unsigned int minor;
	struct stat devstat;
	struct dirent *ent;
	static char path[50];
	char buffer[16];
	unsigned int rel;
	char *end;
	DIR *dir;
	int ret;
	int fd;

	/* Locate the first mouse device in sysfs that supports X, Y and wheel
	 * relative axis.
	 */
	dir = opendir("/sys/class/input");
	if (dir == NULL)
		return NULL;

	while ((ent = readdir(dir)) != NULL) {

		if (strncmp(ent->d_name, "event", 5))
			continue;

		sprintf(path, "/sys/class/input/%s/device/capabilities/rel",
			ent->d_name);

		fd = open(path, O_RDONLY);
		if (fd == -1)
			continue;

		ret = read(fd, buffer, sizeof buffer - 1);
		close(fd);
		if (ret < 0)
			continue;

		buffer[ret] = '\0';
		rel = strtoul(buffer, &end, 16);
		if (*end != '\0' && *end != '\n')
			continue;

		if ((rel & rel_mask) != rel_mask)
			continue;

		input_idx = strtoul(ent->d_name + 5, NULL, 10);
		break;
	}

	closedir(dir);

	if (input_idx == 256)
		return NULL;

	/* Read the device major and minor from sysfs. */
	sprintf(path, "/sys/class/input/event%u/dev", input_idx);
	fd = open(path, O_RDONLY);
	if (fd == -1)
		return NULL;

	ret = read(fd, buffer, sizeof buffer - 1);
	close(fd);
	if (ret < 0)
		return NULL;

	buffer[ret] = '\0';
	major = strtoul(buffer, &end, 10);
	if (*end != ':')
		return NULL;

	minor = strtoul(end + 1, &end, 10);
	if (!isspace(*end) && end != '\0')
		return NULL;

	/* Verify that the device node exists. udev might have reordered the
	 * device nodes, make sure the major/minor match as a sanity check. We
	 * should really use libudev.
	 */
	sprintf(path, "/dev/input/event%u", input_idx);
	ret = stat(path, &devstat);
	if (ret < 0)
		return NULL;

	if (major(devstat.st_rdev) != major || minor(devstat.st_rdev) != minor)
		return NULL;

	return path;
}

static int input_init(struct input *input)
{
	const char *devname;

	input->fd = -1;
	input->zoom = 1.0;
	input->x = 0.5;
	input->y = 0.5;

	devname = input_find_device();
	if (devname == NULL) {
		printf("No compatible input device found, disabling digital zoom\n");
		return -ENODEV;
	}

	input->fd = open(devname, O_RDWR);
	if (input->fd == -1)
		return -errno;

	events_watch_fd(&events, input->fd, OMAP3_ISP_EVENT_READ, input_process,
			input);

	return 0;
}

static void input_cleanup(struct input *input)
{
	if (input->fd == -1)
		return;

	events_unwatch_fd(&events, input->fd);
	close(input->fd);
}

/* -----------------------------------------------------------------------------
 * Video output
 */

static void video_out_process(void *priv __attribute__((__unused__)))
{
	struct v4l2_video_buffer buffer;
	int ret;

	ret = vo_dequeue_buffer(vo, &buffer);
	if (ret < 0)
		return;

	vo_bufmask |= 1 << buffer.index;

	/* Requeue the buffer */
	if (state == STATE_VIEWFINDER)
		ret = omap3_isp_viewfinder_put_buffer(isp, &buffer);
	else if (state == STATE_DISPLAY)
		ret = omap3_isp_scaler_put_buffer(isp, &buffer);
	else
		return;

	if (ret < 0) {
		printf("error: unable to requeue buffer: %s (%d)\n",
		       strerror(-ret), ret);
		return;
	}
}

static void events_watch_fd_vo(int fd)
{
	events_watch_fd(&events, fd, OMAP3_ISP_EVENT_WRITE, video_out_process, NULL);
}

static struct video_out_operations vo_ops = {
	.watch_fd = events_watch_fd_vo,
	.unwatch_fd = __events_unwatch_fd,
};

static const char *video_out_find(void)
{
	unsigned int video_idx = 256;
	static char devname[36];
	static char dev[20];
	unsigned int major;
	unsigned int minor;
	struct stat devstat;
	struct dirent *ent;
	char *end;
	DIR *dir;
	int ret;
	int fd;

	dir = opendir("/sys/bus/platform/devices/omap_vout/video4linux");
	if (dir == NULL)
		return NULL;

	while ((ent = readdir(dir)) != NULL) {
		unsigned int idx;

		if (strncmp(ent->d_name, "video", 5))
			continue;

		idx = strtoul(ent->d_name + 5, &end, 10);
		if (*end != '\0')
			continue;

		if (idx < video_idx)
			video_idx = idx;
	}

	closedir(dir);

	if (video_idx == 256)
		return NULL;

	/* Read the device major and minor from sysfs. */
	sprintf(devname, "/sys/class/video4linux/video%u/dev", video_idx);
	fd = open(devname, O_RDONLY);
	if (fd == -1)
		return NULL;

	ret = read(fd, dev, sizeof dev - 1);
	close(fd);
	if (ret < 0)
		return NULL;

	dev[ret] = '\0';
	major = strtoul(dev, &end, 10);
	if (*end != ':')
		return NULL;

	minor = strtoul(end + 1, &end, 10);
	if (!isspace(*end) && end != '\0')
		return NULL;

	/* Verify that the device node exists. udev might have reordered the
	 * device nodes, make sure the major/minor match as a sanity check. We
	 * should really use libudev.
	 */
	sprintf(devname, "/dev/video%u", video_idx);
	ret = stat(devname, &devstat);
	if (ret < 0)
		return NULL;

	if (major(devstat.st_rdev) != major || minor(devstat.st_rdev) != minor)
		return NULL;

	return devname;
}

/* -----------------------------------------------------------------------------
 * Frame buffer
 */

struct color_rgb24 {
	unsigned int value:24;
} __attribute__((__packed__));

static int fb_init(struct v4l2_rect *rect)
{
	struct color_24bpp ;

	struct fb_fix_screeninfo fix;
	struct fb_var_screeninfo var;
	unsigned int i;
	void *mem = NULL;
	int ret;
	int fd;

	fd = open("/dev/fb0", O_RDWR);
	if (fd == -1)
		return -ENODEV;

	ret = ioctl(fd, FBIOGET_FSCREENINFO, &fix);
	if (ret < 0) {
		printf("error: failed to get fixed screen info\n");
		goto done;
	}

	ret = ioctl(fd, FBIOGET_VSCREENINFO, &var);
	if (ret < 0) {
		printf("error: failed to get variable screen info\n");
		goto done;
	}

	mem = mmap(NULL, fix.smem_len, PROT_READ | PROT_WRITE, MAP_SHARED,
		   fd, 0);
	if (mem == MAP_FAILED) {
		printf("error: unable to map frame buffer\n");
		ret = -ENOMEM;
		goto done;
	}

	/* Fill the frame buffer with the background color. */
	printf("%u bpp\n", var.bits_per_pixel);
	switch (var.bits_per_pixel) {
	case 16: {
		uint16_t color = 0x11b6;
		uint16_t *pix = mem;

		for (i = 0; i < fix.smem_len; i += 2)
			*pix++ = color;

		ret = color;
		break;
	}

	case 24: {
		struct color_rgb24 color = { .value = 0x123456, };
		struct color_rgb24 *pix = mem;

		for (i = 0; i < fix.smem_len; i += 3)
			*pix++ = color;

		ret = color.value;
		break;
	}

	case 32: {
		uint32_t color = 0x00123456;
		uint32_t *pix = mem;

		for (i = 0; i < fix.smem_len; i += 4)
			*pix++ = color;

		ret = color;
		break;
	}

	default:
		printf("error: unsupported %ubpp color depth\n",
		       var.bits_per_pixel);
		ret = -EINVAL;
		goto done;
	}

	/* Return the frame buffer size. */
	rect->left = var.xoffset;
	rect->top = var.yoffset;
	rect->width = var.xres;
	rect->height = var.yres;

done:
	if (mem != NULL)
		munmap(mem, fix.smem_len);
	close(fd);

	return ret;
}

/* -------------------------------------------------------------------------- */

static void sigint_handler(int signal __attribute__((__unused__)))
{
	/* Stop the main loop when the user presses CTRL-C. */
	events_stop(&events);
}

static void usage(const char *argv0)
{
	printf("Usage: %s [options]\n", argv0);
	printf("Supported options:\n");
	printf("-b, --buffers n		Use n display buffers\n");
	printf("-h, --help		Show this help screen\n");
	printf("-s, --skip n		Skip display of n frames out of n+1\n");
	printf("    --aewb param=value	Set AEWB algorithm parameter 'param' to 'value'\n");
	printf("    --disable-aewb	Disable AEWB\n");
	printf("    --saturation value	Set the saturation value [0.0-2.0]\n");
	printf("\nSupported AEWB parameters are:\n");
 	printf("- ae-delay		Number of frames to skip at stream start before enabling AE\n");
	printf("- ae-interval		Number of frames between AE algorithm runs (>= 1)\n");
	printf("- exposure-def		Exposure time default value\n");
	printf("- exposure-min		Exposure time minimum value\n");
	printf("- exposure-max		Exposure time maximum value\n");
	printf("- gain-def		Sensor gain default value\n");
	printf("- gain-min		Sensor gain minimum value\n");
	printf("- gain-max		Sensor gain maximum value\n");
	printf("- mean-level		Mean luminance target level (float [0-1])\n");
	printf("- window-left		Statistics window left (float [0-1])\n");
	printf("- window-top		Statistics window top (float [0-1])\n");
	printf("- window-width		Statistics window width (float [0-1])\n");
	printf("- window-height		Statistics window height (float [0-1])\n");
}

#define OPT_AEWB_PARAM		256
#define OPT_SATURATION		257
#define OPT_AEWB_DISABLE	258

static struct option opts[] = {
	{ "aewb", 1, 0, OPT_AEWB_PARAM },
	{ "disable-aewb", 0, 0, OPT_AEWB_DISABLE },
	{ "help", 0, 0, 'h' },
	{ "saturation", 1, 0, OPT_SATURATION },
	{ "skip", 1, 0, 's' },
	{ 0, 0, 0, 0 }
};

int main(int argc __attribute__((__unused__)), char *argv[] __attribute__((__unused__)))
{
	struct v4l2_pix_format format;
	struct timespec start, end;
	struct iq_params iq_params;
	unsigned int buffers = 3;
	struct v4l2_rect rect;
	int exit_code = EXIT_FAILURE;
	const char *vo_devname;
	unsigned int colorkey;
	bool enable_aewb = true;
	float fps;
	int ret;
	int c;

	iq_params_init(&iq_params);

	while ((c = getopt_long(argc, argv, "b:hs:z", opts, NULL)) != -1) {
		switch (c) {
		case 'b':
			buffers = atoi(optarg);
			break;
		case 'h':
			usage(argv[0]);
			return 0;
		case 's':
			frame_skip = atoi(optarg);
			break;
		case OPT_AEWB_DISABLE:
			enable_aewb = false;
			break;
		case OPT_AEWB_PARAM:
			ret = iq_params_parse(&iq_params, optarg);
			if (ret < 0) {
				printf("%s AEWB argument %s.\n",
				       ret == -ERANGE ? "out-of-range" : "invalid",
				       optarg);
				return 1;
			}
			break;
		case OPT_SATURATION:
			iq_params.saturation = strtof(optarg, NULL);
			break;
		default:
			printf("Invalid option -%c\n", c);
			printf("Run %s -h for help.\n", argv[0]);
			return 1;
		}
	}

	events_init(&events);
	input_init(&input);

	memset(&rect, 0, sizeof rect);
	ret = fb_init(&rect);
	if (ret < 0) {
		printf("error: unable to initialize frame buffer\n");
		goto cleanup;
	}

	colorkey = ret;

	/* Register a signal handler for SIGINT, received when the user presses
	 * CTRL-C. This will allow the main loop to be interrupted, and resources
	 * to be freed cleanly.
	 */
	signal(SIGINT, sigint_handler);

	/* Initialize video output. */
	vo_devname = video_out_find();
	if (vo_devname == NULL) {
		printf("error: unable to find video output device\n");
		goto cleanup;
	}

	memset(&format, 0, sizeof format);
	format.pixelformat = V4L2_PIX_FMT_YUYV;
	format.width = rect.width;
	format.height = rect.height;
	vo = vo_init(vo_devname, &vo_ops, buffers, &format, 0);
	if (vo == NULL) {
		printf("error: unable to initialize video output\n");
		goto cleanup;
	}

	vo_enable_colorkey(vo, colorkey);

	/* Open the OMAP3 ISP device and setup the capture pipeline. */
	isp = omap3_isp_open(MEDIA_DEVICE, &isp_ops);
	if (isp == NULL) {
		printf("error: unable to open media device %s\n", MEDIA_DEVICE);
		goto cleanup;
	}

	ret = viewfinder_init(isp, &rect);
	if (ret < 0)
		goto cleanup;

	/* Configure snapshot, scaler and AEWB. */
	ret = snapshot_init(isp, &rect);
	if (ret < 0)
		goto cleanup;

	if (enable_aewb) {
		iq = iq_init(isp, &iq_params);
		if (iq == NULL) {
			printf("error: unable to initialize image quality tuning\n");
			goto cleanup;
		}
	}

	/* Start the ISP. */
	ret = omap3_isp_viewfinder_start(isp, vo_bufmask);
	if (ret < 0)
		goto cleanup;

	clock_gettime(CLOCK_MONOTONIC, &start);

	/* Main capture loop. */
	if (events_loop(&events))
		goto cleanup;

	clock_gettime(CLOCK_MONOTONIC, &end);

	/* Stop the ISP. */
	omap3_isp_viewfinder_stop(isp);

	/* Print some statistics. */
	end.tv_sec -= start.tv_sec;
	end.tv_nsec -= start.tv_nsec;
	if (end.tv_nsec < 0) {
		end.tv_sec--;
		end.tv_nsec += 1000000000;
	}

	fps = frame_count / (end.tv_sec + end.tv_nsec / 1000000000.0);

	printf("%u images processed in %lu.%06lu seconds (%f fps)\n",
	       frame_count, end.tv_sec, end.tv_nsec / 1000, fps);

	exit_code = EXIT_SUCCESS;

cleanup:
	input_cleanup(&input);

	/* Cleanup the ISP and video output resources. */
	if (isp)
		omap3_isp_close(isp);
	if (vo)
		vo_cleanup(vo);
	if (iq)
		iq_cleanup(iq);

	return exit_code;
}