You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
420 lines
8.2 KiB
420 lines
8.2 KiB
// 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 unsigned int gnss_poll(struct file *file, poll_table *wait)
|
|
{
|
|
struct gnss_device *gdev = file->private_data;
|
|
unsigned int mask = 0;
|
|
|
|
poll_wait(file, &gdev->read_queue, wait);
|
|
|
|
if (!kfifo_is_empty(&gdev->read_fifo))
|
|
mask |= POLLIN | POLLRDNORM;
|
|
if (gdev->disconnected)
|
|
mask |= POLLHUP;
|
|
|
|
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");
|
|
|