docapture.c

基于ARM9TDMI的CMOS摄像头驱动源程序

/*
 * 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., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 * Copyright (C) 2002, 2003 Motorola Semiconductors HK Ltd
 *
 */


#include <stdio.h>
#include <fcntl.h>
#include <linux/fb.h>
#include <asm/mman.h>

#include "type.h"

//#define CONSOLE_MSG
#ifdef CONSOLE_MSG
#define printcmsg(str...) printf("<"__FUNCTION__"> "str)
#else
#define printcmsg(str...)	// nothing
#endif

#define BUS_CLOCK			48
#define SENSOR_CLOCK		24
//#define SENSOR_CLOCK		8
//#define SENSOR_CLOCK		12

#define LCD_SATURATION			1.3	// value used for color correction for built-in LCD
#define BMP_SATURATION			1.0	// value used for color correction for saved bitmap

#define AEC_WITH_GAIN			0
#define AEC_WITH_VF				1

#define IMAGE_SIZE				640*480
#define SUBSAMPLE_DIVIDER		4
#define VF_WIDTH					160		// viewfinder width
#define VF_HEIGHT					120		// viewfinder height
#define VF_IMAGE_SIZE			VF_WIDTH*VF_HEIGHT
#define SCREEN_WIDTH				240
#define SCREEN_HEIGHT			320
#define ORIGIN_X_OFFSET			40			// origin start from column 40
#define ORIGIN_Y_OFFSET			100			// origin start from row 100
#define ORIGIN_OFFSET			(ORIGIN_Y_OFFSET*SCREEN_WIDTH + ORIGIN_X_OFFSET)
#define ROW_OFFSET				(SCREEN_WIDTH-VF_WIDTH)

#define AVG_RED_TARGET	100	// targeted value of average RED pixel intensity for auto exposure control
#define TARGET_MARGIN	4		// margin beyond which action should be taken for auto exposure control

#define IOCTL_CSI_INIT			1
#define IOCTL_I2C_WRITE			2
#define IOCTL_I2C_READ			3
#define IOCTL_SUBSAMPLE			4
#define IOCTL_SET_GAIN			5
#define IOCTL_SET_VF_WIDTH		6
#define IOCTL_SET_INT_TIME		7
#define IOCTL_DMA_CAPTURE		8
#define IOCTL_STOP_CAPTURE		13
#define IOCTL_INC_FRM			20		// increment frame rate meter

//	Bayer color
#define BAYER_G		0
#define BAYER_R		1
#define BAYER_B		2
#define BAYER_g		3

// modes for main loop (used under QT only)
#define MODE_INIT_AEC_GAIN		0
#define MODE_INIT_AEC_VF		1
#define MODE_VIEWFINDER			2
#define MODE_CAPTURE				3
#define MODE_CLEANUP				4

int	fd_csi2c;
static U8 lastGlobalGain;
static U16 lastVFsize;
static U8 gAECmode = AEC_WITH_GAIN;
static U32 frameCount;
static U8 *rawData;
static U8 *redData;
static U8 *greenData;
static U8 *blueData;
static U16 *displayBuffer;
static U32 histogram[4][256];
static U8 average[4];
static int fd_fb;
static U16 *fb;
#ifdef __STANDALONE__
static U8 quit=0;
#endif


void I2C_write(U32 reg, U32 data)
{
	ioctl(fd_csi2c, IOCTL_I2C_WRITE, (reg << 8) | data);
}

void I2C_read(U32 reg, U32 * _data)
{
	*_data = ioctl(fd_csi2c, IOCTL_I2C_READ, reg);
}

void captureInit()
{
	frameCount = 0;
	printf("Set sensor clock to %d\n", ioctl(fd_csi2c, IOCTL_CSI_INIT, (BUS_CLOCK << 8) | SENSOR_CLOCK));	

	lastGlobalGain=0;		// gain value of 0 => global gain of 1.0
	
//	sensorInitVideoMode();
	
	
	ioctl(fd_csi2c, IOCTL_SUBSAMPLE, SUBSAMPLE_DIVIDER);
	ioctl(fd_csi2c, IOCTL_SET_INT_TIME, 0xFFFF);

	if (gAECmode == AEC_WITH_GAIN)
	{	
		#if (SENSOR_CLOCK == 6)
		ioctl(fd_csi2c, IOCTL_SET_VF_WIDTH, 0x02C0);
		#elif (SENSOR_CLOCK == 8)
		ioctl(fd_csi2c, IOCTL_SET_VF_WIDTH, 0x03C0);
		#else	// i.e. SENSOR_CLOCK == 12
		ioctl(fd_csi2c, IOCTL_SET_VF_WIDTH, 0x0580);
		#endif

		lastGlobalGain = 0x10;
	}
	else
	{
		ioctl(fd_csi2c, IOCTL_SET_VF_WIDTH, 0x0340);
		lastGlobalGain = 0;
	}		
	ioctl(fd_csi2c, IOCTL_SET_GAIN, lastGlobalGain);

	printcmsg("Init video mode done !\n");
	
}

//----------------------------------------------------------
//
//	Generate histogram for each component of a Bayer image
//
//----------------------------------------------------------
//static void get_hist(U8 * _data, U32 imgH, U32 imgV, U32 subsample, U32 * _hist[4])
//static void generateHistogram(U8 * _data, U32 imgH, U32 imgV, U32 subsample, U32 * _hist[4])
static void generateHistogram(U8 * _data, U32 imgH, U32 imgV, U32 subsample)
{
	U8 * _end_of_line, * _end_of_frame;
	U32 skip_pixel, skip_line;
	U32 subsample2 = subsample * subsample;
	U32 i;

//init
	_end_of_line  = _data + imgH;			//end of row 0th
	_end_of_frame = _data + imgH * imgV;	//end of image
	skip_line  = (2 * subsample - 1) * imgH;
	skip_pixel = 2 * subsample;

//clear histogram
	for(i = 0; i < 256; i ++)
//		_hist[0][i] = _hist[1][i] = _hist[2][i] = _hist[3][i] = 0;
		histogram[0][i] = histogram[1][i] = histogram[2][i] = histogram[3][i] = 0;
	
		//
		//	Fast access to bayer image
		//
		//	This routine do access to a group of [GRBg]
		//	in each loop.  This can help to reduce the
		//	"for" loop overhead
		//
		//	Here takes GRBg pattern just as an example,
		//	actually it applies for all color patterns
		//       
		//       skip_pixel
		//      |-----|
		//	[GR]GRGRGR[GR]...[GR] <---- end_of_line
		//	[Bg]BgBgBg[Bg]...[Bg] -
		//	...                   |
		//	...                   |
		//	...         skip_line |
		//	...                   |
		//	...                   |
		//	...                   |
		//	[GR]GRGRGR[GR]...[GR] -
		//	[Bg]BgBgBg[Bg]...[Bg]
		//	...
		//	.................[Bg] <---- end_of_frame
		//
	
//take sum for each color
	while(_end_of_line <= _end_of_frame)
	{
		for(; _data < _end_of_line; _data += skip_pixel)
		{
			histogram[0][_data[0]] ++;
			histogram[1][_data[1]] ++;
			histogram[2][_data[imgH]] ++;
			histogram[3][_data[imgH + 1]] ++;
		}
		_data += skip_line;
		_end_of_line = _data + imgH;
	}

//restore the orginal histogram
	if(subsample != 1)
	{
		for(i = 0; i < 256; i ++)
		{
			histogram[0][i] *= subsample2;
			histogram[1][i] *= subsample2;
			histogram[2][i] *= subsample2;
			histogram[3][i] *= subsample2;
		}
	}
}



//-------------------------------------------
//
//	Calculate mean values from histograms
//
//-------------------------------------------
//static void get_avg(U32 * _hist[4], U8 avg[4], U32 imgSize)
//static void takeAverage(U32 * _hist[4], U8 avg[4], U32 imgSize)
static void takeAverage(U32 imgSize)
{
	U32 i;
	U32 sum[4] = {0};

	for(i = 0; i < 256; i ++)
	{
		sum[0] += histogram[0][i] * i;
		sum[1] += histogram[1][i] * i;
		sum[2] += histogram[2][i] * i;
		sum[3] += histogram[3][i] * i;
	}
	average[0] = sum[0] / (imgSize / 4);
	average[1] = sum[1] / (imgSize / 4);
	average[2] = sum[2] / (imgSize / 4);
	average[3] = sum[3] / (imgSize / 4);
}

//-----------------------------------------
//
//	find the index of the max value
//
//-----------------------------------------
static U32 find_max_index(U8 m[4])
{
	U32 max_index;

	max_index = 0;

	if(m[1] > m[max_index])
		max_index = 1;
	if(m[2] > m[max_index])
		max_index = 2;
	if(m[3] > m[max_index])
		max_index = 3;

	return max_index;
}

//---------------------------------------------------------------
//
//	Generate trim table
//
//	This generates a table that covers the range between
//	-511 to +511.  The no. is trimmed to 0 to +255 range
//
//	Note that _tab should point to the position of 0
//
//	Input	: (1) _tab		: table buffer
//	Ouput	: void
//
//---------------------------------------------------------------
static void gen_trim_table(U8 * _tab)
{
	S32 i;

	for(i = -511; i <= 511; i ++)
	{
		if(i > 255)
			_tab[i] = 255;
		else if(i < 0)
			_tab[i] = 0;
		else
			_tab[i] = (U8) i;
	}
	
	return;
}

//----------------------------------------------------------------
//
//	Generate multiplication table
//
//	This can generate a mapping table for fast multiplication.
//	The range is 8-bit (0 - 255) with clipping.
//
//	input	: (1) c		: multiplier
//			  (2) tab	: mapping table
//	output	: void
//
//----------------------------------------------------------------
static void table_gen(float c, U8 tab[256])
{
#define SHIFT 7

	U32 i, temp;
	U32 coef = (U32)(c * (float)(1 << SHIFT));

	for(i = 0; i < 256; i ++)
	{
		temp = (i * coef) >> SHIFT;
		if(temp > 255)
			temp = 255;
		tab[i] = (U8) temp;
	}
	
	return;
}

//----------------------------------------------------------------
//
//	Apply WB coefficients
//
//	Each pixel on the entire image is multiplied by the 
//	corresponding WB coefficient through table lookup.
//
//	input	: (1) _data			: ptr to bayer image
//			  (2) imgH			: image width
//			  (3) imgV			: image height
//			  (4) _tab0			: multiplication table for color 0
//			  (5) _tab1			: multiplication table for color 1
//			  (6) _tab2			: multiplication table for color 2
//			  (7) _tab3			: multiplication table for color 3
//			  (8) sum_max_index	: index of the sum which has max value
//
//	output	: void
//
//----------------------------------------------------------------
static void apply_coef(U8 * _srcData, U8 * _resData, U32 imgH, U32 imgV, U8 tab[4][256], U32 avg_max_index)
{
	U8 * _end_of_line, * _end_of_frame;

//copy 2D pointer into 1D point to speed up access time
	U8 * _tab0 = tab[0];
	U8 * _tab1 = tab[1];
	U8 * _tab2 = tab[2];
	U8 * _tab3 = tab[3];

	_end_of_line  = _srcData + imgH;				//end of 0th row
	_end_of_frame = _srcData + imgH * imgV;	//end of image

//
//	apply white balance coef
//	this part employs the same algorithm as take_sum()
//

//	dispatch which pixel need NOT to be applied white balance coef,
//	since it's coef is already 1.00
	while(_end_of_line <= _end_of_frame)
	{
		for(; _srcData < _end_of_line; _srcData += 2)
		{
			switch(avg_max_index)
			{
				case 0:
					//no need to apply coef on d0
					_resData[1]     	 = _tab1[_srcData[1]];
					_resData[imgH]	    = _tab2[_srcData[imgH]];
					_resData[imgH + 1] = _tab3[_srcData[imgH + 1]];
					break;
				case 1:
					_resData[0]    	= _tab0[_srcData[0]];
					//no need to apply coef on d1
					_resData[imgH]	    = _tab2[_srcData[imgH]];
					_resData[imgH + 1] = _tab3[_srcData[imgH + 1]];
					break;
				case 2:
					_resData[0]    	 = _tab0[_srcData[0]];
					_resData[1]        = _tab1[_srcData[1]];
					//no need to apply coef on d2
					_resData[imgH + 1] = _tab3[_srcData[imgH + 1]];
					break;
				case 3:
					_resData[0]    	 = _tab0[_srcData[0]];
					_resData[1]        = _tab1[_srcData[1]];
					_resData[imgH]	    = _tab2[_srcData[imgH]];
					//no need to apply coef on d3
					break;
			}
		 	_resData += 2;
		}
		_srcData += imgH;
		_resData += imgH;
		_end_of_line = _srcData + imgH;
   }
}

//------------------------------
//
//	White Balance
//	(Grey World Model)
//
//	Top Level function call
//
//------------------------------
void white_balance(U32 imageWidth, U32 imageHeight)
{
	static U8 tab[4][256];
	U32 avg_max_index;
	float coef[4];

	//find max
	avg_max_index = find_max_index(average);

	//find WB coef
	coef[BAYER_G] = (float)average[avg_max_index] / (float)average[BAYER_G];
	coef[BAYER_R] = (float)average[avg_max_index] / (float)average[BAYER_R];
	coef[BAYER_B] = (float)average[avg_max_index] / (float)average[BAYER_B];
	coef[BAYER_g] = (float)average[avg_max_index] / (float)average[BAYER_g];

	// Blue Adjustment
	// REMARK: This is just a very simple adjustment.
	coef[BAYER_B] *= 0.85;
	coef[BAYER_G] *= 0.95;
	coef[BAYER_g] *= 0.95;

	//generate mapping tables for fast multiply
	if(avg_max_index != BAYER_G)	table_gen(coef[BAYER_G], tab[BAYER_G]);