From ad20bd4bb35361a633169dec4557811b38197742 Mon Sep 17 00:00:00 2001 From: Jiuzhu Dong Date: Wed, 29 Jun 2022 17:34:55 +0800 Subject: [PATCH] driver/sensor: fix rpmsg send failed when buffer is NULL Signed-off-by: Jiuzhu Dong --- drivers/sensors/sensor.c | 76 +++++++++++++++++++--------------- drivers/sensors/sensor_rpmsg.c | 14 +++---- include/nuttx/sensors/ioctl.h | 7 ++++ include/nuttx/sensors/sensor.h | 10 +++++ 4 files changed, 67 insertions(+), 40 deletions(-) diff --git a/drivers/sensors/sensor.c b/drivers/sensors/sensor.c index cd8b06eccc..6e793801af 100644 --- a/drivers/sensors/sensor.c +++ b/drivers/sensors/sensor.c @@ -84,9 +84,7 @@ struct sensor_user_s * appear in dual role */ - unsigned long generation; /* Last generation subscriber has seen */ - unsigned long interval; /* The interval for subscriber */ - unsigned long latency; /* The bactch latency for subscriber */ + struct sensor_ustate_s state; }; /* This structure describes the state of the upper half driver */ @@ -200,29 +198,29 @@ static int sensor_update_interval(FAR struct file *filep, FAR struct sensor_user_s *tmp; unsigned long min_interval = interval; unsigned long min_latency = interval != ULONG_MAX ? - user->latency : ULONG_MAX; + user->state.latency : ULONG_MAX; int ret = 0; - if (interval == user->interval) + if (interval == user->state.interval) { return 0; } list_for_every_entry(&upper->userlist, tmp, struct sensor_user_s, node) { - if (tmp == user || tmp->interval == ULONG_MAX) + if (tmp == user || tmp->state.interval == ULONG_MAX) { continue; } - if (min_interval > tmp->interval) + if (min_interval > tmp->state.interval) { - min_interval = tmp->interval; + min_interval = tmp->state.interval; } - if (min_latency > tmp->latency) + if (min_latency > tmp->state.latency) { - min_latency = tmp->latency; + min_latency = tmp->state.latency; } } @@ -261,7 +259,7 @@ static int sensor_update_interval(FAR struct file *filep, } upper->state.min_interval = min_interval; - user->interval = interval; + user->state.interval = interval; sensor_pollnotify(upper, POLLPRI); return ret; } @@ -276,14 +274,14 @@ static int sensor_update_latency(FAR struct file *filep, unsigned long min_latency = latency; int ret = 0; - if (latency == user->latency) + if (latency == user->state.latency) { return 0; } - if (user->interval == ULONG_MAX) + if (user->state.interval == ULONG_MAX) { - user->latency = latency; + user->state.latency = latency; return 0; } @@ -294,14 +292,14 @@ static int sensor_update_latency(FAR struct file *filep, list_for_every_entry(&upper->userlist, tmp, struct sensor_user_s, node) { - if (tmp == user || tmp->interval == ULONG_MAX) + if (tmp == user || tmp->state.interval == ULONG_MAX) { continue; } - if (min_latency > tmp->latency) + if (min_latency > tmp->state.latency) { - min_latency = tmp->latency; + min_latency = tmp->state.latency; } } @@ -313,7 +311,7 @@ update: if (min_latency == upper->state.min_latency) { - user->latency = latency; + user->state.latency = latency; return ret; } @@ -327,7 +325,7 @@ update: } upper->state.min_latency = min_latency; - user->latency = latency; + user->state.latency = latency; sensor_pollnotify(upper, POLLPRI); return ret; } @@ -348,13 +346,13 @@ static void sensor_generate_timing(FAR struct sensor_upperhalf_s *upper, static bool sensor_is_updated(FAR struct sensor_upperhalf_s *upper, FAR struct sensor_user_s *user) { - long delta = upper->state.generation - user->generation; + long delta = upper->state.generation - user->state.generation; if (delta <= 0) { return false; } - else if (user->interval == ULONG_MAX) + else if (user->state.interval == ULONG_MAX) { return true; } @@ -368,7 +366,8 @@ static bool sensor_is_updated(FAR struct sensor_upperhalf_s *upper, * next generation user want */ - return delta >= user->interval - (upper->state.min_interval >> 1); + return delta >= user->state.interval - + (upper->state.min_interval >> 1); } } @@ -379,18 +378,19 @@ static void sensor_catch_up(FAR struct sensor_upperhalf_s *upper, long delta; circbuf_peek(&upper->timing, &generation, TIMING_BUF_ESIZE); - delta = generation - user->generation; + delta = generation - user->state.generation; if (delta > 0) { user->bufferpos = upper->timing.tail / TIMING_BUF_ESIZE; - if (user->interval == ULONG_MAX) + if (user->state.interval == ULONG_MAX) { - user->generation = generation - 1; + user->state.generation = generation - 1; } else { delta -= upper->state.min_interval >> 1; - user->generation += ROUND_DOWN(delta, user->interval); + user->state.generation += ROUND_DOWN(delta, + user->state.interval); } } } @@ -416,7 +416,7 @@ static ssize_t sensor_do_samples(FAR struct sensor_upperhalf_s *upper, /* Take samples continuously */ - if (user->interval == ULONG_MAX) + if (user->state.interval == ULONG_MAX) { ret = circbuf_peekat(&upper->buffer, user->bufferpos * upper->state.esize, @@ -424,7 +424,7 @@ static ssize_t sensor_do_samples(FAR struct sensor_upperhalf_s *upper, user->bufferpos += nums; circbuf_peekat(&upper->timing, (user->bufferpos - 1) * TIMING_BUF_ESIZE, - &user->generation, TIMING_BUF_ESIZE); + &user->state.generation, TIMING_BUF_ESIZE); return ret; } @@ -465,14 +465,14 @@ static ssize_t sensor_do_samples(FAR struct sensor_upperhalf_s *upper, } delta = next_generation + generation - - ((user->generation + user->interval) << 1); + ((user->state.generation + user->state.interval) << 1); if (delta >= 0) { ret += circbuf_peekat(&upper->buffer, (pos - 1) * upper->state.esize, buffer + ret, upper->state.esize); user->bufferpos = pos; - user->generation += user->interval; + user->state.generation += user->state.interval; if (ret >= len) { break; @@ -573,16 +573,17 @@ static int sensor_open(FAR struct file *filep) if (upper->state.generation && lower->persist) { - user->generation = upper->state.generation - 1; + user->state.generation = upper->state.generation - 1; user->bufferpos = upper->timing.head / TIMING_BUF_ESIZE - 1; } else { - user->generation = upper->state.generation; + user->state.generation = upper->state.generation; user->bufferpos = upper->timing.head / TIMING_BUF_ESIZE; } - user->interval = ULONG_MAX; + user->state.interval = ULONG_MAX; + user->state.esize = upper->state.esize; nxsem_init(&user->buffersem, 0, 0); nxsem_set_protocol(&user->buffersem, SEM_PRIO_NONE); list_add_tail(&upper->userlist, &user->node); @@ -748,6 +749,15 @@ static int sensor_ioctl(FAR struct file *filep, int cmd, unsigned long arg) } break; + case SNIOC_GET_USTATE: + { + nxrmutex_lock(&upper->lock); + memcpy((FAR void *)(uintptr_t)arg, + &user->state, sizeof(user->state)); + nxrmutex_unlock(&upper->lock); + } + break; + case SNIOC_SET_INTERVAL: { nxrmutex_lock(&upper->lock); diff --git a/drivers/sensors/sensor_rpmsg.c b/drivers/sensors/sensor_rpmsg.c index 2b608d3f3e..0243119f3a 100644 --- a/drivers/sensors/sensor_rpmsg.c +++ b/drivers/sensors/sensor_rpmsg.c @@ -726,22 +726,22 @@ static void sensor_rpmsg_push_event_one(FAR struct sensor_rpmsg_dev_s *dev, FAR struct sensor_rpmsg_cell_s *cell; FAR struct sensor_rpmsg_ept_s *sre; FAR struct sensor_rpmsg_data_s *msg; - struct sensor_state_s state; + struct sensor_ustate_s state; uint64_t now; bool updated; int ret; /* Get state of device to do send data with timeout */ - ret = file_ioctl(&stub->file, SNIOC_GET_STATE, &state); + ret = file_ioctl(&stub->file, SNIOC_GET_USTATE, &state); if (ret < 0) { return; } - if (state.min_interval == ULONG_MAX) + if (state.interval == ULONG_MAX) { - state.min_interval = 0; + state.interval = 0; } sre = container_of(stub->ept, struct sensor_rpmsg_ept_s, ept); @@ -807,7 +807,7 @@ static void sensor_rpmsg_push_event_one(FAR struct sensor_rpmsg_dev_s *dev, */ now = sensor_get_timestamp(); - if (sre->expire <= now) + if (sre->expire <= now && sre->buffer) { ret = rpmsg_send_nocopy(&sre->ept, sre->buffer, sre->written); sre->buffer = NULL; @@ -820,9 +820,9 @@ static void sensor_rpmsg_push_event_one(FAR struct sensor_rpmsg_dev_s *dev, else { if (sre->expire == UINT64_MAX || - sre->expire - now > state.min_interval / 2) + sre->expire - now > state.interval / 2) { - sre->expire = now + state.min_interval / 2; + sre->expire = now + state.interval / 2; } work_queue(HPWORK, &sre->work, sensor_rpmsg_data_worker, sre, diff --git a/include/nuttx/sensors/ioctl.h b/include/nuttx/sensors/ioctl.h index 992367cc14..403bac1a28 100644 --- a/include/nuttx/sensors/ioctl.h +++ b/include/nuttx/sensors/ioctl.h @@ -334,4 +334,11 @@ #define SNIOC_UPDATED _SNIOC(0x0091) +/* Command: SNIOC_GET_USTATE + * Description: Get state for user. + * Argument: This is the state pointer(struct sensor_state_s) + */ + +#define SNIOC_GET_USTATE _SNIOC(0x0092) + #endif /* __INCLUDE_NUTTX_SENSORS_IOCTL_H */ diff --git a/include/nuttx/sensors/sensor.h b/include/nuttx/sensors/sensor.h index 8af4eca43f..e58f41d3af 100644 --- a/include/nuttx/sensors/sensor.h +++ b/include/nuttx/sensors/sensor.h @@ -975,6 +975,16 @@ struct sensor_state_s FAR void *priv; /* The pointer to private data of userspace user */ }; +/* This structure describes the state for the sensor user */ + +struct sensor_ustate_s +{ + unsigned long esize; /* The element size of circular buffer */ + unsigned long latency; /* The batch latency for user, in us */ + unsigned long interval; /* The subscription interval for user, in us */ + unsigned long generation; /* The recent generation of circular buffer */ +}; + /* This structure describes the register info for the user sensor */ #ifdef CONFIG_USENSOR