421 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * GNSS receiver core4 *5 * Copyright (C) 2018 Johan Hovold <johan@kernel.org>6 */7 8#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt9 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 1626 27static DEFINE_IDA(gnss_minors);28static dev_t gnss_first;29 30/* FIFO size must be a power of two */31#define GNSS_READ_FIFO_SIZE 409632#define GNSS_WRITE_BUF_SIZE 102433 34#define to_gnss_device(d) container_of((d), struct gnss_device, dev)35 36static 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 }59unlock:60 up_write(&gdev->rwsem);61 62 if (ret)63 put_device(&gdev->dev);64 65 return ret;66}67 68static 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 }80unlock:81 up_write(&gdev->rwsem);82 83 put_device(&gdev->dev);84 85 return 0;86}87 88static 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 123static 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_SIZE158 * bytes.159 *160 * FIXME: revisit161 */162 down_read(&gdev->rwsem);163 if (!gdev->disconnected)164 ret = gdev->ops->write_raw(gdev, gdev->write_buf, n);165 else166 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;181out_unlock:182 mutex_unlock(&gdev->write_mutex);183 184 return ret;185}186 187static __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 202static 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};210 211static struct class *gnss_class;212 213static void gnss_device_release(struct device *dev)214{215 struct gnss_device *gdev = to_gnss_device(dev);216 217 kfree(gdev->write_buf);218 kfifo_free(&gdev->read_fifo);219 ida_free(&gnss_minors, gdev->id);220 kfree(gdev);221}222 223struct gnss_device *gnss_allocate_device(struct device *parent)224{225 struct gnss_device *gdev;226 struct device *dev;227 int id;228 int ret;229 230 gdev = kzalloc(sizeof(*gdev), GFP_KERNEL);231 if (!gdev)232 return NULL;233 234 id = ida_alloc_max(&gnss_minors, GNSS_MINORS - 1, GFP_KERNEL);235 if (id < 0) {236 kfree(gdev);237 return NULL;238 }239 240 gdev->id = id;241 242 dev = &gdev->dev;243 device_initialize(dev);244 dev->devt = gnss_first + id;245 dev->class = gnss_class;246 dev->parent = parent;247 dev->release = gnss_device_release;248 dev_set_drvdata(dev, gdev);249 dev_set_name(dev, "gnss%d", id);250 251 init_rwsem(&gdev->rwsem);252 mutex_init(&gdev->read_mutex);253 mutex_init(&gdev->write_mutex);254 init_waitqueue_head(&gdev->read_queue);255 256 ret = kfifo_alloc(&gdev->read_fifo, GNSS_READ_FIFO_SIZE, GFP_KERNEL);257 if (ret)258 goto err_put_device;259 260 gdev->write_buf = kzalloc(GNSS_WRITE_BUF_SIZE, GFP_KERNEL);261 if (!gdev->write_buf)262 goto err_put_device;263 264 cdev_init(&gdev->cdev, &gnss_fops);265 gdev->cdev.owner = THIS_MODULE;266 267 return gdev;268 269err_put_device:270 put_device(dev);271 272 return NULL;273}274EXPORT_SYMBOL_GPL(gnss_allocate_device);275 276void gnss_put_device(struct gnss_device *gdev)277{278 put_device(&gdev->dev);279}280EXPORT_SYMBOL_GPL(gnss_put_device);281 282int gnss_register_device(struct gnss_device *gdev)283{284 int ret;285 286 /* Set a flag which can be accessed without holding the rwsem. */287 if (gdev->ops->write_raw != NULL)288 gdev->flags |= GNSS_FLAG_HAS_WRITE_RAW;289 290 ret = cdev_device_add(&gdev->cdev, &gdev->dev);291 if (ret) {292 dev_err(&gdev->dev, "failed to add device: %d\n", ret);293 return ret;294 }295 296 return 0;297}298EXPORT_SYMBOL_GPL(gnss_register_device);299 300void gnss_deregister_device(struct gnss_device *gdev)301{302 down_write(&gdev->rwsem);303 gdev->disconnected = true;304 if (gdev->count) {305 wake_up_interruptible(&gdev->read_queue);306 gdev->ops->close(gdev);307 }308 up_write(&gdev->rwsem);309 310 cdev_device_del(&gdev->cdev, &gdev->dev);311}312EXPORT_SYMBOL_GPL(gnss_deregister_device);313 314/*315 * Caller guarantees serialisation.316 *317 * Must not be called for a closed device.318 */319int gnss_insert_raw(struct gnss_device *gdev, const unsigned char *buf,320 size_t count)321{322 int ret;323 324 ret = kfifo_in(&gdev->read_fifo, buf, count);325 326 wake_up_interruptible(&gdev->read_queue);327 328 return ret;329}330EXPORT_SYMBOL_GPL(gnss_insert_raw);331 332static const char * const gnss_type_names[GNSS_TYPE_COUNT] = {333 [GNSS_TYPE_NMEA] = "NMEA",334 [GNSS_TYPE_SIRF] = "SiRF",335 [GNSS_TYPE_UBX] = "UBX",336 [GNSS_TYPE_MTK] = "MTK",337};338 339static const char *gnss_type_name(const struct gnss_device *gdev)340{341 const char *name = NULL;342 343 if (gdev->type < GNSS_TYPE_COUNT)344 name = gnss_type_names[gdev->type];345 346 if (!name)347 dev_WARN(&gdev->dev, "type name not defined\n");348 349 return name;350}351 352static ssize_t type_show(struct device *dev, struct device_attribute *attr,353 char *buf)354{355 struct gnss_device *gdev = to_gnss_device(dev);356 357 return sprintf(buf, "%s\n", gnss_type_name(gdev));358}359static DEVICE_ATTR_RO(type);360 361static struct attribute *gnss_attrs[] = {362 &dev_attr_type.attr,363 NULL,364};365ATTRIBUTE_GROUPS(gnss);366 367static int gnss_uevent(const struct device *dev, struct kobj_uevent_env *env)368{369 const struct gnss_device *gdev = to_gnss_device(dev);370 int ret;371 372 ret = add_uevent_var(env, "GNSS_TYPE=%s", gnss_type_name(gdev));373 if (ret)374 return ret;375 376 return 0;377}378 379static int __init gnss_module_init(void)380{381 int ret;382 383 ret = alloc_chrdev_region(&gnss_first, 0, GNSS_MINORS, "gnss");384 if (ret < 0) {385 pr_err("failed to allocate device numbers: %d\n", ret);386 return ret;387 }388 389 gnss_class = class_create("gnss");390 if (IS_ERR(gnss_class)) {391 ret = PTR_ERR(gnss_class);392 pr_err("failed to create class: %d\n", ret);393 goto err_unregister_chrdev;394 }395 396 gnss_class->dev_groups = gnss_groups;397 gnss_class->dev_uevent = gnss_uevent;398 399 pr_info("GNSS driver registered with major %d\n", MAJOR(gnss_first));400 401 return 0;402 403err_unregister_chrdev:404 unregister_chrdev_region(gnss_first, GNSS_MINORS);405 406 return ret;407}408module_init(gnss_module_init);409 410static void __exit gnss_module_exit(void)411{412 class_destroy(gnss_class);413 unregister_chrdev_region(gnss_first, GNSS_MINORS);414 ida_destroy(&gnss_minors);415}416module_exit(gnss_module_exit);417 418MODULE_AUTHOR("Johan Hovold <johan@kernel.org>");419MODULE_DESCRIPTION("GNSS receiver core");420MODULE_LICENSE("GPL v2");421