Root/
1 | /* |
2 | * This program is free software; you can redistribute it and/or modify |
3 | * it under the terms of the GNU General Public License as published by |
4 | * the Free Software Foundation; either version 2 of the License, or |
5 | * (at your option) any later version. |
6 | * |
7 | * Copyright (C) Jonathan Naylor G4KLX (g4klx@g4klx.demon.co.uk) |
8 | */ |
9 | #include <linux/errno.h> |
10 | #include <linux/types.h> |
11 | #include <linux/socket.h> |
12 | #include <linux/in.h> |
13 | #include <linux/kernel.h> |
14 | #include <linux/timer.h> |
15 | #include <linux/string.h> |
16 | #include <linux/sockios.h> |
17 | #include <linux/net.h> |
18 | #include <linux/slab.h> |
19 | #include <net/ax25.h> |
20 | #include <linux/inet.h> |
21 | #include <linux/netdevice.h> |
22 | #include <linux/skbuff.h> |
23 | #include <net/sock.h> |
24 | #include <net/tcp_states.h> |
25 | #include <asm/system.h> |
26 | #include <linux/fcntl.h> |
27 | #include <linux/mm.h> |
28 | #include <linux/interrupt.h> |
29 | #include <net/rose.h> |
30 | |
31 | static int rose_create_facilities(unsigned char *buffer, struct rose_sock *rose); |
32 | |
33 | /* |
34 | * This routine purges all of the queues of frames. |
35 | */ |
36 | void rose_clear_queues(struct sock *sk) |
37 | { |
38 | skb_queue_purge(&sk->sk_write_queue); |
39 | skb_queue_purge(&rose_sk(sk)->ack_queue); |
40 | } |
41 | |
42 | /* |
43 | * This routine purges the input queue of those frames that have been |
44 | * acknowledged. This replaces the boxes labelled "V(a) <- N(r)" on the |
45 | * SDL diagram. |
46 | */ |
47 | void rose_frames_acked(struct sock *sk, unsigned short nr) |
48 | { |
49 | struct sk_buff *skb; |
50 | struct rose_sock *rose = rose_sk(sk); |
51 | |
52 | /* |
53 | * Remove all the ack-ed frames from the ack queue. |
54 | */ |
55 | if (rose->va != nr) { |
56 | while (skb_peek(&rose->ack_queue) != NULL && rose->va != nr) { |
57 | skb = skb_dequeue(&rose->ack_queue); |
58 | kfree_skb(skb); |
59 | rose->va = (rose->va + 1) % ROSE_MODULUS; |
60 | } |
61 | } |
62 | } |
63 | |
64 | void rose_requeue_frames(struct sock *sk) |
65 | { |
66 | struct sk_buff *skb, *skb_prev = NULL; |
67 | |
68 | /* |
69 | * Requeue all the un-ack-ed frames on the output queue to be picked |
70 | * up by rose_kick. This arrangement handles the possibility of an |
71 | * empty output queue. |
72 | */ |
73 | while ((skb = skb_dequeue(&rose_sk(sk)->ack_queue)) != NULL) { |
74 | if (skb_prev == NULL) |
75 | skb_queue_head(&sk->sk_write_queue, skb); |
76 | else |
77 | skb_append(skb_prev, skb, &sk->sk_write_queue); |
78 | skb_prev = skb; |
79 | } |
80 | } |
81 | |
82 | /* |
83 | * Validate that the value of nr is between va and vs. Return true or |
84 | * false for testing. |
85 | */ |
86 | int rose_validate_nr(struct sock *sk, unsigned short nr) |
87 | { |
88 | struct rose_sock *rose = rose_sk(sk); |
89 | unsigned short vc = rose->va; |
90 | |
91 | while (vc != rose->vs) { |
92 | if (nr == vc) return 1; |
93 | vc = (vc + 1) % ROSE_MODULUS; |
94 | } |
95 | |
96 | return nr == rose->vs; |
97 | } |
98 | |
99 | /* |
100 | * This routine is called when the packet layer internally generates a |
101 | * control frame. |
102 | */ |
103 | void rose_write_internal(struct sock *sk, int frametype) |
104 | { |
105 | struct rose_sock *rose = rose_sk(sk); |
106 | struct sk_buff *skb; |
107 | unsigned char *dptr; |
108 | unsigned char lci1, lci2; |
109 | char buffer[100]; |
110 | int len, faclen = 0; |
111 | |
112 | len = AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + ROSE_MIN_LEN + 1; |
113 | |
114 | switch (frametype) { |
115 | case ROSE_CALL_REQUEST: |
116 | len += 1 + ROSE_ADDR_LEN + ROSE_ADDR_LEN; |
117 | faclen = rose_create_facilities(buffer, rose); |
118 | len += faclen; |
119 | break; |
120 | case ROSE_CALL_ACCEPTED: |
121 | case ROSE_CLEAR_REQUEST: |
122 | case ROSE_RESET_REQUEST: |
123 | len += 2; |
124 | break; |
125 | } |
126 | |
127 | if ((skb = alloc_skb(len, GFP_ATOMIC)) == NULL) |
128 | return; |
129 | |
130 | /* |
131 | * Space for AX.25 header and PID. |
132 | */ |
133 | skb_reserve(skb, AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + 1); |
134 | |
135 | dptr = skb_put(skb, skb_tailroom(skb)); |
136 | |
137 | lci1 = (rose->lci >> 8) & 0x0F; |
138 | lci2 = (rose->lci >> 0) & 0xFF; |
139 | |
140 | switch (frametype) { |
141 | case ROSE_CALL_REQUEST: |
142 | *dptr++ = ROSE_GFI | lci1; |
143 | *dptr++ = lci2; |
144 | *dptr++ = frametype; |
145 | *dptr++ = 0xAA; |
146 | memcpy(dptr, &rose->dest_addr, ROSE_ADDR_LEN); |
147 | dptr += ROSE_ADDR_LEN; |
148 | memcpy(dptr, &rose->source_addr, ROSE_ADDR_LEN); |
149 | dptr += ROSE_ADDR_LEN; |
150 | memcpy(dptr, buffer, faclen); |
151 | dptr += faclen; |
152 | break; |
153 | |
154 | case ROSE_CALL_ACCEPTED: |
155 | *dptr++ = ROSE_GFI | lci1; |
156 | *dptr++ = lci2; |
157 | *dptr++ = frametype; |
158 | *dptr++ = 0x00; /* Address length */ |
159 | *dptr++ = 0; /* Facilities length */ |
160 | break; |
161 | |
162 | case ROSE_CLEAR_REQUEST: |
163 | *dptr++ = ROSE_GFI | lci1; |
164 | *dptr++ = lci2; |
165 | *dptr++ = frametype; |
166 | *dptr++ = rose->cause; |
167 | *dptr++ = rose->diagnostic; |
168 | break; |
169 | |
170 | case ROSE_RESET_REQUEST: |
171 | *dptr++ = ROSE_GFI | lci1; |
172 | *dptr++ = lci2; |
173 | *dptr++ = frametype; |
174 | *dptr++ = ROSE_DTE_ORIGINATED; |
175 | *dptr++ = 0; |
176 | break; |
177 | |
178 | case ROSE_RR: |
179 | case ROSE_RNR: |
180 | *dptr++ = ROSE_GFI | lci1; |
181 | *dptr++ = lci2; |
182 | *dptr = frametype; |
183 | *dptr++ |= (rose->vr << 5) & 0xE0; |
184 | break; |
185 | |
186 | case ROSE_CLEAR_CONFIRMATION: |
187 | case ROSE_RESET_CONFIRMATION: |
188 | *dptr++ = ROSE_GFI | lci1; |
189 | *dptr++ = lci2; |
190 | *dptr++ = frametype; |
191 | break; |
192 | |
193 | default: |
194 | printk(KERN_ERR "ROSE: rose_write_internal - invalid frametype %02X\n", frametype); |
195 | kfree_skb(skb); |
196 | return; |
197 | } |
198 | |
199 | rose_transmit_link(skb, rose->neighbour); |
200 | } |
201 | |
202 | int rose_decode(struct sk_buff *skb, int *ns, int *nr, int *q, int *d, int *m) |
203 | { |
204 | unsigned char *frame; |
205 | |
206 | frame = skb->data; |
207 | |
208 | *ns = *nr = *q = *d = *m = 0; |
209 | |
210 | switch (frame[2]) { |
211 | case ROSE_CALL_REQUEST: |
212 | case ROSE_CALL_ACCEPTED: |
213 | case ROSE_CLEAR_REQUEST: |
214 | case ROSE_CLEAR_CONFIRMATION: |
215 | case ROSE_RESET_REQUEST: |
216 | case ROSE_RESET_CONFIRMATION: |
217 | return frame[2]; |
218 | default: |
219 | break; |
220 | } |
221 | |
222 | if ((frame[2] & 0x1F) == ROSE_RR || |
223 | (frame[2] & 0x1F) == ROSE_RNR) { |
224 | *nr = (frame[2] >> 5) & 0x07; |
225 | return frame[2] & 0x1F; |
226 | } |
227 | |
228 | if ((frame[2] & 0x01) == ROSE_DATA) { |
229 | *q = (frame[0] & ROSE_Q_BIT) == ROSE_Q_BIT; |
230 | *d = (frame[0] & ROSE_D_BIT) == ROSE_D_BIT; |
231 | *m = (frame[2] & ROSE_M_BIT) == ROSE_M_BIT; |
232 | *nr = (frame[2] >> 5) & 0x07; |
233 | *ns = (frame[2] >> 1) & 0x07; |
234 | return ROSE_DATA; |
235 | } |
236 | |
237 | return ROSE_ILLEGAL; |
238 | } |
239 | |
240 | static int rose_parse_national(unsigned char *p, struct rose_facilities_struct *facilities, int len) |
241 | { |
242 | unsigned char *pt; |
243 | unsigned char l, lg, n = 0; |
244 | int fac_national_digis_received = 0; |
245 | |
246 | do { |
247 | switch (*p & 0xC0) { |
248 | case 0x00: |
249 | p += 2; |
250 | n += 2; |
251 | len -= 2; |
252 | break; |
253 | |
254 | case 0x40: |
255 | if (*p == FAC_NATIONAL_RAND) |
256 | facilities->rand = ((p[1] << 8) & 0xFF00) + ((p[2] << 0) & 0x00FF); |
257 | p += 3; |
258 | n += 3; |
259 | len -= 3; |
260 | break; |
261 | |
262 | case 0x80: |
263 | p += 4; |
264 | n += 4; |
265 | len -= 4; |
266 | break; |
267 | |
268 | case 0xC0: |
269 | l = p[1]; |
270 | if (*p == FAC_NATIONAL_DEST_DIGI) { |
271 | if (!fac_national_digis_received) { |
272 | memcpy(&facilities->source_digis[0], p + 2, AX25_ADDR_LEN); |
273 | facilities->source_ndigis = 1; |
274 | } |
275 | } |
276 | else if (*p == FAC_NATIONAL_SRC_DIGI) { |
277 | if (!fac_national_digis_received) { |
278 | memcpy(&facilities->dest_digis[0], p + 2, AX25_ADDR_LEN); |
279 | facilities->dest_ndigis = 1; |
280 | } |
281 | } |
282 | else if (*p == FAC_NATIONAL_FAIL_CALL) { |
283 | memcpy(&facilities->fail_call, p + 2, AX25_ADDR_LEN); |
284 | } |
285 | else if (*p == FAC_NATIONAL_FAIL_ADD) { |
286 | memcpy(&facilities->fail_addr, p + 3, ROSE_ADDR_LEN); |
287 | } |
288 | else if (*p == FAC_NATIONAL_DIGIS) { |
289 | fac_national_digis_received = 1; |
290 | facilities->source_ndigis = 0; |
291 | facilities->dest_ndigis = 0; |
292 | for (pt = p + 2, lg = 0 ; lg < l ; pt += AX25_ADDR_LEN, lg += AX25_ADDR_LEN) { |
293 | if (pt[6] & AX25_HBIT) |
294 | memcpy(&facilities->dest_digis[facilities->dest_ndigis++], pt, AX25_ADDR_LEN); |
295 | else |
296 | memcpy(&facilities->source_digis[facilities->source_ndigis++], pt, AX25_ADDR_LEN); |
297 | } |
298 | } |
299 | p += l + 2; |
300 | n += l + 2; |
301 | len -= l + 2; |
302 | break; |
303 | } |
304 | } while (*p != 0x00 && len > 0); |
305 | |
306 | return n; |
307 | } |
308 | |
309 | static int rose_parse_ccitt(unsigned char *p, struct rose_facilities_struct *facilities, int len) |
310 | { |
311 | unsigned char l, n = 0; |
312 | char callsign[11]; |
313 | |
314 | do { |
315 | switch (*p & 0xC0) { |
316 | case 0x00: |
317 | p += 2; |
318 | n += 2; |
319 | len -= 2; |
320 | break; |
321 | |
322 | case 0x40: |
323 | p += 3; |
324 | n += 3; |
325 | len -= 3; |
326 | break; |
327 | |
328 | case 0x80: |
329 | p += 4; |
330 | n += 4; |
331 | len -= 4; |
332 | break; |
333 | |
334 | case 0xC0: |
335 | l = p[1]; |
336 | if (*p == FAC_CCITT_DEST_NSAP) { |
337 | memcpy(&facilities->source_addr, p + 7, ROSE_ADDR_LEN); |
338 | memcpy(callsign, p + 12, l - 10); |
339 | callsign[l - 10] = '\0'; |
340 | asc2ax(&facilities->source_call, callsign); |
341 | } |
342 | if (*p == FAC_CCITT_SRC_NSAP) { |
343 | memcpy(&facilities->dest_addr, p + 7, ROSE_ADDR_LEN); |
344 | memcpy(callsign, p + 12, l - 10); |
345 | callsign[l - 10] = '\0'; |
346 | asc2ax(&facilities->dest_call, callsign); |
347 | } |
348 | p += l + 2; |
349 | n += l + 2; |
350 | len -= l + 2; |
351 | break; |
352 | } |
353 | } while (*p != 0x00 && len > 0); |
354 | |
355 | return n; |
356 | } |
357 | |
358 | int rose_parse_facilities(unsigned char *p, |
359 | struct rose_facilities_struct *facilities) |
360 | { |
361 | int facilities_len, len; |
362 | |
363 | facilities_len = *p++; |
364 | |
365 | if (facilities_len == 0) |
366 | return 0; |
367 | |
368 | while (facilities_len > 0) { |
369 | if (*p == 0x00) { |
370 | facilities_len--; |
371 | p++; |
372 | |
373 | switch (*p) { |
374 | case FAC_NATIONAL: /* National */ |
375 | len = rose_parse_national(p + 1, facilities, facilities_len - 1); |
376 | facilities_len -= len + 1; |
377 | p += len + 1; |
378 | break; |
379 | |
380 | case FAC_CCITT: /* CCITT */ |
381 | len = rose_parse_ccitt(p + 1, facilities, facilities_len - 1); |
382 | facilities_len -= len + 1; |
383 | p += len + 1; |
384 | break; |
385 | |
386 | default: |
387 | printk(KERN_DEBUG "ROSE: rose_parse_facilities - unknown facilities family %02X\n", *p); |
388 | facilities_len--; |
389 | p++; |
390 | break; |
391 | } |
392 | } else |
393 | break; /* Error in facilities format */ |
394 | } |
395 | |
396 | return 1; |
397 | } |
398 | |
399 | static int rose_create_facilities(unsigned char *buffer, struct rose_sock *rose) |
400 | { |
401 | unsigned char *p = buffer + 1; |
402 | char *callsign; |
403 | char buf[11]; |
404 | int len, nb; |
405 | |
406 | /* National Facilities */ |
407 | if (rose->rand != 0 || rose->source_ndigis == 1 || rose->dest_ndigis == 1) { |
408 | *p++ = 0x00; |
409 | *p++ = FAC_NATIONAL; |
410 | |
411 | if (rose->rand != 0) { |
412 | *p++ = FAC_NATIONAL_RAND; |
413 | *p++ = (rose->rand >> 8) & 0xFF; |
414 | *p++ = (rose->rand >> 0) & 0xFF; |
415 | } |
416 | |
417 | /* Sent before older facilities */ |
418 | if ((rose->source_ndigis > 0) || (rose->dest_ndigis > 0)) { |
419 | int maxdigi = 0; |
420 | *p++ = FAC_NATIONAL_DIGIS; |
421 | *p++ = AX25_ADDR_LEN * (rose->source_ndigis + rose->dest_ndigis); |
422 | for (nb = 0 ; nb < rose->source_ndigis ; nb++) { |
423 | if (++maxdigi >= ROSE_MAX_DIGIS) |
424 | break; |
425 | memcpy(p, &rose->source_digis[nb], AX25_ADDR_LEN); |
426 | p[6] |= AX25_HBIT; |
427 | p += AX25_ADDR_LEN; |
428 | } |
429 | for (nb = 0 ; nb < rose->dest_ndigis ; nb++) { |
430 | if (++maxdigi >= ROSE_MAX_DIGIS) |
431 | break; |
432 | memcpy(p, &rose->dest_digis[nb], AX25_ADDR_LEN); |
433 | p[6] &= ~AX25_HBIT; |
434 | p += AX25_ADDR_LEN; |
435 | } |
436 | } |
437 | |
438 | /* For compatibility */ |
439 | if (rose->source_ndigis > 0) { |
440 | *p++ = FAC_NATIONAL_SRC_DIGI; |
441 | *p++ = AX25_ADDR_LEN; |
442 | memcpy(p, &rose->source_digis[0], AX25_ADDR_LEN); |
443 | p += AX25_ADDR_LEN; |
444 | } |
445 | |
446 | /* For compatibility */ |
447 | if (rose->dest_ndigis > 0) { |
448 | *p++ = FAC_NATIONAL_DEST_DIGI; |
449 | *p++ = AX25_ADDR_LEN; |
450 | memcpy(p, &rose->dest_digis[0], AX25_ADDR_LEN); |
451 | p += AX25_ADDR_LEN; |
452 | } |
453 | } |
454 | |
455 | *p++ = 0x00; |
456 | *p++ = FAC_CCITT; |
457 | |
458 | *p++ = FAC_CCITT_DEST_NSAP; |
459 | |
460 | callsign = ax2asc(buf, &rose->dest_call); |
461 | |
462 | *p++ = strlen(callsign) + 10; |
463 | *p++ = (strlen(callsign) + 9) * 2; /* ??? */ |
464 | |
465 | *p++ = 0x47; *p++ = 0x00; *p++ = 0x11; |
466 | *p++ = ROSE_ADDR_LEN * 2; |
467 | memcpy(p, &rose->dest_addr, ROSE_ADDR_LEN); |
468 | p += ROSE_ADDR_LEN; |
469 | |
470 | memcpy(p, callsign, strlen(callsign)); |
471 | p += strlen(callsign); |
472 | |
473 | *p++ = FAC_CCITT_SRC_NSAP; |
474 | |
475 | callsign = ax2asc(buf, &rose->source_call); |
476 | |
477 | *p++ = strlen(callsign) + 10; |
478 | *p++ = (strlen(callsign) + 9) * 2; /* ??? */ |
479 | |
480 | *p++ = 0x47; *p++ = 0x00; *p++ = 0x11; |
481 | *p++ = ROSE_ADDR_LEN * 2; |
482 | memcpy(p, &rose->source_addr, ROSE_ADDR_LEN); |
483 | p += ROSE_ADDR_LEN; |
484 | |
485 | memcpy(p, callsign, strlen(callsign)); |
486 | p += strlen(callsign); |
487 | |
488 | len = p - buffer; |
489 | buffer[0] = len - 1; |
490 | |
491 | return len; |
492 | } |
493 | |
494 | void rose_disconnect(struct sock *sk, int reason, int cause, int diagnostic) |
495 | { |
496 | struct rose_sock *rose = rose_sk(sk); |
497 | |
498 | rose_stop_timer(sk); |
499 | rose_stop_idletimer(sk); |
500 | |
501 | rose_clear_queues(sk); |
502 | |
503 | rose->lci = 0; |
504 | rose->state = ROSE_STATE_0; |
505 | |
506 | if (cause != -1) |
507 | rose->cause = cause; |
508 | |
509 | if (diagnostic != -1) |
510 | rose->diagnostic = diagnostic; |
511 | |
512 | sk->sk_state = TCP_CLOSE; |
513 | sk->sk_err = reason; |
514 | sk->sk_shutdown |= SEND_SHUTDOWN; |
515 | |
516 | if (!sock_flag(sk, SOCK_DEAD)) { |
517 | sk->sk_state_change(sk); |
518 | sock_set_flag(sk, SOCK_DEAD); |
519 | } |
520 | } |
521 |
Branches:
ben-wpan
ben-wpan-stefan
javiroman/ks7010
jz-2.6.34
jz-2.6.34-rc5
jz-2.6.34-rc6
jz-2.6.34-rc7
jz-2.6.35
jz-2.6.36
jz-2.6.37
jz-2.6.38
jz-2.6.39
jz-3.0
jz-3.1
jz-3.11
jz-3.12
jz-3.13
jz-3.15
jz-3.16
jz-3.18-dt
jz-3.2
jz-3.3
jz-3.4
jz-3.5
jz-3.6
jz-3.6-rc2-pwm
jz-3.9
jz-3.9-clk
jz-3.9-rc8
jz47xx
jz47xx-2.6.38
master
Tags:
od-2011-09-04
od-2011-09-18
v2.6.34-rc5
v2.6.34-rc6
v2.6.34-rc7
v3.9