Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 098dafcf authored by Thierry Escande's avatar Thierry Escande Committed by Samuel Ortiz
Browse files

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: default avatarThierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: default avatarSamuel Ortiz <sameo@linux.intel.com>
parent 0b23d666

net/nfc/llcp/llcp.c

+68 −12
Original line number Diff line number Diff line
@@ -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,8 +1451,31 @@ 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);

	local->rx_pending = NULL;

net/nfc/llcp/llcp.h

+1 −0
Original line number Diff line number Diff line
@@ -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