nfc/hci/llc_shdlc.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * shdlc Link Layer Control
0004  *
0005  * Copyright (C) 2012  Intel Corporation. All rights reserved.
0006  */
0007
0008 #define pr_fmt(fmt) "shdlc: %s: " fmt, __func__
0009
0010 #include <linux/types.h>
0011 #include <linux/sched.h>
0012 #include <linux/wait.h>
0013 #include <linux/slab.h>
0014 #include <linux/skbuff.h>
0015
0016 #include "llc.h"
0017
0018 enum shdlc_state {
0019     SHDLC_DISCONNECTED = 0,
0020     SHDLC_CONNECTING = 1,
0021     SHDLC_NEGOTIATING = 2,
0022     SHDLC_HALF_CONNECTED = 3,
0023     SHDLC_CONNECTED = 4
0024 };
0025
0026 struct llc_shdlc {
0027     struct nfc_hci_dev *hdev;
0028     xmit_to_drv_t xmit_to_drv;
0029     rcv_to_hci_t rcv_to_hci;
0030
0031     struct mutex state_mutex;
0032     enum shdlc_state state;
0033     int hard_fault;
0034
0035     wait_queue_head_t *connect_wq;
0036     int connect_tries;
0037     int connect_result;
0038     struct timer_list connect_timer;/* aka T3 in spec 10.6.1 */
0039
0040     u8 w;               /* window size */
0041     bool srej_support;
0042
0043     struct timer_list t1_timer; /* send ack timeout */
0044     bool t1_active;
0045
0046     struct timer_list t2_timer; /* guard/retransmit timeout */
0047     bool t2_active;
0048
0049     int ns;             /* next seq num for send */
0050     int nr;             /* next expected seq num for receive */
0051     int dnr;            /* oldest sent unacked seq num */
0052
0053     struct sk_buff_head rcv_q;
0054
0055     struct sk_buff_head send_q;
0056     bool rnr;           /* other side is not ready to receive */
0057
0058     struct sk_buff_head ack_pending_q;
0059
0060     struct work_struct sm_work;
0061
0062     int tx_headroom;
0063     int tx_tailroom;
0064
0065     llc_failure_t llc_failure;
0066 };
0067
0068 #define SHDLC_LLC_HEAD_ROOM 2
0069
0070 #define SHDLC_MAX_WINDOW    4
0071 #define SHDLC_SREJ_SUPPORT  false
0072
0073 #define SHDLC_CONTROL_HEAD_MASK 0xe0
0074 #define SHDLC_CONTROL_HEAD_I    0x80
0075 #define SHDLC_CONTROL_HEAD_I2   0xa0
0076 #define SHDLC_CONTROL_HEAD_S    0xc0
0077 #define SHDLC_CONTROL_HEAD_U    0xe0
0078
0079 #define SHDLC_CONTROL_NS_MASK   0x38
0080 #define SHDLC_CONTROL_NR_MASK   0x07
0081 #define SHDLC_CONTROL_TYPE_MASK 0x18
0082
0083 #define SHDLC_CONTROL_M_MASK    0x1f
0084
0085 enum sframe_type {
0086     S_FRAME_RR = 0x00,
0087     S_FRAME_REJ = 0x01,
0088     S_FRAME_RNR = 0x02,
0089     S_FRAME_SREJ = 0x03
0090 };
0091
0092 enum uframe_modifier {
0093     U_FRAME_UA = 0x06,
0094     U_FRAME_RSET = 0x19
0095 };
0096
0097 #define SHDLC_CONNECT_VALUE_MS  5
0098 #define SHDLC_T1_VALUE_MS(w)    ((5 * w) / 4)
0099 #define SHDLC_T2_VALUE_MS   300
0100
0101 #define SHDLC_DUMP_SKB(info, skb)                 \
0102 do {                                  \
0103     pr_debug("%s:\n", info);                  \
0104     print_hex_dump(KERN_DEBUG, "shdlc: ", DUMP_PREFIX_OFFSET, \
0105                16, 1, skb->data, skb->len, 0);        \
0106 } while (0)
0107
0108 /* checks x < y <= z modulo 8 */
0109 static bool llc_shdlc_x_lt_y_lteq_z(int x, int y, int z)
0110 {
0111     if (x < z)
0112         return ((x < y) && (y <= z)) ? true : false;
0113     else
0114         return ((y > x) || (y <= z)) ? true : false;
0115 }
0116
0117 /* checks x <= y < z modulo 8 */
0118 static bool llc_shdlc_x_lteq_y_lt_z(int x, int y, int z)
0119 {
0120     if (x <= z)
0121         return ((x <= y) && (y < z)) ? true : false;
0122     else            /* x > z -> z+8 > x */
0123         return ((y >= x) || (y < z)) ? true : false;
0124 }
0125
0126 static struct sk_buff *llc_shdlc_alloc_skb(const struct llc_shdlc *shdlc,
0127                        int payload_len)
0128 {
0129     struct sk_buff *skb;
0130
0131     skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +
0132             shdlc->tx_tailroom + payload_len, GFP_KERNEL);
0133     if (skb)
0134         skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);
0135
0136     return skb;
0137 }
0138
0139 /* immediately sends an S frame. */
0140 static int llc_shdlc_send_s_frame(const struct llc_shdlc *shdlc,
0141                   enum sframe_type sframe_type, int nr)
0142 {
0143     int r;
0144     struct sk_buff *skb;
0145
0146     pr_debug("sframe_type=%d nr=%d\n", sframe_type, nr);
0147
0148     skb = llc_shdlc_alloc_skb(shdlc, 0);
0149     if (skb == NULL)
0150         return -ENOMEM;
0151
0152     *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_S | (sframe_type << 3) | nr;
0153
0154     r = shdlc->xmit_to_drv(shdlc->hdev, skb);
0155
0156     kfree_skb(skb);
0157
0158     return r;
0159 }
0160
0161 /* immediately sends an U frame. skb may contain optional payload */
0162 static int llc_shdlc_send_u_frame(const struct llc_shdlc *shdlc,
0163                   struct sk_buff *skb,
0164                   enum uframe_modifier uframe_modifier)
0165 {
0166     int r;
0167
0168     pr_debug("uframe_modifier=%d\n", uframe_modifier);
0169
0170     *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_U | uframe_modifier;
0171
0172     r = shdlc->xmit_to_drv(shdlc->hdev, skb);
0173
0174     kfree_skb(skb);
0175
0176     return r;
0177 }
0178
0179 /*
0180  * Free ack_pending frames until y_nr - 1, and reset t2 according to
0181  * the remaining oldest ack_pending frame sent time
0182  */
0183 static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)
0184 {
0185     struct sk_buff *skb;
0186     int dnr = shdlc->dnr;   /* MUST initially be < y_nr */
0187
0188     pr_debug("release ack pending up to frame %d excluded\n", y_nr);
0189
0190     while (dnr != y_nr) {
0191         pr_debug("release ack pending frame %d\n", dnr);
0192
0193         skb = skb_dequeue(&shdlc->ack_pending_q);
0194         kfree_skb(skb);
0195
0196         dnr = (dnr + 1) % 8;
0197     }
0198
0199     if (skb_queue_empty(&shdlc->ack_pending_q)) {
0200         if (shdlc->t2_active) {
0201             del_timer_sync(&shdlc->t2_timer);
0202             shdlc->t2_active = false;
0203
0204             pr_debug("All sent frames acked. Stopped T2(retransmit)\n");
0205         }
0206     } else {
0207         skb = skb_peek(&shdlc->ack_pending_q);
0208
0209         mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +
0210               msecs_to_jiffies(SHDLC_T2_VALUE_MS));
0211         shdlc->t2_active = true;
0212
0213         pr_debug("Start T2(retransmit) for remaining unacked sent frames\n");
0214     }
0215 }
0216
0217 /*
0218  * Receive validated frames from lower layer. skb contains HCI payload only.
0219  * Handle according to algorithm at spec:10.8.2
0220  */
0221 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,
0222                   struct sk_buff *skb, int ns, int nr)
0223 {
0224     int x_ns = ns;
0225     int y_nr = nr;
0226
0227     pr_debug("recvd I-frame %d, remote waiting frame %d\n", ns, nr);
0228
0229     if (shdlc->state != SHDLC_CONNECTED)
0230         goto exit;
0231
0232     if (x_ns != shdlc->nr) {
0233         llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);
0234         goto exit;
0235     }
0236
0237     if (!shdlc->t1_active) {
0238         shdlc->t1_active = true;
0239         mod_timer(&shdlc->t1_timer, jiffies +
0240               msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));
0241         pr_debug("(re)Start T1(send ack)\n");
0242     }
0243
0244     if (skb->len) {
0245         shdlc->rcv_to_hci(shdlc->hdev, skb);
0246         skb = NULL;
0247     }
0248
0249     shdlc->nr = (shdlc->nr + 1) % 8;
0250
0251     if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
0252         llc_shdlc_reset_t2(shdlc, y_nr);
0253
0254         shdlc->dnr = y_nr;
0255     }
0256
0257 exit:
0258     kfree_skb(skb);
0259 }
0260
0261 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)
0262 {
0263     pr_debug("remote acked up to frame %d excluded\n", y_nr);
0264
0265     if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {
0266         llc_shdlc_reset_t2(shdlc, y_nr);
0267         shdlc->dnr = y_nr;
0268     }
0269 }
0270
0271 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)
0272 {
0273     struct sk_buff *skb;
0274
0275     pr_debug("ns reset to %d\n", shdlc->dnr);
0276
0277     while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {
0278         skb_pull(skb, 1);   /* remove control field */
0279         skb_queue_head(&shdlc->send_q, skb);
0280     }
0281     shdlc->ns = shdlc->dnr;
0282 }
0283
0284 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)
0285 {
0286     struct sk_buff *skb;
0287
0288     pr_debug("remote asks retransmission from frame %d\n", y_nr);
0289
0290     if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {
0291         if (shdlc->t2_active) {
0292             del_timer_sync(&shdlc->t2_timer);
0293             shdlc->t2_active = false;
0294             pr_debug("Stopped T2(retransmit)\n");
0295         }
0296
0297         if (shdlc->dnr != y_nr) {
0298             while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {
0299                 skb = skb_dequeue(&shdlc->ack_pending_q);
0300                 kfree_skb(skb);
0301             }
0302         }
0303
0304         llc_shdlc_requeue_ack_pending(shdlc);
0305     }
0306 }
0307
0308 /* See spec RR:10.8.3 REJ:10.8.4 */
0309 static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,
0310                   enum sframe_type s_frame_type, int nr)
0311 {
0312     struct sk_buff *skb;
0313
0314     if (shdlc->state != SHDLC_CONNECTED)
0315         return;
0316
0317     switch (s_frame_type) {
0318     case S_FRAME_RR:
0319         llc_shdlc_rcv_ack(shdlc, nr);
0320         if (shdlc->rnr == true) {   /* see SHDLC 10.7.7 */
0321             shdlc->rnr = false;
0322             if (shdlc->send_q.qlen == 0) {
0323                 skb = llc_shdlc_alloc_skb(shdlc, 0);
0324                 if (skb)
0325                     skb_queue_tail(&shdlc->send_q, skb);
0326             }
0327         }
0328         break;
0329     case S_FRAME_REJ:
0330         llc_shdlc_rcv_rej(shdlc, nr);
0331         break;
0332     case S_FRAME_RNR:
0333         llc_shdlc_rcv_ack(shdlc, nr);
0334         shdlc->rnr = true;
0335         break;
0336     default:
0337         break;
0338     }
0339 }
0340
0341 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)
0342 {
0343     pr_debug("result=%d\n", r);
0344
0345     del_timer_sync(&shdlc->connect_timer);
0346
0347     if (r == 0) {
0348         shdlc->ns = 0;
0349         shdlc->nr = 0;
0350         shdlc->dnr = 0;
0351
0352         shdlc->state = SHDLC_HALF_CONNECTED;
0353     } else {
0354         shdlc->state = SHDLC_DISCONNECTED;
0355     }
0356
0357     shdlc->connect_result = r;
0358
0359     wake_up(shdlc->connect_wq);
0360 }
0361
0362 static int llc_shdlc_connect_initiate(const struct llc_shdlc *shdlc)
0363 {
0364     struct sk_buff *skb;
0365
0366     skb = llc_shdlc_alloc_skb(shdlc, 2);
0367     if (skb == NULL)
0368         return -ENOMEM;
0369
0370     skb_put_u8(skb, SHDLC_MAX_WINDOW);
0371     skb_put_u8(skb, SHDLC_SREJ_SUPPORT ? 1 : 0);
0372
0373     return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);
0374 }
0375
0376 static int llc_shdlc_connect_send_ua(const struct llc_shdlc *shdlc)
0377 {
0378     struct sk_buff *skb;
0379
0380     skb = llc_shdlc_alloc_skb(shdlc, 0);
0381     if (skb == NULL)
0382         return -ENOMEM;
0383
0384     return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);
0385 }
0386
0387 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,
0388                   struct sk_buff *skb,
0389                   enum uframe_modifier u_frame_modifier)
0390 {
0391     u8 w = SHDLC_MAX_WINDOW;
0392     bool srej_support = SHDLC_SREJ_SUPPORT;
0393     int r;
0394
0395     pr_debug("u_frame_modifier=%d\n", u_frame_modifier);
0396
0397     switch (u_frame_modifier) {
0398     case U_FRAME_RSET:
0399         switch (shdlc->state) {
0400         case SHDLC_NEGOTIATING:
0401         case SHDLC_CONNECTING:
0402             /*
0403              * We sent RSET, but chip wants to negotiate or we
0404              * got RSET before we managed to send out our.
0405              */
0406             if (skb->len > 0)
0407                 w = skb->data[0];
0408
0409             if (skb->len > 1)
0410                 srej_support = skb->data[1] & 0x01 ? true :
0411                            false;
0412
0413             if ((w <= SHDLC_MAX_WINDOW) &&
0414                 (SHDLC_SREJ_SUPPORT || (srej_support == false))) {
0415                 shdlc->w = w;
0416                 shdlc->srej_support = srej_support;
0417                 r = llc_shdlc_connect_send_ua(shdlc);
0418                 llc_shdlc_connect_complete(shdlc, r);
0419             }
0420             break;
0421         case SHDLC_HALF_CONNECTED:
0422             /*
0423              * Chip resent RSET due to its timeout - Ignote it
0424              * as we already sent UA.
0425              */
0426             break;
0427         case SHDLC_CONNECTED:
0428             /*
0429              * Chip wants to reset link. This is unexpected and
0430              * unsupported.
0431              */
0432             shdlc->hard_fault = -ECONNRESET;
0433             break;
0434         default:
0435             break;
0436         }
0437         break;
0438     case U_FRAME_UA:
0439         if ((shdlc->state == SHDLC_CONNECTING &&
0440              shdlc->connect_tries > 0) ||
0441             (shdlc->state == SHDLC_NEGOTIATING)) {
0442             llc_shdlc_connect_complete(shdlc, 0);
0443             shdlc->state = SHDLC_CONNECTED;
0444         }
0445         break;
0446     default:
0447         break;
0448     }
0449
0450     kfree_skb(skb);
0451 }
0452
0453 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)
0454 {
0455     struct sk_buff *skb;
0456     u8 control;
0457     int nr;
0458     int ns;
0459     enum sframe_type s_frame_type;
0460     enum uframe_modifier u_frame_modifier;
0461
0462     if (shdlc->rcv_q.qlen)
0463         pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);
0464
0465     while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {
0466         control = skb->data[0];
0467         skb_pull(skb, 1);
0468         switch (control & SHDLC_CONTROL_HEAD_MASK) {
0469         case SHDLC_CONTROL_HEAD_I:
0470         case SHDLC_CONTROL_HEAD_I2:
0471             if (shdlc->state == SHDLC_HALF_CONNECTED)
0472                 shdlc->state = SHDLC_CONNECTED;
0473
0474             ns = (control & SHDLC_CONTROL_NS_MASK) >> 3;
0475             nr = control & SHDLC_CONTROL_NR_MASK;
0476             llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);
0477             break;
0478         case SHDLC_CONTROL_HEAD_S:
0479             if (shdlc->state == SHDLC_HALF_CONNECTED)
0480                 shdlc->state = SHDLC_CONNECTED;
0481
0482             s_frame_type = (control & SHDLC_CONTROL_TYPE_MASK) >> 3;
0483             nr = control & SHDLC_CONTROL_NR_MASK;
0484             llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);
0485             kfree_skb(skb);
0486             break;
0487         case SHDLC_CONTROL_HEAD_U:
0488             u_frame_modifier = control & SHDLC_CONTROL_M_MASK;
0489             llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);
0490             break;
0491         default:
0492             pr_err("UNKNOWN Control=%d\n", control);
0493             kfree_skb(skb);
0494             break;
0495         }
0496     }
0497 }
0498
0499 static int llc_shdlc_w_used(int ns, int dnr)
0500 {
0501     int unack_count;
0502
0503     if (dnr <= ns)
0504         unack_count = ns - dnr;
0505     else
0506         unack_count = 8 - dnr + ns;
0507
0508     return unack_count;
0509 }
0510
0511 /* Send frames according to algorithm at spec:10.8.1 */
0512 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)
0513 {
0514     struct sk_buff *skb;
0515     int r;
0516     unsigned long time_sent;
0517
0518     if (shdlc->send_q.qlen)
0519         pr_debug("sendQlen=%d ns=%d dnr=%d rnr=%s w_room=%d unackQlen=%d\n",
0520              shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,
0521              shdlc->rnr == false ? "false" : "true",
0522              shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),
0523              shdlc->ack_pending_q.qlen);
0524
0525     while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&
0526            (shdlc->rnr == false)) {
0527
0528         if (shdlc->t1_active) {
0529             del_timer_sync(&shdlc->t1_timer);
0530             shdlc->t1_active = false;
0531             pr_debug("Stopped T1(send ack)\n");
0532         }
0533
0534         skb = skb_dequeue(&shdlc->send_q);
0535
0536         *(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |
0537                     shdlc->nr;
0538
0539         pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,
0540              shdlc->nr);
0541         SHDLC_DUMP_SKB("shdlc frame written", skb);
0542
0543         r = shdlc->xmit_to_drv(shdlc->hdev, skb);
0544         if (r < 0) {
0545             shdlc->hard_fault = r;
0546             break;
0547         }
0548
0549         shdlc->ns = (shdlc->ns + 1) % 8;
0550
0551         time_sent = jiffies;
0552         *(unsigned long *)skb->cb = time_sent;
0553
0554         skb_queue_tail(&shdlc->ack_pending_q, skb);
0555
0556         if (shdlc->t2_active == false) {
0557             shdlc->t2_active = true;
0558             mod_timer(&shdlc->t2_timer, time_sent +
0559                   msecs_to_jiffies(SHDLC_T2_VALUE_MS));
0560             pr_debug("Started T2 (retransmit)\n");
0561         }
0562     }
0563 }
0564
0565 static void llc_shdlc_connect_timeout(struct timer_list *t)
0566 {
0567     struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);
0568
0569     schedule_work(&shdlc->sm_work);
0570 }
0571
0572 static void llc_shdlc_t1_timeout(struct timer_list *t)
0573 {
0574     struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);
0575
0576     pr_debug("SoftIRQ: need to send ack\n");
0577
0578     schedule_work(&shdlc->sm_work);
0579 }
0580
0581 static void llc_shdlc_t2_timeout(struct timer_list *t)
0582 {
0583     struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);
0584
0585     pr_debug("SoftIRQ: need to retransmit\n");
0586
0587     schedule_work(&shdlc->sm_work);
0588 }
0589
0590 static void llc_shdlc_sm_work(struct work_struct *work)
0591 {
0592     struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);
0593     int r;
0594
0595     mutex_lock(&shdlc->state_mutex);
0596
0597     switch (shdlc->state) {
0598     case SHDLC_DISCONNECTED:
0599         skb_queue_purge(&shdlc->rcv_q);
0600         skb_queue_purge(&shdlc->send_q);
0601         skb_queue_purge(&shdlc->ack_pending_q);
0602         break;
0603     case SHDLC_CONNECTING:
0604         if (shdlc->hard_fault) {
0605             llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
0606             break;
0607         }
0608
0609         if (shdlc->connect_tries++ < 5)
0610             r = llc_shdlc_connect_initiate(shdlc);
0611         else
0612             r = -ETIME;
0613         if (r < 0) {
0614             llc_shdlc_connect_complete(shdlc, r);
0615         } else {
0616             mod_timer(&shdlc->connect_timer, jiffies +
0617                   msecs_to_jiffies(SHDLC_CONNECT_VALUE_MS));
0618
0619             shdlc->state = SHDLC_NEGOTIATING;
0620         }
0621         break;
0622     case SHDLC_NEGOTIATING:
0623         if (timer_pending(&shdlc->connect_timer) == 0) {
0624             shdlc->state = SHDLC_CONNECTING;
0625             schedule_work(&shdlc->sm_work);
0626         }
0627
0628         llc_shdlc_handle_rcv_queue(shdlc);
0629
0630         if (shdlc->hard_fault) {
0631             llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);
0632             break;
0633         }
0634         break;
0635     case SHDLC_HALF_CONNECTED:
0636     case SHDLC_CONNECTED:
0637         llc_shdlc_handle_rcv_queue(shdlc);
0638         llc_shdlc_handle_send_queue(shdlc);
0639
0640         if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {
0641             pr_debug("Handle T1(send ack) elapsed (T1 now inactive)\n");
0642
0643             shdlc->t1_active = false;
0644             r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,
0645                            shdlc->nr);
0646             if (r < 0)
0647                 shdlc->hard_fault = r;
0648         }
0649
0650         if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {
0651             pr_debug("Handle T2(retransmit) elapsed (T2 inactive)\n");
0652
0653             shdlc->t2_active = false;
0654
0655             llc_shdlc_requeue_ack_pending(shdlc);
0656             llc_shdlc_handle_send_queue(shdlc);
0657         }
0658
0659         if (shdlc->hard_fault)
0660             shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);
0661         break;
0662     default:
0663         break;
0664     }
0665     mutex_unlock(&shdlc->state_mutex);
0666 }
0667
0668 /*
0669  * Called from syscall context to establish shdlc link. Sleeps until
0670  * link is ready or failure.
0671  */
0672 static int llc_shdlc_connect(struct llc_shdlc *shdlc)
0673 {
0674     DECLARE_WAIT_QUEUE_HEAD_ONSTACK(connect_wq);
0675
0676     mutex_lock(&shdlc->state_mutex);
0677
0678     shdlc->state = SHDLC_CONNECTING;
0679     shdlc->connect_wq = &connect_wq;
0680     shdlc->connect_tries = 0;
0681     shdlc->connect_result = 1;
0682
0683     mutex_unlock(&shdlc->state_mutex);
0684
0685     schedule_work(&shdlc->sm_work);
0686
0687     wait_event(connect_wq, shdlc->connect_result != 1);
0688
0689     return shdlc->connect_result;
0690 }
0691
0692 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)
0693 {
0694     mutex_lock(&shdlc->state_mutex);
0695
0696     shdlc->state = SHDLC_DISCONNECTED;
0697
0698     mutex_unlock(&shdlc->state_mutex);
0699
0700     schedule_work(&shdlc->sm_work);
0701 }
0702
0703 /*
0704  * Receive an incoming shdlc frame. Frame has already been crc-validated.
0705  * skb contains only LLC header and payload.
0706  * If skb == NULL, it is a notification that the link below is dead.
0707  */
0708 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)
0709 {
0710     if (skb == NULL) {
0711         pr_err("NULL Frame -> link is dead\n");
0712         shdlc->hard_fault = -EREMOTEIO;
0713     } else {
0714         SHDLC_DUMP_SKB("incoming frame", skb);
0715         skb_queue_tail(&shdlc->rcv_q, skb);
0716     }
0717
0718     schedule_work(&shdlc->sm_work);
0719 }
0720
0721 static void *llc_shdlc_init(struct nfc_hci_dev *hdev, xmit_to_drv_t xmit_to_drv,
0722                 rcv_to_hci_t rcv_to_hci, int tx_headroom,
0723                 int tx_tailroom, int *rx_headroom, int *rx_tailroom,
0724                 llc_failure_t llc_failure)
0725 {
0726     struct llc_shdlc *shdlc;
0727
0728     *rx_headroom = SHDLC_LLC_HEAD_ROOM;
0729     *rx_tailroom = 0;
0730
0731     shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);
0732     if (shdlc == NULL)
0733         return NULL;
0734
0735     mutex_init(&shdlc->state_mutex);
0736     shdlc->state = SHDLC_DISCONNECTED;
0737
0738     timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0);
0739     timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0);
0740     timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0);
0741
0742     shdlc->w = SHDLC_MAX_WINDOW;
0743     shdlc->srej_support = SHDLC_SREJ_SUPPORT;
0744
0745     skb_queue_head_init(&shdlc->rcv_q);
0746     skb_queue_head_init(&shdlc->send_q);
0747     skb_queue_head_init(&shdlc->ack_pending_q);
0748
0749     INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);
0750
0751     shdlc->hdev = hdev;
0752     shdlc->xmit_to_drv = xmit_to_drv;
0753     shdlc->rcv_to_hci = rcv_to_hci;
0754     shdlc->tx_headroom = tx_headroom;
0755     shdlc->tx_tailroom = tx_tailroom;
0756     shdlc->llc_failure = llc_failure;
0757
0758     return shdlc;
0759 }
0760
0761 static void llc_shdlc_deinit(struct nfc_llc *llc)
0762 {
0763     struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
0764
0765     skb_queue_purge(&shdlc->rcv_q);
0766     skb_queue_purge(&shdlc->send_q);
0767     skb_queue_purge(&shdlc->ack_pending_q);
0768
0769     kfree(shdlc);
0770 }
0771
0772 static int llc_shdlc_start(struct nfc_llc *llc)
0773 {
0774     struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
0775
0776     return llc_shdlc_connect(shdlc);
0777 }
0778
0779 static int llc_shdlc_stop(struct nfc_llc *llc)
0780 {
0781     struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
0782
0783     llc_shdlc_disconnect(shdlc);
0784
0785     return 0;
0786 }
0787
0788 static void llc_shdlc_rcv_from_drv(struct nfc_llc *llc, struct sk_buff *skb)
0789 {
0790     struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
0791
0792     llc_shdlc_recv_frame(shdlc, skb);
0793 }
0794
0795 static int llc_shdlc_xmit_from_hci(struct nfc_llc *llc, struct sk_buff *skb)
0796 {
0797     struct llc_shdlc *shdlc = nfc_llc_get_data(llc);
0798
0799     skb_queue_tail(&shdlc->send_q, skb);
0800
0801     schedule_work(&shdlc->sm_work);
0802
0803     return 0;
0804 }
0805
0806 static const struct nfc_llc_ops llc_shdlc_ops = {
0807     .init = llc_shdlc_init,
0808     .deinit = llc_shdlc_deinit,
0809     .start = llc_shdlc_start,
0810     .stop = llc_shdlc_stop,
0811     .rcv_from_drv = llc_shdlc_rcv_from_drv,
0812     .xmit_from_hci = llc_shdlc_xmit_from_hci,
0813 };
0814
0815 int nfc_llc_shdlc_register(void)
0816 {
0817     return nfc_llc_register(LLC_SHDLC_NAME, &llc_shdlc_ops);
0818 }