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

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

NFC: llcp: Service Name Lookup SDRES aggregation 



This modifies the way SDRES PDUs are sent back. If multiple SDREQs are
received within a single SNL PDU, all SDRES replies are sent packed in
one SNL PDU too.

Signed-off-by: default avatarThierry Escande <thierry.escande@linux.intel.com>
Signed-off-by: default avatarSamuel Ortiz <sameo@linux.intel.com>
parent 8af362d1

net/nfc/llcp/commands.c

+61 −21
Original line number Diff line number Diff line
@@ -117,6 +117,39 @@ u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length) 
	return tlv;

}



struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap)

{

	struct nfc_llcp_sdp_tlv *sdres;

	u8 value[2];



	sdres = kzalloc(sizeof(struct nfc_llcp_sdp_tlv), GFP_KERNEL);

	if (sdres == NULL)

		return NULL;



	value[0] = tid;

	value[1] = sap;



	sdres->tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, value, 2,

					&sdres->tlv_len);

	if (sdres->tlv == NULL) {

		kfree(sdres);

		return NULL;

	}



	sdres->tid = tid;

	sdres->sap = sap;



	INIT_HLIST_NODE(&sdres->node);



	return sdres;

}



void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp)

{

	kfree(sdp->tlv);

	kfree(sdp);

}



int nfc_llcp_parse_gb_tlv(struct nfc_llcp_local *local,

			  u8 *tlv_array, u16 tlv_array_len)

{
@@ -425,48 +458,55 @@ int nfc_llcp_send_cc(struct nfc_llcp_sock *sock) 
	return err;

}



int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap)

static struct sk_buff *nfc_llcp_allocate_snl(struct nfc_llcp_local *local,

					     size_t tlv_length)

{

	struct sk_buff *skb;

	struct nfc_dev *dev;

	u8 *sdres_tlv = NULL, sdres_tlv_length, sdres[2];

	u16 size = 0;



	pr_debug("Sending SNL tid 0x%x sap 0x%x\n", tid, sap);



	if (local == NULL)

		return -ENODEV;

		return ERR_PTR(-ENODEV);



	dev = local->dev;

	if (dev == NULL)

		return -ENODEV;



	sdres[0] = tid;

	sdres[1] = sap;

	sdres_tlv = nfc_llcp_build_tlv(LLCP_TLV_SDRES, sdres, 0,

				       &sdres_tlv_length);

	if (sdres_tlv == NULL)

		return -ENOMEM;

		return ERR_PTR(-ENODEV);



	size += LLCP_HEADER_SIZE;

	size += dev->tx_headroom + dev->tx_tailroom + NFC_HEADER_SIZE;

	size += sdres_tlv_length;

	size += tlv_length;



	skb = alloc_skb(size, GFP_KERNEL);

	if (skb == NULL) {

		kfree(sdres_tlv);

		return -ENOMEM;

	}

	if (skb == NULL)

		return ERR_PTR(-ENOMEM);



	skb_reserve(skb, dev->tx_headroom + NFC_HEADER_SIZE);



	skb = llcp_add_header(skb, LLCP_SAP_SDP, LLCP_SAP_SDP, LLCP_PDU_SNL);



	memcpy(skb_put(skb, sdres_tlv_length), sdres_tlv, sdres_tlv_length);

	return skb;

}



	skb_queue_tail(&local->tx_queue, skb);

int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,

			    struct hlist_head *tlv_list, size_t tlvs_len)

{

	struct nfc_llcp_sdp_tlv *sdp;

	struct hlist_node *n;

	struct sk_buff *skb;



	skb = nfc_llcp_allocate_snl(local, tlvs_len);

	if (IS_ERR(skb))

		return PTR_ERR(skb);



	hlist_for_each_entry_safe(sdp, n, tlv_list, node) {

		memcpy(skb_put(skb, sdp->tlv_len), sdp->tlv, sdp->tlv_len);



	kfree(sdres_tlv);

		hlist_del(&sdp->node);



		nfc_llcp_free_sdp_tlv(sdp);

	}



	skb_queue_tail(&local->tx_queue, skb);



	return 0;

}

net/nfc/llcp/llcp.c

+18 −5
Original line number Diff line number Diff line
@@ -1144,6 +1144,9 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 
	u16 tlv_len, offset;

	char *service_name;

	size_t service_name_len;

	struct nfc_llcp_sdp_tlv *sdp;

	HLIST_HEAD(llc_sdres_list);

	size_t sdres_tlvs_len;



	dsap = nfc_llcp_dsap(skb);

	ssap = nfc_llcp_ssap(skb);
@@ -1158,6 +1161,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 
	tlv = &skb->data[LLCP_HEADER_SIZE];

	tlv_len = skb->len - LLCP_HEADER_SIZE;

	offset = 0;

	sdres_tlvs_len = 0;



	while (offset < tlv_len) {

		type = tlv[0];
@@ -1175,14 +1179,14 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 
			    !strncmp(service_name, "urn:nfc:sn:sdp",

				     service_name_len)) {

				sap = 1;

				goto send_snl;

				goto add_snl;

			}



			llcp_sock = nfc_llcp_sock_from_sn(local, service_name,

							  service_name_len);

			if (!llcp_sock) {

				sap = 0;

				goto send_snl;

				goto add_snl;

			}



			/*
@@ -1199,7 +1203,7 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 


				if (sap == LLCP_SAP_MAX) {

					sap = 0;

					goto send_snl;

					goto add_snl;

				}



				client_count =
@@ -1216,8 +1220,13 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 


			pr_debug("%p %d\n", llcp_sock, sap);



send_snl:

			nfc_llcp_send_snl(local, tid, sap);

add_snl:

			sdp = nfc_llcp_build_sdres_tlv(tid, sap);

			if (sdp == NULL)

				goto exit;



			sdres_tlvs_len += sdp->tlv_len;

			hlist_add_head(&sdp->node, &llc_sdres_list);

			break;



		default:
@@ -1228,6 +1237,10 @@ static void nfc_llcp_recv_snl(struct nfc_llcp_local *local, 
		offset += length + 2;

		tlv += length + 2;

	}



exit:

	if (!hlist_empty(&llc_sdres_list))

		nfc_llcp_send_snl_sdres(local, &llc_sdres_list, sdres_tlvs_len);

}



static void nfc_llcp_rx_work(struct work_struct *work)

net/nfc/llcp/llcp.h

+15 −1
Original line number Diff line number Diff line
@@ -46,6 +46,17 @@ struct llcp_sock_list { 
	rwlock_t          lock;

};



struct nfc_llcp_sdp_tlv {

	u8 *tlv;

	u8 tlv_len;



	char *uri;

	u8 tid;

	u8 sap;



	struct hlist_node node;

};



struct nfc_llcp_local {

	struct list_head list;

	struct nfc_dev *dev;
@@ -218,12 +229,15 @@ int nfc_llcp_parse_connection_tlv(struct nfc_llcp_sock *sock, 
/* Commands API */

void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);

u8 *nfc_llcp_build_tlv(u8 type, u8 *value, u8 value_length, u8 *tlv_length);

struct nfc_llcp_sdp_tlv *nfc_llcp_build_sdres_tlv(u8 tid, u8 sap);

void nfc_llcp_free_sdp_tlv(struct nfc_llcp_sdp_tlv *sdp);

void nfc_llcp_recv(void *data, struct sk_buff *skb, int err);

int nfc_llcp_disconnect(struct nfc_llcp_sock *sock);

int nfc_llcp_send_symm(struct nfc_dev *dev);

int nfc_llcp_send_connect(struct nfc_llcp_sock *sock);

int nfc_llcp_send_cc(struct nfc_llcp_sock *sock);

int nfc_llcp_send_snl(struct nfc_llcp_local *local, u8 tid, u8 sap);

int nfc_llcp_send_snl_sdres(struct nfc_llcp_local *local,

			    struct hlist_head *tlv_list, size_t tlvs_len);

int nfc_llcp_send_dm(struct nfc_llcp_local *local, u8 ssap, u8 dsap, u8 reason);

int nfc_llcp_send_disconnect(struct nfc_llcp_sock *sock);

int nfc_llcp_send_i_frame(struct nfc_llcp_sock *sock,