xref: /openbmc/linux/drivers/gnss/core.c (revision 1aaba11d)
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 
gnss_open(struct inode * inode,struct file * file)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 
gnss_release(struct inode * inode,struct file * file)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 
gnss_read(struct file * file,char __user * buf,size_t count,loff_t * pos)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 
gnss_write(struct file * file,const char __user * buf,size_t count,loff_t * pos)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 
gnss_poll(struct file * file,poll_table * wait)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 
gnss_device_release(struct device * dev)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 
gnss_allocate_device(struct device * parent)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 
gnss_put_device(struct gnss_device * gdev)277 void gnss_put_device(struct gnss_device *gdev)
278 {
279 	put_device(&gdev->dev);
280 }
281 EXPORT_SYMBOL_GPL(gnss_put_device);
282 
gnss_register_device(struct gnss_device * gdev)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 
gnss_deregister_device(struct gnss_device * gdev)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  */
gnss_insert_raw(struct gnss_device * gdev,const unsigned char * buf,size_t count)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 
gnss_type_name(const struct gnss_device * gdev)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 
type_show(struct device * dev,struct device_attribute * attr,char * buf)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 
gnss_uevent(const struct device * dev,struct kobj_uevent_env * env)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 
gnss_module_init(void)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 
gnss_module_exit(void)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