1 /* -----------------------------------------------------------------------------
2 * Copyright (c) 2011 Ozmo Inc
3 * Released under the GNU General Public License Version 2 (GPLv2).
4 * -----------------------------------------------------------------------------
5 */
6 #include <linux/module.h>
7 #include <linux/fs.h>
8 #include <linux/cdev.h>
9 #include <linux/uaccess.h>
10 #include <linux/netdevice.h>
11 #include <linux/etherdevice.h>
12 #include <linux/poll.h>
13 #include <linux/sched.h>
14 #include "ozdbg.h"
15 #include "ozprotocol.h"
16 #include "ozappif.h"
17 #include "ozeltbuf.h"
18 #include "ozpd.h"
19 #include "ozproto.h"
20 #include "ozcdev.h"
21
22 #define OZ_RD_BUF_SZ 256
23 struct oz_cdev {
24 dev_t devnum;
25 struct cdev cdev;
26 wait_queue_head_t rdq;
27 spinlock_t lock;
28 u8 active_addr[ETH_ALEN];
29 struct oz_pd *active_pd;
30 };
31
32 /* Per PD context for the serial service stored in the PD. */
33 struct oz_serial_ctx {
34 atomic_t ref_count;
35 u8 tx_seq_num;
36 u8 rx_seq_num;
37 u8 rd_buf[OZ_RD_BUF_SZ];
38 int rd_in;
39 int rd_out;
40 };
41
42 static struct oz_cdev g_cdev;
43 static struct class *g_oz_class;
44
45 /*
46 * Context: process and softirq
47 */
oz_cdev_claim_ctx(struct oz_pd * pd)48 static struct oz_serial_ctx *oz_cdev_claim_ctx(struct oz_pd *pd)
49 {
50 struct oz_serial_ctx *ctx;
51
52 spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
53 ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL];
54 if (ctx)
55 atomic_inc(&ctx->ref_count);
56 spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
57 return ctx;
58 }
59
60 /*
61 * Context: softirq or process
62 */
oz_cdev_release_ctx(struct oz_serial_ctx * ctx)63 static void oz_cdev_release_ctx(struct oz_serial_ctx *ctx)
64 {
65 if (atomic_dec_and_test(&ctx->ref_count)) {
66 oz_dbg(ON, "Dealloc serial context\n");
67 kfree(ctx);
68 }
69 }
70
71 /*
72 * Context: process
73 */
oz_cdev_open(struct inode * inode,struct file * filp)74 static int oz_cdev_open(struct inode *inode, struct file *filp)
75 {
76 struct oz_cdev *dev = container_of(inode->i_cdev, struct oz_cdev, cdev);
77
78 oz_dbg(ON, "major = %d minor = %d\n", imajor(inode), iminor(inode));
79
80 filp->private_data = dev;
81 return 0;
82 }
83
84 /*
85 * Context: process
86 */
oz_cdev_release(struct inode * inode,struct file * filp)87 static int oz_cdev_release(struct inode *inode, struct file *filp)
88 {
89 return 0;
90 }
91
92 /*
93 * Context: process
94 */
oz_cdev_read(struct file * filp,char __user * buf,size_t count,loff_t * fpos)95 static ssize_t oz_cdev_read(struct file *filp, char __user *buf, size_t count,
96 loff_t *fpos)
97 {
98 int n;
99 int ix;
100
101 struct oz_pd *pd;
102 struct oz_serial_ctx *ctx;
103
104 spin_lock_bh(&g_cdev.lock);
105 pd = g_cdev.active_pd;
106 if (pd)
107 oz_pd_get(pd);
108 spin_unlock_bh(&g_cdev.lock);
109 if (pd == NULL)
110 return -1;
111 ctx = oz_cdev_claim_ctx(pd);
112 if (ctx == NULL)
113 goto out2;
114 n = ctx->rd_in - ctx->rd_out;
115 if (n < 0)
116 n += OZ_RD_BUF_SZ;
117 if (count > n)
118 count = n;
119 ix = ctx->rd_out;
120 n = OZ_RD_BUF_SZ - ix;
121 if (n > count)
122 n = count;
123 if (copy_to_user(buf, &ctx->rd_buf[ix], n)) {
124 count = 0;
125 goto out1;
126 }
127 ix += n;
128 if (ix == OZ_RD_BUF_SZ)
129 ix = 0;
130 if (n < count) {
131 if (copy_to_user(&buf[n], ctx->rd_buf, count-n)) {
132 count = 0;
133 goto out1;
134 }
135 ix = count-n;
136 }
137 ctx->rd_out = ix;
138 out1:
139 oz_cdev_release_ctx(ctx);
140 out2:
141 oz_pd_put(pd);
142 return count;
143 }
144
145 /*
146 * Context: process
147 */
oz_cdev_write(struct file * filp,const char __user * buf,size_t count,loff_t * fpos)148 static ssize_t oz_cdev_write(struct file *filp, const char __user *buf,
149 size_t count, loff_t *fpos)
150 {
151 struct oz_pd *pd;
152 struct oz_elt_buf *eb;
153 struct oz_elt_info *ei;
154 struct oz_elt *elt;
155 struct oz_app_hdr *app_hdr;
156 struct oz_serial_ctx *ctx;
157
158 if (count > sizeof(ei->data) - sizeof(*elt) - sizeof(*app_hdr))
159 return -EINVAL;
160
161 spin_lock_bh(&g_cdev.lock);
162 pd = g_cdev.active_pd;
163 if (pd)
164 oz_pd_get(pd);
165 spin_unlock_bh(&g_cdev.lock);
166 if (pd == NULL)
167 return -ENXIO;
168 if (!(pd->state & OZ_PD_S_CONNECTED))
169 return -EAGAIN;
170 eb = &pd->elt_buff;
171 ei = oz_elt_info_alloc(eb);
172 if (ei == NULL) {
173 count = 0;
174 goto out;
175 }
176 elt = (struct oz_elt *)ei->data;
177 app_hdr = (struct oz_app_hdr *)(elt+1);
178 elt->length = sizeof(struct oz_app_hdr) + count;
179 elt->type = OZ_ELT_APP_DATA;
180 ei->app_id = OZ_APPID_SERIAL;
181 ei->length = elt->length + sizeof(struct oz_elt);
182 app_hdr->app_id = OZ_APPID_SERIAL;
183 if (copy_from_user(app_hdr+1, buf, count))
184 goto out;
185 spin_lock_bh(&pd->app_lock[OZ_APPID_USB]);
186 ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL];
187 if (ctx) {
188 app_hdr->elt_seq_num = ctx->tx_seq_num++;
189 if (ctx->tx_seq_num == 0)
190 ctx->tx_seq_num = 1;
191 spin_lock(&eb->lock);
192 if (oz_queue_elt_info(eb, 0, 0, ei) == 0)
193 ei = NULL;
194 spin_unlock(&eb->lock);
195 }
196 spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]);
197 out:
198 if (ei) {
199 count = 0;
200 spin_lock_bh(&eb->lock);
201 oz_elt_info_free(eb, ei);
202 spin_unlock_bh(&eb->lock);
203 }
204 oz_pd_put(pd);
205 return count;
206 }
207
208 /*
209 * Context: process
210 */
oz_set_active_pd(const u8 * addr)211 static int oz_set_active_pd(const u8 *addr)
212 {
213 int rc = 0;
214 struct oz_pd *pd;
215 struct oz_pd *old_pd;
216
217 pd = oz_pd_find(addr);
218 if (pd) {
219 spin_lock_bh(&g_cdev.lock);
220 ether_addr_copy(g_cdev.active_addr, addr);
221 old_pd = g_cdev.active_pd;
222 g_cdev.active_pd = pd;
223 spin_unlock_bh(&g_cdev.lock);
224 if (old_pd)
225 oz_pd_put(old_pd);
226 } else {
227 if (is_zero_ether_addr(addr)) {
228 spin_lock_bh(&g_cdev.lock);
229 pd = g_cdev.active_pd;
230 g_cdev.active_pd = NULL;
231 memset(g_cdev.active_addr, 0,
232 sizeof(g_cdev.active_addr));
233 spin_unlock_bh(&g_cdev.lock);
234 if (pd)
235 oz_pd_put(pd);
236 } else {
237 rc = -1;
238 }
239 }
240 return rc;
241 }
242
243 /*
244 * Context: process
245 */
oz_cdev_ioctl(struct file * filp,unsigned int cmd,unsigned long arg)246 static long oz_cdev_ioctl(struct file *filp, unsigned int cmd,
247 unsigned long arg)
248 {
249 int rc = 0;
250
251 if (_IOC_TYPE(cmd) != OZ_IOCTL_MAGIC)
252 return -ENOTTY;
253 if (_IOC_NR(cmd) > OZ_IOCTL_MAX)
254 return -ENOTTY;
255 if (_IOC_DIR(cmd) & _IOC_READ)
256 rc = !access_ok(VERIFY_WRITE, (void __user *)arg,
257 _IOC_SIZE(cmd));
258 else if (_IOC_DIR(cmd) & _IOC_WRITE)
259 rc = !access_ok(VERIFY_READ, (void __user *)arg,
260 _IOC_SIZE(cmd));
261 if (rc)
262 return -EFAULT;
263 switch (cmd) {
264 case OZ_IOCTL_GET_PD_LIST: {
265 struct oz_pd_list list;
266
267 oz_dbg(ON, "OZ_IOCTL_GET_PD_LIST\n");
268 memset(&list, 0, sizeof(list));
269 list.count = oz_get_pd_list(list.addr, OZ_MAX_PDS);
270 if (copy_to_user((void __user *)arg, &list,
271 sizeof(list)))
272 return -EFAULT;
273 }
274 break;
275 case OZ_IOCTL_SET_ACTIVE_PD: {
276 u8 addr[ETH_ALEN];
277
278 oz_dbg(ON, "OZ_IOCTL_SET_ACTIVE_PD\n");
279 if (copy_from_user(addr, (void __user *)arg, ETH_ALEN))
280 return -EFAULT;
281 rc = oz_set_active_pd(addr);
282 }
283 break;
284 case OZ_IOCTL_GET_ACTIVE_PD: {
285 u8 addr[ETH_ALEN];
286
287 oz_dbg(ON, "OZ_IOCTL_GET_ACTIVE_PD\n");
288 spin_lock_bh(&g_cdev.lock);
289 ether_addr_copy(addr, g_cdev.active_addr);
290 spin_unlock_bh(&g_cdev.lock);
291 if (copy_to_user((void __user *)arg, addr, ETH_ALEN))
292 return -EFAULT;
293 }
294 break;
295 case OZ_IOCTL_ADD_BINDING:
296 case OZ_IOCTL_REMOVE_BINDING: {
297 struct oz_binding_info b;
298
299 if (copy_from_user(&b, (void __user *)arg,
300 sizeof(struct oz_binding_info))) {
301 return -EFAULT;
302 }
303 /* Make sure name is null terminated. */
304 b.name[OZ_MAX_BINDING_LEN-1] = 0;
305 if (cmd == OZ_IOCTL_ADD_BINDING)
306 oz_binding_add(b.name);
307 else
308 oz_binding_remove(b.name);
309 }
310 break;
311 }
312 return rc;
313 }
314
315 /*
316 * Context: process
317 */
oz_cdev_poll(struct file * filp,poll_table * wait)318 static unsigned int oz_cdev_poll(struct file *filp, poll_table *wait)
319 {
320 unsigned int ret = 0;
321 struct oz_cdev *dev = filp->private_data;
322
323 oz_dbg(ON, "Poll called wait = %p\n", wait);
324 spin_lock_bh(&dev->lock);
325 if (dev->active_pd) {
326 struct oz_serial_ctx *ctx = oz_cdev_claim_ctx(dev->active_pd);
327
328 if (ctx) {
329 if (ctx->rd_in != ctx->rd_out)
330 ret |= POLLIN | POLLRDNORM;
331 oz_cdev_release_ctx(ctx);
332 }
333 }
334 spin_unlock_bh(&dev->lock);
335 if (wait)
336 poll_wait(filp, &dev->rdq, wait);
337 return ret;
338 }
339
340 /*
341 */
342 static const struct file_operations oz_fops = {
343 .owner = THIS_MODULE,
344 .open = oz_cdev_open,
345 .release = oz_cdev_release,
346 .read = oz_cdev_read,
347 .write = oz_cdev_write,
348 .unlocked_ioctl = oz_cdev_ioctl,
349 .poll = oz_cdev_poll
350 };
351
352 /*
353 * Context: process
354 */
oz_cdev_register(void)355 int oz_cdev_register(void)
356 {
357 int err;
358 struct device *dev;
359
360 memset(&g_cdev, 0, sizeof(g_cdev));
361 err = alloc_chrdev_region(&g_cdev.devnum, 0, 1, "ozwpan");
362 if (err < 0)
363 return err;
364 oz_dbg(ON, "Alloc dev number %d:%d\n",
365 MAJOR(g_cdev.devnum), MINOR(g_cdev.devnum));
366 cdev_init(&g_cdev.cdev, &oz_fops);
367 g_cdev.cdev.owner = THIS_MODULE;
368 spin_lock_init(&g_cdev.lock);
369 init_waitqueue_head(&g_cdev.rdq);
370 err = cdev_add(&g_cdev.cdev, g_cdev.devnum, 1);
371 if (err < 0) {
372 oz_dbg(ON, "Failed to add cdev\n");
373 goto unregister;
374 }
375 g_oz_class = class_create(THIS_MODULE, "ozmo_wpan");
376 if (IS_ERR(g_oz_class)) {
377 oz_dbg(ON, "Failed to register ozmo_wpan class\n");
378 err = PTR_ERR(g_oz_class);
379 goto delete;
380 }
381 dev = device_create(g_oz_class, NULL, g_cdev.devnum, NULL, "ozwpan");
382 if (IS_ERR(dev)) {
383 oz_dbg(ON, "Failed to create sysfs entry for cdev\n");
384 err = PTR_ERR(dev);
385 goto delete;
386 }
387 return 0;
388
389 delete:
390 cdev_del(&g_cdev.cdev);
391 unregister:
392 unregister_chrdev_region(g_cdev.devnum, 1);
393 return err;
394 }
395
396 /*
397 * Context: process
398 */
oz_cdev_deregister(void)399 int oz_cdev_deregister(void)
400 {
401 cdev_del(&g_cdev.cdev);
402 unregister_chrdev_region(g_cdev.devnum, 1);
403 if (g_oz_class) {
404 device_destroy(g_oz_class, g_cdev.devnum);
405 class_destroy(g_oz_class);
406 }
407 return 0;
408 }
409
410 /*
411 * Context: process
412 */
oz_cdev_init(void)413 int oz_cdev_init(void)
414 {
415 oz_app_enable(OZ_APPID_SERIAL, 1);
416 return 0;
417 }
418
419 /*
420 * Context: process
421 */
oz_cdev_term(void)422 void oz_cdev_term(void)
423 {
424 oz_app_enable(OZ_APPID_SERIAL, 0);
425 }
426
427 /*
428 * Context: softirq-serialized
429 */
oz_cdev_start(struct oz_pd * pd,int resume)430 int oz_cdev_start(struct oz_pd *pd, int resume)
431 {
432 struct oz_serial_ctx *ctx;
433 struct oz_serial_ctx *old_ctx;
434
435 if (resume) {
436 oz_dbg(ON, "Serial service resumed\n");
437 return 0;
438 }
439 ctx = kzalloc(sizeof(struct oz_serial_ctx), GFP_ATOMIC);
440 if (ctx == NULL)
441 return -ENOMEM;
442 atomic_set(&ctx->ref_count, 1);
443 ctx->tx_seq_num = 1;
444 spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
445 old_ctx = pd->app_ctx[OZ_APPID_SERIAL];
446 if (old_ctx) {
447 spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
448 kfree(ctx);
449 } else {
450 pd->app_ctx[OZ_APPID_SERIAL] = ctx;
451 spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
452 }
453 spin_lock(&g_cdev.lock);
454 if ((g_cdev.active_pd == NULL) &&
455 ether_addr_equal(pd->mac_addr, g_cdev.active_addr)) {
456 oz_pd_get(pd);
457 g_cdev.active_pd = pd;
458 oz_dbg(ON, "Active PD arrived\n");
459 }
460 spin_unlock(&g_cdev.lock);
461 oz_dbg(ON, "Serial service started\n");
462 return 0;
463 }
464
465 /*
466 * Context: softirq or process
467 */
oz_cdev_stop(struct oz_pd * pd,int pause)468 void oz_cdev_stop(struct oz_pd *pd, int pause)
469 {
470 struct oz_serial_ctx *ctx;
471
472 if (pause) {
473 oz_dbg(ON, "Serial service paused\n");
474 return;
475 }
476 spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
477 ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL];
478 pd->app_ctx[OZ_APPID_SERIAL] = NULL;
479 spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]);
480 if (ctx)
481 oz_cdev_release_ctx(ctx);
482 spin_lock(&g_cdev.lock);
483 if (pd == g_cdev.active_pd)
484 g_cdev.active_pd = NULL;
485 else
486 pd = NULL;
487 spin_unlock(&g_cdev.lock);
488 if (pd) {
489 oz_pd_put(pd);
490 oz_dbg(ON, "Active PD departed\n");
491 }
492 oz_dbg(ON, "Serial service stopped\n");
493 }
494
495 /*
496 * Context: softirq-serialized
497 */
oz_cdev_rx(struct oz_pd * pd,struct oz_elt * elt)498 void oz_cdev_rx(struct oz_pd *pd, struct oz_elt *elt)
499 {
500 struct oz_serial_ctx *ctx;
501 struct oz_app_hdr *app_hdr;
502 u8 *data;
503 int len;
504 int space;
505 int copy_sz;
506 int ix;
507
508 ctx = oz_cdev_claim_ctx(pd);
509 if (ctx == NULL) {
510 oz_dbg(ON, "Cannot claim serial context\n");
511 return;
512 }
513
514 app_hdr = (struct oz_app_hdr *)(elt+1);
515 /* If sequence number is non-zero then check it is not a duplicate.
516 */
517 if (app_hdr->elt_seq_num != 0) {
518 if (((ctx->rx_seq_num - app_hdr->elt_seq_num) & 0x80) == 0) {
519 /* Reject duplicate element. */
520 oz_dbg(ON, "Duplicate element:%02x %02x\n",
521 app_hdr->elt_seq_num, ctx->rx_seq_num);
522 goto out;
523 }
524 }
525 ctx->rx_seq_num = app_hdr->elt_seq_num;
526 len = elt->length - sizeof(struct oz_app_hdr);
527 data = ((u8 *)(elt+1)) + sizeof(struct oz_app_hdr);
528 if (len <= 0)
529 goto out;
530 space = ctx->rd_out - ctx->rd_in - 1;
531 if (space < 0)
532 space += OZ_RD_BUF_SZ;
533 if (len > space) {
534 oz_dbg(ON, "Not enough space:%d %d\n", len, space);
535 len = space;
536 }
537 ix = ctx->rd_in;
538 copy_sz = OZ_RD_BUF_SZ - ix;
539 if (copy_sz > len)
540 copy_sz = len;
541 memcpy(&ctx->rd_buf[ix], data, copy_sz);
542 len -= copy_sz;
543 ix += copy_sz;
544 if (ix == OZ_RD_BUF_SZ)
545 ix = 0;
546 if (len) {
547 memcpy(ctx->rd_buf, data+copy_sz, len);
548 ix = len;
549 }
550 ctx->rd_in = ix;
551 wake_up(&g_cdev.rdq);
552 out:
553 oz_cdev_release_ctx(ctx);
554 }
555