cam2fb.c

三星s3c2440 ARM9芯片在linux下的摄像头驱动

/*
 * CAMIF test
 *
 * [e-mail]
 *
 * $Id: cam2fb.c,v 1.1.1.1 2004/01/20 10:29:10 laputa Exp $
 *
 */
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <ctype.h>
#include <errno.h>
#include <sys/mman.h>
#include <sys/time.h>
#include <sys/ioctl.h>


/*
 * must be 
 *   CAM_WIDTH  == FB_WIDTH
 *   CAM_HEIGHT <= FB_HEIGHT
 */
#define CAM_WIDTH 240
#define CAM_HEIGHT 180
#define FB_WIDTH 240
#define FB_HEIGHT 320

/* kernel stuff */
#define CAMIF_TYPE_RGB16 0
#define CAMIF_TYPE_RGB32 1
#define CAMIF_TYPE_YUV420 2
static struct s3c2440_camif_cfg_t {
	int src_x;
	int src_y;
	int dst_x;
	int dst_y;
	int use_vpost;
	int dst_type;
	void (*camif_frame_handler)(int frame, struct s3c2440_camif_cfg_t *cfg);
} camif_cfg;
/****************/

static int cam_fp = -1;
static int fb_fp = -1;
static char *fb = NULL;

static inline int cam_init(void)
{
	int dev_fp = -1;

	dev_fp = open("/dev/misc/cam", O_RDWR);
	if (dev_fp < 0) {
		perror("/dev/misc/cam");
		return -1;
	}

	return dev_fp;
}

static inline int fb_init(void)
{
	int dev_fp = -1;

	dev_fp = open("/dev/fb/0", O_RDWR);
	if (dev_fp < 0) {
		perror("/dev/fb/0");
		return -1;
	}

	if ((fb = (char*)mmap(0, FB_WIDTH*FB_HEIGHT*2, 
			PROT_READ | PROT_WRITE, MAP_SHARED, dev_fp, 0)) < 0) {
		perror("mmap()");
		return -1;
	}

	return dev_fp;
}

static inline void start_capture(int fp)
{
	char c;

	write(fp, &c, 1);
}

static inline int read_data(int fp, char *buf, int width, int height, int bpp)
{
	int ret;

	if ((ret = read(fp, buf, width*height*bpp/8)) != width*height*bpp/8)
		return 0;

	return ret;
}

static inline void clear(char *dest)
{
#ifdef FB_IMG_BPP
	memcpy(dest, &img_data[0], FB_WIDTH*FB_HEIGHT*2);
#else
	memset(dest, 0, FB_WIDTH*FB_HEIGHT*2);
#endif
}

#define MAX(x,y) ((x)>(y)?(x):(y))
#define MIN(x,y) ((x)>(y)?(y):(x))
static inline void draw(char *dest, char *src, int width, int height, int bpp
		,int src_offset_x, int src_offset_y)
{
	int x,y;
	unsigned short *rgb16; 
	unsigned long *rgb32;
	unsigned long offset_x = 0;
	unsigned long offset_y = 0;
	int end_y = MIN(FB_HEIGHT, height - src_offset_y);
	int end_x = MIN(FB_WIDTH, width - src_offset_x);

	offset_x = src_offset_x * bpp/8;
	offset_y = src_offset_y * width * bpp/8;
	src += offset_y;

	if (bpp==16) {
		for (y=0;y<end_y;y++)
			memcpy(dest+y*FB_WIDTH*2, src+y*width*2+offset_x, end_x*2);
	} else {
		for (y=0; y<end_y;y++) {
			rgb32 = (unsigned long*)(src + y * width * 4 + offset_x);
			rgb16 = (unsigned short*)(dest + y * FB_WIDTH * 2);
			for (x=0;x<end_x;x++) {
				*rgb16 = ((((*(rgb32)>>16)&0xff) >> 3) << 11) // red
						| ((((*(rgb32)>>8)&0xff) >> 2) << 5)  // green
						| ((((*(rgb32)>>0)&0xff) >> 3) << 0) ; // blue
				rgb32 ++;
				rgb16 ++;
			}
		}
	}
}

static inline void print_fps(struct timeval *s, struct timeval *e)
{
	unsigned long time;
	unsigned long sec;
	unsigned long usec;
	int fps = 0;

	sec = e->tv_sec - s->tv_sec;
	if (e->tv_usec > s->tv_usec)
		usec = e->tv_usec - s->tv_usec;
	else {
		usec = e->tv_usec + 1000000 - s->tv_usec;	
		sec--;
	}
	time = sec * 1000 + (usec+1) / 1000;
	fps = 1000 / (time / 30);
	printf("%d fps\n", fps);
}

int main(int argc, char *argv[])
{
	char rgb[640*480*4]; // MAX
	unsigned int frames = 0;
	struct timeval start_tv, end_tv;
	struct timezone tz;
	int width, height, bpp;
	int y_offset, x_offset;

	if (argc != 6) {
		printf("%s <width> <height> <bpp> <x_offset> <y_offset> \n", argv[0]);
		return 0;
	}
	camif_cfg.dst_x = width = atoi(argv[1]);
	camif_cfg.dst_y = height = atoi(argv[2]);
	bpp = atoi(argv[3]);
	if ((bpp!=16) && (bpp!=32)) {
		printf("BPP must be 16 or 32\n");
		return 0;
	}
	if (bpp==16)
		camif_cfg.dst_type = CAMIF_TYPE_RGB16;
	else
		camif_cfg.dst_type = CAMIF_TYPE_RGB32;
	x_offset = atoi(argv[4]);
	y_offset = atoi(argv[5]);

	if ((cam_fp = cam_init()) < 0)
		goto err;

	if ((fb_fp = fb_init()) < 0)
		goto err;

	printf("cam2fb: %dx%d %dbpp, use %s\n", 
			width, height, bpp);
	printf("displayed window: left-top (%d,%d), width(%d), height(%d)\n", 
			x_offset, y_offset, 
			MIN(FB_WIDTH, width-x_offset),
			MIN(FB_HEIGHT, height-y_offset));
	fflush(stdout);
	fflush(stdout);
	sleep(1);

	if (ioctl(cam_fp, 0x10, &camif_cfg)) {
		perror("ioctl");
		goto err;
	}

	clear(fb);

	gettimeofday(&start_tv, &tz);

	start_capture(cam_fp);

	while (1) {
		if (!read_data(cam_fp, &rgb[0], width, height, bpp))
			break;
		start_capture(cam_fp);

		draw(fb, &rgb[0], width, height, bpp, x_offset, y_offset);

		frames ++;
		
		if ((frames % 30) == 0) {
			gettimeofday(&end_tv, &tz);
			print_fps(&start_tv, &end_tv);
			gettimeofday(&start_tv, &tz);
		}
	}


err:
	if (cam_fp)
		close(cam_fp);

	if (fb)
		munmap(fb, FB_WIDTH*FB_HEIGHT*2);

	if (fb_fp)
		close(fb_fp);

	return 0;
}