dc1394_vloopback.c

This library provides functionality to control any camera that conforms to the 1394-Based Digital C

/**************************************************************************
**       Title: Turn a Digital Camera into a V4L device using vloopback
**    $RCSfile: dc1394_vloopback.c,v $
**   $Revision: 1.11.2.29 $$Name:  $
**       $Date: 2006/07/25 01:00:50 $
**   Copyright: LGPL $Author: ddouxchamps $
** Description:
**
**    Sends format0 640x480 RGB to the vloopback input device so that it
**    can be consumed by V4L applications on the vloopback output device.
**    Get vloopback 0.90 from http://motion.technolust.cx/vloopback/
**    Apply the patch from effectv http://effectv.sf.net/
**    It has been tested with EffecTV (exciting!), GnomeMeeting, ffmpeg,
**    camsource, Kino, and veejay.
**
** TODO:
**    - Support video controls
**    - do cropping on v4l window offset
**    - supply audio from sound card?
**
**-------------------------------------------------------------------------
**
**************************************************************************/

#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <fcntl.h>
#include <sys/time.h>
#include <sys/mman.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/poll.h>
#include <unistd.h>
#include <signal.h>
#define _LINUX_TIME_H
#define _DEVICE_H_
#include <linux/videodev.h>
#include <getopt.h>

// #include <libraw1394/raw1394.h>
#include <libdc1394/dc1394_control.h>
#include "affine.h"

#define CLAMP(x, low, high)  (((x) > (high)) ? (high) : (((x) < (low)) ? (low) : (x)))

/* uncomment the following to drop frames to prevent delays */
#define DROP_FRAMES DC1394_RING_BUFFER_LAST
#define MAX_PORTS 4
#define DC1394_BUFFERS 8
#define DC1394_WIDTH 640
#define DC1394_HEIGHT 480
#define MAX_WIDTH 1024
#define MAX_HEIGHT 768
#define MIN_WIDTH 160
#define MIN_HEIGHT 120
#define MAX_BPP 3
#define V4L_BUFFERS 2
#define MAX_RESETS 5

/* declarations for libdc1394 */
nodeid_t *camera_nodes;
dc1394featureset_t features;
dc1394camera_t *camera;

/* Video4Linux globals */
int v4l_dev = -1;
unsigned char *out_pipe = NULL, *out_mmap = NULL;
#define MAXIOCTL 1024
char ioctlbuf[MAXIOCTL];

/* cmdline options */
int g_daemon = 0;
enum v4l_modes {
	V4L_MODE_NONE,
	V4L_MODE_PIPE,
	V4L_MODE_MMAP
};
enum v4l_modes g_v4l_mode = V4L_MODE_MMAP;
char *dc_dev_name = NULL;
char *v4l_dev_name = NULL;
int g_v4l_fmt = VIDEO_PALETTE_YUV422;
int g_width = DC1394_WIDTH;
int g_height = DC1394_HEIGHT;
u_int64_t g_guid = 0;

static struct option long_options[]={
	{"daemon",0,NULL,0},
	{"pipe",0,NULL,0},
	{"guid",1,NULL,0},
	{"video1394",1,NULL,0},
	{"vloopback",1,NULL,0},
	{"palette",1,NULL,0},
	{"width",1,NULL,0},
	{"height",1,NULL,0},
	{"help",0,NULL,0},
	{NULL,0,0,0}
	};

void get_options(int argc,char *argv[])
{
	int option_index=0;
	
	while(getopt_long(argc,argv,"",long_options,&option_index)>=0){
			switch(option_index){ 
				/* case values must match long_options */
				case 0:
					g_daemon = 1;
					break;
				case 1:
					g_v4l_mode = V4L_MODE_PIPE;
					break;
				case 2:
					sscanf(optarg, "%llx", &g_guid);
					break;
				case 3:
					dc_dev_name=strdup(optarg);
					break;
				case 4:
					v4l_dev_name=strdup(optarg);
					break;
				case 5:
					if (strcasecmp(optarg, "rgb24") == 0)
						g_v4l_fmt = VIDEO_PALETTE_RGB24;
					else if (strcasecmp(optarg, "yuv422p") == 0)
						g_v4l_fmt = VIDEO_PALETTE_YUV422P;
					else if (strcasecmp(optarg, "yuv420p") == 0)
						g_v4l_fmt = VIDEO_PALETTE_YUV420P;
					break;
				case 6:
					g_width = atoi(optarg);
					break;
				case 7:
					g_height = atoi(optarg);
					break;
				default:
					printf( "\n"
						"%s - send video from dc1394 through vloopback\n\n"
						"Usage:\n"
						"    %s [--daemon] [--pipe] [--guid=camera-euid]\n"
						"       [--video1394=/dev/video1394/x] [--vloopback=/dev/video0]\n"
						"       [--palette=format] [--width=n] [--height=n]\n\n"
						"    --daemon    - run as a daemon, detached from console (optional)\n"
						"    --pipe      - write images to vloopback device instead of using\n"
						"                  zero-copy mmap mode (optional)\n"
						"    --guid      - select camera to use (optional)\n"
						"                  default is first camera on any port\n"
						"    --video1394 - specifies video1394 device to use (optional)\n"
						"                  default = /dev/video1394/<port#>\n"
						"    --vloopback - specifies video4linux device to use (optional)\n"
						"                  by default, this is automatically determined!\n"
						"    --palette   - specified the video palette to use (optional)\n"
						"                  yuv422 (default), yuv422p, yuv420p, or rgb24\n"
						"    --width     - set the initial width (default=640)\n"
						"    --height    - set the initial height (default=480)\n"
						"    --help      - prints this message\n\n"
						,argv[0],argv[0]);
					exit(0);
			}
	}
	
}

/***** IMAGE PROCESSING *******************************************************/

typedef void (*affine_transform_cb)(const unsigned char *src, unsigned char *dest);

void transform_yuv422(const unsigned char *src, unsigned char *dest)
{
	swab(src, dest, 4);
}

void transform_rgb24(const unsigned char *src, unsigned char *dest)
{
	dest[2] = src[0]; //b
	dest[1] = src[1]; //g
	dest[0] = src[2]; //r
}

void affine_scale( const unsigned char *src, int src_width, int src_height, unsigned char *dest, int dest_width, int dest_height,
                   int bpp, affine_transform_cb transform )
{	
	affine_transform_t affine;
	double scale_x = (double) dest_width / (double) src_width;
	double scale_y = (double) dest_height / (double) src_height;
	register unsigned char *d = dest;
    register const unsigned char  *s = src;
	register int i, j, x, y;

	/* optimization for no scaling required */
	if (scale_x == 1.0 && scale_y == 1.0) {
		for (i = 0; i < src_width*src_height; i++) {
			transform(s, d);
			s += bpp;
			d += bpp;
		}
		return;
	}
	
	affine_transform_init( &affine );
	
	if ( scale_x <= 1.0 && scale_y <= 1.0 )
	{
		affine_transform_scale( &affine, scale_x, scale_y );

		for( j = 0; j < src_height; j++ )
			for( i = 0; i < src_width; i++ )
			{
				x = (int) ( affine_transform_mapx( &affine, i - src_width/2, j - src_height/2 ) );
				y = (int) ( affine_transform_mapy( &affine, i - src_width/2, j - src_height/2 ) );
				x += dest_width/2;
				x = CLAMP( x, 0, dest_width);
				y += dest_height/2;
				y = CLAMP( y, 0, dest_height);
				s = src + (j*src_width*bpp) + i*bpp;
				d = dest + y*dest_width*bpp + x*bpp;
				transform(s, d);
				d += bpp;
				s += bpp;
			}
	}
	else if ( scale_x >= 1.0 && scale_y >= 1.0 )
	{
		affine_transform_scale( &affine, 1.0/scale_x, 1.0/scale_y );
	
		for( y = 0; y < dest_height; y++ )
			for( x = 0; x < dest_width; x++ )
			{
				i = (int) ( affine_transform_mapx( &affine, x - dest_width/2, y - dest_height/2 ) );
				j = (int) ( affine_transform_mapy( &affine, x - dest_width/2, y - dest_height/2 ) );
				i += src_width/2;
				i = CLAMP( i, 0, dest_width);
				j += src_height/2;
				j = CLAMP( j, 0, dest_height);
				s = src + (j*src_width*bpp) + i*bpp;
				d = dest + y*dest_width*bpp + x*bpp;
				transform(s, d);
				d += bpp;
				s += bpp;
			}
	}
}

inline void
yuy2_to_yv16( const unsigned char *src, unsigned char *dest, int width, int height)
{
	register int total = (width * height) >> 1;
	register int i;
	register unsigned char *img_y = dest;
	register unsigned char *img_cb = img_y + (width * height);
	register unsigned char *img_cr = img_cb + total;

	for (i = 0; i < total; i++) {
		*img_y++  = *src++;
		*img_cb++ = *src++;
		*img_y++  = *src++;
		*img_cr++ = *src++;
	}
}

inline void
yuy2_to_yv12( const unsigned char *src, unsigned char *dest, int width, int height)
{
	register int stride = width >> 1;
	register int i, j;
	register unsigned char *img_y = dest;
	register unsigned char *img_cb = img_y + (width * height);
	register unsigned char *img_cr = img_cb + stride * (height >> 1);

	for (i = 0; i < height; i++) {
		for (j = 0; j < stride; j++) {
			if (i%2) {
				*img_y++  = *src++;
				src++;
				*img_y++  = *src++;
				src++;
			} else {
				*img_y++  = *src++;
				*img_cb++ = (src[width << 1] + src[0]) >> 1;
				src++;
				*img_y++  = *src++;
				*img_cr++ = (src[width << 1] + src[0]) >> 1;
				src++;
			}
		}
	}
}

/***** IMAGE CAPTURE **********************************************************/

int capture_pipe(int dev, const unsigned char *image_in)
{
	int size = g_width * g_height;
	int bpp = 0;
	int ppp = 1; /* pixels per pixel! */
	affine_transform_cb transform = NULL;

	switch (g_v4l_fmt) {
		case VIDEO_PALETTE_RGB24:
			bpp = 3;
			ppp = 1;
			transform = transform_rgb24;
			break;

		case VIDEO_PALETTE_YUV422:
		case VIDEO_PALETTE_YUV422P:
		case VIDEO_PALETTE_YUV420P:
			bpp = 2;
			ppp = 2;
			transform = transform_yuv422;
			break;

		default:
			return 0;
	}
	
	affine_scale( image_in, DC1394_WIDTH/ppp, DC1394_HEIGHT,
		out_pipe, g_width/ppp, g_height,
		ppp * bpp, transform);
		
	if (g_v4l_fmt == VIDEO_PALETTE_YUV422P && out_pipe != NULL) {
		size_t memsize = (g_width * g_height) << 1;
		unsigned char *buffer = malloc(memsize);
		if (buffer) {
			memcpy( buffer, out_pipe, memsize);
			yuy2_to_yv16( buffer, out_pipe, g_width, g_height);
			free(buffer);
		}
	}
	else if (g_v4l_fmt == VIDEO_PALETTE_YUV420P && out_pipe != NULL) {
		size_t memsize = (g_width * g_height) << 1;
		unsigned char *buffer = malloc(memsize);
		if (buffer) {
			memcpy( buffer, out_pipe, memsize);
			yuy2_to_yv12( buffer, out_pipe, g_width, g_height);
			free(buffer);
		}
		size = g_width * g_height * 3 / 2;
		bpp = 1;
	}
	
	if (write(dev, out_pipe, size*bpp) != (size*bpp)) {
		perror("Error writing image to pipe");
		return 0;
	}
	return 1;
}

int capture_mmap(int frame)
{
	int bpp = 0;
	int ppp = 1; /* pixels per pixel! */
	affine_transform_cb transform = NULL;
	
	switch (g_v4l_fmt) {
		case VIDEO_PALETTE_RGB24:
			bpp = 3;
			ppp = 1;
			transform = transform_rgb24;
			break;

		case VIDEO_PALETTE_YUV422:
		case VIDEO_PALETTE_YUV422P:
		case VIDEO_PALETTE_YUV420P:
			bpp = 2;
			ppp = 2;
			transform = transform_yuv422;
			break;

		default:
			return 0;
	}
	
	if (g_v4l_fmt == VIDEO_PALETTE_YUV422P && out_pipe != NULL) {
		if (dc1394_capture_dma(&camera,1,DC1394_VIDEO1394_WAIT) == DC1394_SUCCESS) {
			affine_scale( (unsigned char *) dc1394_capture_get_dma_buffer (camera), DC1394_WIDTH/ppp, DC1394_HEIGHT,
				out_pipe, g_width/ppp, g_height,
				ppp * bpp, transform);
			dc1394_capture_dma_done_with_buffer(camera);
		}
		yuy2_to_yv16( out_pipe, out_mmap + (MAX_WIDTH * MAX_HEIGHT * 3 * frame), g_width, g_height);
	}
	else if (g_v4l_fmt == VIDEO_PALETTE_YUV420P && out_pipe != NULL) {
		if (dc1394_capture_dma(&camera,1,DC1394_VIDEO1394_WAIT) == DC1394_SUCCESS) {
			affine_scale( (unsigned char *) dc1394_capture_get_dma_buffer (camera), DC1394_WIDTH/ppp, DC1394_HEIGHT,
				out_pipe, g_width/ppp, g_height,
				ppp * bpp, transform);
			dc1394_capture_dma_done_with_buffer(camera);
		}
		yuy2_to_yv12( out_pipe, out_mmap + (MAX_WIDTH * MAX_HEIGHT * 3 * frame), g_width, g_height);
	}
	else if (dc1394_capture_dma(&camera,1,DC1394_VIDEO1394_WAIT) == DC1394_SUCCESS) {
		affine_scale( (unsigned char *) dc1394_capture_get_dma_buffer, DC1394_WIDTH/ppp, DC1394_HEIGHT,
			out_mmap + (MAX_WIDTH * MAX_HEIGHT * 3 * frame), g_width/ppp, g_height,
			ppp * bpp, transform);
		dc1394_capture_dma_done_with_buffer(camera);
	}

	
	return 1;
}

/***** DIGITAL CAMERA *********************************************************/

int dc_init()
{
  int reset;
  uint_t camCount = 0;
  int found = 0;
  dc1394camera_t **cameras=NULL;
  int err, i;
  int cam=-1;
  
  for (reset=0;reset<MAX_RESETS;reset++) {
    err=dc1394_find_cameras(&cameras, &camCount);
    DC1394_ERR_RTN(err,"Could not find cameras");
    if (camCount > 0) {
      if (g_guid == 0) {
	/* use the first camera found */
	cam=0;
	found = 1;
      }
      else {
	/* attempt to locate camera by guid */
	for (i = 0; i< camCount && found == 0; i++) {
	  if (cameras[i]->euid_64 == g_guid) {
	    cam=i;
	    found = 1;
	  }
	}
      }
    }
    if (found == 1) {
      camera=cameras[cam];
      for (i=0;i<camCount;i++) {
	if (i!=cam)
	  dc1394_free_camera(cameras[i]);
      }
      free(cameras);
      cameras=NULL;

      dc1394_print_camera_info(camera);
    
#if 0
      /* camera can not be root--highest order node */
      if (camera->node == raw1394_get_nodecount(camera->handle)-1) {
	/* reset and retry if root */
	//fprintf(stderr,"reset\n");
	raw1394_reset_bus(camera->handle);
	sleep(2);
	found = 0;
	dc1394_free_camera(camera);
      }
      else
#endif
	break;
    }
  } /* next reset retry */

  if (found) {
    /*have the camera start sending us data*/
    if (dc1394_video_set_transmission(camera, DC1394_ON) !=DC1394_SUCCESS) {
      perror("unable to start camera iso transmission\n");
      exit(-1);
    }
  }
  if (found == 0 && g_guid != 0) {
    fprintf( stderr, "Unable to locate camera node by guid\n");
    exit(-1);
  }
  else if (camCount == 0) {
    fprintf( stderr, "no cameras found :(\n");
    exit(-1);
  }
  if (reset == MAX_RESETS) {
    fprintf( stderr, "failed to not make camera root node :(\n");
    exit(-1);
  }

  //fprintf(stderr,"found\n");
  return found;
}

int dc_start(int palette)
{
	dc1394speed_t speed;
	dc1394video_mode_t mode;