From b3ea6522f4083813c7e010b535e8a1732e59854f Mon Sep 17 00:00:00 2001 From: Jiuzhu Dong Date: Thu, 3 Mar 2022 21:40:58 +0800 Subject: [PATCH] driver/usensor: support register user sensor 1.cmd:SNIOC_REGISTER with struct sensor_reginfo_s to register user sensor 2.cmd:SNIOC_UNREGISTER to unregister user sensor Signed-off-by: Jiuzhu Dong --- drivers/sensors/Kconfig | 6 + drivers/sensors/Make.defs | 4 + drivers/sensors/usensor.c | 249 +++++++++++++++++++++++++++++++++ include/nuttx/sensors/ioctl.h | 20 +++ include/nuttx/sensors/sensor.h | 25 ++++ 5 files changed, 304 insertions(+) create mode 100644 drivers/sensors/usensor.c diff --git a/drivers/sensors/Kconfig b/drivers/sensors/Kconfig index f6e9259f90..d4d50e8d87 100644 --- a/drivers/sensors/Kconfig +++ b/drivers/sensors/Kconfig @@ -19,6 +19,12 @@ config SENSORS_NPOLLWAITERS Maximum number of threads than can be waiting for POLL events. Default: 2 +config USENSOR + bool "Usensor Device Support" + default n + ---help--- + Allow application to register user sensor by /dev/usensor. + config SENSORS_WTGAHRS2 bool "Wtgahrs2 Sensor Support" default n diff --git a/drivers/sensors/Make.defs b/drivers/sensors/Make.defs index 70277a90c3..f372b78ec2 100644 --- a/drivers/sensors/Make.defs +++ b/drivers/sensors/Make.defs @@ -24,6 +24,10 @@ ifeq ($(CONFIG_SENSORS),y) CSRCS += sensor.c +ifeq ($(CONFIG_USENSOR),y) +CSRCS += usensor.c +endif + ifeq ($(CONFIG_SENSORS_WTGAHRS2),y) CSRCS += wtgahrs2.c endif diff --git a/drivers/sensors/usensor.c b/drivers/sensors/usensor.c new file mode 100644 index 0000000000..a32a97e3f4 --- /dev/null +++ b/drivers/sensors/usensor.c @@ -0,0 +1,249 @@ +/**************************************************************************** + * drivers/sensors/usensor.c + * + * Licensed to the Apache Software Foundation (ASF) under one or more + * contributor license agreements. See the NOTICE file distributed with + * this work for additional information regarding copyright ownership. The + * ASF licenses this file to you under the Apache License, Version 2.0 (the + * "License"); you may not use this file except in compliance with the + * License. You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT + * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the + * License for the specific language governing permissions and limitations + * under the License. + * + ****************************************************************************/ + +/**************************************************************************** + * Included Files + ****************************************************************************/ + +#include +#include +#include +#include +#include +#include + +/**************************************************************************** + * Pre-processor Definitions + ****************************************************************************/ + +#define USENSOR_PATH "/dev/usensor" + +/**************************************************************************** + * Private Function Prototypes + ****************************************************************************/ + +static int usensor_open(FAR struct file *filep); +static int usensor_close(FAR struct file *filep); +static ssize_t usensor_read(FAR struct file *filep, FAR char *buffer, + size_t buflen); +static ssize_t usensor_write(FAR struct file *filep, FAR const char *buffer, + size_t buflen); +static int usensor_ioctl(FAR struct file *filep, int cmd, + unsigned long arg); + +/**************************************************************************** + * Private Types + ****************************************************************************/ + +/* This structure describes context of usensor */ + +struct usensor_context_s +{ + sem_t exclsem; /* Manages exclusive access to file operations */ + struct list_node list; /* List of node registered */ +}; + +/* This structure describes lowerhalf driver for an usensor registered */ + +struct usensor_lowerhalf_s +{ + struct list_node node; /* node in all usensor list */ + struct sensor_lowerhalf_s driver; /* The lowerhalf driver of user sensor */ + char path[1]; /* The path of usensor character device */ +}; + +/**************************************************************************** + * Private Data + ****************************************************************************/ + +static const struct file_operations g_usensor_fops = +{ + usensor_open, /* open */ + usensor_close, /* close */ + usensor_read, /* read */ + usensor_write, /* write */ + NULL, /* seek */ + usensor_ioctl, /* ioctl */ + NULL, /* poll */ +}; + +static const struct sensor_ops_s g_usensor_ops = +{ +}; + +/**************************************************************************** + * Private Functions + ****************************************************************************/ + +static int usensor_register(FAR struct usensor_context_s *usensor, + FAR const struct sensor_reginfo_s *info) +{ + FAR struct usensor_lowerhalf_s *lower; + int ret; + + lower = kmm_zalloc(sizeof(*lower) + strlen(info->path)); + if (!lower) + { + return -ENOMEM; + } + + lower->driver.buffer_number = info->nqueue; + lower->driver.ops = &g_usensor_ops; + strcpy(lower->path, info->path); + ret = sensor_custom_register(&lower->driver, lower->path, info->esize); + if (ret < 0) + { + goto errout_with_lower; + } + + ret = nxsem_wait(&usensor->exclsem); + if (ret < 0) + { + goto errout_with_register; + } + + list_add_tail(&usensor->list, &lower->node); + nxsem_post(&usensor->exclsem); + + return ret; + +errout_with_register: + sensor_custom_unregister(&lower->driver, info->path); +errout_with_lower: + kmm_free(lower); + return ret; +} + +static int usensor_unregister(FAR struct usensor_context_s *usensor, + FAR const char *path) +{ + FAR struct usensor_lowerhalf_s *lower; + int ret; + + ret = nxsem_wait(&usensor->exclsem); + if (ret < 0) + { + return ret; + } + + list_for_every_entry(&usensor->list, lower, struct usensor_lowerhalf_s, + node) + { + if (strcmp(path, lower->path) == 0) + { + list_delete(&lower->node); + nxsem_post(&usensor->exclsem); + sensor_custom_unregister(&lower->driver, path); + kmm_free(lower); + return 0; + } + } + + nxsem_post(&usensor->exclsem); + return -ENOENT; +} + +static int usensor_open(FAR struct file *filep) +{ + return 0; +} + +static int usensor_close(FAR struct file *filep) +{ + return 0; +} + +static ssize_t usensor_write(FAR struct file *filep, FAR const char *buffer, + size_t buflen) +{ + return buflen; +} + +static ssize_t usensor_read(FAR struct file *filep, FAR char *buffer, + size_t buflen) +{ + return buflen; +} + +static int usensor_ioctl(FAR struct file *filep, int cmd, + unsigned long arg) +{ + FAR struct inode *inode = filep->f_inode; + FAR struct usensor_context_s *usensor = inode->i_private; + int ret = -ENOTTY; + + if (cmd == SNIOC_REGISTER) + { + FAR const struct sensor_reginfo_s *info; + + info = (FAR const struct sensor_reginfo_s *)(uintptr_t)arg; + ret = usensor_register(usensor, info); + } + else if (cmd == SNIOC_UNREGISTER) + { + FAR const char *path; + + path = (FAR const char *)(uintptr_t)arg; + ret = usensor_unregister(usensor, path); + } + + return ret; +} + +/**************************************************************************** + * Public Functions + ****************************************************************************/ + +/**************************************************************************** + * Name: usensor_initialize + * + * Description: + * This function registers usensor character node "/dev/usensor", so that + * application can register user sensor by this node. The node will + * manager all user sensors in this character dirver. + ****************************************************************************/ + +int usensor_initialize(void) +{ + FAR struct usensor_context_s *usensor; + int ret; + + usensor = kmm_zalloc(sizeof(*usensor)); + if (!usensor) + { + return -ENOMEM; + } + + nxsem_init(&usensor->exclsem, 0, 1); + list_initialize(&usensor->list); + + ret = register_driver(USENSOR_PATH, &g_usensor_fops, 0666, usensor); + if (ret < 0) + { + goto errout_with_sem; + } + + return ret; + +errout_with_sem: + nxsem_destroy(&usensor->exclsem); + kmm_free(usensor); + return ret; +} diff --git a/include/nuttx/sensors/ioctl.h b/include/nuttx/sensors/ioctl.h index ff9d76679e..210708f38d 100644 --- a/include/nuttx/sensors/ioctl.h +++ b/include/nuttx/sensors/ioctl.h @@ -315,4 +315,24 @@ #define SNIOC_CALIBRATE _SNIOC(0x0088) +#ifdef CONFIG_USENSOR +/* Command: SNIOC_REGISTER + * Description: Register user sensor. + * Argument: A pointer of structure sensor_reginfo_s for user sensor. + * Note: If register is failed, return errno, otherwise, + * return OK. + */ + +#define SNIOC_REGISTER _SNIOC(0x0089) + +/* Command: SNIOC_UNREGISTER + * Description: Unregister user sensor. + * Argument: The path name of user sensor character node. + * Note: If register is failed, return errno, otherwise, + * return OK. + */ + +#define SNIOC_UNREGISTER _SNIOC(0x0090) +#endif + #endif /* __INCLUDE_NUTTX_SENSORS_IOCTL_H */ diff --git a/include/nuttx/sensors/sensor.h b/include/nuttx/sensors/sensor.h index 0285144904..6c694be01f 100644 --- a/include/nuttx/sensors/sensor.h +++ b/include/nuttx/sensors/sensor.h @@ -874,6 +874,17 @@ struct sensor_lowerhalf_s FAR void *priv; }; +/* This structure describes the register info for the user sensor */ + +#ifdef CONFIG_USENSOR +struct sensor_reginfo_s +{ + FAR const char *path; /* The path of user sensor */ + uint16_t esize; /* The element size of user sensor */ + uint32_t nqueue; /* The number of queue buffered elements */ +}; +#endif + /**************************************************************************** * Public Function Prototypes ****************************************************************************/ @@ -978,6 +989,20 @@ void sensor_unregister(FAR struct sensor_lowerhalf_s *dev, int devno); void sensor_custom_unregister(FAR struct sensor_lowerhalf_s *dev, FAR const char *path); + +/**************************************************************************** + * Name: usensor_initialize + * + * Description: + * This function registers usensor character node "/dev/usensor", so that + * application can register user sensor by this node. The node will + * manager all user sensor in this character dirver. + ****************************************************************************/ + +#ifdef CONFIG_USENSOR +int usensor_initialize(void); +#endif + #undef EXTERN #if defined(__cplusplus) }