projects/linux·blob master · refs

420 lines · 8.2 KiB

1// SPDX-License-Identifier: GPL-2.0
2/*
3 * GNSS receiver core
4 *
5 * Copyright (C) 2018 Johan Hovold <[email protected]>
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
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 4096
32#define GNSS_WRITE_BUF_SIZE 1024
33
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_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;
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_obj(*gdev);
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 <[email protected]>");
419MODULE_DESCRIPTION("GNSS receiver core");
420MODULE_LICENSE("GPL v2");