NFC: llcp: Aggregated frames support

This adds support for AGF PDUs. For each PDU contained in the AGF, a new sk_buff
is allocated and dispatched to its corresponding handler.

Signed-off-by: Thierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 7de0368..79de8ba 100644 (file)
--- a/net/nfc/llcp/llcp.c
+++ b/net/nfc/llcp/llcp.c
@@ -31,6 +31,8 @@ static u8 llcp_magic[3] = {0x46, 0x66, 0x6d};
 
 static struct list_head llcp_devices;
 
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb);
+
 void nfc_llcp_sock_link(struct llcp_sock_list *l, struct sock *sk)
 {
        write_lock(&l->lock);
@@ -1349,19 +1351,54 @@ exit:
                nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);
 }
 
-static void nfc_llcp_rx_work(struct work_struct *work)
+static void nfc_llcp_recv_agf(struct nfc_llcp_local *local, struct sk_buff *skb)
 {
-       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
-                                                   rx_work);
-       u8 dsap, ssap, ptype;
-       struct sk_buff *skb;
+       u8 ptype;
+       u16 pdu_len;
+       struct sk_buff *new_skb;
 
-       skb = local->rx_pending;
-       if (skb == NULL) {
-               pr_debug("No pending SKB\n");
+       if (skb->len <= LLCP_HEADER_SIZE) {
+               pr_err("Malformed AGF PDU\n");
                return;
        }
 
+       skb_pull(skb, LLCP_HEADER_SIZE);
+
+       while (skb->len > LLCP_AGF_PDU_HEADER_SIZE) {
+               pdu_len = skb->data[0] << 8 | skb->data[1];
+
+               skb_pull(skb, LLCP_AGF_PDU_HEADER_SIZE);
+
+               if (pdu_len < LLCP_HEADER_SIZE || pdu_len > skb->len) {
+                       pr_err("Malformed AGF PDU\n");
+                       return;
+               }
+
+               ptype = nfc_llcp_ptype(skb);
+
+               if (ptype == LLCP_PDU_SYMM || ptype == LLCP_PDU_AGF)
+                       goto next;
+
+               new_skb = nfc_alloc_recv_skb(pdu_len, GFP_KERNEL);
+               if (new_skb == NULL) {
+                       pr_err("Could not allocate PDU\n");
+                       return;
+               }
+
+               memcpy(skb_put(new_skb, pdu_len), skb->data, pdu_len);
+
+               nfc_llcp_rx_skb(local, new_skb);
+
+               kfree_skb(new_skb);
+next:
+               skb_pull(skb, pdu_len);
+       }
+}
+
+static void nfc_llcp_rx_skb(struct nfc_llcp_local *local, struct sk_buff *skb)
+{
+       u8 dsap, ssap, ptype;
+
        ptype = nfc_llcp_ptype(skb);
        dsap = nfc_llcp_dsap(skb);
        ssap = nfc_llcp_ssap(skb);
@@ -1372,10 +1409,6 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                print_hex_dump(KERN_DEBUG, "LLCP Rx: ", DUMP_PREFIX_OFFSET,
                               16, 1, skb->data, skb->len, true);
 
-       __net_timestamp(skb);
-
-       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
-
        switch (ptype) {
        case LLCP_PDU_SYMM:
                pr_debug("SYMM\n");
@@ -1418,7 +1451,30 @@ static void nfc_llcp_rx_work(struct work_struct *work)
                nfc_llcp_recv_hdlc(local, skb);
                break;
 
+       case LLCP_PDU_AGF:
+               pr_debug("AGF frame\n");
+               nfc_llcp_recv_agf(local, skb);
+               break;
        }
+}
+
+static void nfc_llcp_rx_work(struct work_struct *work)
+{
+       struct nfc_llcp_local *local = container_of(work, struct nfc_llcp_local,
+                                                   rx_work);
+       struct sk_buff *skb;
+
+       skb = local->rx_pending;
+       if (skb == NULL) {
+               pr_debug("No pending SKB\n");
+               return;
+       }
+
+       __net_timestamp(skb);
+
+       nfc_llcp_send_to_raw_sock(local, skb, NFC_LLCP_DIRECTION_RX);
+
+       nfc_llcp_rx_skb(local, skb);
 
        schedule_work(&local->tx_work);
        kfree_skb(local->rx_pending);

diff --git a/net/nfc/llcp/llcp.h b/net/nfc/llcp/llcp.h
index 53054d3..6dfde1e 100644 (file)
--- a/net/nfc/llcp/llcp.h
+++ b/net/nfc/llcp/llcp.h
@@ -162,6 +162,7 @@ struct nfc_llcp_ui_cb {
 
 #define LLCP_HEADER_SIZE   2
 #define LLCP_SEQUENCE_SIZE 1
+#define LLCP_AGF_PDU_HEADER_SIZE 2
 
 /* LLCP versions: 1.1 is 1.0 plus SDP */
 #define LLCP_VERSION_10 0x10