1 files changed, 20 insertions, 19 deletions

diff --git a/src/drivers/i2c/tpm/cr50.c b/src/drivers/i2c/tpm/cr50.c
index fbc3ece09b..a3ff7814de 100644
--- a/src/drivers/i2c/tpm/cr50.c
+++ b/src/drivers/i2c/tpm/cr50.c
@@ -137,13 +137,13 @@ static int cr50_i2c_write(struct tpm_chip *chip,
 
 static int check_locality(struct tpm_chip *chip, int loc)
 {
+	uint8_t mask = TPM_ACCESS_VALID | TPM_ACCESS_ACTIVE_LOCALITY;
 	uint8_t buf;
 
 	if (cr50_i2c_read(chip, TPM_ACCESS(loc), &buf, 1) < 0)
 		return -1;
 
-	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;
 	}
@@ -151,40 +151,41 @@ static int check_locality(struct tpm_chip *chip, int loc)
 	return -1;
 }
 
-static void release_locality(struct tpm_chip *chip, int loc, int force)
+static void release_locality(struct tpm_chip *chip, int force)
 {
+	uint8_t mask = TPM_ACCESS_VALID | TPM_ACCESS_REQUEST_PENDING;
+	uint8_t addr = TPM_ACCESS(chip->vendor.locality);
 	uint8_t buf;
 
-	if (cr50_i2c_read(chip, TPM_ACCESS(loc), &buf, 1) < 0)
+	if (cr50_i2c_read(chip, addr, &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;
-		cr50_i2c_write(chip, TPM_ACCESS(loc), &buf, 1);
+		cr50_i2c_write(chip, addr, &buf, 1);
 	}
+
+	chip->vendor.locality = 0;
 }
 
 static int request_locality(struct tpm_chip *chip, int loc)
 {
 	uint8_t buf = TPM_ACCESS_REQUEST_USE;
+	struct stopwatch sw;
 
 	if (check_locality(chip, loc) >= 0)
-		return loc; /* we already have the locality */
+		return loc;
 
 	if (cr50_i2c_write(chip, TPM_ACCESS(loc), &buf, 1) < 0)
 		return -1;
 
-	/* wait for burstcount */
-	int timeout = 2 * 1000; /* 2s timeout */
-	while (timeout) {
-		if (check_locality(chip, loc) >= 0)
-			return loc;
-		mdelay(TPM_TIMEOUT);
-		timeout--;
+	stopwatch_init_msecs_expire(&sw, CR50_TIMEOUT_LONG_MS);
+	while (check_locality(chip, loc) < 0) {
+		if (stopwatch_expired(&sw))
+			return -1;
+		mdelay(CR50_TIMEOUT_SHORT_MS);
 	}
-
-	return -1;
+	return loc;
 }
 
 /* cr50 requires all 4 bytes of status register to be read */
@@ -458,11 +459,11 @@ int tpm_vendor_init(struct tpm_chip *chip, unsigned bus, uint32_t dev_addr)
 	return 0;
 
 out_err:
-	release_locality(chip, 0, 1);
+	release_locality(chip, 1);
 	return -1;
 }
 
 void tpm_vendor_cleanup(struct tpm_chip *chip)
 {
-	release_locality(chip, chip->vendor.locality, 1);
+	release_locality(chip, 1);
 }