mirror of
https://git.kernel.org/pub/scm/linux/kernel/git/stable/linux.git
synced 2024-08-26 02:39:48 +00:00
![Dan Carpenter](/assets/img/avatar_default.png)
The gnss_allocate_device() function returns a mix of NULL and error
pointers on error. It should only return one or the other. Since the
callers both check for NULL, I've modified it to return NULL on error.
Fixes: 2b6a440351
("gnss: add GNSS receiver subsystem")
Signed-off-by: Dan Carpenter <dan.carpenter@oracle.com>
Signed-off-by: Johan Hovold <johan@kernel.org>
Signed-off-by: Greg Kroah-Hartman <gregkh@linuxfoundation.org>
420 lines
8.2 KiB
C
420 lines
8.2 KiB
C
// SPDX-License-Identifier: GPL-2.0
|
|
/*
|
|
* GNSS receiver core
|
|
*
|
|
* Copyright (C) 2018 Johan Hovold <johan@kernel.org>
|
|
*/
|
|
|
|
#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
|
|
|
|
#include <linux/cdev.h>
|
|
#include <linux/errno.h>
|
|
#include <linux/fs.h>
|
|
#include <linux/gnss.h>
|
|
#include <linux/idr.h>
|
|
#include <linux/init.h>
|
|
#include <linux/kernel.h>
|
|
#include <linux/module.h>
|
|
#include <linux/poll.h>
|
|
#include <linux/slab.h>
|
|
#include <linux/uaccess.h>
|
|
#include <linux/wait.h>
|
|
|
|
#define GNSS_FLAG_HAS_WRITE_RAW BIT(0)
|
|
|
|
#define GNSS_MINORS 16
|
|
|
|
static DEFINE_IDA(gnss_minors);
|
|
static dev_t gnss_first;
|
|
|
|
/* FIFO size must be a power of two */
|
|
#define GNSS_READ_FIFO_SIZE 4096
|
|
#define GNSS_WRITE_BUF_SIZE 1024
|
|
|
|
#define to_gnss_device(d) container_of((d), struct gnss_device, dev)
|
|
|
|
static int gnss_open(struct inode *inode, struct file *file)
|
|
{
|
|
struct gnss_device *gdev;
|
|
int ret = 0;
|
|
|
|
gdev = container_of(inode->i_cdev, struct gnss_device, cdev);
|
|
|
|
get_device(&gdev->dev);
|
|
|
|
nonseekable_open(inode, file);
|
|
file->private_data = gdev;
|
|
|
|
down_write(&gdev->rwsem);
|
|
if (gdev->disconnected) {
|
|
ret = -ENODEV;
|
|
goto unlock;
|
|
}
|
|
|
|
if (gdev->count++ == 0) {
|
|
ret = gdev->ops->open(gdev);
|
|
if (ret)
|
|
gdev->count--;
|
|
}
|
|
unlock:
|
|
up_write(&gdev->rwsem);
|
|
|
|
if (ret)
|
|
put_device(&gdev->dev);
|
|
|
|
return ret;
|
|
}
|
|
|
|
static int gnss_release(struct inode *inode, struct file *file)
|
|
{
|
|
struct gnss_device *gdev = file->private_data;
|
|
|
|
down_write(&gdev->rwsem);
|
|
if (gdev->disconnected)
|
|
goto unlock;
|
|
|
|
if (--gdev->count == 0) {
|
|
gdev->ops->close(gdev);
|
|
kfifo_reset(&gdev->read_fifo);
|
|
}
|
|
unlock:
|
|
up_write(&gdev->rwsem);
|
|
|
|
put_device(&gdev->dev);
|
|
|
|
return 0;
|
|
}
|
|
|
|
static ssize_t gnss_read(struct file *file, char __user *buf,
|
|
size_t count, loff_t *pos)
|
|
{
|
|
struct gnss_device *gdev = file->private_data;
|
|
unsigned int copied;
|
|
int ret;
|
|
|
|
mutex_lock(&gdev->read_mutex);
|
|
while (kfifo_is_empty(&gdev->read_fifo)) {
|
|
mutex_unlock(&gdev->read_mutex);
|
|
|
|
if (gdev->disconnected)
|
|
return 0;
|
|
|
|
if (file->f_flags & O_NONBLOCK)
|
|
return -EAGAIN;
|
|
|
|
ret = wait_event_interruptible(gdev->read_queue,
|
|
gdev->disconnected ||
|
|
!kfifo_is_empty(&gdev->read_fifo));
|
|
if (ret)
|
|
return -ERESTARTSYS;
|
|
|
|
mutex_lock(&gdev->read_mutex);
|
|
}
|
|
|
|
ret = kfifo_to_user(&gdev->read_fifo, buf, count, &copied);
|
|
if (ret == 0)
|
|
ret = copied;
|
|
|
|
mutex_unlock(&gdev->read_mutex);
|
|
|
|
return ret;
|
|
}
|
|
|
|
static ssize_t gnss_write(struct file *file, const char __user *buf,
|
|
size_t count, loff_t *pos)
|
|
{
|
|
struct gnss_device *gdev = file->private_data;
|
|
size_t written = 0;
|
|
int ret;
|
|
|
|
if (gdev->disconnected)
|
|
return -EIO;
|
|
|
|
if (!count)
|
|
return 0;
|
|
|
|
if (!(gdev->flags & GNSS_FLAG_HAS_WRITE_RAW))
|
|
return -EIO;
|
|
|
|
/* Ignoring O_NONBLOCK, write_raw() is synchronous. */
|
|
|
|
ret = mutex_lock_interruptible(&gdev->write_mutex);
|
|
if (ret)
|
|
return -ERESTARTSYS;
|
|
|
|
for (;;) {
|
|
size_t n = count - written;
|
|
|
|
if (n > GNSS_WRITE_BUF_SIZE)
|
|
n = GNSS_WRITE_BUF_SIZE;
|
|
|
|
if (copy_from_user(gdev->write_buf, buf, n)) {
|
|
ret = -EFAULT;
|
|
goto out_unlock;
|
|
}
|
|
|
|
/*
|
|
* Assumes write_raw can always accept GNSS_WRITE_BUF_SIZE
|
|
* bytes.
|
|
*
|
|
* FIXME: revisit
|
|
*/
|
|
down_read(&gdev->rwsem);
|
|
if (!gdev->disconnected)
|
|
ret = gdev->ops->write_raw(gdev, gdev->write_buf, n);
|
|
else
|
|
ret = -EIO;
|
|
up_read(&gdev->rwsem);
|
|
|
|
if (ret < 0)
|
|
break;
|
|
|
|
written += ret;
|
|
buf += ret;
|
|
|
|
if (written == count)
|
|
break;
|
|
}
|
|
|
|
if (written)
|
|
ret = written;
|
|
out_unlock:
|
|
mutex_unlock(&gdev->write_mutex);
|
|
|
|
return ret;
|
|
}
|
|
|
|
static __poll_t gnss_poll(struct file *file, poll_table *wait)
|
|
{
|
|
struct gnss_device *gdev = file->private_data;
|
|
__poll_t mask = 0;
|
|
|
|
poll_wait(file, &gdev->read_queue, wait);
|
|
|
|
if (!kfifo_is_empty(&gdev->read_fifo))
|
|
mask |= EPOLLIN | EPOLLRDNORM;
|
|
if (gdev->disconnected)
|
|
mask |= EPOLLHUP;
|
|
|
|
return mask;
|
|
}
|
|
|
|
static const struct file_operations gnss_fops = {
|
|
.owner = THIS_MODULE,
|
|
.open = gnss_open,
|
|
.release = gnss_release,
|
|
.read = gnss_read,
|
|
.write = gnss_write,
|
|
.poll = gnss_poll,
|
|
.llseek = no_llseek,
|
|
};
|
|
|
|
static struct class *gnss_class;
|
|
|
|
static void gnss_device_release(struct device *dev)
|
|
{
|
|
struct gnss_device *gdev = to_gnss_device(dev);
|
|
|
|
kfree(gdev->write_buf);
|
|
kfifo_free(&gdev->read_fifo);
|
|
ida_simple_remove(&gnss_minors, gdev->id);
|
|
kfree(gdev);
|
|
}
|
|
|
|
struct gnss_device *gnss_allocate_device(struct device *parent)
|
|
{
|
|
struct gnss_device *gdev;
|
|
struct device *dev;
|
|
int id;
|
|
int ret;
|
|
|
|
gdev = kzalloc(sizeof(*gdev), GFP_KERNEL);
|
|
if (!gdev)
|
|
return NULL;
|
|
|
|
id = ida_simple_get(&gnss_minors, 0, GNSS_MINORS, GFP_KERNEL);
|
|
if (id < 0) {
|
|
kfree(gdev);
|
|
return NULL;
|
|
}
|
|
|
|
gdev->id = id;
|
|
|
|
dev = &gdev->dev;
|
|
device_initialize(dev);
|
|
dev->devt = gnss_first + id;
|
|
dev->class = gnss_class;
|
|
dev->parent = parent;
|
|
dev->release = gnss_device_release;
|
|
dev_set_drvdata(dev, gdev);
|
|
dev_set_name(dev, "gnss%d", id);
|
|
|
|
init_rwsem(&gdev->rwsem);
|
|
mutex_init(&gdev->read_mutex);
|
|
mutex_init(&gdev->write_mutex);
|
|
init_waitqueue_head(&gdev->read_queue);
|
|
|
|
ret = kfifo_alloc(&gdev->read_fifo, GNSS_READ_FIFO_SIZE, GFP_KERNEL);
|
|
if (ret)
|
|
goto err_put_device;
|
|
|
|
gdev->write_buf = kzalloc(GNSS_WRITE_BUF_SIZE, GFP_KERNEL);
|
|
if (!gdev->write_buf)
|
|
goto err_put_device;
|
|
|
|
cdev_init(&gdev->cdev, &gnss_fops);
|
|
gdev->cdev.owner = THIS_MODULE;
|
|
|
|
return gdev;
|
|
|
|
err_put_device:
|
|
put_device(dev);
|
|
|
|
return NULL;
|
|
}
|
|
EXPORT_SYMBOL_GPL(gnss_allocate_device);
|
|
|
|
void gnss_put_device(struct gnss_device *gdev)
|
|
{
|
|
put_device(&gdev->dev);
|
|
}
|
|
EXPORT_SYMBOL_GPL(gnss_put_device);
|
|
|
|
int gnss_register_device(struct gnss_device *gdev)
|
|
{
|
|
int ret;
|
|
|
|
/* Set a flag which can be accessed without holding the rwsem. */
|
|
if (gdev->ops->write_raw != NULL)
|
|
gdev->flags |= GNSS_FLAG_HAS_WRITE_RAW;
|
|
|
|
ret = cdev_device_add(&gdev->cdev, &gdev->dev);
|
|
if (ret) {
|
|
dev_err(&gdev->dev, "failed to add device: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
EXPORT_SYMBOL_GPL(gnss_register_device);
|
|
|
|
void gnss_deregister_device(struct gnss_device *gdev)
|
|
{
|
|
down_write(&gdev->rwsem);
|
|
gdev->disconnected = true;
|
|
if (gdev->count) {
|
|
wake_up_interruptible(&gdev->read_queue);
|
|
gdev->ops->close(gdev);
|
|
}
|
|
up_write(&gdev->rwsem);
|
|
|
|
cdev_device_del(&gdev->cdev, &gdev->dev);
|
|
}
|
|
EXPORT_SYMBOL_GPL(gnss_deregister_device);
|
|
|
|
/*
|
|
* Caller guarantees serialisation.
|
|
*
|
|
* Must not be called for a closed device.
|
|
*/
|
|
int gnss_insert_raw(struct gnss_device *gdev, const unsigned char *buf,
|
|
size_t count)
|
|
{
|
|
int ret;
|
|
|
|
ret = kfifo_in(&gdev->read_fifo, buf, count);
|
|
|
|
wake_up_interruptible(&gdev->read_queue);
|
|
|
|
return ret;
|
|
}
|
|
EXPORT_SYMBOL_GPL(gnss_insert_raw);
|
|
|
|
static const char * const gnss_type_names[GNSS_TYPE_COUNT] = {
|
|
[GNSS_TYPE_NMEA] = "NMEA",
|
|
[GNSS_TYPE_SIRF] = "SiRF",
|
|
[GNSS_TYPE_UBX] = "UBX",
|
|
};
|
|
|
|
static const char *gnss_type_name(struct gnss_device *gdev)
|
|
{
|
|
const char *name = NULL;
|
|
|
|
if (gdev->type < GNSS_TYPE_COUNT)
|
|
name = gnss_type_names[gdev->type];
|
|
|
|
if (!name)
|
|
dev_WARN(&gdev->dev, "type name not defined\n");
|
|
|
|
return name;
|
|
}
|
|
|
|
static ssize_t type_show(struct device *dev, struct device_attribute *attr,
|
|
char *buf)
|
|
{
|
|
struct gnss_device *gdev = to_gnss_device(dev);
|
|
|
|
return sprintf(buf, "%s\n", gnss_type_name(gdev));
|
|
}
|
|
static DEVICE_ATTR_RO(type);
|
|
|
|
static struct attribute *gnss_attrs[] = {
|
|
&dev_attr_type.attr,
|
|
NULL,
|
|
};
|
|
ATTRIBUTE_GROUPS(gnss);
|
|
|
|
static int gnss_uevent(struct device *dev, struct kobj_uevent_env *env)
|
|
{
|
|
struct gnss_device *gdev = to_gnss_device(dev);
|
|
int ret;
|
|
|
|
ret = add_uevent_var(env, "GNSS_TYPE=%s", gnss_type_name(gdev));
|
|
if (ret)
|
|
return ret;
|
|
|
|
return 0;
|
|
}
|
|
|
|
static int __init gnss_module_init(void)
|
|
{
|
|
int ret;
|
|
|
|
ret = alloc_chrdev_region(&gnss_first, 0, GNSS_MINORS, "gnss");
|
|
if (ret < 0) {
|
|
pr_err("failed to allocate device numbers: %d\n", ret);
|
|
return ret;
|
|
}
|
|
|
|
gnss_class = class_create(THIS_MODULE, "gnss");
|
|
if (IS_ERR(gnss_class)) {
|
|
ret = PTR_ERR(gnss_class);
|
|
pr_err("failed to create class: %d\n", ret);
|
|
goto err_unregister_chrdev;
|
|
}
|
|
|
|
gnss_class->dev_groups = gnss_groups;
|
|
gnss_class->dev_uevent = gnss_uevent;
|
|
|
|
pr_info("GNSS driver registered with major %d\n", MAJOR(gnss_first));
|
|
|
|
return 0;
|
|
|
|
err_unregister_chrdev:
|
|
unregister_chrdev_region(gnss_first, GNSS_MINORS);
|
|
|
|
return ret;
|
|
}
|
|
module_init(gnss_module_init);
|
|
|
|
static void __exit gnss_module_exit(void)
|
|
{
|
|
class_destroy(gnss_class);
|
|
unregister_chrdev_region(gnss_first, GNSS_MINORS);
|
|
ida_destroy(&gnss_minors);
|
|
}
|
|
module_exit(gnss_module_exit);
|
|
|
|
MODULE_AUTHOR("Johan Hovold <johan@kernel.org>");
|
|
MODULE_DESCRIPTION("GNSS receiver core");
|
|
MODULE_LICENSE("GPL v2");
|