Пример #1
0
/**
* @brief 	PPU text number ram ptr set function.
* @param 	p_register_set [in]: PPU struct value set.
* @param 	text_index [in]: text_index:0:TEXT0,1: TEXT1,2: TEXT2,3: TEXT3.
* @param 	value:[in]: value: 32-bit segment address.
* @return 	SUCCESS/ERROR_ID.
*/
signed int 
gp_ppu_text_number_array_set_ptr(
PPU_REGISTER_SETS *p_register_set, 
unsigned int text_index, 
unsigned int value
)
{
	unsigned int temp,temp1;
	
	if (!p_register_set || text_index>C_PPU_TEXT4) {
		return -ENOIOCTLCMD;
	}
  
    temp = (unsigned int)gp_user_va_to_pa((unsigned short *)value);
    temp1 = (unsigned int)gp_chunk_va((unsigned int)temp);
    p_register_set->text[text_index].n_ptr = temp1;
    p_register_set->text[text_index].n_ptr_pa = temp;
  
	// Notify PPU driver to update text registers
	gp_ppu_text_set_update_reg_flag(p_register_set, text_index);

	return 0;
}
Пример #2
0
/**
 * @brief   Chunkmem device ioctl function
 */
static long chunkmem_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
	chunk_block_t block;
	void *ka;           /* kernel_addr */
	unsigned int va;    /* user_addr */
	unsigned int pa;    /* phy_addr*/
	long ret = 0;
	unsigned int offset = 0;

	switch (cmd) {
	case CHUNK_MEM_ALLOC:
	case CHUNK_MEM_SHARE:
	case CHUNK_MEM_MMAP:
		{
			if (copy_from_user(&block, (void __user*)arg, sizeof(block))) {
				ret = -EFAULT;
				break;
			}

			/* alloc|share|mmap memory */
			if (cmd == CHUNK_MEM_MMAP) {
				DIAG_VERB("CHUNK_MEM_MMAP:\n");
				ka = gp_chunk_va(block.phy_addr);
				if (ka == NULL) {
					DIAG_ERROR("CHUNK_MEM_MMAP: bad address! (%s:%08X)\n", current->comm, block.phy_addr);
					ret = -EFAULT; /* mmap fail */
					break;
				}
				/* page alignment */
				offset = block.phy_addr & ~PAGE_MASK;
				ka = (void *)((unsigned long)ka & PAGE_MASK);
				DIAG_VERB("CHUNK_MEM_MMAP: phy_addr                  = %08X\n", block.phy_addr);
				DIAG_VERB("CHUNK_MEM_MMAP: size                      = %08X\n", block.size);
				DIAG_VERB("CHUNK_MEM_MMAP: ka                        = %08X\n", (unsigned int)ka);
				DIAG_VERB("CHUNK_MEM_MMAP: offset                    = %08X\n", offset);
				DIAG_VERB("CHUNK_MEM_MMAP: PAGE_ALIGN(size + offset) = %08X\n", PAGE_ALIGN(block.size + offset));
			}
			else {
				if (cmd == CHUNK_MEM_ALLOC) {
					DIAG_VERB("CHUNK_MEM_ALLOC:\n");
					DIAG_VERB("size = %08X (%d)\n", block.size, block.size);
					ka = gp_chunk_malloc(current->tgid, block.size);
					DIAG_VERB("gp_chunk_malloc return ka=%08X\n", ka);
					if (ka == NULL) {
						DIAG_ERROR("CHUNK_MEM_ALLOC: out of memory! (%s:%08X)\n", current->comm, block.size);
						dlMalloc_Status(NULL);
						ret = -ENOMEM;
						break;
					}
					block.phy_addr = gp_chunk_pa(ka);
				}
				else { /* CHUNK_MEM_SHARE */
					DIAG_VERB("CHUNK_MEM_SHARE:\n");
					ka = gp_chunk_va(block.phy_addr);
					if ((ka == NULL) || (dlShare(ka) == 0)) {
						DIAG_ERROR("CHUNK_MEM_SHARE: bad address! (%s:%08X)\n", current->comm, block.phy_addr);
						ret = -EFAULT; /* share fail */
						break;
					}
				}
				block.size = dlMalloc_Usable_Size(ka) & PAGE_MASK; /* actual allocated size */
				DIAG_VERB("actual size = %08X (%d)\n", block.size, block.size);
				DIAG_VERB("ka = %08X\n", (unsigned int)ka);
			}

			/* mmap to userspace */
			down(&chunkmem->sem);
			down_write(&current->mm->mmap_sem);
			chunkmem->mmap_enable = 1; /* enable mmap in CHUNK_MEM_ALLOC */
			va = do_mmap_pgoff(
				file, 0, PAGE_ALIGN(block.size + offset),
				PROT_READ|PROT_WRITE,
				MAP_SHARED,
				(ka - chunkmem->vbase) >> PAGE_SHIFT);
			chunkmem->mmap_enable = 0; /* disable it */
			up_write(&current->mm->mmap_sem);
			up(&chunkmem->sem);
			if (IS_ERR_VALUE(va)) {
				ret = va; /* errcode */
				DIAG_ERROR("%s: chunkmem mmap fail(%d)! (%s)\n",
						   (cmd == CHUNK_MEM_MMAP) ? "CHUNK_MEM_MMAP" : ((cmd == CHUNK_MEM_ALLOC) ? "CHUNK_MEM_ALLOC" : "CHUNK_MEM_SHARE"),
						   ret, current->comm);
				break;
			}
			va += offset;
			block.addr = (void *)va;
			DIAG_VERB("va = %08X\n\n", va);

			if (copy_to_user((void __user*)arg, &block, sizeof(block))) {
				ret = -EFAULT;
				break;
			}
		}
		break;

	case CHUNK_MEM_FREE:
		{
			if (copy_from_user(&block, (void __user*)arg, sizeof(block))) {
				ret = -EFAULT;
				break;
			}

			/* translate user_va to ka */
			DIAG_VERB("CHUNK_MEM_FREE:\n");
			DIAG_VERB("va = %08X\n", (unsigned int)block.addr);
			pa = gp_user_va_to_pa(block.addr);    /* user_addr to phy_addr */
			if (pa == 0) {
				DIAG_ERROR("CHUNK_MEM_FREE: chunkmem user_va_to_pa fail! (%s:%08X)\n", current->comm, block.addr);
				ret = -EFAULT;
				break;
			}
			DIAG_VERB("pa = %08X\n", pa);
			ka = gp_chunk_va(pa);                  /* phy_addr to kernel_addr */
			if (ka == NULL) {
				DIAG_ERROR("CHUNK_MEM_FREE: not a chunkmem address! (%s:%08X)\n", current->comm, pa);
				ret = -EFAULT;
				break;
			}
			block.size = dlMalloc_Usable_Size(ka) & PAGE_MASK;
			DIAG_VERB("ka = %08X\n", (unsigned int)ka);
			DIAG_VERB("actual size = %08X (%d)\n\n", block.size, block.size);

			/* munmap memory */
			down_write(&current->mm->mmap_sem);
			do_munmap(current->mm, (unsigned int)block.addr, block.size);
			up_write(&current->mm->mmap_sem);

			/* free memory */
			gp_chunk_free(ka);
#if (DIAG_LEVEL >= DIAG_LVL_VERB) && !defined(DIAG_VERB_OFF)
			dlMalloc_Status(NULL);
#endif
		}
		break;

	case CHUNK_MEM_INFO:
		{
			chunk_info_t info;

            if (copy_from_user(&info, (void __user*)arg, sizeof(info))) {
                ret = -EFAULT;
                break;
            }

            if (info.pid == (unsigned int)(-1)) {
                info.pid = current->tgid;
            }

#if CHUNK_SUSPEND_TEST
			if (info.pid) {
				dlMalloc_Status(NULL);
			}
			else {
				gp_chunk_suspend(my_save_data);
				memset(chunkmem->vbase, 0, chunkmem->size);
				/* restore */
				while (blocks != NULL) {
					data_block_t *block = blocks;
					blocks = block->next;
					DIAG_DEBUG("restore data: %p %08X\n", block->addr, block->size);
					memcpy(block->addr, &block->data, block->size);
					kfree(block);
				}
			}
#else
			down(&chunkmem->sem);
			dlMalloc_Status((mem_info_t *)&info);
			up(&chunkmem->sem);
#endif
			if (copy_to_user((void __user*)arg, &info, sizeof(info))) {
				ret = -EFAULT;
				break;
			}
		}
		break;

	case CHUNK_MEM_VA2PA:
		{
			ret = -EFAULT;
			if (copy_from_user(&block, (void __user*)arg, sizeof(block))) {
				break;
			}

			pa = gp_user_va_to_pa(block.addr);    /* user_addr to phy_addr */
			if (pa != 0) {
				ka = gp_chunk_va(pa);             /* phy_addr to kernel_addr */
				if (ka != NULL) {
					block.phy_addr = pa;
					if (copy_to_user((void __user*)arg, &block, sizeof(block)) == 0) {
						ret = 0;
					}
				}
			}
		}
		break;

	case CHUNK_MEM_MUNMAP:
		{
			if (copy_from_user(&block, (void __user*)arg, sizeof(block))) {
				ret = -EFAULT;
				break;
			}

			va = (unsigned int)block.addr;
			/* page alignment */
			offset = va & ~PAGE_MASK;
			va &= PAGE_MASK;

			/* munmap memory */
			down_write(&current->mm->mmap_sem);
			do_munmap(current->mm, va, PAGE_ALIGN(block.size + offset));
			up_write(&current->mm->mmap_sem);
		}
		break;
	
	case CHUNK_MEM_FREEALL:
		gp_chunk_free_all((unsigned int)arg);
		printk(KERN_WARNING "CHUNK_MEM_FREEALL(%ld)\n", arg);
		break;
	
	case CHUNK_MEM_DUMP:
		dlMalloc_Status(0);
		break;
	
	default:
		ret = -ENOTTY; /* Inappropriate ioctl for device */
		break;
	}

	return ret;
}
Пример #3
0
/**
 * @brief   Scale do scaling function
 * @param   ctx [in] scale parameter
 * @return  success: 0,  fail: errcode
 * @see
 */
static int scale_trigger(scale_content_t *ctx)
{
	int ret = 0;
#ifndef GP_SYNC_OPTION
	unsigned int y_addr, u_addr, v_addr, output_addr;
	unsigned int format = gpHalScaleGetFmtByBmpType(ctx->src_img.type);
#endif
	if (down_interruptible(&scale->sem) != 0) {
		return -ERESTARTSYS;
	}

#ifndef GP_SYNC_OPTION
	y_addr = (unsigned int)ctx->src_img.pData;
	u_addr = (unsigned int)ctx->src_img.pDataU;
	v_addr = (unsigned int)ctx->src_img.pDataV;
	output_addr = (unsigned int)ctx->dst_img.pData;

	/* translate address from user_va to pa */
	ctx->src_img.pData  = (void *)gp_user_va_to_pa(ctx->src_img.pData);
	if (ctx->src_img.pDataU != NULL) {
		ctx->src_img.pDataU = (void *)gp_user_va_to_pa(ctx->src_img.pDataU);
		ctx->src_img.pDataV = (void *)gp_user_va_to_pa(ctx->src_img.pDataV);
	}

	ctx->dst_img.pData  = (void *)gp_user_va_to_pa(ctx->dst_img.pData);
	DIAG_VERB("[SCALE_IOCTL_TRIGGER] src:%08X  dst:%08X\n", ctx->src_imgpData, ctx->dst_img.pData);
#endif
	/* start scaling */
	ret = gp_line_buffer_request(LINE_BUFFER_MODULE_SCALER, ctx->scale_rgn.width);
	if (ret != 0) {
		DIAG_ERROR("Scalar request line buffer fail: %d\n", ret);
		goto out;
	}

	/* clean dcache */
#ifndef GP_SYNC_OPTION
	gp_clean_dcache_range(y_addr, ctx->src_img.height * ctx->src_img.bpl);
	if ((ctx->src_img.pDataU != NULL)
		&& (format == SE_CFMT_YCbCr420 || format == SE_CFMT_YCbCr422 || format == SE_CFMT_YCbCr444)) {
		unsigned int uv_size = ctx->src_img.height / ((format == SE_CFMT_YCbCr420)?2:1) * ctx->src_img.strideUV;
		gp_clean_dcache_range(u_addr, uv_size);
		gp_clean_dcache_range(v_addr, uv_size);
	}
#else
	GP_SYNC_CACHE();
#endif
	scale->done = false;
	ret = gpHalScaleExec(&ctx->src_img, &ctx->clip_rgn,
						 &ctx->dst_img, &ctx->scale_rgn);
	if (ret != SP_OK) {
		gp_line_buffer_release(LINE_BUFFER_MODULE_SCALER);
		DIAG_ERROR("gpHalScaleExec fail: %d\n", ret);
		ret = -ret;
		goto out;
	}

	if (ctx->timeout == 0) {
		ctx->timeout = SCALE_DEFAULT_TIMEOUT;
	}
#if POLLING_TEST
	DIAG_VERB("Waiting for Scaling Done\n");
	if (HAL_BUSY_WAITING(gpHalScaleDone(), ctx->timeout) >= 0) {
		gpHalScaleClearDone();
		DIAG_VERB("Scaling Done\n");
	}
	else {
		DIAG_ERROR("---------------------------------> Scaler1 Timeout (polling %dms) !!!!!!!!!!!!\n", ctx->timeout);
#if DEBUG_DUMP
		scale_param_dump(ctx);
		gpHalScaleRegDump();
#endif
	}
	gp_line_buffer_release(LINE_BUFFER_MODULE_SCALER);
#else
	if (ctx->timeout != 0xFFFFFFFF) {
		/* waiting for done */
		if (wait_event_timeout(scale->done_wait, scale->done, (ctx->timeout * HZ) / 1000) == 0) {
			ret = -ETIMEDOUT;
			gp_line_buffer_release(LINE_BUFFER_MODULE_SCALER);

			DIAG_ERROR("---------------------------------> Scaler1 Timeout (IRQ %dms) !!!!!!!!!!!!\n", ctx->timeout);
#if DEBUG_DUMP
			scale_param_dump(ctx);
			gpHalScaleRegDump();
#endif
		}
		else {
			/* invalidate dcache */
#ifndef GP_SYNC_OPTION
			#if 0
			gp_invalidate_dcache_range(output_addr + ctx->dst_img.bpl * ctx->scale_rgn.y,
									   ctx->dst_img.bpl * ctx->scale_rgn.height);
			#else
			gp_invalidate_dcache_range(output_addr, ctx->dst_img.bpl * ctx->dst_img.height);
			#endif
#else
			GP_SYNC_CACHE();
#endif
		}
	}
#endif
out:
	up(&scale->sem);
	return ret;
}
Пример #4
0
/**
 * @brief   charater device ioctl function
 * @return  success: 0
 * @see
 */
static long gp_csi_ioctl(struct file *filp, uint32_t cmd, unsigned long arg)
{
    struct v4l2_input *in;
    struct v4l2_queryctrl *qc;
    struct v4l2_fmtdesc *fmtd;
    struct v4l2_format *fmt;
    struct v4l2_buffer *bf;
    struct v4l2_control *ctrl;
    struct v4l2_streamparm *param;
    struct v4l2_subdev *sd;
    struct v4l2_cropcap *cc;
    struct v4l2_crop *crop;
    struct v4l2_interface *interface;
    struct clk *clock;
    callbackfunc_t *cb;
    gpCsiMclk_t mclk;
    char *port;
    uint8_t div;
    int16_t *addr;
    int32_t idx;
    int32_t ret=0;
    int32_t i;
    int32_t setctrl;

    if( down_interruptible(&csi_devices->sem) ) {
        return -ERESTARTSYS;
    }
    /* Success */
    switch(cmd) {
    case VIDIOC_QUERYCAP:
        copy_to_user((struct v4l2_capability*)arg, &csi_cap, sizeof(struct v4l2_capability));
        break;

    case VIDIOC_ENUMINPUT:
        in = (struct v4l2_input*)arg;
        ret = gp_get_sensorinfo( in->index, (int*)&sd, (int*)&cb, (int*)&port, (int*)&sensor );
        if( ret==-1 )
        {
            DIAG_ERROR("Index Out of bound or unregister\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        in->type = V4L2_INPUT_TYPE_CAMERA;
        strcpy( in->name, sd->name );
        break;

    case VIDIOC_S_INPUT:
        if((int32_t)arg==csi_devices->sdidx) {
            DIAG_ERROR("The Same input\n");
            goto ioctlret;
        }
        if( csi_devices->sdidx!=NO_INPUT )	{
            ret = gp_get_sensorinfo( (int32_t)arg, (int*)&sd, (int*)&cb, (int*)&port, (int*)&sensor );
            if(ret==-1)	{
                DIAG_ERROR("Set input fail\n");
                ret = -EINVAL;
                goto ioctlret;
            }
            ret = gp_get_sensorinfo( csi_devices->sdidx, (int*)&sd, (int*)&cb, (int*)&port, (int*)&sensor );
            if(ret==-1)	{
                DIAG_ERROR("suspend fail\n");
                ret = -EINVAL;
                goto ioctlret;
            }
            csi_devices->sd->ops->ext->suspend(csi_devices->sd);
            if( csi_devices->cb->standby != NULL )
                csi_devices->cb->standby(1);
            if( csi_devices->cb->powerctl!=NULL )
                csi_devices->cb->powerctl(0);
        }
        ret = gp_get_sensorinfo( (int32_t)arg, (int*)&csi_devices->sd, (int*)&(csi_devices->cb), (int*)&port, (int*)&sensor );
        if(ret==-1)
        {
            DIAG_ERROR("Set input fail\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        csi_devices->sdidx = (int32_t)arg;

        if( csi_devices->cb->powerctl!=NULL )
            csi_devices->cb->powerctl(1);
        if( csi_devices->cb->standby != NULL )
            csi_devices->cb->standby(0);
        if( csi_devices->cb->set_port!=NULL )
            csi_devices->cb->set_port(port);

        /* open mclk, some sensor need mclk befor init */
        /*		if(sensor->fmt[0].mclk_src == CSI_CLK_SPLL) {
        			clock = clk_get(NULL, "clk_ref_ceva");
        			ret = clk_get_rate(clock);
        		}else{
        			ret = USBPHY_CLK;
        		}
        		div = ret/sensor->fmt[0].mclk;
        		if((ret % sensor->fmt[0].mclk) == 0) div--;
        		DEBUG("mclk = %d\n", ret/(div + 1));
        		gpHalCsiSetMclk(sensor->fmt[0].mclk_src, div, 0, 0);*/

        sd = csi_devices->sd;
        ret = sd->ops->core->reset(sd, 0);
        ret = sd->ops->core->init(sd, 0);
        if(ret < 0)
        {
            DIAG_ERROR("sensor init fail\n");
            ret=-EINVAL;
            goto ioctlret;
        }
        break;

    case VIDIOC_G_INPUT:
        ret = csi_devices->sdidx;
        break;

    case VIDIOC_S_FMT:
        printk("gp_csi \n");
        fmt = (struct v4l2_format*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }

        if(fmt->fmt.pix.pixelformat==V4L2_PIX_FMT_YUYV)
            gpHalCsiSetFormat( CSI_YUVIN | CSI_INSEQ_YUYV );
        else if(fmt->fmt.pix.pixelformat==V4L2_PIX_FMT_UYVY)
            gpHalCsiSetFormat( CSI_YUVIN | CSI_INSEQ_UYVY );
        else if(fmt->fmt.pix.pixelformat==V4L2_PIX_FMT_RGB565)
            gpHalCsiSetFormat( CSI_RGBIN );
        else if(fmt->fmt.pix.pixelformat==V4L2_PIX_FMT_RGB555X)
            gpHalCsiSetFormat( CSI_RGBIN | CSI_RGB1555 );
        else
        {
            ret = -EINVAL;
            goto ioctlret;
        }

        for(i=0; i<sensor->sensor_fmt_num; i++) {
            if( (fmt->fmt.pix.width == sensor->fmt[i].hpixel) && (fmt->fmt.pix.height == sensor->fmt[i].vline) ) {
                sd = csi_devices->sd;
                ret = sd->ops->video->s_fmt(sd, fmt);
                if(ret >= 0) {
                    gpHalCsiSetResolution( sensor->fmt[i].hpixel-1, sensor->fmt[i].vline-1, sensor->fmt[i].hpixel*2,
                                           sensor->fmt[i].hoffset, sensor->fmt[i].voffset);

                    /*	if(sensor->fmt[0].mclk_src == CSI_CLK_SPLL) {
                    		clock = clk_get(NULL, "clk_ref_ceva");
                    		ret = clk_get_rate(clock);
                    	}else{
                    		ret = USBPHY_CLK;
                    	}
                    	div = ret/sensor->fmt[0].mclk;
                    	if((ret % sensor->fmt[0].mclk) == 0) div--;
                    //	DEBUG("mclk = %d\n", ret/(div + 1));
                    //	gpHalCsiSetMclk(sensor->fmt[0].mclk_src, div, 0, 0);*/
                }
                break;
            }
        }

        if(sensor->sensor_fmt_num == i) {
            ret = -EINVAL;
            goto ioctlret;
        }
        break;

    case VIDIOC_G_FMT:
        fmt = (struct v4l2_format*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->g_fmt(sd, fmt);
        break;

    case VIDIOC_TRY_FMT:
        fmt = (struct v4l2_format*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->try_fmt(sd, fmt);
        break;

    case VIDIOC_ENUM_FMT:
        fmtd = (struct v4l2_fmtdesc*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->enum_fmt(sd, fmtd);
        break;

    case VIDIOC_QUERYCTRL:
        qc = (struct v4l2_queryctrl*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->core->queryctrl(sd, qc);
        break;

    case VIDIOC_G_CTRL:
        ctrl = (struct v4l2_control*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->core->g_ctrl(sd, ctrl);
        break;

    case VIDIOC_S_CTRL:
        ctrl = (struct v4l2_control*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->core->s_ctrl(sd, ctrl);
        break;

    case VIDIOC_S_INTERFACE:
        interface = (struct v4l2_interface*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        setctrl = 0;

        if( interface->Interface==1 )
            setctrl |= CSI_CCIR656;
        if( interface->HsyncAct==1 )
            setctrl |= CSI_HSYNC_HACT;
        if( interface->VsyncAct==1 )
            setctrl |= CSI_VSYNC_HACT;
        if( interface->Field==1 )
            setctrl |= CSI_FIELD_ODDH;
        if( interface->Interlace==1 )
            setctrl |= CSI_INTERLACE;
        if( interface->FrameEndMode==1 )
            setctrl |= CSI_FMMODE_ODDFIELD;
        else if( interface->FrameEndMode==2 )
            setctrl |= CSI_FMMODE_EVENFIELD;
        if( interface->SampleEdge==1 )
            setctrl |= CSI_SAMPLE_NEG;
        if( interface->FmtOut==1 )
            setctrl |= CSI_YUVOUT;

        //	setctrl |= CSI_EN | CSI_UPDATESET;

        gpHalCsiSetCtrl(setctrl);
        ret = sd->ops->ext->s_interface(sd, interface);
        break;

    case VIDIOC_G_INTERFACE:
        interface = (struct v4l2_interface*)arg;
        gpHalCsiGetCtrl(&setctrl);

        if(setctrl&CSI_CCIR656)
            interface->Interface=1;
        if(setctrl&CSI_HSYNC_HACT)
            interface->HsyncAct=1;
        if(setctrl&CSI_VSYNC_HACT)
            interface->VsyncAct=1;
        if(setctrl&CSI_FIELD_ODDH)
            interface->Field=1;
        if(setctrl&CSI_INTERLACE)
            interface->Interlace=1;
        if(setctrl&CSI_FMMODE_ODDFIELD)
            interface->FrameEndMode=1;
        else if(setctrl&CSI_FMMODE_EVENFIELD)
            interface->FrameEndMode=2;
        if(setctrl&CSI_SAMPLE_NEG)
            interface->SampleEdge=1;
        if(setctrl&CSI_YUVOUT)
            interface->FmtOut=1;
        break;

    case VIDIOC_S_MCLK:
        /*		ret = copy_from_user((void*)&mclk, (void __user*)arg, sizeof(mclk));
        		if(ret < 0)
        		{
        			DIAG_ERROR("mclk set error\n");
        			ret = -EINVAL;
        			goto ioctlret;
        		}

        		if(mclk.mclk_out == 0)
        		{
        			mclk.mclk_sel = div = 0;
        			mclk.pclk_dly = 0;
        			mclk.pclk_revb = 0;
        			DEBUG("mclk = 0\n");
        		}
        		else
        		{
        			if(mclk.mclk_sel == CSI_CLK_SPLL)
        			{
        				clock = clk_get(NULL, "clk_ref_ceva");
        				ret = clk_get_rate(clock);
        			}
        			else
        			{
        				ret = USBPHY_CLK;
        			}
        			div = ret/mclk.mclk_out;
        			if((ret % mclk.mclk_out) == 0) div--;
        			DEBUG("mclk = %d\n", ret/(div + 1));
        		}
        		gpHalCsiSetMclk(mclk.mclk_sel, div, mclk.pclk_dly, mclk.pclk_revb);*/
        break;

    case VIDIOC_G_MCLK:
        gpHalCsiGetMclk(&mclk.mclk_sel, &div, &mclk.pclk_dly, &mclk.pclk_revb);
        if(mclk.mclk_sel == CSI_CLK_SPLL)
        {
            clock = clk_get(NULL, "clk_ref_ceva");
            ret = clk_get_rate(clock);
        }
        else
        {
            ret = USBPHY_CLK;
        }

        mclk.mclk_out = ret/(div + 1);
        ret = copy_to_user((void __user*)arg, (void*)&mclk, sizeof(mclk));
        if(ret < 0)
        {
            DIAG_ERROR("mclk get error\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        break;

    case VIDIOC_CROPCAP:
        cc = (struct v4l2_cropcap*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->cropcap(sd, cc);
        break;

    case VIDIOC_G_CROP:
        crop = (struct v4l2_crop*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->g_crop(sd, crop);
        break;

    case VIDIOC_S_CROP:
        crop = (struct v4l2_crop*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->s_crop(sd, crop);
        break;

    case VIDIOC_G_PARM:
        param = (struct v4l2_streamparm*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->g_parm(sd, param);
        break;

    case VIDIOC_S_PARM:
        param = (struct v4l2_streamparm*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        ret = sd->ops->video->s_parm(sd, param);
        break;

    case VIDIOC_REQBUFS:
        copy_from_user(&(csi_devices->rbuf),(struct v4l2_requestbuffers*)arg, sizeof(struct v4l2_requestbuffers));
        ret = check_rqbuf_type();
        break;

    case VIDIOC_STREAMON:
        //sd = csi_devices->sd;
        //ret = sd->ops->core->init(sd, 0);
        if(ret < 0)
        {
            DIAG_ERROR("sensor init fail\n");
            ret = -EINVAL;
            goto ioctlret;
        }

        if(arg == (unsigned long)NULL)
        {
            if(csi_devices->in_que[0] == 0xFF)
            {
                DIAG_ERROR("No buffer in Que\n");
                ret = -EINVAL;
                goto ioctlret;
            }
            gpHalCsiSetBuf(csi_devices->bfaddr[csi_devices->in_que[0]]);
            gpHalCsiStart();
        }
        else
        {
            DIAG_ERROR("csi start fail\n");
            ret=-EINVAL;
        }
        break;

    case VIDIOC_STREAMOFF:
        gpHalCsiSetMclk(0, 0, 0, 0);
        gpHalCsiStop();
        CLEAR(csi_devices->bfaddr, 0);
        CLEAR(csi_devices->bf, 0);
        CLEAR(csi_devices->in_que, 0xFF);
        CLEAR(csi_devices->out_que, 0xFF);
        csi_devices->csi_feint_flag = 0;
        break;

    case VIDIOC_QBUF:
        bf = (struct v4l2_buffer*)arg;
        if( bf->type != csi_devices->rbuf.type ) {
            DIAG_ERROR("QBuf Type error\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        if( bf->index>=csi_devices->rbuf.count ) {
            DIAG_ERROR("QBuf index out of bound\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        for( i=0; i<CSI_MAX_QUE; i++ ) {
            if(csi_devices->in_que[i]==0xFF)	{
                csi_devices->in_que[i]=bf->index;
                break;
            }
            if(i==(CSI_MAX_QUE-1)) {
                DIAG_ERROR("Que buffer is full\n");
                ret = -EINVAL;
                goto ioctlret;
            }
        }
        idx = bf->index;
        copy_from_user(&(csi_devices->bf[idx]), (struct v4l2_buffer*)arg, sizeof(struct v4l2_buffer));
        csi_devices->bf[idx].flags = V4L2_BUF_FLAG_QUEUED;
        addr = (int16_t *)gp_user_va_to_pa((int16_t *)bf->m.userptr);
        csi_devices->bfaddr[idx] = (int)addr;
        break;

    case VIDIOC_DQBUF:
        bf = (struct v4l2_buffer*)arg;
        if( bf->type != csi_devices->rbuf.type )
        {
            ret = -EINVAL;
            goto ioctlret;
        }
        if( csi_devices->out_que[0]==0xFF )
        {
            DIAG_ERROR("no buffer ready\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        copy_to_user((struct v4l2_buffer*)arg, &(csi_devices->bf[csi_devices->out_que[0]]), sizeof(struct v4l2_buffer));
        // shift the out_queue buffer
        for(i=0; i<(CSI_MAX_QUE-1); i++)
            csi_devices->out_que[i] = csi_devices->out_que[i+1];

        csi_devices->out_que[CSI_MAX_QUE-1]=0xFF;
        break;

    case VIDIOC_QUERYSTD:
    {
        v4l2_std_id *std = (v4l2_std_id*)arg;
        if(csi_devices->sdidx==NO_INPUT)
        {
            DIAG_ERROR("please set input first\n");
            ret = -EINVAL;
            goto ioctlret;
        }
        sd = csi_devices->sd;
        if ( sd->ops->video->querystd )
            ret = sd->ops->video->querystd(sd, std);
        else
            ret = ENOIOCTLCMD;
        break;
    }
    }

ioctlret:
    up(&csi_devices->sem);

    return ret;
}