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 }; 210 211 static struct class *gnss_class; 212 213 static 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 223 struct 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 269 err_put_device: 270 put_device(dev); 271 272 return NULL; 273 } 274 EXPORT_SYMBOL_GPL(gnss_allocate_device); 275 276 void gnss_put_device(struct gnss_device *gdev) 277 { 278 put_device(&gdev->dev); 279 } 280 EXPORT_SYMBOL_GPL(gnss_put_device); 281 282 int 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 } 298 EXPORT_SYMBOL_GPL(gnss_register_device); 299 300 void 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 } 312 EXPORT_SYMBOL_GPL(gnss_deregister_device); 313 314 /* 315 * Caller guarantees serialisation. 316 * 317 * Must not be called for a closed device. 318 */ 319 int 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 } 330 EXPORT_SYMBOL_GPL(gnss_insert_raw); 331 332 static 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 339 static 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 352 static 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 } 359 static DEVICE_ATTR_RO(type); 360 361 static struct attribute *gnss_attrs[] = { 362 &dev_attr_type.attr, 363 NULL, 364 }; 365 ATTRIBUTE_GROUPS(gnss); 366 367 static 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 379 static 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 403 err_unregister_chrdev: 404 unregister_chrdev_region(gnss_first, GNSS_MINORS); 405 406 return ret; 407 } 408 module_init(gnss_module_init); 409 410 static 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 } 416 module_exit(gnss_module_exit); 417 418 MODULE_AUTHOR("Johan Hovold <johan@kernel.org>"); 419 MODULE_DESCRIPTION("GNSS receiver core"); 420 MODULE_LICENSE("GPL v2"); 421