From 354e29bb88feece608c61559e5bd949f18fae177 Mon Sep 17 00:00:00 2001
From: Andrey Gusakov <dron0gus@gmail.com>
Date: Sun, 11 Feb 2024 15:04:01 +0300
Subject: [PATCH] lis3dsh: check WHO_AM_I

---
 os/ex/devices/ST/lis3dsh.c | 29 ++++++++++++++++++++++-------
 os/ex/devices/ST/lis3dsh.h |  2 +-
 2 files changed, 23 insertions(+), 8 deletions(-)

diff --git a/os/ex/devices/ST/lis3dsh.c b/os/ex/devices/ST/lis3dsh.c
index 865ff79dd..4d11e8777 100644
--- a/os/ex/devices/ST/lis3dsh.c
+++ b/os/ex/devices/ST/lis3dsh.c
@@ -464,9 +464,10 @@ void lis3dshObjectInit(LIS3DSHDriver *devp) {
  *
  * @api
  */
-void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {
+msg_t lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {
   uint32_t i;
   uint8_t cr;
+  uint8_t devid;
   osalDbgCheck((devp != NULL) && (config != NULL));
 
   osalDbgAssert((devp->state == LIS3DSH_STOP) ||
@@ -475,6 +476,24 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {
 
   devp->config = config;
 
+#if LIS3DSH_USE_SPI
+#if LIS3DSH_SHARED_SPI
+  spiAcquireBus(devp->config->spip);
+#endif /* LIS3DSH_SHARED_SPI */
+  spiStart(devp->config->spip, devp->config->spicfg);
+
+
+  /* Check WHO_I_AM */
+  lis3dshSPIReadRegister(devp->config->spip, LIS3DSH_AD_WHO_AM_I,
+                         1, &devid);
+  if (devid != 0x3f)
+  {
+#if LIS3DSH_SHARED_SPI
+    spiReleaseBus(devp->config->spip);
+#endif /* LIS3DSH_SHARED_SPI */
+    return MSG_RESET;
+  }
+
   /* Control register 4 configuration block.*/
   {
     cr = LIS3DSH_CTRL_REG4_XEN | LIS3DSH_CTRL_REG4_YEN | LIS3DSH_CTRL_REG4_ZEN |
@@ -484,12 +503,6 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {
 #endif
   }
 
-#if LIS3DSH_USE_SPI
-#if LIS3DSH_SHARED_SPI
-  spiAcquireBus(devp->config->spip);
-#endif /* LIS3DSH_SHARED_SPI */
-  spiStart(devp->config->spip, devp->config->spicfg);
-
   lis3dshSPIWriteRegister(devp->config->spip, LIS3DSH_AD_CTRL_REG4, 1, &cr);
 
 #if LIS3DSH_SHARED_SPI
@@ -601,6 +614,8 @@ void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config) {
   osalThreadSleepMilliseconds(10);
 
   devp->state = LIS3DSH_READY;
+
+  return MSG_OK;
 }
 
 /**
diff --git a/os/ex/devices/ST/lis3dsh.h b/os/ex/devices/ST/lis3dsh.h
index 062f7b220..c6365b3e0 100644
--- a/os/ex/devices/ST/lis3dsh.h
+++ b/os/ex/devices/ST/lis3dsh.h
@@ -696,7 +696,7 @@ struct LIS3DSHDriver {
 extern "C" {
 #endif
   void lis3dshObjectInit(LIS3DSHDriver *devp);
-  void lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config);
+  msg_t lis3dshStart(LIS3DSHDriver *devp, const LIS3DSHConfig *config);
   void lis3dshStop(LIS3DSHDriver *devp);
 #ifdef __cplusplus
 }