path: root/isp-dsp.c

/*
 * Sample ISP & DSP application
 *
 * Copyright (C) 2010-2011 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 Street, Fifth Floor, Boston, MA 02110-1301 USA
 */

#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/select.h>
#include <sys/time.h>

#include <mediactl/media.h>
#include <mediactl/subdev.h>

#include "bridge/dmm_buffer.h"
#include "bridge/dsp_bridge.h"
#include "v4l2.h"

#define BUFFERS_COUNT		4

/* -----------------------------------------------------------------------------
 * DSP
 */

#define DSP_LIBRARY		"/lib/dsp/threshold.dll64P"

struct omap3_dsp_device {
	int handle;
	void *proc;
	struct dsp_uuid uuid;
	struct dsp_node *node;

	dmm_buffer_t *buffers[BUFFERS_COUNT];
};

static int omap3dsp_setup(struct omap3_dsp_device *dsp, struct v4l2_video_buffer *vbufs)
{
	/* Dummy UUID, must be identical to the one used in the DSP dll64P library */
	static const struct dsp_uuid uuid = { 0x3dac26d0, 0x6d4b, 0x11dd, 0xad, 0x8b,
					      { 0x08, 0x00, 0x20, 0x0c, 0x9a, 0x66 } };
	unsigned int i;
	int ret;

	/* Open the DSP, attach, register library and node and create the
	 * node.
	 */
	dsp->handle = dsp_open();
	if (dsp->handle < 0) {
		printf("error: failed to open DSP.\n");
		return dsp->handle;
	}

	ret = dsp_attach(dsp->handle, 0, NULL, &dsp->proc);
	if (!ret) {
		printf("error: DSP attach failed.\n");
		return -1;
	}

	dsp->uuid = uuid;

	ret = dsp_register(dsp->handle, &dsp->uuid, DSP_DCD_LIBRARYTYPE,
			   DSP_LIBRARY);
	if (!ret) {
		printf("error: DSP library registration failed.\n");
		return -1;
	}

	ret = dsp_register(dsp->handle, &dsp->uuid, DSP_DCD_NODETYPE,
			   DSP_LIBRARY);
	if (!ret) {
		printf("error: DSP library registration failed.\n");
		return -1;
	}

	ret = dsp_node_allocate(dsp->handle, dsp->proc, &dsp->uuid, NULL, NULL,
				&dsp->node);
	if (!ret) {
		printf("error: DSP node allocation failed.\n");
		return -1;
	}

	ret = dsp_node_create(dsp->handle, dsp->node);
	if (!ret) {
		printf("error: DSP node creation failed.\n");
		return -1;
	}

	/* Allocate and map the buffers. The threshold algorithm performs
	 * in-place processing, so we can use the same buffer for both the
	 * CPU-DSP and DSP-CPU direction.
	 *
	 * Use the V4L2 buffers memory for the DSP buffers, avoiding a costly
	 * memcpy() operation. An alternative with similar efficiency would be
	 * to allocated DSP buffers memory here and use it as USERPTR for video
	 * capture.
	 */
	for (i = 0; i < BUFFERS_COUNT; ++i) {
		dmm_buffer_t *buffer;

		buffer = dmm_buffer_new(dsp->handle, dsp->proc,
					DMA_BIDIRECTIONAL);
		if (buffer == NULL) {
			printf("error: DSP buffer new failed.\n");
			return -1;
		}

		dmm_buffer_use(buffer, vbufs[i].mem, vbufs[i].size);
		ret = dmm_buffer_map(buffer);
		printf("DSP buffer %u mapped: %d\n", i, ret);
		dsp->buffers[i] = buffer;
	}

	return 0;
}

static int omap3dsp_start(struct omap3_dsp_device *dsp)
{
	int ret;

	ret = dsp_node_run(dsp->handle, dsp->node);
	if (!ret) {
		printf("error: failed to start DSP node\n");
		return -1;
	}

	printf("DSP node running\n");
	return 0;
}

static int omap3dsp_stop(struct omap3_dsp_device *dsp)
{
	unsigned long status;
	int ret;

	ret = dsp_node_terminate(dsp->handle, dsp->node, &status);
	if (!ret) {
		printf("error: failed to stop DSP node\n");
		return -1;
	}

	printf("DSP node stopped\n");
	return 0;
}

static void omap3dsp_cleanup(struct omap3_dsp_device *dsp)
{
	unsigned int i;

	for (i = 0; i < BUFFERS_COUNT; ++i) {
		if (dsp->buffers[i]) {
			dmm_buffer_unmap(dsp->buffers[i]);
			dmm_buffer_free(dsp->buffers[i]);
		}
	}

	if (dsp->node)
		dsp_node_free(dsp->handle, dsp->node);
	if (dsp->proc)
		dsp_detach(dsp->handle, dsp->proc);
	if (dsp->handle > 0)
		dsp_close(dsp->handle);
}

/* -----------------------------------------------------------------------------
 * Video acquisition
 */

#define MEDIA_DEVICE		"/dev/media0"
#define ENTITY_CCDC		"OMAP3 ISP CCDC"
#define ENTITY_SENSOR		"mt9v032 2-005c"

struct omap3_isp_device {
	struct media_device *mdev;

	struct media_entity *ccdc;
	struct media_entity *sensor;
	struct media_entity *video;

	struct v4l2_mbus_framefmt format;
	struct v4l2_device *vdev;

	fd_set fds;
};

static int omap3isp_pipeline_setup(struct omap3_isp_device *isp)
{
	struct v4l2_mbus_framefmt format;
	struct media_entity *entity;
	unsigned int i;
	int ret;

	/* Reset all links to make sure we're in a consistent, known state. */
	ret = media_reset_links(isp->mdev);
	if (ret < 0) {
		printf("error: unable to reset links.\n");
		return ret;
	}

	/* Setup a Sensor -> CCDC -> memory pipeline.
	 *
	 * Start by locating the three entities. The output video node is
	 * located by looking for a devnode connected to the CCDC.
	 */
	isp->ccdc = media_get_entity_by_name(isp->mdev, ENTITY_CCDC,
					     strlen(ENTITY_CCDC));
	if (isp->ccdc == NULL) {
		printf("error: unable to locate CCDC.\n");
		return -ENOENT;
	}

	isp->sensor = media_get_entity_by_name(isp->mdev, ENTITY_SENSOR,
					       strlen(ENTITY_CCDC));
	if (isp->sensor == NULL) {
		printf("error: unable to locate sensor.\n");
		return -ENOENT;
	}

	for (i = 0; i < isp->ccdc->info.links; ++i) {
		entity = isp->ccdc->links[i].sink->entity;
		if (media_entity_type(entity) == MEDIA_ENT_T_DEVNODE)
			break;
	}

	if (i == isp->ccdc->info.links) {
		printf("error: unable to locate CCDC output video node.\n");
		return -ENOENT;
	}

	isp->video = entity;

	/* Enable the Sensor -> CCDC and CCDC -> memory links. */
	ret = media_setup_link(isp->mdev, &isp->sensor->pads[0],
			       &isp->ccdc->pads[0], MEDIA_LNK_FL_ENABLED);
	if (ret < 0) {
		printf("error: unable to setup sensor -> CCDC link.\n");
		return ret;
	}

	ret = media_setup_link(isp->mdev, &isp->ccdc->pads[1],
			       &isp->video->pads[0], MEDIA_LNK_FL_ENABLED);
	if (ret < 0) {
		printf("error: unable to setup CCDC -> devnode link.\n");
		return ret;
	}

	/* Configure formats. Retrieve the default format at the sensor output
	 * and propagate it through the pipeline. As the CCDC will not perform
	 * any cropping we can just apply the same format on all pads.
	 */
	ret = v4l2_subdev_get_format(isp->sensor, &format, 0,
				     V4L2_SUBDEV_FORMAT_TRY);
	if (ret < 0) {
		printf("error: get format on sensor output failed.\n");
		return ret;
	}

	ret = v4l2_subdev_set_format(isp->sensor, &format, 0,
				     V4L2_SUBDEV_FORMAT_ACTIVE);
	if (ret < 0) {
		printf("error: set format failed on %s:%u.\n",
			isp->sensor->info.name, 0);
		return ret;
	}

	ret = v4l2_subdev_set_format(isp->ccdc, &format, 0,
				     V4L2_SUBDEV_FORMAT_ACTIVE);
	if (ret < 0) {
		printf("error: set format failed on %s:%u.\n",
			isp->ccdc->info.name, 1);
		return ret;
	}

	ret = v4l2_subdev_set_format(isp->ccdc, &format, 1,
				     V4L2_SUBDEV_FORMAT_ACTIVE);
	if (ret < 0) {
		printf("error: set format failed on %s:%u.\n",
			isp->ccdc->info.name, 1);
		return ret;
	}

	isp->format = format;

	return 0;
}

static int omap3isp_video_setup(struct omap3_isp_device *isp)
{
	struct v4l2_pix_format format;
	int ret;

	/* Set the capture format on the CCDC output video node. */
	memset(&format, 0, sizeof format);
	format.pixelformat = V4L2_PIX_FMT_SGRBG10;
	format.width = isp->format.width;
	format.height = isp->format.height;

	ret = v4l2_set_format(isp->vdev, &format);
	if (ret < 0)
		return ret;

	/* Allocate video buffers. */
	ret = v4l2_alloc_buffers(isp->vdev, V4L2_MEMORY_MMAP, BUFFERS_COUNT);
	if (ret < 0)
		return ret;

	/* Store the CCDC output device node file handle in the file descriptors
	 * set, to be used by select() in the main loop.
	 */
	FD_ZERO(&isp->fds);
	FD_SET(isp->vdev->fd, &isp->fds);

	return 0;
}

static int omap3isp_video_start(struct omap3_isp_device *isp)
{
	struct v4l2_video_buffer buffer;
	unsigned int i;
	int ret;

	/* Queue all buffers for video capture. */
	for (i = 0; i < isp->vdev->nbufs; ++i) {
		buffer.index = i;

		ret = v4l2_queue_buffer(isp->vdev, &buffer);
		if (ret < 0) {
			printf("error: unable to queue buffer %u\n", i);
			return -errno;
		}	
	}

	/* Start video streaming. */
	ret = v4l2_stream_on(isp->vdev);
	if (ret < 0) {
		printf("error: failed to start video stream: %s (%d)\n",
			strerror(-ret), ret);
		return ret;
	}

	return 0;
}

static int omap3isp_video_stop(struct omap3_isp_device *isp)
{
	int ret;

	/* Stop video streaming. */
	ret = v4l2_stream_off(isp->vdev);
	if (ret < 0) {
		printf("error: failed to stop video stream: %s (%d)\n",
			strerror(-ret), ret);
		return ret;
	}

	return 0;
}

static void omap3isp_video_cleanup(struct omap3_isp_device *isp)
{
	if (isp->vdev) {
		v4l2_free_buffers(isp->vdev);
		v4l2_close(isp->vdev);
	}
}

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

#define SELECT_TIMEOUT		2000		/* in milliseconds */

static bool done = false;

static void sigint_handler(int signal __attribute__((__unused__)))
{
	/* Set the done flag to true when the user presses CTRL-C to interrupt
	 * the main loop.
	 */
	done = true;
}

static int process_image(struct omap3_isp_device *isp,
			 struct omap3_dsp_device *dsp)
{
	struct v4l2_video_buffer buffer;
	dmm_buffer_t *dsp_buffer;
	struct dsp_msg msg;
	int ret;

	/* Dequeue the buffer */
	ret = v4l2_dequeue_buffer(isp->vdev, &buffer);
	if (ret < 0) {
		printf("error: unable to dequeue buffer: %s (%d)\n",
			strerror(-ret), ret);
		return ret;
	}

	if (buffer.error) {
		printf("warning: error in dequeued buffer, skipping\n");
		return 0;
	}

	/* Send the data to the DSP and wait for the answer.
	 *
	 * Cache is cleaned first with dmm_buffer_begin() before passing it to
	 * the DSP, to make sure that all data has hit memory. Similarly
	 * dmm_buffer_end() makes sure that the CPU won't read stale data back
	 * from the cache. As the data isn't touched by the CPU between captured
	 * by the ISP and processing by the DSP this could be optimized.
	 */
	dsp_buffer = dsp->buffers[buffer.index];
	dmm_buffer_begin(dsp_buffer, buffer.bytesused);

	msg.cmd = 0;
	msg.arg_1 = (uint32_t)dsp_buffer->map;
	msg.arg_2 = buffer.bytesused;
	dsp_node_put_message(dsp->handle, dsp->node, &msg, -1);
	dsp_node_get_message(dsp->handle, dsp->node, &msg, -1);

	dmm_buffer_end(dsp_buffer, buffer.bytesused);

	/* Requeue the buffer */
	ret = v4l2_queue_buffer(isp->vdev, &buffer);
	if (ret < 0) {
		printf("error: unable to requeue buffer: %s (%d)\n",
			strerror(-ret), ret);
		return ret;
	}

	return 0;
}

int main(int argc __attribute__((__unused__)), char *argv[] __attribute__((__unused__)))
{
	struct omap3_dsp_device dsp;
	struct omap3_isp_device isp;
	struct timespec start, end;
	unsigned int count = 0;
	int exit_code = EXIT_FAILURE;
	float fps;
	int 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);

	memset(&dsp, 0, sizeof dsp);
	memset(&isp, 0, sizeof isp);

	/* Open the media device and setup the capture pipeline. The pipeline
	 * topology is hardcoded to sensor -> CCDC -> CCDC output.
	 */
	isp.mdev = media_open(MEDIA_DEVICE, 0);
	if (isp.mdev == NULL) {
		printf("error: unable to open media device %s\n", MEDIA_DEVICE);
		goto cleanup;
	}

	ret = omap3isp_pipeline_setup(&isp);
	if (ret < 0) {
		printf("error: unable to setup pipeline\n");
		goto cleanup;
	}

	/* Open the video capture device, setup the format and allocate the
	 * buffers.
	 */
	isp.vdev = v4l2_open(isp.video->devname);
	if (isp.vdev == NULL) {
		printf("error: unable to open video capture device %s\n",
			isp.video->devname);
		goto cleanup;
	}

	ret = omap3isp_video_setup(&isp);
	if (ret < 0) {
		printf("error: unable to setup video capture\n");
		goto cleanup;
	}

	/* Initialize the DSP. */
	ret = omap3dsp_setup(&dsp, isp.vdev->buffers);
	if (ret < 0) {
		printf("error: unable to setup DSP\n");
		goto cleanup;
	}

	/* Start the DSP and ISP. */
	ret = omap3dsp_start(&dsp);
	if (ret < 0)
		goto cleanup;

	ret = omap3isp_video_start(&isp);
	if (ret < 0)
		goto cleanup;

	clock_gettime(CLOCK_MONOTONIC, &start);

	/* Main capture loop. Wait for a video buffer using select() and process
	 * it.
	 */
	while (!done) {
		struct timeval timeout;
		fd_set rfds;

		timeout.tv_sec = SELECT_TIMEOUT / 1000;
		timeout.tv_usec = (SELECT_TIMEOUT % 1000) * 1000;
		rfds = isp.fds;

		ret = select(isp.vdev->fd + 1, &rfds, NULL, NULL, &timeout);
		if (ret < 0) {
			/* EINTR means that a signal has been received, continue
			 * to the next iteration in that case.
			 */
			if (errno == EINTR)
				continue;

			printf("error: select failed with %d\n", errno);
			goto cleanup;
		}
		if (ret == 0) {
			/* select() should never time out as the ISP is supposed
			 * to capture images continuously. A timeout is thus
			 * considered as a fatal error.
			 */
			printf("error: select timeout\n");
			goto cleanup;
		}

		process_image(&isp, &dsp);
		count++;
	}

	clock_gettime(CLOCK_MONOTONIC, &end);

	/* Stop the DSP and ISP. */
	omap3isp_video_stop(&isp);
	omap3dsp_stop(&dsp);

	/* 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 = count / (end.tv_sec + end.tv_nsec / 1000000000.0);

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

	exit_code = EXIT_SUCCESS;

cleanup:
	/* Cleanup the DSP and ISP resources. */
	omap3dsp_cleanup(&dsp);
	omap3isp_video_cleanup(&isp);
	if (isp.mdev)
		media_close(isp.mdev);

	return exit_code;
}