drivers/gnss/core.c

   1 // SPDX-License-Identifier: GPL-2.0
   2 /*
   3  * GNSS receiver core
   4  *
   5  * Copyright (C) 2018 Johan Hovold <johan@kernel.org>
   6  */
   7
   8 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
   9
  10 #include <linux/cdev.h>
  11 #include <linux/errno.h>
  12 #include <linux/fs.h>
  13 #include <linux/gnss.h>
  14 #include <linux/idr.h>
  15 #include <linux/init.h>
  16 #include <linux/kernel.h>
  17 #include <linux/module.h>
  18 #include <linux/poll.h>
  19 #include <linux/slab.h>
  20 #include <linux/uaccess.h>
  21 #include <linux/wait.h>
  22
  23 #define GNSS_FLAG_HAS_WRITE_RAW         BIT(0)
  24
  25 #define GNSS_MINORS     16
  26
  27 static DEFINE_IDA(gnss_minors);
  28 static dev_t gnss_first;
  29
  30 /* FIFO size must be a power of two */
  31 #define GNSS_READ_FIFO_SIZE     4096
  32 #define GNSS_WRITE_BUF_SIZE     1024
  33
  34 #define to_gnss_device(d) container_of((d), struct gnss_device, dev)
  35
  36 static int gnss_open(struct inode *inode, struct file *file)
  37 {
  38         struct gnss_device *gdev;
  39         int ret = 0;
  40
  41         gdev = container_of(inode->i_cdev, struct gnss_device, cdev);
  42
  43         get_device(&gdev->dev);
  44
  45         stream_open(inode, file);
  46         file->private_data = gdev;
  47
  48         down_write(&gdev->rwsem);
  49         if (gdev->disconnected) {
  50                 ret = -ENODEV;
  51                 goto unlock;
  52         }
  53
  54         if (gdev->count++ == 0) {
  55                 ret = gdev->ops->open(gdev);
  56                 if (ret)
  57                         gdev->count--;
  58         }
  59 unlock:
  60         up_write(&gdev->rwsem);
  61
  62         if (ret)
  63                 put_device(&gdev->dev);
  64
  65         return ret;
  66 }
  67
  68 static int gnss_release(struct inode *inode, struct file *file)
  69 {
  70         struct gnss_device *gdev = file->private_data;
  71
  72         down_write(&gdev->rwsem);
  73         if (gdev->disconnected)
  74                 goto unlock;
  75
  76         if (--gdev->count == 0) {
  77                 gdev->ops->close(gdev);
  78                 kfifo_reset(&gdev->read_fifo);
  79         }
  80 unlock:
  81         up_write(&gdev->rwsem);
  82
  83         put_device(&gdev->dev);
  84
  85         return 0;
  86 }
  87
  88 static ssize_t gnss_read(struct file *file, char __user *buf,
  89                                 size_t count, loff_t *pos)
  90 {
  91         struct gnss_device *gdev = file->private_data;
  92         unsigned int copied;
  93         int ret;
  94
  95         mutex_lock(&gdev->read_mutex);
  96         while (kfifo_is_empty(&gdev->read_fifo)) {
  97                 mutex_unlock(&gdev->read_mutex);
  98
  99                 if (gdev->disconnected)
 100                         return 0;
 101
 102                 if (file->f_flags & O_NONBLOCK)
 103                         return -EAGAIN;
 104
 105                 ret = wait_event_interruptible(gdev->read_queue,
 106                                 gdev->disconnected ||
 107                                 !kfifo_is_empty(&gdev->read_fifo));
 108                 if (ret)
 109                         return -ERESTARTSYS;
 110
 111                 mutex_lock(&gdev->read_mutex);
 112         }
 113
 114         ret = kfifo_to_user(&gdev->read_fifo, buf, count, &copied);
 115         if (ret == 0)
 116                 ret = copied;
 117
 118         mutex_unlock(&gdev->read_mutex);
 119
 120         return ret;
 121 }
 122
 123 static ssize_t gnss_write(struct file *file, const char __user *buf,
 124                                 size_t count, loff_t *pos)
 125 {
 126         struct gnss_device *gdev = file->private_data;
 127         size_t written = 0;
 128         int ret;
 129
 130         if (gdev->disconnected)
 131                 return -EIO;
 132
 133         if (!count)
 134                 return 0;
 135
 136         if (!(gdev->flags & GNSS_FLAG_HAS_WRITE_RAW))
 137                 return -EIO;
 138
 139         /* Ignoring O_NONBLOCK, write_raw() is synchronous. */
 140
 141         ret = mutex_lock_interruptible(&gdev->write_mutex);
 142         if (ret)
 143                 return -ERESTARTSYS;
 144
 145         for (;;) {
 146                 size_t n = count - written;
 147
 148                 if (n > GNSS_WRITE_BUF_SIZE)
 149                         n = GNSS_WRITE_BUF_SIZE;
 150
 151                 if (copy_from_user(gdev->write_buf, buf, n)) {
 152                         ret = -EFAULT;
 153                         goto out_unlock;
 154                 }
 155
 156                 /*
 157                  * Assumes write_raw can always accept GNSS_WRITE_BUF_SIZE
 158                  * bytes.
 159                  *
 160                  * FIXME: revisit
 161                  */
 162                 down_read(&gdev->rwsem);
 163                 if (!gdev->disconnected)
 164                         ret = gdev->ops->write_raw(gdev, gdev->write_buf, n);
 165                 else
 166                         ret = -EIO;
 167                 up_read(&gdev->rwsem);
 168
 169                 if (ret < 0)
 170                         break;
 171
 172                 written += ret;
 173                 buf += ret;
 174
 175                 if (written == count)
 176                         break;
 177         }
 178
 179         if (written)
 180                 ret = written;
 181 out_unlock:
 182         mutex_unlock(&gdev->write_mutex);
 183
 184         return ret;
 185 }
 186
 187 static __poll_t gnss_poll(struct file *file, poll_table *wait)
 188 {
 189         struct gnss_device *gdev = file->private_data;
 190         __poll_t mask = 0;
 191
 192         poll_wait(file, &gdev->read_queue, wait);
 193
 194         if (!kfifo_is_empty(&gdev->read_fifo))
 195                 mask |= EPOLLIN | EPOLLRDNORM;
 196         if (gdev->disconnected)
 197                 mask |= EPOLLHUP;
 198
 199         return mask;
 200 }
 201
 202 static const struct file_operations gnss_fops = {
 203         .owner          = THIS_MODULE,
 204         .open           = gnss_open,
 205         .release        = gnss_release,
 206         .read           = gnss_read,
 207         .write          = gnss_write,
 208         .poll           = gnss_poll,
 209         .llseek         = no_llseek,
 210 };
 211
 212 static struct class *gnss_class;
 213
 214 static void gnss_device_release(struct device *dev)
 215 {
 216         struct gnss_device *gdev = to_gnss_device(dev);
 217
 218         kfree(gdev->write_buf);
 219         kfifo_free(&gdev->read_fifo);
 220         ida_simple_remove(&gnss_minors, gdev->id);
 221         kfree(gdev);
 222 }
 223
 224 struct gnss_device *gnss_allocate_device(struct device *parent)
 225 {
 226         struct gnss_device *gdev;
 227         struct device *dev;
 228         int id;
 229         int ret;
 230
 231         gdev = kzalloc(sizeof(*gdev), GFP_KERNEL);
 232         if (!gdev)
 233                 return NULL;
 234
 235         id = ida_simple_get(&gnss_minors, 0, GNSS_MINORS, GFP_KERNEL);
 236         if (id < 0) {
 237                 kfree(gdev);
 238                 return NULL;
 239         }
 240
 241         gdev->id = id;
 242
 243         dev = &gdev->dev;
 244         device_initialize(dev);
 245         dev->devt = gnss_first + id;
 246         dev->class = gnss_class;
 247         dev->parent = parent;
 248         dev->release = gnss_device_release;
 249         dev_set_drvdata(dev, gdev);
 250         dev_set_name(dev, "gnss%d", id);
 251
 252         init_rwsem(&gdev->rwsem);
 253         mutex_init(&gdev->read_mutex);
 254         mutex_init(&gdev->write_mutex);
 255         init_waitqueue_head(&gdev->read_queue);
 256
 257         ret = kfifo_alloc(&gdev->read_fifo, GNSS_READ_FIFO_SIZE, GFP_KERNEL);
 258         if (ret)
 259                 goto err_put_device;
 260
 261         gdev->write_buf = kzalloc(GNSS_WRITE_BUF_SIZE, GFP_KERNEL);
 262         if (!gdev->write_buf)
 263                 goto err_put_device;
 264
 265         cdev_init(&gdev->cdev, &gnss_fops);
 266         gdev->cdev.owner = THIS_MODULE;
 267
 268         return gdev;
 269
 270 err_put_device:
 271         put_device(dev);
 272
 273         return NULL;
 274 }
 275 EXPORT_SYMBOL_GPL(gnss_allocate_device);
 276
 277 void gnss_put_device(struct gnss_device *gdev)
 278 {
 279         put_device(&gdev->dev);
 280 }
 281 EXPORT_SYMBOL_GPL(gnss_put_device);
 282
 283 int gnss_register_device(struct gnss_device *gdev)
 284 {
 285         int ret;
 286
 287         /* Set a flag which can be accessed without holding the rwsem. */
 288         if (gdev->ops->write_raw != NULL)
 289                 gdev->flags |= GNSS_FLAG_HAS_WRITE_RAW;
 290
 291         ret = cdev_device_add(&gdev->cdev, &gdev->dev);
 292         if (ret) {
 293                 dev_err(&gdev->dev, "failed to add device: %d\n", ret);
 294                 return ret;
 295         }
 296
 297         return 0;
 298 }
 299 EXPORT_SYMBOL_GPL(gnss_register_device);
 300
 301 void gnss_deregister_device(struct gnss_device *gdev)
 302 {
 303         down_write(&gdev->rwsem);
 304         gdev->disconnected = true;
 305         if (gdev->count) {
 306                 wake_up_interruptible(&gdev->read_queue);
 307                 gdev->ops->close(gdev);
 308         }
 309         up_write(&gdev->rwsem);
 310
 311         cdev_device_del(&gdev->cdev, &gdev->dev);
 312 }
 313 EXPORT_SYMBOL_GPL(gnss_deregister_device);
 314
 315 /*
 316  * Caller guarantees serialisation.
 317  *
 318  * Must not be called for a closed device.
 319  */
 320 int gnss_insert_raw(struct gnss_device *gdev, const unsigned char *buf,
 321                                 size_t count)
 322 {
 323         int ret;
 324
 325         ret = kfifo_in(&gdev->read_fifo, buf, count);
 326
 327         wake_up_interruptible(&gdev->read_queue);
 328
 329         return ret;
 330 }
 331 EXPORT_SYMBOL_GPL(gnss_insert_raw);
 332
 333 static const char * const gnss_type_names[GNSS_TYPE_COUNT] = {
 334         [GNSS_TYPE_NMEA]        = "NMEA",
 335         [GNSS_TYPE_SIRF]        = "SiRF",
 336         [GNSS_TYPE_UBX]         = "UBX",
 337         [GNSS_TYPE_MTK]         = "MTK",
 338 };
 339
 340 static const char *gnss_type_name(struct gnss_device *gdev)
 341 {
 342         const char *name = NULL;
 343
 344         if (gdev->type < GNSS_TYPE_COUNT)
 345                 name = gnss_type_names[gdev->type];
 346
 347         if (!name)
 348                 dev_WARN(&gdev->dev, "type name not defined\n");
 349
 350         return name;
 351 }
 352
 353 static ssize_t type_show(struct device *dev, struct device_attribute *attr,
 354                                 char *buf)
 355 {
 356         struct gnss_device *gdev = to_gnss_device(dev);
 357
 358         return sprintf(buf, "%s\n", gnss_type_name(gdev));
 359 }
 360 static DEVICE_ATTR_RO(type);
 361
 362 static struct attribute *gnss_attrs[] = {
 363         &dev_attr_type.attr,
 364         NULL,
 365 };
 366 ATTRIBUTE_GROUPS(gnss);
 367
 368 static int gnss_uevent(struct device *dev, struct kobj_uevent_env *env)
 369 {
 370         struct gnss_device *gdev = to_gnss_device(dev);
 371         int ret;
 372
 373         ret = add_uevent_var(env, "GNSS_TYPE=%s", gnss_type_name(gdev));
 374         if (ret)
 375                 return ret;
 376
 377         return 0;
 378 }
 379
 380 static int __init gnss_module_init(void)
 381 {
 382         int ret;
 383
 384         ret = alloc_chrdev_region(&gnss_first, 0, GNSS_MINORS, "gnss");
 385         if (ret < 0) {
 386                 pr_err("failed to allocate device numbers: %d\n", ret);
 387                 return ret;
 388         }
 389
 390         gnss_class = class_create(THIS_MODULE, "gnss");
 391         if (IS_ERR(gnss_class)) {
 392                 ret = PTR_ERR(gnss_class);
 393                 pr_err("failed to create class: %d\n", ret);
 394                 goto err_unregister_chrdev;
 395         }
 396
 397         gnss_class->dev_groups = gnss_groups;
 398         gnss_class->dev_uevent = gnss_uevent;
 399
 400         pr_info("GNSS driver registered with major %d\n", MAJOR(gnss_first));
 401
 402         return 0;
 403
 404 err_unregister_chrdev:
 405         unregister_chrdev_region(gnss_first, GNSS_MINORS);
 406
 407         return ret;
 408 }
 409 module_init(gnss_module_init);
 410
 411 static void __exit gnss_module_exit(void)
 412 {
 413         class_destroy(gnss_class);
 414         unregister_chrdev_region(gnss_first, GNSS_MINORS);
 415         ida_destroy(&gnss_minors);
 416 }
 417 module_exit(gnss_module_exit);
 418
 419 MODULE_AUTHOR("Johan Hovold <johan@kernel.org>");
 420 MODULE_DESCRIPTION("GNSS receiver core");
 421 MODULE_LICENSE("GPL v2");