static void vga_set_modelist(void)
{
	int i, j = 0, modelen = 0;
	struct fb_videomode *mode = NULL;
	struct list_head	*modelist  = &ddev->modelist;
	struct fb_monspecs	*specs = &ddev->specs;
	int pixclock;

	fb_destroy_modelist(modelist);

	for(i = 1; i <= specs->modedb_len; i++) {
		mode = &specs->modedb[i % specs->modedb_len];	
		//printk("%d %d %d %d %d %d %d %d %d %d %d %d %d\n",mode->refresh,mode->xres,mode->yres,mode->pixclock,mode->left_margin,mode->right_margin,mode->upper_margin, \
		 //  mode->lower_margin,mode->hsync_len,mode->vsync_len, mode->sync,mode->vmode,mode->flag);
		pixclock = PICOS2KHZ(mode->pixclock);
		if (pixclock < (specs->dclkmax / 1000)) {
			for (j = 0; j < get_vga_mode_len(); j++) {
				if (default_modedb[j].xres  == mode->xres &&
						default_modedb[j].yres == mode->yres &&
						    (default_modedb[j].refresh == mode->refresh ||
							  default_modedb[j].refresh == mode->refresh + 1 ||
							    default_modedb[j].refresh == mode->refresh -1 )) {
					fb_add_videomode(&default_modedb[j], modelist);
					modelen++;
					break;
				}
			}
		}
	}
	
	ddev->modelen = modelen;
}

int vga_switch_default_screen(void)
{
	int i, mode_num = DEFAULT_MODE;
	const struct fb_videomode *mode = NULL;
	static int init_flag = 0;
	
	if (ddev == NULL) {
		printk("vga-ddc: No DDC Dev.\n");
		return -ENODEV;
	}

	mode = vga_find_best_mode();
	if (mode) {
		printk("vga-ddc: best mode %dx%[email protected]%d[pixclock-%ld KHZ]\n", mode->xres, mode->yres,
				mode->refresh, PICOS2KHZ(mode->pixclock));
	    for(i = 0; i < get_vga_mode_len(); i++)
	    {
		    if(fb_mode_is_equal(&sda7123_vga_mode[i], mode))
		    {	
			   mode_num = i + 1;
			   break;
		    }
	    }
	}

	return mode_num;
}

static int sh_mobile_check_var(struct fb_var_screeninfo *var, struct fb_info *info)
{
	struct sh_mobile_lcdc_chan *ch = info->par;
	struct sh_mobile_lcdc_priv *p = ch->lcdc;

	if (var->xres > MAX_XRES || var->yres > MAX_YRES ||
	    var->xres * var->yres * (ch->cfg.bpp / 8) * 2 > info->fix.smem_len) {
		dev_warn(info->dev, "Invalid info: %u-%u-%u-%u x %u-%u-%u-%u @ %lukHz!\n",
			 var->left_margin, var->xres, var->right_margin, var->hsync_len,
			 var->upper_margin, var->yres, var->lower_margin, var->vsync_len,
			 PICOS2KHZ(var->pixclock));
		return -EINVAL;
	}

	/* only accept the forced_bpp for dual channel configurations */
	if (p->forced_bpp && p->forced_bpp != var->bits_per_pixel)
		return -EINVAL;

	switch (var->bits_per_pixel) {
	case 16: /* PKF[4:0] = 00011 - RGB 565 */
	case 24: /* PKF[4:0] = 01011 - RGB 888 */
	case 32: /* PKF[4:0] = 00000 - RGBA 888 */
		break;
	default:
		return -EINVAL;
	}

	return 0;
}

void hdmi_clk_regenerator_update_pixel_clock(u32 pixclock)
{

	/* Translate pixel clock in ps (pico seconds) to Hz  */
	pixel_clk_rate = PICOS2KHZ(pixclock) * 1000UL;
	hdmi_set_clk_regenerator();
}

static int ldb_disp_setup(struct mxc_dispdrv_handle *disp, struct fb_info *fbi)
{
	uint32_t reg, val;
	uint32_t pixel_clk, rounded_pixel_clk;
	struct clk *ldb_clk_parent;
	struct ldb_data *ldb = mxc_dispdrv_getdata(disp);
	int setting_idx, di;

	setting_idx = find_ldb_setting(ldb, fbi);
	if (setting_idx < 0)
		return setting_idx;

	di = ldb->setting[setting_idx].di;

	/* restore channel mode setting */
	val = readl(ldb->control_reg);
	val |= ldb->setting[setting_idx].ch_val;
	writel(val, ldb->control_reg);
	dev_dbg(&ldb->pdev->dev, "LDB setup, control reg:0x%x\n",
			readl(ldb->control_reg));

	/* vsync setup */
	reg = readl(ldb->control_reg);
	if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT) {
		if (di == 0)
			reg = (reg & ~LDB_DI0_VS_POL_MASK)
				| LDB_DI0_VS_POL_ACT_HIGH;
		else
			reg = (reg & ~LDB_DI1_VS_POL_MASK)
				| LDB_DI1_VS_POL_ACT_HIGH;
	} else {
		if (di == 0)
			reg = (reg & ~LDB_DI0_VS_POL_MASK)
				| LDB_DI0_VS_POL_ACT_LOW;
		else
			reg = (reg & ~LDB_DI1_VS_POL_MASK)
				| LDB_DI1_VS_POL_ACT_LOW;
	}
	writel(reg, ldb->control_reg);

	/* clk setup */
	if (ldb->setting[setting_idx].clk_en)
		clk_disable(ldb->setting[setting_idx].ldb_di_clk);
	pixel_clk = (PICOS2KHZ(fbi->var.pixclock)) * 1000UL;
	ldb_clk_parent = clk_get_parent(ldb->setting[setting_idx].ldb_di_clk);
	if ((ldb->mode == LDB_SPL_DI0) || (ldb->mode == LDB_SPL_DI1))
		clk_set_rate(ldb_clk_parent, pixel_clk * 7 / 2);
	else
		clk_set_rate(ldb_clk_parent, pixel_clk * 7);
	rounded_pixel_clk = clk_round_rate(ldb->setting[setting_idx].ldb_di_clk,
			pixel_clk);
	clk_set_rate(ldb->setting[setting_idx].ldb_di_clk, rounded_pixel_clk);
	clk_enable(ldb->setting[setting_idx].ldb_di_clk);
	if (!ldb->setting[setting_idx].clk_en)
		ldb->setting[setting_idx].clk_en = true;

	return 0;
}

int tegra_dc_set_fb_mode(struct tegra_dc *dc,
		const struct fb_videomode *fbmode, bool stereo_mode)
{
	struct tegra_dc_mode mode;

	if (!fbmode->pixclock)
		return -EINVAL;

	mode.pclk = PICOS2KHZ(fbmode->pixclock) * 1000;
	mode.h_sync_width = fbmode->hsync_len;
	mode.v_sync_width = fbmode->vsync_len;
	mode.h_back_porch = fbmode->left_margin;
	mode.v_back_porch = fbmode->upper_margin;
	mode.h_active = fbmode->xres;
	mode.v_active = fbmode->yres;
	mode.h_front_porch = fbmode->right_margin;
	mode.v_front_porch = fbmode->lower_margin;
	mode.stereo_mode = stereo_mode;
	if (dc->out->type == TEGRA_DC_OUT_HDMI) {
		/* HDMI controller requires h_ref=1, v_ref=1 */
		mode.h_ref_to_sync = 1;
		mode.v_ref_to_sync = 1;
	} else {
		calc_ref_to_sync(&mode);
	}
	if (!check_ref_to_sync(&mode)) {
		dev_err(&dc->ndev->dev,
				"Display timing doesn't meet restrictions.\n");
		return -EINVAL;
	}
	dev_dbg(&dc->ndev->dev, "Using mode %dx%d pclk=%d href=%d vref=%d\n",
		mode.h_active, mode.v_active, mode.pclk,
		mode.h_ref_to_sync, mode.v_ref_to_sync
	);

#ifndef CONFIG_TEGRA_HDMI_74MHZ_LIMIT
	/* Double the pixel clock and update v_active only for
	 * frame packed mode */
	if (mode.stereo_mode) {
		mode.pclk *= 2;
		/* total v_active = yres*2 + activespace */
		mode.v_active = fbmode->yres * 2 +
				fbmode->vsync_len +
				fbmode->upper_margin +
				fbmode->lower_margin;
	}
#endif

	mode.flags = 0;

	if (!(fbmode->sync & FB_SYNC_HOR_HIGH_ACT))
		mode.flags |= TEGRA_DC_MODE_FLAG_NEG_H_SYNC;

	if (!(fbmode->sync & FB_SYNC_VERT_HIGH_ACT))
		mode.flags |= TEGRA_DC_MODE_FLAG_NEG_V_SYNC;

	return tegra_dc_set_mode(dc, &mode);
}

struct fb_videomode *vga_find_max_mode(void)
{
	struct fb_videomode *mode = NULL/*, *nearest_mode = NULL*/;
	struct fb_monspecs *specs = NULL;
	int i, pixclock;
	
	if (ddev == NULL)
		return NULL;

	specs = &ddev->specs;
	if(specs->modedb_len) {

		/* Get max resolution timing */
		mode = &specs->modedb[0];
		
		for (i = 0; i < specs->modedb_len; i++) {
			if(specs->modedb[i].xres > mode->xres)
				mode = &specs->modedb[i];
			else if( (specs->modedb[i].xres == mode->xres) && (specs->modedb[i].yres > mode->yres) )
				mode = &specs->modedb[i];
		}

		// For some monitor, the max pixclock read from EDID is smaller
		// than the clock of max resolution mode supported. We fix it.
		pixclock = PICOS2KHZ(mode->pixclock);
		pixclock /= 250;
		pixclock *= 250;
		pixclock *= 1000;
		if(pixclock == 148250000)
			pixclock = 148500000;
		if(pixclock > specs->dclkmax)
			specs->dclkmax = pixclock;


		printk("vga-ddc: max mode %dx%[email protected]%d[pixclock-%ld KHZ]\n", mode->xres, mode->yres,
				mode->refresh, PICOS2KHZ(mode->pixclock));
	}

	return mode;
}

static void sii902x_setup(struct fb_info *fbi)
{
	u16 data[4];
	u32 refresh;
	u8 *tmp;
	int i;

	dev_dbg(&sii902x.client->dev, "Sii902x: setup..\n");

	/* Power up */
	i2c_smbus_write_byte_data(sii902x.client, 0x1E, 0x00);

	/* set TPI video mode */
	data[0] = PICOS2KHZ(fbi->var.pixclock) / 10;
	data[2] = fbi->var.hsync_len + fbi->var.left_margin +
		  fbi->var.xres + fbi->var.right_margin;
	data[3] = fbi->var.vsync_len + fbi->var.upper_margin +
		  fbi->var.yres + fbi->var.lower_margin;
	refresh = data[2] * data[3];
	refresh = (PICOS2KHZ(fbi->var.pixclock) * 1000) / refresh;
	data[1] = refresh * 100;
	tmp = (u8 *)data;
	for (i = 0; i < 8; i++)
		i2c_smbus_write_byte_data(sii902x.client, i, tmp[i]);

	/* input bus/pixel: full pixel wide (24bit), rising edge */
	i2c_smbus_write_byte_data(sii902x.client, 0x08, 0x70);
	/* Set input format to RGB */
	i2c_smbus_write_byte_data(sii902x.client, 0x09, 0x00);
	/* set output format to RGB */
	i2c_smbus_write_byte_data(sii902x.client, 0x0A, 0x00);
	/* audio setup */
	i2c_smbus_write_byte_data(sii902x.client, 0x25, 0x00);
	i2c_smbus_write_byte_data(sii902x.client, 0x26, 0x40);
	i2c_smbus_write_byte_data(sii902x.client, 0x27, 0x00);
}

static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
{
	struct fb_var_screeninfo *var = &hdmi->var;
	struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
	struct fb_videomode *lcd_cfg = &pdata->lcd_chan->lcd_cfg;
	unsigned long height = var->height, width = var->width;
	int i;
	u8 edid[128];

	/* Read EDID */
	pr_debug("Read back EDID code:");
	for (i = 0; i < 128; i++) {
		edid[i] = hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
#ifdef DEBUG
		if ((i % 16) == 0) {
			printk(KERN_CONT "\n");
			printk(KERN_DEBUG "%02X | %02X", i, edid[i]);
		} else {
			printk(KERN_CONT " %02X", edid[i]);
		}
#endif
	}
#ifdef DEBUG
	printk(KERN_CONT "\n");
#endif
	fb_parse_edid(edid, var);
	pr_debug("%u-%u-%u-%u x %u-%u-%u-%u @ %lu kHz monitor detected\n",
		 var->left_margin, var->xres, var->right_margin, var->hsync_len,
		 var->upper_margin, var->yres, var->lower_margin, var->vsync_len,
		 PICOS2KHZ(var->pixclock));

	var->width		= width;
	var->xres		= lcd_cfg->xres;
	var->xres_virtual	= lcd_cfg->xres;
	var->left_margin	= lcd_cfg->left_margin;
	var->right_margin	= lcd_cfg->right_margin;
	var->hsync_len		= lcd_cfg->hsync_len;
	var->height		= height;
	var->yres		= lcd_cfg->yres;
	var->yres_virtual	= lcd_cfg->yres * 2;
	var->upper_margin	= lcd_cfg->upper_margin;
	var->lower_margin	= lcd_cfg->lower_margin;
	var->vsync_len		= lcd_cfg->vsync_len;
	var->sync		= lcd_cfg->sync;
	var->pixclock		= lcd_cfg->pixclock;

	hdmi_external_video_param(hdmi);
}

static unsigned long sh_hdmi_rate_error(struct sh_hdmi *hdmi,
					const struct fb_videomode *mode)
{
	long target = PICOS2KHZ(mode->pixclock) * 1000,
		rate = clk_round_rate(hdmi->hdmi_clk, target);
	unsigned long rate_error = rate > 0 ? abs(rate - target) : ULONG_MAX;

	dev_dbg(hdmi->dev, "%u-%u-%u-%u x %u-%u-%u-%u\n",
		mode->left_margin, mode->xres,
		mode->right_margin, mode->hsync_len,
		mode->upper_margin, mode->yres,
		mode->lower_margin, mode->vsync_len);

	dev_dbg(hdmi->dev, "\[email protected]%lu(+/-%lu)Hz, e=%lu / 1000, r=%uHz\n", target,
		 rate_error, rate_error ? 10000 / (10 * target / rate_error) : 0,
		 mode->refresh);

	return rate_error;
}

void adf_modeinfo_from_fb_videomode(const struct fb_videomode *vmode,
		struct drm_mode_modeinfo *mode)
{
	memset(mode, 0, sizeof(*mode));

	mode->hdisplay = vmode->xres;
	mode->hsync_start = mode->hdisplay + vmode->right_margin;
	mode->hsync_end = mode->hsync_start + vmode->hsync_len;
	mode->htotal = mode->hsync_end + vmode->left_margin;

	mode->vdisplay = vmode->yres;
	mode->vsync_start = mode->vdisplay + vmode->lower_margin;
	mode->vsync_end = mode->vsync_start + vmode->vsync_len;
	mode->vtotal = mode->vsync_end + vmode->upper_margin;

	mode->clock = vmode->pixclock ? PICOS2KHZ(vmode->pixclock) : 0;

	mode->flags = 0;
	if (vmode->sync & FB_SYNC_HOR_HIGH_ACT)
		mode->flags |= DRM_MODE_FLAG_PHSYNC;
	if (vmode->sync & FB_SYNC_VERT_HIGH_ACT)
		mode->flags |= DRM_MODE_FLAG_PVSYNC;
	if (vmode->sync & FB_SYNC_COMP_HIGH_ACT)
		mode->flags |= DRM_MODE_FLAG_PCSYNC;
	if (vmode->sync & FB_SYNC_BROADCAST)
		mode->flags |= DRM_MODE_FLAG_BCAST;
	if (vmode->vmode & FB_VMODE_INTERLACED)
		mode->flags |= DRM_MODE_FLAG_INTERLACE;
	if (vmode->vmode & FB_VMODE_DOUBLE)
		mode->flags |= DRM_MODE_FLAG_DBLSCAN;

	if (vmode->refresh)
		mode->vrefresh = vmode->refresh;
	else
		adf_modeinfo_set_vrefresh(mode);

	if (vmode->name)
		strlcpy(mode->name, vmode->name, sizeof(mode->name));
	else
		adf_modeinfo_set_name(mode);
}

/**
 * fb_var_to_videomode - convert fb_var_screeninfo to fb_videomode
 * @mode: pointer to struct fb_videomode
 * @var: pointer to struct fb_var_screeninfo
 */
void fb_var_to_videomode(struct fb_videomode *mode,
			 const struct fb_var_screeninfo *var)
{
	u32 pixclock, hfreq, htotal, vtotal;

	mode->name = NULL;
	mode->xres = var->xres;
	mode->yres = var->yres;
	mode->pixclock = var->pixclock;
	mode->hsync_len = var->hsync_len;
	mode->vsync_len = var->vsync_len;
	mode->left_margin = var->left_margin;
	mode->right_margin = var->right_margin;
	mode->upper_margin = var->upper_margin;
	mode->lower_margin = var->lower_margin;
	mode->sync = var->sync;
	mode->vmode = var->vmode & FB_VMODE_MASK;
	mode->flag = FB_MODE_IS_FROM_VAR;
	mode->refresh = 0;

	if (!var->pixclock)
		return;

	pixclock = PICOS2KHZ(var->pixclock) * 1000;

	htotal = var->xres + var->right_margin + var->hsync_len +
		var->left_margin;
	vtotal = var->yres + var->lower_margin + var->vsync_len +
		var->upper_margin;

	if (var->vmode & FB_VMODE_INTERLACED)
		vtotal /= 2;
	if (var->vmode & FB_VMODE_DOUBLE)
		vtotal *= 2;

	hfreq = pixclock/htotal;
	mode->refresh = hfreq/vtotal;
}

int s3cfb_set_clock(struct s3cfb_global *ctrl)
{
	struct s3c_platform_fb *pdata = to_fb_plat(ctrl->dev);
	u32 cfg, maxclk, src_clk, vclk, div;

	/* spec is under 100MHz */
	maxclk = 100 * 1000000;

	cfg = readl(ctrl->regs + S3C_VIDCON0);

	if (pdata->hw_ver == 0x70) {
		cfg &= ~(S3C_VIDCON0_CLKVALUP_MASK |
			S3C_VIDCON0_VCLKEN_MASK);
		cfg |= (S3C_VIDCON0_CLKVALUP_ALWAYS |
			S3C_VIDCON0_VCLKEN_FREERUN);

		src_clk = clk_get_rate(ctrl->clock);
		printk(KERN_DEBUG "FIMD src sclk = %d\n", src_clk);
	} else {
		cfg &= ~(S3C_VIDCON0_CLKSEL_MASK |
			S3C_VIDCON0_CLKVALUP_MASK |
			S3C_VIDCON0_VCLKEN_MASK |
			S3C_VIDCON0_CLKDIR_MASK);
		cfg |= (S3C_VIDCON0_CLKVALUP_ALWAYS |
			S3C_VIDCON0_VCLKEN_NORMAL |
			S3C_VIDCON0_CLKDIR_DIVIDED);

		if (strcmp(pdata->clk_name, "sclk_fimd") == 0) {
			cfg |= S3C_VIDCON0_CLKSEL_SCLK;
			src_clk = clk_get_rate(ctrl->clock);
			printk(KERN_DEBUG "FIMD src sclk = %d\n", src_clk);

		} else {
			cfg |= S3C_VIDCON0_CLKSEL_HCLK;
			src_clk = ctrl->clock->parent->rate;
			printk(KERN_DEBUG "FIMD src hclk = %d\n", src_clk);
		}
	}

	vclk = PICOS2KHZ(ctrl->fb[pdata->default_win]->var.pixclock) * 1000;

	if (vclk > maxclk) {
		dev_info(ctrl->dev, "vclk(%d) should be smaller than %d\n",
			vclk, maxclk);
		/* vclk = maxclk; */
	}

	div = DIV_ROUND_CLOSEST(src_clk, vclk);

	if (div == 0) {
		dev_err(ctrl->dev, "div(%d) should be non-zero\n", div);
		div = 1;
	}

	if ((src_clk/div) > maxclk)
		dev_info(ctrl->dev, "vclk(%d) should be smaller than %d Hz\n",
			src_clk/div, maxclk);

	cfg &= ~S3C_VIDCON0_CLKVAL_F(0xff);
	cfg |= S3C_VIDCON0_CLKVAL_F(div - 1);
	writel(cfg, ctrl->regs + S3C_VIDCON0);

	dev_info(ctrl->dev, "parent clock: %d, vclk: %d, vclk div: %d\n",
			src_clk, vclk, div);

	return 0;
}

/**
 *      atmel_lcdfb_set_par - Alters the hardware state.
 *      @info: frame buffer structure that represents a single frame buffer
 *
 *	Using the fb_var_screeninfo in fb_info we set the resolution
 *	of the this particular framebuffer. This function alters the
 *	par AND the fb_fix_screeninfo stored in fb_info. It doesn't
 *	not alter var in fb_info since we are using that data. This
 *	means we depend on the data in var inside fb_info to be
 *	supported by the hardware.  atmel_lcdfb_check_var is always called
 *	before atmel_lcdfb_set_par to ensure this.  Again if you can't
 *	change the resolution you don't need this function.
 *
 */
static int atmel_lcdfb_set_par(struct fb_info *info)
{
    struct atmel_lcdfb_info *sinfo = info->par;
    unsigned long hozval_linesz;
    unsigned long value;
    unsigned long clk_value_khz;
    unsigned long bits_per_line;

    dev_dbg(info->device, "%s:\n", __func__);
    dev_dbg(info->device, "  * resolution: %ux%u (%ux%u virtual)\n",
            info->var.xres, info->var.yres,
            info->var.xres_virtual, info->var.yres_virtual);

    /* Turn off the LCD controller and the DMA controller */
    lcdc_writel(sinfo, ATMEL_LCDC_PWRCON, sinfo->guard_time << ATMEL_LCDC_GUARDT_OFFSET);

    /* Wait for the LCDC core to become idle */
    while (lcdc_readl(sinfo, ATMEL_LCDC_PWRCON) & ATMEL_LCDC_BUSY)
        msleep(10);

    lcdc_writel(sinfo, ATMEL_LCDC_DMACON, 0);

    if (info->var.bits_per_pixel == 1)
        info->fix.visual = FB_VISUAL_MONO01;
    else if (info->var.bits_per_pixel <= 8)
        info->fix.visual = FB_VISUAL_PSEUDOCOLOR;
    else
        info->fix.visual = FB_VISUAL_TRUECOLOR;

    bits_per_line = info->var.xres_virtual * info->var.bits_per_pixel;
    info->fix.line_length = DIV_ROUND_UP(bits_per_line, 8);

    /* Re-initialize the DMA engine... */
    dev_dbg(info->device, "  * update DMA engine\n");
    atmel_lcdfb_update_dma(info, &info->var);

    /* ...set frame size and burst length = 8 words (?) */
    value = (info->var.yres * info->var.xres * info->var.bits_per_pixel) / 32;
    value |= ((ATMEL_LCDC_DMA_BURST_LEN - 1) << ATMEL_LCDC_BLENGTH_OFFSET);
    lcdc_writel(sinfo, ATMEL_LCDC_DMAFRMCFG, value);

    /* Now, the LCDC core... */

    /* Set pixel clock */
    clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;

    value = DIV_ROUND_UP(clk_value_khz, PICOS2KHZ(info->var.pixclock));

    value = (value / 2) - 1;
    dev_dbg(info->device, "  * programming CLKVAL = 0x%08lx\n", value);

    if (value <= 0) {
        dev_notice(info->device, "Bypassing pixel clock divider\n");
        lcdc_writel(sinfo, ATMEL_LCDC_LCDCON1, ATMEL_LCDC_BYPASS);
    } else {
        lcdc_writel(sinfo, ATMEL_LCDC_LCDCON1, value << ATMEL_LCDC_CLKVAL_OFFSET);
        info->var.pixclock = KHZ2PICOS(clk_value_khz / (2 * (value + 1)));
        dev_dbg(info->device, "  updated pixclk:     %lu KHz\n",
                PICOS2KHZ(info->var.pixclock));
    }


    /* Initialize control register 2 */
    value = sinfo->default_lcdcon2;

    if (!(info->var.sync & FB_SYNC_HOR_HIGH_ACT))
        value |= ATMEL_LCDC_INVLINE_INVERTED;
    if (!(info->var.sync & FB_SYNC_VERT_HIGH_ACT))
        value |= ATMEL_LCDC_INVFRAME_INVERTED;

    switch (info->var.bits_per_pixel) {
    case 1:
        value |= ATMEL_LCDC_PIXELSIZE_1;
        break;
    case 2:
        value |= ATMEL_LCDC_PIXELSIZE_2;
        break;
    case 4:
        value |= ATMEL_LCDC_PIXELSIZE_4;
        break;
    case 8:
        value |= ATMEL_LCDC_PIXELSIZE_8;
        break;
    case 15: /* fall through */
    case 16:
        value |= ATMEL_LCDC_PIXELSIZE_16;
//.........这里部分代码省略.........

static int atmel_lcdfb_check_var(struct fb_info *info)
{
	struct device_d *dev = &info->dev;
	struct atmel_lcdfb_info *sinfo = info->priv;
	struct atmel_lcdfb_platform_data *pdata = sinfo->pdata;
	struct fb_videomode *mode = info->mode;
	unsigned long clk_value_khz;

	clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;

	dev_dbg(dev, "%s:\n", __func__);

	if (!(mode->pixclock && info->bits_per_pixel)) {
		dev_err(dev, "needed value not specified\n");
		return -EINVAL;
	}

	dev_dbg(dev, "  resolution: %ux%u\n", mode->xres, mode->yres);
	dev_dbg(dev, "  pixclk:     %lu KHz\n", PICOS2KHZ(mode->pixclock));
	dev_dbg(dev, "  bpp:        %u\n", info->bits_per_pixel);
	dev_dbg(dev, "  clk:        %lu KHz\n", clk_value_khz);

	if (PICOS2KHZ(mode->pixclock) > clk_value_khz) {
		dev_err(dev, "%lu KHz pixel clock is too fast\n", PICOS2KHZ(mode->pixclock));
		return -EINVAL;
	}

	/* Saturate vertical and horizontal timings at maximum values */
	if (sinfo->dev_data->limit_screeninfo)
		sinfo->dev_data->limit_screeninfo(mode);

	mode->vsync_len = min_t(u32, mode->vsync_len,
			(ATMEL_LCDC_VPW >> ATMEL_LCDC_VPW_OFFSET) + 1);
	mode->upper_margin = min_t(u32, mode->upper_margin,
			ATMEL_LCDC_VBP >> ATMEL_LCDC_VBP_OFFSET);
	mode->lower_margin = min_t(u32, mode->lower_margin,
			ATMEL_LCDC_VFP);
	mode->right_margin = min_t(u32, mode->right_margin,
			(ATMEL_LCDC_HFP >> ATMEL_LCDC_HFP_OFFSET) + 1);
	mode->hsync_len = min_t(u32, mode->hsync_len,
			(ATMEL_LCDC_HPW >> ATMEL_LCDC_HPW_OFFSET) + 1);
	mode->left_margin = min_t(u32, mode->left_margin,
			ATMEL_LCDC_HBP + 1);

	/* Some parameters can't be zero */
	mode->vsync_len = max_t(u32, mode->vsync_len, 1);
	mode->right_margin = max_t(u32, mode->right_margin, 1);
	mode->hsync_len = max_t(u32, mode->hsync_len, 1);
	mode->left_margin = max_t(u32, mode->left_margin, 1);

	switch (info->bits_per_pixel) {
	case 1:
	case 2:
	case 4:
	case 8:
		info->red.offset = info->green.offset = info->blue.offset = 0;
		info->red.length = info->green.length = info->blue.length
			= info->bits_per_pixel;
		break;
	case 16:
		/* Older SOCs use IBGR:555 rather than BGR:565. */
		if (pdata->have_intensity_bit)
			info->green.length = 5;
		else
			info->green.length = 6;
		if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
			/* RGB:5X5 mode */
			info->red.offset = info->green.length + 5;
			info->blue.offset = 0;
		} else {
			/* BGR:5X5 mode */
			info->red.offset = 0;
			info->blue.offset = info->green.length + 5;
		}
		info->green.offset = 5;
		info->red.length = info->blue.length = 5;
		break;
	case 32:
		info->transp.offset = 24;
		info->transp.length = 8;
		/* fall through */
	case 24:
		if (pdata->lcd_wiring_mode == ATMEL_LCDC_WIRING_RGB) {
			/* RGB:888 mode */
			info->red.offset = 16;
			info->blue.offset = 0;
		} else {
			/* BGR:888 mode */
			info->red.offset = 0;
			info->blue.offset = 16;
		}
		info->green.offset = 8;
		info->red.length = info->green.length = info->blue.length = 8;
		break;
	default:
		dev_err(dev, "color depth %d not supported\n",
					info->bits_per_pixel);
		return -EINVAL;
	}

//.........这里部分代码省略.........

/*
 * Set framebuffer parameters and change the operating mode.
 *
 * @param       info     framebuffer information pointer
 */
static int mxcfb_set_par(struct fb_info *fbi)
{
	int retval = 0;
	u32 mem_len;
	ipu_di_signal_cfg_t sig_cfg;
	struct mxcfb_info *mxc_fbi = (struct mxcfb_info *)fbi->par;
	uint32_t out_pixel_fmt;

	ipu_disable_channel(mxc_fbi->ipu_ch);
	ipu_uninit_channel(mxc_fbi->ipu_ch);
	mxcfb_set_fix(fbi);

	mem_len = fbi->var.yres_virtual * fbi->fix.line_length;
	if (!fbi->fix.smem_start || (mem_len > fbi->fix.smem_len)) {
		if (fbi->fix.smem_start)
			mxcfb_unmap_video_memory(fbi);

		if (mxcfb_map_video_memory(fbi) < 0)
			return -ENOMEM;
	}

	setup_disp_channel1(fbi);

	memset(&sig_cfg, 0, sizeof(sig_cfg));
	if (fbi->var.vmode & FB_VMODE_INTERLACED) {
		sig_cfg.interlaced = 1;
		out_pixel_fmt = IPU_PIX_FMT_YUV444;
	} else {
		if (mxc_fbi->ipu_di_pix_fmt)
			out_pixel_fmt = mxc_fbi->ipu_di_pix_fmt;
		else
			out_pixel_fmt = IPU_PIX_FMT_RGB666;
	}
	if (fbi->var.vmode & FB_VMODE_ODD_FLD_FIRST) /* PAL */
		sig_cfg.odd_field_first = 1;
	if ((fbi->var.sync & FB_SYNC_EXT) || ext_clk_used)
		sig_cfg.ext_clk = 1;
	if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
		sig_cfg.Hsync_pol = 1;
	if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT)
		sig_cfg.Vsync_pol = 1;
	if (!(fbi->var.sync & FB_SYNC_CLK_LAT_FALL))
		sig_cfg.clk_pol = 1;
	if (fbi->var.sync & FB_SYNC_DATA_INVERT)
		sig_cfg.data_pol = 1;
	if (!(fbi->var.sync & FB_SYNC_OE_LOW_ACT))
		sig_cfg.enable_pol = 1;
	if (fbi->var.sync & FB_SYNC_CLK_IDLE_EN)
		sig_cfg.clkidle_en = 1;

	debug("pixclock = %lu Hz\n", PICOS2KHZ(fbi->var.pixclock) * 1000UL);

	if (ipu_init_sync_panel(mxc_fbi->ipu_di,
				(PICOS2KHZ(fbi->var.pixclock)) * 1000UL,
				fbi->var.xres, fbi->var.yres,
				out_pixel_fmt,
				fbi->var.left_margin,
				fbi->var.hsync_len,
				fbi->var.right_margin,
				fbi->var.upper_margin,
				fbi->var.vsync_len,
				fbi->var.lower_margin,
				0, sig_cfg) != 0) {
		puts("mxcfb: Error initializing panel.\n");
		return -EINVAL;
	}

	retval = setup_disp_channel2(fbi);
	if (retval)
		return retval;

	if (mxc_fbi->blank == FB_BLANK_UNBLANK)
		ipu_enable_channel(mxc_fbi->ipu_ch);

	return retval;
}

void video_hw_init(void *lcdbase)
{
	int ret;
	unsigned int div = 0, best = 0, pix_clk;
	u32 frac1;
	const unsigned long lcd_clk = 480000000;
	u32 lcd_ctrl = LCD_CTRL_DEFAULT | LCDIF_CTRL_RUN;
	u32 lcd_ctrl1 = LCD_CTRL1_DEFAULT, lcd_ctrl2 = LCD_CTRL2_DEFAULT;
	u32 lcd_vdctrl0 = LCD_VDCTRL0_DEFAULT;
	u32 lcd_vdctrl1 = LCD_VDCTRL1_DEFAULT;
	u32 lcd_vdctrl2 = LCD_VDCTRL2_DEFAULT;
	u32 lcd_vdctrl3 = LCD_VDCTRL3_DEFAULT;
	u32 lcd_vdctrl4 = LCD_VDCTRL4_DEFAULT;
	struct mxs_clkctrl_regs *clk_regs = (void *)MXS_CLKCTRL_BASE;
	char buf1[16], buf2[16];

	/* pixel format in memory */
	switch (color_depth) {
	case 8:
		lcd_ctrl |= LCDIF_CTRL_WORD_LENGTH_8BIT;
		lcd_ctrl1 |= LCDIF_CTRL1_BYTE_PACKING_FORMAT(1);
		break;

	case 16:
		lcd_ctrl |= LCDIF_CTRL_WORD_LENGTH_16BIT;
		lcd_ctrl1 |= LCDIF_CTRL1_BYTE_PACKING_FORMAT(3);
		break;

	case 18:
		lcd_ctrl |= LCDIF_CTRL_WORD_LENGTH_18BIT;
		lcd_ctrl1 |= LCDIF_CTRL1_BYTE_PACKING_FORMAT(7);
		break;

	case 24:
		lcd_ctrl |= LCDIF_CTRL_WORD_LENGTH_24BIT;
		lcd_ctrl1 |= LCDIF_CTRL1_BYTE_PACKING_FORMAT(7);
		break;

	default:
		printf("Invalid bpp: %d\n", color_depth);
		return;
	}

	/* pixel format on the LCD data pins */
	switch (pix_fmt) {
	case PIX_FMT_RGB332:
		lcd_ctrl |= LCDIF_CTRL_LCD_DATABUS_WIDTH_8BIT;
		break;

	case PIX_FMT_RGB565:
		lcd_ctrl |= LCDIF_CTRL_LCD_DATABUS_WIDTH_16BIT;
		break;

	case PIX_FMT_BGR666:
		lcd_ctrl |= 1 << LCDIF_CTRL_INPUT_DATA_SWIZZLE_OFFSET;
		/* fallthru */
	case PIX_FMT_RGB666:
		lcd_ctrl |= LCDIF_CTRL_LCD_DATABUS_WIDTH_18BIT;
		break;

	case PIX_FMT_BGR24:
		lcd_ctrl |= 1 << LCDIF_CTRL_INPUT_DATA_SWIZZLE_OFFSET;
		/* fallthru */
	case PIX_FMT_RGB24:
		lcd_ctrl |= LCDIF_CTRL_LCD_DATABUS_WIDTH_24BIT;
		break;

	default:
		printf("Invalid pixel format: %c%c%c%c\n", fourcc_str(pix_fmt));
		return;
	}

	pix_clk = PICOS2KHZ(mxsfb_var.pixclock);
	debug("designated pix_clk: %sMHz\n", strmhz(buf1, pix_clk * 1000));

	for (frac1 = 18; frac1 < 36; frac1++) {
		static unsigned int err = ~0;
		unsigned long clk = lcd_clk / 1000 * 18 / frac1;
		unsigned int d = (clk + pix_clk - 1) / pix_clk;
		unsigned int diff = abs(clk / d - pix_clk);

		debug("frac1=%u div=%u lcd_clk=%-8sMHz pix_clk=%-8sMHz diff=%u err=%u\n",
			frac1, d, strmhz(buf1, clk * 1000), strmhz(buf2, clk * 1000 / d),
			diff, err);

		if (clk < pix_clk)
			break;
		if (d > 255)
			continue;

		if (diff < err) {
			best = frac1;
			div = d;
			err = diff;
			if (err == 0)
				break;
		}
	}
	if (div == 0) {
		printf("Requested pixel clock %sMHz out of range\n",
//.........这里部分代码省略.........

static int __set_par(struct fb_info *fbi, bool lock)
{
	u32 mem_len;
	struct ipu_di_signal_cfg sig_cfg;
	enum ipu_panel mode = IPU_PANEL_TFT;
	struct mx3fb_info *mx3_fbi = fbi->par;
	struct mx3fb_data *mx3fb = mx3_fbi->mx3fb;
	struct idmac_channel *ichan = mx3_fbi->idmac_channel;
	struct idmac_video_param *video = &ichan->params.video;
	struct scatterlist *sg = mx3_fbi->sg;

	/* Total cleanup */
	if (mx3_fbi->txd)
		sdc_disable_channel(mx3_fbi);

	mx3fb_set_fix(fbi);

	mem_len = fbi->var.yres_virtual * fbi->fix.line_length;
	if (mem_len > fbi->fix.smem_len) {
		if (fbi->fix.smem_start)
			mx3fb_unmap_video_memory(fbi);

		if (mx3fb_map_video_memory(fbi, mem_len, lock) < 0)
			return -ENOMEM;
	}

	sg_init_table(&sg[0], 1);
	sg_init_table(&sg[1], 1);

	sg_dma_address(&sg[0]) = fbi->fix.smem_start;
	sg_set_page(&sg[0], virt_to_page(fbi->screen_base),
		    fbi->fix.smem_len,
		    offset_in_page(fbi->screen_base));

	if (mx3_fbi->ipu_ch == IDMAC_SDC_0) {
		memset(&sig_cfg, 0, sizeof(sig_cfg));
		if (fbi->var.sync & FB_SYNC_HOR_HIGH_ACT)
			sig_cfg.Hsync_pol = true;
		if (fbi->var.sync & FB_SYNC_VERT_HIGH_ACT)
			sig_cfg.Vsync_pol = true;
		if (fbi->var.sync & FB_SYNC_CLK_INVERT)
			sig_cfg.clk_pol = true;
		if (fbi->var.sync & FB_SYNC_DATA_INVERT)
			sig_cfg.data_pol = true;
		if (fbi->var.sync & FB_SYNC_OE_ACT_HIGH)
			sig_cfg.enable_pol = true;
		if (fbi->var.sync & FB_SYNC_CLK_IDLE_EN)
			sig_cfg.clkidle_en = true;
		if (fbi->var.sync & FB_SYNC_CLK_SEL_EN)
			sig_cfg.clksel_en = true;
		if (fbi->var.sync & FB_SYNC_SHARP_MODE)
			mode = IPU_PANEL_SHARP_TFT;

		dev_dbg(fbi->device, "pixclock = %ul Hz\n",
			(u32) (PICOS2KHZ(fbi->var.pixclock) * 1000UL));

		if (sdc_init_panel(mx3fb, mode,
				   (PICOS2KHZ(fbi->var.pixclock)) * 1000UL,
				   fbi->var.xres, fbi->var.yres,
				   (fbi->var.sync & FB_SYNC_SWAP_RGB) ?
				   IPU_PIX_FMT_BGR666 : IPU_PIX_FMT_RGB666,
				   fbi->var.left_margin,
				   fbi->var.hsync_len,
				   fbi->var.right_margin +
				   fbi->var.hsync_len,
				   fbi->var.upper_margin,
				   fbi->var.vsync_len,
				   fbi->var.lower_margin +
				   fbi->var.vsync_len, sig_cfg) != 0) {
			dev_err(fbi->device,
				"mx3fb: Error initializing panel.\n");
			return -EINVAL;
		}
	}

	sdc_set_window_pos(mx3fb, mx3_fbi->ipu_ch, 0, 0);

	mx3_fbi->cur_ipu_buf	= 0;

	video->out_pixel_fmt	= bpp_to_pixfmt(fbi->var.bits_per_pixel);
	video->out_width	= fbi->var.xres;
	video->out_height	= fbi->var.yres;
	video->out_stride	= fbi->var.xres_virtual;

	if (mx3_fbi->blank == FB_BLANK_UNBLANK)
		sdc_enable_channel(mx3_fbi);

	return 0;
}

static int __init sh_hdmi_probe(struct platform_device *pdev)
{
	struct sh_mobile_hdmi_info *pdata = pdev->dev.platform_data;
	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	int irq = platform_get_irq(pdev, 0), ret;
	struct sh_hdmi *hdmi;
	long rate;

	if (!res || !pdata || irq < 0)
		return -ENODEV;

	hdmi = kzalloc(sizeof(*hdmi), GFP_KERNEL);
	if (!hdmi) {
		dev_err(&pdev->dev, "Cannot allocate device data\n");
		return -ENOMEM;
	}

	hdmi->dev = &pdev->dev;

	hdmi->hdmi_clk = clk_get(&pdev->dev, "ick");
	if (IS_ERR(hdmi->hdmi_clk)) {
		ret = PTR_ERR(hdmi->hdmi_clk);
		dev_err(&pdev->dev, "Unable to get clock: %d\n", ret);
		goto egetclk;
	}

	rate = PICOS2KHZ(pdata->lcd_chan->lcd_cfg.pixclock) * 1000;

	rate = clk_round_rate(hdmi->hdmi_clk, rate);
	if (rate < 0) {
		ret = rate;
		dev_err(&pdev->dev, "Cannot get suitable rate: %ld\n", rate);
		goto erate;
	}

	ret = clk_set_rate(hdmi->hdmi_clk, rate);
	if (ret < 0) {
		dev_err(&pdev->dev, "Cannot set rate %ld: %d\n", rate, ret);
		goto erate;
	}

	pr_debug("HDMI set frequency %lu\n", rate);

	ret = clk_enable(hdmi->hdmi_clk);
	if (ret < 0) {
		dev_err(&pdev->dev, "Cannot enable clock: %d\n", ret);
		goto eclkenable;
	}

	dev_info(&pdev->dev, "Enabled HDMI clock at %luHz\n", rate);

	if (!request_mem_region(res->start, resource_size(res), dev_name(&pdev->dev))) {
		dev_err(&pdev->dev, "HDMI register region already claimed\n");
		ret = -EBUSY;
		goto ereqreg;
	}

	hdmi->base = ioremap(res->start, resource_size(res));
	if (!hdmi->base) {
		dev_err(&pdev->dev, "HDMI register region already claimed\n");
		ret = -ENOMEM;
		goto emap;
	}

	platform_set_drvdata(pdev, hdmi);

	/* Product and revision IDs are 0 in sh-mobile version */
	dev_info(&pdev->dev, "Detected HDMI controller 0x%x:0x%x\n",
		 hdmi_read(hdmi, HDMI_PRODUCT_ID), hdmi_read(hdmi, HDMI_REVISION_ID));

	/* Set up LCDC callbacks */
	pdata->lcd_chan->board_cfg.board_data = hdmi;
	pdata->lcd_chan->board_cfg.display_on = hdmi_display_on;
	pdata->lcd_chan->board_cfg.display_off = hdmi_display_off;

	INIT_DELAYED_WORK(&hdmi->edid_work, edid_work_fn);

	pm_runtime_enable(&pdev->dev);
	pm_runtime_resume(&pdev->dev);

	ret = request_irq(irq, sh_hdmi_hotplug, 0,
			  dev_name(&pdev->dev), hdmi);
	if (ret < 0) {
		dev_err(&pdev->dev, "Unable to request irq: %d\n", ret);
		goto ereqirq;
	}

	return 0;

ereqirq:
	pm_runtime_disable(&pdev->dev);
	iounmap(hdmi->base);
emap:
	release_mem_region(res->start, resource_size(res));
ereqreg:
	clk_disable(hdmi->hdmi_clk);
eclkenable:
erate:
	clk_put(hdmi->hdmi_clk);
egetclk:
//.........这里部分代码省略.........

/**
 *      atmel_lcdfb_check_var - Validates a var passed in.
 *      @var: frame buffer variable screen structure
 *      @info: frame buffer structure that represents a single frame buffer
 *
 *	Checks to see if the hardware supports the state requested by
 *	var passed in. This function does not alter the hardware
 *	state!!!  This means the data stored in struct fb_info and
 *	struct atmel_lcdfb_info do not change. This includes the var
 *	inside of struct fb_info.  Do NOT change these. This function
 *	can be called on its own if we intent to only test a mode and
 *	not actually set it. The stuff in modedb.c is a example of
 *	this. If the var passed in is slightly off by what the
 *	hardware can support then we alter the var PASSED in to what
 *	we can do. If the hardware doesn't support mode change a
 *	-EINVAL will be returned by the upper layers. You don't need
 *	to implement this function then. If you hardware doesn't
 *	support changing the resolution then this function is not
 *	needed. In this case the driver would just provide a var that
 *	represents the static state the screen is in.
 *
 *	Returns negative errno on error, or zero on success.
 */
static int atmel_lcdfb_check_var(struct fb_var_screeninfo *var,
			     struct fb_info *info)
{
	struct device *dev = info->device;
	struct atmel_lcdfb_info *sinfo = info->par;
	unsigned long clk_value_khz;

	clk_value_khz = clk_get_rate(sinfo->lcdc_clk) / 1000;

	dev_dbg(dev, "%s:\n", __func__);
	dev_dbg(dev, "  resolution: %ux%u\n", var->xres, var->yres);
	dev_dbg(dev, "  pixclk:     %lu KHz\n", PICOS2KHZ(var->pixclock));
	dev_dbg(dev, "  bpp:        %u\n", var->bits_per_pixel);
	dev_dbg(dev, "  clk:        %lu KHz\n", clk_value_khz);

	if ((PICOS2KHZ(var->pixclock) * var->bits_per_pixel / 8) > clk_value_khz) {
		dev_err(dev, "%lu KHz pixel clock is too fast\n", PICOS2KHZ(var->pixclock));
		return -EINVAL;
	}

	/* Force same alignment for each line */
	var->xres = (var->xres + 3) & ~3UL;
	var->xres_virtual = (var->xres_virtual + 3) & ~3UL;

	var->red.msb_right = var->green.msb_right = var->blue.msb_right = 0;
	var->transp.msb_right = 0;
	var->transp.offset = var->transp.length = 0;
	var->xoffset = var->yoffset = 0;

	/* Saturate vertical and horizontal timings at max