1 files changed, 47 insertions, 0 deletions

diff --git a/drivers/net/fs_enet/fs_enet-main.c b/drivers/net/fs_enet/fs_enet-main.c
index 4297021214d1..0770e2f6da6b 100644
--- a/drivers/net/fs_enet/fs_enet-main.c
+++ b/drivers/net/fs_enet/fs_enet-main.c
@@ -580,6 +580,40 @@ void fs_cleanup_bds(struct net_device *dev)
 
 /**********************************************************************************/
 
+#ifdef CONFIG_FS_ENET_MPC5121_FEC
+/*
+ * MPC5121 FEC requeries 4-byte alignment for TX data buffer!
+ */
+static struct sk_buff *tx_skb_align_workaround(struct net_device *dev,
+                                               struct sk_buff *skb)
+{
+        struct sk_buff *new_skb;
+        struct fs_enet_private *fep = netdev_priv(dev);
+
+        /* Alloc new skb */
+        new_skb = dev_alloc_skb(skb->len + 4);
+        if (!new_skb) {
+                if (net_ratelimit()) {
+                        dev_warn(fep->dev,
+                                 "Memory squeeze, dropping tx packet.\n");
+                }
+                return NULL;
+        }
+
+        /* Make sure new skb is properly aligned */
+        skb_align(new_skb, 4);
+
+        /* Copy data to new skb ... */
+        skb_copy_from_linear_data(skb, new_skb->data, skb->len);
+        skb_put(new_skb, skb->len);
+
+        /* ... and free an old one */
+        dev_kfree_skb_any(skb);
+
+        return new_skb;
+}
+#endif
+
 static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
 {
         struct fs_enet_private *fep = netdev_priv(dev);
@@ -588,6 +622,19 @@ static int fs_enet_start_xmit(struct sk_buff *skb, struct net_device *dev)
         u16 sc;
         unsigned long flags;
 
+#ifdef CONFIG_FS_ENET_MPC5121_FEC
+        if (((unsigned long)skb->data) & 0x3) {
+                skb = tx_skb_align_workaround(dev, skb);
+                if (!skb) {
+                        /*
+                         * We have lost packet due to memory allocation error
+                         * in tx_skb_align_workaround(). Hopefully original
+                         * skb is still valid, so try transmit it later.
+                         */
+                        return NETDEV_TX_BUSY;
+                }
+        }
+#endif
         spin_lock_irqsave(&fep->tx_lock, flags);
 
         /*