Commit | Line | Data |
---|---|---|
eb738fe5 EL |
1 | /* |
2 | * Copyright (C) 2012 Intel Corporation. All rights reserved. | |
3 | * | |
4 | * This program is free software; you can redistribute it and/or modify | |
5 | * it under the terms of the GNU General Public License as published by | |
6 | * the Free Software Foundation; either version 2 of the License, or | |
7 | * (at your option) any later version. | |
8 | * | |
9 | * This program is distributed in the hope that it will be useful, | |
10 | * but WITHOUT ANY WARRANTY; without even the implied warranty of | |
11 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the | |
12 | * GNU General Public License for more details. | |
13 | * | |
14 | * You should have received a copy of the GNU General Public License | |
15 | * along with this program; if not, write to the | |
16 | * Free Software Foundation, Inc., | |
17 | * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. | |
18 | */ | |
19 | ||
20 | #define pr_fmt(fmt) "shdlc: %s: " fmt, __func__ | |
21 | ||
22 | #include <linux/sched.h> | |
23 | #include <linux/export.h> | |
24 | #include <linux/wait.h> | |
25 | #include <linux/crc-ccitt.h> | |
26 | #include <linux/slab.h> | |
27 | #include <linux/skbuff.h> | |
28 | ||
29 | #include <net/nfc/hci.h> | |
30 | #include <net/nfc/shdlc.h> | |
31 | ||
32 | #define SHDLC_LLC_HEAD_ROOM 2 | |
33 | #define SHDLC_LLC_TAIL_ROOM 2 | |
34 | ||
35 | #define SHDLC_MAX_WINDOW 4 | |
36 | #define SHDLC_SREJ_SUPPORT false | |
37 | ||
38 | #define SHDLC_CONTROL_HEAD_MASK 0xe0 | |
39 | #define SHDLC_CONTROL_HEAD_I 0x80 | |
40 | #define SHDLC_CONTROL_HEAD_I2 0xa0 | |
41 | #define SHDLC_CONTROL_HEAD_S 0xc0 | |
42 | #define SHDLC_CONTROL_HEAD_U 0xe0 | |
43 | ||
44 | #define SHDLC_CONTROL_NS_MASK 0x38 | |
45 | #define SHDLC_CONTROL_NR_MASK 0x07 | |
46 | #define SHDLC_CONTROL_TYPE_MASK 0x18 | |
47 | ||
48 | #define SHDLC_CONTROL_M_MASK 0x1f | |
49 | ||
50 | enum sframe_type { | |
51 | S_FRAME_RR = 0x00, | |
52 | S_FRAME_REJ = 0x01, | |
53 | S_FRAME_RNR = 0x02, | |
54 | S_FRAME_SREJ = 0x03 | |
55 | }; | |
56 | ||
57 | enum uframe_modifier { | |
58 | U_FRAME_UA = 0x06, | |
59 | U_FRAME_RSET = 0x19 | |
60 | }; | |
61 | ||
62 | #define SHDLC_CONNECT_VALUE_MS 5 | |
63 | #define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4) | |
64 | #define SHDLC_T2_VALUE_MS 300 | |
65 | ||
66 | #define SHDLC_DUMP_SKB(info, skb) \ | |
67 | do { \ | |
68 | pr_debug("%s:\n", info); \ | |
69 | print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \ | |
70 | 16, 1, skb->data, skb->len, 0); \ | |
71 | } while (0) | |
72 | ||
73 | /* checks x < y <= z modulo 8 */ | |
74 | static bool nfc_shdlc_x_lt_y_lteq_z(int x, int y, int z) | |
75 | { | |
76 | if (x < z) | |
77 | return ((x < y) && (y <= z)) ? true : false; | |
78 | else | |
79 | return ((y > x) || (y <= z)) ? true : false; | |
80 | } | |
81 | ||
82 | /* checks x <= y < z modulo 8 */ | |
83 | static bool nfc_shdlc_x_lteq_y_lt_z(int x, int y, int z) | |
84 | { | |
85 | if (x <= z) | |
86 | return ((x <= y) && (y < z)) ? true : false; | |
87 | else /* x > z -> z+8 > x */ | |
88 | return ((y >= x) || (y < z)) ? true : false; | |
89 | } | |
90 | ||
91 | static struct sk_buff *nfc_shdlc_alloc_skb(struct nfc_shdlc *shdlc, | |
92 | int payload_len) | |
93 | { | |
94 | struct sk_buff *skb; | |
95 | ||
96 | skb = alloc_skb(shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM + | |
97 | shdlc->client_tailroom + SHDLC_LLC_TAIL_ROOM + | |
98 | payload_len, GFP_KERNEL); | |
99 | if (skb) | |
100 | skb_reserve(skb, shdlc->client_headroom + SHDLC_LLC_HEAD_ROOM); | |
101 | ||
102 | return skb; | |
103 | } | |
104 | ||
105 | static void nfc_shdlc_add_len_crc(struct sk_buff *skb) | |
106 | { | |
107 | u16 crc; | |
108 | int len; | |
109 | ||
110 | len = skb->len + 2; | |
111 | *skb_push(skb, 1) = len; | |
112 | ||
113 | crc = crc_ccitt(0xffff, skb->data, skb->len); | |
114 | crc = ~crc; | |
115 | *skb_put(skb, 1) = crc & 0xff; | |
116 | *skb_put(skb, 1) = crc >> 8; | |
117 | } | |
118 | ||
119 | /* immediately sends an S frame. */ | |
120 | static int nfc_shdlc_send_s_frame(struct nfc_shdlc *shdlc, | |
121 | enum sframe_type sframe_type, int nr) | |
122 | { | |
123 | int r; | |
124 | struct sk_buff *skb; | |
125 | ||
126 | pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr); | |
127 | ||
128 | skb = nfc_shdlc_alloc_skb(shdlc, 0); | |
129 | if (skb == NULL) | |
130 | return -ENOMEM; | |
131 | ||
132 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr; | |
133 | ||
134 | nfc_shdlc_add_len_crc(skb); | |
135 | ||
136 | r = shdlc->ops->xmit(shdlc, skb); | |
137 | ||
138 | kfree_skb(skb); | |
139 | ||
140 | return r; | |
141 | } | |
142 | ||
143 | /* immediately sends an U frame. skb may contain optional payload */ | |
144 | static int nfc_shdlc_send_u_frame(struct nfc_shdlc *shdlc, | |
145 | struct sk_buff *skb, | |
146 | enum uframe_modifier uframe_modifier) | |
147 | { | |
148 | int r; | |
149 | ||
150 | pr_debug("uframe_modifier=%d\n", uframe_modifier); | |
151 | ||
152 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier; | |
153 | ||
154 | nfc_shdlc_add_len_crc(skb); | |
155 | ||
156 | r = shdlc->ops->xmit(shdlc, skb); | |
157 | ||
158 | kfree_skb(skb); | |
159 | ||
160 | return r; | |
161 | } | |
162 | ||
163 | /* | |
164 | * Free ack_pending frames until y_nr - 1, and reset t2 according to | |
165 | * the remaining oldest ack_pending frame sent time | |
166 | */ | |
167 | static void nfc_shdlc_reset_t2(struct nfc_shdlc *shdlc, int y_nr) | |
168 | { | |
169 | struct sk_buff *skb; | |
170 | int dnr = shdlc->dnr; /* MUST initially be < y_nr */ | |
171 | ||
172 | pr_debug("release ack pending up to frame %d excluded\n", y_nr); | |
173 | ||
174 | while (dnr != y_nr) { | |
175 | pr_debug("release ack pending frame %d\n", dnr); | |
176 | ||
177 | skb = skb_dequeue(&shdlc->ack_pending_q); | |
178 | kfree_skb(skb); | |
179 | ||
180 | dnr = (dnr + 1) % 8; | |
181 | } | |
182 | ||
183 | if (skb_queue_empty(&shdlc->ack_pending_q)) { | |
184 | if (shdlc->t2_active) { | |
185 | del_timer_sync(&shdlc->t2_timer); | |
186 | shdlc->t2_active = false; | |
187 | ||
188 | pr_debug | |
189 | ("All sent frames acked. Stopped T2(retransmit)\n"); | |
190 | } | |
191 | } else { | |
192 | skb = skb_peek(&shdlc->ack_pending_q); | |
193 | ||
194 | mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb + | |
195 | msecs_to_jiffies(SHDLC_T2_VALUE_MS)); | |
196 | shdlc->t2_active = true; | |
197 | ||
198 | pr_debug | |
199 | ("Start T2(retransmit) for remaining unacked sent frames\n"); | |
200 | } | |
201 | } | |
202 | ||
203 | /* | |
204 | * Receive validated frames from lower layer. skb contains HCI payload only. | |
205 | * Handle according to algorithm at spec:10.8.2 | |
206 | */ | |
207 | static void nfc_shdlc_rcv_i_frame(struct nfc_shdlc *shdlc, | |
208 | struct sk_buff *skb, int ns, int nr) | |
209 | { | |
210 | int x_ns = ns; | |
211 | int y_nr = nr; | |
212 | ||
213 | pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr); | |
214 | ||
215 | if (shdlc->state != SHDLC_CONNECTED) | |
216 | goto exit; | |
217 | ||
218 | if (x_ns != shdlc->nr) { | |
219 | nfc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr); | |
220 | goto exit; | |
221 | } | |
222 | ||
223 | if (shdlc->t1_active == false) { | |
224 | shdlc->t1_active = true; | |
225 | mod_timer(&shdlc->t1_timer, | |
226 | msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w))); | |
227 | pr_debug("(re)Start T1(send ack)\n"); | |
228 | } | |
229 | ||
230 | if (skb->len) { | |
231 | nfc_hci_recv_frame(shdlc->hdev, skb); | |
232 | skb = NULL; | |
233 | } | |
234 | ||
235 | shdlc->nr = (shdlc->nr + 1) % 8; | |
236 | ||
237 | if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { | |
238 | nfc_shdlc_reset_t2(shdlc, y_nr); | |
239 | ||
240 | shdlc->dnr = y_nr; | |
241 | } | |
242 | ||
243 | exit: | |
244 | if (skb) | |
245 | kfree_skb(skb); | |
246 | } | |
247 | ||
248 | static void nfc_shdlc_rcv_ack(struct nfc_shdlc *shdlc, int y_nr) | |
249 | { | |
250 | pr_debug("remote acked up to frame %d excluded\n", y_nr); | |
251 | ||
252 | if (nfc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) { | |
253 | nfc_shdlc_reset_t2(shdlc, y_nr); | |
254 | shdlc->dnr = y_nr; | |
255 | } | |
256 | } | |
257 | ||
258 | static void nfc_shdlc_requeue_ack_pending(struct nfc_shdlc *shdlc) | |
259 | { | |
260 | struct sk_buff *skb; | |
261 | ||
262 | pr_debug("ns reset to %d\n", shdlc->dnr); | |
263 | ||
264 | while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) { | |
265 | skb_pull(skb, 2); /* remove len+control */ | |
266 | skb_trim(skb, skb->len - 2); /* remove crc */ | |
267 | skb_queue_head(&shdlc->send_q, skb); | |
268 | } | |
269 | shdlc->ns = shdlc->dnr; | |
270 | } | |
271 | ||
272 | static void nfc_shdlc_rcv_rej(struct nfc_shdlc *shdlc, int y_nr) | |
273 | { | |
274 | struct sk_buff *skb; | |
275 | ||
276 | pr_debug("remote asks retransmition from frame %d\n", y_nr); | |
277 | ||
278 | if (nfc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) { | |
279 | if (shdlc->t2_active) { | |
280 | del_timer_sync(&shdlc->t2_timer); | |
281 | shdlc->t2_active = false; | |
282 | pr_debug("Stopped T2(retransmit)\n"); | |
283 | } | |
284 | ||
285 | if (shdlc->dnr != y_nr) { | |
286 | while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) { | |
287 | skb = skb_dequeue(&shdlc->ack_pending_q); | |
288 | kfree_skb(skb); | |
289 | } | |
290 | } | |
291 | ||
292 | nfc_shdlc_requeue_ack_pending(shdlc); | |
293 | } | |
294 | } | |
295 | ||
296 | /* See spec RR:10.8.3 REJ:10.8.4 */ | |
297 | static void nfc_shdlc_rcv_s_frame(struct nfc_shdlc *shdlc, | |
298 | enum sframe_type s_frame_type, int nr) | |
299 | { | |
300 | struct sk_buff *skb; | |
301 | ||
302 | if (shdlc->state != SHDLC_CONNECTED) | |
303 | return; | |
304 | ||
305 | switch (s_frame_type) { | |
306 | case S_FRAME_RR: | |
307 | nfc_shdlc_rcv_ack(shdlc, nr); | |
308 | if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */ | |
309 | shdlc->rnr = false; | |
310 | if (shdlc->send_q.qlen == 0) { | |
311 | skb = nfc_shdlc_alloc_skb(shdlc, 0); | |
312 | if (skb) | |
313 | skb_queue_tail(&shdlc->send_q, skb); | |
314 | } | |
315 | } | |
316 | break; | |
317 | case S_FRAME_REJ: | |
318 | nfc_shdlc_rcv_rej(shdlc, nr); | |
319 | break; | |
320 | case S_FRAME_RNR: | |
321 | nfc_shdlc_rcv_ack(shdlc, nr); | |
322 | shdlc->rnr = true; | |
323 | break; | |
324 | default: | |
325 | break; | |
326 | } | |
327 | } | |
328 | ||
329 | static void nfc_shdlc_connect_complete(struct nfc_shdlc *shdlc, int r) | |
330 | { | |
331 | pr_debug("result=%d\n", r); | |
332 | ||
333 | del_timer_sync(&shdlc->connect_timer); | |
334 | ||
335 | if (r == 0) { | |
336 | shdlc->ns = 0; | |
337 | shdlc->nr = 0; | |
338 | shdlc->dnr = 0; | |
339 | ||
340 | shdlc->state = SHDLC_CONNECTED; | |
341 | } else { | |
342 | shdlc->state = SHDLC_DISCONNECTED; | |
343 | ||
344 | /* | |
345 | * TODO: Could it be possible that there are pending | |
346 | * executing commands that are waiting for connect to complete | |
347 | * before they can be carried? As connect is a blocking | |
348 | * operation, it would require that the userspace process can | |
349 | * send commands on the same device from a second thread before | |
350 | * the device is up. I don't think that is possible, is it? | |
351 | */ | |
352 | } | |
353 | ||
354 | shdlc->connect_result = r; | |
355 | ||
356 | wake_up(shdlc->connect_wq); | |
357 | } | |
358 | ||
359 | static int nfc_shdlc_connect_initiate(struct nfc_shdlc *shdlc) | |
360 | { | |
361 | struct sk_buff *skb; | |
362 | ||
363 | pr_debug("\n"); | |
364 | ||
365 | skb = nfc_shdlc_alloc_skb(shdlc, 2); | |
366 | if (skb == NULL) | |
367 | return -ENOMEM; | |
368 | ||
369 | *skb_put(skb, 1) = SHDLC_MAX_WINDOW; | |
370 | *skb_put(skb, 1) = SHDLC_SREJ_SUPPORT ? 1 : 0; | |
371 | ||
372 | return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET); | |
373 | } | |
374 | ||
375 | static int nfc_shdlc_connect_send_ua(struct nfc_shdlc *shdlc) | |
376 | { | |
377 | struct sk_buff *skb; | |
378 | ||
379 | pr_debug("\n"); | |
380 | ||
381 | skb = nfc_shdlc_alloc_skb(shdlc, 0); | |
382 | if (skb == NULL) | |
383 | return -ENOMEM; | |
384 | ||
385 | return nfc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA); | |
386 | } | |
387 | ||
388 | static void nfc_shdlc_rcv_u_frame(struct nfc_shdlc *shdlc, | |
389 | struct sk_buff *skb, | |
390 | enum uframe_modifier u_frame_modifier) | |
391 | { | |
392 | u8 w = SHDLC_MAX_WINDOW; | |
393 | bool srej_support = SHDLC_SREJ_SUPPORT; | |
394 | int r; | |
395 | ||
396 | pr_debug("u_frame_modifier=%d\n", u_frame_modifier); | |
397 | ||
398 | switch (u_frame_modifier) { | |
399 | case U_FRAME_RSET: | |
400 | if (shdlc->state == SHDLC_NEGOCIATING) { | |
401 | /* we sent RSET, but chip wants to negociate */ | |
402 | if (skb->len > 0) | |
403 | w = skb->data[0]; | |
404 | ||
405 | if (skb->len > 1) | |
406 | srej_support = skb->data[1] & 0x01 ? true : | |
407 | false; | |
408 | ||
409 | if ((w <= SHDLC_MAX_WINDOW) && | |
410 | (SHDLC_SREJ_SUPPORT || (srej_support == false))) { | |
411 | shdlc->w = w; | |
412 | shdlc->srej_support = srej_support; | |
413 | r = nfc_shdlc_connect_send_ua(shdlc); | |
414 | nfc_shdlc_connect_complete(shdlc, r); | |
415 | } | |
416 | } else if (shdlc->state > SHDLC_NEGOCIATING) { | |
417 | /* | |
418 | * TODO: Chip wants to reset link | |
419 | * send ua, empty skb lists, reset counters | |
420 | * propagate info to HCI layer | |
421 | */ | |
422 | } | |
423 | break; | |
424 | case U_FRAME_UA: | |
425 | if ((shdlc->state == SHDLC_CONNECTING && | |
426 | shdlc->connect_tries > 0) || | |
427 | (shdlc->state == SHDLC_NEGOCIATING)) | |
428 | nfc_shdlc_connect_complete(shdlc, 0); | |
429 | break; | |
430 | default: | |
431 | break; | |
432 | } | |
433 | ||
434 | kfree_skb(skb); | |
435 | } | |
436 | ||
437 | static void nfc_shdlc_handle_rcv_queue(struct nfc_shdlc *shdlc) | |
438 | { | |
439 | struct sk_buff *skb; | |
440 | u8 control; | |
441 | int nr; | |
442 | int ns; | |
443 | enum sframe_type s_frame_type; | |
444 | enum uframe_modifier u_frame_modifier; | |
445 | ||
446 | if (shdlc->rcv_q.qlen) | |
447 | pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen); | |
448 | ||
449 | while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) { | |
450 | control = skb->data[0]; | |
451 | skb_pull(skb, 1); | |
452 | switch (control & SHDLC_CONTROL_HEAD_MASK) { | |
453 | case SHDLC_CONTROL_HEAD_I: | |
454 | case SHDLC_CONTROL_HEAD_I2: | |
455 | ns = (control & SHDLC_CONTROL_NS_MASK) >> 3; | |
456 | nr = control & SHDLC_CONTROL_NR_MASK; | |
457 | nfc_shdlc_rcv_i_frame(shdlc, skb, ns, nr); | |
458 | break; | |
459 | case SHDLC_CONTROL_HEAD_S: | |
460 | s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3; | |
461 | nr = control & SHDLC_CONTROL_NR_MASK; | |
462 | nfc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr); | |
463 | kfree_skb(skb); | |
464 | break; | |
465 | case SHDLC_CONTROL_HEAD_U: | |
466 | u_frame_modifier = control & SHDLC_CONTROL_M_MASK; | |
467 | nfc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier); | |
468 | break; | |
469 | default: | |
470 | pr_err("UNKNOWN Control=%d\n", control); | |
471 | kfree_skb(skb); | |
472 | break; | |
473 | } | |
474 | } | |
475 | } | |
476 | ||
477 | static int nfc_shdlc_w_used(int ns, int dnr) | |
478 | { | |
479 | int unack_count; | |
480 | ||
481 | if (dnr <= ns) | |
482 | unack_count = ns - dnr; | |
483 | else | |
484 | unack_count = 8 - dnr + ns; | |
485 | ||
486 | return unack_count; | |
487 | } | |
488 | ||
489 | /* Send frames according to algorithm at spec:10.8.1 */ | |
490 | static void nfc_shdlc_handle_send_queue(struct nfc_shdlc *shdlc) | |
491 | { | |
492 | struct sk_buff *skb; | |
493 | int r; | |
494 | unsigned long time_sent; | |
495 | ||
496 | if (shdlc->send_q.qlen) | |
497 | pr_debug | |
498 | ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n", | |
499 | shdlc->send_q.qlen, shdlc->ns, shdlc->dnr, | |
500 | shdlc->rnr == false ? "false" : "true", | |
501 | shdlc->w - nfc_shdlc_w_used(shdlc->ns, shdlc->dnr), | |
502 | shdlc->ack_pending_q.qlen); | |
503 | ||
504 | while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w && | |
505 | (shdlc->rnr == false)) { | |
506 | ||
507 | if (shdlc->t1_active) { | |
508 | del_timer_sync(&shdlc->t1_timer); | |
509 | shdlc->t1_active = false; | |
510 | pr_debug("Stopped T1(send ack)\n"); | |
511 | } | |
512 | ||
513 | skb = skb_dequeue(&shdlc->send_q); | |
514 | ||
515 | *skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) | | |
516 | shdlc->nr; | |
517 | ||
518 | pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns, | |
519 | shdlc->nr); | |
520 | /* SHDLC_DUMP_SKB("shdlc frame written", skb); */ | |
521 | ||
522 | nfc_shdlc_add_len_crc(skb); | |
523 | ||
524 | r = shdlc->ops->xmit(shdlc, skb); | |
525 | if (r < 0) { | |
526 | /* | |
527 | * TODO: Cannot send, shdlc machine is dead, we | |
528 | * must propagate the information up to HCI. | |
529 | */ | |
530 | shdlc->hard_fault = r; | |
531 | break; | |
532 | } | |
533 | ||
534 | shdlc->ns = (shdlc->ns + 1) % 8; | |
535 | ||
536 | time_sent = jiffies; | |
537 | *(unsigned long *)skb->cb = time_sent; | |
538 | ||
539 | skb_queue_tail(&shdlc->ack_pending_q, skb); | |
540 | ||
541 | if (shdlc->t2_active == false) { | |
542 | shdlc->t2_active = true; | |
543 | mod_timer(&shdlc->t2_timer, time_sent + | |
544 | msecs_to_jiffies(SHDLC_T2_VALUE_MS)); | |
545 | pr_debug("Started T2 (retransmit)\n"); | |
546 | } | |
547 | } | |
548 | } | |
549 | ||
550 | static void nfc_shdlc_connect_timeout(unsigned long data) | |
551 | { | |
552 | struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data; | |
553 | ||
554 | pr_debug("\n"); | |
555 | ||
556 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
557 | } | |
558 | ||
559 | static void nfc_shdlc_t1_timeout(unsigned long data) | |
560 | { | |
561 | struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data; | |
562 | ||
563 | pr_debug("SoftIRQ: need to send ack\n"); | |
564 | ||
565 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
566 | } | |
567 | ||
568 | static void nfc_shdlc_t2_timeout(unsigned long data) | |
569 | { | |
570 | struct nfc_shdlc *shdlc = (struct nfc_shdlc *)data; | |
571 | ||
572 | pr_debug("SoftIRQ: need to retransmit\n"); | |
573 | ||
574 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
575 | } | |
576 | ||
577 | static void nfc_shdlc_sm_work(struct work_struct *work) | |
578 | { | |
579 | struct nfc_shdlc *shdlc = container_of(work, struct nfc_shdlc, sm_work); | |
580 | int r; | |
581 | ||
582 | pr_debug("\n"); | |
583 | ||
584 | mutex_lock(&shdlc->state_mutex); | |
585 | ||
586 | switch (shdlc->state) { | |
587 | case SHDLC_DISCONNECTED: | |
588 | skb_queue_purge(&shdlc->rcv_q); | |
589 | skb_queue_purge(&shdlc->send_q); | |
590 | skb_queue_purge(&shdlc->ack_pending_q); | |
591 | break; | |
592 | case SHDLC_CONNECTING: | |
593 | if (shdlc->connect_tries++ < 5) | |
594 | r = nfc_shdlc_connect_initiate(shdlc); | |
595 | else | |
596 | r = -ETIME; | |
597 | if (r < 0) | |
598 | nfc_shdlc_connect_complete(shdlc, r); | |
599 | else { | |
600 | mod_timer(&shdlc->connect_timer, jiffies + | |
601 | msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS)); | |
602 | ||
603 | shdlc->state = SHDLC_NEGOCIATING; | |
604 | } | |
605 | break; | |
606 | case SHDLC_NEGOCIATING: | |
607 | if (timer_pending(&shdlc->connect_timer) == 0) { | |
608 | shdlc->state = SHDLC_CONNECTING; | |
609 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
610 | } | |
611 | ||
612 | nfc_shdlc_handle_rcv_queue(shdlc); | |
613 | break; | |
614 | case SHDLC_CONNECTED: | |
615 | nfc_shdlc_handle_rcv_queue(shdlc); | |
616 | nfc_shdlc_handle_send_queue(shdlc); | |
617 | ||
618 | if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) { | |
619 | pr_debug | |
620 | ("Handle T1(send ack) elapsed (T1 now inactive)\n"); | |
621 | ||
622 | shdlc->t1_active = false; | |
623 | r = nfc_shdlc_send_s_frame(shdlc, S_FRAME_RR, | |
624 | shdlc->nr); | |
625 | if (r < 0) | |
626 | shdlc->hard_fault = r; | |
627 | } | |
628 | ||
629 | if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) { | |
630 | pr_debug | |
631 | ("Handle T2(retransmit) elapsed (T2 inactive)\n"); | |
632 | ||
633 | shdlc->t2_active = false; | |
634 | ||
635 | nfc_shdlc_requeue_ack_pending(shdlc); | |
636 | nfc_shdlc_handle_send_queue(shdlc); | |
637 | } | |
638 | ||
639 | if (shdlc->hard_fault) { | |
640 | /* | |
641 | * TODO: Handle hard_fault that occured during | |
642 | * this invocation of the shdlc worker | |
643 | */ | |
644 | } | |
645 | break; | |
646 | default: | |
647 | break; | |
648 | } | |
649 | mutex_unlock(&shdlc->state_mutex); | |
650 | } | |
651 | ||
652 | /* | |
653 | * Called from syscall context to establish shdlc link. Sleeps until | |
654 | * link is ready or failure. | |
655 | */ | |
656 | static int nfc_shdlc_connect(struct nfc_shdlc *shdlc) | |
657 | { | |
658 | DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq); | |
659 | ||
660 | pr_debug("\n"); | |
661 | ||
662 | mutex_lock(&shdlc->state_mutex); | |
663 | ||
664 | shdlc->state = SHDLC_CONNECTING; | |
665 | shdlc->connect_wq = &connect_wq; | |
666 | shdlc->connect_tries = 0; | |
667 | shdlc->connect_result = 1; | |
668 | ||
669 | mutex_unlock(&shdlc->state_mutex); | |
670 | ||
671 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
672 | ||
673 | wait_event(connect_wq, shdlc->connect_result != 1); | |
674 | ||
675 | return shdlc->connect_result; | |
676 | } | |
677 | ||
678 | static void nfc_shdlc_disconnect(struct nfc_shdlc *shdlc) | |
679 | { | |
680 | pr_debug("\n"); | |
681 | ||
682 | mutex_lock(&shdlc->state_mutex); | |
683 | ||
684 | shdlc->state = SHDLC_DISCONNECTED; | |
685 | ||
686 | mutex_unlock(&shdlc->state_mutex); | |
687 | ||
688 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
689 | } | |
690 | ||
691 | /* | |
692 | * Receive an incoming shdlc frame. Frame has already been crc-validated. | |
693 | * skb contains only LLC header and payload. | |
694 | * If skb == NULL, it is a notification that the link below is dead. | |
695 | */ | |
696 | void nfc_shdlc_recv_frame(struct nfc_shdlc *shdlc, struct sk_buff *skb) | |
697 | { | |
698 | if (skb == NULL) { | |
699 | pr_err("NULL Frame -> link is dead\n"); | |
700 | shdlc->hard_fault = -EREMOTEIO; | |
701 | } else { | |
702 | SHDLC_DUMP_SKB("incoming frame", skb); | |
703 | skb_queue_tail(&shdlc->rcv_q, skb); | |
704 | } | |
705 | ||
706 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
707 | } | |
708 | EXPORT_SYMBOL(nfc_shdlc_recv_frame); | |
709 | ||
710 | static int nfc_shdlc_open(struct nfc_hci_dev *hdev) | |
711 | { | |
712 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
713 | int r; | |
714 | ||
715 | pr_debug("\n"); | |
716 | ||
717 | if (shdlc->ops->open) { | |
718 | r = shdlc->ops->open(shdlc); | |
719 | if (r < 0) | |
720 | return r; | |
721 | } | |
722 | ||
723 | r = nfc_shdlc_connect(shdlc); | |
724 | if (r < 0 && shdlc->ops->close) | |
725 | shdlc->ops->close(shdlc); | |
726 | ||
727 | return r; | |
728 | } | |
729 | ||
730 | static void nfc_shdlc_close(struct nfc_hci_dev *hdev) | |
731 | { | |
732 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
733 | ||
734 | pr_debug("\n"); | |
735 | ||
736 | nfc_shdlc_disconnect(shdlc); | |
737 | ||
738 | if (shdlc->ops->close) | |
739 | shdlc->ops->close(shdlc); | |
740 | } | |
741 | ||
742 | static int nfc_shdlc_hci_ready(struct nfc_hci_dev *hdev) | |
743 | { | |
744 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
745 | int r = 0; | |
746 | ||
747 | pr_debug("\n"); | |
748 | ||
749 | if (shdlc->ops->hci_ready) | |
750 | r = shdlc->ops->hci_ready(shdlc); | |
751 | ||
752 | return r; | |
753 | } | |
754 | ||
755 | static int nfc_shdlc_xmit(struct nfc_hci_dev *hdev, struct sk_buff *skb) | |
756 | { | |
757 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
758 | ||
759 | SHDLC_DUMP_SKB("queuing HCP packet to shdlc", skb); | |
760 | ||
761 | skb_queue_tail(&shdlc->send_q, skb); | |
762 | ||
763 | queue_work(shdlc->sm_wq, &shdlc->sm_work); | |
764 | ||
765 | return 0; | |
766 | } | |
767 | ||
fe7c5800 SO |
768 | static int nfc_shdlc_start_poll(struct nfc_hci_dev *hdev, |
769 | u32 im_protocols, u32 tm_protocols) | |
eb738fe5 EL |
770 | { |
771 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
772 | ||
773 | pr_debug("\n"); | |
774 | ||
775 | if (shdlc->ops->start_poll) | |
fe7c5800 SO |
776 | return shdlc->ops->start_poll(shdlc, |
777 | im_protocols, tm_protocols); | |
eb738fe5 EL |
778 | |
779 | return 0; | |
780 | } | |
781 | ||
782 | static int nfc_shdlc_target_from_gate(struct nfc_hci_dev *hdev, u8 gate, | |
783 | struct nfc_target *target) | |
784 | { | |
785 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
786 | ||
787 | if (shdlc->ops->target_from_gate) | |
788 | return shdlc->ops->target_from_gate(shdlc, gate, target); | |
789 | ||
790 | return -EPERM; | |
791 | } | |
792 | ||
793 | static int nfc_shdlc_complete_target_discovered(struct nfc_hci_dev *hdev, | |
794 | u8 gate, | |
795 | struct nfc_target *target) | |
796 | { | |
797 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
798 | ||
799 | pr_debug("\n"); | |
800 | ||
801 | if (shdlc->ops->complete_target_discovered) | |
802 | return shdlc->ops->complete_target_discovered(shdlc, gate, | |
803 | target); | |
804 | ||
805 | return 0; | |
806 | } | |
807 | ||
808 | static int nfc_shdlc_data_exchange(struct nfc_hci_dev *hdev, | |
809 | struct nfc_target *target, | |
810 | struct sk_buff *skb, | |
811 | struct sk_buff **res_skb) | |
812 | { | |
813 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
814 | ||
815 | if (shdlc->ops->data_exchange) | |
816 | return shdlc->ops->data_exchange(shdlc, target, skb, res_skb); | |
817 | ||
818 | return -EPERM; | |
819 | } | |
820 | ||
1676f751 EL |
821 | static int nfc_shdlc_check_presence(struct nfc_hci_dev *hdev, |
822 | struct nfc_target *target) | |
823 | { | |
824 | struct nfc_shdlc *shdlc = nfc_hci_get_clientdata(hdev); | |
825 | ||
826 | if (shdlc->ops->check_presence) | |
827 | return shdlc->ops->check_presence(shdlc, target); | |
828 | ||
829 | return 0; | |
830 | } | |
831 | ||
eb738fe5 EL |
832 | static struct nfc_hci_ops shdlc_ops = { |
833 | .open = nfc_shdlc_open, | |
834 | .close = nfc_shdlc_close, | |
835 | .hci_ready = nfc_shdlc_hci_ready, | |
836 | .xmit = nfc_shdlc_xmit, | |
837 | .start_poll = nfc_shdlc_start_poll, | |
838 | .target_from_gate = nfc_shdlc_target_from_gate, | |
839 | .complete_target_discovered = nfc_shdlc_complete_target_discovered, | |
840 | .data_exchange = nfc_shdlc_data_exchange, | |
1676f751 | 841 | .check_presence = nfc_shdlc_check_presence, |
eb738fe5 EL |
842 | }; |
843 | ||
844 | struct nfc_shdlc *nfc_shdlc_allocate(struct nfc_shdlc_ops *ops, | |
845 | struct nfc_hci_init_data *init_data, | |
846 | u32 protocols, | |
847 | int tx_headroom, int tx_tailroom, | |
848 | int max_link_payload, const char *devname) | |
849 | { | |
850 | struct nfc_shdlc *shdlc; | |
851 | int r; | |
852 | char name[32]; | |
853 | ||
854 | if (ops->xmit == NULL) | |
855 | return NULL; | |
856 | ||
857 | shdlc = kzalloc(sizeof(struct nfc_shdlc), GFP_KERNEL); | |
858 | if (shdlc == NULL) | |
859 | return NULL; | |
860 | ||
861 | mutex_init(&shdlc->state_mutex); | |
862 | shdlc->ops = ops; | |
863 | shdlc->state = SHDLC_DISCONNECTED; | |
864 | ||
865 | init_timer(&shdlc->connect_timer); | |
866 | shdlc->connect_timer.data = (unsigned long)shdlc; | |
867 | shdlc->connect_timer.function = nfc_shdlc_connect_timeout; | |
868 | ||
869 | init_timer(&shdlc->t1_timer); | |
870 | shdlc->t1_timer.data = (unsigned long)shdlc; | |
871 | shdlc->t1_timer.function = nfc_shdlc_t1_timeout; | |
872 | ||
873 | init_timer(&shdlc->t2_timer); | |
874 | shdlc->t2_timer.data = (unsigned long)shdlc; | |
875 | shdlc->t2_timer.function = nfc_shdlc_t2_timeout; | |
876 | ||
877 | shdlc->w = SHDLC_MAX_WINDOW; | |
878 | shdlc->srej_support = SHDLC_SREJ_SUPPORT; | |
879 | ||
880 | skb_queue_head_init(&shdlc->rcv_q); | |
881 | skb_queue_head_init(&shdlc->send_q); | |
882 | skb_queue_head_init(&shdlc->ack_pending_q); | |
883 | ||
884 | INIT_WORK(&shdlc->sm_work, nfc_shdlc_sm_work); | |
885 | snprintf(name, sizeof(name), "%s_shdlc_sm_wq", devname); | |
886 | shdlc->sm_wq = alloc_workqueue(name, WQ_NON_REENTRANT | WQ_UNBOUND | | |
887 | WQ_MEM_RECLAIM, 1); | |
888 | if (shdlc->sm_wq == NULL) | |
889 | goto err_allocwq; | |
890 | ||
891 | shdlc->client_headroom = tx_headroom; | |
892 | shdlc->client_tailroom = tx_tailroom; | |
893 | ||
894 | shdlc->hdev = nfc_hci_allocate_device(&shdlc_ops, init_data, protocols, | |
895 | tx_headroom + SHDLC_LLC_HEAD_ROOM, | |
896 | tx_tailroom + SHDLC_LLC_TAIL_ROOM, | |
897 | max_link_payload); | |
898 | if (shdlc->hdev == NULL) | |
899 | goto err_allocdev; | |
900 | ||
901 | nfc_hci_set_clientdata(shdlc->hdev, shdlc); | |
902 | ||
903 | r = nfc_hci_register_device(shdlc->hdev); | |
904 | if (r < 0) | |
905 | goto err_regdev; | |
906 | ||
907 | return shdlc; | |
908 | ||
909 | err_regdev: | |
910 | nfc_hci_free_device(shdlc->hdev); | |
911 | ||
912 | err_allocdev: | |
913 | destroy_workqueue(shdlc->sm_wq); | |
914 | ||
915 | err_allocwq: | |
916 | kfree(shdlc); | |
917 | ||
918 | return NULL; | |
919 | } | |
920 | EXPORT_SYMBOL(nfc_shdlc_allocate); | |
921 | ||
922 | void nfc_shdlc_free(struct nfc_shdlc *shdlc) | |
923 | { | |
924 | pr_debug("\n"); | |
925 | ||
926 | /* TODO: Check that this cannot be called while still in use */ | |
927 | ||
928 | nfc_hci_unregister_device(shdlc->hdev); | |
929 | nfc_hci_free_device(shdlc->hdev); | |
930 | ||
931 | destroy_workqueue(shdlc->sm_wq); | |
932 | ||
933 | skb_queue_purge(&shdlc->rcv_q); | |
934 | skb_queue_purge(&shdlc->send_q); | |
935 | skb_queue_purge(&shdlc->ack_pending_q); | |
936 | ||
937 | kfree(shdlc); | |
938 | } | |
939 | EXPORT_SYMBOL(nfc_shdlc_free); | |
940 | ||
941 | void nfc_shdlc_set_clientdata(struct nfc_shdlc *shdlc, void *clientdata) | |
942 | { | |
943 | pr_debug("\n"); | |
944 | ||
945 | shdlc->clientdata = clientdata; | |
946 | } | |
947 | EXPORT_SYMBOL(nfc_shdlc_set_clientdata); | |
948 | ||
949 | void *nfc_shdlc_get_clientdata(struct nfc_shdlc *shdlc) | |
950 | { | |
951 | return shdlc->clientdata; | |
952 | } | |
953 | EXPORT_SYMBOL(nfc_shdlc_get_clientdata); | |
954 | ||
955 | struct nfc_hci_dev *nfc_shdlc_get_hci_dev(struct nfc_shdlc *shdlc) | |
956 | { | |
957 | return shdlc->hdev; | |
958 | } | |
959 | EXPORT_SYMBOL(nfc_shdlc_get_hci_dev); |