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(dev: &gdev->dev); |
44 | |
45 | stream_open(inode, filp: file); |
46 | file->private_data = gdev; |
47 | |
48 | down_write(sem: &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(sem: &gdev->rwsem); |
61 | |
62 | if (ret) |
63 | put_device(dev: &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(sem: &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(sem: &gdev->rwsem); |
82 | |
83 | put_device(dev: &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(lock: &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(lock: &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(to: gdev->write_buf, from: 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(sem: &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(sem: &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(lock: &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(filp: file, wait_address: &gdev->read_queue, p: 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(objp: gdev->write_buf); |
219 | kfifo_free(&gdev->read_fifo); |
220 | ida_free(&gnss_minors, id: gdev->id); |
221 | kfree(objp: 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(size: sizeof(*gdev), GFP_KERNEL); |
232 | if (!gdev) |
233 | return NULL; |
234 | |
235 | id = ida_alloc_max(ida: &gnss_minors, GNSS_MINORS - 1, GFP_KERNEL); |
236 | if (id < 0) { |
237 | kfree(objp: 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, data: gdev); |
250 | dev_set_name(dev, name: "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(dev: &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(cdev: &gdev->cdev, dev: &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(sem: &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(sem: &gdev->rwsem); |
310 | |
311 | cdev_device_del(cdev: &gdev->cdev, dev: &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, fmt: "%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, format: "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(name: "gnss" ); |
391 | if (IS_ERR(ptr: gnss_class)) { |
392 | ret = PTR_ERR(ptr: 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(cls: gnss_class); |
414 | unregister_chrdev_region(gnss_first, GNSS_MINORS); |
415 | ida_destroy(ida: &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 | |