forked from Qortal/Brooklyn
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.
421 lines
8.3 KiB
421 lines
8.3 KiB
// SPDX-License-Identifier: GPL-2.0 |
|
/* |
|
* GNSS receiver core |
|
* |
|
* Copyright (C) 2018 Johan Hovold <[email protected]> |
|
*/ |
|
|
|
#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); |
|
|
|
stream_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", |
|
[GNSS_TYPE_MTK] = "MTK", |
|
}; |
|
|
|
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 <[email protected]>"); |
|
MODULE_DESCRIPTION("GNSS receiver core"); |
|
MODULE_LICENSE("GPL v2");
|
|
|