drivers/gnss/core.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * GNSS receiver core
0004  *
0005  * Copyright (C) 2018 Johan Hovold <johan@kernel.org>
0006  */
0007
0008 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
0009
0010 #include <linux/cdev.h>
0011 #include <linux/errno.h>
0012 #include <linux/fs.h>
0013 #include <linux/gnss.h>
0014 #include <linux/idr.h>
0015 #include <linux/init.h>
0016 #include <linux/kernel.h>
0017 #include <linux/module.h>
0018 #include <linux/poll.h>
0019 #include <linux/slab.h>
0020 #include <linux/uaccess.h>
0021 #include <linux/wait.h>
0022
0023 #define GNSS_FLAG_HAS_WRITE_RAW     BIT(0)
0024
0025 #define GNSS_MINORS 16
0026
0027 static DEFINE_IDA(gnss_minors);
0028 static dev_t gnss_first;
0029
0030 /* FIFO size must be a power of two */
0031 #define GNSS_READ_FIFO_SIZE 4096
0032 #define GNSS_WRITE_BUF_SIZE 1024
0033
0034 #define to_gnss_device(d) container_of((d), struct gnss_device, dev)
0035
0036 static int gnss_open(struct inode *inode, struct file *file)
0037 {
0038     struct gnss_device *gdev;
0039     int ret = 0;
0040
0041     gdev = container_of(inode->i_cdev, struct gnss_device, cdev);
0042
0043     get_device(&gdev->dev);
0044
0045     stream_open(inode, file);
0046     file->private_data = gdev;
0047
0048     down_write(&gdev->rwsem);
0049     if (gdev->disconnected) {
0050         ret = -ENODEV;
0051         goto unlock;
0052     }
0053
0054     if (gdev->count++ == 0) {
0055         ret = gdev->ops->open(gdev);
0056         if (ret)
0057             gdev->count--;
0058     }
0059 unlock:
0060     up_write(&gdev->rwsem);
0061
0062     if (ret)
0063         put_device(&gdev->dev);
0064
0065     return ret;
0066 }
0067
0068 static int gnss_release(struct inode *inode, struct file *file)
0069 {
0070     struct gnss_device *gdev = file->private_data;
0071
0072     down_write(&gdev->rwsem);
0073     if (gdev->disconnected)
0074         goto unlock;
0075
0076     if (--gdev->count == 0) {
0077         gdev->ops->close(gdev);
0078         kfifo_reset(&gdev->read_fifo);
0079     }
0080 unlock:
0081     up_write(&gdev->rwsem);
0082
0083     put_device(&gdev->dev);
0084
0085     return 0;
0086 }
0087
0088 static ssize_t gnss_read(struct file *file, char __user *buf,
0089                 size_t count, loff_t *pos)
0090 {
0091     struct gnss_device *gdev = file->private_data;
0092     unsigned int copied;
0093     int ret;
0094
0095     mutex_lock(&gdev->read_mutex);
0096     while (kfifo_is_empty(&gdev->read_fifo)) {
0097         mutex_unlock(&gdev->read_mutex);
0098
0099         if (gdev->disconnected)
0100             return 0;
0101
0102         if (file->f_flags & O_NONBLOCK)
0103             return -EAGAIN;
0104
0105         ret = wait_event_interruptible(gdev->read_queue,
0106                 gdev->disconnected ||
0107                 !kfifo_is_empty(&gdev->read_fifo));
0108         if (ret)
0109             return -ERESTARTSYS;
0110
0111         mutex_lock(&gdev->read_mutex);
0112     }
0113
0114     ret = kfifo_to_user(&gdev->read_fifo, buf, count, &copied);
0115     if (ret == 0)
0116         ret = copied;
0117
0118     mutex_unlock(&gdev->read_mutex);
0119
0120     return ret;
0121 }
0122
0123 static ssize_t gnss_write(struct file *file, const char __user *buf,
0124                 size_t count, loff_t *pos)
0125 {
0126     struct gnss_device *gdev = file->private_data;
0127     size_t written = 0;
0128     int ret;
0129
0130     if (gdev->disconnected)
0131         return -EIO;
0132
0133     if (!count)
0134         return 0;
0135
0136     if (!(gdev->flags & GNSS_FLAG_HAS_WRITE_RAW))
0137         return -EIO;
0138
0139     /* Ignoring O_NONBLOCK, write_raw() is synchronous. */
0140
0141     ret = mutex_lock_interruptible(&gdev->write_mutex);
0142     if (ret)
0143         return -ERESTARTSYS;
0144
0145     for (;;) {
0146         size_t n = count - written;
0147
0148         if (n > GNSS_WRITE_BUF_SIZE)
0149             n = GNSS_WRITE_BUF_SIZE;
0150
0151         if (copy_from_user(gdev->write_buf, buf, n)) {
0152             ret = -EFAULT;
0153             goto out_unlock;
0154         }
0155
0156         /*
0157          * Assumes write_raw can always accept GNSS_WRITE_BUF_SIZE
0158          * bytes.
0159          *
0160          * FIXME: revisit
0161          */
0162         down_read(&gdev->rwsem);
0163         if (!gdev->disconnected)
0164             ret = gdev->ops->write_raw(gdev, gdev->write_buf, n);
0165         else
0166             ret = -EIO;
0167         up_read(&gdev->rwsem);
0168
0169         if (ret < 0)
0170             break;
0171
0172         written += ret;
0173         buf += ret;
0174
0175         if (written == count)
0176             break;
0177     }
0178
0179     if (written)
0180         ret = written;
0181 out_unlock:
0182     mutex_unlock(&gdev->write_mutex);
0183
0184     return ret;
0185 }
0186
0187 static __poll_t gnss_poll(struct file *file, poll_table *wait)
0188 {
0189     struct gnss_device *gdev = file->private_data;
0190     __poll_t mask = 0;
0191
0192     poll_wait(file, &gdev->read_queue, wait);
0193
0194     if (!kfifo_is_empty(&gdev->read_fifo))
0195         mask |= EPOLLIN | EPOLLRDNORM;
0196     if (gdev->disconnected)
0197         mask |= EPOLLHUP;
0198
0199     return mask;
0200 }
0201
0202 static const struct file_operations gnss_fops = {
0203     .owner      = THIS_MODULE,
0204     .open       = gnss_open,
0205     .release    = gnss_release,
0206     .read       = gnss_read,
0207     .write      = gnss_write,
0208     .poll       = gnss_poll,
0209     .llseek     = no_llseek,
0210 };
0211
0212 static struct class *gnss_class;
0213
0214 static void gnss_device_release(struct device *dev)
0215 {
0216     struct gnss_device *gdev = to_gnss_device(dev);
0217
0218     kfree(gdev->write_buf);
0219     kfifo_free(&gdev->read_fifo);
0220     ida_simple_remove(&gnss_minors, gdev->id);
0221     kfree(gdev);
0222 }
0223
0224 struct gnss_device *gnss_allocate_device(struct device *parent)
0225 {
0226     struct gnss_device *gdev;
0227     struct device *dev;
0228     int id;
0229     int ret;
0230
0231     gdev = kzalloc(sizeof(*gdev), GFP_KERNEL);
0232     if (!gdev)
0233         return NULL;
0234
0235     id = ida_simple_get(&gnss_minors, 0, GNSS_MINORS, GFP_KERNEL);
0236     if (id < 0) {
0237         kfree(gdev);
0238         return NULL;
0239     }
0240
0241     gdev->id = id;
0242
0243     dev = &gdev->dev;
0244     device_initialize(dev);
0245     dev->devt = gnss_first + id;
0246     dev->class = gnss_class;
0247     dev->parent = parent;
0248     dev->release = gnss_device_release;
0249     dev_set_drvdata(dev, gdev);
0250     dev_set_name(dev, "gnss%d", id);
0251
0252     init_rwsem(&gdev->rwsem);
0253     mutex_init(&gdev->read_mutex);
0254     mutex_init(&gdev->write_mutex);
0255     init_waitqueue_head(&gdev->read_queue);
0256
0257     ret = kfifo_alloc(&gdev->read_fifo, GNSS_READ_FIFO_SIZE, GFP_KERNEL);
0258     if (ret)
0259         goto err_put_device;
0260
0261     gdev->write_buf = kzalloc(GNSS_WRITE_BUF_SIZE, GFP_KERNEL);
0262     if (!gdev->write_buf)
0263         goto err_put_device;
0264
0265     cdev_init(&gdev->cdev, &gnss_fops);
0266     gdev->cdev.owner = THIS_MODULE;
0267
0268     return gdev;
0269
0270 err_put_device:
0271     put_device(dev);
0272
0273     return NULL;
0274 }
0275 EXPORT_SYMBOL_GPL(gnss_allocate_device);
0276
0277 void gnss_put_device(struct gnss_device *gdev)
0278 {
0279     put_device(&gdev->dev);
0280 }
0281 EXPORT_SYMBOL_GPL(gnss_put_device);
0282
0283 int gnss_register_device(struct gnss_device *gdev)
0284 {
0285     int ret;
0286
0287     /* Set a flag which can be accessed without holding the rwsem. */
0288     if (gdev->ops->write_raw != NULL)
0289         gdev->flags |= GNSS_FLAG_HAS_WRITE_RAW;
0290
0291     ret = cdev_device_add(&gdev->cdev, &gdev->dev);
0292     if (ret) {
0293         dev_err(&gdev->dev, "failed to add device: %d\n", ret);
0294         return ret;
0295     }
0296
0297     return 0;
0298 }
0299 EXPORT_SYMBOL_GPL(gnss_register_device);
0300
0301 void gnss_deregister_device(struct gnss_device *gdev)
0302 {
0303     down_write(&gdev->rwsem);
0304     gdev->disconnected = true;
0305     if (gdev->count) {
0306         wake_up_interruptible(&gdev->read_queue);
0307         gdev->ops->close(gdev);
0308     }
0309     up_write(&gdev->rwsem);
0310
0311     cdev_device_del(&gdev->cdev, &gdev->dev);
0312 }
0313 EXPORT_SYMBOL_GPL(gnss_deregister_device);
0314
0315 /*
0316  * Caller guarantees serialisation.
0317  *
0318  * Must not be called for a closed device.
0319  */
0320 int gnss_insert_raw(struct gnss_device *gdev, const unsigned char *buf,
0321                 size_t count)
0322 {
0323     int ret;
0324
0325     ret = kfifo_in(&gdev->read_fifo, buf, count);
0326
0327     wake_up_interruptible(&gdev->read_queue);
0328
0329     return ret;
0330 }
0331 EXPORT_SYMBOL_GPL(gnss_insert_raw);
0332
0333 static const char * const gnss_type_names[GNSS_TYPE_COUNT] = {
0334     [GNSS_TYPE_NMEA]    = "NMEA",
0335     [GNSS_TYPE_SIRF]    = "SiRF",
0336     [GNSS_TYPE_UBX]     = "UBX",
0337     [GNSS_TYPE_MTK]     = "MTK",
0338 };
0339
0340 static const char *gnss_type_name(struct gnss_device *gdev)
0341 {
0342     const char *name = NULL;
0343
0344     if (gdev->type < GNSS_TYPE_COUNT)
0345         name = gnss_type_names[gdev->type];
0346
0347     if (!name)
0348         dev_WARN(&gdev->dev, "type name not defined\n");
0349
0350     return name;
0351 }
0352
0353 static ssize_t type_show(struct device *dev, struct device_attribute *attr,
0354                 char *buf)
0355 {
0356     struct gnss_device *gdev = to_gnss_device(dev);
0357
0358     return sprintf(buf, "%s\n", gnss_type_name(gdev));
0359 }
0360 static DEVICE_ATTR_RO(type);
0361
0362 static struct attribute *gnss_attrs[] = {
0363     &dev_attr_type.attr,
0364     NULL,
0365 };
0366 ATTRIBUTE_GROUPS(gnss);
0367
0368 static int gnss_uevent(struct device *dev, struct kobj_uevent_env *env)
0369 {
0370     struct gnss_device *gdev = to_gnss_device(dev);
0371     int ret;
0372
0373     ret = add_uevent_var(env, "GNSS_TYPE=%s", gnss_type_name(gdev));
0374     if (ret)
0375         return ret;
0376
0377     return 0;
0378 }
0379
0380 static int __init gnss_module_init(void)
0381 {
0382     int ret;
0383
0384     ret = alloc_chrdev_region(&gnss_first, 0, GNSS_MINORS, "gnss");
0385     if (ret < 0) {
0386         pr_err("failed to allocate device numbers: %d\n", ret);
0387         return ret;
0388     }
0389
0390     gnss_class = class_create(THIS_MODULE, "gnss");
0391     if (IS_ERR(gnss_class)) {
0392         ret = PTR_ERR(gnss_class);
0393         pr_err("failed to create class: %d\n", ret);
0394         goto err_unregister_chrdev;
0395     }
0396
0397     gnss_class->dev_groups = gnss_groups;
0398     gnss_class->dev_uevent = gnss_uevent;
0399
0400     pr_info("GNSS driver registered with major %d\n", MAJOR(gnss_first));
0401
0402     return 0;
0403
0404 err_unregister_chrdev:
0405     unregister_chrdev_region(gnss_first, GNSS_MINORS);
0406
0407     return ret;
0408 }
0409 module_init(gnss_module_init);
0410
0411 static void __exit gnss_module_exit(void)
0412 {
0413     class_destroy(gnss_class);
0414     unregister_chrdev_region(gnss_first, GNSS_MINORS);
0415     ida_destroy(&gnss_minors);
0416 }
0417 module_exit(gnss_module_exit);
0418
0419 MODULE_AUTHOR("Johan Hovold <johan@kernel.org>");
0420 MODULE_DESCRIPTION("GNSS receiver core");
0421 MODULE_LICENSE("GPL v2");