[RFC] gadget ethernet dma align fix

[matthieu castet <castet.matthieu@xxxxxxx>]

Hi,

I attach a patch that should allow to use gadget ethernet with driver
not supporting dma on unaligned address.

This is untested on real hardware.


Merry Christmas


Matthieu

diff --git a/drivers/usb/gadget/gadget_chips.h b/drivers/usb/gadget/gadget_chips.h
index f2d270b..0b5bf5f 100644
--- a/drivers/usb/gadget/gadget_chips.h
+++ b/drivers/usb/gadget/gadget_chips.h
@@ -273,4 +273,14 @@ static inline bool gadget_supports_altsettings(struct usb_gadget *gadget)
 	return true;
 }
 
+/**
+ * gadget_dma32 - return true if we want buffer aligned on 32 bits (for dma)
+ * @gadget: the gadget in question
+ */
+static inline bool gadget_dma32(struct usb_gadget *gadget)
+{
+	if (gadget_is_musbhdrc(gadget))
+		return true;
+	return false
+}
 #endif /* __GADGET_CHIPS_H */
diff --git a/drivers/usb/gadget/u_ether.c b/drivers/usb/gadget/u_ether.c
index 2fc02bd..a259f4d 100644
--- a/drivers/usb/gadget/u_ether.c
+++ b/drivers/usb/gadget/u_ether.c
@@ -249,7 +249,8 @@ rx_submit(struct eth_dev *dev, struct usb_request *req, gfp_t gfp_flags)
 	 * but on at least one, checksumming fails otherwise.  Note:
 	 * RNDIS headers involve variable numbers of LE32 values.
 	 */
-	skb_reserve(skb, NET_IP_ALIGN);
+	if (!gadget_dma32(dev->gadget))
+		skb_reserve(skb, NET_IP_ALIGN);
 
 	req->buf = skb->data;
 	req->length = size;
@@ -282,6 +283,13 @@ static void rx_complete(struct usb_ep *ep, struct usb_request *req)
 	/* normal completion */
 	case 0:
 		skb_put(skb, req->actual);
+		if (gadget_dma32(dev->gadget) && NET_IP_ALIGN) {
+			u8 *data = skb->data;
+			size_t len = skb_headlen(skb);
+			skb_put(skb, NET_IP_ALIGN);
+			skb->data += NET_IP_ALIGN;
+			memmove(skb->data, data, len);
+		}
 
 		if (dev->unwrap) {
 			unsigned long	flags;
@@ -573,6 +581,19 @@ static netdev_tx_t eth_start_xmit(struct sk_buff *skb,
 
 		length = skb->len;
 	}
+	if (gadget_dma32(dev->gadget) && skb->data & 3) {
+		if (WARN_ON(skb_headroom(skb) < 3)) {
+			dev_kfree_skb_any(skb);
+			goto drop;
+		}
+		else {
+			u8 *data = skb->data;
+			size_t len = skb_headlen(skb);
+			skb->data -= skb->data & 3;
+			memmove(skb->data, data, len);
+			skb_set_tail_pointer(skb, len);
+		}
+	}
 	req->buf = skb->data;
 	req->context = skb;
 	req->complete = tx_complete;