ENGR00299323-10 net:fec: add enet AVB Ubuntu Gstreamer demo support

Support Gstreamer AVB demo support.

ring1 -> ClassA, ring2 -> ClassB, ring0 -> Best Effort

For QoS: ring1 > ring2 > ring0
For bandwidth reverse:
      50% bandwidth -> ClassA
      33% bandwidth -> ClassB
      17% bandwidth -> Best effort queue

In general, ClassA run audio, ClassB run video.
Since AVB demo use big bandwidth streaming, video cost more than
33Mbps bandwidth, and with Qos limitation: ClassA >= ClassB > Best effort,
so we have to change ring2 bandwidth equal to ring1 bandwidth (50%).
After validate on FPGA, AVB demo can work fine for audio and video.

Signed-off-by: Fugang Duan <B38611@freescale.com>
(cherry picked from commit 93d6579a7b3d2dafa721c835df5d5f7d30ed386e)

diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c
index 3bcae1e9f5ff11b22670802737d18d2ccf36a9cf..47a4c5421126a11da47c536fbf8dd0ec5d070439 100644 (file)
--- a/drivers/net/ethernet/freescale/fec_main.c
+++ b/drivers/net/ethernet/freescale/fec_main.c
@@ -18,7 +18,7 @@
  * Bug fixes and cleanup by Philippe De Muyter (phdm@macqel.be)
  * Copyright (c) 2004-2006 Macq Electronique SA.
  *
- * Copyright (C) 2010-2011 Freescale Semiconductor, Inc.
+ * Copyright (C) 2010-2014 Freescale Semiconductor, Inc.
  */
 
 #include <linux/module.h>
@@ -69,6 +69,7 @@ static void fec_enet_itr_coal_init(struct net_device *ndev);
 #define DRIVER_NAME    "fec"
 
 #define FEC_ENET_GET_QUQUE(_x) ((_x == 0) ? 1 : ((_x == 1) ? 2 : 0))
+static const u16 fec_enet_vlan_pri_to_queue[8] = {1, 1, 1, 1, 2, 2, 2, 2};
 
 /* Pause frame feild and FIFO threshold */
 #define FEC_ENET_FCE   (1 << 5)
@@ -2983,10 +2984,42 @@ static int fec_set_features(struct net_device *netdev,
        return 0;
 }
 
+u16 fec_enet_get_raw_vlan_tci(struct sk_buff *skb)
+{
+       struct vlan_ethhdr *vhdr;
+       unsigned short vlan_TCI = 0;
+
+       if (skb->protocol == ntohs(ETH_P_ALL)) {
+               vhdr = (struct vlan_ethhdr *)(skb->data);
+               vlan_TCI = ntohs(vhdr->h_vlan_TCI);
+       }
+
+       return vlan_TCI;
+}
+
+u16 fec_enet_select_queue(struct net_device *ndev, struct sk_buff *skb,
+                         void *accel_priv, select_queue_fallback_t fallback)
+{
+       struct fec_enet_private *fep = netdev_priv(ndev);
+       const struct platform_device_id *id_entry =
+                       platform_get_device_id(fep->pdev);
+       u16 vlan_tag;
+
+       if (!(id_entry->driver_data & FEC_QUIRK_HAS_AVB))
+               return skb_tx_hash(ndev, skb);
+
+       vlan_tag = fec_enet_get_raw_vlan_tci(skb);
+       if (!vlan_tag)
+               return vlan_tag;
+
+       return  fec_enet_vlan_pri_to_queue[vlan_tag >> 13];
+}
+
 static const struct net_device_ops fec_netdev_ops = {
        .ndo_open               = fec_enet_open,
        .ndo_stop               = fec_enet_close,
        .ndo_start_xmit         = fec_enet_start_xmit,
+       .ndo_select_queue       = fec_enet_select_queue,
        .ndo_set_rx_mode        = set_multicast_list,
        .ndo_change_mtu         = eth_change_mtu,
        .ndo_validate_addr      = eth_validate_addr,