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_free(&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_alloc_max(&gnss_minors, GNSS_MINORS - 1, 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(const 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(const struct device *dev, struct kobj_uevent_env *env) 369 { 370 const 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("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"); 422