Commit | Line | Data |
---|---|---|
4a61cd66 EL |
1 | /* |
2 | * shdlc Link Layer Control | |
3 | * | |
4 | * Copyright (C) 2012 Intel Corporation. All rights reserved. | |
5 | * | |
6 | * This program is free software; you can redistribute it and/or modify it | |
7 | * under the terms and conditions of the GNU General Public License, | |
8 | * version 2, as published by the Free Software Foundation. | |
9 | * | |
10 | * This program is distributed in the hope that it will be useful, | |
11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
13 | * GNU General Public License for more details. | |
14 | * | |
15 | * You should have received a copy of the GNU General Public License | |
98b32dec | 16 | * along with this program; if not, see <http://www.gnu.org/licenses/>. |
4a61cd66 EL |
17 | */ |
18 | ||
19 | #define pr_fmt(fmt) "shdlc: %s: " fmt, __func__ | |
20 | ||
21 | #include <linux/types.h> | |
22 | #include <linux/sched.h> | |
4a61cd66 EL |
23 | #include <linux/wait.h> |
24 | #include <linux/slab.h> | |
25 | #include <linux/skbuff.h> | |
26 | ||
27 | #include "llc.h" | |
28 | ||
29 | enum shdlc_state { | |
30 | SHDLC_DISCONNECTED = 0, | |
31 | SHDLC_CONNECTING = 1, | |
4c0ba9ac | 32 | SHDLC_NEGOTIATING = 2, |
12bfd1e8 WR |
33 | SHDLC_HALF_CONNECTED = 3, |
34 | SHDLC_CONNECTED = 4 | |
4a61cd66 EL |
35 | }; |
36 | ||
37 | struct llc_shdlc { | |
38 | struct nfc_hci_dev *hdev; | |
39 | xmit_to_drv_t xmit_to_drv; | |
40 | rcv_to_hci_t rcv_to_hci; | |
41 | ||
42 | struct mutex state_mutex; | |
43 | enum shdlc_state state; | |
44 | int hard_fault; | |
45 | ||
46 | wait_queue_head_t *connect_wq; | |
47 | int connect_tries; | |
48 | int connect_result; | |
49 | struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */ | |
50 | ||
51 | u8 w; /* window size */ | |
52 | bool srej_support; | |
53 | ||
54 | struct timer_list t1_timer; /* send ack timeout */ | |
55 | bool t1_active; | |
56 | ||
57 | struct timer_list t2_timer; /* guard/retransmit timeout */ | |
58 | bool t2_active; | |
59 | ||
60 | int ns; /* next seq num for send */ | |
61 | int nr; /* next expected seq num for receive */ | |
62 | int dnr; /* oldest sent unacked seq num */ | |
63 | ||
64 | struct sk_buff_head rcv_q; | |
65 | ||
66 | struct sk_buff_head send_q; | |
67 | bool rnr; /* other side is not ready to receive */ | |
68 | ||
69 | struct sk_buff_head ack_pending_q; | |
70 | ||
71 | struct work_struct sm_work; | |
72 | ||
73 | int tx_headroom; | |
74 | int tx_tailroom; | |
75 | ||
76 | llc_failure_t llc_failure; | |
77 | }; | |
78 | ||
79 | #define SHDLC_LLC_HEAD_ROOM 2 | |
80 | ||
81 | #define SHDLC_MAX_WINDOW 4 | |
82 | #define SHDLC_SREJ_SUPPORT false | |
83 | ||
84 | #define SHDLC_CONTROL_HEAD_MASK 0xe0 | |
85 | #define SHDLC_CONTROL_HEAD_I 0x80 | |
86 | #define SHDLC_CONTROL_HEAD_I2 0xa0 | |
87 | #define SHDLC_CONTROL_HEAD_S 0xc0 | |
88 | #define SHDLC_CONTROL_HEAD_U 0xe0 | |
89 | ||
90 | #define SHDLC_CONTROL_NS_MASK 0x38 | |
91 | #define SHDLC_CONTROL_NR_MASK 0x07 | |
92 | #define SHDLC_CONTROL_TYPE_MASK 0x18 | |
93 | ||
94 | #define SHDLC_CONTROL_M_MASK 0x1f | |
95 | ||
96 | enum sframe_type { | |
97 | S_FRAME_RR = 0x00, | |
98 | S_FRAME_REJ = 0x01, | |
99 | S_FRAME_RNR = 0x02, | |
100 | S_FRAME_SREJ = 0x03 | |
101 | }; | |
102 | ||
103 | enum uframe_modifier { | |
104 | U_FRAME_UA = 0x06, | |
105 | U_FRAME_RSET = 0x19 | |
106 | }; | |
107 | ||
108 | #define SHDLC_CONNECT_VALUE_MS 5 | |
109 | #define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4) | |
110 | #define SHDLC_T2_VALUE_MS 300 | |
111 | ||
112 | #define SHDLC_DUMP_SKB(info, skb) \ | |
113 | do { \ | |
114 | pr_debug("%s:\n", info); \ | |
115 | print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \ | |
116 | 16, 1, skb->data, skb->len, 0); \ | |
117 | } while (0) | |
118 | ||
119 | /* checks x < y <= z modulo 8 */ | |
120 | static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z) | |
121 | { | |
122 | if (x < z) | |
123 | return ((x < y) && (y <= z)) ? true : false; | |
124 | else | |
125 | return ((y > x) || (y <= z)) ? true : false; | |
126 | } | |
127 | ||
128 | /* checks x <= y < z modulo 8 */ | |
129 | static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z) | |
130 | { | |
131 | if (x <= z) | |
132 | return ((x <= y) && (y < z)) ? true : false; | |
133 | else /* x > z -> z+8 > x */ | |
134 | return ((y >= x) || (y < z)) ? true : false; | |
135 | } | |
136 | ||
137 | static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc, | |
138 | int payload_len) | |
139 | { | |
140 | struct sk_buff *skb; | |
141 | ||
142 | skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM + | |
143 | shdlc->tx_tailroom + payload_len, GFP_KERNEL); | |
144 | if (skb) | |
145 | skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM); | |
146 | ||
147 | return skb; | |
148 | } | |
149 | ||
150 | /* immediately sends an S frame. */ | |
151 | static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc, | |
152 | enum sframe_type sframe_type, int nr) | |
153 | { | |
154 | int r; | |
155 | struct sk_buff *skb; | |
156 | ||
157 | pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr); | |
158 | ||
159 | skb = llc_shdlc_alloc_skb(shdlc, 0); | |
160 | if (skb == NULL) | |
161 | return -ENOMEM; | |
162 | ||
163 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr; | |
164 | ||
165 | r = shdlc->xmit_to_drv(shdlc->hdev, skb); | |
166 | ||
167 | kfree_skb(skb); | |
168 | ||
169 | return r; | |
170 | } | |
171 | ||
172 | /* immediately sends an U frame. skb may contain optional payload */ | |
173 | static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc, | |
174 | struct sk_buff *skb, | |
175 | enum uframe_modifier uframe_modifier) | |
176 | { | |
177 | int r; | |
178 | ||
179 | pr_debug("uframe_modifier=%d\n", uframe_modifier); | |
180 | ||
181 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier; | |
182 | ||
183 | r = shdlc->xmit_to_drv(shdlc->hdev, skb); | |
184 | ||
185 | kfree_skb(skb); | |
186 | ||
187 | return r; | |
188 | } | |
189 | ||
190 | /* | |
191 | * Free ack_pending frames until y_nr - 1, and reset t2 according to | |
192 | * the remaining oldest ack_pending frame sent time | |
193 | */ | |
194 | static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr) | |
195 | { | |
196 | struct sk_buff *skb; | |
197 | int dnr = shdlc->dnr; /* MUST initially be < y_nr */ | |
198 | ||
199 | pr_debug("release ack pending up to frame %d excluded\n", y_nr); | |
200 | ||
201 | while (dnr != y_nr) { | |
202 | pr_debug("release ack pending frame %d\n", dnr); | |
203 | ||
204 | skb = skb_dequeue(&shdlc->ack_pending_q); | |
205 | kfree_skb(skb); | |
206 | ||
207 | dnr = (dnr + 1) % 8; | |
208 | } | |
209 | ||
210 | if (skb_queue_empty(&shdlc->ack_pending_q)) { | |
211 | if (shdlc->t2_active) { | |
212 | del_timer_sync(&shdlc->t2_timer); | |
213 | shdlc->t2_active = false; | |
214 | ||
215 | pr_debug | |
216 | ("All sent frames acked. Stopped T2(retransmit)\n"); | |
217 | } | |
218 | } else { | |
219 | skb = skb_peek(&shdlc->ack_pending_q); | |
220 | ||
221 | mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb + | |
222 | msecs_to_jiffies(SHDLC_T2_VALUE_MS)); | |
223 | shdlc->t2_active = true; | |
224 | ||
225 | pr_debug | |
226 | ("Start T2(retransmit) for remaining unacked sent frames\n"); | |
227 | } | |
228 | } | |
229 | ||
230 | /* | |
231 | * Receive validated frames from lower layer. skb contains HCI payload only. | |
232 | * Handle according to algorithm at spec:10.8.2 | |
233 | */ | |
234 | static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc, | |
235 | struct sk_buff *skb, int ns, int nr) | |
236 | { | |
237 | int x_ns = ns; | |
238 | int y_nr = nr; | |
239 | ||
240 | pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr); | |
241 | ||
242 | if (shdlc->state != SHDLC_CONNECTED) | |
243 | goto exit; | |
244 | ||
245 | if (x_ns != shdlc->nr) { | |
246 | llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr); | |
247 | goto exit; | |
248 | } | |
249 | ||
250 | if (shdlc->t1_active == false) { | |
251 | shdlc->t1_active = true; | |
252 | mod_timer(&shdlc->t1_timer, jiffies + | |
253 | msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w))); | |
254 | pr_debug("(re)Start T1(send ack)\n"); | |
255 | } | |
256 | ||
257 | if (skb->len) { | |
258 | shdlc->rcv_to_hci(shdlc->hdev, skb); | |
259 | skb = NULL; | |
260 | } | |
261 | ||
262 | shdlc->nr = (shdlc->nr + 1) % 8; | |
263 | ||
264 | if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { | |
265 | llc_shdlc_reset_t2(shdlc, y_nr); | |
266 | ||
267 | shdlc->dnr = y_nr; | |
268 | } | |
269 | ||
270 | exit: | |
271 | kfree_skb(skb); | |
272 | } | |
273 | ||
274 | static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr) | |
275 | { | |
276 | pr_debug("remote acked up to frame %d excluded\n", y_nr); | |
277 | ||
278 | if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { | |
279 | llc_shdlc_reset_t2(shdlc, y_nr); | |
280 | shdlc->dnr = y_nr; | |
281 | } | |
282 | } | |
283 | ||
284 | static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc) | |
285 | { | |
286 | struct sk_buff *skb; | |
287 | ||
288 | pr_debug("ns reset to %d\n", shdlc->dnr); | |
289 | ||
290 | while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) { | |
291 | skb_pull(skb, 1); /* remove control field */ | |
292 | skb_queue_head(&shdlc->send_q, skb); | |
293 | } | |
294 | shdlc->ns = shdlc->dnr; | |
295 | } | |
296 | ||
297 | static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr) | |
298 | { | |
299 | struct sk_buff *skb; | |
300 | ||
88bfe6ea | 301 | pr_debug("remote asks retransmission from frame %d\n", y_nr); |
4a61cd66 EL |
302 | |
303 | if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) { | |
304 | if (shdlc->t2_active) { | |
305 | del_timer_sync(&shdlc->t2_timer); | |
306 | shdlc->t2_active = false; | |
307 | pr_debug("Stopped T2(retransmit)\n"); | |
308 | } | |
309 | ||
310 | if (shdlc->dnr != y_nr) { | |
311 | while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) { | |
312 | skb = skb_dequeue(&shdlc->ack_pending_q); | |
313 | kfree_skb(skb); | |
314 | } | |
315 | } | |
316 | ||
317 | llc_shdlc_requeue_ack_pending(shdlc); | |
318 | } | |
319 | } | |
320 | ||
321 | /* See spec RR:10.8.3 REJ:10.8.4 */ | |
322 | static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc, | |
323 | enum sframe_type s_frame_type, int nr) | |
324 | { | |
325 | struct sk_buff *skb; | |
326 | ||
327 | if (shdlc->state != SHDLC_CONNECTED) | |
328 | return; | |
329 | ||
330 | switch (s_frame_type) { | |
331 | case S_FRAME_RR: | |
332 | llc_shdlc_rcv_ack(shdlc, nr); | |
333 | if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */ | |
334 | shdlc->rnr = false; | |
335 | if (shdlc->send_q.qlen == 0) { | |
336 | skb = llc_shdlc_alloc_skb(shdlc, 0); | |
337 | if (skb) | |
338 | skb_queue_tail(&shdlc->send_q, skb); | |
339 | } | |
340 | } | |
341 | break; | |
342 | case S_FRAME_REJ: | |
343 | llc_shdlc_rcv_rej(shdlc, nr); | |
344 | break; | |
345 | case S_FRAME_RNR: | |
346 | llc_shdlc_rcv_ack(shdlc, nr); | |
347 | shdlc->rnr = true; | |
348 | break; | |
349 | default: | |
350 | break; | |
351 | } | |
352 | } | |
353 | ||
354 | static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r) | |
355 | { | |
356 | pr_debug("result=%d\n", r); | |
357 | ||
358 | del_timer_sync(&shdlc->connect_timer); | |
359 | ||
360 | if (r == 0) { | |
361 | shdlc->ns = 0; | |
362 | shdlc->nr = 0; | |
363 | shdlc->dnr = 0; | |
364 | ||
12bfd1e8 | 365 | shdlc->state = SHDLC_HALF_CONNECTED; |
4a61cd66 EL |
366 | } else { |
367 | shdlc->state = SHDLC_DISCONNECTED; | |
368 | } | |
369 | ||
370 | shdlc->connect_result = r; | |
371 | ||
372 | wake_up(shdlc->connect_wq); | |
373 | } | |
374 | ||
375 | static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc) | |
376 | { | |
377 | struct sk_buff *skb; | |
378 | ||
379 | pr_debug("\n"); | |
380 | ||
381 | skb = llc_shdlc_alloc_skb(shdlc, 2); | |
382 | if (skb == NULL) | |
383 | return -ENOMEM; | |
384 | ||
385 | *skb_put(skb, 1) = SHDLC_MAX_WINDOW; | |
386 | *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0; | |
387 | ||
388 | return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET); | |
389 | } | |
390 | ||
391 | static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc) | |
392 | { | |
393 | struct sk_buff *skb; | |
394 | ||
395 | pr_debug("\n"); | |
396 | ||
397 | skb = llc_shdlc_alloc_skb(shdlc, 0); | |
398 | if (skb == NULL) | |
399 | return -ENOMEM; | |
400 | ||
401 | return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA); | |
402 | } | |
403 | ||
404 | static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc, | |
405 | struct sk_buff *skb, | |
406 | enum uframe_modifier u_frame_modifier) | |
407 | { | |
408 | u8 w = SHDLC_MAX_WINDOW; | |
409 | bool srej_support = SHDLC_SREJ_SUPPORT; | |
410 | int r; | |
411 | ||
412 | pr_debug("u_frame_modifier=%d\n", u_frame_modifier); | |
413 | ||
414 | switch (u_frame_modifier) { | |
415 | case U_FRAME_RSET: | |
12bfd1e8 | 416 | switch (shdlc->state) { |
4c0ba9ac | 417 | case SHDLC_NEGOTIATING: |
12bfd1e8 WR |
418 | case SHDLC_CONNECTING: |
419 | /* | |
420 | * We sent RSET, but chip wants to negociate or we | |
421 | * got RSET before we managed to send out our. | |
422 | */ | |
4a61cd66 EL |
423 | if (skb->len > 0) |
424 | w = skb->data[0]; | |
425 | ||
426 | if (skb->len > 1) | |
427 | srej_support = skb->data[1] & 0x01 ? true : | |
428 | false; | |
429 | ||
430 | if ((w <= SHDLC_MAX_WINDOW) && | |
431 | (SHDLC_SREJ_SUPPORT || (srej_support == false))) { | |
432 | shdlc->w = w; | |
433 | shdlc->srej_support = srej_support; | |
434 | r = llc_shdlc_connect_send_ua(shdlc); | |
435 | llc_shdlc_connect_complete(shdlc, r); | |
436 | } | |
12bfd1e8 WR |
437 | break; |
438 | case SHDLC_HALF_CONNECTED: | |
439 | /* | |
440 | * Chip resent RSET due to its timeout - Ignote it | |
441 | * as we already sent UA. | |
442 | */ | |
443 | break; | |
444 | case SHDLC_CONNECTED: | |
4a61cd66 EL |
445 | /* |
446 | * Chip wants to reset link. This is unexpected and | |
447 | * unsupported. | |
448 | */ | |
449 | shdlc->hard_fault = -ECONNRESET; | |
12bfd1e8 WR |
450 | break; |
451 | default: | |
452 | break; | |
4a61cd66 EL |
453 | } |
454 | break; | |
455 | case U_FRAME_UA: | |
456 | if ((shdlc->state == SHDLC_CONNECTING && | |
457 | shdlc->connect_tries > 0) || | |
4c0ba9ac | 458 | (shdlc->state == SHDLC_NEGOTIATING)) { |
4a61cd66 | 459 | llc_shdlc_connect_complete(shdlc, 0); |
12bfd1e8 WR |
460 | shdlc->state = SHDLC_CONNECTED; |
461 | } | |
4a61cd66 EL |
462 | break; |
463 | default: | |
464 | break; | |
465 | } | |
466 | ||
467 | kfree_skb(skb); | |
468 | } | |
469 | ||
470 | static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc) | |
471 | { | |
472 | struct sk_buff *skb; | |
473 | u8 control; | |
474 | int nr; | |
475 | int ns; | |
476 | enum sframe_type s_frame_type; | |
477 | enum uframe_modifier u_frame_modifier; | |
478 | ||
479 | if (shdlc->rcv_q.qlen) | |
480 | pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen); | |
481 | ||
482 | while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) { | |
483 | control = skb->data[0]; | |
484 | skb_pull(skb, 1); | |
485 | switch (control & SHDLC_CONTROL_HEAD_MASK) { | |
486 | case SHDLC_CONTROL_HEAD_I: | |
487 | case SHDLC_CONTROL_HEAD_I2: | |
12bfd1e8 WR |
488 | if (shdlc->state == SHDLC_HALF_CONNECTED) |
489 | shdlc->state = SHDLC_CONNECTED; | |
490 | ||
4a61cd66 EL |
491 | ns = (control & SHDLC_CONTROL_NS_MASK) >> 3; |
492 | nr = control & SHDLC_CONTROL_NR_MASK; | |
493 | llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr); | |
494 | break; | |
495 | case SHDLC_CONTROL_HEAD_S: | |
12bfd1e8 WR |
496 | if (shdlc->state == SHDLC_HALF_CONNECTED) |
497 | shdlc->state = SHDLC_CONNECTED; | |
498 | ||
4a61cd66 EL |
499 | s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3; |
500 | nr = control & SHDLC_CONTROL_NR_MASK; | |
501 | llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr); | |
502 | kfree_skb(skb); | |
503 | break; | |
504 | case SHDLC_CONTROL_HEAD_U: | |
505 | u_frame_modifier = control & SHDLC_CONTROL_M_MASK; | |
506 | llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier); | |
507 | break; | |
508 | default: | |
509 | pr_err("UNKNOWN Control=%d\n", control); | |
510 | kfree_skb(skb); | |
511 | break; | |
512 | } | |
513 | } | |
514 | } | |
515 | ||
516 | static int llc_shdlc_w_used(int ns, int dnr) | |
517 | { | |
518 | int unack_count; | |
519 | ||
520 | if (dnr <= ns) | |
521 | unack_count = ns - dnr; | |
522 | else | |
523 | unack_count = 8 - dnr + ns; | |
524 | ||
525 | return unack_count; | |
526 | } | |
527 | ||
528 | /* Send frames according to algorithm at spec:10.8.1 */ | |
529 | static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc) | |
530 | { | |
531 | struct sk_buff *skb; | |
532 | int r; | |
533 | unsigned long time_sent; | |
534 | ||
535 | if (shdlc->send_q.qlen) | |
536 | pr_debug | |
537 | ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n", | |
538 | shdlc->send_q.qlen, shdlc->ns, shdlc->dnr, | |
539 | shdlc->rnr == false ? "false" : "true", | |
540 | shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr), | |
541 | shdlc->ack_pending_q.qlen); | |
542 | ||
543 | while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w && | |
544 | (shdlc->rnr == false)) { | |
545 | ||
546 | if (shdlc->t1_active) { | |
547 | del_timer_sync(&shdlc->t1_timer); | |
548 | shdlc->t1_active = false; | |
549 | pr_debug("Stopped T1(send ack)\n"); | |
550 | } | |
551 | ||
552 | skb = skb_dequeue(&shdlc->send_q); | |
553 | ||
554 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) | | |
555 | shdlc->nr; | |
556 | ||
557 | pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns, | |
558 | shdlc->nr); | |
412fda53 | 559 | SHDLC_DUMP_SKB("shdlc frame written", skb); |
4a61cd66 EL |
560 | |
561 | r = shdlc->xmit_to_drv(shdlc->hdev, skb); | |
562 | if (r < 0) { | |
563 | shdlc->hard_fault = r; | |
564 | break; | |
565 | } | |
566 | ||
567 | shdlc->ns = (shdlc->ns + 1) % 8; | |
568 | ||
569 | time_sent = jiffies; | |
570 | *(unsigned long *)skb->cb = time_sent; | |
571 | ||
572 | skb_queue_tail(&shdlc->ack_pending_q, skb); | |
573 | ||
574 | if (shdlc->t2_active == false) { | |
575 | shdlc->t2_active = true; | |
576 | mod_timer(&shdlc->t2_timer, time_sent + | |
577 | msecs_to_jiffies(SHDLC_T2_VALUE_MS)); | |
578 | pr_debug("Started T2 (retransmit)\n"); | |
579 | } | |
580 | } | |
581 | } | |
582 | ||
583 | static void llc_shdlc_connect_timeout(unsigned long data) | |
584 | { | |
585 | struct llc_shdlc *shdlc = (struct llc_shdlc *)data; | |
586 | ||
587 | pr_debug("\n"); | |
588 | ||
916082b0 | 589 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
590 | } |
591 | ||
592 | static void llc_shdlc_t1_timeout(unsigned long data) | |
593 | { | |
594 | struct llc_shdlc *shdlc = (struct llc_shdlc *)data; | |
595 | ||
596 | pr_debug("SoftIRQ: need to send ack\n"); | |
597 | ||
916082b0 | 598 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
599 | } |
600 | ||
601 | static void llc_shdlc_t2_timeout(unsigned long data) | |
602 | { | |
603 | struct llc_shdlc *shdlc = (struct llc_shdlc *)data; | |
604 | ||
605 | pr_debug("SoftIRQ: need to retransmit\n"); | |
606 | ||
916082b0 | 607 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
608 | } |
609 | ||
610 | static void llc_shdlc_sm_work(struct work_struct *work) | |
611 | { | |
612 | struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work); | |
613 | int r; | |
614 | ||
615 | pr_debug("\n"); | |
616 | ||
617 | mutex_lock(&shdlc->state_mutex); | |
618 | ||
619 | switch (shdlc->state) { | |
620 | case SHDLC_DISCONNECTED: | |
621 | skb_queue_purge(&shdlc->rcv_q); | |
622 | skb_queue_purge(&shdlc->send_q); | |
623 | skb_queue_purge(&shdlc->ack_pending_q); | |
624 | break; | |
625 | case SHDLC_CONNECTING: | |
626 | if (shdlc->hard_fault) { | |
627 | llc_shdlc_connect_complete(shdlc, shdlc->hard_fault); | |
628 | break; | |
629 | } | |
630 | ||
631 | if (shdlc->connect_tries++ < 5) | |
632 | r = llc_shdlc_connect_initiate(shdlc); | |
633 | else | |
634 | r = -ETIME; | |
0f450772 | 635 | if (r < 0) { |
4a61cd66 | 636 | llc_shdlc_connect_complete(shdlc, r); |
0f450772 | 637 | } else { |
4a61cd66 EL |
638 | mod_timer(&shdlc->connect_timer, jiffies + |
639 | msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS)); | |
640 | ||
4c0ba9ac | 641 | shdlc->state = SHDLC_NEGOTIATING; |
4a61cd66 EL |
642 | } |
643 | break; | |
4c0ba9ac | 644 | case SHDLC_NEGOTIATING: |
4a61cd66 EL |
645 | if (timer_pending(&shdlc->connect_timer) == 0) { |
646 | shdlc->state = SHDLC_CONNECTING; | |
916082b0 | 647 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
648 | } |
649 | ||
650 | llc_shdlc_handle_rcv_queue(shdlc); | |
651 | ||
652 | if (shdlc->hard_fault) { | |
653 | llc_shdlc_connect_complete(shdlc, shdlc->hard_fault); | |
654 | break; | |
655 | } | |
656 | break; | |
12bfd1e8 | 657 | case SHDLC_HALF_CONNECTED: |
4a61cd66 EL |
658 | case SHDLC_CONNECTED: |
659 | llc_shdlc_handle_rcv_queue(shdlc); | |
660 | llc_shdlc_handle_send_queue(shdlc); | |
661 | ||
662 | if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) { | |
663 | pr_debug | |
664 | ("Handle T1(send ack) elapsed (T1 now inactive)\n"); | |
665 | ||
666 | shdlc->t1_active = false; | |
667 | r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR, | |
668 | shdlc->nr); | |
669 | if (r < 0) | |
670 | shdlc->hard_fault = r; | |
671 | } | |
672 | ||
673 | if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) { | |
674 | pr_debug | |
675 | ("Handle T2(retransmit) elapsed (T2 inactive)\n"); | |
676 | ||
677 | shdlc->t2_active = false; | |
678 | ||
679 | llc_shdlc_requeue_ack_pending(shdlc); | |
680 | llc_shdlc_handle_send_queue(shdlc); | |
681 | } | |
682 | ||
0f450772 | 683 | if (shdlc->hard_fault) |
4a61cd66 | 684 | shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault); |
4a61cd66 EL |
685 | break; |
686 | default: | |
687 | break; | |
688 | } | |
689 | mutex_unlock(&shdlc->state_mutex); | |
690 | } | |
691 | ||
692 | /* | |
693 | * Called from syscall context to establish shdlc link. Sleeps until | |
694 | * link is ready or failure. | |
695 | */ | |
696 | static int llc_shdlc_connect(struct llc_shdlc *shdlc) | |
697 | { | |
698 | DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq); | |
699 | ||
700 | pr_debug("\n"); | |
701 | ||
702 | mutex_lock(&shdlc->state_mutex); | |
703 | ||
704 | shdlc->state = SHDLC_CONNECTING; | |
705 | shdlc->connect_wq = &connect_wq; | |
706 | shdlc->connect_tries = 0; | |
707 | shdlc->connect_result = 1; | |
708 | ||
709 | mutex_unlock(&shdlc->state_mutex); | |
710 | ||
916082b0 | 711 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
712 | |
713 | wait_event(connect_wq, shdlc->connect_result != 1); | |
714 | ||
715 | return shdlc->connect_result; | |
716 | } | |
717 | ||
718 | static void llc_shdlc_disconnect(struct llc_shdlc *shdlc) | |
719 | { | |
720 | pr_debug("\n"); | |
721 | ||
722 | mutex_lock(&shdlc->state_mutex); | |
723 | ||
724 | shdlc->state = SHDLC_DISCONNECTED; | |
725 | ||
726 | mutex_unlock(&shdlc->state_mutex); | |
727 | ||
916082b0 | 728 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
729 | } |
730 | ||
731 | /* | |
732 | * Receive an incoming shdlc frame. Frame has already been crc-validated. | |
733 | * skb contains only LLC header and payload. | |
734 | * If skb == NULL, it is a notification that the link below is dead. | |
735 | */ | |
736 | static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb) | |
737 | { | |
738 | if (skb == NULL) { | |
739 | pr_err("NULL Frame -> link is dead\n"); | |
740 | shdlc->hard_fault = -EREMOTEIO; | |
741 | } else { | |
742 | SHDLC_DUMP_SKB("incoming frame", skb); | |
743 | skb_queue_tail(&shdlc->rcv_q, skb); | |
744 | } | |
745 | ||
916082b0 | 746 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
747 | } |
748 | ||
749 | static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv, | |
750 | rcv_to_hci_t rcv_to_hci, int tx_headroom, | |
751 | int tx_tailroom, int *rx_headroom, int *rx_tailroom, | |
752 | llc_failure_t llc_failure) | |
753 | { | |
754 | struct llc_shdlc *shdlc; | |
755 | ||
756 | *rx_headroom = SHDLC_LLC_HEAD_ROOM; | |
757 | *rx_tailroom = 0; | |
758 | ||
759 | shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL); | |
760 | if (shdlc == NULL) | |
761 | return NULL; | |
762 | ||
763 | mutex_init(&shdlc->state_mutex); | |
764 | shdlc->state = SHDLC_DISCONNECTED; | |
765 | ||
766 | init_timer(&shdlc->connect_timer); | |
767 | shdlc->connect_timer.data = (unsigned long)shdlc; | |
768 | shdlc->connect_timer.function = llc_shdlc_connect_timeout; | |
769 | ||
770 | init_timer(&shdlc->t1_timer); | |
771 | shdlc->t1_timer.data = (unsigned long)shdlc; | |
772 | shdlc->t1_timer.function = llc_shdlc_t1_timeout; | |
773 | ||
774 | init_timer(&shdlc->t2_timer); | |
775 | shdlc->t2_timer.data = (unsigned long)shdlc; | |
776 | shdlc->t2_timer.function = llc_shdlc_t2_timeout; | |
777 | ||
778 | shdlc->w = SHDLC_MAX_WINDOW; | |
779 | shdlc->srej_support = SHDLC_SREJ_SUPPORT; | |
780 | ||
781 | skb_queue_head_init(&shdlc->rcv_q); | |
782 | skb_queue_head_init(&shdlc->send_q); | |
783 | skb_queue_head_init(&shdlc->ack_pending_q); | |
784 | ||
785 | INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work); | |
786 | ||
787 | shdlc->hdev = hdev; | |
788 | shdlc->xmit_to_drv = xmit_to_drv; | |
789 | shdlc->rcv_to_hci = rcv_to_hci; | |
790 | shdlc->tx_headroom = tx_headroom; | |
791 | shdlc->tx_tailroom = tx_tailroom; | |
792 | shdlc->llc_failure = llc_failure; | |
793 | ||
794 | return shdlc; | |
795 | } | |
796 | ||
797 | static void llc_shdlc_deinit(struct nfc_llc *llc) | |
798 | { | |
799 | struct llc_shdlc *shdlc = nfc_llc_get_data(llc); | |
800 | ||
801 | skb_queue_purge(&shdlc->rcv_q); | |
802 | skb_queue_purge(&shdlc->send_q); | |
803 | skb_queue_purge(&shdlc->ack_pending_q); | |
804 | ||
805 | kfree(shdlc); | |
806 | } | |
807 | ||
808 | static int llc_shdlc_start(struct nfc_llc *llc) | |
809 | { | |
810 | struct llc_shdlc *shdlc = nfc_llc_get_data(llc); | |
811 | ||
812 | return llc_shdlc_connect(shdlc); | |
813 | } | |
814 | ||
815 | static int llc_shdlc_stop(struct nfc_llc *llc) | |
816 | { | |
817 | struct llc_shdlc *shdlc = nfc_llc_get_data(llc); | |
818 | ||
819 | llc_shdlc_disconnect(shdlc); | |
820 | ||
821 | return 0; | |
822 | } | |
823 | ||
824 | static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb) | |
825 | { | |
826 | struct llc_shdlc *shdlc = nfc_llc_get_data(llc); | |
827 | ||
828 | llc_shdlc_recv_frame(shdlc, skb); | |
829 | } | |
830 | ||
831 | static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb) | |
832 | { | |
833 | struct llc_shdlc *shdlc = nfc_llc_get_data(llc); | |
834 | ||
835 | skb_queue_tail(&shdlc->send_q, skb); | |
836 | ||
916082b0 | 837 | schedule_work(&shdlc->sm_work); |
4a61cd66 EL |
838 | |
839 | return 0; | |
840 | } | |
841 | ||
842 | static struct nfc_llc_ops llc_shdlc_ops = { | |
843 | .init = llc_shdlc_init, | |
844 | .deinit = llc_shdlc_deinit, | |
845 | .start = llc_shdlc_start, | |
846 | .stop = llc_shdlc_stop, | |
847 | .rcv_from_drv = llc_shdlc_rcv_from_drv, | |
848 | .xmit_from_hci = llc_shdlc_xmit_from_hci, | |
849 | }; | |
850 | ||
a7d0281b | 851 | int nfc_llc_shdlc_register(void) |
4a61cd66 EL |
852 | { |
853 | return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops); | |
854 | } |