]>
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 | |
23af8c2a CK |
52 | spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); |
53 | ctx = (struct oz_serial_ctx *)pd->app_ctx[OZ_APPID_SERIAL-1]; | |
54 | if (ctx) | |
55 | atomic_inc(&ctx->ref_count); | |
56 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); | |
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; | |
185 | spin_lock_bh(&pd->app_lock[OZ_APPID_USB-1]); | |
186 | ctx = (struct oz_serial_ctx *)pd->app_ctx[OZ_APPID_SERIAL-1]; | |
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 | } | |
196 | spin_unlock_bh(&pd->app_lock[OZ_APPID_USB-1]); | |
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; | |
f724b584 | 266 | oz_dbg(ON, "OZ_IOCTL_GET_PD_LIST\n"); |
2516ffac | 267 | memset(&list, 0, sizeof(list)); |
23af8c2a CK |
268 | list.count = oz_get_pd_list(list.addr, OZ_MAX_PDS); |
269 | if (copy_to_user((void __user *)arg, &list, | |
270 | sizeof(list))) | |
271 | return -EFAULT; | |
272 | } | |
273 | break; | |
274 | case OZ_IOCTL_SET_ACTIVE_PD: { | |
275 | u8 addr[ETH_ALEN]; | |
f724b584 | 276 | oz_dbg(ON, "OZ_IOCTL_SET_ACTIVE_PD\n"); |
23af8c2a CK |
277 | if (copy_from_user(addr, (void __user *)arg, ETH_ALEN)) |
278 | return -EFAULT; | |
279 | rc = oz_set_active_pd(addr); | |
280 | } | |
281 | break; | |
282 | case OZ_IOCTL_GET_ACTIVE_PD: { | |
283 | u8 addr[ETH_ALEN]; | |
f724b584 | 284 | oz_dbg(ON, "OZ_IOCTL_GET_ACTIVE_PD\n"); |
23af8c2a | 285 | spin_lock_bh(&g_cdev.lock); |
072dc114 | 286 | ether_addr_copy(addr, g_cdev.active_addr); |
23af8c2a CK |
287 | spin_unlock_bh(&g_cdev.lock); |
288 | if (copy_to_user((void __user *)arg, addr, ETH_ALEN)) | |
289 | return -EFAULT; | |
290 | } | |
291 | break; | |
23af8c2a CK |
292 | case OZ_IOCTL_ADD_BINDING: |
293 | case OZ_IOCTL_REMOVE_BINDING: { | |
294 | struct oz_binding_info b; | |
295 | if (copy_from_user(&b, (void __user *)arg, | |
296 | sizeof(struct oz_binding_info))) { | |
297 | return -EFAULT; | |
298 | } | |
299 | /* Make sure name is null terminated. */ | |
300 | b.name[OZ_MAX_BINDING_LEN-1] = 0; | |
301 | if (cmd == OZ_IOCTL_ADD_BINDING) | |
302 | oz_binding_add(b.name); | |
303 | else | |
304 | oz_binding_remove(b.name); | |
305 | } | |
306 | break; | |
307 | } | |
308 | return rc; | |
309 | } | |
6e244a83 | 310 | |
4e7fb829 | 311 | /* |
23af8c2a CK |
312 | * Context: process |
313 | */ | |
a7f74c30 | 314 | static unsigned int oz_cdev_poll(struct file *filp, poll_table *wait) |
23af8c2a CK |
315 | { |
316 | unsigned int ret = 0; | |
317 | struct oz_cdev *dev = filp->private_data; | |
18f8191e | 318 | |
f724b584 | 319 | oz_dbg(ON, "Poll called wait = %p\n", wait); |
23af8c2a CK |
320 | spin_lock_bh(&dev->lock); |
321 | if (dev->active_pd) { | |
322 | struct oz_serial_ctx *ctx = oz_cdev_claim_ctx(dev->active_pd); | |
323 | if (ctx) { | |
324 | if (ctx->rd_in != ctx->rd_out) | |
325 | ret |= POLLIN | POLLRDNORM; | |
326 | oz_cdev_release_ctx(ctx); | |
327 | } | |
328 | } | |
329 | spin_unlock_bh(&dev->lock); | |
330 | if (wait) | |
331 | poll_wait(filp, &dev->rdq, wait); | |
332 | return ret; | |
333 | } | |
6e244a83 | 334 | |
4e7fb829 | 335 | /* |
23af8c2a | 336 | */ |
a7f74c30 | 337 | static const struct file_operations oz_fops = { |
23af8c2a CK |
338 | .owner = THIS_MODULE, |
339 | .open = oz_cdev_open, | |
340 | .release = oz_cdev_release, | |
341 | .read = oz_cdev_read, | |
342 | .write = oz_cdev_write, | |
343 | .unlocked_ioctl = oz_cdev_ioctl, | |
344 | .poll = oz_cdev_poll | |
345 | }; | |
6e244a83 | 346 | |
4e7fb829 | 347 | /* |
23af8c2a CK |
348 | * Context: process |
349 | */ | |
350 | int oz_cdev_register(void) | |
351 | { | |
352 | int err; | |
6261c1ee | 353 | struct device *dev; |
8079154a | 354 | |
23af8c2a CK |
355 | memset(&g_cdev, 0, sizeof(g_cdev)); |
356 | err = alloc_chrdev_region(&g_cdev.devnum, 0, 1, "ozwpan"); | |
357 | if (err < 0) | |
8079154a | 358 | return err; |
f724b584 JP |
359 | oz_dbg(ON, "Alloc dev number %d:%d\n", |
360 | MAJOR(g_cdev.devnum), MINOR(g_cdev.devnum)); | |
23af8c2a CK |
361 | cdev_init(&g_cdev.cdev, &oz_fops); |
362 | g_cdev.cdev.owner = THIS_MODULE; | |
363 | g_cdev.cdev.ops = &oz_fops; | |
364 | spin_lock_init(&g_cdev.lock); | |
365 | init_waitqueue_head(&g_cdev.rdq); | |
366 | err = cdev_add(&g_cdev.cdev, g_cdev.devnum, 1); | |
6261c1ee | 367 | if (err < 0) { |
f724b584 | 368 | oz_dbg(ON, "Failed to add cdev\n"); |
8079154a | 369 | goto unregister; |
6261c1ee | 370 | } |
ba0a7ae7 RG |
371 | g_oz_class = class_create(THIS_MODULE, "ozmo_wpan"); |
372 | if (IS_ERR(g_oz_class)) { | |
f724b584 | 373 | oz_dbg(ON, "Failed to register ozmo_wpan class\n"); |
b491564e | 374 | err = PTR_ERR(g_oz_class); |
8079154a | 375 | goto delete; |
6261c1ee | 376 | } |
ba0a7ae7 | 377 | dev = device_create(g_oz_class, NULL, g_cdev.devnum, NULL, "ozwpan"); |
6261c1ee | 378 | if (IS_ERR(dev)) { |
f724b584 | 379 | oz_dbg(ON, "Failed to create sysfs entry for cdev\n"); |
b491564e | 380 | err = PTR_ERR(dev); |
8079154a | 381 | goto delete; |
6261c1ee | 382 | } |
23af8c2a | 383 | return 0; |
8079154a RG |
384 | |
385 | delete: | |
6261c1ee | 386 | cdev_del(&g_cdev.cdev); |
8079154a | 387 | unregister: |
6261c1ee | 388 | unregister_chrdev_region(g_cdev.devnum, 1); |
6261c1ee | 389 | return err; |
23af8c2a | 390 | } |
6e244a83 | 391 | |
4e7fb829 | 392 | /* |
23af8c2a CK |
393 | * Context: process |
394 | */ | |
395 | int oz_cdev_deregister(void) | |
396 | { | |
397 | cdev_del(&g_cdev.cdev); | |
398 | unregister_chrdev_region(g_cdev.devnum, 1); | |
ba0a7ae7 RG |
399 | if (g_oz_class) { |
400 | device_destroy(g_oz_class, g_cdev.devnum); | |
401 | class_destroy(g_oz_class); | |
402 | } | |
23af8c2a CK |
403 | return 0; |
404 | } | |
6e244a83 | 405 | |
4e7fb829 | 406 | /* |
23af8c2a CK |
407 | * Context: process |
408 | */ | |
409 | int oz_cdev_init(void) | |
410 | { | |
23af8c2a CK |
411 | oz_app_enable(OZ_APPID_SERIAL, 1); |
412 | return 0; | |
413 | } | |
6e244a83 | 414 | |
4e7fb829 | 415 | /* |
23af8c2a CK |
416 | * Context: process |
417 | */ | |
418 | void oz_cdev_term(void) | |
419 | { | |
23af8c2a CK |
420 | oz_app_enable(OZ_APPID_SERIAL, 0); |
421 | } | |
6e244a83 | 422 | |
4e7fb829 | 423 | /* |
23af8c2a CK |
424 | * Context: softirq-serialized |
425 | */ | |
426 | int oz_cdev_start(struct oz_pd *pd, int resume) | |
427 | { | |
428 | struct oz_serial_ctx *ctx; | |
bc501c55 | 429 | struct oz_serial_ctx *old_ctx; |
18f8191e | 430 | |
23af8c2a | 431 | if (resume) { |
f724b584 | 432 | oz_dbg(ON, "Serial service resumed\n"); |
23af8c2a CK |
433 | return 0; |
434 | } | |
1ec41a31 | 435 | ctx = kzalloc(sizeof(struct oz_serial_ctx), GFP_ATOMIC); |
3d9562a6 | 436 | if (ctx == NULL) |
1ec41a31 | 437 | return -ENOMEM; |
23af8c2a CK |
438 | atomic_set(&ctx->ref_count, 1); |
439 | ctx->tx_seq_num = 1; | |
440 | spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); | |
441 | old_ctx = pd->app_ctx[OZ_APPID_SERIAL-1]; | |
442 | if (old_ctx) { | |
443 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); | |
1ec41a31 | 444 | kfree(ctx); |
23af8c2a CK |
445 | } else { |
446 | pd->app_ctx[OZ_APPID_SERIAL-1] = ctx; | |
447 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); | |
448 | } | |
449 | spin_lock(&g_cdev.lock); | |
3d9562a6 | 450 | if ((g_cdev.active_pd == NULL) && |
93dc5e42 | 451 | ether_addr_equal(pd->mac_addr, g_cdev.active_addr)) { |
23af8c2a CK |
452 | oz_pd_get(pd); |
453 | g_cdev.active_pd = pd; | |
f724b584 | 454 | oz_dbg(ON, "Active PD arrived\n"); |
23af8c2a CK |
455 | } |
456 | spin_unlock(&g_cdev.lock); | |
f724b584 | 457 | oz_dbg(ON, "Serial service started\n"); |
23af8c2a CK |
458 | return 0; |
459 | } | |
6e244a83 | 460 | |
4e7fb829 | 461 | /* |
23af8c2a CK |
462 | * Context: softirq or process |
463 | */ | |
464 | void oz_cdev_stop(struct oz_pd *pd, int pause) | |
465 | { | |
466 | struct oz_serial_ctx *ctx; | |
18f8191e | 467 | |
23af8c2a | 468 | if (pause) { |
f724b584 | 469 | oz_dbg(ON, "Serial service paused\n"); |
23af8c2a CK |
470 | return; |
471 | } | |
472 | spin_lock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); | |
473 | ctx = (struct oz_serial_ctx *)pd->app_ctx[OZ_APPID_SERIAL-1]; | |
bc501c55 | 474 | pd->app_ctx[OZ_APPID_SERIAL-1] = NULL; |
23af8c2a CK |
475 | spin_unlock_bh(&pd->app_lock[OZ_APPID_SERIAL-1]); |
476 | if (ctx) | |
477 | oz_cdev_release_ctx(ctx); | |
478 | spin_lock(&g_cdev.lock); | |
479 | if (pd == g_cdev.active_pd) | |
bc501c55 | 480 | g_cdev.active_pd = NULL; |
23af8c2a | 481 | else |
bc501c55 | 482 | pd = NULL; |
23af8c2a CK |
483 | spin_unlock(&g_cdev.lock); |
484 | if (pd) { | |
485 | oz_pd_put(pd); | |
f724b584 | 486 | oz_dbg(ON, "Active PD departed\n"); |
23af8c2a | 487 | } |
f724b584 | 488 | oz_dbg(ON, "Serial service stopped\n"); |
23af8c2a | 489 | } |
6e244a83 | 490 | |
4e7fb829 | 491 | /* |
23af8c2a CK |
492 | * Context: softirq-serialized |
493 | */ | |
494 | void oz_cdev_rx(struct oz_pd *pd, struct oz_elt *elt) | |
495 | { | |
496 | struct oz_serial_ctx *ctx; | |
497 | struct oz_app_hdr *app_hdr; | |
498 | u8 *data; | |
499 | int len; | |
500 | int space; | |
501 | int copy_sz; | |
502 | int ix; | |
503 | ||
504 | ctx = oz_cdev_claim_ctx(pd); | |
3d9562a6 | 505 | if (ctx == NULL) { |
f724b584 | 506 | oz_dbg(ON, "Cannot claim serial context\n"); |
23af8c2a CK |
507 | return; |
508 | } | |
509 | ||
510 | app_hdr = (struct oz_app_hdr *)(elt+1); | |
511 | /* If sequence number is non-zero then check it is not a duplicate. | |
512 | */ | |
513 | if (app_hdr->elt_seq_num != 0) { | |
514 | if (((ctx->rx_seq_num - app_hdr->elt_seq_num) & 0x80) == 0) { | |
515 | /* Reject duplicate element. */ | |
f724b584 JP |
516 | oz_dbg(ON, "Duplicate element:%02x %02x\n", |
517 | app_hdr->elt_seq_num, ctx->rx_seq_num); | |
23af8c2a CK |
518 | goto out; |
519 | } | |
520 | } | |
521 | ctx->rx_seq_num = app_hdr->elt_seq_num; | |
522 | len = elt->length - sizeof(struct oz_app_hdr); | |
523 | data = ((u8 *)(elt+1)) + sizeof(struct oz_app_hdr); | |
524 | if (len <= 0) | |
525 | goto out; | |
526 | space = ctx->rd_out - ctx->rd_in - 1; | |
527 | if (space < 0) | |
528 | space += OZ_RD_BUF_SZ; | |
529 | if (len > space) { | |
f724b584 | 530 | oz_dbg(ON, "Not enough space:%d %d\n", len, space); |
23af8c2a CK |
531 | len = space; |
532 | } | |
533 | ix = ctx->rd_in; | |
534 | copy_sz = OZ_RD_BUF_SZ - ix; | |
535 | if (copy_sz > len) | |
536 | copy_sz = len; | |
537 | memcpy(&ctx->rd_buf[ix], data, copy_sz); | |
538 | len -= copy_sz; | |
539 | ix += copy_sz; | |
540 | if (ix == OZ_RD_BUF_SZ) | |
541 | ix = 0; | |
542 | if (len) { | |
543 | memcpy(ctx->rd_buf, data+copy_sz, len); | |
544 | ix = len; | |
545 | } | |
546 | ctx->rd_in = ix; | |
547 | wake_up(&g_cdev.rdq); | |
548 | out: | |
549 | oz_cdev_release_ctx(ctx); | |
550 | } |