From 1613ded06075edffb9fb530e7ccc05c021eea916 Mon Sep 17 00:00:00 2001
From: SPRESENSE <41312067+SPRESENSE@users.noreply.github.com>
Date: Fri, 25 Feb 2022 12:21:09 +0900
Subject: [PATCH] drivers/video/isx012: Add exclusive controls for I2C access

Add exclusive control to prevent from I2C write(for sending requst) and
I2C read(for receiving response).
---
 drivers/video/isx012.c | 43 +++++++++++++++++++++++++++++++++---------
 1 file changed, 34 insertions(+), 9 deletions(-)

diff --git a/drivers/video/isx012.c b/drivers/video/isx012.c
index c9ada689d3..e31b7902ce 100644
--- a/drivers/video/isx012.c
+++ b/drivers/video/isx012.c
@@ -30,6 +30,7 @@
 #include <errno.h>
 #include <debug.h>
 
+#include <nuttx/semaphore.h>
 #include <nuttx/arch.h>
 #include <nuttx/signal.h>
 #include <nuttx/fs/fs.h>
@@ -168,6 +169,7 @@ typedef struct isx012_reg_s isx012_reg_t;
 
 struct isx012_dev_s
 {
+  sem_t                   i2c_lock;
   FAR struct i2c_master_s *i2c;        /* I2C interface */
   uint8_t                 i2c_addr;    /* I2C address */
   int                     i2c_freq;    /* Frequency */
@@ -635,6 +637,16 @@ static struct imgsensor_ops_s g_isx012_ops =
  * Private Functions
  ****************************************************************************/
 
+static void i2c_lock(void)
+{
+  nxsem_wait_uninterruptible(&g_isx012_private.i2c_lock);
+}
+
+static void i2c_unlock(void)
+{
+  nxsem_post(&g_isx012_private.i2c_lock);
+}
+
 static uint16_t isx012_getreg(isx012_dev_t *priv,
                               uint16_t regaddr, uint16_t regsize)
 {
@@ -651,25 +663,32 @@ static uint16_t isx012_getreg(isx012_dev_t *priv,
   buffer[0] = regaddr >> 8;
   buffer[1] = regaddr & 0xff;
 
+  i2c_lock();
+
   /* Write the register address */
 
   ret = i2c_write(priv->i2c, &config, (uint8_t *)buffer, 2);
   if (ret < 0)
     {
       verr("i2c_write failed: %d\n", ret);
-      return 0;
     }
-
-  /* Restart and read 16bits from the register */
-
-  ret = i2c_read(priv->i2c, &config, (uint8_t *)buffer, regsize);
-  if (ret < 0)
+  else
     {
-      verr("i2c_read failed: %d\n", ret);
-      return 0;
+      /* Restart and read 16bits from the register */
+
+      ret = i2c_read(priv->i2c, &config, (uint8_t *)buffer, regsize);
+      if (ret < 0)
+        {
+          verr("i2c_read failed: %d\n", ret);
+        }
     }
 
-  memcpy((uint8_t *)&regval, (uint8_t *)buffer, regsize);
+  i2c_unlock();
+
+  if (ret >= 0)
+    {
+      memcpy((uint8_t *)&regval, (uint8_t *)buffer, regsize);
+    }
 
   return regval;
 }
@@ -694,6 +713,8 @@ static int isx012_putreg(isx012_dev_t *priv,
 
   memcpy((uint8_t *)&buffer[2], (uint8_t *)&regval, regsize);
 
+  i2c_lock();
+
   /* And do it */
 
   ret = i2c_write(priv->i2c, &config,
@@ -703,6 +724,8 @@ static int isx012_putreg(isx012_dev_t *priv,
       verr("i2c_write failed: %d\n", ret);
     }
 
+  i2c_unlock();
+
   return ret;
 }
 
@@ -2924,6 +2947,8 @@ int isx012_initialize(void)
 
   priv->state      = STATE_ISX012_POWEROFF;
 
+  nxsem_init(&priv->i2c_lock, 0, 1);
+
   return OK;
 }