Something went wrong. Try again.
Linux kernel mirror (for testing) git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
Something went wrong. Try again.
123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421// 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);
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,};
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_free(&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_obj(*gdev); if (!gdev) return NULL;
id = ida_alloc_max(&gnss_minors, GNSS_MINORS - 1, 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(const 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(const struct device *dev, struct kobj_uevent_env *env){ const 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("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");