driver/sensor: add new feature about sensor driver

1.support for multi-user access
2.support special cmd to control sensor
3.support userspace to set size of intermediate buffer
  by ioctl: SNOIC_SET_BUFFER_SIZE

Change-Id: I9ce3a65b88b12c28388ec397431f1a277b120c2a
Signed-off-by: dongjiuzhu <dongjiuzhu1@xiaomi.com>

drivers/sensors/Kconfig

@@ -11,6 +11,13 @@ menuconfig SENSORS
 
 if SENSORS
 
+config SENSORS_NPOLLWAITERS
+	int "Number of poll threads"
+	default 2
+	---help---
+		Maximum number of threads than can be waiting for POLL events.
+		Default: 2
+
 config SENSORS_WTGAHRS2
 	bool "Wtgahrs2 Sensor Support"
 	default n

drivers/sensors/sensor.c

@@ -73,9 +73,12 @@ struct sensor_buffer_s
 
 struct sensor_upperhalf_s
 {
+
+  /* poll structures of threads waiting for driver events. */
+
+  FAR struct pollfd             *fds[CONFIG_SENSORS_NPOLLWAITERS];
   FAR struct sensor_lowerhalf_s *lower;  /* the handle of lower half driver */
   FAR struct sensor_buffer_s    *buffer; /* The circualr buffer of sensor device */
-  FAR struct pollfd *fds;                /* poll structures of threads waiting for driver events. */
   uint8_t            crefs;              /* Number of times the device has been opened */
   sem_t              exclsem;            /* Manages exclusive access to file operations */
   sem_t              buffersem;          /* Wakeup user waiting for data in circular buffer */
@@ -297,20 +300,26 @@ static void sensor_buffer_release(FAR struct sensor_buffer_s *buffer)
 static void sensor_pollnotify(FAR struct sensor_upperhalf_s *upper,
                               pollevent_t eventset)
 {
+  FAR struct pollfd *fd;
   int semcount;
+  int i;
 
-  if (upper->fds)
+  for (i = 0; i < CONFIG_SENSORS_NPOLLWAITERS; i++)
     {
-      upper->fds->revents |= (upper->fds->events & eventset);
+      fd = upper->fds[i];
-
+      if (fd)
-      if (upper->fds->revents != 0)
         {
-          sninfo("Report events: %02x\n", upper->fds->revents);
+          fd->revents |= (fd->events & eventset);
 
-          nxsem_get_value(upper->fds->sem, &semcount);
+          if (fd->revents != 0)
-          if (semcount < 1)
             {
-              nxsem_post(upper->fds->sem);
+              sninfo("Report events: %02x\n", fd->revents);
+
+              nxsem_get_value(fd->sem, &semcount);
+              if (semcount < 1)
+                {
+                  nxsem_post(fd->sem);
+                }
             }
         }
     }
@@ -320,6 +329,7 @@ static int sensor_open(FAR struct file *filep)
 {
   FAR struct inode *inode = filep->f_inode;
   FAR struct sensor_upperhalf_s *upper = inode->i_private;
+  uint8_t tmp;
   int ret;
 
   ret = nxsem_wait(&upper->exclsem);
@@ -328,17 +338,21 @@ static int sensor_open(FAR struct file *filep)
       return ret;
     }
 
-  if (upper->crefs)
+  tmp = upper->crefs + 1;
+  if (tmp == 0)
     {
-      ret = -EBUSY;
+      /* More than 255 opens; uint8_t overflows to zero */
+
+      ret = -EMFILE;
+      goto err;
     }
-  else
+  else if (tmp == 1)
     {
-      upper->crefs++;
-      upper->fds = NULL;
       sensor_buffer_reset(upper->buffer);
     }
 
+  upper->crefs = tmp;
+err:
   nxsem_post(&upper->exclsem);
   return ret;
 }
@@ -421,16 +435,17 @@ static ssize_t sensor_read(FAR struct file *filep, FAR char *buffer,
 
   ret = sensor_buffer_pop(upper->buffer, buffer, len);
 
-  /* Release some buffer space when current mode isn't batch mode and last
+  /* Release some buffer space when current mode isn't batch mode
-   * mode is batch mode, and the number of bytes avaliable in buffer is
+   * and last mode is batch mode, and the number of bytes avaliable
-   * less than the number of bytes origin.
+   * in buffer is less than the number of bytes origin.
    */
 
   if (upper->latency == 0 &&
-      upper->buffer->size > lower->buffer_bytes &&
+      upper->buffer->size > lower->buffer_size &&
-      sensor_buffer_len(upper->buffer) <= lower->buffer_bytes)
+      sensor_buffer_len(upper->buffer) <= lower->buffer_size)
     {
-      sensor_buffer_resize(&upper->buffer, lower->type, lower->buffer_bytes);
+      sensor_buffer_resize(&upper->buffer, lower->type,
+                           lower->buffer_size);
     }
 
 again:
@@ -511,7 +526,7 @@ static int sensor_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
                   /* Adjust length of buffer in batch mode */
 
                   sensor_buffer_resize(&upper->buffer, lower->type,
-                                       lower->buffer_bytes +
+                                       lower->buffer_size +
                                        ROUNDUP(*val, upper->interval) /
                                        upper->interval *
                                        g_sensor_info[lower->type].esize);
@@ -522,12 +537,36 @@ static int sensor_ioctl(FAR struct file *filep, int cmd, unsigned long arg)
 
       case SNIOC_GET_NEVENTBUF:
         {
-          *val = lower->buffer_bytes / g_sensor_info[lower->type].esize;
+          *val = lower->buffer_size / g_sensor_info[lower->type].esize;
+        }
+        break;
+
+      case SNIOC_SET_BUFFER_SIZE:
+        {
+          if (*val != 0)
+            {
+              lower->buffer_size = ROUNDUP(*val,
+                                   g_sensor_info[lower->type].esize);
+              sensor_buffer_resize(&upper->buffer, lower->type,
+                                   lower->buffer_size);
+              *val = lower->buffer_size;
+            }
         }
         break;
 
       default:
-        ret = -ENOTTY;
+
+        /* Lowerhalf driver process other cmd. */
+
+        if (lower->ops->control)
+          {
+            ret = lower->ops->control(lower, cmd, arg);
+          }
+        else
+          {
+            ret = -ENOTTY;
+          }
+
         break;
     }
 
@@ -542,6 +581,7 @@ static int sensor_poll(FAR struct file *filep,
   FAR struct sensor_upperhalf_s *upper = inode->i_private;
   pollevent_t eventset = 0;
   int ret;
+  int i;
 
   ret = nxsem_wait(&upper->exclsem);
   if (ret < 0)
@@ -551,14 +591,23 @@ static int sensor_poll(FAR struct file *filep,
 
   if (setup)
     {
-      if (upper->fds)
+      for (i = 0; i < CONFIG_SENSORS_NPOLLWAITERS; i++)
         {
-          ret = -EBUSY;
+          if (NULL == upper->fds[i])
-          goto errout;
+            {
+              upper->fds[i] = fds;
+              fds->priv = &upper->fds[i];
+              break;
+            }
         }
 
-      upper->fds = fds;
+      /* Don't have enough space to store fds */
-      fds->priv = &upper->fds;
+
+      if (i == CONFIG_SENSORS_NPOLLWAITERS)
+        {
+          ret = -ENOSPC;
+          goto errout;
+        }
 
       if (!sensor_buffer_is_empty(upper->buffer))
         {
@@ -572,16 +621,15 @@ static int sensor_poll(FAR struct file *filep,
     }
   else if (fds->priv != NULL)
     {
-      FAR struct pollfd **slot = (FAR struct pollfd **)fds->priv;
+      for (i = 0; i < CONFIG_SENSORS_NPOLLWAITERS; i++)
-
-      if (!slot)
         {
-          ret = -EIO;
+          if (fds == upper->fds[i])
-          goto errout;
+            {
+              upper->fds[i] = NULL;
+              fds->priv = NULL;
+              break;
+            }
         }
-
-      *slot = NULL;
-      fds->priv = NULL;
     }
 
 errout:
@@ -590,7 +638,7 @@ errout:
 }
 
 static void sensor_push_event(FAR void *priv, FAR const void *data,
-                                     uint32_t bytes)
+                              uint32_t bytes)
 {
   FAR struct sensor_upperhalf_s *upper = priv;
   int semcount;
@@ -677,15 +725,15 @@ int sensor_register(FAR struct sensor_lowerhalf_s *lower, int devno)
   lower->priv = upper;
   lower->push_event = sensor_push_event;
 
-  if (!lower->buffer_bytes)
+  if (!lower->buffer_size)
     {
-      lower->buffer_bytes = g_sensor_info[lower->type].esize;
+      lower->buffer_size = g_sensor_info[lower->type].esize;
     }
 
   /* Initialize sensor buffer */
 
   ret = sensor_buffer_create(&upper->buffer,
-                             lower->type, lower->buffer_bytes);
+                             lower->type, lower->buffer_size);
   if (ret)
     {
       goto buf_err;

drivers/sensors/wtgahrs2.c

@@ -437,7 +437,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
 {
   FAR struct wtgahrs2_dev_s *rtdata;
   FAR struct wtgahrs2_sensor_s *tmp;
-#if CONFIG_SERIAL_TERMIOS
+#ifdef CONFIG_SERIAL_TERMIOS
   struct termios opt;
 #endif
   FAR char *argv[2];
@@ -466,7 +466,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
       goto open_err;
     }
 
-#if CONFIG_SERIAL_TERMIOS
+#ifdef CONFIG_SERIAL_TERMIOS
   file_ioctl(&rtdata->file, TCGETS, &opt);
   cfmakeraw(&opt);
   cfsetispeed(&opt, B115200);
@@ -479,7 +479,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
   tmp = &rtdata->dev[WTGAHRS2_ACCEL_IDX];
   tmp->lower.ops = &g_wtgahrs2_ops;
   tmp->lower.type = SENSOR_TYPE_ACCELEROMETER;
-  tmp->lower.buffer_bytes = sizeof(struct sensor_event_accel);
+  tmp->lower.buffer_size = sizeof(struct sensor_event_accel);
   ret = sensor_register(&tmp->lower, devno);
   if (ret < 0)
     {
@@ -491,7 +491,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
   tmp = &rtdata->dev[WTGAHRS2_GYRO_IDX];
   tmp->lower.ops = &g_wtgahrs2_ops;
   tmp->lower.type = SENSOR_TYPE_GYROSCOPE;
-  tmp->lower.buffer_bytes = sizeof(struct sensor_event_gyro);
+  tmp->lower.buffer_size = sizeof(struct sensor_event_gyro);
   ret = sensor_register(&tmp->lower, devno);
   if (ret < 0)
     {
@@ -503,7 +503,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
   tmp = &rtdata->dev[WTGAHRS2_MAG_IDX];
   tmp->lower.ops = &g_wtgahrs2_ops;
   tmp->lower.type = SENSOR_TYPE_MAGNETIC_FIELD;
-  tmp->lower.buffer_bytes = sizeof(struct sensor_event_mag);
+  tmp->lower.buffer_size = sizeof(struct sensor_event_mag);
   ret = sensor_register(&tmp->lower, devno);
   if (ret < 0)
     {
@@ -515,7 +515,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
   tmp = &rtdata->dev[WTGAHRS2_BARO_IDX];
   tmp->lower.ops = &g_wtgahrs2_ops;
   tmp->lower.type = SENSOR_TYPE_BAROMETER;
-  tmp->lower.buffer_bytes = sizeof(struct sensor_event_baro);
+  tmp->lower.buffer_size = sizeof(struct sensor_event_baro);
   ret = sensor_register(&tmp->lower, devno);
   if (ret < 0)
     {
@@ -527,7 +527,7 @@ int wtgahrs2_initialize(FAR const char *path, int devno)
   tmp = &rtdata->dev[WTGAHRS2_GPS_IDX];
   tmp->lower.ops = &g_wtgahrs2_ops;
   tmp->lower.type = SENSOR_TYPE_GPS;
-  tmp->lower.buffer_bytes = sizeof(struct sensor_event_gps);
+  tmp->lower.buffer_size = sizeof(struct sensor_event_gps);
   ret = sensor_register(&tmp->lower, devno);
   if (ret < 0)
     {

include/nuttx/sensors/ioctl.h

@@ -262,15 +262,25 @@
 /* Command:      SNIOC_GET_NEVENTBUF
  * Description:  the number of sensor events that sensor buffer of upper half holds.
  * Argument:     This is the number of events pointer, is output parameter.
- * Note:         We need to tell the application layer number of sensor events in
+ * Note:         Tell the application layer number of sensor events in sensor buffer.
- *               sensor buffer. This buffer is used to solve the problem that the
+ *               This buffer is used to solve the problem that the application layer
- *               application layer can't read the sensor event in time. We recommend
+ *               can't read the sensor event in time. Recommend the number of sensor
- *               the number of sensor events in application layer's buffer is same as
+ *               events in application layer's buffer is same as result by call this
- *               result by call this function.
+ *               function.
  *               This is number of sensor events rather than the length of buffer.
  *               See sensor.h(struct sensor_lower_half_s buffer_bytes).
  */
 
 #define SNIOC_GET_NEVENTBUF        _SNIOC(0x0070)
 
+/* Command:      SNIOC_SET_BUFFER_SIZE
+ * Description:  Set size of intermediate circualr buffer in upper half driver.
+ * Argument:     This is the size of buffer pointer.
+ * Note:         The application layer can set size of intermediate circualr buffer
+ *               by this ioctl command. The size is in bytes, it should be a multiple
+ *               of an event.
+ */
+
+#define SNIOC_SET_BUFFER_SIZE      _SNIOC(0x0071)
+
 #endif /* __INCLUDE_NUTTX_SENSORS_IOCTL_H */

include/nuttx/sensors/sensor.h

@@ -437,7 +437,7 @@ struct sensor_ops_s
    *   If *period_us > max_delay it will be truncated to max_dealy and if
    *   *period_us < min_delay it will be replaced by min_delay.
    *
-   *   Before changing the interval, we need to push the prepared data to
+   *   Before changing the interval, you need to push the prepared data to
    *   ensure that they are not lost.
    *
    * Input Parameters:
@@ -462,24 +462,25 @@ struct sensor_ops_s
    *   This function can be called while the sensor is activated,
    *   in which case it must not cause any sensor measurements to be lost.
    *   So, it is necessary to flush fifo or read ready data from data
-   *   register to prevent data lost before we using batch mode.
+   *   register to prevent data lost before you using batch mode.
    *
-   *   This sensor default mode isn't batch mode, so we need call this
+   *   This sensor default mode isn't batch mode, so you need call this
    *   function and *latency_us != 0.
    *   If *latency_us > max_report_latency it will be truncated to
    *   max_report_latency and return *latency_us to user
-   *   And we must flush fifo data to prevent data lost, then adjust latency.
+   *   And you must flush fifo data to prevent data lost, then adjust
+   *   latency.
    *
-   *   We can exit batch mode by call this function with *latency_us = 0.
+   *   You can exit batch mode by call this function with *latency_us = 0.
-   *   And we must flush fifo data to prevent data lost, then stop batch.
+   *   And you must flush fifo data to prevent data lost, then stop batch.
    *
    *   If sensor doesn't support batching (FIFO size zero), set batch to
    *   NULL.
    *
-   *   We must set interval by calling set_interval before calling batch(),
+   *   You must set interval by calling set_interval before calling batch(),
    *   othrewise, -EINVAL is returned.
    *
-   *   The reason why we don't have flush operation is that we need to push
+   *   The reason why you don't have flush operation is that you need to push
    *   the prepared data out before adjusting the latency to ensure that the
    *   data will not be lost.
    *
@@ -495,6 +496,27 @@ struct sensor_ops_s
 
   CODE int (*batch)(FAR struct sensor_lowerhalf_s *lower,
                     FAR unsigned int *latency_us);
+
+  /**************************************************************************
+   * Name: control
+   *
+   * In this method, we allow user to set some special config for the sensor,
+   * such as changing the custom mode, setting the custom resolution, reset,
+   * etc, which are all parsed and implemented by lower half driver.
+   *
+   * Input Parameters:
+   *   lower      - The instance of lower half sensor driver.
+   *   cmd        - The special cmd for sensor.
+   *   arg        - The parameters associated with cmd.
+   *
+   * Returned Value:
+   *   Zero (OK) on success; a negated errno value on failure.
+   *   -ENOTTY    - The cmd don't support.
+   *
+   **************************************************************************/
+
+  CODE int (*control)(FAR struct sensor_lowerhalf_s *lower,
+                      int cmd, unsigned long arg);
 };
 
 /* This structure is the generic form of state structure used by lower half
@@ -507,7 +529,7 @@ struct sensor_lowerhalf_s
 
   int type;
 
-  /* The bytes length of the circular buffer used.
+  /* The size of the circular buffer used, in bytes units.
    * This sensor circular buffer is used to slove issue that application
    * can't read sensor event in time. If this length of buffer is too large,
    * the latency of sensor event will be too larage. If the length of buffer
@@ -517,7 +539,7 @@ struct sensor_lowerhalf_s
    * or three length of sensor event.
    */
 
-  uint32_t buffer_bytes;
+  uint32_t buffer_size;
 
   /* The uncalibrated use to describe whether the sensor event is
    * uncalibrated. True is uncalibrated data, false is calibrated data,
@@ -582,7 +604,7 @@ extern "C"
  *
  * Input Parameters:
  *   dev   - A pointer to an instance of lower half sensor driver. This
- *           instance is bound to the sensor driver and must persists as long
+ *           instance is bound to the sensor driver and must persist as long
  *           as the driver persists.
  *   devno - The user specifies which device of this type, from 0. If the
  *           devno alerady exists, -EEXIST will be returned.
@@ -604,7 +626,7 @@ int sensor_register(FAR struct sensor_lowerhalf_s *dev, int devno);
  *
  * Input Parameters:
  *   dev   - A pointer to an instance of lower half sensor driver. This
- *           instance is bound to the sensor driver and must persists as long
+ *           instance is bound to the sensor driver and must persist as long
  *           as the driver persists.
  *   devno - The user specifies which device of this type, from 0.
  ****************************************************************************/