From 66ca8833c177de5c10eedc5f5a2b2632cdfc3265 Mon Sep 17 00:00:00 2001
From: Andrey Gusakov <dron0gus@gmail.com>
Date: Sun, 11 Feb 2024 14:42:30 +0300
Subject: [PATCH] lsm303agr: check WHO_AM_I first, release bus if device is not
 detected

---
 os/ex/devices/ST/lsm303agr.c | 29 ++++++++++++++++-------------
 1 file changed, 16 insertions(+), 13 deletions(-)

diff --git a/os/ex/devices/ST/lsm303agr.c b/os/ex/devices/ST/lsm303agr.c
index b563da5ce..07722a012 100644
--- a/os/ex/devices/ST/lsm303agr.c
+++ b/os/ex/devices/ST/lsm303agr.c
@@ -787,6 +787,22 @@ msg_t lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {
 
   devp->config = config;
 
+#if (LSM303AGR_SHARED_I2C == TRUE) || (LSM303AGR_SHARED_SPI == TRUE)
+  lsm303agrAccureBus(devp->config);
+#endif /* LSM303AGR_SHARED_I2C || LSM303AGR_SHARED_SPI */
+  lsm303agrStartBus(devp->config);
+
+  /* Check WHO_I_AM */
+  lsm303agrReadRegister(devp->config, LSM303AGR_SAD_ACC,
+                          LSM303AGR_AD_WHO_AM_I_A, &devid, 1);
+  if (devid != 0x33)
+  {
+#if (LSM303AGR_SHARED_I2C == TRUE) || (LSM303AGR_SHARED_SPI == TRUE)
+    lsm303agrReleaseBus(devp->config);
+#endif /* LSM303AGR_SHARED_I2C || LSM303AGR_SHARED_SPI */
+    return MSG_RESET;
+  }
+
   /* Configuring Accelerometer subsystem.*/
 
   /* Control register 1 configuration block.*/
@@ -867,19 +883,6 @@ msg_t lsm303agrStart(LSM303AGRDriver *devp, const LSM303AGRConfig *config) {
     for(i = 0; i < LSM303AGR_ACC_NUMBER_OF_AXES; i++)
       devp->accbias[i] = LSM303AGR_ACC_BIAS;
 
-#if (LSM303AGR_SHARED_I2C == TRUE) || (LSM303AGR_SHARED_SPI == TRUE)
-  lsm303agrAccureBus(devp->config);
-#endif /* LSM303AGR_SHARED_I2C || LSM303AGR_SHARED_SPI */
-  lsm303agrStartBus(devp->config);
-
-  /* Check WHO_I_AM */
-  lsm303agrReadRegister(devp->config, LSM303AGR_SAD_ACC,
-                          LSM303AGR_AD_WHO_AM_I_A, &devid, 1);
-  if (devid != 0x33)
-  {
-    return MSG_RESET;
-  }
-
   lsm303agrWriteRegister(devp->config, LSM303AGR_SAD_ACC,
                          LSM303AGR_AD_CTRL_REG1_A, cr, 4);