video: Add S3C24xx framebuffer support

index 9231927..98d10e3 100644
@@ -135,7 +135,7 @@
 #endif
 #endif
 
-#ifdef CONFIG_VIDEO_MXS
+#if defined(CONFIG_VIDEO_MXS) || defined(CONFIG_VIDEO_S3C)
 #define VIDEO_FB_16BPP_WORD_SWAP
 #endif
 

172 lines not on this page · raw message

index d40185e..a2ce7c5 100644
@@ -201,7 +201,7 @@
 /* NAND configuration */
 #ifdef CONFIG_CMD_NAND
 #define CONFIG_NAND_S3C2410
-#define CONFIG_SYS_S3C2410_NAND_HWECC
+#define CONFIG_S3C2410_NAND_HWECC
 #define CONFIG_SYS_MAX_NAND_DEVICE	1
 #define CONFIG_SYS_NAND_BASE		0x4E000000
 #define CONFIG_S3C24XX_CUSTOM_NAND_TIMING

index d4ae19f..0d0da28 100644
@@ -172,7 +172,7 @@
  */
 #ifdef CONFIG_CMD_NAND
 #define CONFIG_NAND_S3C2410
-#define CONFIG_SYS_S3C2410_NAND_HWECC
+#define CONFIG_S3C2410_NAND_HWECC
 #define CONFIG_SYS_MAX_NAND_DEVICE	1
 #define CONFIG_SYS_NAND_BASE		0x4E000000
 #endif

index db87d07..399f2bc 100644
@@ -43,7 +43,7 @@ static void s3c2410_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 	struct nand_chip *chip = mtd->priv;
 	struct s3c2410_nand *nand = s3c2410_get_base_nand();
 
-	debug("hwcontrol(): 0x%02x 0x%02x\n", cmd, ctrl);
+	debug("hwcontrol(): 0x%02x 0x%08x\n", cmd & 0xff, ctrl);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
 		ulong IO_ADDR_W = (ulong)nand;

index ce4186f..8773ce3 100644
@@ -83,9 +83,9 @@ static inline struct s3c24x0_lcd *s3c24x0_get_base_lcd(void)
 	return (struct s3c24x0_lcd *)S3C24X0_LCD_BASE;
 }
 
-static inline struct s3c2410_nand *s3c2410_get_base_nand(void)
+static inline struct s3c24x0_nand *s3c24x0_get_base_nand(void)
 {
-	return (struct s3c2410_nand *)S3C2410_NAND_BASE;
+	return (struct s3c24x0_nand *)S3C2410_NAND_BASE;
 }
 
 static inline struct s3c24x0_uart

index 3f44bdc..7a525f2 100644
@@ -81,9 +81,9 @@ static inline struct s3c24x0_lcd *s3c24x0_get_base_lcd(void)
 	return (struct s3c24x0_lcd *)S3C24X0_LCD_BASE;
 }
 
-static inline struct s3c2440_nand *s3c2440_get_base_nand(void)
+static inline struct s3c24x0_nand *s3c24x0_get_base_nand(void)
 {
-	return (struct s3c2440_nand *)S3C2440_NAND_BASE;
+	return (struct s3c24x0_nand *)S3C2440_NAND_BASE;
 }
 
 static inline struct s3c24x0_uart

index ed9df34..2dae9fc 100644
@@ -135,34 +135,33 @@ struct s3c24x0_lcd {
 };
 
 
-#ifdef CONFIG_S3C2410
-/* NAND FLASH (see S3C2410 manual chapter 6) */
-struct s3c2410_nand {
-	u32	nfconf;
-	u32	nfcmd;
-	u32	nfaddr;
-	u32	nfdata;
-	u32	nfstat;
-	u32	nfecc;
-};
-#endif
-#ifdef CONFIG_S3C2440
-/* NAND FLASH (see S3C2440 manual chapter 6) */
-struct s3c2440_nand {
+/* NAND FLASH (see manual chapter 6) */
+struct s3c24x0_nand {
 	u32	nfconf;
+#ifndef CONFIG_S3C2410
 	u32	nfcont;
+#endif
 	u32	nfcmd;
 	u32	nfaddr;
 	u32	nfdata;
+#ifndef CONFIG_S3C2410
 	u32	nfeccd0;
 	u32	nfeccd1;
 	u32	nfeccd;
+#endif
 	u32	nfstat;
+#ifdef CONFIG_S3C2410
+	u32	nfecc;
+#else
 	u32	nfstat0;
 	u32	nfstat1;
-};
+	u32	nfmecc0;
+	u32	nfmecc1;
+	u32	nfsecc;
+	u32	nfsblk;
+	u32	nfeblk;
 #endif
-
+};
 
 /* UART (see manual chapter 11) */
 struct s3c24x0_uart {

index 399f2bc..8db432f 100644
@@ -38,10 +38,10 @@ static void nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 }
 #endif
 
-static void s3c2410_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
+static void s3c24x0_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
 	struct nand_chip *chip = mtd->priv;
-	struct s3c2410_nand *nand = s3c2410_get_base_nand();
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
 
 	debug("hwcontrol(): 0x%02x 0x%08x\n", cmd & 0xff, ctrl);
 

@@ -67,35 +67,35 @@ static void s3c2410_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 		writeb(cmd, chip->IO_ADDR_W);
 }
 
-static int s3c2410_dev_ready(struct mtd_info *mtd)
+static int s3c24x0_dev_ready(struct mtd_info *mtd)
 {
-	struct s3c2410_nand *nand = s3c2410_get_base_nand();
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
 	debug("dev_ready\n");
 	return readl(&nand->nfstat) & 0x01;
 }
 
 #ifdef CONFIG_S3C2410_NAND_HWECC
-void s3c2410_nand_enable_hwecc(struct mtd_info *mtd, int mode)
+void s3c24x0_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 {
-	struct s3c2410_nand *nand = s3c2410_get_base_nand();
-	debug("s3c2410_nand_enable_hwecc(%p, %d)\n", mtd, mode);
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
+	debug("s3c24x0_nand_enable_hwecc(%p, %d)\n", mtd, mode);
 	writel(readl(&nand->nfconf) | S3C2410_NFCONF_INITECC, &nand->nfconf);
 }
 
-static int s3c2410_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
+static int s3c24x0_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 				      u_char *ecc_code)
 {
-	struct s3c2410_nand *nand = s3c2410_get_base_nand();
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
 	ecc_code[0] = readb(&nand->nfecc);
 	ecc_code[1] = readb(&nand->nfecc + 1);
 	ecc_code[2] = readb(&nand->nfecc + 2);
-	debug("s3c2410_nand_calculate_hwecc(%p,): 0x%02x 0x%02x 0x%02x\n",
-	       mtd , ecc_code[0], ecc_code[1], ecc_code[2]);
+	debug("s3c24x0_nand_calculate_hwecc(%p,): 0x%02x 0x%02x 0x%02x\n",
+	      mtd , ecc_code[0], ecc_code[1], ecc_code[2]);
 
 	return 0;
 }
 
-static int s3c2410_nand_correct_data(struct mtd_info *mtd, u_char *dat,
+static int s3c24x0_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				     u_char *read_ecc, u_char *calc_ecc)
 {
 	if (read_ecc[0] == calc_ecc[0] &&

@@ -103,7 +103,7 @@ static int s3c2410_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 	    read_ecc[2] == calc_ecc[2])
 		return 0;
 
-	printf("s3c2410_nand_correct_data: not implemented\n");
+	printf("s3c24x0_nand_correct_data: not implemented\n");
 	return -1;
 }
 #endif

@@ -113,7 +113,7 @@ int board_nand_init(struct nand_chip *nand)
 	u_int32_t cfg;
 	u_int8_t tacls, twrph0, twrph1;
 	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
-	struct s3c2410_nand *nand_reg = s3c2410_get_base_nand();
+	struct s3c24x0_nand *nand_reg = s3c24x0_get_base_nand();
 
 	debug("board_nand_init()\n");
 

@@ -149,14 +149,14 @@ int board_nand_init(struct nand_chip *nand)
 #endif
 
 	/* hwcontrol always must be implemented */
-	nand->cmd_ctrl = s3c2410_hwcontrol;
+	nand->cmd_ctrl = s3c24x0_hwcontrol;
 
-	nand->dev_ready = s3c2410_dev_ready;
+	nand->dev_ready = s3c24x0_dev_ready;
 
 #ifdef CONFIG_S3C2410_NAND_HWECC
-	nand->ecc.hwctl = s3c2410_nand_enable_hwecc;
-	nand->ecc.calculate = s3c2410_nand_calculate_ecc;
-	nand->ecc.correct = s3c2410_nand_correct_data;
+	nand->ecc.hwctl = s3c24x0_nand_enable_hwecc;
+	nand->ecc.calculate = s3c24x0_nand_calculate_ecc;
+	nand->ecc.correct = s3c24x0_nand_correct_data;
 	nand->ecc.mode = NAND_ECC_HW;
 	nand->ecc.size = CONFIG_SYS_NAND_ECCSIZE;
 	nand->ecc.bytes = CONFIG_SYS_NAND_ECCBYTES;

index 8db432f..4971342 100644
@@ -12,13 +12,22 @@
 #include <asm/io.h>
 
 #define S3C2410_NFCONF_EN          (1<<15)
+#define S3C2440_NFCONT_EN          (1<<0)
 #define S3C2410_NFCONF_512BYTE     (1<<14)
 #define S3C2410_NFCONF_4STEP       (1<<13)
 #define S3C2410_NFCONF_INITECC     (1<<12)
+#define S3C2440_NFCONT_INITECC     (1<<4)
 #define S3C2410_NFCONF_nFCE        (1<<11)
+#define S3C2440_NFCONT_nFCE        (1<<1)
+#ifdef CONFIG_S3C2410
 #define S3C2410_NFCONF_TACLS(x)    ((x)<<8)
 #define S3C2410_NFCONF_TWRPH0(x)   ((x)<<4)
 #define S3C2410_NFCONF_TWRPH1(x)   ((x)<<0)
+#else	/* S3C2412, S3C2440 */
+#define S3C2410_NFCONF_TACLS(x)    ((x)<<12)
+#define S3C2410_NFCONF_TWRPH0(x)   ((x)<<8)
+#define S3C2410_NFCONF_TWRPH1(x)   ((x)<<4)
+#endif
 
 #define S3C2410_ADDR_NALE 4
 #define S3C2410_ADDR_NCLE 8

@@ -42,25 +51,30 @@ static void s3c24x0_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)
 {
 	struct nand_chip *chip = mtd->priv;
 	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
+	uint32_t sel_reg, sel_bit;
 
 	debug("hwcontrol(): 0x%02x 0x%08x\n", cmd & 0xff, ctrl);
 
 	if (ctrl & NAND_CTRL_CHANGE) {
-		ulong IO_ADDR_W = (ulong)nand;
+		chip->IO_ADDR_W = &nand->nfconf;
 
 		if (!(ctrl & NAND_CLE))
-			IO_ADDR_W |= S3C2410_ADDR_NCLE;
+			chip->IO_ADDR_W = &nand->nfaddr;
 		if (!(ctrl & NAND_ALE))
-			IO_ADDR_W |= S3C2410_ADDR_NALE;
+			chip->IO_ADDR_W = &nand->nfcmd;
 
-		chip->IO_ADDR_W = (void *)IO_ADDR_W;
+#ifdef CONFIG_S3C2410
+		sel_reg = (uint32_t)&nand->nfconf;
+		sel_bit = S3C2410_NFCONF_nFCE;
+#else
+		sel_reg = (uint32_t)&nand->nfcont;
+		sel_bit = S3C2440_NFCONT_nFCE;
+#endif
 
 		if (ctrl & NAND_NCE)
-			writel(readl(&nand->nfconf) & ~S3C2410_NFCONF_nFCE,
-			       &nand->nfconf);
+			clrbits_le32(sel_reg, sel_bit);
 		else
-			writel(readl(&nand->nfconf) | S3C2410_NFCONF_nFCE,
-			       &nand->nfconf);
+			setbits_le32(sel_reg, sel_bit);
 	}
 
 	if (cmd != NAND_CMD_NONE)

@@ -79,16 +93,27 @@ void s3c24x0_nand_enable_hwecc(struct mtd_info *mtd, int mode)
 {
 	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
 	debug("s3c24x0_nand_enable_hwecc(%p, %d)\n", mtd, mode);
-	writel(readl(&nand->nfconf) | S3C2410_NFCONF_INITECC, &nand->nfconf);
+#ifdef CONFIG_S3C2410
+	setbits_le32(&nand->nfconf, S3C2410_NFCONF_INITECC);
+#else
+	setbits_le32(&nand->nfcont, S3C2440_NFCONT_INITECC);
+#endif
 }
 
 static int s3c24x0_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 				      u_char *ecc_code)
 {
 	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
+#ifdef CONFIG_S3C2410
 	ecc_code[0] = readb(&nand->nfecc);
 	ecc_code[1] = readb(&nand->nfecc + 1);
 	ecc_code[2] = readb(&nand->nfecc + 2);
+#else
+	uint32_t ecc = readl(&nand->nfmecc0);
+	ecc_code[0] = ecc;
+	ecc_code[1] = ecc >> 8;
+	ecc_code[2] = ecc >> 16;
+#endif
 	debug("s3c24x0_nand_calculate_hwecc(%p,): 0x%02x 0x%02x 0x%02x\n",
 	      mtd , ecc_code[0], ecc_code[1], ecc_code[2]);
 

@@ -110,14 +135,14 @@ static int s3c24x0_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 
 int board_nand_init(struct nand_chip *nand)
 {
-	u_int32_t cfg;
+	uint32_t cfg = 0;
 	u_int8_t tacls, twrph0, twrph1;
 	struct s3c24x0_clock_power *clk_power = s3c24x0_get_base_clock_power();
 	struct s3c24x0_nand *nand_reg = s3c24x0_get_base_nand();
 
 	debug("board_nand_init()\n");
 
-	writel(readl(&clk_power->clkcon) | (1 << 4), &clk_power->clkcon);
+	setbits_le32(&clk_power->clkcon, 1 << 4);
 
 	/* initialize hardware */
 #if defined(CONFIG_S3C24XX_CUSTOM_NAND_TIMING)

@@ -130,12 +155,18 @@ int board_nand_init(struct nand_chip *nand)
 	twrph1 = 8;
 #endif
 
-	cfg = S3C2410_NFCONF_EN;
+#ifdef CONFIG_S3C2410
+	cfg |= S3C2410_NFCONF_EN;
+#endif
 	cfg |= S3C2410_NFCONF_TACLS(tacls - 1);
 	cfg |= S3C2410_NFCONF_TWRPH0(twrph0 - 1);
 	cfg |= S3C2410_NFCONF_TWRPH1(twrph1 - 1);
 	writel(cfg, &nand_reg->nfconf);
 
+#ifndef CONFIG_S3C2410
+	writel(S3C2440_NFCONT_EN, &nand_reg->nfcont);
+#endif
+
 	/* initialize nand_chip data structure */
 	nand->IO_ADDR_R = (void *)&nand_reg->nfdata;
 	nand->IO_ADDR_W = (void *)&nand_reg->nfdata;

index 4971342..91db6ca 100644
@@ -10,6 +10,7 @@
 #include <nand.h>
 #include <asm/arch/s3c24x0_cpu.h>
 #include <asm/io.h>
+#include <asm/unaligned.h>
 
 #define S3C2410_NFCONF_EN          (1<<15)
 #define S3C2440_NFCONT_EN          (1<<0)

@@ -45,6 +46,39 @@ static void nand_read_buf(struct mtd_info *mtd, u_char *buf, int len)
 	for (i = 0; i < len; i++)
 		buf[i] = readb(this->IO_ADDR_R);
 }
+#elif !defined(CONFIG_S3C2410)
+static void s3c2440_read_buf(struct mtd_info *mtd, u_char *buf, int len)
+{
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
+	uint32_t data;
+
+	while (len >= 4) {
+		data = readl(&nand->nfdata);
+		put_unaligned_le32(data, buf);
+		buf += 4;
+		len -= 4;
+	}
+
+	for (; len & 3; len--)
+		*buf++ = readb(&nand->nfdata);
+}
+
+static void s3c2440_write_buf(struct mtd_info *mtd, const u_char *buf,
+			      int len)
+{
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
+	uint32_t data;
+
+	while (len >= 4) {
+		data = get_unaligned_le32(buf);
+		writel(data, &nand->nfdata);
+		buf += 4;
+		len -= 4;
+	}
+
+	for (; len & 3; len--, buf++)
+		writeb(*buf, &nand->nfdata);
+}
 #endif
 
 static void s3c24x0_hwcontrol(struct mtd_info *mtd, int cmd, unsigned int ctrl)

@@ -177,6 +211,9 @@ int board_nand_init(struct nand_chip *nand)
 	/* read_byte and write_byte are default */
 #ifdef CONFIG_NAND_SPL
 	nand->read_buf = nand_read_buf;
+#elif !defined(CONFIG_S3C2410)
+	nand->read_buf = s3c2440_read_buf;
+	nand->write_buf = s3c2440_write_buf;
 #endif
 
 	/* hwcontrol always must be implemented */

index 91db6ca..f23a1c8 100644
@@ -157,16 +157,93 @@ static int s3c24x0_nand_calculate_ecc(struct mtd_info *mtd, const u_char *dat,
 static int s3c24x0_nand_correct_data(struct mtd_info *mtd, u_char *dat,
 				     u_char *read_ecc, u_char *calc_ecc)
 {
-	if (read_ecc[0] == calc_ecc[0] &&
-	    read_ecc[1] == calc_ecc[1] &&
-	    read_ecc[2] == calc_ecc[2])
+	unsigned int diff0, diff1, diff2;
+	unsigned int bit, byte;
+
+	debug("%s(%p,%p,%p,%p)\n", __func__, mtd, dat, read_ecc, calc_ecc);
+
+	diff0 = read_ecc[0] ^ calc_ecc[0];
+	diff1 = read_ecc[1] ^ calc_ecc[1];
+	diff2 = read_ecc[2] ^ calc_ecc[2];
+
+	debug("%s: rd %*phN calc %*phN diff %02x%02x%02x\n",
+	      __func__, 3, read_ecc, 3, calc_ecc,
+	      diff0, diff1, diff2);
+
+	if (diff0 == 0 && diff1 == 0 && diff2 == 0)
+		return 0;		/* ECC is ok */
+
+	/* sometimes people do not think about using the ECC, so check
+	 * to see if we have an 0xff,0xff,0xff read ECC and then ignore
+	 * the error, on the assumption that this is an un-eccd page.
+	 */
+	if (read_ecc[0] == 0xff && read_ecc[1] == 0xff && read_ecc[2] == 0xff
+	    /*&& info->platform->ignore_unset_ecc*/)
 		return 0;
 
-	printf("s3c24x0_nand_correct_data: not implemented\n");
+	/* Can we correct this ECC (ie, one row and column change).
+	 * Note, this is similar to the 256 error code on smartmedia */
+
+	if (((diff0 ^ (diff0 >> 1)) & 0x55) == 0x55 &&
+	    ((diff1 ^ (diff1 >> 1)) & 0x55) == 0x55 &&
+	    ((diff2 ^ (diff2 >> 1)) & 0x55) == 0x55) {
+		/* calculate the bit position of the error */
+
+		bit  = ((diff2 >> 3) & 1) |
+		       ((diff2 >> 4) & 2) |
+		       ((diff2 >> 5) & 4);
+
+		/* calculate the byte position of the error */
+
+		byte = ((diff2 << 7) & 0x100) |
+		       ((diff1 << 0) & 0x80)  |
+		       ((diff1 << 1) & 0x40)  |
+		       ((diff1 << 2) & 0x20)  |
+		       ((diff1 << 3) & 0x10)  |
+		       ((diff0 >> 4) & 0x08)  |
+		       ((diff0 >> 3) & 0x04)  |
+		       ((diff0 >> 2) & 0x02)  |
+		       ((diff0 >> 1) & 0x01);
+
+		debug("correcting error bit %d, byte %d\n", bit, byte);
+
+		dat[byte] ^= (1 << bit);
+		return 1;
+	}
+
+	/* if there is only one bit difference in the ECC, then
+	 * one of only a row or column parity has changed, which
+	 * means the error is most probably in the ECC itself */
+
+	diff0 |= (diff1 << 8);
+	diff0 |= (diff2 << 16);
+
+	if ((diff0 & ~(1<<fls(diff0))) == 0)
+		return 1;
+
 	return -1;
 }
 #endif
 
+static void s3c24x0_nand_select_chip(struct mtd_info *mtd, int chip)
+{
+	struct s3c24x0_nand *nand = s3c24x0_get_base_nand();
+	uint32_t sel_reg, sel_bit;
+
+#ifdef CONFIG_S3C2410
+	sel_reg = (uint32_t)&nand->nfconf;
+	sel_bit = S3C2410_NFCONF_nFCE;
+#else
+	sel_reg = (uint32_t)&nand->nfcont;
+	sel_bit = S3C2440_NFCONT_nFCE;
+#endif
+
+	if (chip == -1)
+		setbits_le32(sel_reg, sel_bit);
+	else
+		clrbits_le32(sel_reg, sel_bit);
+}
+
 int board_nand_init(struct nand_chip *nand)
 {
 	uint32_t cfg = 0;

@@ -205,7 +282,7 @@ int board_nand_init(struct nand_chip *nand)
 	nand->IO_ADDR_R = (void *)&nand_reg->nfdata;
 	nand->IO_ADDR_W = (void *)&nand_reg->nfdata;
 
-	nand->select_chip = NULL;
+	nand->select_chip = s3c24x0_nand_select_chip;
 
 	/* read_buf and write_buf are default */
 	/* read_byte and write_byte are default */

@@ -221,6 +298,8 @@ int board_nand_init(struct nand_chip *nand)
 
 	nand->dev_ready = s3c24x0_dev_ready;
 
+	nand->chip_delay = 50;
+
 #ifdef CONFIG_S3C2410_NAND_HWECC
 	nand->ecc.hwctl = s3c24x0_nand_enable_hwecc;
 	nand->ecc.calculate = s3c24x0_nand_calculate_ecc;

index fd328f0..927cf61 100644
@@ -153,7 +153,7 @@ static int GetI2CSDA(void)
 {
 	struct s3c24x0_gpio *gpio = s3c24x0_get_base_gpio();
 
-#ifdef CONFIG_S3C2410
+#if defined(CONFIG_S3C2410) || defined(CONFIG_S3C2440)
 	return (readl(&gpio->gpedat) & 0x8000) >> 15;
 #endif
 #ifdef CONFIG_S3C2400

@@ -165,7 +165,7 @@ static void SetI2CSCL(int x)
 {
 	struct s3c24x0_gpio *gpio = s3c24x0_get_base_gpio();
 
-#ifdef CONFIG_S3C2410
+#if defined(CONFIG_S3C2410) || defined(CONFIG_S3C2440)
 	writel((readl(&gpio->gpedat) & ~0x4000) |
 					(x & 1) << 14, &gpio->gpedat);
 #endif

@@ -427,7 +427,7 @@ static void s3c24x0_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 	int i;
 
 	if ((readl(&i2c->iicstat) & I2CSTAT_BSY) || GetI2CSDA() == 0) {
-#ifdef CONFIG_S3C2410
+#if defined(CONFIG_S3C2410) || defined(CONFIG_S3C2440)
 		ulong old_gpecon = readl(&gpio->gpecon);
 #endif
 #ifdef CONFIG_S3C2400

@@ -436,7 +436,7 @@ static void s3c24x0_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 		/* bus still busy probably by (most) previously interrupted
 		   transfer */
 
-#ifdef CONFIG_S3C2410
+#if defined(CONFIG_S3C2410) || defined(CONFIG_S3C2440)
 		/* set I2CSDA and I2CSCL (GPE15, GPE14) to GPIO */
 		writel((readl(&gpio->gpecon) & ~0xF0000000) | 0x10000000,
 		       &gpio->gpecon);

@@ -462,7 +462,7 @@ static void s3c24x0_i2c_init(struct i2c_adapter *adap, int speed, int slaveadd)
 		udelay(1000);
 
 		/* restore pin functions */
-#ifdef CONFIG_S3C2410
+#if defined(CONFIG_S3C2410) || defined(CONFIG_S3C2440)
 		writel(old_gpecon, &gpio->gpecon);
 #endif
 #ifdef CONFIG_S3C2400

index e1e2d3f..d6c7eeb 100644
@@ -8,53 +8,50 @@
 #include <asm/arch/s3c2440.h>
 #include <asm/gpio.h>
 #include <asm/io.h>
+#include <errno.h>
 
 #define GPIO_INPUT  0x0
 #define GPIO_OUTPUT 0x1
 
-/* 0x4 means that we want DAT and not CON register */
-#define GPIO_PORT(x)	((((x) >> 5) & 0x3) + 0x4)
-#define GPIO_BIT(x)		((x) & 0x3f)
+#define S3C_GPIO_CON	0x0
+#define S3C_GPIO_DAT	0x4
 
-/*
- * It's how we calculate the full port address
- * We have to get the number of the port + 1 (Port A is at 0x56000001 ...)
- * We move it at the second digit, and finally we add 0x4 because we want
- * to modify GPIO DAT and not CON
- */
-#define GPIO_FULLPORT(x) (S3C24X0_GPIO_BASE | ((GPIO_PORT(gpio) + 1) << 1))
+static uint32_t s3c_gpio_get_bank_addr(unsigned gpio)
+{
+	/* There is up to 16 pins per bank, one bank is 0x10 big. */
+	uint32_t addr = gpio & ~0xf;
+
+	if (addr >= 0x80 && addr != 0xd0) {	/* Wrong GPIO bank. */
+		printf("Invalid GPIO bank (bank %02x)\n", addr);
+		return 0xffffffff;
+	}
+
+	return addr | S3C24X0_GPIO_BASE;
+}
 
 int gpio_set_value(unsigned gpio, int value)
 {
-	unsigned l = readl(GPIO_FULLPORT(gpio));
-	unsigned bit;
-	unsigned port = GPIO_FULLPORT(gpio);
-
-	/*
-	 * All GPIO Port have a configuration on
-	 * 2 bits excepted the first GPIO (A) which
-	 * have only 1 bit of configuration.
-	 */
-	if (!GPIO_PORT(gpio))
-		bit = (0x1 << GPIO_BIT(gpio));
-	else
-		bit = (0x3 << GPIO_BIT(gpio));
+	uint32_t addr = s3c_gpio_get_bank_addr(gpio);
+
+	if (addr == 0xffffffff)
+		return -EINVAL;
 
 	if (value)
-		l |= bit;
+		setbits_le32(addr | S3C_GPIO_DAT, 1 << (gpio & 0xf));
 	else
-		l &= ~bit;
+		clrbits_le32(addr | S3C_GPIO_DAT, 1 << (gpio & 0xf));
 
-	return writel(l, port);
+	return 0;
 }
 
 int gpio_get_value(unsigned gpio)
 {
-	unsigned l = readl(GPIO_FULLPORT(gpio));
+	uint32_t addr = s3c_gpio_get_bank_addr(gpio);
+
+	if (addr == 0xffffffff)
+		return -EINVAL;
 
-	if (GPIO_PORT(gpio) == 0) /* PORT A */
-		return (l >> GPIO_BIT(gpio)) & 0x1;
-	return (l >> GPIO_BIT(gpio)) & 0x3;
+	return !!(readl(addr | S3C_GPIO_DAT) & (1 << (gpio & 0xf)));
 }
 
 int gpio_request(unsigned gpio, const char *label)

@@ -67,13 +64,25 @@ int gpio_free(unsigned gpio)
 	return 0;
 }
 
+static int s3c_gpio_direction(unsigned gpio, uint8_t dir)
+{
+	uint32_t addr = s3c_gpio_get_bank_addr(gpio);
+	const uint32_t mask = 0x3 << ((gpio & 0xf) << 1);
+	const uint32_t dirm = dir << ((gpio & 0xf) << 1);
+
+	if (addr == 0xffffffff)
+		return -EINVAL;
+
+	clrsetbits_le32(addr | S3C_GPIO_CON, mask, dirm);
+	return 0;
+}
+
 int gpio_direction_input(unsigned gpio)
 {
-	return writel(GPIO_INPUT << GPIO_BIT(gpio), GPIO_FULLPORT(gpio));
+	return s3c_gpio_direction(gpio, GPIO_INPUT);
 }
 
 int gpio_direction_output(unsigned gpio, int value)
 {
-	writel(GPIO_OUTPUT << GPIO_BIT(gpio), GPIO_FULLPORT(gpio));
-	return gpio_set_value(gpio, value);
+	return s3c_gpio_direction(gpio, GPIO_OUTPUT);
 }

index b097c1a..5959672 100644
@@ -187,6 +187,7 @@ static int smc911x_send(struct eth_device *dev, void *packet, int length)
 static void smc911x_halt(struct eth_device *dev)
 {
 	smc911x_reset(dev);
+	smc911x_handle_mac_address(dev);
 }
 
 static int smc911x_rx(struct eth_device *dev)