From 86ec1c37fa29ba4ffc1d54a0c438de1cd536f51c Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 5 Dec 2013 09:06:53 +1100
Subject: [PATCH] l3gd20: added retries to disable_i2c()

---
 src/drivers/l3gd20/l3gd20.cpp | 12 ++++++++++--
 1 file changed, 10 insertions(+), 2 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 17b53bfa96..176ef648bb 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -715,8 +715,16 @@ L3GD20::stop()
 void
 L3GD20::disable_i2c(void)
 {
-	uint8_t a = read_reg(0x05);
-	write_reg(0x05, (0x20 | a));
+	uint8_t retries = 10;
+	while (retries--) {
+		// add retries
+		uint8_t a = read_reg(0x05);
+		write_reg(0x05, (0x20 | a));
+		if (read_reg(0x05) == (a | 0x20)) {
+			return;
+		}
+	}
+	debug("FAILED TO DISABLE I2C");
 }
 
 void