NFC: Fragment LLCP I frames

Based on the receiver MIU, we have to fragment the frame to be transmitted. Signed-off-by: Samuel Ortiz <sameo@linux.intel.com> Signed-off-by: John W. Linville <linville@tuxdriver.com>

NFC: Fragment LLCP I frames
Based on the receiver MIU, we have to fragment the frame to be transmitted. Signed-off-by: Samuel Ortiz <sameo@linux.intel.com> Signed-off-by: John W. Linville <linville@tuxdriver.com>
Samuel Ortiz · John W. Linville
1 parent eda21f16a5
Showing 1 changed file with 43 additions and 14 deletions Side-by-side Diff
net/nfc/llcp/commands.c
@@ -444,29 +444,58 @@
 				struct msghdr *msg, size_t len)
 {
 	struct sk_buff *pdu;
-	struct sock *sk;
+	struct sock *sk = &sock->sk;
+	struct nfc_llcp_local *local;
+	size_t frag_len = 0, remaining_len;
+	u8 *msg_data, *msg_ptr;
  
-	pr_debug("Send I frame\n");
+	pr_debug("Send I frame len %zd\n", len);
  
-	pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, len + LLCP_SEQUENCE_SIZE);
-	if (pdu == NULL)
+	local = sock->local;
+	if (local == NULL)
+		return -ENODEV;
+
+	msg_data = kzalloc(len, GFP_KERNEL);
+	if (msg_data == NULL)
 		return -ENOMEM;
  
-	skb_put(pdu, LLCP_SEQUENCE_SIZE);
-
-	if (memcpy_fromiovec(skb_put(pdu, len), msg->msg_iov, len)) {
-		kfree_skb(pdu);
-		return -EFAULT;
+	if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
+			kfree(msg_data);
+			return -EFAULT;
 	}
  
-	skb_queue_head(&sock->tx_queue, pdu);
+	remaining_len = len;
+	msg_ptr = msg_data;
  
-	sk = &sock->sk;
-	lock_sock(sk);
+	while (remaining_len > 0) {
  
-	nfc_llcp_queue_i_frames(sock);
+		frag_len = min_t(u16, local->remote_miu, remaining_len);
  
-	release_sock(sk);
+		pr_debug("Fragment %zd bytes remaining %zd",
+			 frag_len, remaining_len);
+
+		pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
+					frag_len + LLCP_SEQUENCE_SIZE);
+		if (pdu == NULL)
+			return -ENOMEM;
+
+		skb_put(pdu, LLCP_SEQUENCE_SIZE);
+
+		memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
+
+		skb_queue_head(&sock->tx_queue, pdu);
+
+		lock_sock(sk);
+
+		nfc_llcp_queue_i_frames(sock);
+
+		release_sock(sk);
+
+		remaining_len -= frag_len;
+		msg_ptr += len;
+	}
+
+	kfree(msg_data);
  
 	return 0;
 }
...	...	@@ -444,29 +444,58 @@
444	444	struct msghdr *msg, size_t len)
445	445	{
446	446	struct sk_buff *pdu;
447		- struct sock *sk;
	447	+ struct sock *sk = &sock->sk;
	448	+ struct nfc_llcp_local *local;
	449	+ size_t frag_len = 0, remaining_len;
	450	+ u8 msg_data, msg_ptr;
448	451
449		- pr_debug("Send I frame\n");
	452	+ pr_debug("Send I frame len %zd\n", len);
450	453
451		- pdu = llcp_allocate_pdu(sock, LLCP_PDU_I, len + LLCP_SEQUENCE_SIZE);
452		- if (pdu == NULL)
	454	+ local = sock->local;
	455	+ if (local == NULL)
	456	+ return -ENODEV;
	457	+
	458	+ msg_data = kzalloc(len, GFP_KERNEL);
	459	+ if (msg_data == NULL)
453	460	return -ENOMEM;
454	461
455		- skb_put(pdu, LLCP_SEQUENCE_SIZE);
456		-
457		- if (memcpy_fromiovec(skb_put(pdu, len), msg->msg_iov, len)) {
458		- kfree_skb(pdu);
459		- return -EFAULT;
	462	+ if (memcpy_fromiovec(msg_data, msg->msg_iov, len)) {
	463	+ kfree(msg_data);
	464	+ return -EFAULT;
460	465	}
461	466
462		- skb_queue_head(&sock->tx_queue, pdu);
	467	+ remaining_len = len;
	468	+ msg_ptr = msg_data;
463	469
464		- sk = &sock->sk;
465		- lock_sock(sk);
	470	+ while (remaining_len > 0) {
466	471
467		- nfc_llcp_queue_i_frames(sock);
	472	+ frag_len = min_t(u16, local->remote_miu, remaining_len);
468	473
469		- release_sock(sk);
	474	+ pr_debug("Fragment %zd bytes remaining %zd",
	475	+ frag_len, remaining_len);
	476	+
	477	+ pdu = llcp_allocate_pdu(sock, LLCP_PDU_I,
	478	+ frag_len + LLCP_SEQUENCE_SIZE);
	479	+ if (pdu == NULL)
	480	+ return -ENOMEM;
	481	+
	482	+ skb_put(pdu, LLCP_SEQUENCE_SIZE);
	483	+
	484	+ memcpy(skb_put(pdu, frag_len), msg_ptr, frag_len);
	485	+
	486	+ skb_queue_head(&sock->tx_queue, pdu);
	487	+
	488	+ lock_sock(sk);
	489	+
	490	+ nfc_llcp_queue_i_frames(sock);
	491	+
	492	+ release_sock(sk);
	493	+
	494	+ remaining_len -= frag_len;
	495	+ msg_ptr += len;
	496	+ }
	497	+
	498	+ kfree(msg_data);
470	499
471	500	return 0;
472	501	}