diff --git a/board/kmc/kzm9g/kzm9g.c b/board/kmc/kzm9g/kzm9g.c
index 525c97aea6f57885b4787fe7b4e90143dfa2395f..54f25e097cbc4868f637f6c94d0883e6c4c42674 100644
--- a/board/kmc/kzm9g/kzm9g.c
+++ b/board/kmc/kzm9g/kzm9g.c
@@ -43,6 +43,7 @@ DECLARE_GLOBAL_DATA_PTR;
 #define SMSTPCR1_CMT0	(1 << 24)
 #define SMSTPCR1_I2C0	(1 << 16)
 #define SMSTPCR3_USB	(1 << 22)
+#define SMSTPCR3_I2C1	(1 << 23)
 
 #define PORT32CR (0xE6051020)
 #define PORT33CR (0xE6051021)
@@ -287,8 +288,8 @@ int board_early_init_f(void)
 
 	clrbits_le32(&cpg->smstpcr1, (SMSTPCR1_CMT0|SMSTPCR1_I2C0));
 	clrbits_le32(&cpg_srcr->srcr1, (SMSTPCR1_CMT0|SMSTPCR1_I2C0));
-	clrbits_le32(&cpg->smstpcr3, SMSTPCR3_USB);
-	clrbits_le32(&cpg_srcr->srcr3, SMSTPCR3_USB);
+	clrbits_le32(&cpg->smstpcr3, (SMSTPCR3_USB|SMSTPCR3_I2C1));
+	clrbits_le32(&cpg_srcr->srcr3, (SMSTPCR3_USB|SMSTPCR3_I2C1));
 	writel(VCLKCR1_D, &cpg->vclkcr1);
 
 	/* Setup SCIF4 / workaround */
@@ -343,6 +344,8 @@ int board_init(void)
 	gpio_direction_output(GPIO_PORT15, 1);
 
 	/* I2C */
+	gpio_request(GPIO_FN_PORT237_I2C_SCL2, NULL);
+	gpio_request(GPIO_FN_PORT236_I2C_SDA2, NULL);
 	gpio_request(GPIO_FN_PORT27_I2C_SCL3, NULL);
 	gpio_request(GPIO_FN_PORT28_I2C_SDA3, NULL);
 
diff --git a/common/cmd_i2c.c b/common/cmd_i2c.c
index 795814d88f461d08c20804159baa6349e23710e5..82e63e13224cb6c45d129585a4dd3de59f69ae2c 100644
--- a/common/cmd_i2c.c
+++ b/common/cmd_i2c.c
@@ -223,6 +223,54 @@ static int do_i2c_read ( cmd_tbl_t *cmdtp, int flag, int argc, char * const argv
 	return 0;
 }
 
+static int do_i2c_write(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	u_char	chip;
+	uint	devaddr, alen, length;
+	u_char  *memaddr;
+
+	if (argc != 5)
+		return cmd_usage(cmdtp);
+
+	/*
+	 * memaddr is the address where to store things in memory
+	 */
+	memaddr = (u_char *)simple_strtoul(argv[1], NULL, 16);
+
+	/*
+	 * I2C chip address
+	 */
+	chip = simple_strtoul(argv[2], NULL, 16);
+
+	/*
+	 * I2C data address within the chip.  This can be 1 or
+	 * 2 bytes long.  Some day it might be 3 bytes long :-).
+	 */
+	devaddr = simple_strtoul(argv[3], NULL, 16);
+	alen = get_alen(argv[3]);
+	if (alen > 3)
+		return cmd_usage(cmdtp);
+
+	/*
+	 * Length is the number of objects, not number of bytes.
+	 */
+	length = simple_strtoul(argv[4], NULL, 16);
+
+	while (length-- > 0) {
+		if (i2c_write(chip, devaddr++, alen, memaddr++, 1) != 0) {
+			puts("Error writing to the chip.\n");
+			return 1;
+		}
+/*
+ * No write delay with FRAM devices.
+ */
+#if !defined(CONFIG_SYS_I2C_FRAM)
+		udelay(11000);
+#endif
+	}
+	return 0;
+}
+
 /*
  * Syntax:
  *	i2c md {i2c_chip} {addr}{.0, .1, .2} {len}
@@ -557,18 +605,28 @@ mod_i2c_mem(cmd_tbl_t *cmdtp, int incrflag, int flag, int argc, char * const arg
 
 /*
  * Syntax:
- *	i2c probe {addr}{.0, .1, .2}
+ *	i2c probe {addr}
+ *
+ * Returns zero (success) if one or more I2C devices was found
  */
 static int do_i2c_probe (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
 {
 	int j;
+	int addr = -1;
+	int found = 0;
 #if defined(CONFIG_SYS_I2C_NOPROBES)
 	int k, skip;
 	uchar bus = GET_BUS_NUM;
 #endif	/* NOPROBES */
 
+	if (argc == 2)
+		addr = simple_strtol(argv[1], 0, 16);
+
 	puts ("Valid chip addresses:");
 	for (j = 0; j < 128; j++) {
+		if ((0 <= addr) && (j != addr))
+			continue;
+
 #if defined(CONFIG_SYS_I2C_NOPROBES)
 		skip = 0;
 		for (k=0; k < NUM_ELEMENTS_NOPROBE; k++) {
@@ -580,8 +638,10 @@ static int do_i2c_probe (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv
 		if (skip)
 			continue;
 #endif
-		if (i2c_probe(j) == 0)
+		if (i2c_probe(j) == 0) {
 			printf(" %02X", j);
+			found++;
+		}
 	}
 	putc ('\n');
 
@@ -594,7 +654,7 @@ static int do_i2c_probe (cmd_tbl_t *cmdtp, int flag, int argc, char * const argv
 	putc ('\n');
 #endif
 
-	return 0;
+	return (0 == found);
 }
 
 /*
@@ -1282,6 +1342,7 @@ static cmd_tbl_t cmd_i2c_sub[] = {
 	U_BOOT_CMD_MKENT(nm, 2, 1, do_i2c_nm, "", ""),
 	U_BOOT_CMD_MKENT(probe, 0, 1, do_i2c_probe, "", ""),
 	U_BOOT_CMD_MKENT(read, 5, 1, do_i2c_read, "", ""),
+	U_BOOT_CMD_MKENT(write, 5, 0, do_i2c_write, "", ""),
 	U_BOOT_CMD_MKENT(reset, 0, 1, do_i2c_reset, "", ""),
 #if defined(CONFIG_CMD_SDRAM)
 	U_BOOT_CMD_MKENT(sdram, 1, 1, do_sdram, "", ""),
@@ -1331,8 +1392,9 @@ U_BOOT_CMD(
 	"i2c mm chip address[.0, .1, .2] - write to I2C device (auto-incrementing)\n"
 	"i2c mw chip address[.0, .1, .2] value [count] - write to I2C device (fill)\n"
 	"i2c nm chip address[.0, .1, .2] - write to I2C device (constant address)\n"
-	"i2c probe - show devices on the I2C bus\n"
+	"i2c probe [address] - test for and show device(s) on the I2C bus\n"
 	"i2c read chip address[.0, .1, .2] length memaddress - read to memory \n"
+	"i2c write memaddress chip address[.0, .1, .2] length - write memory to i2c\n"
 	"i2c reset - re-init the I2C Controller\n"
 #if defined(CONFIG_CMD_SDRAM)
 	"i2c sdram chip - print SDRAM configuration information\n"
diff --git a/drivers/i2c/sh_i2c.c b/drivers/i2c/sh_i2c.c
index 3147123bba878093d91d91547faacf067ecb628a..44ba90ef57a76e610db4ff01df4dc148e1f7646a 100644
--- a/drivers/i2c/sh_i2c.c
+++ b/drivers/i2c/sh_i2c.c
@@ -43,12 +43,18 @@ static struct sh_i2c *base;
 #define SH_I2C_ICCR_SCP		(1 << 0)
 
 /* ICSR / ICIC */
-#define SH_IC_BUSY	(1 << 3)
+#define SH_IC_BUSY	(1 << 4)
 #define SH_IC_TACK	(1 << 2)
 #define SH_IC_WAIT	(1 << 1)
 #define SH_IC_DTE	(1 << 0)
 
-static u8 iccl, icch;
+#ifdef CONFIG_SH_I2C_8BIT
+/* store 8th bit of iccl and icch in ICIC register */
+#define SH_I2C_ICIC_ICCLB8	(1 << 7)
+#define SH_I2C_ICIC_ICCHB8	(1 << 6)
+#endif
+
+static u16 iccl, icch;
 
 #define IRQ_WAIT 1000
 
@@ -63,6 +69,20 @@ static void irq_dte(struct sh_i2c *base)
 	}
 }
 
+static int irq_dte_with_tack(struct sh_i2c *base)
+{
+	int i;
+
+	for (i = 0 ; i < IRQ_WAIT ; i++) {
+		if (SH_IC_DTE & readb(&base->icsr))
+			break;
+		if (SH_IC_TACK & readb(&base->icsr))
+			return -1;
+		udelay(10);
+	}
+	return 0;
+}
+
 static void irq_busy(struct sh_i2c *base)
 {
 	int i;
@@ -74,71 +94,97 @@ static void irq_busy(struct sh_i2c *base)
 	}
 }
 
-static void i2c_set_addr(struct sh_i2c *base, u8 id, u8 reg, int stop)
+static int i2c_set_addr(struct sh_i2c *base, u8 id, u8 reg, int stop)
 {
-	writeb(readb(&base->iccr) & ~SH_I2C_ICCR_ICE, &base->iccr);
-	writeb(readb(&base->iccr) | SH_I2C_ICCR_ICE, &base->iccr);
-
-	writeb(iccl, &base->iccl);
-	writeb(icch, &base->icch);
-	writeb(0, &base->icic);
+	u8 icic = SH_IC_TACK;
+
+	clrbits_8(&base->iccr, SH_I2C_ICCR_ICE);
+	setbits_8(&base->iccr, SH_I2C_ICCR_ICE);
+
+	writeb(iccl & 0xff, &base->iccl);
+	writeb(icch & 0xff, &base->icch);
+#ifdef CONFIG_SH_I2C_8BIT
+	if (iccl > 0xff)
+		icic |= SH_I2C_ICIC_ICCLB8;
+	if (icch > 0xff)
+		icic |= SH_I2C_ICIC_ICCHB8;
+#endif
+	writeb(icic, &base->icic);
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RTS|SH_I2C_ICCR_BUSY), &base->iccr);
 	irq_dte(base);
 
+	clrbits_8(&base->icsr, SH_IC_TACK);
 	writeb(id << 1, &base->icdr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		return -1;
 
 	writeb(reg, &base->icdr);
 	if (stop)
 		writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RTS), &base->iccr);
 
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		return -1;
+	return 0;
 }
 
 static void i2c_finish(struct sh_i2c *base)
 {
 	writeb(0, &base->icsr);
-	writeb(readb(&base->iccr) & ~SH_I2C_ICCR_ICE, &base->iccr);
+	clrbits_8(&base->iccr, SH_I2C_ICCR_ICE);
 }
 
-static void i2c_raw_write(struct sh_i2c *base, u8 id, u8 reg, u8 val)
+static int i2c_raw_write(struct sh_i2c *base, u8 id, u8 reg, u8 val)
 {
-	i2c_set_addr(base, id, reg, 0);
+	int ret = -1;
+	if (i2c_set_addr(base, id, reg, 0) != 0)
+		goto exit0;
 	udelay(10);
 
 	writeb(val, &base->icdr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 
 	writeb((SH_I2C_ICCR_ICE | SH_I2C_ICCR_RTS), &base->iccr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 	irq_busy(base);
-
+	ret = 0;
+exit0:
 	i2c_finish(base);
+	return ret;
 }
 
-static u8 i2c_raw_read(struct sh_i2c *base, u8 id, u8 reg)
+static int i2c_raw_read(struct sh_i2c *base, u8 id, u8 reg)
 {
-	u8 ret;
-
-	i2c_set_addr(base, id, reg, 1);
+	int ret = -1;
+
+#if defined(CONFIG_SH73A0)
+	if (i2c_set_addr(base, id, reg, 0) != 0)
+		goto exit0;
+#else
+	if (i2c_set_addr(base, id, reg, 1) != 0)
+		goto exit0;
 	udelay(100);
+#endif
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RTS|SH_I2C_ICCR_BUSY), &base->iccr);
 	irq_dte(base);
 
 	writeb(id << 1 | 0x01, &base->icdr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_SCP), &base->iccr);
-	irq_dte(base);
+	if (irq_dte_with_tack(base) != 0)
+		goto exit0;
 
-	ret = readb(&base->icdr);
+	ret = readb(&base->icdr) & 0xff;
 
 	writeb((SH_I2C_ICCR_ICE|SH_I2C_ICCR_RACK), &base->iccr);
 	readb(&base->icdr); /* Dummy read */
 	irq_busy(base);
-
+exit0:
 	i2c_finish(base);
 
 	return ret;
@@ -166,6 +212,21 @@ int i2c_set_bus_num(unsigned int bus)
 	case 1:
 		base = (void *)CONFIG_SH_I2C_BASE1;
 		break;
+#ifdef CONFIG_SH_I2C_BASE2
+	case 2:
+		base = (void *)CONFIG_SH_I2C_BASE2;
+		break;
+#endif
+#ifdef CONFIG_SH_I2C_BASE3
+	case 3:
+		base = (void *)CONFIG_SH_I2C_BASE3;
+		break;
+#endif
+#ifdef CONFIG_SH_I2C_BASE4
+	case 4:
+		base = (void *)CONFIG_SH_I2C_BASE4;
+		break;
+#endif
 	default:
 		return -1;
 	}
@@ -206,18 +267,18 @@ void i2c_init(int speed, int slaveaddr)
 	denom = speed * (CONFIG_SH_I2C_DATA_HIGH + CONFIG_SH_I2C_DATA_LOW);
 	tmp = num * 10 / denom;
 	if (tmp % 10 >= 5)
-		iccl = (u8)((num/denom) + 1);
+		iccl = (u16)((num/denom) + 1);
 	else
-		iccl = (u8)(num/denom);
+		iccl = (u16)(num/denom);
 
 	/* Calculate the value for icch. From the data sheet:
 	   icch = (p clock / transfer rate) * (H / (L + H)) */
 	num = CONFIG_SH_I2C_CLOCK * CONFIG_SH_I2C_DATA_HIGH;
 	tmp = num * 10 / denom;
 	if (tmp % 10 >= 5)
-		icch = (u8)((num/denom) + 1);
+		icch = (u16)((num/denom) + 1);
 	else
-		icch = (u8)(num/denom);
+		icch = (u16)(num/denom);
 }
 
 /*
@@ -235,10 +296,14 @@ void i2c_init(int speed, int slaveaddr)
  */
 int i2c_read(u8 chip, u32 addr, int alen, u8 *buffer, int len)
 {
+	int ret;
 	int i = 0;
-	for (i = 0 ; i < len ; i++)
-		buffer[i] = i2c_raw_read(base, chip, addr + i);
-
+	for (i = 0 ; i < len ; i++) {
+		ret = i2c_raw_read(base, chip, addr + i);
+		if (ret < 0)
+			return -1;
+		buffer[i] = ret & 0xff;
+	}
 	return 0;
 }
 
@@ -259,8 +324,8 @@ int i2c_write(u8 chip, u32 addr, int alen, u8 *buffer, int len)
 {
 	int i = 0;
 	for (i = 0; i < len ; i++)
-		i2c_raw_write(base, chip, addr + i, buffer[i]);
-
+		if (i2c_raw_write(base, chip, addr + i, buffer[i]) != 0)
+			return -1;
 	return 0;
 }
 
@@ -272,5 +337,9 @@ int i2c_write(u8 chip, u32 addr, int alen, u8 *buffer, int len)
  */
 int i2c_probe(u8 chip)
 {
-	return 0;
+	int ret;
+
+	ret = i2c_set_addr(base, chip, 0, 1);
+	i2c_finish(base);
+	return ret;
 }
diff --git a/include/configs/kzm9g.h b/include/configs/kzm9g.h
index 3a882e396b2849dee202f3cbaee22cb5275b2a9b..4898fb60f4b66e37f5c317b37d66df210cbdba8e 100644
--- a/include/configs/kzm9g.h
+++ b/include/configs/kzm9g.h
@@ -154,16 +154,20 @@
 /* I2C */
 #define CONFIG_CMD_I2C
 #define CONFIG_SH_I2C 1
+#define CONFIG_SH_I2C_8BIT
 #define CONFIG_HARD_I2C
 #define CONFIG_I2C_MULTI_BUS
-#define CONFIG_SYS_MAX_I2C_BUS  (2)
+#define CONFIG_SYS_MAX_I2C_BUS  (5)
 #define CONFIG_SYS_I2C_MODULE
 #define CONFIG_SYS_I2C_SPEED    (100000) /* 100 kHz */
 #define CONFIG_SYS_I2C_SLAVE    (0x7F)
 #define CONFIG_SH_I2C_DATA_HIGH (4)
 #define CONFIG_SH_I2C_DATA_LOW  (5)
-#define CONFIG_SH_I2C_CLOCK     (41666666)
+#define CONFIG_SH_I2C_CLOCK     (104000000) /* 104 MHz */
 #define CONFIG_SH_I2C_BASE0     (0xE6820000)
 #define CONFIG_SH_I2C_BASE1     (0xE6822000)
+#define CONFIG_SH_I2C_BASE2     (0xE6824000)
+#define CONFIG_SH_I2C_BASE3     (0xE6826000)
+#define CONFIG_SH_I2C_BASE4     (0xE6828000)
 
 #endif /* __KZM9G_H */