Merge master.kernel.org:/pub/scm/linux/kernel/git/lethal/genesis-2.6 into devel-stable

Conflicts:
	drivers/video/sh_mobile_hdmi.c

drivers/video/sh_mobile_hdmi.c

@@ -28,6 +28,8 @@
 #include <video/sh_mobile_hdmi.h>
 #include <video/sh_mobile_lcdc.h>
 
+#include "sh_mobile_lcdcfb.h"
+
 #define HDMI_SYSTEM_CTRL			0x00 /* System control */
 #define HDMI_L_R_DATA_SWAP_CTRL_RPKT		0x01 /* L/R data swap control,
 							bits 19..16 of 20-bit N for Audio Clock Regeneration packet */
@@ -206,12 +208,15 @@ enum hotplug_state {
 
 struct sh_hdmi {
 	void __iomem *base;
-	enum hotplug_state hp_state;
+	enum hotplug_state hp_state;	/* hot-plug status */
+	bool preprogrammed_mode;	/* use a pre-programmed VIC or the external mode */
 	struct clk *hdmi_clk;
 	struct device *dev;
 	struct fb_info *info;
+	struct mutex mutex;		/* Protect the info pointer */
 	struct delayed_work edid_work;
 	struct fb_var_screeninfo var;
+	struct fb_monspecs monspec;
 };
 
 static void hdmi_write(struct sh_hdmi *hdmi, u8 data, u8 reg)
@@ -277,7 +282,7 @@ static struct snd_soc_codec_driver soc_codec_dev_sh_hdmi = {
  */
 
 /* External video parameter settings */
-static void hdmi_external_video_param(struct sh_hdmi *hdmi)
+static void sh_hdmi_external_video_param(struct sh_hdmi *hdmi)
 {
 	struct fb_var_screeninfo *var = &hdmi->var;
 	u16 htotal, hblank, hdelay, vtotal, vblank, vdelay, voffset;
@@ -309,9 +314,9 @@ static void hdmi_external_video_param(struct sh_hdmi *hdmi)
 	if (var->sync & FB_SYNC_VERT_HIGH_ACT)
 		sync |= 8;
 
-	pr_debug("H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
-		 htotal, hblank, hdelay, var->hsync_len,
-		 vtotal, vblank, vdelay, var->vsync_len, sync);
+	dev_dbg(hdmi->dev, "H: %u, %u, %u, %u; V: %u, %u, %u, %u; sync 0x%x\n",
+		htotal, hblank, hdelay, var->hsync_len,
+		vtotal, vblank, vdelay, var->vsync_len, sync);
 
 	hdmi_write(hdmi, sync | (voffset << 4), HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
 
@@ -336,7 +341,10 @@ static void hdmi_external_video_param(struct sh_hdmi *hdmi)
 
 	hdmi_write(hdmi, var->vsync_len, HDMI_EXTERNAL_V_DURATION);
 
-	/* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for manual mode */
+	/* Set bit 0 of HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS here for external mode */
+	if (!hdmi->preprogrammed_mode)
+		hdmi_write(hdmi, sync | 1 | (voffset << 4),
+			   HDMI_EXTERNAL_VIDEO_PARAM_SETTINGS);
 }
 
 /**
@@ -454,21 +462,61 @@ static void sh_hdmi_audio_config(struct sh_hdmi *hdmi)
 }
 
 /**
- * sh_hdmi_phy_config()
+ * sh_hdmi_phy_config() - configure the HDMI PHY for the used video mode
  */
 static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
 {
-	/* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
-	hdmi_write(hdmi, 0x19, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
-	hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
-	hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
-	/* PLLA_CONFIG[7:0]: VCO gain, VCO offset, LPF resistance[0] */
-	hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
-	hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
-	hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
-	hdmi_write(hdmi, 0x0E, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
-	hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
-	hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+	if (hdmi->var.yres > 480) {
+		/* 720p, 8bit, 74.25MHz. Might need to be adjusted for other formats */
+		/*
+		 * [1:0]	Speed_A
+		 * [3:2]	Speed_B
+		 * [4]		PLLA_Bypass
+		 * [6]		DRV_TEST_EN
+		 * [7]		DRV_TEST_IN
+		 */
+		hdmi_write(hdmi, 0x0f, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+		/* PLLB_CONFIG[17], PLLA_CONFIG[17] - not in PHY datasheet */
+		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+		/*
+		 * [2:0]	BGR_I_OFFSET
+		 * [6:4]	BGR_V_OFFSET
+		 */
+		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
+		/* PLLA_CONFIG[7:0]: VCO gain, VCO offset, LPF resistance[0] */
+		hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+		/*
+		 * PLLA_CONFIG[15:8]: regulator voltage[0], CP current,
+		 * LPF capacitance, LPF resistance[1]
+		 */
+		hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+		/* PLLB_CONFIG[7:0]: LPF resistance[0], VCO offset, VCO gain */
+		hdmi_write(hdmi, 0x4A, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+		/*
+		 * PLLB_CONFIG[15:8]: regulator voltage[0], CP current,
+		 * LPF capacitance, LPF resistance[1]
+		 */
+		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+		/* DRV_CONFIG, PE_CONFIG */
+		hdmi_write(hdmi, 0x25, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+		/*
+		 * [2:0]	AMON_SEL (4 == LPF voltage)
+		 * [4]		PLLA_CONFIG[16]
+		 * [5]		PLLB_CONFIG[16]
+		 */
+		hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+	} else {
+		/* for 480p8bit 27MHz */
+		hdmi_write(hdmi, 0x19, HDMI_SLIPHDMIT_PARAM_SETTINGS_1);
+		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_2);
+		hdmi_write(hdmi, 0x00, HDMI_SLIPHDMIT_PARAM_SETTINGS_3);
+		hdmi_write(hdmi, 0x44, HDMI_SLIPHDMIT_PARAM_SETTINGS_5);
+		hdmi_write(hdmi, 0x32, HDMI_SLIPHDMIT_PARAM_SETTINGS_6);
+		hdmi_write(hdmi, 0x48, HDMI_SLIPHDMIT_PARAM_SETTINGS_7);
+		hdmi_write(hdmi, 0x0F, HDMI_SLIPHDMIT_PARAM_SETTINGS_8);
+		hdmi_write(hdmi, 0x20, HDMI_SLIPHDMIT_PARAM_SETTINGS_9);
+		hdmi_write(hdmi, 0x04, HDMI_SLIPHDMIT_PARAM_SETTINGS_10);
+	}
 }
 
 /**
@@ -476,6 +524,8 @@ static void sh_hdmi_phy_config(struct sh_hdmi *hdmi)
  */
 static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
 {
+	u8 vic;
+
 	/* AVI InfoFrame */
 	hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_INDEX);
 
@@ -500,9 +550,9 @@ static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
 	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB1);
 
 	/*
-	 * C = No Data
-	 * M = 16:9 Picture Aspect Ratio
-	 * R = Same as picture aspect ratio
+	 * [7:6] C = Colorimetry: no data
+	 * [5:4] M = 2: 16:9, 1: 4:3 Picture Aspect Ratio
+	 * [3:0] R = 8: Active Frame Aspect Ratio: same as picture aspect ratio
 	 */
 	hdmi_write(hdmi, 0x28, HDMI_CTRL_PKT_BUF_ACCESS_PB2);
 
@@ -516,9 +566,15 @@ static void sh_hdmi_avi_infoframe_setup(struct sh_hdmi *hdmi)
 
 	/*
 	 * VIC = 1280 x 720p: ignored if external config is used
-	 * Send 2 for 720 x 480p, 16 for 1080p
+	 * Send 2 for 720 x 480p, 16 for 1080p, ignored in external mode
 	 */
-	hdmi_write(hdmi, 4, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
+	if (hdmi->var.yres == 1080 && hdmi->var.xres == 1920)
+		vic = 16;
+	else if (hdmi->var.yres == 480 && hdmi->var.xres == 720)
+		vic = 2;
+	else
+		vic = 4;
+	hdmi_write(hdmi, vic, HDMI_CTRL_PKT_BUF_ACCESS_PB4);
 
 	/* PR = No Repetition */
 	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB5);
@@ -591,100 +647,6 @@ static void sh_hdmi_audio_infoframe_setup(struct sh_hdmi *hdmi)
 	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB10);
 }
 
-/**
- * sh_hdmi_gamut_metadata_setup() - Gamut Metadata Packet of CONTROL PACKET
- */
-static void sh_hdmi_gamut_metadata_setup(struct sh_hdmi *hdmi)
-{
-	int i;
-
-	/* Gamut Metadata Packet */
-	hdmi_write(hdmi, 0x04, HDMI_CTRL_PKT_BUF_INDEX);
-
-	/* Packet Type = 0x0A */
-	hdmi_write(hdmi, 0x0A, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-	/* Gamut Packet is not used, so default value */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-	/* Gamut Packet is not used, so default value */
-	hdmi_write(hdmi, 0x10, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-	/* GBD bytes 0 through 27 */
-	for (i = 0; i <= 27; i++)
-		/* HDMI_CTRL_PKT_BUF_ACCESS_PB0_63H - PB27_7EH */
-		hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
- * sh_hdmi_acp_setup() - Audio Content Protection Packet (ACP)
- */
-static void sh_hdmi_acp_setup(struct sh_hdmi *hdmi)
-{
-	int i;
-
-	/* Audio Content Protection Packet (ACP) */
-	hdmi_write(hdmi, 0x01, HDMI_CTRL_PKT_BUF_INDEX);
-
-	/* Packet Type = 0x04 */
-	hdmi_write(hdmi, 0x04, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-	/* ACP_Type */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-	/* Reserved (0) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-	/* GBD bytes 0 through 27 */
-	for (i = 0; i <= 27; i++)
-		/* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
-		hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
- * sh_hdmi_isrc1_setup() - ISRC1 Packet
- */
-static void sh_hdmi_isrc1_setup(struct sh_hdmi *hdmi)
-{
-	int i;
-
-	/* ISRC1 Packet */
-	hdmi_write(hdmi, 0x02, HDMI_CTRL_PKT_BUF_INDEX);
-
-	/* Packet Type = 0x05 */
-	hdmi_write(hdmi, 0x05, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-	/* ISRC_Cont, ISRC_Valid, Reserved (0), ISRC_Status */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-	/* Reserved (0) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-	/* PB0 UPC_EAN_ISRC_0-15 */
-	/* Bytes PB16-PB27 shall be set to a value of 0. */
-	for (i = 0; i <= 27; i++)
-		/* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
-		hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
-/**
- * sh_hdmi_isrc2_setup() - ISRC2 Packet
- */
-static void sh_hdmi_isrc2_setup(struct sh_hdmi *hdmi)
-{
-	int i;
-
-	/* ISRC2 Packet */
-	hdmi_write(hdmi, 0x03, HDMI_CTRL_PKT_BUF_INDEX);
-
-	/* HB0 Packet Type = 0x06 */
-	hdmi_write(hdmi, 0x06, HDMI_CTRL_PKT_BUF_ACCESS_HB0);
-	/* Reserved (0) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB1);
-	/* Reserved (0) */
-	hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_HB2);
-
-	/* PB0 UPC_EAN_ISRC_16-31 */
-	/* Bytes PB16-PB27 shall be set to a value of 0. */
-	for (i = 0; i <= 27; i++)
-		/* HDMI_CTRL_PKT_BUF_ACCESS_PB0 - PB27 */
-		hdmi_write(hdmi, 0x00, HDMI_CTRL_PKT_BUF_ACCESS_PB0 + i);
-}
-
 /**
  * sh_hdmi_configure() - Initialise HDMI for output
  */
@@ -705,18 +667,6 @@ static void sh_hdmi_configure(struct sh_hdmi *hdmi)
 	/* Audio InfoFrame */
 	sh_hdmi_audio_infoframe_setup(hdmi);
 
-	/* Gamut Metadata packet */
-	sh_hdmi_gamut_metadata_setup(hdmi);
-
-	/* Audio Content Protection (ACP) Packet */
-	sh_hdmi_acp_setup(hdmi);
-
-	/* ISRC1 Packet */
-	sh_hdmi_isrc1_setup(hdmi);
-
-	/* ISRC2 Packet */
-	sh_hdmi_isrc2_setup(hdmi);
-
 	/*
 	 * Control packet auto send with VSYNC control: auto send
 	 * General control, Gamut metadata, ISRC, and ACP packets
@@ -734,17 +684,42 @@ static void sh_hdmi_configure(struct sh_hdmi *hdmi)
 	hdmi_write(hdmi, 0x40, HDMI_SYSTEM_CTRL);
 }
 
-static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
+static unsigned long sh_hdmi_rate_error(struct sh_hdmi *hdmi,
+					const struct fb_videomode *mode)
 {
-	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;
+	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, "\t@%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;
+}
+
+static int sh_hdmi_read_edid(struct sh_hdmi *hdmi)
+{
+	struct fb_var_screeninfo tmpvar;
+	struct fb_var_screeninfo *var = &tmpvar;
+	const struct fb_videomode *mode, *found = NULL;
+	struct fb_info *info = hdmi->info;
+	struct fb_modelist *modelist = NULL;
+	unsigned int f_width = 0, f_height = 0, f_refresh = 0;
+	unsigned long found_rate_error = ULONG_MAX; /* silly compiler... */
+	bool exact_match = false;
 	u8 edid[128];
+	char *forced;
+	int i;
 
 	/* Read EDID */
-	pr_debug("Read back EDID code:");
+	dev_dbg(hdmi->dev, "Read back EDID code:");
 	for (i = 0; i < 128; i++) {
 		edid[i] = hdmi_read(hdmi, HDMI_EDID_KSV_FIFO_ACCESS_WINDOW);
 #ifdef DEBUG
@@ -759,29 +734,97 @@ static void sh_hdmi_read_edid(struct sh_hdmi *hdmi)
 #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));
 
-	/* FIXME: Use user-provided configuration instead of EDID */
-	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;
+	fb_edid_to_monspecs(edid, &hdmi->monspec);
 
-	hdmi_external_video_param(hdmi);
+	fb_get_options("sh_mobile_lcdc", &forced);
+	if (forced && *forced) {
+		/* Only primitive parsing so far */
+		i = sscanf(forced, "%ux%u@%u",
+			   &f_width, &f_height, &f_refresh);
+		if (i < 2) {
+			f_width = 0;
+			f_height = 0;
+		}
+		dev_dbg(hdmi->dev, "Forced mode %ux%u@%uHz\n",
+			f_width, f_height, f_refresh);
+	}
+
+	/* Walk monitor modes to find the best or the exact match */
+	for (i = 0, mode = hdmi->monspec.modedb;
+	     f_width && f_height && i < hdmi->monspec.modedb_len && !exact_match;
+	     i++, mode++) {
+		unsigned long rate_error = sh_hdmi_rate_error(hdmi, mode);
+
+		/* No interest in unmatching modes */
+		if (f_width != mode->xres || f_height != mode->yres)
+			continue;
+		if (f_refresh == mode->refresh || (!f_refresh && !rate_error))
+			/*
+			 * Exact match if either the refresh rate matches or it
+			 * hasn't been specified and we've found a mode, for
+			 * which we can configure the clock precisely
+			 */
+			exact_match = true;
+		else if (found && found_rate_error <= rate_error)
+			/*
+			 * We otherwise search for the closest matching clock
+			 * rate - either if no refresh rate has been specified
+			 * or we cannot find an exactly matching one
+			 */
+			continue;
+
+		/* Check if supported: sufficient fb memory, supported clock-rate */
+		fb_videomode_to_var(var, mode);
+
+		if (info && info->fbops->fb_check_var &&
+		    info->fbops->fb_check_var(var, info)) {
+			exact_match = false;
+			continue;
+		}
+
+		found = mode;
+		found_rate_error = rate_error;
+	}
+
+	/*
+	 * TODO 1: if no ->info is present, postpone running the config until
+	 * after ->info first gets registered.
+	 * TODO 2: consider registering the HDMI platform device from the LCDC
+	 * driver, and passing ->info with HDMI platform data.
+	 */
+	if (info && !found) {
+		modelist = hdmi->info->modelist.next &&
+			!list_empty(&hdmi->info->modelist) ?
+			list_entry(hdmi->info->modelist.next,
+				   struct fb_modelist, list) :
+			NULL;
+
+		if (modelist) {
+			found = &modelist->mode;
+			found_rate_error = sh_hdmi_rate_error(hdmi, found);
+		}
+	}
+
+	/* No cookie today */
+	if (!found)
+		return -ENXIO;
+
+	dev_info(hdmi->dev, "Using %s mode %ux%u@%uHz (%luHz), clock error %luHz\n",
+		 modelist ? "default" : "EDID", found->xres, found->yres,
+		 found->refresh, PICOS2KHZ(found->pixclock) * 1000, found_rate_error);
+
+	if ((found->xres == 720 && found->yres == 480) ||
+	    (found->xres == 1280 && found->yres == 720) ||
+	    (found->xres == 1920 && found->yres == 1080))
+		hdmi->preprogrammed_mode = true;
+	else
+		hdmi->preprogrammed_mode = false;
+
+	fb_videomode_to_var(&hdmi->var, found);
+	sh_hdmi_external_video_param(hdmi);
+
+	return 0;
 }
 
 static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
@@ -809,8 +852,8 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
 	hdmi_write(hdmi, 0xFF, HDMI_INTERRUPT_STATUS_2);
 
 	if (printk_ratelimit())
-		pr_debug("IRQ #%d: Status #1: 0x%x & 0x%x, #2: 0x%x & 0x%x\n",
-			 irq, status1, mask1, status2, mask2);
+		dev_dbg(hdmi->dev, "IRQ #%d: Status #1: 0x%x & 0x%x, #2: 0x%x & 0x%x\n",
+			irq, status1, mask1, status2, mask2);
 
 	if (!((status1 & mask1) | (status2 & mask2))) {
 		return IRQ_NONE;
@@ -821,7 +864,7 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
 		udelay(500);
 
 		msens = hdmi_read(hdmi, HDMI_HOT_PLUG_MSENS_STATUS);
-		pr_debug("MSENS 0x%x\n", msens);
+		dev_dbg(hdmi->dev, "MSENS 0x%x\n", msens);
 		/* Check, if hot plug & MSENS pin status are both high */
 		if ((msens & 0xC0) == 0xC0) {
 			/* Display plug in */
@@ -857,83 +900,176 @@ static irqreturn_t sh_hdmi_hotplug(int irq, void *dev_id)
 	return IRQ_HANDLED;
 }
 
-static void hdmi_display_on(void *arg, struct fb_info *info)
+/* locking:	called with info->lock held, or before register_framebuffer() */
+static void sh_hdmi_display_on(void *arg, struct fb_info *info)
 {
+	/*
+	 * info is guaranteed to be valid, when we are called, because our
+	 * FB_EVENT_FB_UNBIND notify is also called with info->lock held
+	 */
 	struct sh_hdmi *hdmi = arg;
 	struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+	struct sh_mobile_lcdc_chan *ch = info->par;
 
-	if (info->var.xres != 1280 || info->var.yres != 720) {
-		dev_warn(info->device, "Unsupported framebuffer geometry %ux%u\n",
-			 info->var.xres, info->var.yres);
-		return;
-	}
+	dev_dbg(hdmi->dev, "%s(%p): state %x\n", __func__,
+		pdata->lcd_dev, info->state);
 
-	pr_debug("%s(%p): state %x\n", __func__, pdata->lcd_dev, info->state);
-	/*
-	 * FIXME: not a good place to store fb_info. And we cannot nullify it
-	 * even on monitor disconnect. What should the lifecycle be?
-	 */
+	/* No need to lock */
 	hdmi->info = info;
+
+	/*
+	 * hp_state can be set to
+	 * HDMI_HOTPLUG_DISCONNECTED:	on monitor unplug
+	 * HDMI_HOTPLUG_CONNECTED:	on monitor plug-in
+	 * HDMI_HOTPLUG_EDID_DONE:	on EDID read completion
+	 */
 	switch (hdmi->hp_state) {
 	case HDMI_HOTPLUG_EDID_DONE:
 		/* PS mode d->e. All functions are active */
 		hdmi_write(hdmi, 0x80, HDMI_SYSTEM_CTRL);
-		pr_debug("HDMI running\n");
+		dev_dbg(hdmi->dev, "HDMI running\n");
 		break;
 	case HDMI_HOTPLUG_DISCONNECTED:
 		info->state = FBINFO_STATE_SUSPENDED;
 	default:
-		hdmi->var = info->var;
+		hdmi->var = ch->display_var;
 	}
 }
 
-static void hdmi_display_off(void *arg)
+/* locking: called with info->lock held */
+static void sh_hdmi_display_off(void *arg)
 {
 	struct sh_hdmi *hdmi = arg;
 	struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
 
-	pr_debug("%s(%p)\n", __func__, pdata->lcd_dev);
+	dev_dbg(hdmi->dev, "%s(%p)\n", __func__, pdata->lcd_dev);
 	/* PS mode e->a */
 	hdmi_write(hdmi, 0x10, HDMI_SYSTEM_CTRL);
 }
 
+static bool sh_hdmi_must_reconfigure(struct sh_hdmi *hdmi)
+{
+	struct fb_info *info = hdmi->info;
+	struct sh_mobile_lcdc_chan *ch = info->par;
+	struct fb_var_screeninfo *new_var = &hdmi->var, *old_var = &ch->display_var;
+	struct fb_videomode mode1, mode2;
+
+	fb_var_to_videomode(&mode1, old_var);
+	fb_var_to_videomode(&mode2, new_var);
+
+	dev_dbg(info->dev, "Old %ux%u, new %ux%u\n",
+		mode1.xres, mode1.yres, mode2.xres, mode2.yres);
+
+	if (fb_mode_is_equal(&mode1, &mode2))
+		return false;
+
+	dev_dbg(info->dev, "Switching %u -> %u lines\n",
+		mode1.yres, mode2.yres);
+	*old_var = *new_var;
+
+	return true;
+}
+
+/**
+ * sh_hdmi_clk_configure() - set HDMI clock frequency and enable the clock
+ * @hdmi:	driver context
+ * @pixclock:	pixel clock period in picoseconds
+ * return:	configured positive rate if successful
+ *		0 if couldn't set the rate, but managed to enable the clock
+ *		negative error, if couldn't enable the clock
+ */
+static long sh_hdmi_clk_configure(struct sh_hdmi *hdmi, unsigned long pixclock)
+{
+	long rate;
+	int ret;
+
+	rate = PICOS2KHZ(pixclock) * 1000;
+	rate = clk_round_rate(hdmi->hdmi_clk, rate);
+	if (rate > 0) {
+		ret = clk_set_rate(hdmi->hdmi_clk, rate);
+		if (ret < 0) {
+			dev_warn(hdmi->dev, "Cannot set rate %ld: %d\n", rate, ret);
+			rate = 0;
+		} else {
+			dev_dbg(hdmi->dev, "HDMI set frequency %lu\n", rate);
+		}
+	} else {
+		rate = 0;
+		dev_warn(hdmi->dev, "Cannot get suitable rate: %ld\n", rate);
+	}
+
+	ret = clk_enable(hdmi->hdmi_clk);
+	if (ret < 0) {
+		dev_err(hdmi->dev, "Cannot enable clock: %d\n", ret);
+		return ret;
+	}
+
+	return rate;
+}
+
 /* Hotplug interrupt occurred, read EDID */
-static void edid_work_fn(struct work_struct *work)
+static void sh_hdmi_edid_work_fn(struct work_struct *work)
 {
 	struct sh_hdmi *hdmi = container_of(work, struct sh_hdmi, edid_work.work);
 	struct sh_mobile_hdmi_info *pdata = hdmi->dev->platform_data;
+	struct sh_mobile_lcdc_chan *ch;
+	int ret;
 
-	pr_debug("%s(%p): begin, hotplug status %d\n", __func__,
-		 pdata->lcd_dev, hdmi->hp_state);
+	dev_dbg(hdmi->dev, "%s(%p): begin, hotplug status %d\n", __func__,
+		pdata->lcd_dev, hdmi->hp_state);
 
 	if (!pdata->lcd_dev)
 		return;
 
+	mutex_lock(&hdmi->mutex);
+
 	if (hdmi->hp_state == HDMI_HOTPLUG_EDID_DONE) {
-		pm_runtime_get_sync(hdmi->dev);
 		/* A device has been plugged in */
-		sh_hdmi_read_edid(hdmi);
+		pm_runtime_get_sync(hdmi->dev);
+
+		ret = sh_hdmi_read_edid(hdmi);
+		if (ret < 0)
+			goto out;
+
+		/* Reconfigure the clock */
+		clk_disable(hdmi->hdmi_clk);
+		ret = sh_hdmi_clk_configure(hdmi, hdmi->var.pixclock);
+		if (ret < 0)
+			goto out;
+
 		msleep(10);
 		sh_hdmi_configure(hdmi);
 		/* Switched to another (d) power-save mode */
 		msleep(10);
 
 		if (!hdmi->info)
-			return;
+			goto out;
+
+		ch = hdmi->info->par;
 
 		acquire_console_sem();
 
 		/* HDMI plug in */
-		hdmi->info->var = hdmi->var;
-		if (hdmi->info->state != FBINFO_STATE_RUNNING)
+		if (!sh_hdmi_must_reconfigure(hdmi) &&
+		    hdmi->info->state == FBINFO_STATE_RUNNING) {
+			/*
+			 * First activation with the default monitor - just turn
+			 * on, if we run a resume here, the logo disappears
+			 */
+			if (lock_fb_info(hdmi->info)) {
+				sh_hdmi_display_on(hdmi, hdmi->info);
+				unlock_fb_info(hdmi->info);
+			}
+		} else {
+			/* New monitor or have to wake up */
 			fb_set_suspend(hdmi->info, 0);
-		else
-			hdmi_display_on(hdmi, hdmi->info);
+		}
 
 		release_console_sem();
 	} else {
+		ret = 0;
 		if (!hdmi->info)
-			return;
+			goto out;
 
 		acquire_console_sem();
 
@@ -942,15 +1078,67 @@ static void edid_work_fn(struct work_struct *work)
 
 		release_console_sem();
 		pm_runtime_put(hdmi->dev);
+		fb_destroy_modedb(hdmi->monspec.modedb);
 	}
 
-	pr_debug("%s(%p): end\n", __func__, pdata->lcd_dev);
+out:
+	if (ret < 0)
+		hdmi->hp_state = HDMI_HOTPLUG_DISCONNECTED;
+	mutex_unlock(&hdmi->mutex);
+
+	dev_dbg(hdmi->dev, "%s(%p): end\n", __func__, pdata->lcd_dev);
+}
+
+static int sh_hdmi_notify(struct notifier_block *nb,
+			  unsigned long action, void *data);
+
+static struct notifier_block sh_hdmi_notifier = {
+	.notifier_call = sh_hdmi_notify,
+};
+
+static int sh_hdmi_notify(struct notifier_block *nb,
+			  unsigned long action, void *data)
+{
+	struct fb_event *event = data;
+	struct fb_info *info = event->info;
+	struct sh_mobile_lcdc_chan *ch = info->par;
+	struct sh_mobile_lcdc_board_cfg	*board_cfg = &ch->cfg.board_cfg;
+	struct sh_hdmi *hdmi = board_cfg->board_data;
+
+	if (nb != &sh_hdmi_notifier || !hdmi || hdmi->info != info)
+		return NOTIFY_DONE;
+
+	switch(action) {
+	case FB_EVENT_FB_REGISTERED:
+		/* Unneeded, activation taken care by sh_hdmi_display_on() */
+		break;
+	case FB_EVENT_FB_UNREGISTERED:
+		/*
+		 * We are called from unregister_framebuffer() with the
+		 * info->lock held. This is bad for us, because we can race with
+		 * the scheduled work, which has to call fb_set_suspend(), which
+		 * takes info->lock internally, so, sh_hdmi_edid_work_fn()
+		 * cannot take and hold info->lock for the whole function
+		 * duration. Using an additional lock creates a classical AB-BA
+		 * lock up. Therefore, we have to release the info->lock
+		 * temporarily, synchronise with the work queue and re-acquire
+		 * the info->lock.
+		 */
+		unlock_fb_info(hdmi->info);
+		mutex_lock(&hdmi->mutex);
+		hdmi->info = NULL;
+		mutex_unlock(&hdmi->mutex);
+		lock_fb_info(hdmi->info);
+		return NOTIFY_OK;
+	}
+	return NOTIFY_DONE;
 }
 
 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);
+	struct sh_mobile_lcdc_board_cfg	*board_cfg;
 	int irq = platform_get_irq(pdev, 0), ret;
 	struct sh_hdmi *hdmi;
 	long rate;
@@ -964,10 +1152,7 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 		return -ENOMEM;
 	}
 
-	ret =  snd_soc_register_codec(&pdev->dev,
-			&soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
-	if (ret < 0)
-		goto esndreg;
+	mutex_init(&hdmi->mutex);
 
 	hdmi->dev = &pdev->dev;
 
@@ -978,30 +1163,14 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 		goto egetclk;
 	}
 
-	rate = PICOS2KHZ(pdata->lcd_chan->lcd_cfg.pixclock) * 1000;
-
-	rate = clk_round_rate(hdmi->hdmi_clk, rate);
+	/* Some arbitrary relaxed pixclock just to get things started */
+	rate = sh_hdmi_clk_configure(hdmi, 37037);
 	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);
+	dev_dbg(&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");
@@ -1018,18 +1187,18 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 
 	platform_set_drvdata(pdev, hdmi);
 
-#if 1
 	/* 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));
-#endif
 
 	/* 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;
+	board_cfg = &pdata->lcd_chan->board_cfg;
+	board_cfg->owner = THIS_MODULE;
+	board_cfg->board_data = hdmi;
+	board_cfg->display_on = sh_hdmi_display_on;
+	board_cfg->display_off = sh_hdmi_display_off;
 
-	INIT_DELAYED_WORK(&hdmi->edid_work, edid_work_fn);
+	INIT_DELAYED_WORK(&hdmi->edid_work, sh_hdmi_edid_work_fn);
 
 	pm_runtime_enable(&pdev->dev);
 	pm_runtime_resume(&pdev->dev);
@@ -1041,8 +1210,17 @@ static int __init sh_hdmi_probe(struct platform_device *pdev)
 		goto ereqirq;
 	}
 
+	ret = snd_soc_register_codec(&pdev->dev,
+			&soc_codec_dev_sh_hdmi, &sh_hdmi_dai, 1);
+	if (ret < 0) {
+		dev_err(&pdev->dev, "codec registration failed\n");
+		goto ecodec;
+	}
+
 	return 0;
 
+ecodec:
+	free_irq(irq, hdmi);
 ereqirq:
 	pm_runtime_disable(&pdev->dev);
 	iounmap(hdmi->base);
@@ -1050,12 +1228,10 @@ emap:
 	release_mem_region(res->start, resource_size(res));
 ereqreg:
 	clk_disable(hdmi->hdmi_clk);
-eclkenable:
 erate:
 	clk_put(hdmi->hdmi_clk);
 egetclk:
-	snd_soc_unregister_codec(&pdev->dev);
-esndreg:
+	mutex_destroy(&hdmi->mutex);
 	kfree(hdmi);
 
 	return ret;
@@ -1066,21 +1242,26 @@ static int __exit sh_hdmi_remove(struct platform_device *pdev)
 	struct sh_mobile_hdmi_info *pdata = pdev->dev.platform_data;
 	struct sh_hdmi *hdmi = platform_get_drvdata(pdev);
 	struct resource *res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
+	struct sh_mobile_lcdc_board_cfg	*board_cfg = &pdata->lcd_chan->board_cfg;
 	int irq = platform_get_irq(pdev, 0);
 
 	snd_soc_unregister_codec(&pdev->dev);
 
-	pdata->lcd_chan->board_cfg.display_on = NULL;
-	pdata->lcd_chan->board_cfg.display_off = NULL;
-	pdata->lcd_chan->board_cfg.board_data = NULL;
+	board_cfg->display_on = NULL;
+	board_cfg->display_off = NULL;
+	board_cfg->board_data = NULL;
+	board_cfg->owner = NULL;
 
+	/* No new work will be scheduled, wait for running ISR */
 	free_irq(irq, hdmi);
-	pm_runtime_disable(&pdev->dev);
+	/* Wait for already scheduled work */
 	cancel_delayed_work_sync(&hdmi->edid_work);
+	pm_runtime_disable(&pdev->dev);
 	clk_disable(hdmi->hdmi_clk);
 	clk_put(hdmi->hdmi_clk);
 	iounmap(hdmi->base);
 	release_mem_region(res->start, resource_size(res));
+	mutex_destroy(&hdmi->mutex);
 	kfree(hdmi);
 
 	return 0;