diff options
Diffstat (limited to 'drivers/net/ethernet/freescale')
| -rw-r--r-- | drivers/net/ethernet/freescale/fec_main.c | 32 |
1 files changed, 32 insertions, 0 deletions
diff --git a/drivers/net/ethernet/freescale/fec_main.c b/drivers/net/ethernet/freescale/fec_main.c index 98cd38379275..8aea707a65a7 100644 --- a/drivers/net/ethernet/freescale/fec_main.c +++ b/drivers/net/ethernet/freescale/fec_main.c @@ -76,6 +76,8 @@ static void fec_enet_itr_coal_init(struct net_device *ndev); #define DRIVER_NAME "fec" +static const u16 fec_enet_vlan_pri_to_queue[8] = {0, 0, 1, 1, 1, 2, 2, 2}; + /* Pause frame feild and FIFO threshold */ #define FEC_ENET_FCE (1 << 5) #define FEC_ENET_RSEM_V 0x84 @@ -3240,10 +3242,40 @@ static int fec_set_features(struct net_device *netdev, return 0; } +static u16 fec_enet_get_raw_vlan_tci(struct sk_buff *skb) +{ + struct vlan_ethhdr *vhdr; + unsigned short vlan_TCI = 0; + + if (skb->protocol == htons(ETH_P_ALL)) { + vhdr = (struct vlan_ethhdr *)(skb->data); + vlan_TCI = ntohs(vhdr->h_vlan_TCI); + } + + return vlan_TCI; +} + +static u16 fec_enet_select_queue(struct net_device *ndev, struct sk_buff *skb, + struct net_device *sb_dev) +{ + struct fec_enet_private *fep = netdev_priv(ndev); + u16 vlan_tag; + + if (!(fep->quirks & FEC_QUIRK_HAS_AVB)) + return netdev_pick_tx(ndev, skb, NULL); + + 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_validate_addr = eth_validate_addr, .ndo_tx_timeout = fec_timeout, |