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>
This commit is contained in:
Thierry Escande 2013-03-29 11:47:43 +01:00 committed by Samuel Ortiz
parent 0b23d666a8
commit 098dafcfb4
2 changed files with 69 additions and 12 deletions

View file

@ -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 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local,
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);

View file

@ -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