llc_shdlc.c 19 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835
  1. // SPDX-License-Identifier: GPL-2.0-only
  2. /*
  3. * shdlc Link Layer Control
  4. *
  5. * Copyright (C) 2012 Intel Corporation. All rights reserved.
  6. */
  7. #define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
  8. #include <linux/types.h>
  9. #include <linux/sched.h>
  10. #include <linux/wait.h>
  11. #include <linux/slab.h>
  12. #include <linux/skbuff.h>
  13. #include "llc.h"
  14. enum shdlc_state {
  15. SHDLC_DISCONNECTED = 0,
  16. SHDLC_CONNECTING = 1,
  17. SHDLC_NEGOTIATING = 2,
  18. SHDLC_HALF_CONNECTED = 3,
  19. SHDLC_CONNECTED = 4
  20. };
  21. struct llc_shdlc {
  22. struct nfc_hci_dev *hdev;
  23. xmit_to_drv_t xmit_to_drv;
  24. rcv_to_hci_t rcv_to_hci;
  25. struct mutex state_mutex;
  26. enum shdlc_state state;
  27. int hard_fault;
  28. wait_queue_head_t *connect_wq;
  29. int connect_tries;
  30. int connect_result;
  31. struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
  32. u8 w; /* window size */
  33. bool srej_support;
  34. struct timer_list t1_timer; /* send ack timeout */
  35. bool t1_active;
  36. struct timer_list t2_timer; /* guard/retransmit timeout */
  37. bool t2_active;
  38. int ns; /* next seq num for send */
  39. int nr; /* next expected seq num for receive */
  40. int dnr; /* oldest sent unacked seq num */
  41. struct sk_buff_head rcv_q;
  42. struct sk_buff_head send_q;
  43. bool rnr; /* other side is not ready to receive */
  44. struct sk_buff_head ack_pending_q;
  45. struct work_struct sm_work;
  46. int tx_headroom;
  47. int tx_tailroom;
  48. llc_failure_t llc_failure;
  49. };
  50. #define SHDLC_LLC_HEAD_ROOM 2
  51. #define SHDLC_MAX_WINDOW 4
  52. #define SHDLC_SREJ_SUPPORT false
  53. #define SHDLC_CONTROL_HEAD_MASK 0xe0
  54. #define SHDLC_CONTROL_HEAD_I 0x80
  55. #define SHDLC_CONTROL_HEAD_I2 0xa0
  56. #define SHDLC_CONTROL_HEAD_S 0xc0
  57. #define SHDLC_CONTROL_HEAD_U 0xe0
  58. #define SHDLC_CONTROL_NS_MASK 0x38
  59. #define SHDLC_CONTROL_NR_MASK 0x07
  60. #define SHDLC_CONTROL_TYPE_MASK 0x18
  61. #define SHDLC_CONTROL_M_MASK 0x1f
  62. enum sframe_type {
  63. S_FRAME_RR = 0x00,
  64. S_FRAME_REJ = 0x01,
  65. S_FRAME_RNR = 0x02,
  66. S_FRAME_SREJ = 0x03
  67. };
  68. enum uframe_modifier {
  69. U_FRAME_UA = 0x06,
  70. U_FRAME_RSET = 0x19
  71. };
  72. #define SHDLC_CONNECT_VALUE_MS 5
  73. #define SHDLC_T1_VALUE_MS(w) ((5 * w) / 4)
  74. #define SHDLC_T2_VALUE_MS 300
  75. #define SHDLC_DUMP_SKB(info, skb) \
  76. do { \
  77. pr_debug("%s:\n", info); \
  78. print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
  79. 16, 1, skb->data, skb->len, 0); \
  80. } while (0)
  81. /* checks x < y <= z modulo 8 */
  82. static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
  83. {
  84. if (x < z)
  85. return ((x < y) && (y <= z)) ? true : false;
  86. else
  87. return ((y > x) || (y <= z)) ? true : false;
  88. }
  89. /* checks x <= y < z modulo 8 */
  90. static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
  91. {
  92. if (x <= z)
  93. return ((x <= y) && (y < z)) ? true : false;
  94. else /* x > z -> z+8 > x */
  95. return ((y >= x) || (y < z)) ? true : false;
  96. }
  97. static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc,
  98. int payload_len)
  99. {
  100. struct sk_buff *skb;
  101. skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
  102. shdlc->tx_tailroom + payload_len, GFP_KERNEL);
  103. if (skb)
  104. skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
  105. return skb;
  106. }
  107. /* immediately sends an S frame. */
  108. static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc,
  109. enum sframe_type sframe_type, int nr)
  110. {
  111. int r;
  112. struct sk_buff *skb;
  113. pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
  114. skb = llc_shdlc_alloc_skb(shdlc, 0);
  115. if (skb == NULL)
  116. return -ENOMEM;
  117. *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
  118. r = shdlc->xmit_to_drv(shdlc->hdev, skb);
  119. kfree_skb(skb);
  120. return r;
  121. }
  122. /* immediately sends an U frame. skb may contain optional payload */
  123. static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc,
  124. struct sk_buff *skb,
  125. enum uframe_modifier uframe_modifier)
  126. {
  127. int r;
  128. pr_debug("uframe_modifier=%d\n", uframe_modifier);
  129. *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
  130. r = shdlc->xmit_to_drv(shdlc->hdev, skb);
  131. kfree_skb(skb);
  132. return r;
  133. }
  134. /*
  135. * Free ack_pending frames until y_nr - 1, and reset t2 according to
  136. * the remaining oldest ack_pending frame sent time
  137. */
  138. static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
  139. {
  140. struct sk_buff *skb;
  141. int dnr = shdlc->dnr; /* MUST initially be < y_nr */
  142. pr_debug("release ack pending up to frame %d excluded\n", y_nr);
  143. while (dnr != y_nr) {
  144. pr_debug("release ack pending frame %d\n", dnr);
  145. skb = skb_dequeue(&shdlc->ack_pending_q);
  146. kfree_skb(skb);
  147. dnr = (dnr + 1) % 8;
  148. }
  149. if (skb_queue_empty(&shdlc->ack_pending_q)) {
  150. if (shdlc->t2_active) {
  151. del_timer_sync(&shdlc->t2_timer);
  152. shdlc->t2_active = false;
  153. pr_debug
  154. ("All sent frames acked. Stopped T2(retransmit)\n");
  155. }
  156. } else {
  157. skb = skb_peek(&shdlc->ack_pending_q);
  158. mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
  159. msecs_to_jiffies(SHDLC_T2_VALUE_MS));
  160. shdlc->t2_active = true;
  161. pr_debug
  162. ("Start T2(retransmit) for remaining unacked sent frames\n");
  163. }
  164. }
  165. /*
  166. * Receive validated frames from lower layer. skb contains HCI payload only.
  167. * Handle according to algorithm at spec:10.8.2
  168. */
  169. static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
  170. struct sk_buff *skb, int ns, int nr)
  171. {
  172. int x_ns = ns;
  173. int y_nr = nr;
  174. pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
  175. if (shdlc->state != SHDLC_CONNECTED)
  176. goto exit;
  177. if (x_ns != shdlc->nr) {
  178. llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
  179. goto exit;
  180. }
  181. if (shdlc->t1_active == false) {
  182. shdlc->t1_active = true;
  183. mod_timer(&shdlc->t1_timer, jiffies +
  184. msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
  185. pr_debug("(re)Start T1(send ack)\n");
  186. }
  187. if (skb->len) {
  188. shdlc->rcv_to_hci(shdlc->hdev, skb);
  189. skb = NULL;
  190. }
  191. shdlc->nr = (shdlc->nr + 1) % 8;
  192. if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
  193. llc_shdlc_reset_t2(shdlc, y_nr);
  194. shdlc->dnr = y_nr;
  195. }
  196. exit:
  197. kfree_skb(skb);
  198. }
  199. static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
  200. {
  201. pr_debug("remote acked up to frame %d excluded\n", y_nr);
  202. if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
  203. llc_shdlc_reset_t2(shdlc, y_nr);
  204. shdlc->dnr = y_nr;
  205. }
  206. }
  207. static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
  208. {
  209. struct sk_buff *skb;
  210. pr_debug("ns reset to %d\n", shdlc->dnr);
  211. while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
  212. skb_pull(skb, 1); /* remove control field */
  213. skb_queue_head(&shdlc->send_q, skb);
  214. }
  215. shdlc->ns = shdlc->dnr;
  216. }
  217. static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
  218. {
  219. struct sk_buff *skb;
  220. pr_debug("remote asks retransmission from frame %d\n", y_nr);
  221. if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
  222. if (shdlc->t2_active) {
  223. del_timer_sync(&shdlc->t2_timer);
  224. shdlc->t2_active = false;
  225. pr_debug("Stopped T2(retransmit)\n");
  226. }
  227. if (shdlc->dnr != y_nr) {
  228. while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
  229. skb = skb_dequeue(&shdlc->ack_pending_q);
  230. kfree_skb(skb);
  231. }
  232. }
  233. llc_shdlc_requeue_ack_pending(shdlc);
  234. }
  235. }
  236. /* See spec RR:10.8.3 REJ:10.8.4 */
  237. static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,
  238. enum sframe_type s_frame_type, int nr)
  239. {
  240. struct sk_buff *skb;
  241. if (shdlc->state != SHDLC_CONNECTED)
  242. return;
  243. switch (s_frame_type) {
  244. case S_FRAME_RR:
  245. llc_shdlc_rcv_ack(shdlc, nr);
  246. if (shdlc->rnr == true) { /* see SHDLC 10.7.7 */
  247. shdlc->rnr = false;
  248. if (shdlc->send_q.qlen == 0) {
  249. skb = llc_shdlc_alloc_skb(shdlc, 0);
  250. if (skb)
  251. skb_queue_tail(&shdlc->send_q, skb);
  252. }
  253. }
  254. break;
  255. case S_FRAME_REJ:
  256. llc_shdlc_rcv_rej(shdlc, nr);
  257. break;
  258. case S_FRAME_RNR:
  259. llc_shdlc_rcv_ack(shdlc, nr);
  260. shdlc->rnr = true;
  261. break;
  262. default:
  263. break;
  264. }
  265. }
  266. static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
  267. {
  268. pr_debug("result=%d\n", r);
  269. del_timer_sync(&shdlc->connect_timer);
  270. if (r == 0) {
  271. shdlc->ns = 0;
  272. shdlc->nr = 0;
  273. shdlc->dnr = 0;
  274. shdlc->state = SHDLC_HALF_CONNECTED;
  275. } else {
  276. shdlc->state = SHDLC_DISCONNECTED;
  277. }
  278. shdlc->connect_result = r;
  279. wake_up(shdlc->connect_wq);
  280. }
  281. static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)
  282. {
  283. struct sk_buff *skb;
  284. pr_debug("\n");
  285. skb = llc_shdlc_alloc_skb(shdlc, 2);
  286. if (skb == NULL)
  287. return -ENOMEM;
  288. skb_put_u8(skb, SHDLC_MAX_WINDOW);
  289. skb_put_u8(skb, SHDLC_SREJ_SUPPORT ? 1 : 0);
  290. return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
  291. }
  292. static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)
  293. {
  294. struct sk_buff *skb;
  295. pr_debug("\n");
  296. skb = llc_shdlc_alloc_skb(shdlc, 0);
  297. if (skb == NULL)
  298. return -ENOMEM;
  299. return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
  300. }
  301. static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
  302. struct sk_buff *skb,
  303. enum uframe_modifier u_frame_modifier)
  304. {
  305. u8 w = SHDLC_MAX_WINDOW;
  306. bool srej_support = SHDLC_SREJ_SUPPORT;
  307. int r;
  308. pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
  309. switch (u_frame_modifier) {
  310. case U_FRAME_RSET:
  311. switch (shdlc->state) {
  312. case SHDLC_NEGOTIATING:
  313. case SHDLC_CONNECTING:
  314. /*
  315. * We sent RSET, but chip wants to negociate or we
  316. * got RSET before we managed to send out our.
  317. */
  318. if (skb->len > 0)
  319. w = skb->data[0];
  320. if (skb->len > 1)
  321. srej_support = skb->data[1] & 0x01 ? true :
  322. false;
  323. if ((w <= SHDLC_MAX_WINDOW) &&
  324. (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
  325. shdlc->w = w;
  326. shdlc->srej_support = srej_support;
  327. r = llc_shdlc_connect_send_ua(shdlc);
  328. llc_shdlc_connect_complete(shdlc, r);
  329. }
  330. break;
  331. case SHDLC_HALF_CONNECTED:
  332. /*
  333. * Chip resent RSET due to its timeout - Ignote it
  334. * as we already sent UA.
  335. */
  336. break;
  337. case SHDLC_CONNECTED:
  338. /*
  339. * Chip wants to reset link. This is unexpected and
  340. * unsupported.
  341. */
  342. shdlc->hard_fault = -ECONNRESET;
  343. break;
  344. default:
  345. break;
  346. }
  347. break;
  348. case U_FRAME_UA:
  349. if ((shdlc->state == SHDLC_CONNECTING &&
  350. shdlc->connect_tries > 0) ||
  351. (shdlc->state == SHDLC_NEGOTIATING)) {
  352. llc_shdlc_connect_complete(shdlc, 0);
  353. shdlc->state = SHDLC_CONNECTED;
  354. }
  355. break;
  356. default:
  357. break;
  358. }
  359. kfree_skb(skb);
  360. }
  361. static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
  362. {
  363. struct sk_buff *skb;
  364. u8 control;
  365. int nr;
  366. int ns;
  367. enum sframe_type s_frame_type;
  368. enum uframe_modifier u_frame_modifier;
  369. if (shdlc->rcv_q.qlen)
  370. pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
  371. while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
  372. control = skb->data[0];
  373. skb_pull(skb, 1);
  374. switch (control & SHDLC_CONTROL_HEAD_MASK) {
  375. case SHDLC_CONTROL_HEAD_I:
  376. case SHDLC_CONTROL_HEAD_I2:
  377. if (shdlc->state == SHDLC_HALF_CONNECTED)
  378. shdlc->state = SHDLC_CONNECTED;
  379. ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
  380. nr = control & SHDLC_CONTROL_NR_MASK;
  381. llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
  382. break;
  383. case SHDLC_CONTROL_HEAD_S:
  384. if (shdlc->state == SHDLC_HALF_CONNECTED)
  385. shdlc->state = SHDLC_CONNECTED;
  386. s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
  387. nr = control & SHDLC_CONTROL_NR_MASK;
  388. llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
  389. kfree_skb(skb);
  390. break;
  391. case SHDLC_CONTROL_HEAD_U:
  392. u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
  393. llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
  394. break;
  395. default:
  396. pr_err("UNKNOWN Control=%d\n", control);
  397. kfree_skb(skb);
  398. break;
  399. }
  400. }
  401. }
  402. static int llc_shdlc_w_used(int ns, int dnr)
  403. {
  404. int unack_count;
  405. if (dnr <= ns)
  406. unack_count = ns - dnr;
  407. else
  408. unack_count = 8 - dnr + ns;
  409. return unack_count;
  410. }
  411. /* Send frames according to algorithm at spec:10.8.1 */
  412. static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
  413. {
  414. struct sk_buff *skb;
  415. int r;
  416. unsigned long time_sent;
  417. if (shdlc->send_q.qlen)
  418. pr_debug
  419. ("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
  420. shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
  421. shdlc->rnr == false ? "false" : "true",
  422. shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
  423. shdlc->ack_pending_q.qlen);
  424. while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
  425. (shdlc->rnr == false)) {
  426. if (shdlc->t1_active) {
  427. del_timer_sync(&shdlc->t1_timer);
  428. shdlc->t1_active = false;
  429. pr_debug("Stopped T1(send ack)\n");
  430. }
  431. skb = skb_dequeue(&shdlc->send_q);
  432. *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
  433. shdlc->nr;
  434. pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
  435. shdlc->nr);
  436. SHDLC_DUMP_SKB("shdlc frame written", skb);
  437. r = shdlc->xmit_to_drv(shdlc->hdev, skb);
  438. if (r < 0) {
  439. shdlc->hard_fault = r;
  440. break;
  441. }
  442. shdlc->ns = (shdlc->ns + 1) % 8;
  443. time_sent = jiffies;
  444. *(unsigned long *)skb->cb = time_sent;
  445. skb_queue_tail(&shdlc->ack_pending_q, skb);
  446. if (shdlc->t2_active == false) {
  447. shdlc->t2_active = true;
  448. mod_timer(&shdlc->t2_timer, time_sent +
  449. msecs_to_jiffies(SHDLC_T2_VALUE_MS));
  450. pr_debug("Started T2 (retransmit)\n");
  451. }
  452. }
  453. }
  454. static void llc_shdlc_connect_timeout(struct timer_list *t)
  455. {
  456. struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);
  457. pr_debug("\n");
  458. schedule_work(&shdlc->sm_work);
  459. }
  460. static void llc_shdlc_t1_timeout(struct timer_list *t)
  461. {
  462. struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);
  463. pr_debug("SoftIRQ: need to send ack\n");
  464. schedule_work(&shdlc->sm_work);
  465. }
  466. static void llc_shdlc_t2_timeout(struct timer_list *t)
  467. {
  468. struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);
  469. pr_debug("SoftIRQ: need to retransmit\n");
  470. schedule_work(&shdlc->sm_work);
  471. }
  472. static void llc_shdlc_sm_work(struct work_struct *work)
  473. {
  474. struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
  475. int r;
  476. pr_debug("\n");
  477. mutex_lock(&shdlc->state_mutex);
  478. switch (shdlc->state) {
  479. case SHDLC_DISCONNECTED:
  480. skb_queue_purge(&shdlc->rcv_q);
  481. skb_queue_purge(&shdlc->send_q);
  482. skb_queue_purge(&shdlc->ack_pending_q);
  483. break;
  484. case SHDLC_CONNECTING:
  485. if (shdlc->hard_fault) {
  486. llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
  487. break;
  488. }
  489. if (shdlc->connect_tries++ < 5)
  490. r = llc_shdlc_connect_initiate(shdlc);
  491. else
  492. r = -ETIME;
  493. if (r < 0) {
  494. llc_shdlc_connect_complete(shdlc, r);
  495. } else {
  496. mod_timer(&shdlc->connect_timer, jiffies +
  497. msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
  498. shdlc->state = SHDLC_NEGOTIATING;
  499. }
  500. break;
  501. case SHDLC_NEGOTIATING:
  502. if (timer_pending(&shdlc->connect_timer) == 0) {
  503. shdlc->state = SHDLC_CONNECTING;
  504. schedule_work(&shdlc->sm_work);
  505. }
  506. llc_shdlc_handle_rcv_queue(shdlc);
  507. if (shdlc->hard_fault) {
  508. llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
  509. break;
  510. }
  511. break;
  512. case SHDLC_HALF_CONNECTED:
  513. case SHDLC_CONNECTED:
  514. llc_shdlc_handle_rcv_queue(shdlc);
  515. llc_shdlc_handle_send_queue(shdlc);
  516. if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
  517. pr_debug
  518. ("Handle T1(send ack) elapsed (T1 now inactive)\n");
  519. shdlc->t1_active = false;
  520. r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
  521. shdlc->nr);
  522. if (r < 0)
  523. shdlc->hard_fault = r;
  524. }
  525. if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
  526. pr_debug
  527. ("Handle T2(retransmit) elapsed (T2 inactive)\n");
  528. shdlc->t2_active = false;
  529. llc_shdlc_requeue_ack_pending(shdlc);
  530. llc_shdlc_handle_send_queue(shdlc);
  531. }
  532. if (shdlc->hard_fault)
  533. shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
  534. break;
  535. default:
  536. break;
  537. }
  538. mutex_unlock(&shdlc->state_mutex);
  539. }
  540. /*
  541. * Called from syscall context to establish shdlc link. Sleeps until
  542. * link is ready or failure.
  543. */
  544. static int llc_shdlc_connect(struct llc_shdlc *shdlc)
  545. {
  546. DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
  547. pr_debug("\n");
  548. mutex_lock(&shdlc->state_mutex);
  549. shdlc->state = SHDLC_CONNECTING;
  550. shdlc->connect_wq = &connect_wq;
  551. shdlc->connect_tries = 0;
  552. shdlc->connect_result = 1;
  553. mutex_unlock(&shdlc->state_mutex);
  554. schedule_work(&shdlc->sm_work);
  555. wait_event(connect_wq, shdlc->connect_result != 1);
  556. return shdlc->connect_result;
  557. }
  558. static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
  559. {
  560. pr_debug("\n");
  561. mutex_lock(&shdlc->state_mutex);
  562. shdlc->state = SHDLC_DISCONNECTED;
  563. mutex_unlock(&shdlc->state_mutex);
  564. schedule_work(&shdlc->sm_work);
  565. }
  566. /*
  567. * Receive an incoming shdlc frame. Frame has already been crc-validated.
  568. * skb contains only LLC header and payload.
  569. * If skb == NULL, it is a notification that the link below is dead.
  570. */
  571. static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
  572. {
  573. if (skb == NULL) {
  574. pr_err("NULL Frame -> link is dead\n");
  575. shdlc->hard_fault = -EREMOTEIO;
  576. } else {
  577. SHDLC_DUMP_SKB("incoming frame", skb);
  578. skb_queue_tail(&shdlc->rcv_q, skb);
  579. }
  580. schedule_work(&shdlc->sm_work);
  581. }
  582. static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
  583. rcv_to_hci_t rcv_to_hci, int tx_headroom,
  584. int tx_tailroom, int *rx_headroom, int *rx_tailroom,
  585. llc_failure_t llc_failure)
  586. {
  587. struct llc_shdlc *shdlc;
  588. *rx_headroom = SHDLC_LLC_HEAD_ROOM;
  589. *rx_tailroom = 0;
  590. shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
  591. if (shdlc == NULL)
  592. return NULL;
  593. mutex_init(&shdlc->state_mutex);
  594. shdlc->state = SHDLC_DISCONNECTED;
  595. timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0);
  596. timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0);
  597. timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0);
  598. shdlc->w = SHDLC_MAX_WINDOW;
  599. shdlc->srej_support = SHDLC_SREJ_SUPPORT;
  600. skb_queue_head_init(&shdlc->rcv_q);
  601. skb_queue_head_init(&shdlc->send_q);
  602. skb_queue_head_init(&shdlc->ack_pending_q);
  603. INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
  604. shdlc->hdev = hdev;
  605. shdlc->xmit_to_drv = xmit_to_drv;
  606. shdlc->rcv_to_hci = rcv_to_hci;
  607. shdlc->tx_headroom = tx_headroom;
  608. shdlc->tx_tailroom = tx_tailroom;
  609. shdlc->llc_failure = llc_failure;
  610. return shdlc;
  611. }
  612. static void llc_shdlc_deinit(struct nfc_llc *llc)
  613. {
  614. struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
  615. skb_queue_purge(&shdlc->rcv_q);
  616. skb_queue_purge(&shdlc->send_q);
  617. skb_queue_purge(&shdlc->ack_pending_q);
  618. kfree(shdlc);
  619. }
  620. static int llc_shdlc_start(struct nfc_llc *llc)
  621. {
  622. struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
  623. return llc_shdlc_connect(shdlc);
  624. }
  625. static int llc_shdlc_stop(struct nfc_llc *llc)
  626. {
  627. struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
  628. llc_shdlc_disconnect(shdlc);
  629. return 0;
  630. }
  631. static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
  632. {
  633. struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
  634. llc_shdlc_recv_frame(shdlc, skb);
  635. }
  636. static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
  637. {
  638. struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
  639. skb_queue_tail(&shdlc->send_q, skb);
  640. schedule_work(&shdlc->sm_work);
  641. return 0;
  642. }
  643. static struct nfc_llc_ops llc_shdlc_ops = {
  644. .init = llc_shdlc_init,
  645. .deinit = llc_shdlc_deinit,
  646. .start = llc_shdlc_start,
  647. .stop = llc_shdlc_stop,
  648. .rcv_from_drv = llc_shdlc_rcv_from_drv,
  649. .xmit_from_hci = llc_shdlc_xmit_from_hci,
  650. };
  651. int nfc_llc_shdlc_register(void)
  652. {
  653. return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
  654. }