LogicPD Support for OMAP3/DM3/AM3 boards

From Logic BSP-2.0-5-01

15 files changed, 2153 insertions, 31 deletions

diff --git a/drivers/gpio/Makefile b/drivers/gpio/Makefile
index a5fa2b5d85..fc6eb59244 100644
--- a/drivers/gpio/Makefile
+++ b/drivers/gpio/Makefile
@@ -31,6 +31,8 @@ COBJS-$(CONFIG_MARVELL_MFP)	+= mvmfp.o
 COBJS-$(CONFIG_MXC_GPIO)	+= mxc_gpio.o
 COBJS-$(CONFIG_PCA953X)		+= pca953x.o
 COBJS-$(CONFIG_S5P)		+= s5p_gpio.o
+COBJS-$(CONFIG_TWL4030_GPIO)	+= twl4030-gpio.o
+COBJS-$(CONFIG_TWL4030_PWM)	+= twl4030-pwm.o
 
 COBJS	:= $(COBJS-y)
 SRCS 	:= $(COBJS:.o=.c)
diff --git a/drivers/gpio/twl4030-gpio.c b/drivers/gpio/twl4030-gpio.c
new file mode 100644
index 0000000000..57c12fcfb9
--- /dev/null
+++ b/drivers/gpio/twl4030-gpio.c
@@ -0,0 +1,105 @@
+#include <common.h>
+#include <twl4030.h>
+
+#define BIT(x) (1 << (x))
+
+/* GPIO_CTRL Fields */
+#define MASK_GPIO_CTRL_GPIO_ON		BIT(2)
+
+/* store usage of each GPIO. - each bit represents one GPIO */
+static unsigned int gpio_usage_count;
+
+static inline int gpio_twl4030_write(u8 address, u8 data)
+{
+	return twl4030_i2c_write_u8(TWL4030_CHIP_GPIO, data, address);
+}
+
+static inline int gpio_twl4030_read(u8 address)
+{
+	u8 data;
+	int ret = 0;
+
+	ret = twl4030_i2c_read_u8(TWL4030_CHIP_GPIO, &data, address);
+	return (ret < 0) ? ret : data;
+}
+
+int twl4030_set_gpio_direction(unsigned int gpio, unsigned int is_input)
+{
+	u8 d_bnk = gpio >> 3;
+	u8 d_msk = BIT(gpio & 0x7);
+	u8 reg = 0;
+	u8 base = REG_GPIODATADIR1 + d_bnk;
+	int ret = 0;
+
+	ret = gpio_twl4030_read(base);
+	if (ret >= 0) {
+		if (is_input)
+			reg = ret & ~d_msk;
+		else
+			reg = ret | d_msk;
+
+		ret = gpio_twl4030_write(base, reg);
+	}
+
+	return ret;
+}
+
+int twl4030_set_gpio_dataout(unsigned int gpio, unsigned int enable)
+{
+	u8 d_bnk = gpio >> 3;
+	u8 d_msk = BIT(gpio & 0x7);
+	u8 base = 0;
+
+	if (enable)
+		base = REG_SETGPIODATAOUT1 + d_bnk;
+	else
+		base = REG_CLEARGPIODATAOUT1 + d_bnk;
+
+	return gpio_twl4030_write(base, d_msk);
+}
+
+int twl4030_get_gpio_datain(unsigned int gpio)
+{
+	u8 d_bnk = gpio >> 3;
+	u8 d_off = gpio & 0x7;
+	u8 base = 0;
+	int ret = 0;
+
+	if (unlikely((gpio >= TWL4030_GPIO_MAX)
+		|| !(gpio_usage_count & BIT(gpio))))
+		return -1;
+
+	base = REG_GPIODATAIN1 + d_bnk;
+	ret = gpio_twl4030_read(base);
+	if (ret > 0)
+		ret = (ret >> d_off) & 0x1;
+
+	return ret;
+}
+
+int twl4030_request_gpio(unsigned int gpio)
+{
+	int status = 0;
+
+	/* on first use, turn GPIO module "on" */
+	if (!gpio_usage_count) {
+		// struct twl4030_gpio_platform_data *pdata;
+		u8 value = MASK_GPIO_CTRL_GPIO_ON;
+
+		status = gpio_twl4030_write(REG_GPIO_CTRL, value);
+	}
+
+	if (!status)
+		gpio_usage_count |= (0x1 << gpio);
+
+	return status;
+}
+
+void twl4030_free_gpio(unsigned int gpio)
+{
+	gpio_usage_count &= ~BIT(gpio);
+
+	/* on last use, switch off GPIO module */
+	if (!gpio_usage_count)
+		gpio_twl4030_write(REG_GPIO_CTRL, 0x0);
+}
diff --git a/drivers/gpio/twl4030-pwm.c b/drivers/gpio/twl4030-pwm.c
new file mode 100644
index 0000000000..dfbb7f5b59
--- /dev/null
+++ b/drivers/gpio/twl4030-pwm.c
@@ -0,0 +1,126 @@
+#include <common.h>
+#include <twl4030.h>
+
+/* INTRB register offsets (TWL4030_CHIP_INTBR) */
+#define TWL_INTBR_PMBR1	0x92
+#define TWL_INTBR_GPBR1	0x91
+
+/*PWM0 register offsets (TWL4030_CHIP_PWM0) */
+#define TWL_LED_PWMON	0xf8
+#define TWL_LED_PWMOFF	0xf9
+
+#if 0
+#define PWM_PRINT(fmt, args...) printf(fmt, ## args)
+#else
+#define PWM_PRINT(fmt, args...)
+#endif
+
+int twl4030_set_pwm0(int level, int max_brightness)
+{
+	u8 mux_pwm, enb_pwm;
+	unsigned char c;
+	int status;
+
+	PWM_PRINT("%s: level %d\n", __FUNCTION__, level);
+	if (level > max_brightness)
+		return -1;
+
+	twl4030_i2c_read_u8(TWL4030_CHIP_INTBR, &mux_pwm, TWL_INTBR_PMBR1);
+	twl4030_i2c_read_u8(TWL4030_CHIP_INTBR, &enb_pwm, TWL_INTBR_GPBR1);
+
+	PWM_PRINT("%s: enb_pwm %02x mux_pwm %02x\n", __FUNCTION__, enb_pwm, mux_pwm);
+
+	if (level == 0) {
+		/* disable pwm0 output and clock */
+		enb_pwm &= ~0x05;
+		/* change pwm0 pin to gpio pin */
+		mux_pwm &= ~0x0c;
+
+		PWM_PRINT("%s: disable enb_pwm %02x mux_pwm %02x\n", __FUNCTION__, enb_pwm, mux_pwm);
+
+		status = twl4030_i2c_write_u8(TWL4030_CHIP_INTBR,
+					enb_pwm, TWL_INTBR_GPBR1);
+		if (status) {
+			printf("%s:%d status %d\n", __FUNCTION__, __LINE__, status);
+			return -2;
+		}
+		status = twl4030_i2c_write_u8(TWL4030_CHIP_INTBR,
+					mux_pwm, TWL_INTBR_PMBR1);
+		if (status) {
+			printf("%s:%d status %d\n", __FUNCTION__, __LINE__, status);
+			return -3;
+		}
+#if 0
+		PWM_PRINT("%s: turn off GPIO_%d as backlight!\n", __FUNCTION__, omap3logic_dss_lcd_data.lcd_gpio_backlight);
+		/* Turn off the backlight! */
+		gpio_set_value(omap3logic_dss_lcd_data.lcd_gpio_backlight, 0);
+#endif
+		return 0;
+	}
+
+	if (((enb_pwm & 0x5) != 0x5) || ((mux_pwm & 0x0c) != 0x4)) {
+		/* change gpio pin to pwm0 pin */
+		mux_pwm = (mux_pwm & ~0xc) | 0x04;
+		/* enable pwm0 output and clock*/
+		enb_pwm = (enb_pwm & ~0x5) | 0x05;
+
+		PWM_PRINT("%s: enable enb_pwm %02x mux_pwm %02x\n", __FUNCTION__, enb_pwm, mux_pwm);
+		status = twl4030_i2c_write_u8(TWL4030_CHIP_INTBR,
+					mux_pwm, TWL_INTBR_PMBR1);
+		if (status) {
+			printf("%s:%d status %d\n", __FUNCTION__, __LINE__, status);
+			return -4;
+		}
+		status = twl4030_i2c_write_u8(TWL4030_CHIP_INTBR,
+					enb_pwm, TWL_INTBR_GPBR1);
+		if (status) {
+			printf("%s:%d status %d\n", __FUNCTION__, __LINE__, status);
+			return -5;
+		}
+#if 0
+		PWM_PRINT("%s: turn on GPIO_%d as backlight!\n", __FUNCTION__, omap3logic_dss_lcd_data.lcd_gpio_backlight);
+		/* Turn on the backlight! */
+		gpio_set_value(omap3logic_dss_lcd_data.lcd_gpio_backlight, 1);
+#endif
+	}
+
+	/* 255 -> 1, 1 -> 126 */
+	c = (max_brightness * 126 + (1 - 126) * level) / (max_brightness - 1);
+
+	PWM_PRINT("%s: c %d (%d%% on)\n", __FUNCTION__, c, (((max_brightness+1)-c) * 100)/(max_brightness+1));
+	status = twl4030_i2c_write_u8(TWL4030_CHIP_PWM0, 0x7F, TWL_LED_PWMOFF);
+	if (status) {
+		PWM_PRINT("%s:%d status %d\n", __FUNCTION__, __LINE__, status);
+		return -6;
+	}
+	status = twl4030_i2c_write_u8(TWL4030_CHIP_PWM0, c, TWL_LED_PWMON);
+	if (status) {
+		PWM_PRINT("%s:%d status %d\n", __FUNCTION__, __LINE__, status);
+		return -7;
+	}
+	return 0;
+}
+
+void twl4030_dump_pwm0(void)
+{
+	int result;
+	u8 mux_pwm, enb_pwm;
+	u8 off_period, on_period;
+
+	result = twl4030_i2c_read_u8(TWL4030_CHIP_INTBR, &mux_pwm,
+				TWL_INTBR_PMBR1);
+	result |= twl4030_i2c_read_u8(TWL4030_CHIP_INTBR, &enb_pwm,
+				TWL_INTBR_GPBR1);
+
+	result |= twl4030_i2c_read_u8(TWL4030_CHIP_PWM0, &off_period,
+				TWL_LED_PWMOFF);
+	result |=twl4030_i2c_read_u8(TWL4030_CHIP_PWM0, &on_period,
+				TWL_LED_PWMON);
+
+	if (result) {
+		printf("%s: failed to read TWL regitsters; result %d\n", __FUNCTION__, result);
+		return;
+	}
+	printf("%s: mux_pwm %02x enb_pwm %02x off_period %02x on_period %02x\n",
+		__FUNCTION__, mux_pwm, enb_pwm, off_period, on_period);
+}
diff --git a/drivers/i2c/omap24xx_i2c.c b/drivers/i2c/omap24xx_i2c.c
index 71251d8007..ec02a5d33e 100644
--- a/drivers/i2c/omap24xx_i2c.c
+++ b/drivers/i2c/omap24xx_i2c.c
@@ -318,6 +318,10 @@ int i2c_probe (uchar chip)
 		return res;
 	}
 
+    // Pre-enable the peripheral. Otherwise it randomly would not transmit.
+    // When it doesn't tramsit the probe command reports spurious addresses.
+	writew(I2C_CON_EN, &i2c_base->con);
+
 	/* wait until bus not busy */
 	wait_for_bb ();
 
diff --git a/drivers/mmc/omap3_mmc.c b/drivers/mmc/omap3_mmc.c
index 15d41e55bd..83d2791887 100644
--- a/drivers/mmc/omap3_mmc.c
+++ b/drivers/mmc/omap3_mmc.c
@@ -33,6 +33,36 @@
 
 #include "omap3_mmc.h"
 
+#undef DEBUG_MMC
+
+#ifdef DEBUG_MMC
+#define MMC_PRINTF(fmt, args...) printf(fmt, ## args)
+#else
+#define MMC_PRINTF(fmt, args...)
+#endif
+
+#define USE_NEW_TRANSPEED_ENCODING
+
+#ifdef USE_NEW_TRANSPEED_ENCODING
+/* TRAN_SPEED Bit encoding:
+ * bits 2:0 transfer rate unit
+ *  0=100kbit/s, 1=1Mbit/s, 2=10Mbit/s, 3=100Mbit/s, 4... 7=reserved
+ * 6:3 time value
+ *  0=reserved, 1=1.0, 2=1.2, 3=1.3, 4=1.5, 5=2.0, 6=2.5, 7=3.0,
+ *  8=3.5, 9=4.0, A=4.5, B=5.0, C=5.5, D=6.0, E=7.0, F=8.0
+ */
+
+/* trans_rate is 1/10 of actual while trans_value is 10x actual.
+ * multiplication of both together gives speed, or negative if
+ * trans_rate is reserved */
+int trans_rate[8] = {
+	10000, 100000, 1000000, 10000000, -1, -1 -1 -1
+};
+int trans_value[16] = {
+	-1, 10, 12, 13, 15, 20, 25, 30, 35, 40, 45, 50, 55, 60, 70, 80
+};
+#else
+
 static const unsigned short mmc_transspeed_val[15][4] = {
 	{CLKD(10, 1), CLKD(10, 10), CLKD(10, 100), CLKD(10, 1000)},
 	{CLKD(12, 1), CLKD(12, 10), CLKD(12, 100), CLKD(12, 1000)},
@@ -50,6 +80,7 @@ static const unsigned short mmc_transspeed_val[15][4] = {
 	{CLKD(70, 1), CLKD(70, 10), CLKD(70, 100), CLKD(70, 1000)},
 	{CLKD(80, 1), CLKD(80, 10), CLKD(80, 100), CLKD(80, 1000)}
 };
+#endif
 
 static mmc_card_data cur_card_data;
 static block_dev_desc_t mmc_blk_dev;
@@ -475,12 +506,13 @@ static unsigned long mmc_bread(int dev_num, unsigned long blknr,
 	return i;
 }
 
-static unsigned char configure_mmc(mmc_card_data *mmc_card_cur)
+/* static */ unsigned char configure_mmc(mmc_card_data *mmc_card_cur)
 {
 	unsigned char ret_val;
 	unsigned int argument;
-	unsigned int trans_clk, trans_fact, trans_unit, retries = 2;
-	unsigned char trans_speed;
+	unsigned int trans_clk, retries = 2;
+	unsigned int trans_fact, trans_unit;
+	int trans_speed;
 	mmc_resp_t mmc_resp;
 
 	ret_val = mmc_init_setup();
@@ -501,12 +533,40 @@ static unsigned char configure_mmc(mmc_card_data *mmc_card_cur)
 	if (mmc_card_cur->card_type == MMC_CARD)
 		mmc_card_cur->version = mmc_resp.Card_CSD.spec_vers;
 
+	MMC_PRINTF("%s: %08x %08x %08x %08x\n", __FUNCTION__, mmc_resp.resp[0],
+		mmc_resp.resp[1], mmc_resp.resp[2], mmc_resp.resp[3]);
+
 	trans_speed = mmc_resp.Card_CSD.tran_speed;
 
+	MMC_PRINTF("%s: tran_speed %#x\n", __FUNCTION__, trans_speed);
+
 	ret_val = mmc_send_cmd(MMC_CMD4, MMC_DSR_DEFAULT << 16, mmc_resp.resp);
 	if (ret_val != 1)
 		return ret_val;
 
+	if (mmc_card_cur->max_freq && trans_speed > mmc_card_cur->max_freq) {
+		trans_speed = mmc_card_cur->max_freq;
+	}
+
+#ifdef USE_NEW_TRANSPEED_ENCODING
+	trans_unit = trans_speed & MMC_CSD_TRAN_SPEED_UNIT_MASK;
+	trans_fact = trans_speed & MMC_CSD_TRAN_SPEED_FACTOR_MASK;
+	trans_unit >>= 0;
+	trans_fact >>= 3;
+
+	MMC_PRINTF("%s: trans_unit %u trans_fact %u\n", __FUNCTION__, trans_unit, trans_fact);
+	trans_speed = trans_rate[trans_unit] * trans_value[trans_fact];
+	MMC_PRINTF("%s: trans_speed %d\n", __FUNCTION__, trans_speed);
+	if (trans_speed < 0)
+		return 0;
+	MMC_PRINTF("%s: MMC_CLOCK %u\n", __FUNCTION__, (MMC_CLOCK_REFERENCE * 1000000));
+	trans_clk = (MMC_CLOCK_REFERENCE * 1000000) / trans_speed;
+	MMC_PRINTF("%s: trans_clk %u\n", __FUNCTION__, trans_clk);
+	if (trans_speed * trans_clk < (MMC_CLOCK_REFERENCE * 1000000))
+		trans_clk++;
+	if (trans_clk > 1023)
+		trans_clk = 1023;
+#else
 	trans_unit = trans_speed & MMC_CSD_TRAN_SPEED_UNIT_MASK;
 	trans_fact = trans_speed & MMC_CSD_TRAN_SPEED_FACTOR_MASK;
 
@@ -521,6 +581,8 @@ static unsigned char configure_mmc(mmc_card_data *mmc_card_cur)
 	trans_fact >>= 3;
 
 	trans_clk = mmc_transspeed_val[trans_fact - 1][trans_unit] * 2;
+#endif
+	MMC_PRINTF("%s:%d trans_clk %#x\n", __FUNCTION__, __LINE__, trans_clk);
 	ret_val = mmc_clock_config(CLK_MISC, trans_clk);
 
 	if (ret_val != 1)
@@ -547,9 +609,23 @@ static unsigned char configure_mmc(mmc_card_data *mmc_card_cur)
 
 int mmc_legacy_init(int dev)
 {
+	char *p, *q, buf[32];
+
 	if (mmc_set_dev(dev) != 0)
 		return 1;
 
+	sprintf(buf, "mmc%d_max_freq_mhz", dev);
+	p = getenv(buf);
+	if (p) {
+		cur_card_data.max_freq = (unsigned int)simple_strtoull(p, &q, 0);
+		if (q && *q)
+			cur_card_data.max_freq = 0;
+	} else
+		cur_card_data.max_freq = 0;
+
+	if (cur_card_data.max_freq)
+		printf("MMC%d: max frequency limited to %uMHz\n", dev, cur_card_data.max_freq);
+
 	if (configure_mmc(&cur_card_data) != 1)
 		return 1;
 
diff --git a/drivers/mtd/Makefile b/drivers/mtd/Makefile
index 5a5ecdfe3c..f01f157c15 100644
--- a/drivers/mtd/Makefile
+++ b/drivers/mtd/Makefile
@@ -25,6 +25,7 @@ include $(TOPDIR)/config.mk
 
 LIB	:= $(obj)libmtd.o
 
+COBJS-$(CONFIG_MTD_DEBUG) += mtd_debug.o
 COBJS-$(CONFIG_MTD_DEVICE) += mtdcore.o
 COBJS-$(CONFIG_MTD_PARTITIONS) += mtdpart.o
 COBJS-$(CONFIG_MTD_CONCAT) += mtdconcat.o
diff --git a/drivers/mtd/mtd_debug.c b/drivers/mtd/mtd_debug.c
new file mode 100644
index 0000000000..31913f0565
--- /dev/null
+++ b/drivers/mtd/mtd_debug.c
@@ -0,0 +1,18 @@
+/*
+ * MTD verbose debug 
+ *
+ * (C) 2011 Peter Barada <peter.barada@logicpd.com>
+ *
+ * This code is GPL
+ */
+
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/compat.h>
+#include <ubi_uboot.h>
+
+#ifdef CONFIG_MTD_DEBUG
+#ifndef CONFIG_MTD_DEBUG_VERBOSE
+#define CONFIG_MTD_DEBUG_VERBOSE -1
+#endif
+int mtd_debug_verbose = CONFIG_MTD_DEBUG_VERBOSE;
+#endif
diff --git a/drivers/mtd/nand/Makefile b/drivers/mtd/nand/Makefile
index 8b598f6bfe..975d152e79 100644
--- a/drivers/mtd/nand/Makefile
+++ b/drivers/mtd/nand/Makefile
@@ -50,6 +50,7 @@ COBJS-$(CONFIG_NAND_S3C64XX) += s3c64xx.o
 COBJS-$(CONFIG_NAND_SPEAR) += spr_nand.o
 COBJS-$(CONFIG_NAND_OMAP_GPMC) += omap_gpmc.o
 COBJS-$(CONFIG_NAND_PLAT) += nand_plat.o
+COBJS-$(CONFIG_MTD_NAND_ECC_BCH) += nand_bch.o
 endif
 
 COBJS	:= $(COBJS-y)
diff --git a/drivers/mtd/nand/nand_base.c b/drivers/mtd/nand/nand_base.c
index 52f8575aac..1092a79732 100644
--- a/drivers/mtd/nand/nand_base.c
+++ b/drivers/mtd/nand/nand_base.c
@@ -43,6 +43,7 @@
 #include <linux/mtd/mtd.h>
 #include <linux/mtd/nand.h>
 #include <linux/mtd/nand_ecc.h>
+#include <linux/mtd/nand_bch.h>
 
 #ifdef CONFIG_MTD_PARTITIONS
 #include <linux/mtd/partitions.h>
@@ -136,7 +137,11 @@ static void nand_release_device (struct mtd_info *mtd)
 static uint8_t nand_read_byte(struct mtd_info *mtd)
 {
 	struct nand_chip *chip = mtd->priv;
-	return readb(chip->IO_ADDR_R);
+	uint8_t byte;
+
+	byte = readb(chip->IO_ADDR_R);
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND R  %02x\n", byte);
+	return byte;
 }
 
 /**
@@ -149,7 +154,11 @@ static uint8_t nand_read_byte(struct mtd_info *mtd)
 static uint8_t nand_read_byte16(struct mtd_info *mtd)
 {
 	struct nand_chip *chip = mtd->priv;
-	return (uint8_t) cpu_to_le16(readw(chip->IO_ADDR_R));
+	uint8_t byte;
+
+	byte = (uint8_t) cpu_to_le16(readw(chip->IO_ADDR_R));
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND R  %02x\n", byte);
+	return byte;
 }
 
 /**
@@ -162,7 +171,11 @@ static uint8_t nand_read_byte16(struct mtd_info *mtd)
 static u16 nand_read_word(struct mtd_info *mtd)
 {
 	struct nand_chip *chip = mtd->priv;
-	return readw(chip->IO_ADDR_R);
+	u16 word;
+
+	word = readw(chip->IO_ADDR_R);
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND R  %04x\n", word);
+	return word;
 }
 
 /**
@@ -201,6 +214,8 @@ static void nand_write_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 	int i;
 	struct nand_chip *chip = mtd->priv;
 
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND W  %d bytes\n", len);
+
 	for (i = 0; i < len; i++)
 		writeb(buf[i], chip->IO_ADDR_W);
 }
@@ -218,6 +233,8 @@ static void nand_read_buf(struct mtd_info *mtd, uint8_t *buf, int len)
 	int i;
 	struct nand_chip *chip = mtd->priv;
 
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND R  %d bytes\n", len);
+
 	for (i = 0; i < len; i++)
 		buf[i] = readb(chip->IO_ADDR_R);
 }
@@ -236,8 +253,10 @@ static int nand_verify_buf(struct mtd_info *mtd, const uint8_t *buf, int len)
 	struct nand_chip *chip = mtd->priv;
 
 	for (i = 0; i < len; i++)
-		if (buf[i] != readb(chip->IO_ADDR_R))
+		if (buf[i] != readb(chip->IO_ADDR_R)) {
+			MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND verify fail at offset %d\n", i);
 			return -EFAULT;
+		}
 	return 0;
 }
 
@@ -256,6 +275,8 @@ static void nand_write_buf16(struct mtd_info *mtd, const uint8_t *buf, int len)
 	u16 *p = (u16 *) buf;
 	len >>= 1;
 
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND W  %d words\n", len);
+
 	for (i = 0; i < len; i++)
 		writew(p[i], chip->IO_ADDR_W);
 
@@ -276,8 +297,20 @@ static void nand_read_buf16(struct mtd_info *mtd, uint8_t *buf, int len)
 	u16 *p = (u16 *) buf;
 	len >>= 1;
 
-	for (i = 0; i < len; i++)
-		p[i] = readw(chip->IO_ADDR_R);
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND R  %d words\n", len);
+
+	for (i = 0; i < len; i+=8) {
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+		*p++ = readw(chip->IO_ADDR_R);
+	}
+	for (; i < len; ++i)
+		*p++ = readw(chip->IO_ADDR_R);
 }
 
 /**
@@ -399,11 +432,88 @@ static int nand_default_block_markbad(struct mtd_info *mtd, loff_t ofs)
 static int nand_check_wp(struct mtd_info *mtd)
 {
 	struct nand_chip *chip = mtd->priv;
-	/* Check the WP bit */
+
+	/* Check the WP bit. To do so requires resetting the device to
+	   force the status back to its reset value (so WP becomes whether
+	   the WP pin is set). */
+	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+
+	/* Now check the WP bit */
 	chip->cmdfunc(mtd, NAND_CMD_STATUS, -1, -1);
 	return (chip->read_byte(mtd) & NAND_STATUS_WP) ? 0 : 1;
 }
 
+/*
+ * nand_get_features: - Read the features of the NAND chip
+ * @param mtd		nand mtd instance
+ * @param faddr		nand feature address
+ * @param features	return 4-byte array of features
+ *
+ * @return		0 on success, -1 in case of errors
+ */
+int nand_get_features(struct mtd_info *mtd, uint8_t faddr, uint8_t *features)
+{
+	struct nand_chip *chip = mtd->priv;
+	int i;
+
+	chip->select_chip(mtd, 0);
+
+	/* Send the status command */
+	chip->cmd_ctrl(mtd, NAND_CMD_GET_FEATURES, NAND_CTRL_CHANGE | NAND_CTRL_CLE);
+
+	/* Send the feature address */
+	chip->cmd_ctrl(mtd, faddr, NAND_CTRL_CHANGE | NAND_CTRL_ALE);
+	/* Switch to data access */
+	chip->cmd_ctrl(mtd, NAND_CMD_NONE, NAND_CTRL_CHANGE | NAND_NCE);
+
+	ndelay(100);
+
+	for (i=0; i<4; ++i)
+		features[i] = chip->read_byte(mtd);
+
+	MTDDEBUG(MTD_DEBUG_LEVEL4, "%s: %02x %02x %02x %02x\n", __FUNCTION__, features[0],
+		features[1], features[2], features[3]);
+
+	return 0;
+}
+
+/**
+ * nand_set_features - [GENERIC] set features array
+ * @mtd:	MTD device structure
+ * @faddr:	feature address
+ * @params:	4-byte array of parameters to write
+ */
+int nand_set_features(struct mtd_info *mtd, uint8_t faddr, uint8_t *features)
+{
+	struct nand_chip *chip;
+
+	chip = mtd->priv;
+
+	chip->select_chip(mtd, 0);
+
+	/* Send the status command */
+	chip->cmd_ctrl(mtd, NAND_CMD_SET_FEATURES, NAND_CTRL_CHANGE | NAND_CTRL_CLE);
+	/* Send the feature address */
+	chip->cmd_ctrl(mtd, faddr, NAND_CTRL_CHANGE | NAND_CTRL_ALE);
+	/* Switch to data access */
+	chip->cmd_ctrl(mtd, NAND_CMD_NONE, NAND_CTRL_CHANGE | NAND_NCE);
+
+	ndelay(100);
+	if (chip->options & NAND_BUSWIDTH_16) {
+		uint16_t ftrs16[4];
+		int i;
+		for (i=0; i<4; ++i)
+			ftrs16[i] = features[i];
+		chip->write_buf(mtd, (uint8_t *)ftrs16, sizeof(ftrs16));
+	} else
+		chip->write_buf(mtd, features, 4);
+
+	udelay(2);
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: faddr %02x [%02x %02x %02x %02x]\n", __FUNCTION__, faddr, features[0], features[1], features[2], features[3]);
+
+	return 0;
+}
+
 /**
  * nand_block_checkbad - [GENERIC] Check if a block is marked bad
  * @mtd:	MTD device structure
@@ -751,7 +861,7 @@ static int nand_wait(struct mtd_info *mtd, struct nand_chip *this)
  *
  * Not for syndrome calculating ecc controllers, which use a special oob layout
  */
-static int nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+/* static */ int nand_read_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 			      uint8_t *buf, int page)
 {
 	chip->read_buf(mtd, buf, mtd->writesize);
@@ -808,7 +918,7 @@ static int nand_read_page_raw_syndrome(struct mtd_info *mtd, struct nand_chip *c
  * @buf:	buffer to store read data
  * @page:	page number to read
  */
-static int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
+/* static */ int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 				uint8_t *buf, int page)
 {
 	int i, eccsize = chip->ecc.size;
@@ -850,7 +960,7 @@ static int nand_read_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
  * @readlen:	data length
  * @bufpoi:	buffer to store read data
  */
-static int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip, uint32_t data_offs, uint32_t readlen, uint8_t *bufpoi)
+/* static */ int nand_read_subpage(struct mtd_info *mtd, struct nand_chip *chip, uint32_t data_offs, uint32_t readlen, uint8_t *bufpoi)
 {
 	int start_step, end_step, num_steps;
 	uint32_t *eccpos = chip->ecc.layout->eccpos;
@@ -1170,6 +1280,26 @@ static int nand_do_read_ops(struct mtd_info *mtd, loff_t from,
 				sndcmd = 0;
 			}
 
+			/* If in chipe ECC mode, need to read the status
+			   to see if an ECC error occurred. */
+			if (chip->ecc.mode == NAND_ECC_CHIP) {
+				int status;
+				chip->cmd_ctrl(mtd, NAND_CMD_STATUS,
+					NAND_CTRL_CLE | NAND_CTRL_CHANGE);
+				chip->cmd_ctrl(mtd,
+					NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+				status = chip->read_byte(mtd);
+				chip->cmd_ctrl(mtd, NAND_CMD_READ0,
+					NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+				chip->cmd_ctrl(mtd, NAND_CMD_NONE,
+					NAND_NCE | NAND_CTRL_CHANGE);
+
+				if (status & 0x1)
+					mtd->ecc_stats.failed++;
+				else if (status & 0x10)
+					mtd->ecc_stats.corrected++;
+			}
+
 			/* Now read the page into the buffer */
 			if (unlikely(ops->mode == MTD_OOB_RAW))
 				ret = chip->ecc.read_page_raw(mtd, chip,
@@ -1306,7 +1436,7 @@ static int nand_read(struct mtd_info *mtd, loff_t from, size_t len,
  * @page:	page number to read
  * @sndcmd:	flag whether to issue read command or not
  */
-static int nand_read_oob_std(struct mtd_info *mtd, struct nand_chip *chip,
+/* static */ int nand_read_oob_std(struct mtd_info *mtd, struct nand_chip *chip,
 			     int page, int sndcmd)
 {
 	if (sndcmd) {
@@ -1362,7 +1492,7 @@ static int nand_read_oob_syndrome(struct mtd_info *mtd, struct nand_chip *chip,
  * @chip:	nand chip info structure
  * @page:	page number to write
  */
-static int nand_write_oob_std(struct mtd_info *mtd, struct nand_chip *chip,
+/* static */ int nand_write_oob_std(struct mtd_info *mtd, struct nand_chip *chip,
 			      int page)
 {
 	int status = 0;
@@ -1586,7 +1716,7 @@ static int nand_read_oob(struct mtd_info *mtd, loff_t from,
  *
  * Not for syndrome calculating ecc controllers, which use a special oob layout
  */
-static void nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
+/* static */ void nand_write_page_raw(struct mtd_info *mtd, struct nand_chip *chip,
 				const uint8_t *buf)
 {
 	chip->write_buf(mtd, buf, mtd->writesize);
@@ -1637,7 +1767,7 @@ static void nand_write_page_raw_syndrome(struct mtd_info *mtd, struct nand_chip
  * @chip:	nand chip info structure
  * @buf:	data buffer
  */
-static void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
+/* static */ void nand_write_page_swecc(struct mtd_info *mtd, struct nand_chip *chip,
 				  const uint8_t *buf)
 {
 	int i, eccsize = chip->ecc.size;
@@ -1996,7 +2126,7 @@ static int nand_do_write_oob(struct mtd_info *mtd, loff_t to,
 	/* Do not allow write past end of page */
 	if ((ops->ooboffs + ops->ooblen) > len) {
 		MTDDEBUG (MTD_DEBUG_LEVEL0, "nand_write_oob: "
-			  "Attempt to write past end of page\n");
+			"Attempt to write past end of page(%d+%d>%d)\n", ops->ooboffs, ops->ooblen, len);
 		return -EINVAL;
 	}
 
@@ -2585,6 +2715,9 @@ static const struct nand_flash_dev *nand_get_flash_type(struct mtd_info *mtd,
 		return ERR_PTR(-ENODEV);
 	}
 
+	chip->maf_id = tmp_manf;
+	chip->dev_id = tmp_id;
+
 	if (!type)
 		type = nand_flash_ids;
 
@@ -2763,7 +2896,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 	/*
 	 * If no default placement scheme is given, select an appropriate one
 	 */
-	if (!chip->ecc.layout) {
+	if (!chip->ecc.layout && (chip->ecc.mode != NAND_ECC_SOFT_BCH)) {
 		switch (mtd->oobsize) {
 		case 8:
 			chip->ecc.layout = &nand_oob_8;
@@ -2862,6 +2995,71 @@ int nand_scan_tail(struct mtd_info *mtd)
 		chip->ecc.write_oob = nand_write_oob_std;
 		chip->ecc.size = 256;
 		chip->ecc.bytes = 3;
+		if (chip->has_chip_ecc) {
+			/* Put chip into no-ECC mode */
+			uint8_t params[4] = {0x00, 0x00, 0x00, 0x00};
+			nand_set_features(mtd, 0x90, params);
+		}
+		break;
+
+	case NAND_ECC_SOFT_BCH:
+		printk(KERN_INFO "NAND ECC: SOFT_BCH\n");
+		if (!mtd_nand_has_bch()) {
+			printk(KERN_WARNING "CONFIG_MTD_ECC_BCH not enabled\n");
+			BUG();
+		}
+		chip->ecc.calculate = nand_bch_calculate_ecc;
+		chip->ecc.correct = nand_bch_correct_data;
+		chip->ecc.read_page = nand_read_page_swecc;
+		chip->ecc.read_subpage = nand_read_subpage;
+		chip->ecc.write_page = nand_write_page_swecc;
+		chip->ecc.read_page_raw = nand_read_page_raw;
+		chip->ecc.write_page_raw = nand_write_page_raw;
+		chip->ecc.read_oob = nand_read_oob_std;
+		chip->ecc.write_oob = nand_write_oob_std;
+		/*
+		 * Board driver should supply ecc.size and ecc.bytes values to
+		 * select how many bits are correctable; see nand_bch_init()
+		 * for details.
+		 * Otherwise, default to 4 bits for large page devices
+		 */
+		if (!chip->ecc.size && (mtd->oobsize >= 64)) {
+			chip->ecc.size = 512;
+			chip->ecc.bytes = 7;
+		}
+		chip->ecc.priv = nand_bch_init(mtd,
+					       chip->ecc.size,
+					       chip->ecc.bytes,
+					       &chip->ecc.layout);
+		if (!chip->ecc.priv) {
+			printk(KERN_WARNING "BCH ECC initialization failed!\n");
+			BUG();
+		}
+
+		if (chip->has_chip_ecc) {
+			/* Put chip into no-ECC mode */
+			uint8_t params[4] = {0x00, 0x00, 0x00, 0x00};
+			nand_set_features(mtd, 0x90, params);
+		}
+		break;
+
+	case NAND_ECC_CHIP:
+		if (!chip->ecc.read_page_raw)
+			chip->ecc.read_page_raw = nand_read_page_raw;
+		if (!chip->ecc.write_page_raw)
+			chip->ecc.write_page_raw = nand_write_page_raw;
+		chip->ecc.read_page = nand_read_page_raw;
+		chip->ecc.write_page = nand_write_page_raw;
+		chip->ecc.read_oob = nand_read_oob_std;
+		chip->ecc.write_oob = nand_write_oob_std;
+		chip->ecc.size = mtd->writesize;
+		chip->ecc.bytes = 0;
+
+		if (chip->has_chip_ecc) {
+			/* Put chip into ECC mode */
+			uint8_t params[4] = {0x08, 0x00, 0x00, 0x00};
+			nand_set_features(mtd, 0x90, params);
+		}
 		break;
 
 	case NAND_ECC_NONE:
@@ -2875,6 +3073,11 @@ int nand_scan_tail(struct mtd_info *mtd)
 		chip->ecc.write_oob = nand_write_oob_std;
 		chip->ecc.size = mtd->writesize;
 		chip->ecc.bytes = 0;
+		if (chip->has_chip_ecc) {
+			/* Put chip into ECC mode */
+			uint8_t params[4] = {0x08, 0x00, 0x00, 0x00};
+			nand_set_features(mtd, 0x90, params);
+		}
 		break;
 
 	default:
@@ -2893,6 +3096,7 @@ int nand_scan_tail(struct mtd_info *mtd)
 		chip->ecc.layout->oobavail +=
 			chip->ecc.layout->oobfree[i].length;
 	mtd->oobavail = chip->ecc.layout->oobavail;
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: oobavail %d\n", __FUNCTION__, mtd->oobavail);
 
 	/*
 	 * Set the number of read / write steps for one page depending on ECC
@@ -2994,6 +3198,9 @@ void nand_release(struct mtd_info *mtd)
 	del_mtd_partitions(mtd);
 #endif
 
+	if (chip->ecc.mode == NAND_ECC_SOFT_BCH)
+		nand_bch_free((struct nand_bch_control *)chip->ecc.priv);
+
 	/* Free bad block table memory */
 	kfree(chip->bbt);
 	if (!(chip->options & NAND_OWN_BUFFERS))
diff --git a/drivers/mtd/nand/nand_bch.c b/drivers/mtd/nand/nand_bch.c
new file mode 100644
index 0000000000..632dc7ae7c
--- /dev/null
+++ b/drivers/mtd/nand/nand_bch.c
@@ -0,0 +1,287 @@
+/*
+ * This file provides ECC correction for more than 1 bit per block of data,
+ * using binary BCH codes. It relies on the generic BCH library lib/bch.c.
+ *
+ * Copyright © 2011 Ivan Djelic <ivan.djelic@parrot.com>
+ *
+ * This file is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation; either version 2 or (at your option) any
+ * later version.
+ *
+ * This file is distributed in the hope that it will be useful, but WITHOUT
+ * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
+ * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
+ * for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this file; if not, write to the Free Software Foundation, Inc.,
+ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA.
+ */
+
+#if 1
+#if 1
+#include <config.h>
+#include <common.h>
+#include <linux/mtd/compat.h>
+#include <malloc.h>
+#endif
+#include <linux/types.h>
+// #include <linux/kernel.h>
+// #include <linux/module.h>
+// #include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/nand_bch.h>
+#include <linux/bch.h>
+#else
+#include <linux/types.h>
+#include <linux/kernel.h>
+#include <linux/module.h>
+#include <linux/slab.h>
+#include <linux/bitops.h>
+#include <linux/mtd/mtd.h>
+#include <linux/mtd/nand.h>
+#include <linux/mtd/nand_bch.h>
+#include <linux/bch.h>
+#endif
+
+/**
+ * struct nand_bch_control - private NAND BCH control structure
+ * @bch:       BCH control structure
+ * @ecclayout: private ecc layout for this BCH configuration
+ * @errloc:    error location array
+ * @eccmask:   XOR ecc mask, allows erased pages to be decoded as valid
+ */
+struct nand_bch_control {
+	struct bch_control   *bch;
+	struct nand_ecclayout ecclayout;
+	unsigned int         *errloc;
+	unsigned char        *eccmask;
+};
+
+/**
+ * nand_bch_calculate_ecc - [NAND Interface] Calculate ECC for data block
+ * @mtd:	MTD block structure
+ * @buf:	input buffer with raw data
+ * @code:	output buffer with ECC
+ */
+int nand_bch_calculate_ecc(struct mtd_info *mtd, const unsigned char *buf,
+			   unsigned char *code)
+{
+	const struct nand_chip *chip = mtd->priv;
+	struct nand_bch_control *nbc = chip->ecc.priv;
+	unsigned int i;
+
+#ifdef DEBUG_BCH
+	printf("%s:%d buf %p code %p\n", __FUNCTION__, __LINE__, buf, code);
+	for (i=0; i<16; ++i)
+		printf("%s%02x", !i?"dat: ":" ", buf[i]);
+	printf("\n");
+#endif
+	memset(code, 0, chip->ecc.bytes);
+	encode_bch(nbc->bch, buf, chip->ecc.size, code);
+
+	/* apply mask so that an erased page is a valid codeword */
+	for (i = 0; i < chip->ecc.bytes; i++)
+		code[i] ^= nbc->eccmask[i];
+
+#ifdef DEBUG_BCH
+	printf("%s:%d code:", __FUNCTION__, __LINE__);
+	for (i=0; i<7; ++i)
+		printf(" %02x", code[i]);
+	printf("\n");
+#endif
+
+	return 0;
+}
+// EXPORT_SYMBOL(nand_bch_calculate_ecc);
+
+/**
+ * nand_bch_correct_data - [NAND Interface] Detect and correct bit error(s)
+ * @mtd:	MTD block structure
+ * @buf:	raw data read from the chip
+ * @read_ecc:	ECC from the chip
+ * @calc_ecc:	the ECC calculated from raw data
+ *
+ * Detect and correct bit errors for a data byte block
+ */
+int nand_bch_correct_data(struct mtd_info *mtd, unsigned char *buf,
+			  unsigned char *read_ecc, unsigned char *calc_ecc)
+{
+	const struct nand_chip *chip = mtd->priv;
+	struct nand_bch_control *nbc = chip->ecc.priv;
+	unsigned int *errloc = nbc->errloc;
+	int i, count;
+
+#ifdef DEBUG_BCH
+	printf("%s:%d buf %p read_ecc %p calc_ecc %p\n", __FUNCTION__, __LINE__, buf, read_ecc, calc_ecc);
+	for (i=0; i<16; ++i)
+		printf("%s%02x", !i?"dat: ":" ", buf[i]);
+	printf("\n");
+	for (i=0; i<7; ++i)
+		printf("%s%02x", !i?"read_ecc: ":" ", read_ecc[i]);
+	printf("\n");
+	for (i=0; i<7; ++i)
+		printf("%s%02x", !i?"calc_ecc: ":" ", calc_ecc[i]);
+	printf("\n");
+#endif
+
+	count = decode_bch(nbc->bch, NULL, chip->ecc.size, read_ecc, calc_ecc,
+			   NULL, errloc);
+	if (count > 0) {
+		for (i = 0; i < count; i++) {
+			if (errloc[i] < (chip->ecc.size*8))
+				/* error is located in data, correct it */
+				buf[errloc[i] >> 3] ^= (1 << (errloc[i] & 7));
+			/* else error in ecc, no action needed */
+
+			MTDDEBUG(MTD_DEBUG_LEVEL0, "%s: corrected bitflip %u\n",
+			      __func__, errloc[i]);
+		}
+	} else if (count < 0) {
+		printk(KERN_ERR "ecc unrecoverable error\n");
+		count = -1;
+	}
+	return count;
+}
+// EXPORT_SYMBOL(nand_bch_correct_data);
+
+/**
+ * nand_bch_init - [NAND Interface] Initialize NAND BCH error correction
+ * @mtd:	MTD block structure
+ * @eccsize:	ecc block size in bytes
+ * @eccbytes:	ecc length in bytes
+ * @ecclayout:	output default layout
+ *
+ * Returns:
+ *  a pointer to a new NAND BCH control structure, or NULL upon failure
+ *
+ * Initialize NAND BCH error correction. Parameters @eccsize and @eccbytes
+ * are used to compute BCH parameters m (Galois field order) and t (error
+ * correction capability). @eccbytes should be equal to the number of bytes
+ * required to store m*t bits, where m is such that 2^m-1 > @eccsize*8.
+ *
+ * Example: to configure 4 bit correction per 512 bytes, you should pass
+ * @eccsize = 512  (thus, m=13 is the smallest integer such that 2^m-1 > 512*8)
+ * @eccbytes = 7   (7 bytes are required to store m*t = 13*4 = 52 bits)
+ */
+struct nand_bch_control *
+nand_bch_init(struct mtd_info *mtd, unsigned int eccsize, unsigned int eccbytes,
+	      struct nand_ecclayout **ecclayout)
+{
+	unsigned int m, t, eccsteps, i;
+	struct nand_ecclayout *layout;
+	struct nand_bch_control *nbc = NULL;
+	unsigned char *erased_page;
+
+	if (!eccsize || !eccbytes) {
+		printk(KERN_WARNING "ecc parameters not supplied\n");
+		goto fail;
+	}
+
+	m = fls(1+8*eccsize);
+	t = (eccbytes*8)/m;
+
+	nbc = kzalloc(sizeof(*nbc), GFP_KERNEL);
+	if (!nbc)
+		goto fail;
+
+	nbc->bch = init_bch(m, t, 0);
+	if (!nbc->bch)
+		goto fail;
+
+	/* verify that eccbytes has the expected value */
+	if (nbc->bch->ecc_bytes != eccbytes) {
+		printk(KERN_WARNING "invalid eccbytes %u, should be %u\n",
+		       eccbytes, nbc->bch->ecc_bytes);
+		goto fail;
+	}
+
+	eccsteps = mtd->writesize/eccsize;
+
+	/* if no ecc placement scheme was provided, build one */
+	if (!*ecclayout) {
+
+		/* handle large page devices only */
+		if (mtd->oobsize < 64) {
+			printk(KERN_WARNING "must provide an oob scheme for "
+			       "oobsize %d\n", mtd->oobsize);
+			goto fail;
+		}
+
+		layout = &nbc->ecclayout;
+		layout->eccbytes = eccsteps*eccbytes;
+
+		/* reserve 2 bytes for bad block marker */
+		if (layout->eccbytes+2 > mtd->oobsize) {
+			printk(KERN_WARNING "no suitable oob scheme available "
+			       "for oobsize %d eccbytes %u\n", mtd->oobsize,
+			       eccbytes);
+			goto fail;
+		}
+		/* put ecc bytes at oob tail */
+		for (i = 0; i < layout->eccbytes; i++)
+			layout->eccpos[i] = mtd->oobsize-layout->eccbytes+i;
+
+		layout->oobfree[0].offset = 2;
+		layout->oobfree[0].length = mtd->oobsize-2-layout->eccbytes;
+
+		*ecclayout = layout;
+	}
+
+	/* sanity checks */
+	if (8*(eccsize+eccbytes) >= (1 << m)) {
+		printk(KERN_WARNING "eccsize %u is too large\n", eccsize);
+		goto fail;
+	}
+	if ((*ecclayout)->eccbytes != (eccsteps*eccbytes)) {
+		printk(KERN_WARNING "invalid ecc layout\n");
+		goto fail;
+	}
+
+	nbc->eccmask = kmalloc(eccbytes, GFP_KERNEL);
+	nbc->errloc = kmalloc(t*sizeof(*nbc->errloc), GFP_KERNEL);
+	if (!nbc->eccmask || !nbc->errloc)
+		goto fail;
+	/*
+	 * compute and store the inverted ecc of an erased ecc block
+	 */
+	erased_page = kmalloc(eccsize, GFP_KERNEL);
+	if (!erased_page)
+		goto fail;
+
+	memset(erased_page, 0xff, eccsize);
+	memset(nbc->eccmask, 0, eccbytes);
+	encode_bch(nbc->bch, erased_page, eccsize, nbc->eccmask);
+	kfree(erased_page);
+
+	for (i = 0; i < eccbytes; i++)
+		nbc->eccmask[i] ^= 0xff;
+
+	return nbc;
+fail:
+	nand_bch_free(nbc);
+	return NULL;
+}
+// EXPORT_SYMBOL(nand_bch_init);
+
+/**
+ * nand_bch_free - [NAND Interface] Release NAND BCH ECC resources
+ * @nbc:	NAND BCH control structure
+ */
+void nand_bch_free(struct nand_bch_control *nbc)
+{
+	if (nbc) {
+		free_bch(nbc->bch);
+		kfree(nbc->errloc);
+		kfree(nbc->eccmask);
+		kfree(nbc);
+	}
+}
+// EXPORT_SYMBOL(nand_bch_free);
+
+// MODULE_LICENSE("GPL");
+// MODULE_AUTHOR("Ivan Djelic <ivan.djelic@parrot.com>");
+// MODULE_DESCRIPTION("NAND software BCH ECC support");
diff --git a/drivers/mtd/nand/nand_util.c b/drivers/mtd/nand/nand_util.c
index 5a6f7aec88..82e661d39c 100644
--- a/drivers/mtd/nand/nand_util.c
+++ b/drivers/mtd/nand/nand_util.c
@@ -120,6 +120,8 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 		priv_nand->bbt = NULL;
 	}
 
+	lcd_percent_init(erase_length);
+
 	for (erased_length = 0;
 	     erased_length < erase_length;
 	     erase.addr += meminfo->erasesize) {
@@ -138,6 +140,8 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 				if (!opts->spread)
 					erased_length++;
 
+				lcd_percent_update(erased_length);
+
 				continue;
 
 			} else if (ret < 0) {
@@ -157,6 +161,7 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 			continue;
 		}
 
+
 		/* format for JFFS2 ? */
 		if (opts->jffs2 && chip->ecc.layout->oobavail >= 8) {
 			chip->ops.ooblen = 8;
@@ -197,10 +202,13 @@ int nand_erase_opts(nand_info_t *meminfo, const nand_erase_options_t *opts)
 					       erase.addr);
 			}
 		}
+		lcd_percent_update(erased_length);
 	}
+	lcd_percent_update(erased_length);
 	if (!opts->quiet)
 		printf("\n");
 
+
 	if (nand_block_bad_old) {
 		struct nand_chip *priv_nand = meminfo->priv;
 
@@ -337,8 +345,11 @@ int nand_unlock(struct mtd_info *mtd, ulong start, ulong length)
 	int status;
 	int page;
 	struct nand_chip *chip = mtd->priv;
+
+#if 0
 	printf ("nand_unlock: start: %08x, length: %d!\n",
 		(int)start, (int)length);
+#endif
 
 	/* select the NAND device */
 	chipnr = (int)(start >> chip->chip_shift);
@@ -455,7 +466,7 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 			u_char *buffer, int withoob)
 {
 	int rval = 0, blocksize;
-	size_t left_to_write = *length;
+	size_t left_to_write = *length, total_to_write;
 	u_char *p_buffer = buffer;
 	int need_skip;
 
@@ -499,7 +510,7 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		return -EINVAL;
 	}
 
-	if (!need_skip) {
+	if (!need_skip && !withoob) {
 		rval = nand_write (nand, offset, length, buffer);
 		if (rval == 0)
 			return 0;
@@ -510,6 +521,9 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		return rval;
 	}
 
+	total_to_write = left_to_write;
+	lcd_percent_init(total_to_write);
+
 	while (left_to_write > 0) {
 		size_t block_offset = offset & (nand->erasesize - 1);
 		size_t write_size;
@@ -548,7 +562,7 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 				ops.oobbuf = ops.datbuf + pagesize;
 
 				rval = nand->write_oob(nand, offset, &ops);
-				if (!rval)
+				if (rval)
 					break;
 
 				offset += pagesize;
@@ -571,8 +585,11 @@ int nand_write_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		}
 
 		left_to_write -= write_size;
+		lcd_percent_update(total_to_write - left_to_write);
 	}
 
+	lcd_percent_update(total_to_write);
+
 	return 0;
 }
 
@@ -594,7 +611,7 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		       u_char *buffer)
 {
 	int rval;
-	size_t left_to_read = *length;
+	size_t left_to_read = *length, total_to_read;
 	u_char *p_buffer = buffer;
 	int need_skip;
 
@@ -622,6 +639,9 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		return rval;
 	}
 
+	total_to_read = left_to_read;
+	lcd_percent_init(total_to_read);
+
 	while (left_to_read > 0) {
 		size_t block_offset = offset & (nand->erasesize - 1);
 		size_t read_length;
@@ -651,7 +671,11 @@ int nand_read_skip_bad(nand_info_t *nand, loff_t offset, size_t *length,
 		left_to_read -= read_length;
 		offset       += read_length;
 		p_buffer     += read_length;
+
+		lcd_percent_update(total_to_read - left_to_read);
 	}
 
+	lcd_percent_update(total_to_read);
+
 	return 0;
 }
diff --git a/drivers/mtd/nand/omap_gpmc.c b/drivers/mtd/nand/omap_gpmc.c
index 99b9cef17c..d5ee9bc583 100644
--- a/drivers/mtd/nand/omap_gpmc.c
+++ b/drivers/mtd/nand/omap_gpmc.c
@@ -26,11 +26,15 @@
 #include <asm/errno.h>
 #include <asm/arch/mem.h>
 #include <asm/arch/omap_gpmc.h>
+#include <asm/arch/sys_proto.h>
+#include <linux/mtd/mtd.h>
 #include <linux/mtd/nand_ecc.h>
+#include <linux/mtd/nand_bch.h>
 #include <nand.h>
 
 static uint8_t cs;
 static struct nand_ecclayout hw_nand_oob = GPMC_NAND_HW_ECC_LAYOUT;
+static struct nand_ecclayout chip_nand_oob = GPMC_NAND_CHIP_ECC_LAYOUT;
 
 /*
  * omap_nand_hwcontrol - Set the address pointers corretly for the
@@ -57,8 +61,17 @@ static void omap_nand_hwcontrol(struct mtd_info *mtd, int32_t cmd,
 		break;
 	}
 
-	if (cmd != NAND_CMD_NONE)
+	if (cmd != NAND_CMD_NONE) {
+#ifdef CONFIG_MTD_DEBUG
+		if (this->IO_ADDR_W == &gpmc_cfg->cs[cs].nand_cmd)
+			MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND C: %02x\n", cmd & 0xff);
+		else if (this->IO_ADDR_W == &gpmc_cfg->cs[cs].nand_adr)
+			MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND A: %02x\n", cmd & 0xff);
+		else if (this->IO_ADDR_W == &gpmc_cfg->cs[cs].nand_dat)
+			MTDDEBUG(MTD_DEBUG_LEVEL4, "NAND D: %02x\n", cmd & 0xff);
+#endif
 		writeb(cmd, this->IO_ADDR_W);
+	}
 }
 
 /*
@@ -156,6 +169,25 @@ static int omap_correct_data(struct mtd_info *mtd, uint8_t *dat,
 	return 0;
 }
 
+static int omap_correct_chip_hwecc(struct mtd_info *mtd, u_char *dat,
+				u_char *read_ecc, u_char *calc_ecc)
+{
+	struct nand_chip *chip;
+
+	chip = mtd->priv;
+
+	printf("%s: ecc_status %02x\n", __func__, chip->ecc_status);
+	/* We stored the read status in info->ecc_status in the read.
+	   If bit 0 is set, then there was an uncorrectable ECC error.
+	   If bit 3 is set, then there was a correctable error (up to
+	   four bits of correction). */
+	if (chip->ecc_status & 0x01)
+		return -1;
+	if (chip->ecc_status & 0x08)
+		return 4;
+	return 0;
+}
+
 /*
  *  omap_calculate_ecc - Generate non-inverted ECC bytes.
  *
@@ -192,6 +224,14 @@ static int omap_calculate_ecc(struct mtd_info *mtd, const uint8_t *dat,
 	return 0;
 }
 
+static int omap_calculate_chip_hwecc(struct mtd_info *mtd, const u_char *dat,
+				u_char *ecc_code)
+{
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s:\n", __func__);
+	return 0;
+}
+
+
 /*
  * omap_enable_ecc - This function enables the hardware ecc functionality
  * @mtd:        MTD device structure
@@ -224,14 +264,308 @@ static void omap_enable_hwecc(struct mtd_info *mtd, int32_t mode)
 	}
 }
 
+static void omap_enable_chip_hwecc(struct mtd_info *mtd, int mode)
+{
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s:\n", __func__);
+}
+
+/*
+ * omap_nand_chip_has_ecc - return true if chip has internal ECC
+ */
+int omap_nand_chip_has_ecc(void)
+{
+	struct nand_chip *chip;
+	struct mtd_info *mtd;
+	int		i;
+	uint8_t		ident[5];
+
+	if (nand_curr_device < 0 ||
+	    nand_curr_device >= CONFIG_SYS_MAX_NAND_DEVICE ||
+	    !nand_info[nand_curr_device].name) {
+		printf("Error: Can't switch ecc, no devices available\n");
+		return 0;
+	}
+
+	mtd = &nand_info[nand_curr_device];
+	chip = mtd->priv;
+
+#if 1
+	chip->select_chip(mtd, 0);
+	chip->cmdfunc(mtd, NAND_CMD_RESET, -1, -1);
+	chip->cmdfunc(mtd, NAND_CMD_READID, 0x00, -1);
+
+	/* Wait for the chip to get the ID ready */
+	ndelay(100);
+
+	for (i=0; i<2; ++i)
+		ident[i] = chip->read_byte(mtd);
+
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s:%d %02x %02x\n", __FUNCTION__, __LINE__, ident[0], ident[1]);
+	if (ident[0] == NAND_MFR_MICRON) {
+		for (i=2; i<5; ++i)
+			ident[i] = chip->read_byte(mtd);
+		MTDDEBUG(MTD_DEBUG_LEVEL3, "%s:%d %02x %02x %02x\n", __FUNCTION__, __LINE__, ident[2], ident[3], ident[4]);
+		if (ident[4] & 0x3)
+			chip->has_chip_ecc = 1;
+	}
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: has_chip_ecc %d\n", __FUNCTION__, chip->has_chip_ecc);
+#else
+	if (nand->maf_id == NAND_MFR_MICRON) {
+		switch(nand->dev_id) {
+		case 0x2c:
+		case 0xdc:
+		case 0xcc:
+		case 0xac:
+		case 0xbc:
+		case 0xa3:
+		case 0xb3:
+		case 0xd3:
+		case 0xc3:
+			nand->has_chip_ecc = 1;
+			return 1;
+		default:
+			break;
+		}
+	}
+#endif
+	return chip->has_chip_ecc;
+}
+
+
+static void micron_set_chip_ecc(struct mtd_info *mtd, int enable)
+{
+	uint8_t params[4];
+
+	MTDDEBUG(MTD_DEBUG_LEVEL3,"%s:%d enable %d\n", __FUNCTION__, __LINE__, enable);
+
+	memset(params, 0x00, sizeof(params));
+	if (enable)
+		params[0] = 0x08;
+	nand_set_features(mtd, 0x90, params);
+
+#if 1
+	nand_get_features(mtd, 0x90, params);
+	MTDDEBUG(MTD_DEBUG_LEVEL4,"%s: %02x %02x %02x %02x\n", __FUNCTION__, params[0], params[1], params[2], params[3]);
+#endif
+}
+
+/**
+ * nand_read_oob_chipecc - read; get status to seeif chip ECC error
+ * @mtd:	mtd info structure
+ * @chip:	nand chip info structure
+ * @page:	page number to read
+ * @sndcmd:	flag whether to issue read command or not
+ */
+static int omap_read_oob_chipecc(struct mtd_info *mtd, struct nand_chip *chip,
+			     int page, int sndcmd)
+{
+	struct nand_chip *nand;
+
+	nand = mtd->priv;
+
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: page = %d, len = %i\n",
+			__func__, page, mtd->oobsize);
+
+	if (sndcmd) {
+		chip->cmdfunc(mtd, NAND_CMD_READOOB, 0, page);
+		sndcmd = 0;
+	}
+
+	/* Send the status command */
+	omap_nand_hwcontrol(mtd, NAND_CMD_STATUS,
+		NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+	/* Switch to data access */
+	omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+		NAND_NCE | NAND_CTRL_CHANGE);
+	chip->ecc_status = chip->read_byte(mtd);
+	MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: ecc_status %02x\n", __func__, chip->ecc_status);
+	if (chip->ecc_status & (0x8|0x1)) {
+		MTDDEBUG(MTD_DEBUG_LEVEL3, "%s:%d page %d ecc_status %02x\n", __FUNCTION__, __LINE__, page, chip->ecc_status);
+		if (chip->ecc_status & 0x1)
+			mtd->ecc_stats.failed++;
+		else if (chip->ecc_status & 0x80)
+			mtd->ecc_stats.corrected += 4;
+	}
+
+	/* Send the read prefix */
+	omap_nand_hwcontrol(mtd, NAND_CMD_READ0,
+		NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+	/* Switch to data access */
+	omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+		NAND_NCE | NAND_CTRL_CHANGE);
+	
+	chip->read_buf(mtd, chip->oob_poi, mtd->oobsize);
+	return sndcmd;
+}
+
+/**
+ * omap_nand_command_lp - Send command to NAND large page device
+ * @mtd:	MTD device structure
+ * @command:	the command to be sent
+ * @column:	the column address for this command, -1 if none
+ * @page_addr:	the page address for this command, -1 if none
+ *
+ * Send command to NAND device. This is the version for the new large page
+ * devices We dont have the separate regions as we have in the small page
+ * devices.  We must emulate NAND_CMD_READOOB to keep the code compatible.
+ */
+static void omap_nand_command_lp(struct mtd_info *mtd, unsigned int command,
+			    int column, int page_addr)
+{
+	register struct nand_chip *chip = mtd->priv;
+
+	/* Emulate NAND_CMD_READOOB */
+	if (command == NAND_CMD_READOOB) {
+		column += mtd->writesize;
+		command = NAND_CMD_READ0;
+	}
+
+	/* Command latch cycle */
+	omap_nand_hwcontrol(mtd, command & 0xff,
+		       NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+
+	if (column != -1 || page_addr != -1) {
+		int ctrl = NAND_CTRL_CHANGE | NAND_NCE | NAND_ALE;
+
+		/* Serially input address */
+		if (column != -1) {
+			/* Adjust columns for 16 bit buswidth */
+			if (chip->options & NAND_BUSWIDTH_16)
+				column >>= 1;
+			omap_nand_hwcontrol(mtd, column, ctrl);
+			ctrl &= ~NAND_CTRL_CHANGE;
+			omap_nand_hwcontrol(mtd, column >> 8, ctrl);
+		}
+		if (page_addr != -1) {
+			omap_nand_hwcontrol(mtd, page_addr, ctrl);
+			omap_nand_hwcontrol(mtd, page_addr >> 8,
+				       NAND_NCE | NAND_ALE);
+			/* One more address cycle for devices > 128MiB */
+			if (chip->chipsize > (128 << 20))
+				omap_nand_hwcontrol(mtd, page_addr >> 16,
+					       NAND_NCE | NAND_ALE);
+		}
+	}
+	omap_nand_hwcontrol(mtd, NAND_CMD_NONE, NAND_NCE | NAND_CTRL_CHANGE);
+
+	/*
+	 * program and erase have their own busy handlers
+	 * status, sequential in, and deplete1 need no delay
+	 */
+	switch (command) {
+
+	case NAND_CMD_CACHEDPROG:
+	case NAND_CMD_PAGEPROG:
+	case NAND_CMD_ERASE1:
+	case NAND_CMD_ERASE2:
+	case NAND_CMD_SEQIN:
+	case NAND_CMD_RNDIN:
+	case NAND_CMD_STATUS:
+	case NAND_CMD_DEPLETE1:
+		return;
+
+		/*
+		 * read error status commands require only a short delay
+		 */
+	case NAND_CMD_STATUS_ERROR:
+	case NAND_CMD_STATUS_ERROR0:
+	case NAND_CMD_STATUS_ERROR1:
+	case NAND_CMD_STATUS_ERROR2:
+	case NAND_CMD_STATUS_ERROR3:
+		udelay(chip->chip_delay);
+		return;
+
+	case NAND_CMD_RESET:
+		if (chip->dev_ready)
+			break;
+		udelay(chip->chip_delay);
+		omap_nand_hwcontrol(mtd, NAND_CMD_STATUS,
+			       NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+		omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+			       NAND_NCE | NAND_CTRL_CHANGE);
+		while (!(chip->read_byte(mtd) & NAND_STATUS_READY)) ;
+		return;
+
+	case NAND_CMD_RNDOUT:
+		/* No ready / busy check necessary */
+		omap_nand_hwcontrol(mtd, NAND_CMD_RNDOUTSTART,
+			       NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+		omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+			       NAND_NCE | NAND_CTRL_CHANGE);
+		return;
+
+	case NAND_CMD_READ0:
+
+		/* Send the read start */
+		omap_nand_hwcontrol(mtd, NAND_CMD_READSTART,
+			       NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+		omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+			       NAND_NCE | NAND_CTRL_CHANGE);
+
+		/* This applies to read commands */
+	default:
+		/*
+		 * If we don't have access to the busy pin, we apply the given
+		 * command delay
+		 */
+		if (!chip->dev_ready) {
+			udelay(chip->chip_delay);
+			goto ready_exit;
+		}
+	}
+
+	/* Apply this short delay always to ensure that we do wait tWB in
+	 * any case on any machine. */
+	ndelay(100);
+
+	nand_wait_ready(mtd);
+
+ready_exit:
+	/* If the chip has internal ECC, then we need to read the status
+	   to determin if there's an ECC error - capture it for handling by
+	   omap_nand_correct_chip_hwecc() later */
+	if (command == NAND_CMD_READ0) {
+		if (chip->has_chip_ecc) {
+
+			/* Send the status command */
+			omap_nand_hwcontrol(mtd, NAND_CMD_STATUS,
+				NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+			/* Switch to data access */
+			omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+				NAND_NCE | NAND_CTRL_CHANGE);
+			chip->ecc_status = chip->read_byte(mtd);
+			MTDDEBUG(MTD_DEBUG_LEVEL3, "%s: ecc_status %02x\n", __func__, chip->ecc_status);
+#if 0
+			if (chip->ecc_status & (0x8|0x1))
+				printk("%s:%d page %d column %d ecc_status %02x\n", __FUNCTION__, __LINE__, page_addr, column, chip->ecc_status);
+#endif
+
+			/* Send the read prefix */
+			omap_nand_hwcontrol(mtd, NAND_CMD_READ0,
+				NAND_NCE | NAND_CLE | NAND_CTRL_CHANGE);
+			/* Switch to data access */
+			omap_nand_hwcontrol(mtd, NAND_CMD_NONE,
+				NAND_NCE | NAND_CTRL_CHANGE);
+
+		}
+	}
+		
+}
+
+static enum omap_nand_ecc_mode current_ecc_method;
+enum omap_nand_ecc_mode omap_nand_current_ecc_method(void)
+{
+	return current_ecc_method;
+}
+
 /*
  * omap_nand_switch_ecc - switch the ECC operation b/w h/w ecc and s/w ecc.
  * The default is to come up on s/w ecc
  *
- * @hardware - 1 -switch to h/w ecc, 0 - s/w ecc
+ * @hardware - 1 -switch to h/w ecc, 0 - s/w ecc, 2 - chip ecc
  *
  */
-void omap_nand_switch_ecc(int32_t hardware)
+void omap_nand_switch_ecc(enum omap_nand_ecc_mode mode)
 {
 	struct nand_chip *nand;
 	struct mtd_info *mtd;
@@ -257,8 +591,50 @@ void omap_nand_switch_ecc(int32_t hardware)
 	nand->ecc.correct = NULL;
 	nand->ecc.calculate = NULL;
 
+	/* If currently in BCH then free the priv pointer */
+	if (nand->ecc.mode == NAND_ECC_SOFT_BCH && nand->ecc.priv) {
+		nand_bch_free((struct nand_bch_control *)nand->ecc.priv);
+		nand->ecc.priv = NULL;
+	}
+
 	/* Setup the ecc configurations again */
-	if (hardware) {
+	if (mode == OMAP_ECC_SOFT_BCH) {
+		nand->ecc.mode = NAND_ECC_SOFT_BCH;
+		nand->ecc.calculate = nand_bch_calculate_ecc;
+		nand->ecc.correct = nand_bch_correct_data;
+		nand->ecc.read_page = nand_read_page_swecc;
+		nand->ecc.read_subpage = nand_read_subpage;
+		nand->ecc.write_page = nand_write_page_swecc;
+		nand->ecc.read_page_raw = nand_read_page_raw;
+		nand->ecc.write_page_raw = nand_write_page_raw;
+		nand->ecc.read_oob = nand_read_oob_std;
+		nand->ecc.write_oob = nand_write_oob_std;
+		/*
+		 * Board driver should supply ecc.size and ecc.bytes values to
+		 * select how many bits are correctable; see nand_bch_init()
+		 * for details.
+		 * Otherwise, default to 4 bits for large page devices
+		 */
+#if 1
+		/* Since switching, clear out previous ECC setup and let
+		 * BCH figure it out */
+		nand->ecc.size = 0;
+		nand->ecc.bytes = 0;
+		nand->ecc.layout = NULL;
+#endif
+		if (!nand->ecc.size && (mtd->oobsize >= 64)) {
+			nand->ecc.size = 512;
+			nand->ecc.bytes = 7;
+		}
+		nand->ecc.priv = nand_bch_init(mtd,
+					       nand->ecc.size,
+					       nand->ecc.bytes,
+					       &nand->ecc.layout);
+		if (!nand->ecc.priv) {
+			printk(KERN_WARNING "BCH ECC initialization failed!\n");
+			BUG();
+		}
+	} else if (mode == OMAP_ECC_HW) {
 		nand->ecc.mode = NAND_ECC_HW;
 		nand->ecc.layout = &hw_nand_oob;
 		nand->ecc.size = 512;
@@ -267,13 +643,41 @@ void omap_nand_switch_ecc(int32_t hardware)
 		nand->ecc.correct = omap_correct_data;
 		nand->ecc.calculate = omap_calculate_ecc;
 		omap_hwecc_init(nand);
-		printf("HW ECC selected\n");
-	} else {
+		printf("NAND: HW ECC selected\n");
+		if (nand->has_chip_ecc)
+			micron_set_chip_ecc(mtd, 0);
+	} else if (mode == OMAP_ECC_SOFT) {
 		nand->ecc.mode = NAND_ECC_SOFT;
 		/* Use mtd default settings */
 		nand->ecc.layout = NULL;
-		printf("SW ECC selected\n");
-	}
+		printf("NAND: SW ECC selected\n");
+		if (nand->has_chip_ecc)
+			micron_set_chip_ecc(mtd, 0);
+	} else if (mode == OMAP_ECC_CHIP) {
+		if (!nand->has_chip_ecc) {
+			printf("NAND: Chip does not have internal ECC!\n");
+			return;
+		}
+		nand->ecc.bytes = 0;
+		nand->ecc.size = 2048;
+		nand->ecc.calculate = omap_calculate_chip_hwecc;
+		nand->ecc.hwctl = omap_enable_chip_hwecc;
+		nand->ecc.correct = omap_correct_chip_hwecc;
+		nand->ecc.read_oob = omap_read_oob_chipecc;
+		nand->ecc.mode = NAND_ECC_CHIP; /* internal to chip */
+		nand->ecc.layout = &chip_nand_oob;
+		if (nand->options & NAND_BUSWIDTH_16)
+			nand->cmdfunc = omap_nand_command_lp;
+		else
+			printf("%s: Huh? not 16-bit wide\n", __FUNCTION__);
+		micron_set_chip_ecc(mtd, 1);
+		printf("NAND: Internal to NAND ECC selected\n");
+	} else {
+		printf("NAND: unknown ECC mode %d\n", mode);
+		return;
+ 	}
+
+	current_ecc_method = mode;
 
 	/* Update NAND handling after ECC mode switch */
 	nand_scan_tail(mtd);
@@ -336,6 +740,11 @@ int board_nand_init(struct nand_chip *nand)
 	if ((readl(&gpmc_cfg->cs[cs].config1) & 0x3000) == 0x1000)
 		nand->options |= NAND_BUSWIDTH_16;
 
+#ifdef CONFIG_MTD_SKIP_BBTSCAN
+	/* Skip the bad block scan */
+	nand->options |= NAND_SKIP_BBTSCAN;
+#endif
+
 	nand->chip_delay = 100;
 	/* Default ECC mode */
 	nand->ecc.mode = NAND_ECC_SOFT;
diff --git a/drivers/power/twl4030.c b/drivers/power/twl4030.c
index 5a7323a715..cf79161aaf 100644
--- a/drivers/power/twl4030.c
+++ b/drivers/power/twl4030.c
@@ -103,3 +103,520 @@ void twl4030_power_mmc_init(void)
 				TWL4030_PM_RECEIVER_VMMC1_DEV_GRP,
 				TWL4030_PM_RECEIVER_DEV_GRP_P1);
 }
+
+void twl4030_power_off(void)
+{
+	u8 val = 0;
+	if (twl4030_i2c_read_u8(TWL4030_CHIP_PM_MASTER, &val,
+				TWL4030_PM_MASTER_P1_SW_EVENTS)) {
+		printf("Error:TWL4030: failed to read the power register\n");
+	} else {
+		val |= TWL4030_PM_MASTER_SW_EVENTS_DEVOFF;
+		if (twl4030_i2c_write_u8(TWL4030_CHIP_PM_MASTER, val,
+					 TWL4030_PM_MASTER_P1_SW_EVENTS)) {
+			printf("Error:TWL4030: failed to write the power register\n");
+			printf("Could not power off\n");
+		}
+	}
+}
+
+int do_poweroff(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	printf("Power down.\n");
+	twl4030_power_off();
+	return 0;
+}
+
+U_BOOT_CMD(poweroff, 1, 1, do_poweroff,
+	"Power down board",
+	""
+);
+
+#ifdef CONFIG_TWL4030_CHARGING
+int twl4030_enable_charging(void)
+{
+	u8 val = 0;
+	/* write 0x57 to 0x4a, 0x85 (BCIMFKEY)*/
+	/* Enabel access to BCIMFEN1 register access */
+	if (twl4030_i2c_write_u8(TWL4030_CHIP_MAIN_CHARGE, 0x57,
+					TWL4030_MAIN_CHARGE_BCIMFKEY)) {
+		printf("Error:TWL4030: failed to write BCIMFKEY\n");
+		return 1;
+	}
+
+	/* read 0x4a, 0x86 */
+	if (twl4030_i2c_read_u8(TWL4030_CHIP_MAIN_CHARGE, &val,
+					TWL4030_MAIN_CHARGE_BCIMFEN1)) {
+		printf("Error:TWL4030: failed to read BCIMFEN1\n");
+		return 1;
+	}
+
+	/* or in 0x80 */
+	/* write to 0x4a, 0x86 (TWL_BCIMFEN1) */
+	val |= TWL4030_MAIN_CHARGE_BCIMFEN1_VBATOV1EN;
+	if (twl4030_i2c_write_u8(TWL4030_CHIP_MAIN_CHARGE, val,
+					TWL4030_MAIN_CHARGE_BCIMFEN1)) {
+		printf("Error:TWL4030: failed to write BCIMFEN1\n");
+		return 1;
+	}
+
+	/* write 0xd2 to 0x4a, 0x85 (BCIMFKEY) */
+	val = TWL4030_MAIN_CHARGE_BCIMFKEY_MFKEY5;
+	if (twl4030_i2c_write_u8(TWL4030_CHIP_MAIN_CHARGE,
+					val, 
+					TWL4030_MAIN_CHARGE_BCIMFKEY)) {
+		printf("Error:TWL4030: failed to write BCIMFKEY\n");
+		return 1;
+	}
+
+	/* clear low four bits in 0x4a, 0x8a (BCIMFTH1) */
+	if (twl4030_i2c_read_u8(TWL4030_CHIP_MAIN_CHARGE, &val,
+					TWL4030_MAIN_CHARGE_BCIMFTH1)) {
+		printf("Error:TWL4030: failed to read BCIMFTH1\n");
+		return 1;
+	}
+	val = (val & ~TWL4030_MAIN_CHARGE_BCIMFTH1_VBATOV1TH_MASK) |
+		TWL4030_MAIN_CHARGE_BCIMFTH1_VBATOV1TH_2636_mV;
+	/* clear low four bits in 0x4a, 0x8a (BCIMFTH1) */
+	if (twl4030_i2c_write_u8(TWL4030_CHIP_MAIN_CHARGE, val,
+					TWL4030_MAIN_CHARGE_BCIMFTH1)) {
+		printf("Error:TWL4030: failed to write BCIMFTH1\n");
+		return 1;
+	}
+
+	/* read 0x4a, 0x86; 0x4a, 0x8a (???) */
+
+	/* Turn on AC charging */
+	/* write 0x90 to 0x49, 0x91 (MADC_HFCLK_EN, DEFAULTMADC_CLK_EN) */
+	val = TWL4030_INTBR_GPBR1_MADC_HFCLK_EN;
+	val |= TWL4030_INTBR_GPBR1_DEFAULT_MADC_CLK_EN;
+	if (twl4030_i2c_write_u8(TWL4030_CHIP_INTBR, val,
+					TWL4030_INTBR_GPBR1)) {
+		printf("Error:TWL4030: failed to write BCIMFTH1\n");
+		return 1;
+	}
+	return 0;
+}
+
+
+struct {
+	u8 idx;
+	u8 reg_base;
+	char *str;
+	u16 mul;
+} adc_regs[] = {
+	{
+		0,
+		0x37,
+		"(GP analog input/Bat type)",
+		1466
+	},
+	{
+		1,
+		0x39,
+		"(GP analog input/Bat temp)",
+		1446
+	},
+	{
+		2,
+		0x3b,
+		"(GP analog input)\t",
+		2444,
+	},
+	{
+		3,
+		0x3d,
+		"(GP analog input)\t",
+		2444,
+	},
+	{
+		4,
+		0x3f,
+		"(GP analog input)\t",
+		2444,
+	},
+	{
+		5,
+		0x41,
+		"(GP analog input)\t",
+		2444,
+	},
+	{
+		6,
+		0x43,
+		"(GP analog input)\t",
+		2444,
+	},
+	{
+		7,
+		0x45,
+		"(GP analog input)\t",
+		2444,
+	},
+	{
+		8,
+		0x47,
+		"(VBUS voltage)\t",
+		6843,
+	},
+	{
+		9,
+		0x49,
+		"(Charger backup bat volt)",
+		4399,
+	},
+	{
+		11,
+		0x4d,
+		"(Battery charger voltage)",
+		9775,
+	},
+	{
+		12,
+		0x4f,
+		"(Main battery voltage)",
+		5865,
+	},
+	{
+		15,
+		0x55,
+		"(VRUSB supply/spkr pol)",
+		3225,
+	},
+};
+
+struct 
+{
+	u8 val;
+	char *str;
+	u8 pr_info;
+} charge_state[] = {
+	{
+		0x00,
+		"No charging device",
+		0,
+	},
+	{
+		0x01,
+		"Off mode",
+		0,
+	},
+	{
+		0x02,
+		"Standby mode",
+		0,
+	},
+	{
+		0x03,
+		"Open bat or USB not debounced",
+		0,
+	},
+	{
+		0x21,
+		"Constant voltage AC",
+		1,
+	},
+	{
+		0x22,
+		"Quick charge AC 1",
+		1,
+	},
+	{
+		0x23,
+		"Quick charge AC 2",
+		1,
+	},
+	{
+		0x24,
+		"Quick charge AC 3",
+		1,
+	},
+	{
+		0x25,
+		"Quick charge AC 4",
+		1,
+	},
+	{
+		0x26,
+		"Quick charge AC 5",
+		1,
+	},
+	{
+		0x27,
+		"Quick charge AC 6",
+		1,
+	},
+	{
+		0x28,
+		"Charge stop AC 1",
+		1,
+	},
+	{
+		0x29,
+		"Charge stop AC 1",
+		1,
+	},
+	{
+		0x2a,
+		"Charge stop AC 1",
+		1,
+	},
+	{
+		0x2b,
+		"Charge AC comp 1",
+		1,
+	},
+	{
+		0x2c,
+		"Charge AC comp 2",
+		1,
+	},
+	{
+		0x2d,
+		"Charge AC comp 3",
+		1,
+	},
+	{
+		0x2e,
+		"Charge AC comp 4",
+		1,
+	},
+	{
+		0x2f,
+		"AC adapter overvoltage",
+		0,
+	},
+	{
+		0x12,
+		"Quick charge USB 1",
+		1,
+	},
+	{
+		0x13,
+		"Quick charge USB 2",
+		1,
+	},
+	{
+		0x14,
+		"Quick charge USB 3",
+		1,
+	},
+	{
+		0x15,
+		"Quick charge USB 4",
+		1,
+	},
+	{
+		0x16,
+		"Quick charge USB 5",
+		1,
+	},
+	{
+		0x17,
+		"Quick charge USB 6",
+		1,
+	},
+	{
+		0x18,
+		"Charge stop USB 1",
+		1,
+	},
+	{
+		0x19,
+		"Charge stop USB 2",
+		1,
+	},
+	{
+		0x1a,
+		"Charge stop USB 3",
+		1,
+	},
+	{
+		0x1b,
+		"Charge USB comp 1",
+		1,
+	},
+	{
+		0x1c,
+		"Charge USB comp 2",
+		1,
+	},
+	{
+		0x1d,
+		"Charge USB comp 3",
+		1,
+	},
+	{
+		0x1e,
+		"Charge USB comp 4",
+		1,
+	},
+	{
+		0x1f,
+		"USB adapter overvoltage",
+		0,
+	},
+};
+
+int read_madc_msblsb(u8 reg)
+{
+	u8 val;
+	int temp;
+
+	twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &val, reg+1);
+	temp = ((int)(val & 0x3)) << 8;
+	twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &val, reg);
+	return temp | val;
+}
+
+#define VOLT_STEP_SIZE	588
+#define VOLT_PSR_R	100
+	
+int pm_battery_voltage(void)
+{
+	int volt = read_madc_msblsb(0x78);
+	return (volt * VOLT_STEP_SIZE) / VOLT_PSR_R;
+}
+
+#define CURR_SLOPE	1194
+
+int pm_battery_current(void)
+{
+	int curr = read_madc_msblsb(0x7c);
+	u8 val;
+
+	twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &val, 0x98);
+	if (val & 0x20)
+		return (curr * 1000) / (2 * CURR_SLOPE);
+	else
+		return (curr * 1000) / (1 * CURR_SLOPE);
+}
+
+static int batt_table[] = {
+/* 0 C*/
+30800, 29500, 28300, 27100,
+26000, 24900, 23900, 22900, 22000, 21100, 20300, 19400, 18700, 17900,
+17200, 16500, 15900, 15300, 14700, 14100, 13600, 13100, 12600, 12100,
+11600, 11200, 10800, 10400, 10000, 9630,  9280,  8950,  8620,  8310,
+8020,  7730,  7460,  7200,  6950,  6710,  6470,  6250,  6040,  5830,
+5640,  5450,  5260,  5090,  4920,  4760,  4600,  4450,  4310,  4170,
+4040,  3910,  3790,  3670,  3550
+};
+
+#define TEMP_STEP_SIZE	147
+#define TEMP_PSR_R	100
+
+int pm_battery_temp(void)
+{
+	u8 val;
+	int temp, curr, volt, res, ret;
+
+	ret = read_madc_msblsb(0x7a);
+
+	volt = (ret * TEMP_STEP_SIZE) / TEMP_PSR_R;
+
+	twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &val, 0x98);
+	curr = ((val & 0x07) + 1) * 10;
+	res = volt * 1000 / curr;
+	for (temp = 58; temp >= 0; temp--) {
+		int actual = batt_table[temp];
+		if ((actual - res) >= 0)
+			break;
+	}
+
+	if (temp < 3) {
+		if (temp == 2)
+			temp = -1;
+		else if (temp == 1)
+			temp = -2;
+		else
+			temp = -3;
+	}
+	return temp + 1;
+}
+
+void print_charge_info(void)
+{
+	printf("\n");
+
+	printf("Charger main battery voltage\t\t      %4u mV\n", pm_battery_voltage());
+
+	printf("Charger current\t\t\t\t      %4u mA\n", pm_battery_current());
+
+	printf("\n");
+
+	printf("Battery temperature (if using Logic battery)\t%2u deg C\n", pm_battery_temp());
+
+}
+		
+int twl4030_info(void)
+{
+	u8 val;
+	int i;
+	
+	twl4030_i2c_write_u8(TWL4030_CHIP_INTBR, 0x90, TWL4030_INTBR_GPBR1);
+
+	twl4030_i2c_write_u8(TWL4030_CHIP_MADC, 0x01, 0x00);
+
+	twl4030_i2c_write_u8(TWL4030_CHIP_MADC, 0xff, 0x06);
+
+	twl4030_i2c_write_u8(TWL4030_CHIP_MADC, 0xff, 0x07);
+
+	twl4030_i2c_write_u8(TWL4030_CHIP_MADC, 0x02, 0x97);
+
+	twl4030_i2c_write_u8(TWL4030_CHIP_MADC, 0x20, 0x12);
+
+	udelay(200000);
+
+	for (i=0; i<sizeof(adc_regs)/sizeof(adc_regs[0]); ++i) {
+		u8 lsb, msb;
+		u32 adc, v;
+		
+		twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &lsb, adc_regs[i].reg_base);
+		twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &msb, adc_regs[i].reg_base+1);
+		adc = lsb / 64 + msb * 4;
+		v = (adc * adc_regs[i].mul) / 1000;
+		printf("ADC%d %s\tvalue 0x%03x = %4u mV\n", adc_regs[i].idx, adc_regs[i].str, adc, v);
+	}
+
+	twl4030_i2c_read_u8(TWL4030_CHIP_MADC, &val, 0x76);
+	
+	for (i=0; i<sizeof(charge_state)/sizeof(charge_state[0]);++i) {
+		if (val == charge_state[i].val) {
+			printf("Charge state \t\t\tvalue 0x%02x  = %s\n", val, charge_state[i].str);
+			if (charge_state[i].pr_info) {
+				print_charge_info();
+			}
+			break;
+		}
+	}
+	if (i >= sizeof(charge_state)/sizeof(charge_state[0])) {
+			printf("Charge state \t\t\tvalue 0x%02x  = %s\n", val, "Unknown charge state");
+	}
+	return 0;
+}
+
+
+int adc_cmd(cmd_tbl_t * cmdtp, int flag, int argc, char * const argv[])
+{
+	char *cmd;
+
+	if (argc < 2)
+		goto usage;
+
+	cmd = argv[1];
+	if (strcmp(cmd, "enable") == 0)
+		return twl4030_enable_charging();
+	if (strcmp(cmd, "info") == 0)
+		return twl4030_info();
+usage:
+	return cmd_usage(cmdtp);
+}
+
+U_BOOT_CMD(
+	madc, 2, 1, adc_cmd,
+	"MADC subsytem",
+	"madc enable - enable charging\n"
+	"madc info - print information on ADC registers/charging state"
+);
+#endif
diff --git a/drivers/video/Makefile b/drivers/video/Makefile
index 086dc05383..691d55602e 100644
--- a/drivers/video/Makefile
+++ b/drivers/video/Makefile
@@ -41,6 +41,8 @@ COBJS-$(CONFIG_SED156X) += sed156x.o
 COBJS-$(CONFIG_VIDEO_SM501) += sm501.o
 COBJS-$(CONFIG_VIDEO_SMI_LYNXEM) += smiLynxEM.o videomodes.o
 COBJS-$(CONFIG_VIDEO_VCXK) += bus_vcxk.o
+COBJS-$(CONFIG_VIDEO_OMAP3) += omap3_dss.o
+
 
 COBJS	:= $(COBJS-y)
 SRCS	:= $(COBJS:.o=.c)
diff --git a/drivers/video/omap3_dss.c b/drivers/video/omap3_dss.c
new file mode 100644
index 0000000000..6b33250d3a
--- /dev/null
+++ b/drivers/video/omap3_dss.c
@@ -0,0 +1,343 @@
+/*
+ * (C) Copyright 2010
+ * Texas Instruments, <www.ti.com>
+ * Syed Mohammed Khasim <khasim <at> ti.com>
+ *
+ * Referred to Linux Kernel DSS driver files for OMAP3 by
+ * Tomi Valkeinen from drivers/video/omap2/dss/
+ *
+ * See file CREDITS for list of people who contributed to this
+ * project.
+ *
+ * This program is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU General Public License as
+ * published by the Free Software Foundation's version 2 and any
+ * later version the License.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston,
+ * MA 02111-1307 USA
+ */
+
+#include <common.h>
+#include <asm/io.h>
+#include <asm/arch/dss.h>
+#include <asm/arch/sys_proto.h>
+
+#ifdef DEBUG
+#define DSS_DBG_CLK(fmt, ...) printf(fmt, ##__VA_ARGS__)
+#else
+#define DSS_DBG_CLK(fmt, ...)
+#endif
+
+/*
+ * Configure VENC for a given Mode (NTSC / PAL)
+ */
+void omap3_dss_venc_config(const struct venc_regs *venc_cfg,
+                           u32 height, u32 width)
+{
+    struct venc_regs *venc = (struct venc_regs *) OMAP3_VENC_BASE;
+    struct dss_regs *dss = (struct dss_regs *) OMAP3_DSS_BASE;
+    struct dispc_regs *dispc = (struct dispc_regs *) OMAP3_DISPC_BASE;
+
+    writel(venc_cfg->status, &venc->status);
+    writel(venc_cfg->f_control, &venc->f_control);
+    writel(venc_cfg->vidout_ctrl, &venc->vidout_ctrl);
+    writel(venc_cfg->sync_ctrl, &venc->sync_ctrl);
+    writel(venc_cfg->llen, &venc->llen);
+    writel(venc_cfg->flens, &venc->flens);
+    writel(venc_cfg->hfltr_ctrl, &venc->hfltr_ctrl);
+    writel(venc_cfg->cc_carr_wss_carr, &venc->cc_carr_wss_carr);
+    writel(venc_cfg->c_phase, &venc->c_phase);
+    writel(venc_cfg->gain_u, &venc->gain_u);
+    writel(venc_cfg->gain_v, &venc->gain_v);
+    writel(venc_cfg->gain_y, &venc->gain_y);
+    writel(venc_cfg->black_level, &venc->black_level);
+    writel(venc_cfg->blank_level, &venc->blank_level);
+    writel(venc_cfg->x_color, &venc->x_color);
+    writel(venc_cfg->m_control, &venc->m_control);
+    writel(venc_cfg->bstamp_wss_data, &venc->bstamp_wss_data);
+    writel(venc_cfg->s_carr, &venc->s_carr);
+    writel(venc_cfg->line21, &venc->line21);
+    writel(venc_cfg->ln_sel, &venc->ln_sel);
+    writel(venc_cfg->l21__wc_ctl, &venc->l21__wc_ctl);
+    writel(venc_cfg->htrigger_vtrigger, &venc->htrigger_vtrigger);
+    writel(venc_cfg->savid__eavid, &venc->savid__eavid);
+    writel(venc_cfg->flen__fal, &venc->flen__fal);
+    writel(venc_cfg->lal__phase_reset, &venc->lal__phase_reset);
+    writel(venc_cfg->hs_int_start_stop_x,
+            &venc->hs_int_start_stop_x);
+    writel(venc_cfg->hs_ext_start_stop_x,
+            &venc->hs_ext_start_stop_x);
+    writel(venc_cfg->vs_int_start_x,
+            &venc->vs_int_start_x);
+    writel(venc_cfg->vs_int_stop_x__vs_int_start_y,
+            &venc->vs_int_stop_x__vs_int_start_y);
+    writel(venc_cfg->vs_int_stop_y__vs_ext_start_x,
+            &venc->vs_int_stop_y__vs_ext_start_x);
+    writel(venc_cfg->vs_ext_stop_x__vs_ext_start_y,
+            &venc->vs_ext_stop_x__vs_ext_start_y);
+    writel(venc_cfg->vs_ext_stop_y,
+            &venc->vs_ext_stop_y);
+    writel(venc_cfg->avid_start_stop_x,
+            &venc->avid_start_stop_x);
+    writel(venc_cfg->avid_start_stop_y,
+            &venc->avid_start_stop_y);
+    writel(venc_cfg->fid_int_start_x__fid_int_start_y,
+            &venc->fid_int_start_x__fid_int_start_y);
+    writel(venc_cfg->fid_int_offset_y__fid_ext_start_x,
+            &venc->fid_int_offset_y__fid_ext_start_x);
+    writel(venc_cfg->fid_ext_start_y__fid_ext_offset_y,
+            &venc->fid_ext_start_y__fid_ext_offset_y);
+    writel(venc_cfg->tvdetgp_int_start_stop_x,
+            &venc->tvdetgp_int_start_stop_x);
+    writel(venc_cfg->tvdetgp_int_start_stop_y,
+            &venc->tvdetgp_int_start_stop_y);
+    writel(venc_cfg->gen_ctrl, &venc->gen_ctrl);
+    writel(venc_cfg->output_control, &venc->output_control);
+    writel(venc_cfg->dac_b__dac_c, &venc->dac_b__dac_c);
+
+    /* Configure DSS for VENC Settings */
+    writel(VENC_DSS_CONFIG, &dss->control);
+
+    /* Configure height and width for Digital out */
+    writel(((height << DIG_LPP_SHIFT) | width), &dispc->size_dig);
+}
+
+/*
+ * Configure Panel Specific Parameters
+ */
+void omap3_dss_panel_config(const struct panel_config *panel_cfg)
+{
+	struct prcm *prcm_base = (struct prcm *)PRCM_BASE;
+    struct dispc_regs *dispc = (struct dispc_regs *) OMAP3_DISPC_BASE;
+    int ret;
+    u32 divisor, cm_clksel_dss;
+    u32 fck_div;
+
+    /* Calculate timing of DISPC_DIVISOR; LCD in 16:23, PCD in 0:7 */
+    ret = omap3_dss_calc_divisor(panel_cfg->panel_type == 1,
+			    panel_cfg->pixel_clock * 1000, &divisor, &fck_div);
+    DSS_DBG_CLK("%s: Need to program CM_CLKSEL_DSS:clksel_dss1 to %u !\n", __FUNCTION__, fck_div);
+    cm_clksel_dss = readl(&prcm_base->clksel_dss);
+    DSS_DBG_CLK("%s: &cm_clksel_dss %p val %#x\n", __FUNCTION__, &prcm_base->clksel_dss, cm_clksel_dss);
+    cm_clksel_dss &= ~0x3f;  /* clear CLKSELDSS1 */
+    cm_clksel_dss |= fck_div; /* or in new clksel_dss1 */
+    writel(cm_clksel_dss, &prcm_base->clksel_dss);
+    DSS_DBG_CLK("%s: cm_clksel_dss %#x\n", __FUNCTION__, cm_clksel_dss);
+
+    writel(panel_cfg->timing_h, &dispc->timing_h);
+    writel(panel_cfg->timing_v, &dispc->timing_v);
+    writel(panel_cfg->pol_freq, &dispc->pol_freq);
+#if 1
+    writel(divisor, &dispc->divisor);
+#else
+    writel(panel_cfg->divisor, &dispc->divisor);
+#endif
+    writel(panel_cfg->lcd_size, &dispc->size_lcd);
+    writel((panel_cfg->load_mode << FRAME_MODE_SHIFT), &dispc->config);
+    writel(((panel_cfg->panel_type << TFTSTN_SHIFT) |
+            (panel_cfg->data_lines << DATALINES_SHIFT)), &dispc->control);
+    writel(panel_cfg->panel_color, &dispc->default_color0);
+}
+
+struct dss_clock_info {
+	/* rates that we get with dividers below */
+	unsigned long fck;
+
+	/* dividers */
+	u16 fck_div;
+};
+
+struct dispc_clock_info {
+	/* rates that we get with dividers below */
+	unsigned long lck;
+	unsigned long pck;
+
+	/* dividers */
+	u16 lck_div;
+	u16 pck_div;
+};
+
+static inline int abs(int x)
+{
+	if (x < 0)
+		return -x;
+	return x;
+}
+
+/* with fck as input clock rate, find dispc dividers that produce req_pck */
+void dispc_find_clk_divs(int is_tft, unsigned long req_pck, unsigned long fck,
+		struct dispc_clock_info *cinfo)
+{
+	u16 pcd_min = is_tft ? 2 : 3;
+	unsigned long best_pck;
+	u16 best_ld, cur_ld;
+	u16 best_pd, cur_pd;
+
+	best_pck = 0;
+	best_ld = 0;
+	best_pd = 0;
+
+	for (cur_ld = 1; cur_ld <= 255; ++cur_ld) {
+		unsigned long lck = fck / cur_ld;
+
+		for (cur_pd = pcd_min; cur_pd <= 255; ++cur_pd) {
+			unsigned long pck = lck / cur_pd;
+			long old_delta, new_delta;
+
+			old_delta = abs(best_pck - req_pck);
+			new_delta = abs(pck - req_pck);
+
+			if (best_pck == 0 || new_delta < old_delta) {
+				DSS_DBG_CLK("%s: cur_ld %u cur_pd %u old_delta %ld new_delta %ld\n", __FUNCTION__, cur_ld, cur_pd, old_delta, new_delta);
+
+				best_pck = pck;
+				best_ld = cur_ld;
+				best_pd = cur_pd;
+
+				DSS_DBG_CLK("%s: best_ld %u best_pd %u\n", __FUNCTION__, best_ld, best_pd);
+
+				if (pck == req_pck)
+					goto found;
+			}
+
+			if (pck < req_pck)
+				break;
+		}
+
+		if (lck / pcd_min < req_pck)
+			break;
+	}
+
+found:
+	cinfo->lck_div = best_ld;
+	cinfo->pck_div = best_pd;
+	cinfo->lck = fck / cinfo->lck_div;
+	cinfo->pck = cinfo->lck / cinfo->pck_div;
+	DSS_DBG_CLK("%s: %d best_ld %u best_pd %u pck %lu\n", __FUNCTION__, __LINE__, best_ld, best_pd, cinfo->pck);
+}
+
+int omap3_dss_calc_divisor(int is_tft, unsigned int req_pck,
+			unsigned int *dispc_divisor,
+			unsigned int *result_fck_div)
+{
+	unsigned long prate;
+	u16 fck_div, fck_div_max, fck_min_div = 1, fck_div_factor;
+	int min_fck_per_pck;
+	unsigned long fck, max_dss_fck = 173000000; /* max DSS VP_CLK */
+	u32 cpu_family = get_cpu_family();
+	struct dispc_clock_info cur_dispc;
+	struct dss_clock_info best_dss;
+	struct dispc_clock_info best_dispc;
+
+	prate = 864000000; /* Fclk of DSS (864Mhz) */
+
+	memset(&best_dss, 0, sizeof(best_dss));
+	memset(&best_dispc, 0, sizeof(best_dispc));
+
+	min_fck_per_pck = 1;
+
+#ifdef CONFIG_OMAP44XX	
+	fck_div_max = 32;
+	fck_div_factor = 2;
+#else
+	fck_div_max = 16;
+	fck_div_factor = 1;
+#endif
+
+	for (fck_div = fck_div_max; fck_div >= fck_min_div; --fck_div) {
+		DSS_DBG_CLK("%s:%d fck_div %d\n", __FUNCTION__, __LINE__, fck_div);
+		fck = prate / fck_div * fck_div_factor;
+
+		if (fck > max_dss_fck) {
+			DSS_DBG_CLK("%s:%d fck %lu > max_dss_fck %lu\n", __FUNCTION__, __LINE__, fck, max_dss_fck);
+			continue;
+		}
+
+		if (min_fck_per_pck &&
+			fck < req_pck * min_fck_per_pck) {
+			DSS_DBG_CLK("%s:%d\n", __FUNCTION__, __LINE__);
+			continue;
+		}
+
+		dispc_find_clk_divs(is_tft, req_pck, fck, &cur_dispc);
+
+		DSS_DBG_CLK("%s:%d cur.pck %u < best_pck %u?\n", __FUNCTION__, __LINE__,
+			abs(cur_dispc.pck - req_pck),
+			abs(best_dispc.pck - req_pck));
+
+		if (abs(cur_dispc.pck - req_pck) <
+			abs(best_dispc.pck - req_pck)) {
+
+			DSS_DBG_CLK("%s:%d yes fck %lu fck_div %u\n", __FUNCTION__, __LINE__, fck, fck_div);
+			best_dss.fck = fck;
+			best_dss.fck_div = fck_div;
+
+			best_dispc = cur_dispc;
+
+			if (cur_dispc.pck == req_pck)
+				break;
+		}
+	}
+
+	/* Setup divisor */
+	*dispc_divisor = (cur_dispc.lck_div << 16) | cur_dispc.pck_div;
+	DSS_DBG_CLK("%s: fck_div %u best_dss.fck_div %u\n", __FUNCTION__, fck_div, best_dss.fck_div); 
+	*result_fck_div = best_dss.fck_div;
+	return 0;
+}
+
+/*
+ * Enable LCD and DIGITAL OUT in DSS
+ */
+void omap3_dss_enable(void)
+{
+    struct dispc_regs *dispc = (struct dispc_regs *) OMAP3_DISPC_BASE;
+    u32 l = 0;
+
+    l = readl(&dispc->control);
+    l |= DISPC_ENABLE;
+    writel(l, &dispc->control);
+}
+
+#ifdef CONFIG_DSS_DUMP
+
+#define DUMP_IT(offset) printf("%p = %08x " #offset "\n", &dispc->offset, readl(&dispc->offset))
+
+void omap3_dss_dump(void)
+{
+	struct dispc_regs *dispc = (struct dispc_regs *) OMAP3_DISPC_BASE;
+	struct prcm *prcm_base = (struct prcm *)PRCM_BASE;
+
+	DUMP_IT(control);
+	DUMP_IT(config);
+	DUMP_IT(timing_h);
+	DUMP_IT(timing_v);
+	DUMP_IT(pol_freq);
+	DUMP_IT(divisor);
+	DUMP_IT(size_lcd);
+	DUMP_IT(gfx_attributes);
+	DUMP_IT(default_color0);
+	printf("%p = %08x cd_clksel_dss\n", &prcm_base->clksel_dss, readl(&prcm_base->clksel_dss));
+}
+
+static int do_dss_dump (cmd_tbl_t * cmdtp, int flag, int argc, char * const argv[])
+{
+	omap3_dss_dump();
+	return (0);
+}
+
+U_BOOT_CMD(
+	dss_dump,	1,	1,	do_dss_dump,
+	"dump DSS registers",
+	""
+);
+
+#endif