Commit | Line | Data |
---|---|---|
23af8c2a CK |
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> | |
07732be2 | 11 | #include <linux/etherdevice.h> |
23af8c2a CK |
12 | #include <linux/poll.h> |
13 | #include <linux/sched.h> | |
f724b584 | 14 | #include "ozdbg.h" |
23af8c2a | 15 | #include "ozprotocol.h" |
23af8c2a CK |
16 | #include "ozappif.h" |
17 | #include "ozeltbuf.h" | |
18 | #include "ozpd.h" | |
19 | #include "ozproto.h" | |
4f9d5b2f | 20 | #include "ozcdev.h" |
4e7fb829 | 21 | |
23af8c2a CK |
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 | }; | |
4e7fb829 | 41 | |
23af8c2a | 42 | static struct oz_cdev g_cdev; |
a7f74c30 | 43 | static struct class *g_oz_class; |
6e244a83 | 44 | |
4e7fb829 | 45 | /* |
23af8c2a CK |
46 | * Context: process and softirq |
47 | */ | |
48 | static struct oz_serial_ctx *oz_cdev_claim_ctx(struct oz_pd *pd) | |
49 | { | |
50 | struct oz_serial_ctx *ctx; | |
18f8191e | 51 | |
a9686e78 CJ |
52 | spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]); |
53 | ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL]; | |
23af8c2a CK |
54 | if (ctx) |
55 | atomic_inc(&ctx->ref_count); | |
a9686e78 | 56 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]); |
23af8c2a CK |
57 | return ctx; |
58 | } | |
6e244a83 | 59 | |
4e7fb829 | 60 | /* |
23af8c2a CK |
61 | * Context: softirq or process |
62 | */ | |
63 | static void oz_cdev_release_ctx(struct oz_serial_ctx *ctx) | |
64 | { | |
65 | if (atomic_dec_and_test(&ctx->ref_count)) { | |
f724b584 | 66 | oz_dbg(ON, "Dealloc serial context\n"); |
1ec41a31 | 67 | kfree(ctx); |
23af8c2a CK |
68 | } |
69 | } | |
6e244a83 | 70 | |
4e7fb829 | 71 | /* |
23af8c2a CK |
72 | * Context: process |
73 | */ | |
a7f74c30 | 74 | static int oz_cdev_open(struct inode *inode, struct file *filp) |
23af8c2a | 75 | { |
f724b584 JP |
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 | ||
23af8c2a CK |
80 | filp->private_data = dev; |
81 | return 0; | |
82 | } | |
6e244a83 | 83 | |
4e7fb829 | 84 | /* |
23af8c2a CK |
85 | * Context: process |
86 | */ | |
a7f74c30 | 87 | static int oz_cdev_release(struct inode *inode, struct file *filp) |
23af8c2a | 88 | { |
23af8c2a CK |
89 | return 0; |
90 | } | |
6e244a83 | 91 | |
4e7fb829 | 92 | /* |
23af8c2a CK |
93 | * Context: process |
94 | */ | |
a7f74c30 | 95 | static ssize_t oz_cdev_read(struct file *filp, char __user *buf, size_t count, |
23af8c2a CK |
96 | loff_t *fpos) |
97 | { | |
98 | int n; | |
99 | int ix; | |
100 | ||
101 | struct oz_pd *pd; | |
bc501c55 | 102 | struct oz_serial_ctx *ctx; |
23af8c2a CK |
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); | |
3d9562a6 | 109 | if (pd == NULL) |
23af8c2a CK |
110 | return -1; |
111 | ctx = oz_cdev_claim_ctx(pd); | |
3d9562a6 | 112 | if (ctx == NULL) |
23af8c2a CK |
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 | } | |
6e244a83 | 144 | |
4e7fb829 | 145 | /* |
23af8c2a CK |
146 | * Context: process |
147 | */ | |
a7f74c30 PH |
148 | static ssize_t oz_cdev_write(struct file *filp, const char __user *buf, |
149 | size_t count, loff_t *fpos) | |
23af8c2a CK |
150 | { |
151 | struct oz_pd *pd; | |
152 | struct oz_elt_buf *eb; | |
bc501c55 | 153 | struct oz_elt_info *ei; |
23af8c2a CK |
154 | struct oz_elt *elt; |
155 | struct oz_app_hdr *app_hdr; | |
156 | struct oz_serial_ctx *ctx; | |
157 | ||
c2c65cd2 DC |
158 | if (count > sizeof(ei->data) - sizeof(*elt) - sizeof(*app_hdr)) |
159 | return -EINVAL; | |
160 | ||
23af8c2a CK |
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); | |
3d9562a6 | 166 | if (pd == NULL) |
b93d85f0 | 167 | return -ENXIO; |
8503f901 RG |
168 | if (!(pd->state & OZ_PD_S_CONNECTED)) |
169 | return -EAGAIN; | |
23af8c2a CK |
170 | eb = &pd->elt_buff; |
171 | ei = oz_elt_info_alloc(eb); | |
3d9562a6 | 172 | if (ei == NULL) { |
23af8c2a CK |
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; | |
a9686e78 CJ |
185 | spin_lock_bh(&pd->app_lock[OZ_APPID_USB]); |
186 | ctx = (struct oz_serial_ctx *) pd->app_ctx[OZ_APPID_SERIAL]; | |
23af8c2a CK |
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) | |
bc501c55 | 193 | ei = NULL; |
23af8c2a CK |
194 | spin_unlock(&eb->lock); |
195 | } | |
a9686e78 | 196 | spin_unlock_bh(&pd->app_lock[OZ_APPID_USB]); |
23af8c2a CK |
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 | } | |
6e244a83 | 207 | |
4e7fb829 | 208 | /* |
23af8c2a CK |
209 | * Context: process |
210 | */ | |
dc7f5b35 | 211 | static int oz_set_active_pd(const u8 *addr) |
23af8c2a CK |
212 | { |
213 | int rc = 0; | |
214 | struct oz_pd *pd; | |
215 | struct oz_pd *old_pd; | |
18f8191e | 216 | |
23af8c2a CK |
217 | pd = oz_pd_find(addr); |
218 | if (pd) { | |
219 | spin_lock_bh(&g_cdev.lock); | |
072dc114 | 220 | ether_addr_copy(g_cdev.active_addr, addr); |
23af8c2a CK |
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 { | |
07732be2 | 227 | if (is_zero_ether_addr(addr)) { |
23af8c2a CK |
228 | spin_lock_bh(&g_cdev.lock); |
229 | pd = g_cdev.active_pd; | |
bc501c55 | 230 | g_cdev.active_pd = NULL; |
23af8c2a CK |
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 | } | |
6e244a83 | 242 | |
4e7fb829 | 243 | /* |
23af8c2a CK |
244 | * Context: process |
245 | */ | |
a7f74c30 PH |
246 | static long oz_cdev_ioctl(struct file *filp, unsigned int cmd, |
247 | unsigned long arg) | |
23af8c2a CK |
248 | { |
249 | int rc = 0; | |
18f8191e | 250 | |
23af8c2a CK |
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; | |
ce6880e1 | 266 | |
f724b584 | 267 | oz_dbg(ON, "OZ_IOCTL_GET_PD_LIST\n"); |
2516ffac | 268 | memset(&list, 0, sizeof(list)); |
23af8c2a CK |
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]; | |
ce6880e1 | 277 | |
f724b584 | 278 | oz_dbg(ON, "OZ_IOCTL_SET_ACTIVE_PD\n"); |
23af8c2a CK |
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]; | |
ce6880e1 | 286 | |
f724b584 | 287 | oz_dbg(ON, "OZ_IOCTL_GET_ACTIVE_PD\n"); |
23af8c2a | 288 | spin_lock_bh(&g_cdev.lock); |
072dc114 | 289 | ether_addr_copy(addr, g_cdev.active_addr); |
23af8c2a CK |
290 | spin_unlock_bh(&g_cdev.lock); |
291 | if (copy_to_user((void __user *)arg, addr, ETH_ALEN)) | |
292 | return -EFAULT; | |
293 | } | |
294 | break; | |
23af8c2a CK |
295 | case OZ_IOCTL_ADD_BINDING: |
296 | case OZ_IOCTL_REMOVE_BINDING: { | |
297 | struct oz_binding_info b; | |
ce6880e1 | 298 | |
23af8c2a CK |
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 | } | |
6e244a83 | 314 | |
4e7fb829 | 315 | /* |
23af8c2a CK |
316 | * Context: process |
317 | */ | |
a7f74c30 | 318 | static unsigned int oz_cdev_poll(struct file *filp, poll_table *wait) |
23af8c2a CK |
319 | { |
320 | unsigned int ret = 0; | |
321 | struct oz_cdev *dev = filp->private_data; | |
18f8191e | 322 | |
f724b584 | 323 | oz_dbg(ON, "Poll called wait = %p\n", wait); |
23af8c2a CK |
324 | spin_lock_bh(&dev->lock); |
325 | if (dev->active_pd) { | |
326 | struct oz_serial_ctx *ctx = oz_cdev_claim_ctx(dev->active_pd); | |
ce6880e1 | 327 | |
23af8c2a CK |
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 | } | |
6e244a83 | 339 | |
4e7fb829 | 340 | /* |
23af8c2a | 341 | */ |
a7f74c30 | 342 | static const struct file_operations oz_fops = { |
23af8c2a CK |
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 | }; | |
6e244a83 | 351 | |
4e7fb829 | 352 | /* |
23af8c2a CK |
353 | * Context: process |
354 | */ | |
355 | int oz_cdev_register(void) | |
356 | { | |
357 | int err; | |
6261c1ee | 358 | struct device *dev; |
8079154a | 359 | |
23af8c2a CK |
360 | memset(&g_cdev, 0, sizeof(g_cdev)); |
361 | err = alloc_chrdev_region(&g_cdev.devnum, 0, 1, "ozwpan"); | |
362 | if (err < 0) | |
8079154a | 363 | return err; |
f724b584 JP |
364 | oz_dbg(ON, "Alloc dev number %d:%d\n", |
365 | MAJOR(g_cdev.devnum), MINOR(g_cdev.devnum)); | |
23af8c2a CK |
366 | cdev_init(&g_cdev.cdev, &oz_fops); |
367 | g_cdev.cdev.owner = THIS_MODULE; | |
23af8c2a CK |
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); | |
6261c1ee | 371 | if (err < 0) { |
f724b584 | 372 | oz_dbg(ON, "Failed to add cdev\n"); |
8079154a | 373 | goto unregister; |
6261c1ee | 374 | } |
ba0a7ae7 RG |
375 | g_oz_class = class_create(THIS_MODULE, "ozmo_wpan"); |
376 | if (IS_ERR(g_oz_class)) { | |
f724b584 | 377 | oz_dbg(ON, "Failed to register ozmo_wpan class\n"); |
b491564e | 378 | err = PTR_ERR(g_oz_class); |
8079154a | 379 | goto delete; |
6261c1ee | 380 | } |
ba0a7ae7 | 381 | dev = device_create(g_oz_class, NULL, g_cdev.devnum, NULL, "ozwpan"); |
6261c1ee | 382 | if (IS_ERR(dev)) { |
f724b584 | 383 | oz_dbg(ON, "Failed to create sysfs entry for cdev\n"); |
b491564e | 384 | err = PTR_ERR(dev); |
8079154a | 385 | goto delete; |
6261c1ee | 386 | } |
23af8c2a | 387 | return 0; |
8079154a RG |
388 | |
389 | delete: | |
6261c1ee | 390 | cdev_del(&g_cdev.cdev); |
8079154a | 391 | unregister: |
6261c1ee | 392 | unregister_chrdev_region(g_cdev.devnum, 1); |
6261c1ee | 393 | return err; |
23af8c2a | 394 | } |
6e244a83 | 395 | |
4e7fb829 | 396 | /* |
23af8c2a CK |
397 | * Context: process |
398 | */ | |
399 | int oz_cdev_deregister(void) | |
400 | { | |
401 | cdev_del(&g_cdev.cdev); | |
402 | unregister_chrdev_region(g_cdev.devnum, 1); | |
ba0a7ae7 RG |
403 | if (g_oz_class) { |
404 | device_destroy(g_oz_class, g_cdev.devnum); | |
405 | class_destroy(g_oz_class); | |
406 | } | |
23af8c2a CK |
407 | return 0; |
408 | } | |
6e244a83 | 409 | |
4e7fb829 | 410 | /* |
23af8c2a CK |
411 | * Context: process |
412 | */ | |
413 | int oz_cdev_init(void) | |
414 | { | |
23af8c2a CK |
415 | oz_app_enable(OZ_APPID_SERIAL, 1); |
416 | return 0; | |
417 | } | |
6e244a83 | 418 | |
4e7fb829 | 419 | /* |
23af8c2a CK |
420 | * Context: process |
421 | */ | |
422 | void oz_cdev_term(void) | |
423 | { | |
23af8c2a CK |
424 | oz_app_enable(OZ_APPID_SERIAL, 0); |
425 | } | |
6e244a83 | 426 | |
4e7fb829 | 427 | /* |
23af8c2a CK |
428 | * Context: softirq-serialized |
429 | */ | |
430 | int oz_cdev_start(struct oz_pd *pd, int resume) | |
431 | { | |
432 | struct oz_serial_ctx *ctx; | |
bc501c55 | 433 | struct oz_serial_ctx *old_ctx; |
18f8191e | 434 | |
23af8c2a | 435 | if (resume) { |
f724b584 | 436 | oz_dbg(ON, "Serial service resumed\n"); |
23af8c2a CK |
437 | return 0; |
438 | } | |
1ec41a31 | 439 | ctx = kzalloc(sizeof(struct oz_serial_ctx), GFP_ATOMIC); |
3d9562a6 | 440 | if (ctx == NULL) |
1ec41a31 | 441 | return -ENOMEM; |
23af8c2a CK |
442 | atomic_set(&ctx->ref_count, 1); |
443 | ctx->tx_seq_num = 1; | |
a9686e78 CJ |
444 | spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL]); |
445 | old_ctx = pd->app_ctx[OZ_APPID_SERIAL]; | |
23af8c2a | 446 | if (old_ctx) { |
a9686e78 | 447 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]); |
1ec41a31 | 448 | kfree(ctx); |
23af8c2a | 449 | } else { |
a9686e78 CJ |
450 | pd->app_ctx[OZ_APPID_SERIAL] = ctx; |
451 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL]); | |
23af8c2a CK |
452 | } |
453 | spin_lock(&g_cdev.lock); | |
3d9562a6 | 454 | if ((g_cdev.active_pd == NULL) && |
93dc5e42 | 455 | ether_addr_equal(pd->mac_addr, g_cdev.active_addr)) { |
23af8c2a CK |
456 | oz_pd_get(pd); |
457 | g_cdev.active_pd = pd; | |
f724b584 | 458 | oz_dbg(ON, "Active PD arrived\n"); |
23af8c2a CK |
459 | } |
460 | spin_unlock(&g_cdev.lock); | |
f724b584 | 461 | oz_dbg(ON, "Serial service started\n"); |
23af8c2a CK |
462 | return 0; |
463 | } | |
6e244a83 | 464 | |
4e7fb829 | 465 | /* |
23af8c2a CK |
466 | * Context: softirq or process |
467 | */ | |
468 | void oz_cdev_stop(struct oz_pd *pd, int pause) | |
469 | { | |
470 | struct oz_serial_ctx *ctx; | |
18f8191e | 471 | |
23af8c2a | 472 | if (pause) { |
f724b584 | 473 | oz_dbg(ON, "Serial service paused\n"); |
23af8c2a CK |
474 | return; |
475 | } | |
a9686e78 CJ |
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]); | |
23af8c2a CK |
480 | if (ctx) |
481 | oz_cdev_release_ctx(ctx); | |
482 | spin_lock(&g_cdev.lock); | |
483 | if (pd == g_cdev.active_pd) | |
bc501c55 | 484 | g_cdev.active_pd = NULL; |
23af8c2a | 485 | else |
bc501c55 | 486 | pd = NULL; |
23af8c2a CK |
487 | spin_unlock(&g_cdev.lock); |
488 | if (pd) { | |
489 | oz_pd_put(pd); | |
f724b584 | 490 | oz_dbg(ON, "Active PD departed\n"); |
23af8c2a | 491 | } |
f724b584 | 492 | oz_dbg(ON, "Serial service stopped\n"); |
23af8c2a | 493 | } |
6e244a83 | 494 | |
4e7fb829 | 495 | /* |
23af8c2a CK |
496 | * Context: softirq-serialized |
497 | */ | |
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); | |
3d9562a6 | 509 | if (ctx == NULL) { |
f724b584 | 510 | oz_dbg(ON, "Cannot claim serial context\n"); |
23af8c2a CK |
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. */ | |
f724b584 JP |
520 | oz_dbg(ON, "Duplicate element:%02x %02x\n", |
521 | app_hdr->elt_seq_num, ctx->rx_seq_num); | |
23af8c2a CK |
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) { | |
f724b584 | 534 | oz_dbg(ON, "Not enough space:%d %d\n", len, space); |
23af8c2a CK |
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 | } |