]>
Commit | Line | Data |
---|---|---|
2b6a4403 JH |
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 | ||
c5bf68fe | 45 | stream_open(inode, file); |
2b6a4403 JH |
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); | |
d9995a0f | 238 | return NULL; |
2b6a4403 JH |
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 | ||
d9995a0f | 273 | return NULL; |
2b6a4403 JH |
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 | ||
10f14663 JH |
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", | |
625239d4 | 337 | [GNSS_TYPE_MTK] = "MTK", |
10f14663 JH |
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 | ||
2b6a4403 JH |
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 | ||
10f14663 JH |
397 | gnss_class->dev_groups = gnss_groups; |
398 | gnss_class->dev_uevent = gnss_uevent; | |
399 | ||
2b6a4403 JH |
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"); |