3 files changed, 48 insertions, 38 deletions

diff --git a/src/southbridge/amd/cs5535/cs5535_early_setup.c b/src/southbridge/amd/cs5535/cs5535_early_setup.c
index 6c32498aa9..a1cab08c88 100644
--- a/src/southbridge/amd/cs5535/cs5535_early_setup.c
+++ b/src/southbridge/amd/cs5535/cs5535_early_setup.c
@@ -113,6 +113,5 @@ static int cs5535_early_setup(void)
 	cs5535_setup_cis_mode();
 	print_debug("Setup smbus\r\n");
 	cs5535_enable_smbus();
-	//get_memory_speed();
 	dummy();
 }
diff --git a/src/southbridge/amd/cs5535/cs5535_early_smbus.c b/src/southbridge/amd/cs5535/cs5535_early_smbus.c
index adc4c5e090..6ff46338f6 100644
--- a/src/southbridge/amd/cs5535/cs5535_early_smbus.c
+++ b/src/southbridge/amd/cs5535/cs5535_early_smbus.c
@@ -7,6 +7,7 @@ static int cs5535_enable_smbus(void)
 {
 	unsigned char val;
 
+	/* reset SMBUS controller */
 	outb(0, SMBUS_IO_BASE + SMB_CTRL2);
 
 	/* Set SCL freq and enable SMB controller */
@@ -18,24 +19,11 @@ static int cs5535_enable_smbus(void)
 	val = inb(SMBUS_IO_BASE + SMB_ADD);
 	val |= (0xEF | SMB_ADD_SAEN);
 	outb(val, SMBUS_IO_BASE + SMB_ADD); 
+}
 
-#if 0
-	print_debug("SMBUS registers ");
-	print_debug_hex8(inb(SMBUS_IO_BASE));
-	print_debug(" ");
-	print_debug_hex8(inb(SMBUS_IO_BASE + 1));
-	print_debug(" ");
-	print_debug_hex8(inb(SMBUS_IO_BASE + 2));
-	print_debug(" ");
-	print_debug_hex8(inb(SMBUS_IO_BASE + 3));
-	print_debug(" ");
-	print_debug_hex8(inb(SMBUS_IO_BASE + 4));
-	print_debug(" ");
-	print_debug_hex8(inb(SMBUS_IO_BASE + 5));
-	print_debug(" ");
-	print_debug_hex8(inb(SMBUS_IO_BASE + 6));
-	print_debug("\n\r");
-#endif
+static int smbus_read_byte(unsigned device, unsigned address)
+{
+        return do_smbus_read_byte(SMBUS_IO_BASE, device, address-1);
 }
 
 #if 0
@@ -49,10 +37,6 @@ static int smbus_send_byte(unsigned device, unsigned char val)
         return do_smbus_send_byte(SMBUS_IO_BASE, device, val);
 }
 
-static int smbus_read_byte(unsigned device, unsigned address)
-{
-        return do_smbus_read_byte(SMBUS_IO_BASE, device, address);
-}
 
 static int smbus_write_byte(unsigned device, unsigned address, unsigned char val)
 {
diff --git a/src/southbridge/amd/cs5535/cs5535_smbus.h b/src/southbridge/amd/cs5535/cs5535_smbus.h
index 2504f5dbef..9cf55ba29c 100644
--- a/src/southbridge/amd/cs5535/cs5535_smbus.h
+++ b/src/southbridge/amd/cs5535/cs5535_smbus.h
@@ -43,6 +43,8 @@
 #define SMBUS_TIMEOUT (100*1000*10)
 #define SMBUS_STATUS_MASK 0xfbff
 
+#define SMBUS_IO_BASE 0x6000
+
 static void smbus_delay(void)
 {
     	outb(0x80, 0x80);
@@ -75,6 +77,29 @@ static int smbus_start_condition(unsigned smbus_io_base)
 	return loops?0:SMBUS_WAIT_UNTIL_READY_TIMEOUT;
 }
 
+static int smbus_check_stop_condition(unsigned smbus_io_base)
+{
+	unsigned char val;
+	unsigned long loops;
+	loops = SMBUS_TIMEOUT;
+	/* check for SDA status */
+	do {
+		smbus_delay();
+		val = inw(smbus_io_base + SMB_CTRL1);
+		if ((val & SMB_CTRL1_STOP) == 0) {
+			break;
+		}
+	} while(--loops);
+	return loops?0:SMBUS_WAIT_UNTIL_READY_TIMEOUT;
+}
+
+static int smbus_stop_condition(unsigned smbus_io_base)
+{
+	unsigned char val;
+	val = inb(smbus_io_base + SMB_CTRL1);
+	outb(SMB_CTRL1_STOP, smbus_io_base + SMB_CTRL1);
+}
+
 static int smbus_send_slave_address(unsigned smbus_io_base, unsigned char device)
 {
 	unsigned char val;
@@ -86,8 +111,8 @@ static int smbus_send_slave_address(unsigned smbus_io_base, unsigned char device
 
 	/* check for bus conflict and NACK */
 	val = inb(smbus_io_base + SMB_STS);
-	if ( ((val & SMB_STS_BER)    != 0) ||
-	     ((val & SMB_STS_NEGACK) != 0))
+	if (((val & SMB_STS_BER)    != 0) ||
+	    ((val & SMB_STS_NEGACK) != 0))
 		return SMBUS_ERROR;
 
 	/* check for SDA status */
@@ -112,8 +137,8 @@ static int smbus_send_command(unsigned smbus_io_base, unsigned char command)
 
 	/* check for bus conflict and NACK */
 	val = inb(smbus_io_base + SMB_STS);
-	if ( ((val & SMB_STS_BER)    != 0) ||
-	     ((val & SMB_STS_NEGACK) != 0))
+	if (((val & SMB_STS_BER)    != 0) ||
+	    ((val & SMB_STS_NEGACK) != 0))
 		return SMBUS_ERROR;
 
 	/* check for SDA status */
@@ -129,25 +154,27 @@ static int smbus_send_command(unsigned smbus_io_base, unsigned char command)
 
 static unsigned char do_smbus_read_byte(unsigned smbus_io_base, unsigned char device, unsigned char address)
 {
-	unsigned char val;
+	unsigned char val, val1;
+
+	smbus_check_stop_condition(smbus_io_base);
 
-	if (smbus_start_condition(smbus_io_base) < 0)
-	    print_debug("smbus error 1");
+	smbus_start_condition(smbus_io_base);
 
-	if (smbus_send_slave_address(smbus_io_base, device) < 0)
-	    print_debug("smbus error 2");
+	smbus_send_slave_address(smbus_io_base, device);
 
-	if (smbus_send_command(smbus_io_base, address) < 0)
-	    print_debug("smbus error 3");
+	smbus_send_command(smbus_io_base, address);
 
-	if (smbus_start_condition(smbus_io_base) < 0)
-	    print_debug("smbus error 4");
+	smbus_start_condition(smbus_io_base);
 
-	if (smbus_send_slave_address(smbus_io_base, device | 0x01))
-	    print_debug("smbus error 5");
+	smbus_send_slave_address(smbus_io_base, device | 0x01);
 
+	/* send NACK to slave */
 	val = inb(smbus_io_base + SMB_CTRL1);
 	outb(val | SMB_CTRL1_ACK, smbus_io_base + SMB_CTRL1);
 
-	return inb(smbus_io_base + SMB_SDA);
+	val = inb(smbus_io_base + SMB_SDA);
+
+	//smbus_stop_condition(smbus_io_base);
+
+	return val;
 }