diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index 2a9bac5d15..560cd6736c 100644 --- a/drivers/sensors/Kconfig +++ b/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 diff --git a/drivers/sensors/sensor.c b/drivers/sensors/sensor.c index 5736946870..e41115c93f 100644 --- a/drivers/sensors/sensor.c +++ b/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); - - if (upper->fds->revents != 0) + fd = upper->fds[i]; + if (fd) { - sninfo("Report events: %02x\n", upper->fds->revents); + fd->revents |= (fd->events & eventset); - nxsem_get_value(upper->fds->sem, &semcount); - if (semcount < 1) + if (fd->revents != 0) { - 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 - * mode is batch mode, and the number of bytes avaliable in buffer is - * less than the number of bytes origin. + /* Release some buffer space when current mode isn't batch mode + * and last mode is batch mode, and the number of bytes avaliable + * in buffer is less than the number of bytes origin. */ if (upper->latency == 0 && - upper->buffer->size > lower->buffer_bytes && - sensor_buffer_len(upper->buffer) <= lower->buffer_bytes) + upper->buffer->size > lower->buffer_size && + 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; - goto errout; + if (NULL == upper->fds[i]) + { + upper->fds[i] = fds; + fds->priv = &upper->fds[i]; + break; + } } - upper->fds = fds; - fds->priv = &upper->fds; + /* Don't have enough space to store 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; - - if (!slot) + for (i = 0; i < CONFIG_SENSORS_NPOLLWAITERS; i++) { - ret = -EIO; - goto errout; + if (fds == upper->fds[i]) + { + 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; diff --git a/drivers/sensors/wtgahrs2.c b/drivers/sensors/wtgahrs2.c index 6d617015bf..d3bd41b73f 100644 --- a/drivers/sensors/wtgahrs2.c +++ b/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) { diff --git a/include/nuttx/sensors/ioctl.h b/include/nuttx/sensors/ioctl.h index 39f09fc413..776a1fad9e 100644 --- a/include/nuttx/sensors/ioctl.h +++ b/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 - * sensor buffer. This buffer is used to solve the problem that the - * application layer can't read the sensor event in time. We recommend - * the number of sensor events in application layer's buffer is same as - * result by call this function. + * Note: Tell the application layer number of sensor events in sensor buffer. + * This buffer is used to solve the problem that the application layer + * can't read the sensor event in time. Recommend the number of sensor + * events in application layer's buffer is same as result by call this + * 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 */ diff --git a/include/nuttx/sensors/sensor.h b/include/nuttx/sensors/sensor.h index 310d0cd25b..f7c3aac08b 100644 --- a/include/nuttx/sensors/sensor.h +++ b/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. - * And we must flush fifo data to prevent data lost, then stop batch. + * You can exit batch mode by call this function with *latency_us = 0. + * 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. ****************************************************************************/