s3c2440_camif.c

2440mmc-and-camera-linux-driver 2440mmc-and-camera-linux-driver

		CLIP(green);
		blue = XlatY[Y] + XlatV_B[V];
		CLIP(blue);
		*(unsigned int*)(rgb) = (red << 16) | (green << 8) | blue;
		rgb += 4;
	}
}


static void cam_to_rgb16_sw(int frame, struct s3c2440_camif_cfg_t *cfg)
{
	struct s3c2440_camif *dev = cfg->dev;
	struct v4l2_pix_format *fmt = &(dev->clientfmt);
	unsigned int i;

#if 0
	unsigned int fact0 = fmt->width * fmt->height;
	unsigned int fact1 = fmt->width / 2;
	unsigned int fact2 = fact0 / 2 * 3;
	unsigned int fact3 = fmt->height * 2;

	DPRINTK("%s()   \n");

	for (i = 0; i < fact3/2; i++)
		yuv_convert_rgb16(yuv_buf[frame].buf + i * (fact1 * 2),
				yuv_buf[frame].buf + fact0 + (i/2) * fact1,
				yuv_buf[frame].buf + fact2 + (i/2) * fact1,
				rgb_buf[frame].buf + i * fact1*4, fact1*2);
#else
	cfg->dst_x = fmt->width;
	cfg->dst_y = fmt->height;

	for (i = 0; i < cfg->dst_y; i++)
		yuv_convert_rgb16(yuv_buf[frame].buf + i * cfg->dst_x,
				yuv_buf[frame].buf + cfg->dst_x*cfg->dst_y
				+ (i/2) * cfg->dst_x / 2,
				yuv_buf[frame].buf + cfg->dst_x*cfg->dst_y/2*3
				+ (i/2) * cfg->dst_x / 2,
				rgb_buf[frame].buf + i * cfg->dst_x * 2,
				cfg->dst_x);

#endif

	dev->captured = 1;
	complete(&dev->capture_done);
}

static void cam_to_rgb32_sw(int frame, struct s3c2440_camif_cfg_t *cfg)
{
	struct s3c2440_camif *dev = cfg->dev;
	struct v4l2_pix_format *fmt = &(dev->clientfmt);

	unsigned int i;

#if 1
	unsigned int fact0 = fmt->width * fmt->height;
	unsigned int fact1 = fmt->width / 2;
	unsigned int fact2 = fact0 / 2 * 3;
	unsigned int fact3 = fmt->height * 2;


	for (i = 0; i < fact3/2 ; i++)
		yuv_convert_rgb32(yuv_buf[frame].buf + i * fact1 * 2,
				yuv_buf[frame].buf + fact0 + (i/2) * fact1,
				yuv_buf[frame].buf + fact2 + (i/2) * fact1,
				rgb_buf[frame].buf + i * fact1*8, fact1*2);
#else
//	cfg->dst_x = fmt->width;
//	cfg->dst_y = fmt->height;

	for (i = 0; i < fmt->height; i++)
		yuv_convert_rgb32(yuv_buf[frame].buf + i * fmt->width,
				yuv_buf[frame].buf + fmt->width*fmt->height
				+ (i/2) * fmt->width / 2,
				yuv_buf[frame].buf + fmt->width*fmt->height/2*3
				+ (i/2) * fmt->width / 2,
				rgb_buf[frame].buf + i * fmt->width * 4,
				fmt->width);
#endif
	dev->captured = 1;
	complete(&dev->capture_done);
}

static void cam_to_yuv420_sw(int frame, struct s3c2440_camif_cfg_t *cfg)
{
	struct s3c2440_camif *dev = cfg->dev;
	struct v4l2_pix_format *fmt = &(dev->clientfmt);

	unsigned int i;

#if 0
	unsigned int fact0 = fmt->width * fmt->height;
	unsigned int fact1 = fmt->width / 2;
	unsigned int fact2 = fact0 / 2 * 3;
	unsigned int fact3 = fmt->height * 2;

	/* Y */
	memcpy(rgb_buf[frame].buf, yuv_buf[frame].buf, fact0);
	/* U */
	for (i = 0; i < fact3; i++)
		memcpy( rgb_buf[frame].buf + fact0 + i * fact1 
				, yuv_buf[frame].buf + fact0 + (i/2) * fact1
				, fact1);
	/* V */
	for (i = 0; i < fact3; i++)
		memcpy( rgb_buf[frame].buf + fact2 + i * fact1
				, yuv_buf[frame].buf + fact2 + (i/2) * fact1
				, fact1);
#else
//	cfg->dst_x = fmt->width;
  //      cfg->dst_y = fmt->height;
        /* Y */
        memcpy(rgb_buf[frame].buf, yuv_buf[frame].buf, fmt->width * fmt->height);
        /* U */
        for (i = 0; i < fmt->height * 2; i++)
                memcpy(
                        rgb_buf[frame].buf + fmt->width * fmt->height + i * fmt->width / 2
                        , yuv_buf[frame].buf + fmt->width * fmt->height + (i/2) * fmt->width / 2
                        , fmt->width / 2);
        /* V */
        for (i = 0; i < fmt->height * 2; i++)
                memcpy(
                        rgb_buf[frame].buf + fmt->width * fmt->height / 2 * 3 + i * fmt->width / 2
                        , yuv_buf[frame].buf + fmt->width * fmt->height / 2 * 3 + (i/2) * fmt->width / 2
                        , fmt->width / 2);
#endif

	dev->captured = 1;
	complete(&dev->capture_done);
}

static void hw_capture_control(struct s3c2440_camif *dev, 
		int index, struct v4l2_control *vc)
{
	control_fn *func = NULL;
	unsigned int ret;

	if (index >= NUM_OF_HW_CONTROL)
		return;

	func = hw_control_func[index];
	if (func)
		func(vc->value, &ret);
}

static void inline __pre_capture_frame(struct s3c2440_camif *dev)
{
	down(&cam_flag);
	SET_CAM(CAM_CAPTURING);
	up(&cam_flag);
	init_completion(&dev->capture_done);
	enable_irq(IRQ_CAM_S);
	dev->captured = 0;
	CAM_CTRL = dev->hw->cfg->ctrl | CTRL_DEFAULT_VAL; 
}

static void inline __post_capture_frame(struct s3c2440_camif *dev)
{
	down(&cam_flag);
	UNSET_CAM(CAM_CAPTURING);
	up(&cam_flag);
	disable_irq(IRQ_CAM_S);
}

static void capture_frame(struct s3c2440_camif *dev)
{
	__pre_capture_frame(dev);
	wait_for_completion(&dev->capture_done);
	__post_capture_frame(dev);
}

static void capture_abort(struct s3c2440_camif *dev)
{
	if (IS_CAM(CAM_CAPTURING)) {
		disable_irq(IRQ_CAM_S);
		dev->captured = 0;
		init_completion(&dev->capture_done);
		complete(&dev->capture_done);
		enable_irq(IRQ_CAM_S);
	}
}

static int new_format(struct s3c2440_camif *dev, struct v4l2_pix_format *pixfmt)
{
	dev->clientfmt = *pixfmt;

	DPRINTK(" D1\n");
	switch (dev->clientfmt.pixelformat)
	{
		case V4L2_PIX_FMT_YUV420:
			dev->clientfmt.depth = 12;
			break;
		case V4L2_PIX_FMT_RGB565:
			dev->clientfmt.depth = 16;
			break;
#ifdef TEST_RGB32
		case V4L2_PIX_FMT_RGB32:
			dev->clientfmt.depth = 32;
			break;
#endif
		default:
#if 0
			DPRINTK("unknown format %4.4s\n",
					(char *)&dev->clientfmt.pixelformat);
			dev->clientfmt.depth = 16;
			dev->clientfmt.pixelformat = V4L2_PIX_FMT_RGB565;
#endif	
			return -EINVAL;
			break;
	}

	DPRINTK(" D2\n");
	if (
            (dev->clientfmt.width < CAMIF_MIN_W || dev->clientfmt.height < CAMIF_MIN_H) ||
            (dev->clientfmt.width > CAMIF_MAX_W ||dev->clientfmt.height > CAMIF_MAX_H)
	) {
		printk("Camera: Not support width %d, height %d \n",
				dev->clientfmt.width, dev->clientfmt.height );
		return -EINVAL;
	}

	DPRINTK(" D3\n");
	 dev->clientfmt.sizeimage = (dev->clientfmt.width
                * dev->clientfmt.height
                * dev->clientfmt.depth)
                / 8;

	s3c2440_camif_configure(&s3c2440_camif_cfg);
	DPRINTK(" D4\n");
	return 0;
}

static int v4l2_cam_open(struct v4l2_device *v, int flags, void **idptr)
{
	struct s3c2440_camif *dev = (struct s3c2440_camif *)v;
	int i = 0, n = -1;
	int cap = 0;

	DPRINTK("%s():\n", __FUNCTION__);

	if (!dev->hw)
		return -ENODEV;

	for (i = 0; i < MAX_OPENS; i++) {
		if (!dev->open_data[i].isopen)
			n = i;
		else if (!dev->open_data[i].noncapturing)
			cap = 1;
	}
	if ( -1 == n) {
		printk("%s(): No more opens on this device\n", __FUNCTION__);
		return -EBUSY;
	}
	if (flags & O_NONCAP)
		dev->open_data[n].noncapturing = 1;
	else if (cap) {
		printk("%s(): No more capturing opens on this device\n",
			__FUNCTION__);
		return -EBUSY;
	} else {
		dev->open_data[n].noncapturing = 0;
		++dev->capturing_opens;
		dev->perf.frames = 0;
		dev->perf.framesdropped = 0;
		dev->perf.bytesout = 0;
	}

	MOD_INC_USE_COUNT;

	++dev->open_count;
	dev->open_data[n].isopen = 1;
	dev->open_data[n].dev = dev;
	*idptr = &dev->open_data[n];

	DPRINTK("open(): dev->opencount = %d\n", dev->open_count);

	if (1 == dev->open_count) {
		/* Application excutes twice times. */
		s3c2440_camif_init();

		s3c2440_camif_configure(&s3c2440_camif_cfg);
//		v4l2_q_init(&dev->stream_q_cap);
//		v4l2_q_init(&dev->stream_q_done);
	}

	dev->hw->grab();

	return 0;
}

int inline find_queryctrl(int id)
{
	int i;

	if (id == V4L2_CID_PRIVATE_BASE ||
			id < V4L2_CID_BASE ||
			id > V4L2_CID_LASTP1) {
		DPRINTK("outof range\n");
		return -EDOM;
	}
	for (i = NUM_OF_HW_CONTROL - 1; i >= 0; i--)
		if (capture_control[i].id == id)
			break;    
	if (i < 0) {
		DPRINTK("there is no id=0x%08x\n", id);
		i = -EINVAL;
	}
	return i;
}

static long inline capture_read(struct s3c2440_camif *dev, __u8 *buf, int buf_size)
{
	int len = buf_size;

	memcpy(buf, rgb_buf[0].buf, buf_size);	

	return len;
}

static void v4l2_cam_close(void *id)
{
	struct device_open *o = (struct device_open *)id;
	struct s3c2440_camif *dev = o->dev;

	DPRINTK("%s():\n", __FUNCTION__);

	if (!o->noncapturing) {
		--dev->capturing_opens;
		if (0 == dev->capturing_opens)
			o->noncapturing = 1;
	}
	o->isopen = 0;
	o->dev = NULL;
	--dev->open_count;
	if (0 == dev->open_count) {
		s3c2440_camif_deinit();
	}

	DPRINTK("close(): dev->opencount = %d\n", dev->open_count);

	MOD_DEC_USE_COUNT;

	dev->hw->ungrab();
}

static void inline mmap_unrequest_buffers(struct s3c2440_camif *dev)
{
	int i;

	if (0 == dev->stream_buffers_requested ||
			dev->stream_buffers_mapped)
		return;
	for (i = 0; i < MAX_NUM_FRAMES; ++i)
			dev->stream_buf[i].requested = 0;
	dev->stream_buffers_requested = 0;
}

static int inline mmap_request_buffers(struct s3c2440_camif *dev,
				struct v4l2_requestbuffers *req)
{
	int i;
	unsigned int buflen;
	unsigned int type;
	struct v4l2_buffer  *vidbuf;

	if (dev->stream_buffers_mapped) {
		DPRINTK("already mapped\n");
		return 0;
	}
	if (req->count < 1)
		req->count = 1;
	if (req->count > MAX_NUM_FRAMES)
		req->count = MAX_NUM_FRAMES;
	type = V4L2_BUF_TYPE_CAPTURE;
	if (req->type & V4L2_BUF_REQ_CONTIG) {
		dev->stream_contig_map = 1;
		req->type = type | V4L2_BUF_REQ_CONTIG;
	}
	buflen = PAGE_ALIGN(dev->clientfmt.sizeimage);

	DPRINTK("buflen = %d\n", buflen);

	for (i = 0; i < req->count; ++i) {
		dev->stream_buf[i].requested = 1;
		vidbuf = &dev->stream_buf[i].vidbuf;
		vidbuf->index = i;
		vidbuf->type = type;
		vidbuf->offset = buflen * (i+1);
		vidbuf->length = buflen;
		vidbuf->bytesused = 0;
		vidbuf->flags = 0;
		vidbuf->timestamp = 0;
		vidbuf->sequence = 0;
		memset(&vidbuf->timecode, 0, sizeof(struct v4l2_timecode));
	}
	for (i = req->count; i < MAX_NUM_FRAMES; ++i)
        dev->stream_buf[i].requested = 0;
    dev->stream_buffers_requested = req->count;
	return 1;
}

static int v4l2_cam_ioctl(void *id, unsigned int cmd, void *arg)
{
	struct device_open *o = (struct device_open *)id;
	struct s3c2440_camif *dev = o->dev;

	switch (cmd) {
		case VIDIOC_QUERYCAP:
			{
				struct v4l2_capability *b = arg;
				b->type = V4L2_TYPE_CAPTURE;
				b->flags = V4L2_FLAG_READ ; // | V4L2_FLAG_STREAMING;
				b->inputs = 1;
				b->outputs = 0;
				b->audios = 0;
				b->maxwidth = CAMIF_MAX_W;
				b->maxheight = CAMIF_MAX_H;
				b->minwidth = CAMIF_MIN_W;
				b->minheight = CAMIF_MIN_H;
				b->maxframerate = MAX_NUM_FRAMES;
				DPRINTK("VIDIOC_QUERYCAP\n");
				break;
			}
		case VIDIOC_ENUM_PIXFMT:
			{
				struct v4l2_fmtdesc *f = arg;
				if (f->index < 0 || f->index >= NUM_CAPFMT) {
					DPRINTK("outof capfmt[]\n");
					return -EINVAL;
				}
				*f = capfmt[f->index];
				DPRINTK("VIDIOC_ENUM_PIXFMT\n");
				break;	
			}
		case VIDIOC_G_FMT:
			{
				struct v4l2_format *fmt = arg;
				if (fmt->type != V4L2_BUF_TYPE_CAPTURE) {
					printk("G_FMT wrong buffer type %d\n",fmt->type);
					return -EINVAL;
				}
				/* memcpy() */
				fmt->fmt.pix = dev->clientfmt;
				DPRINTK("%s(): VIDIOC_G_FMT\n", __FUNCTION__);
				break;	
			}
		case VIDIOC_S_FMT:
			{
				int ret;
				struct v4l2_format *fmt = arg;

				DPRINTK("%s(): VIDIOC_S_FMT\n", __FUNCTION__);
				DPRINTK(" 1 \n");
				if (o->noncapturing) {
					printk("S_FMT illegal in non-capturing open\n");
					return -EPERM;
				}
				if (fmt->type != V4L2_BUF_TYPE_CAPTURE) {
					printk("S_FMT wrong buffer type %d\n",fmt->type);
					return -EINVAL;
				}

				DPRINTK(" 2 \n");
				if (dev->streaming)
					capture_abort(dev);

				DPRINTK(" 3 \n");
				ret = new_format(dev, &(fmt->fmt.pix));

				if (dev->streaming)
					capture_frame(dev);

				fmt->fmt.pix = dev->clientfmt;
				return ret;
			}
		case VIDIOC_G_INPUT:
			{
				memcpy(arg, &dev->input, sizeof(dev->input));
				DPRINTK("VIDIOC_G_INPUT\n");
				break;
			}
		case VIDIOC_S_INPUT:
			{
				int input = *(int *)arg;

				if (input < 0 || input >= 1) { // MV303: only 1 input
					printk("Input out of range %d\n", input);
					return -EINVAL;
				}
				if (input != dev->input) {
					dev->input = input;
#if 0 // needless
					set_video_input(dev, input);
#endif
				}
				DPRINTK("VIDIOC_S_INPUT\n");