diff --git a/drivers/tpm/Makefile b/drivers/tpm/Makefile
index d1f9bbf..913dd9c 100644
--- a/drivers/tpm/Makefile
+++ b/drivers/tpm/Makefile
@@ -25,9 +25,10 @@
 
 $(shell mkdir -p $(obj)slb9635_i2c)
 
-COBJS-$(CONFIG_TPM_TIS_LPC) = tpm_tis_lpc.o
-COBJS-$(CONFIG_INFINEON_TPM_I2C) += tis_i2c.o slb9635_i2c/tpm.o
-COBJS-$(CONFIG_INFINEON_TPM_I2C) += slb9635_i2c/tpm_tis_i2c.o
+# TODO: Merge tpm_tis_lpc.c with tpm.c
+COBJS-$(CONFIG_TPM_TIS_I2C) += tpm.o
+COBJS-$(CONFIG_TPM_TIS_I2C) += tpm_tis_i2c.o
+COBJS-$(CONFIG_TPM_TIS_LPC) += tpm_tis_lpc.o
 
 COBJS	:= $(COBJS-y)
 SRCS	:= $(COBJS:.o=.c)
diff --git a/drivers/tpm/slb9635_i2c/compatibility.h b/drivers/tpm/slb9635_i2c/compatibility.h
deleted file mode 100644
index 62dc9fa..0000000
--- a/drivers/tpm/slb9635_i2c/compatibility.h
+++ /dev/null
@@ -1,51 +0,0 @@
-/*
- * Copyright (C) 2011 Infineon Technologies
- *
- * Authors:
- * Peter Huewe <huewe.external@infineon.com>
- *
- * Version: 2.1.1
- *
- * 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; either version 2 of
- * the License, or (at your option) any later version.
- *
- * 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
- */
-
-#ifndef _COMPATIBILITY_H_
-#define _COMPATIBILITY_H_
-
-/* all includes from U-Boot */
-#include <linux/types.h>
-#include <linux/unaligned/be_byteshift.h>
-#include <asm-generic/errno.h>
-#include <compiler.h>
-#include <common.h>
-
-/* extended error numbers from linux (see errno.h) */
-#define	ECANCELED	125	/* Operation Canceled */
-
-#define msleep(t) udelay((t)*1000)
-
-/* Timer frequency. Corresponds to msec timer resolution*/
-#define HZ             1000
-
-#define dev_dbg(dev, format, arg...) debug(format, ##arg)
-#define dev_err(dev, format, arg...) printf(format, ##arg)
-#define dev_info(dev, format, arg...) debug(format, ##arg)
-#define dbg_printf debug
-
-#endif
diff --git a/drivers/tpm/slb9635_i2c/tpm.c b/drivers/tpm/tpm.c
similarity index 63%
rename from drivers/tpm/slb9635_i2c/tpm.c
rename to drivers/tpm/tpm.c
index 496c48e..b657334 100644
--- a/drivers/tpm/slb9635_i2c/tpm.c
+++ b/drivers/tpm/tpm.c
@@ -32,11 +32,30 @@
  * MA 02111-1307 USA
  */
 
-#include <malloc.h>
-#include "tpm.h"
+#include <config.h>
+#include <common.h>
+#include <compiler.h>
+#include <fdtdec.h>
+#include <i2c.h>
+#include <tpm.h>
+#include <asm-generic/errno.h>
+#include <linux/types.h>
+#include <linux/unaligned/be_byteshift.h>
 
-/* global structure for tpm chip data */
-struct tpm_chip g_chip;
+#include "tpm_private.h"
+
+DECLARE_GLOBAL_DATA_PTR;
+
+/* TPM configuration */
+struct tpm {
+	int i2c_bus;
+	int slave_addr;
+	char inited;
+	int old_bus;
+} tpm;
+
+/* Global structure for tpm chip data */
+static struct tpm_chip g_chip;
 
 enum tpm_duration {
 	TPM_SHORT = 0,
@@ -45,9 +64,18 @@
 	TPM_UNDEFINED,
 };
 
-#define TPM_MAX_ORDINAL 243
-#define TPM_MAX_PROTECTED_ORDINAL 12
-#define TPM_PROTECTED_ORDINAL_MASK 0xFF
+/* Extended error numbers from linux (see errno.h) */
+#define ECANCELED	125	/* Operation Canceled */
+
+/* Timer frequency. Corresponds to msec timer resolution*/
+#define HZ		1000
+
+#define TPM_MAX_ORDINAL			243
+#define TPM_MAX_PROTECTED_ORDINAL	12
+#define TPM_PROTECTED_ORDINAL_MASK	0xFF
+
+#define TPM_CMD_COUNT_BYTE	2
+#define TPM_CMD_ORDINAL_BYTE	6
 
 /*
  * Array with one entry per ordinal defining the maximum amount
@@ -318,34 +346,31 @@
 	TPM_MEDIUM,
 };
 
-/*
- * Returns max number of milliseconds to wait
- */
-unsigned long tpm_calc_ordinal_duration(struct tpm_chip *chip, u32 ordinal)
+/* Returns max number of milliseconds to wait */
+static unsigned long tpm_calc_ordinal_duration(struct tpm_chip *chip,
+		u32 ordinal)
 {
 	int duration_idx = TPM_UNDEFINED;
 	int duration = 0;
 
-	if (ordinal < TPM_MAX_ORDINAL)
+	if (ordinal < TPM_MAX_ORDINAL) {
 		duration_idx = tpm_ordinal_duration[ordinal];
-	else if ((ordinal & TPM_PROTECTED_ORDINAL_MASK) <
-		 TPM_MAX_PROTECTED_ORDINAL)
-		duration_idx =
-		    tpm_protected_ordinal_duration[ordinal &
-						   TPM_PROTECTED_ORDINAL_MASK];
+	} else if ((ordinal & TPM_PROTECTED_ORDINAL_MASK) <
+			TPM_MAX_PROTECTED_ORDINAL) {
+		duration_idx = tpm_protected_ordinal_duration[
+				ordinal & TPM_PROTECTED_ORDINAL_MASK];
+	}
 
 	if (duration_idx != TPM_UNDEFINED)
 		duration = chip->vendor.duration[duration_idx];
+
 	if (duration <= 0)
-		return 2 * 60 * HZ; /*two minutes timeout*/
+		return 2 * 60 * HZ; /* Two minutes timeout */
 	else
 		return duration;
 }
 
-#define TPM_CMD_COUNT_BYTE 2
-#define TPM_CMD_ORDINAL_BYTE 6
-
-ssize_t tpm_transmit(const unsigned char *buf, size_t bufsiz)
+static ssize_t tpm_transmit(const unsigned char *buf, size_t bufsiz)
 {
 	ssize_t rc;
 	u32 count, ordinal;
@@ -358,18 +383,17 @@
 	ordinal = get_unaligned_be32(buf + TPM_CMD_ORDINAL_BYTE);
 
 	if (count == 0) {
-		dev_err(chip->dev, "no data\n");
+		error("no data\n");
 		return -ENODATA;
 	}
 	if (count > bufsiz) {
-		dev_err(chip->dev,
-			"invalid count value %x %zx\n", count, bufsiz);
+		error("invalid count value %x %zx\n", count, bufsiz);
 		return -E2BIG;
 	}
 
 	rc = chip->vendor.send(chip, (u8 *)buf, count);
 	if (rc < 0) {
-		dev_err(chip->dev, "tpm_transmit: tpm_send: error %zd\n", rc);
+		error("tpm_transmit: tpm_send: error %zd\n", rc);
 		goto out;
 	}
 
@@ -379,47 +403,126 @@
 	start = get_timer(0);
 	stop = tpm_calc_ordinal_duration(chip, ordinal);
 	do {
-		dbg_printf("waiting for status...\n");
+		debug("waiting for status...\n");
 		u8 status = chip->vendor.status(chip);
 		if ((status & chip->vendor.req_complete_mask) ==
 		    chip->vendor.req_complete_val) {
-			dbg_printf("...got it;\n");
+			debug("...got it;\n");
 			goto out_recv;
 		}
 
 		if ((status == chip->vendor.req_canceled)) {
-			dev_err(chip->dev, "Operation Canceled\n");
+			error("Operation Canceled\n");
 			rc = -ECANCELED;
 			goto out;
 		}
-		msleep(TPM_TIMEOUT);
+		udelay(TPM_TIMEOUT * 1000);
 	} while (get_timer(start) < stop);
 
 	chip->vendor.cancel(chip);
-	dev_err(chip->dev, "Operation Timed out\n");
+	error("Operation Timed out\n");
 	rc = -ETIME;
 	goto out;
 
 out_recv:
-
-	dbg_printf("out_recv: reading response...\n");
+	debug("out_recv: reading response...\n");
 	rc = chip->vendor.recv(chip, (u8 *)buf, TPM_BUFSIZE);
 	if (rc < 0)
-		dev_err(chip->dev, "tpm_transmit: tpm_recv: error %zd\n", rc);
+		error("tpm_transmit: tpm_recv: error %zd\n", rc);
+
 out:
 	return rc;
 }
 
+static int tpm_open(uint32_t dev_addr)
+{
+	int rc;
+	if (g_chip.is_open)
+		return -EBUSY;
+	rc = tpm_vendor_init(dev_addr);
+	if (rc < 0)
+		g_chip.is_open = 0;
+	return rc;
+}
+
-#define TPM_ERROR_SIZE 10
+static void tpm_close(void)
+{
+	if (g_chip.is_open) {
+		tpm_vendor_cleanup(&g_chip);
+		g_chip.is_open = 0;
+	}
+}
 
-enum tpm_capabilities {
-	TPM_CAP_PROP = cpu_to_be32(5),
-};
+static int tpm_select(void)
+{
+	int ret;
 
-enum tpm_sub_capabilities {
-	TPM_CAP_PROP_TIS_TIMEOUT = cpu_to_be32(0x115),
-	TPM_CAP_PROP_TIS_DURATION = cpu_to_be32(0x120),
-};
+	tpm.old_bus = i2c_get_bus_num();
+	if (tpm.old_bus != tpm.i2c_bus) {
+		ret = i2c_set_bus_num(tpm.i2c_bus);
+		if (ret) {
+			debug("%s: Fail to set i2c bus %d\n", __func__,
+			      tpm.i2c_bus);
+			return -1;
+		}
+	}
+	return 0;
+}
+
+static int tpm_deselect(void)
+{
+	int ret;
+
+	if (tpm.old_bus != i2c_get_bus_num()) {
+		ret = i2c_set_bus_num(tpm.old_bus);
+		if (ret) {
+			debug("%s: Fail to restore i2c bus %d\n",
+			      __func__, tpm.old_bus);
+			return -1;
+		}
+	}
+	tpm.old_bus = -1;
+	return 0;
+}
+
+/**
+ * Decode TPM configuration.
+ *
+ * @param dev	Returns a configuration of TPM device
+ * @return 0 if ok, -1 on error
+ */
+static int tpm_decode_config(struct tpm *dev)
+{
+#ifdef CONFIG_OF_CONTROL
+	const void *blob = gd->fdt_blob;
+	int node, parent;
+	int i2c_bus;
+
+	node = fdtdec_next_compatible(blob, 0, COMPAT_INFINEON_SLB9635_TPM);
+	if (node < 0) {
+		node = fdtdec_next_compatible(blob, 0,
+				COMPAT_INFINEON_SLB9645_TPM);
+	}
+	if (node < 0) {
+		debug("%s: Node not found\n", __func__);
+		return -1;
+	}
+	parent = fdt_parent_offset(blob, node);
+	if (parent < 0) {
+		debug("%s: Cannot find node parent\n", __func__);
+		return -1;
+	}
+	i2c_bus = i2c_get_bus_num_fdt(parent);
+	if (i2c_bus < 0)
+		return -1;
+	dev->i2c_bus = i2c_bus;
+	dev->slave_addr = fdtdec_get_addr(blob, node, "reg");
+#else
+	dev->i2c_bus = CONFIG_TPM_TIS_I2C_BUS_NUMBER;
+	dev->slave_addr = CONFIG_TPM_TIS_I2C_SLAVE_ADDRESS;
+#endif
+	return 0;
+}
 
 struct tpm_chip *tpm_register_hardware(const struct tpm_vendor_specific *entry)
 {
@@ -433,21 +536,94 @@
 	return chip;
 }
 
+int tis_init(void)
+{
+	if (tpm.inited)
+		return 0;
+
+	if (tpm_decode_config(&tpm))
+		return -1;
+
+	if (tpm_select())
+		return -1;
+
+	/*
+	 * Probe TPM twice; the first probing might fail because TPM is asleep,
+	 * and the probing can wake up TPM.
+	 */
+	if (i2c_probe(tpm.slave_addr) && i2c_probe(tpm.slave_addr)) {
+		debug("%s: fail to probe i2c addr 0x%x\n", __func__,
+		      tpm.slave_addr);
+		return -1;
+	}
+
+	tpm_deselect();
+
+	tpm.inited = 1;
+
+	return 0;
+}
+
-int tpm_open(uint32_t dev_addr)
+int tis_open(void)
 {
 	int rc;
-	if (g_chip.is_open)
-		return -EBUSY;
-	rc = tpm_vendor_init(dev_addr);
-	if (rc < 0)
-		g_chip.is_open = 0;
+
+	if (!tpm.inited)
+		return -1;
+
+	if (tpm_select())
+		return -1;
+
+	rc = tpm_open(tpm.slave_addr);
+
+	tpm_deselect();
+
 	return rc;
 }
 
-void tpm_close(void)
+int tis_close(void)
 {
-	if (g_chip.is_open) {
-		tpm_vendor_cleanup(&g_chip);
-		g_chip.is_open = 0;
+	if (!tpm.inited)
+		return -1;
+
+	if (tpm_select())
+		return -1;
+
+	tpm_close();
+
+	tpm_deselect();
+
+	return 0;
+}
+
+int tis_sendrecv(const uint8_t *sendbuf, size_t sbuf_size,
+		uint8_t *recvbuf, size_t *rbuf_len)
+{
+	int len;
+	uint8_t buf[4096];
+
+	if (!tpm.inited)
+		return -1;
+
+	if (sizeof(buf) < sbuf_size)
+		return -1;
+
+	memcpy(buf, sendbuf, sbuf_size);
+
+	if (tpm_select())
+		return -1;
+
+	len = tpm_transmit(buf, sbuf_size);
+
+	tpm_deselect();
+
+	if (len < 10) {
+		*rbuf_len = 0;
+		return -1;
 	}
+
+	memcpy(recvbuf, buf, len);
+	*rbuf_len = len;
+
+	return 0;
 }
diff --git a/drivers/tpm/slb9635_i2c/tpm.h b/drivers/tpm/tpm_private.h
similarity index 71%
rename from drivers/tpm/slb9635_i2c/tpm.h
rename to drivers/tpm/tpm_private.h
index 9ddee86..888a074 100644
--- a/drivers/tpm/slb9635_i2c/tpm.h
+++ b/drivers/tpm/tpm_private.h
@@ -33,12 +33,11 @@
  * MA 02111-1307 USA
  */
 
-#ifndef _TPM_H_
-#define _TPM_H_
+#ifndef _TPM_PRIVATE_H_
+#define _TPM_PRIVATE_H_
 
 #include <linux/compiler.h>
-
-#include "compatibility.h"
+#include <linux/types.h>
 
 enum tpm_timeout {
 	TPM_TIMEOUT = 5,	/* msecs */
@@ -47,13 +46,9 @@
 /* Size of external transmit buffer (used in tpm_transmit)*/
 #define TPM_BUFSIZE 4096
 
-/* Index of fields in TPM command buffer */
-#define TPM_CMD_SIZE_BYTE 2
-#define TPM_CMD_ORDINAL_BYTE 6
-
 /* Index of Count field in TPM response buffer */
-#define TPM_RSP_SIZE_BYTE 2
-#define TPM_RSP_RC_BYTE 6
+#define TPM_RSP_SIZE_BYTE	2
+#define TPM_RSP_RC_BYTE		6
 
 struct tpm_chip;
 
@@ -65,10 +60,10 @@
 	int (*recv) (struct tpm_chip *, u8 *, size_t);
 	int (*send) (struct tpm_chip *, u8 *, size_t);
 	void (*cancel) (struct tpm_chip *);
-	 u8(*status) (struct tpm_chip *);
+	u8(*status) (struct tpm_chip *);
 	int locality;
-	unsigned long timeout_a, timeout_b, timeout_c, timeout_d; /* msec */
-	unsigned long duration[3];	/* msec */
+	unsigned long timeout_a, timeout_b, timeout_c, timeout_d;  /* msec */
+	unsigned long duration[3];  /* msec */
 };
 
 struct tpm_chip {
@@ -132,30 +127,11 @@
 	union tpm_cmd_params params;
 } __packed;
 
-
-/* ---------- Interface for TPM vendor ------------ */
-
-extern struct tpm_chip *tpm_register_hardware(
-	const struct tpm_vendor_specific *);
+struct tpm_chip *tpm_register_hardware(const struct tpm_vendor_specific *);
 
-extern int tpm_vendor_init(uint32_t dev_addr);
+int tpm_vendor_init(uint32_t dev_addr);
 
-extern void tpm_vendor_cleanup(struct tpm_chip *chip);
-
-/* ---------- Interface for TDDL ------------------- */
-
-/*
- * if dev_addr != 0 - redefines TPM device address
- * Returns < 0 on error, 0 on success.
- */
-extern int tpm_open(uint32_t dev_addr);
-
-extern void tpm_close(void);
+void tpm_vendor_cleanup(struct tpm_chip *chip);
 
-/*
- * Transmit bufsiz bytes out of buf to TPM and get results back in buf, too.
- * Returns < 0 on error, 0 on success.
- */
-extern ssize_t tpm_transmit(const unsigned char *buf, size_t bufsiz);
 
 #endif
diff --git a/drivers/tpm/slb9635_i2c/tpm_tis_i2c.c b/drivers/tpm/tpm_tis_i2c.c
similarity index 73%
rename from drivers/tpm/slb9635_i2c/tpm_tis_i2c.c
rename to drivers/tpm/tpm_tis_i2c.c
index c2e1041..2dd8501 100644
--- a/drivers/tpm/slb9635_i2c/tpm_tis_i2c.c
+++ b/drivers/tpm/tpm_tis_i2c.c
@@ -38,34 +38,66 @@
 
 #include <common.h>
 #include <fdtdec.h>
+#include <compiler.h>
 #include <i2c.h>
+#include <tpm.h>
+#include <asm-generic/errno.h>
 #include <linux/types.h>
+#include <linux/unaligned/be_byteshift.h>
 
-#include "compatibility.h"
-#include "tpm.h"
+#include "tpm_private.h"
 
 DECLARE_GLOBAL_DATA_PTR;
 
-/* max. buffer size supported by our tpm */
-#ifdef TPM_BUFSIZE
-#undef TPM_BUFSIZE
-#endif
-#define TPM_BUFSIZE 1260
 /* Address of the TPM on the I2C bus */
-#define TPM_I2C_ADDR 0x20
-/* max. number of iterations after i2c NAK */
-#define MAX_COUNT 3
+#define TPM_I2C_ADDR		0x20
+
+/* Max buffer size supported by our tpm */
+#define TPM_DEV_BUFSIZE		1260
 
-#define SLEEP_DURATION 60 /*in usec*/
+/* Max number of iterations after i2c NAK */
+#define MAX_COUNT		3
 
-/* max. number of iterations after i2c NAK for 'long' commands
- * we need this especially for sending TPM_READY, since the cleanup after the
+/*
+ * Max number of iterations after i2c NAK for 'long' commands
+ *
+ * We need this especially for sending TPM_READY, since the cleanup after the
  * transtion to the ready state may take some time, but it is unpredictable
  * how long it will take.
  */
-#define MAX_COUNT_LONG 50
+#define MAX_COUNT_LONG		50
+
+#define SLEEP_DURATION		60	/* in usec */
+#define SLEEP_DURATION_LONG	210	/* in usec */
+
+#define TPM_HEADER_SIZE		10
+
+/*
+ * Expected value for DIDVID register
+ *
+ * The only device the system knows about at this moment is Infineon slb9635.
+ */
+#define TPM_TIS_I2C_DID_VID	0x000b15d1L
+
+enum tis_access {
+	TPM_ACCESS_VALID		= 0x80,
+	TPM_ACCESS_ACTIVE_LOCALITY	= 0x20,
+	TPM_ACCESS_REQUEST_PENDING	= 0x04,
+	TPM_ACCESS_REQUEST_USE		= 0x02,
+};
+
+enum tis_status {
+	TPM_STS_VALID			= 0x80,
+	TPM_STS_COMMAND_READY		= 0x40,
+	TPM_STS_GO			= 0x20,
+	TPM_STS_DATA_AVAIL		= 0x10,
+	TPM_STS_DATA_EXPECT		= 0x08,
+};
 
-#define SLEEP_DURATION_LONG 210 /* in usec */
+enum tis_defaults {
+	TIS_SHORT_TIMEOUT		= 750,	/* ms */
+	TIS_LONG_TIMEOUT		= 2000,	/* ms */
+};
 
 /* expected value for DIDVID register */
 #define TPM_TIS_I2C_DID_VID_9635 0x000b15d1L
@@ -83,17 +115,24 @@
 	[UNKNOWN] = "unknown/fallback to slb9635",
 };
 
+#define	TPM_ACCESS(l)			(0x0000 | ((l) << 4))
+#define	TPM_STS(l)			(0x0001 | ((l) << 4))
+#define	TPM_DATA_FIFO(l)		(0x0005 | ((l) << 4))
+#define	TPM_DID_VID(l)			(0x0006 | ((l) << 4))
+
 /* Structure to store I2C TPM specific stuff */
-struct tpm_inf_dev {
+struct tpm_dev {
 	uint addr;
-	u8 buf[TPM_BUFSIZE + sizeof(u8)];	/* max. buffer size + addr */
+	u8 buf[TPM_DEV_BUFSIZE + sizeof(u8)];  /* Max buffer size + addr */
 	enum i2c_chip_type chip_type;
 };
 
-static struct tpm_inf_dev tpm_dev = {
+static struct tpm_dev tpm_dev = {
 	.addr = TPM_I2C_ADDR
 };
 
+static struct tpm_dev tpm_dev;
+
 /*
  * iic_tpm_read() - read from TPM register
  * @addr: register address to read from
@@ -108,24 +147,21 @@
  *
  * Return -EIO on error, 0 on success.
  */
-int iic_tpm_read(u8 addr, u8 *buffer, size_t len)
+static int iic_tpm_read(u8 addr, u8 *buffer, size_t len)
 {
 	int rc;
 	int count;
-	uint myaddr = addr;
-	/* we have to use uint here, uchar hangs the board */
+	uint32_t addrbuf = addr;
 
 	if ((tpm_dev.chip_type == SLB9635) || (tpm_dev.chip_type == UNKNOWN)) {
 		/* slb9635 protocol should work in both cases */
 		for (count = 0; count < MAX_COUNT; count++) {
 			rc = i2c_write(tpm_dev.addr, 0, 0,
-				       (uchar *)&myaddr, 1);
+				       (uchar *)&addrbuf, 1);
 			if (rc == 0)
-				break;  /* success, break to skip sleep */
-
+				break;  /* Success, break to skip sleep */
 			udelay(SLEEP_DURATION);
 		}
-
 		if (rc)
 			return -rc;
 
@@ -140,10 +176,11 @@
 				break;  /* success, break to skip sleep */
 		}
 	} else {
-		/* use a combined read for newer chips
-		 * unfortunately the smbus functions are not suitable due to
+		/*
+		 * Use a combined read for newer chips.
+		 * Unfortunately the smbus functions are not suitable due to
 		 * the 32 byte limit of the smbus.
-		 * retries should usually not be needed, but are kept just to
+		 * Retries should usually not be needed, but are kept just to
 		 * be safe on the safe side.
 		 */
 		for (count = 0; count < MAX_COUNT; count++) {
@@ -154,7 +191,7 @@
 		}
 	}
 
-	/* take care of 'guard time' */
+	/* Take care of 'guard time' */
 	udelay(SLEEP_DURATION);
 	if (rc)
 		return -rc;
@@ -163,21 +200,19 @@
 }
 
 static int iic_tpm_write_generic(u8 addr, u8 *buffer, size_t len,
-				unsigned int sleep_time,
-				u8 max_count)
+		unsigned int sleep_time, u8 max_count)
 {
 	int rc = 0;
 	int count;
 
-	/* prepare send buffer */
+	/* Prepare send buffer */
 	tpm_dev.buf[0] = addr;
 	memcpy(&(tpm_dev.buf[1]), buffer, len);
 
 	for (count = 0; count < max_count; count++) {
 		rc = i2c_write(tpm_dev.addr, 0, 0, tpm_dev.buf, len + 1);
 		if (rc == 0)
-			break;  /* success, break to skip sleep */
-
+			break;  /* Success, break to skip sleep */
 		udelay(sleep_time);
 	}
 
@@ -214,42 +249,16 @@
 /*
  * This function is needed especially for the cleanup situation after
  * sending TPM_READY
- * */
+ */
 static int iic_tpm_write_long(u8 addr, u8 *buffer, size_t len)
 {
 	return iic_tpm_write_generic(addr, buffer, len, SLEEP_DURATION_LONG,
 			MAX_COUNT_LONG);
 }
 
-#define TPM_HEADER_SIZE 10
-
-enum tis_access {
-	TPM_ACCESS_VALID = 0x80,
-	TPM_ACCESS_ACTIVE_LOCALITY = 0x20,
-	TPM_ACCESS_REQUEST_PENDING = 0x04,
-	TPM_ACCESS_REQUEST_USE = 0x02,
-};
-
-enum tis_status {
-	TPM_STS_VALID = 0x80,
-	TPM_STS_COMMAND_READY = 0x40,
-	TPM_STS_GO = 0x20,
-	TPM_STS_DATA_AVAIL = 0x10,
-	TPM_STS_DATA_EXPECT = 0x08,
-};
-
-enum tis_defaults {
-	TIS_SHORT_TIMEOUT = 750,	/* ms */
-	TIS_LONG_TIMEOUT = 2000,	/* 2 sec */
-};
-
-#define	TPM_ACCESS(l)			(0x0000 | ((l) << 4))
-#define	TPM_STS(l)			(0x0001 | ((l) << 4))
-#define	TPM_DATA_FIFO(l)		(0x0005 | ((l) << 4))
-#define	TPM_DID_VID(l)			(0x0006 | ((l) << 4))
-
 static int check_locality(struct tpm_chip *chip, int loc)
 {
+	const u8 mask = TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID;
 	u8 buf;
 	int rc;
 
@@ -257,8 +266,7 @@
 	if (rc < 0)
 		return rc;
 
-	if ((buf & (TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID)) ==
-		(TPM_ACCESS_ACTIVE_LOCALITY | TPM_ACCESS_VALID)) {
+	if ((buf & mask) == mask) {
 		chip->vendor.locality = loc;
 		return loc;
 	}
@@ -268,12 +276,13 @@
 
 static void release_locality(struct tpm_chip *chip, int loc, int force)
 {
+	const u8 mask = TPM_ACCESS_REQUEST_PENDING | TPM_ACCESS_VALID;
 	u8 buf;
+
 	if (iic_tpm_read(TPM_ACCESS(loc), &buf, 1) < 0)
 		return;
 
-	if (force || (buf & (TPM_ACCESS_REQUEST_PENDING | TPM_ACCESS_VALID)) ==
-			(TPM_ACCESS_REQUEST_PENDING | TPM_ACCESS_VALID)) {
+	if (force || (buf & mask) == mask) {
 		buf = TPM_ACCESS_ACTIVE_LOCALITY;
 		iic_tpm_write(TPM_ACCESS(loc), &buf, 1);
 	}
@@ -285,17 +294,17 @@
 	u8 buf = TPM_ACCESS_REQUEST_USE;
 
 	if (check_locality(chip, loc) >= 0)
-		return loc; /* we already have the locality */
+		return loc;  /* We already have the locality */
 
 	iic_tpm_write(TPM_ACCESS(loc), &buf, 1);
 
-	/* wait for burstcount */
+	/* Wait for burstcount */
 	start = get_timer(0);
 	stop = chip->vendor.timeout_a;
 	do {
 		if (check_locality(chip, loc) >= 0)
 			return loc;
-		msleep(TPM_TIMEOUT);
+		udelay(TPM_TIMEOUT * 1000);
 	} while (get_timer(start) < stop);
 
 	return -1;
@@ -303,8 +312,9 @@
 
 static u8 tpm_tis_i2c_status(struct tpm_chip *chip)
 {
-	/* NOTE: since i2c read may fail, return 0 in this case --> time-out */
+	/* NOTE: Since i2c read may fail, return 0 in this case --> time-out */
 	u8 buf;
+
 	if (iic_tpm_read(TPM_STS(chip->vendor.locality), &buf, 1) < 0)
 		return 0;
 	else
@@ -313,8 +323,9 @@
 
 static void tpm_tis_i2c_ready(struct tpm_chip *chip)
 {
-	/* this causes the current command to be aborted */
+	/* This causes the current command to be aborted */
 	u8 buf = TPM_STS_COMMAND_READY;
+
 	iic_tpm_write_long(TPM_STS(chip->vendor.locality), &buf, 1);
 }
 
@@ -322,34 +333,34 @@
 {
 	unsigned long start, stop;
 	ssize_t burstcnt;
-	u8 buf[3];
+	u8 addr, buf[3];
 
-	/* wait for burstcount */
-	/* which timeout value, spec has 2 answers (c & d) */
+	/* Wait for burstcount */
+	/* XXX: Which timeout value? Spec has 2 answers (c & d) */
 	start = get_timer(0);
 	stop = chip->vendor.timeout_d;
 	do {
 		/* Note: STS is little endian */
-		if (iic_tpm_read(TPM_STS(chip->vendor.locality) + 1, buf, 3)
-				< 0)
+		addr = TPM_STS(chip->vendor.locality) + 1;
+		if (iic_tpm_read(addr, buf, 3) < 0)
 			burstcnt = 0;
 		else
 			burstcnt = (buf[2] << 16) + (buf[1] << 8) + buf[0];
 
 		if (burstcnt)
 			return burstcnt;
-		msleep(TPM_TIMEOUT);
+		udelay(TPM_TIMEOUT * 1000);
 	} while (get_timer(start) < stop);
 
 	return -EBUSY;
 }
 
 static int wait_for_stat(struct tpm_chip *chip, u8 mask, unsigned long timeout,
-			int *status)
+		int *status)
 {
 	unsigned long start, stop;
 
-	/* check current status */
+	/* Check current status */
 	*status = tpm_tis_i2c_status(chip);
 	if ((*status & mask) == mask)
 		return 0;
@@ -357,11 +368,10 @@
 	start = get_timer(0);
 	stop = timeout;
 	do {
-		msleep(TPM_TIMEOUT);
+		udelay(TPM_TIMEOUT * 1000);
 		*status = tpm_tis_i2c_status(chip);
 		if ((*status & mask) == mask)
 			return 0;
-
 	} while (get_timer(start) < stop);
 
 	return -ETIME;
@@ -376,17 +386,16 @@
 	while (size < count) {
 		burstcnt = get_burstcount(chip);
 
-		/* burstcount < 0 = tpm is busy */
+		/* burstcount < 0 -> tpm is busy */
 		if (burstcnt < 0)
 			return burstcnt;
 
-		/* limit received data to max. left */
+		/* Limit received data to max left */
 		if (burstcnt > (count - size))
 			burstcnt = count - size;
 
 		rc = iic_tpm_read(TPM_DATA_FIFO(chip->vendor.locality),
-				  &(buf[size]),
-				  burstcnt);
+				&(buf[size]), burstcnt);
 		if (rc == 0)
 			size += burstcnt;
 	}
@@ -404,10 +413,10 @@
 		goto out;
 	}
 
-	/* read first 10 bytes, including tag, paramsize, and result */
+	/* Read first 10 bytes, including tag, paramsize, and result */
 	size = recv_data(chip, buf, TPM_HEADER_SIZE);
 	if (size < TPM_HEADER_SIZE) {
-		dev_err(chip->dev, "Unable to read header\n");
+		error("Unable to read header\n");
 		goto out;
 	}
 
@@ -418,23 +427,24 @@
 	}
 
 	size += recv_data(chip, &buf[TPM_HEADER_SIZE],
-				expected - TPM_HEADER_SIZE);
+			expected - TPM_HEADER_SIZE);
 	if (size < expected) {
-		dev_err(chip->dev, "Unable to read remainder of result\n");
+		error("Unable to read remainder of result\n");
 		size = -ETIME;
 		goto out;
 	}
 
 	wait_for_stat(chip, TPM_STS_VALID, chip->vendor.timeout_c, &status);
-	if (status & TPM_STS_DATA_AVAIL) {	/* retry? */
-		dev_err(chip->dev, "Error left over data\n");
+	if (status & TPM_STS_DATA_AVAIL) {  /* Retry? */
+		error("Error left over data\n");
 		size = -EIO;
 		goto out;
 	}
 
 out:
 	tpm_tis_i2c_ready(chip);
-	/* The TPM needs some time to clean up here,
+	/*
+	 * The TPM needs some time to clean up here,
 	 * so we sleep rather than keeping the bus busy
 	 */
 	udelay(2000);
@@ -448,10 +458,11 @@
 	int rc, status;
 	ssize_t burstcnt;
 	size_t count = 0;
+	int retry = 0;
 	u8 sts = TPM_STS_GO;
 
-	if (len > TPM_BUFSIZE)
-		return -E2BIG; /* command is too long for our tpm, sorry */
+	if (len > TPM_DEV_BUFSIZE)
+		return -E2BIG;  /* Command is too long for our tpm, sorry */
 
 	if (request_locality(chip, 0) < 0)
 		return -EBUSY;
@@ -459,44 +470,45 @@
 	status = tpm_tis_i2c_status(chip);
 	if ((status & TPM_STS_COMMAND_READY) == 0) {
 		tpm_tis_i2c_ready(chip);
-		if (wait_for_stat
-		    (chip, TPM_STS_COMMAND_READY,
-		     chip->vendor.timeout_b, &status) < 0) {
+		if (wait_for_stat(chip, TPM_STS_COMMAND_READY,
+				  chip->vendor.timeout_b, &status) < 0) {
 			rc = -ETIME;
 			goto out_err;
 		}
 	}
 
-	while (count < len - 1) {
-		burstcnt = get_burstcount(chip);
+	burstcnt = get_burstcount(chip);
 
-		/* burstcount < 0 = tpm is busy */
-		if (burstcnt < 0)
-			return burstcnt;
+	/* burstcount < 0 -> tpm is busy */
+	if (burstcnt < 0)
+		return burstcnt;
 
-		if (burstcnt > (len-1-count))
-			burstcnt = len-1-count;
+	while (count < len - 1) {
+		if (burstcnt > len - 1 - count)
+			burstcnt = len - 1 - count;
 
-#ifdef CONFIG_TPM_I2C_BURST_LIMITATION
-		if (burstcnt > CONFIG_TPM_I2C_BURST_LIMITATION)
-			burstcnt = CONFIG_TPM_I2C_BURST_LIMITATION;
-#endif /* CONFIG_TPM_I2C_BURST_LIMITATION */
+#ifdef CONFIG_TPM_TIS_I2C_BURST_LIMITATION
+		if (retry && burstcnt > CONFIG_TPM_TIS_I2C_BURST_LIMITATION)
+			burstcnt = CONFIG_TPM_TIS_I2C_BURST_LIMITATION;
+#endif /* CONFIG_TPM_TIS_I2C_BURST_LIMITATION */
 
 		rc = iic_tpm_write(TPM_DATA_FIFO(chip->vendor.locality),
-				   &(buf[count]), burstcnt);
+				&(buf[count]), burstcnt);
 		if (rc == 0)
 			count += burstcnt;
-
-		wait_for_stat(chip, TPM_STS_VALID,
-			      chip->vendor.timeout_c, &status);
+		else {
+			retry++;
+			wait_for_stat(chip, TPM_STS_VALID,
+				      chip->vendor.timeout_c, &status);
 
-		if ((status & TPM_STS_DATA_EXPECT) == 0) {
-			rc = -EIO;
-			goto out_err;
+			if ((status & TPM_STS_DATA_EXPECT) == 0) {
+				rc = -EIO;
+				goto out_err;
+			}
 		}
 	}
 
-	/* write last byte */
+	/* Write last byte */
 	iic_tpm_write(TPM_DATA_FIFO(chip->vendor.locality), &(buf[count]), 1);
 	wait_for_stat(chip, TPM_STS_VALID, chip->vendor.timeout_c, &status);
 	if ((status & TPM_STS_DATA_EXPECT) != 0) {
@@ -504,13 +516,15 @@
 		goto out_err;
 	}
 
-	/* go and do it */
+	/* Go and do it */
 	iic_tpm_write(TPM_STS(chip->vendor.locality), &sts, 1);
 
 	return len;
+
 out_err:
 	tpm_tis_i2c_ready(chip);
-	/* The TPM needs some time to clean up here,
+	/*
+	 * The TPM needs some time to clean up here,
 	 * so we sleep rather than keeping the bus busy
 	 */
 	udelay(2000);
@@ -529,6 +543,7 @@
 	.req_canceled = TPM_STS_COMMAND_READY,
 };
 
+
 static enum i2c_chip_type tpm_vendor_chip_type(void)
 {
 #ifdef CONFIG_OF_CONTROL
@@ -543,9 +558,7 @@
 	return UNKNOWN;
 }
 
-/* initialisation of i2c tpm */
-
-
+/* Initialisation of i2c tpm */
 int tpm_vendor_init(uint32_t dev_addr)
 {
 	u32 vendor;
@@ -575,12 +588,12 @@
 	chip->vendor.timeout_c = TIS_SHORT_TIMEOUT;
 	chip->vendor.timeout_d = TIS_SHORT_TIMEOUT;
 
-	if (request_locality(chip, 0) != 0) {
+	if (request_locality(chip, 0) < 0) {
 		rc = -ENODEV;
 		goto out_err;
 	}
 
-	/* read four bytes from DID_VID register */
+	/* Read four bytes from DID_VID register */
 	if (iic_tpm_read(TPM_DID_VID(0), (uchar *)&vendor, 4) < 0) {
 		rc = -EIO;
 		goto out_release;
@@ -595,13 +608,13 @@
 	}
 
 	if (tpm_dev.chip_type != UNKNOWN && vendor != expected_did_vid) {
-		dev_err(dev, "vendor id did not match! ID was %08x\n", vendor);
+		error("Vendor id did not match! ID was %08x\n", vendor);
 		rc = -ENODEV;
 		goto out_release;
 	}
 
-	dev_info(dev, "1.2 TPM (chip type %s device-id 0x%X)\n",
-		 chip_name[tpm_dev.chip_type], vendor >> 16);
+	debug("1.2 TPM (chip type %s device-id 0x%X)\n",
+	      chip_name[tpm_dev.chip_type], vendor >> 16);
 
 	/*
 	 * A timeout query to TPM can be placed here.