camif_mx2ads.c

pxa270下的摄像头mtd91111的驱动

                             (camfmt.height & 0x7FF);
                }
        }
       
        
        if (chan == 0) {
                prp_cntl_mask &= ~(PRP2_CNTL_CH1_YUV422);
                switch (fmt->pixelformat) {
                        case V4L2_PIX_FMT_YUYV:
                                prppixfmtreg = 0x22000888;
                                prp_cntl |= PRP2_CNTL_CH1_YUV422;
                                break;
                        case V4L2_PIX_FMT_UYVY:
                                prppixfmtreg = 0x30800888;
                                prp_cntl |= PRP2_CNTL_CH1_YUV422; /* FIXME */
                                break;
                        case V4L2_PIX_FMT_RGB565:
                                prppixfmtreg = 0x2CA00565;
                                prp_cntl |= PRP2_CNTL_CH1_RGB16;
                                break;
                        case V4L2_PIX_FMT_RGB555:
                        case V4L2_PIX_FMT_RGB565X:
                        case V4L2_PIX_FMT_BGR24:
                        case V4L2_PIX_FMT_RGB555X:
                                err("This type of pixel format is "
                                    "not supported by EMMA PRP currently\n");
                        default:
                        	spin_unlock_irqrestore(&camif_lock, flags);
                                return -EINVAL;
                }
                PRP_DEST_FRAME_FORMAT_CNTL      = prppixfmtreg;
        } else {
                prp_cntl_mask &= ~(PRP2_CNTL_CH2_YUV422 | PRP2_CNTL_CH2_YUV444);
                switch (fmt->pixelformat) {
                        case V4L2_PIX_FMT_YUV420:
                        case V4L2_PIX_FMT_YVU420:
                                prp_cntl |= (PRP2_CNTL_CH2_YUV420);
                                break;
                        case V4L2_PIX_FMT_YUV422P:
                        case V4L2_PIX_FMT_YVU422P:
                                prp_cntl |= (PRP2_CNTL_CH2_YUV422);
                                break;
                                /* FIXME: Additionaly the eMMa hw can output in
                                 * YUV 4:4:4 but current v4l2 specification has
                                 * not definition fro this type */
                        default:
                                err("This type of pixel format is "
                                    "not supported by EMMA PRP currently\n");
                        	spin_unlock_irqrestore(&camif_lock, flags);
                                return -EINVAL;
                }
        }
        chan_stat[chan].pixfmt = fmt->pixelformat;
        reg = PRP_CNTL;
        PRP_CNTL &= prp_cntl_mask;
        PRP_CNTL |= prp_cntl;
        ret = emma_set_format(chan, fmt);
        
	spin_unlock_irqrestore(&camif_lock, flags);
        return ret;
}

static inline void 
convert_to_BGR24(u16* s, u8* d, int to_user)
{
	unsigned int r,g,b;
	u8 bgr[3];
	
	r = ((*s >> 11) & 0x1f) << 3;
	g = ((*s >>  5) & 0x3f) << 2;
	b = ((*s >>  0) & 0x1f) << 3;
	bgr[2] = b;
	bgr[1] = g;
	bgr[0] = r;
	
	if (to_user)
		copy_to_user(d, bgr, sizeof(bgr));
	else
		memcpy(d, bgr, sizeof(bgr));
}


static int 
camif_convert_image(int chan, u8* src, void* dest, int to_user, int dest_stride,
				struct v4l2_pix_format* fmt)
{
	int x, y;
	int line_gap;
        
        canif_chan_stat_t *cstat = &(chan_stat[chan]);
	int width = cstat->width;
	int height = cstat->height;

	int src_BPP = (cstat->depth+7) >> 3;
	int dest_BPP = (fmt->depth+7) >> 3;
	
	line_gap = dest_stride - (width * dest_BPP);
	
	switch (fmt->pixelformat) {
	case V4L2_PIX_FMT_YUYV:
	case V4L2_PIX_FMT_UYVY:
	case V4L2_PIX_FMT_RGB565:
	case V4L2_PIX_FMT_RGB565X:
	case V4L2_PIX_FMT_RGB555:
	case V4L2_PIX_FMT_RGB555X:
                if (line_gap == 0) {
			if (to_user)
				copy_to_user(dest, src, width*height*src_BPP);
			else
				memcpy(dest, src, width*height*src_BPP);
		} else {
			for (y = 0; y < height; y++) {
				if (to_user)
					copy_to_user(dest, src, width*src_BPP);
				else
					memcpy(dest, src, width*src_BPP);
				src += width*src_BPP;
				dest += width*dest_BPP + line_gap;
                                
			}
		}
		break;
        case V4L2_PIX_FMT_YUV422P:
        case V4L2_PIX_FMT_YVU422P:	
        case V4L2_PIX_FMT_YUV420:
        case V4L2_PIX_FMT_YVU420:
                /* TODO: all planar formats above has differen location for
                 * different color planes with different sizes e.g. YUV 4:2:0:
                 * Y[width * height] U[width/2 * height/2] V[width/2*height/2].
                 * So the method to copy packed formats can not be used here.
                 * Additionaly we should support copy each own color plane to
                 * each own location request by application.
                 * Current implemtation suppose U color plane located
                 * immediately after Y color plane, and V locatead immedtely
                 * after U.
                 */
                if (to_user)
                        copy_to_user(dest, src, width*height*fmt->depth/8);
                else
                        memcpy(dest, src, width*height*fmt->depth/8);
                break;
	case V4L2_PIX_FMT_BGR24:
		/* Convert camera data from RGB565 to BGR24. */
		for (y = 0; y < height; y++) {
			for (x = 0 ; x < width ; x++) {
				convert_to_BGR24((u16*)src,
						 (u8*)dest, to_user);
				src += src_BPP;
				dest += dest_BPP;
			}
			dest += line_gap;
		}
		break;
	default:
		err("unsupported conversion request.\n");
		return -ENXIO;
	}

	return 0;
}


static inline void 
camif_start(void)
{
        uint32_t reg_val;

	CSI_REG_READ(CSICR1, reg_val);      /* Clear register */

        reg_val |= CSICR1_MCLKEN;           /* Master clock enable */
	CSI_REG_WRITE(CSICR1, reg_val);
}


static inline 
void camif_stop(void)
{
        uint32_t reg_val;

	CSI_REG_READ(CSICR1, reg_val);      /* Clear register */

        reg_val &= ~CSICR1_MCLKEN;           /* Master clock disable */
	CSI_REG_WRITE(CSICR1, reg_val);
}

static int
prepare_emma_out_buf(unsigned int chan, dma_addr_t buf)
{
        canif_chan_stat_t *cstat = &(chan_stat[chan]);
        if (chan == 0) {
                PRP_DEST_RGB1_PTR = buf;
                
        } else { /* chan == 1 */
                u32 Ysize, Usize, Ybuf, Ubuf, Vbuf;

                Ysize = cstat->width * cstat->height;

                switch (chan_stat[chan].pixfmt) {
                        case V4L2_PIX_FMT_YUV420:
                        case V4L2_PIX_FMT_YVU420:
                                Usize = Ysize /4;
                                break;

                        case V4L2_PIX_FMT_YUV422P:
                        case V4L2_PIX_FMT_YVU422P:
                                Usize = Ysize /2;
                                break;
                        default:
                                err("Bad format recived\n");
                                return -EINVAL;
                }

                Ybuf = buf;
                Ubuf = Ybuf + Ysize;
                Vbuf = Ubuf + Usize;
                
                PRP_DEST_Y_PTR                  = Ybuf;
                PRP_DEST_CB_PTR                 = Ubuf;
                PRP_DEST_CR_PTR                 = Vbuf;
                
        }

        return 0;
}


static int 
camif_snapshot(unsigned int chan, dma_addr_t buf, int size)
{
	unsigned long flags;
        int ret;
        canif_chan_stat_t *cstat = &(chan_stat[chan]);
        canif_chan_stat_t *ostat = &(chan_stat[1 - chan]); /* opposite 
                                                              channel */
                
        /*Sanity check */
        if ((chan >= CAMIF_CHANNELS_NUM) || (!buf)) {
                return -EINVAL;
        }

	spin_lock_irqsave(&camif_lock, flags);
        
	if (!this->camera) {
		spin_unlock_irqrestore(&camif_lock, flags);
		return -ENODEV;
	}
	if (cstat->snapshot_active) {
		dbg("already active!\n");
		spin_unlock_irqrestore(&camif_lock, flags);
		return 0;
	}
	if (cstat->streaming_active) {
		dbg("streaming is active!\n");
		spin_unlock_irqrestore(&camif_lock, flags);
		return -EINVAL;
	}

        if ((ret = prepare_emma_out_buf(chan, buf))) {
                spin_unlock_irqrestore(&camif_lock, flags);
                return ret;
        }
        
	cstat->snapshot_active = 1;
        
        emma_enable_interrupts(chan);

        /* If no background stream processing -- start the channel, otherwise
         * streaming will be start on next eMMa interrupt */
        if (!(ostat->streaming_active || ostat->snapshot_active)) {
                if (chan == 0) {
                        PRP_CNTL |= PRP2_CNTL_CH1EN;
                } else {
                        PRP_CNTL |= PRP2_CNTL_CH2EN;
                }
        }

	spin_unlock_irqrestore(&camif_lock, flags);

        return 0;
}


/* TODO: The best way to implemete streaming is use emma loop mode.*/
static int 
camif_start_streaming(unsigned int chan, dma_addr_t buf, int size)
{
	unsigned long flags;
        int ret;
        canif_chan_stat_t *cstat = &(chan_stat[chan]);
        canif_chan_stat_t *ostat = &(chan_stat[1 - chan]); /* opposite 
                                                              channel */

	spin_lock_irqsave(&camif_lock, flags);

	if (!this->camera) {
		spin_unlock_irqrestore(&camif_lock, flags);
		return -ENODEV;
	}
	if (cstat->streaming_active) {
		dbg("already active!\n");
		spin_unlock_irqrestore(&camif_lock, flags);
		return 0;
	}
	if (cstat->snapshot_active) {
		dbg("snapshot is active!\n");
		spin_unlock_irqrestore(&camif_lock, flags);
		return -EINVAL;
	}

        if ((ret = prepare_emma_out_buf(chan, buf))) {
                spin_unlock_irqrestore(&camif_lock, flags);
                return ret;
        }

	cstat->streaming_active = 1;

        emma_enable_interrupts(chan);

        /* If no background stream processing -- start the channel, otherwise
         * streaming will be start on next eMMa interrupt */
        if (!(ostat->streaming_active || ostat->snapshot_active)) {
                if (chan == 0) {
                         PRP_CNTL |= PRP2_CNTL_CH1EN;
                } else {
                         PRP_CNTL |= PRP2_CNTL_CH2EN;
                }
        }

	spin_unlock_irqrestore(&camif_lock, flags);
	return 0;
}

static int 
camif_abort(unsigned int chan)
{
	long my_timeout = HZ / 5;
        
        canif_chan_stat_t *cstat = &(chan_stat[chan]);

        if ((cstat->streaming_active || cstat->snapshot_active)) {
                cstat->abort = 1;

                /* Try sfly stop channe in interrupt handler */
                my_timeout = interruptible_sleep_on_timeout (
                                &cstat->abortqueue, my_timeout
                                );

                emma_disable_interrupts(chan);
                chan_stat[chan].snapshot_active =
                         chan_stat[chan].streaming_active = 0;


        }
        return 0;
}


static int camif_set_fp(int fp)
{
	int ret = 0;
        unsigned int exclk_mhz = camera_clk ;

	if (!this->camera)
		return -ENODEV;

	if (fp == this->camera->get_frame_period())
		return fp;
	
	/* first test this frame rate with camera */
	ret = this->camera->set_frame_period(fp, exclk_mhz, 1);

	if (ret >= 0) {
		/* frame rate passed tests, apply it.*/
		ret = this->camera->set_frame_period(fp,
						     exclk_mhz,
						     0);
	}
	return ret;
}

#define MOD(a,b) (((a) > (b))?((a)-(b)):((b)-(a)))

static unsigned int
camera_set_csi_clock(unsigned long cam_clock, unsigned int *hclk, unsigned int *mclk)
{
        unsigned long sysclk;
        unsigned int div1, div2;
        unsigned long mod, minmod;
        unsigned long ret_freq;
        sysclk = CLK_MPLL;
        *mclk = 32;
        *hclk = 64;