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>

2 files changed, 69 insertions, 12 deletions

diff --git a/net/nfc/llcp/llcp.c b/net/nfc/llcp/llcp.c
index 7de0368aff0c..79de8bafd426 100644
--- 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 53054d337bf9..6dfde1ed648f 100644
--- 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