Merge git://git.kernel.org/pub/scm/linux/kernel/git/davem/ide

[karo-tx-linux.git] / drivers / net / phy / dp83640.c

diff --git a/drivers/net/phy/dp83640.c b/drivers/net/phy/dp83640.c
index 185b03c08e16ce9140b2b0d91abdfacd5b9258a1..47b711739ba902cd300f53489a8d00aa87d1a660 100644 (file)
--- a/drivers/net/phy/dp83640.c
+++ b/drivers/net/phy/dp83640.c
@@ -20,6 +20,7 @@
 
 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
 
+#include <linux/crc32.h>
 #include <linux/ethtool.h>
 #include <linux/kernel.h>
 #include <linux/list.h>
@@ -36,8 +37,6 @@
 
 #define DP83640_PHY_ID 0x20005ce1
 #define PAGESEL                0x13
-#define LAYER4         0x02
-#define LAYER2         0x01
 #define MAX_RXTS       64
 #define N_EXT_TS       6
 #define N_PER_OUT      7
@@ -68,6 +67,8 @@
 /* phyter seems to miss the mark by 16 ns */
 #define ADJTIME_FIX    16
 
+#define SKB_TIMESTAMP_TIMEOUT  2 /* jiffies */
+
 #if defined(__BIG_ENDIAN)
 #define ENDIAN_FLAG    0
 #elif defined(__LITTLE_ENDIAN)
@@ -110,7 +111,7 @@ struct dp83640_private {
        struct list_head list;
        struct dp83640_clock *clock;
        struct phy_device *phydev;
-       struct work_struct ts_work;
+       struct delayed_work ts_work;
        int hwts_tx_en;
        int hwts_rx_en;
        int layer;
@@ -284,7 +285,7 @@ static void phy2rxts(struct phy_rxts *p, struct rxts *rxts)
        rxts->seqid = p->seqid;
        rxts->msgtype = (p->msgtype >> 12) & 0xf;
        rxts->hash = p->msgtype & 0x0fff;
-       rxts->tmo = jiffies + 2;
+       rxts->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
 }
 
 static u64 phy2txts(struct phy_txts *p)
@@ -787,9 +788,12 @@ static int decode_evnt(struct dp83640_private *dp83640,
        return parsed;
 }
 
+#define DP83640_PACKET_HASH_OFFSET     20
+#define DP83640_PACKET_HASH_LEN                10
+
 static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
 {
-       u16 *seqid;
+       u16 *seqid, hash;
        unsigned int offset = 0;
        u8 *msgtype, *data = skb_mac_header(skb);
 
@@ -819,11 +823,19 @@ static int match(struct sk_buff *skb, unsigned int type, struct rxts *rxts)
                msgtype = data + offset + OFF_PTP_CONTROL;
        else
                msgtype = data + offset;
+       if (rxts->msgtype != (*msgtype & 0xf))
+               return 0;
 
        seqid = (u16 *)(data + offset + OFF_PTP_SEQUENCE_ID);
+       if (rxts->seqid != ntohs(*seqid))
+               return 0;
+
+       hash = ether_crc(DP83640_PACKET_HASH_LEN,
+                        data + offset + DP83640_PACKET_HASH_OFFSET) >> 20;
+       if (rxts->hash != hash)
+               return 0;
 
-       return rxts->msgtype == (*msgtype & 0xf) &&
-               rxts->seqid   == ntohs(*seqid);
+       return 1;
 }
 
 static void decode_rxts(struct dp83640_private *dp83640,
@@ -1103,7 +1115,7 @@ static int dp83640_probe(struct phy_device *phydev)
                goto no_memory;
 
        dp83640->phydev = phydev;
-       INIT_WORK(&dp83640->ts_work, rx_timestamp_work);
+       INIT_DELAYED_WORK(&dp83640->ts_work, rx_timestamp_work);
 
        INIT_LIST_HEAD(&dp83640->rxts);
        INIT_LIST_HEAD(&dp83640->rxpool);
@@ -1150,7 +1162,7 @@ static void dp83640_remove(struct phy_device *phydev)
                return;
 
        enable_status_frames(phydev, false);
-       cancel_work_sync(&dp83640->ts_work);
+       cancel_delayed_work_sync(&dp83640->ts_work);
 
        skb_queue_purge(&dp83640->rx_queue);
        skb_queue_purge(&dp83640->tx_queue);
@@ -1282,29 +1294,29 @@ static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
        case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
        case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
                dp83640->hwts_rx_en = 1;
-               dp83640->layer = LAYER4;
-               dp83640->version = 1;
+               dp83640->layer = PTP_CLASS_L4;
+               dp83640->version = PTP_CLASS_V1;
                break;
        case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
        case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
        case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
                dp83640->hwts_rx_en = 1;
-               dp83640->layer = LAYER4;
-               dp83640->version = 2;
+               dp83640->layer = PTP_CLASS_L4;
+               dp83640->version = PTP_CLASS_V2;
                break;
        case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
        case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
        case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
                dp83640->hwts_rx_en = 1;
-               dp83640->layer = LAYER2;
-               dp83640->version = 2;
+               dp83640->layer = PTP_CLASS_L2;
+               dp83640->version = PTP_CLASS_V2;
                break;
        case HWTSTAMP_FILTER_PTP_V2_EVENT:
        case HWTSTAMP_FILTER_PTP_V2_SYNC:
        case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
                dp83640->hwts_rx_en = 1;
-               dp83640->layer = LAYER4|LAYER2;
-               dp83640->version = 2;
+               dp83640->layer = PTP_CLASS_L4 | PTP_CLASS_L2;
+               dp83640->version = PTP_CLASS_V2;
                break;
        default:
                return -ERANGE;
@@ -1313,11 +1325,11 @@ static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
        txcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
        rxcfg0 = (dp83640->version & TX_PTP_VER_MASK) << TX_PTP_VER_SHIFT;
 
-       if (dp83640->layer & LAYER2) {
+       if (dp83640->layer & PTP_CLASS_L2) {
                txcfg0 |= TX_L2_EN;
                rxcfg0 |= RX_L2_EN;
        }
-       if (dp83640->layer & LAYER4) {
+       if (dp83640->layer & PTP_CLASS_L4) {
                txcfg0 |= TX_IPV6_EN | TX_IPV4_EN;
                rxcfg0 |= RX_IPV6_EN | RX_IPV4_EN;
        }
@@ -1344,7 +1356,7 @@ static int dp83640_hwtstamp(struct phy_device *phydev, struct ifreq *ifr)
 static void rx_timestamp_work(struct work_struct *work)
 {
        struct dp83640_private *dp83640 =
-               container_of(work, struct dp83640_private, ts_work);
+               container_of(work, struct dp83640_private, ts_work.work);
        struct sk_buff *skb;
 
        /* Deliver expired packets. */
@@ -1361,7 +1373,7 @@ static void rx_timestamp_work(struct work_struct *work)
        }
 
        if (!skb_queue_empty(&dp83640->rx_queue))
-               schedule_work(&dp83640->ts_work);
+               schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
 }
 
 static bool dp83640_rxtstamp(struct phy_device *phydev,
@@ -1383,7 +1395,11 @@ static bool dp83640_rxtstamp(struct phy_device *phydev,
        if (!dp83640->hwts_rx_en)
                return false;
 
+       if ((type & dp83640->version) == 0 || (type & dp83640->layer) == 0)
+               return false;
+
        spin_lock_irqsave(&dp83640->rx_lock, flags);
+       prune_rx_ts(dp83640);
        list_for_each_safe(this, next, &dp83640->rxts) {
                rxts = list_entry(this, struct rxts, list);
                if (match(skb, type, rxts)) {
@@ -1400,9 +1416,11 @@ static bool dp83640_rxtstamp(struct phy_device *phydev,
 
        if (!shhwtstamps) {
                skb_info->ptp_type = type;
-               skb_info->tmo = jiffies + 2;
+               skb_info->tmo = jiffies + SKB_TIMESTAMP_TIMEOUT;
                skb_queue_tail(&dp83640->rx_queue, skb);
-               schedule_work(&dp83640->ts_work);
+               schedule_delayed_work(&dp83640->ts_work, SKB_TIMESTAMP_TIMEOUT);
+       } else {
+               netif_rx_ni(skb);
        }
 
        return true;