From: Sunil Goutham <sgout...@cavium.com>

This adds timestamping support for both receive and transmit
paths. On the receive side no filters are supported i.e either
all pkts will get a timestamp appended infront of the packet or none.
On the transmit side HW doesn't support timestamp insertion but
only generates a separate CQE with transmitted packet's timestamp.
Also HW supports only one packet at a time for timestamping on the
transmit side.

Signed-off-by: Sunil Goutham <sgout...@cavium.com>
Signed-off-by: Aleksey Makarov <aleksey.maka...@cavium.com>
---
 drivers/net/ethernet/cavium/Kconfig                |   1 +
 drivers/net/ethernet/cavium/thunder/nic.h          |  15 ++
 drivers/net/ethernet/cavium/thunder/nic_main.c     |  58 ++++++-
 drivers/net/ethernet/cavium/thunder/nic_reg.h      |   1 +
 .../net/ethernet/cavium/thunder/nicvf_ethtool.c    |  29 +++-
 drivers/net/ethernet/cavium/thunder/nicvf_main.c   | 173 ++++++++++++++++++++-
 drivers/net/ethernet/cavium/thunder/nicvf_queues.c |  26 ++++
 drivers/net/ethernet/cavium/thunder/thunder_bgx.c  |  29 ++++
 drivers/net/ethernet/cavium/thunder/thunder_bgx.h  |   4 +
 9 files changed, 330 insertions(+), 6 deletions(-)

diff --git a/drivers/net/ethernet/cavium/Kconfig 
b/drivers/net/ethernet/cavium/Kconfig
index 2380e9834007..d163c5bdbbcb 100644
--- a/drivers/net/ethernet/cavium/Kconfig
+++ b/drivers/net/ethernet/cavium/Kconfig
@@ -27,6 +27,7 @@ config THUNDER_NIC_PF
 
 config THUNDER_NIC_VF
        tristate "Thunder Virtual function driver"
+       select CAVIUM_PTP
        depends on 64BIT
        ---help---
          This driver supports Thunder's NIC virtual function
diff --git a/drivers/net/ethernet/cavium/thunder/nic.h 
b/drivers/net/ethernet/cavium/thunder/nic.h
index 4a02e618e318..204b234beb9d 100644
--- a/drivers/net/ethernet/cavium/thunder/nic.h
+++ b/drivers/net/ethernet/cavium/thunder/nic.h
@@ -263,6 +263,8 @@ struct nicvf_drv_stats {
        struct u64_stats_sync   syncp;
 };
 
+struct cavium_ptp;
+
 struct nicvf {
        struct nicvf            *pnicvf;
        struct net_device       *netdev;
@@ -312,6 +314,12 @@ struct nicvf {
        struct tasklet_struct   qs_err_task;
        struct work_struct      reset_task;
 
+       /* PTP timestamp */
+       struct cavium_ptp       *ptp_clock;
+       bool                    hw_rx_tstamp;
+       struct sk_buff          *ptp_skb;
+       atomic_t                tx_ptp_skbs;
+
        /* Interrupt coalescing settings */
        u32                     cq_coalesce_usecs;
        u32                     msg_enable;
@@ -371,6 +379,7 @@ struct nicvf {
 #define        NIC_MBOX_MSG_LOOPBACK           0x16    /* Set interface in 
loopback */
 #define        NIC_MBOX_MSG_RESET_STAT_COUNTER 0x17    /* Reset statistics 
counters */
 #define        NIC_MBOX_MSG_PFC                0x18    /* Pause frame control 
*/
+#define        NIC_MBOX_MSG_PTP_CFG            0x19    /* HW packet timestamp 
*/
 #define        NIC_MBOX_MSG_CFG_DONE           0xF0    /* VF configuration 
done */
 #define        NIC_MBOX_MSG_SHUTDOWN           0xF1    /* VF is being shutdown 
*/
 
@@ -521,6 +530,11 @@ struct pfc {
        u8    fc_tx;
 };
 
+struct set_ptp {
+       u8    msg;
+       bool  enable;
+};
+
 /* 128 bit shared memory between PF and each VF */
 union nic_mbx {
        struct { u8 msg; }      msg;
@@ -540,6 +554,7 @@ union nic_mbx {
        struct set_loopback     lbk;
        struct reset_stat_cfg   reset_stat;
        struct pfc              pfc;
+       struct set_ptp          ptp;
 };
 
 #define NIC_NODE_ID_MASK       0x03
diff --git a/drivers/net/ethernet/cavium/thunder/nic_main.c 
b/drivers/net/ethernet/cavium/thunder/nic_main.c
index 8f1dd55b3e08..4c1c5414a162 100644
--- a/drivers/net/ethernet/cavium/thunder/nic_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nic_main.c
@@ -426,13 +426,22 @@ static void nic_init_hw(struct nicpf *nic)
        /* Enable backpressure */
        nic_reg_write(nic, NIC_PF_BP_CFG, (1ULL << 6) | 0x03);
 
-       /* TNS and TNS bypass modes are present only on 88xx */
+       /* TNS and TNS bypass modes are present only on 88xx
+        * Also offset of this CSR has changed in 81xx and 83xx.
+        */
        if (nic->pdev->subsystem_device == PCI_SUBSYS_DEVID_88XX_NIC_PF) {
                /* Disable TNS mode on both interfaces */
                nic_reg_write(nic, NIC_PF_INTF_0_1_SEND_CFG,
-                             (NIC_TNS_BYPASS_MODE << 7) | BGX0_BLOCK);
+                             (NIC_TNS_BYPASS_MODE << 7) |
+                             BGX0_BLOCK | (1ULL << 16));
                nic_reg_write(nic, NIC_PF_INTF_0_1_SEND_CFG | (1 << 8),
-                             (NIC_TNS_BYPASS_MODE << 7) | BGX1_BLOCK);
+                             (NIC_TNS_BYPASS_MODE << 7) |
+                             BGX1_BLOCK | (1ULL << 16));
+       } else {
+               /* Configure timestamp generation timeout to 10us */
+               for (i = 0; i < nic->hw->bgx_cnt; i++)
+                       nic_reg_write(nic, NIC_PF_INTFX_SEND_CFG | (i << 3),
+                                     (1ULL << 16));
        }
 
        nic_reg_write(nic, NIC_PF_INTF_0_1_BP_CFG,
@@ -880,6 +889,46 @@ static void nic_pause_frame(struct nicpf *nic, int vf, 
struct pfc *cfg)
        }
 }
 
+/* Enable or disable HW timestamping by BGX for pkts received on a LMAC */
+static void nic_config_timestamp(struct nicpf *nic, int vf, struct set_ptp 
*ptp)
+{
+       struct pkind_cfg *pkind;
+       u8 lmac, bgx_idx;
+       u64 pkind_val, pkind_idx;
+
+       if (vf >= nic->num_vf_en)
+               return;
+
+       bgx_idx = NIC_GET_BGX_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vf]);
+       lmac = NIC_GET_LMAC_FROM_VF_LMAC_MAP(nic->vf_lmac_map[vf]);
+
+       pkind_idx = lmac + bgx_idx * MAX_LMAC_PER_BGX;
+       pkind_val = nic_reg_read(nic, NIC_PF_PKIND_0_15_CFG | (pkind_idx << 3));
+       pkind = (struct pkind_cfg *)&pkind_val;
+
+       if (ptp->enable && !pkind->hdr_sl) {
+               /* Skiplen to exclude 8byte timestamp while parsing pkt
+                * If not configured, will result in L2 errors.
+                */
+               pkind->hdr_sl = 4;
+               /* Adjust max packet length allowed */
+               pkind->maxlen += (pkind->hdr_sl * 2);
+               bgx_config_timestamping(nic->node, bgx_idx, lmac, true);
+               nic_reg_write(nic,
+                             NIC_PF_RX_ETYPE_0_7 | (1 << 3),
+                             (ETYPE_ALG_ENDPARSE << 16) | ETH_P_1588);
+       } else if (!ptp->enable && pkind->hdr_sl) {
+               pkind->maxlen -= (pkind->hdr_sl * 2);
+               pkind->hdr_sl = 0;
+               bgx_config_timestamping(nic->node, bgx_idx, lmac, false);
+               nic_reg_write(nic,
+                             NIC_PF_RX_ETYPE_0_7 | (1 << 3),
+                             (1ULL << 16) | ETH_P_8021Q); /* reset value */
+       }
+
+       nic_reg_write(nic, NIC_PF_PKIND_0_15_CFG | (pkind_idx << 3), pkind_val);
+}
+
 /* Interrupt handler to handle mailbox messages from VFs */
 static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
 {
@@ -1022,6 +1071,9 @@ static void nic_handle_mbx_intr(struct nicpf *nic, int vf)
        case NIC_MBOX_MSG_PFC:
                nic_pause_frame(nic, vf, &mbx.pfc);
                goto unlock;
+       case NIC_MBOX_MSG_PTP_CFG:
+               nic_config_timestamp(nic, vf, &mbx.ptp);
+               break;
        default:
                dev_err(&nic->pdev->dev,
                        "Invalid msg from VF%d, msg 0x%x\n", vf, mbx.msg.msg);
diff --git a/drivers/net/ethernet/cavium/thunder/nic_reg.h 
b/drivers/net/ethernet/cavium/thunder/nic_reg.h
index 80d46337cf29..a16c48a1ebb2 100644
--- a/drivers/net/ethernet/cavium/thunder/nic_reg.h
+++ b/drivers/net/ethernet/cavium/thunder/nic_reg.h
@@ -99,6 +99,7 @@
 #define   NIC_PF_ECC3_DBE_INT_W1S              (0x2708)
 #define   NIC_PF_ECC3_DBE_ENA_W1C              (0x2710)
 #define   NIC_PF_ECC3_DBE_ENA_W1S              (0x2718)
+#define   NIC_PF_INTFX_SEND_CFG                        (0x4000)
 #define   NIC_PF_MCAM_0_191_ENA                        (0x100000)
 #define   NIC_PF_MCAM_0_191_M_0_5_DATA         (0x110000)
 #define   NIC_PF_MCAM_CTRL                     (0x120000)
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c 
b/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c
index b9ece9cbf98b..ed9f10bdf41e 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_ethtool.c
@@ -9,12 +9,14 @@
 /* ETHTOOL Support for VNIC_VF Device*/
 
 #include <linux/pci.h>
+#include <linux/net_tstamp.h>
 
 #include "nic_reg.h"
 #include "nic.h"
 #include "nicvf_queues.h"
 #include "q_struct.h"
 #include "thunder_bgx.h"
+#include "../common/cavium_ptp.h"
 
 #define DRV_NAME       "thunder-nicvf"
 #define DRV_VERSION     "1.0"
@@ -824,6 +826,31 @@ static int nicvf_set_pauseparam(struct net_device *dev,
        return 0;
 }
 
+static int nicvf_get_ts_info(struct net_device *netdev,
+                            struct ethtool_ts_info *info)
+{
+       struct nicvf *nic = netdev_priv(netdev);
+
+       if (!nic->ptp_clock)
+               return ethtool_op_get_ts_info(netdev, info);
+
+       info->so_timestamping = SOF_TIMESTAMPING_TX_SOFTWARE |
+                               SOF_TIMESTAMPING_RX_SOFTWARE |
+                               SOF_TIMESTAMPING_SOFTWARE |
+                               SOF_TIMESTAMPING_TX_HARDWARE |
+                               SOF_TIMESTAMPING_RX_HARDWARE |
+                               SOF_TIMESTAMPING_RAW_HARDWARE;
+
+       info->phc_index = cavium_ptp_clock_index(nic->ptp_clock);
+
+       info->tx_types = (1 << HWTSTAMP_TX_OFF) | (1 << HWTSTAMP_TX_ON);
+
+       info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
+                          (1 << HWTSTAMP_FILTER_ALL);
+
+       return 0;
+}
+
 static const struct ethtool_ops nicvf_ethtool_ops = {
        .get_link               = nicvf_get_link,
        .get_drvinfo            = nicvf_get_drvinfo,
@@ -847,7 +874,7 @@ static const struct ethtool_ops nicvf_ethtool_ops = {
        .set_channels           = nicvf_set_channels,
        .get_pauseparam         = nicvf_get_pauseparam,
        .set_pauseparam         = nicvf_set_pauseparam,
-       .get_ts_info            = ethtool_op_get_ts_info,
+       .get_ts_info            = nicvf_get_ts_info,
        .get_link_ksettings     = nicvf_get_link_ksettings,
 };
 
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_main.c 
b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
index 52b3a6044f85..8e3338f31e54 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_main.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_main.c
@@ -20,11 +20,13 @@
 #include <linux/bpf.h>
 #include <linux/bpf_trace.h>
 #include <linux/filter.h>
+#include <linux/net_tstamp.h>
 
 #include "nic_reg.h"
 #include "nic.h"
 #include "nicvf_queues.h"
 #include "thunder_bgx.h"
+#include "../common/cavium_ptp.h"
 
 #define DRV_NAME       "thunder-nicvf"
 #define DRV_VERSION    "1.0"
@@ -601,6 +603,44 @@ static inline bool nicvf_xdp_rx(struct nicvf *nic, struct 
bpf_prog *prog,
        return false;
 }
 
+static void nicvf_snd_ptp_handler(struct net_device *netdev,
+                                 struct cqe_send_t *cqe_tx)
+{
+       struct nicvf *nic = netdev_priv(netdev);
+       struct skb_shared_hwtstamps ts;
+       u64 ns;
+
+       nic = nic->pnicvf;
+
+       /* Sync for 'ptp_skb' */
+       smp_rmb();
+
+       /* New timestamp request can be queued now */
+       atomic_set(&nic->tx_ptp_skbs, 0);
+
+       /* Check for timestamp requested skb */
+       if (!nic->ptp_skb)
+               return;
+
+       /* Check if timestamping is timedout, which is set to 10us */
+       if (cqe_tx->send_status == CQ_TX_ERROP_TSTMP_TIMEOUT ||
+           cqe_tx->send_status == CQ_TX_ERROP_TSTMP_CONFLICT)
+               goto no_tstamp;
+
+       /* Get the timestamp */
+       memset(&ts, 0, sizeof(ts));
+       ns = cavium_ptp_tstamp2time(nic->ptp_clock, cqe_tx->ptp_timestamp);
+       ts.hwtstamp = ns_to_ktime(ns);
+       skb_tstamp_tx(nic->ptp_skb, &ts);
+
+no_tstamp:
+       /* Free the original skb */
+       dev_kfree_skb_any(nic->ptp_skb);
+       nic->ptp_skb = NULL;
+       /* Sync 'ptp_skb' */
+       smp_wmb();
+}
+
 static void nicvf_snd_pkt_handler(struct net_device *netdev,
                                  struct cqe_send_t *cqe_tx,
                                  int budget, int *subdesc_cnt,
@@ -657,7 +697,12 @@ static void nicvf_snd_pkt_handler(struct net_device 
*netdev,
                prefetch(skb);
                (*tx_pkts)++;
                *tx_bytes += skb->len;
-               napi_consume_skb(skb, budget);
+               /* If timestamp is requested for this skb, don't free it */
+               if (skb_shinfo(skb)->tx_flags & SKBTX_IN_PROGRESS &&
+                   !nic->pnicvf->ptp_skb)
+                       nic->pnicvf->ptp_skb = skb;
+               else
+                       napi_consume_skb(skb, budget);
                sq->skbuff[cqe_tx->sqe_ptr] = (u64)NULL;
        } else {
                /* In case of SW TSO on 88xx, only last segment will have
@@ -696,6 +741,21 @@ static inline void nicvf_set_rxhash(struct net_device 
*netdev,
        skb_set_hash(skb, hash, hash_type);
 }
 
+static inline void nicvf_set_rxtstamp(struct nicvf *nic, struct sk_buff *skb)
+{
+       u64 ns;
+
+       if (!nic->ptp_clock || !nic->hw_rx_tstamp)
+               return;
+
+       /* The first 8 bytes is the timestamp */
+       ns = cavium_ptp_tstamp2time(nic->ptp_clock,
+                                   be64_to_cpu(*(__be64 *)skb->data));
+       skb_hwtstamps(skb)->hwtstamp = ns_to_ktime(ns);
+
+       __skb_pull(skb, 8);
+}
+
 static void nicvf_rcv_pkt_handler(struct net_device *netdev,
                                  struct napi_struct *napi,
                                  struct cqe_rx_t *cqe_rx, struct snd_queue *sq)
@@ -746,6 +806,7 @@ static void nicvf_rcv_pkt_handler(struct net_device *netdev,
                return;
        }
 
+       nicvf_set_rxtstamp(nic, skb);
        nicvf_set_rxhash(netdev, cqe_rx, skb);
 
        skb_record_rx_queue(skb, rq_idx);
@@ -820,10 +881,12 @@ static int nicvf_cq_intr_handler(struct net_device 
*netdev, u8 cq_idx,
                                              &tx_pkts, &tx_bytes);
                        tx_done++;
                break;
+               case CQE_TYPE_SEND_PTP:
+                       nicvf_snd_ptp_handler(netdev, (void *)cq_desc);
+               break;
                case CQE_TYPE_INVALID:
                case CQE_TYPE_RX_SPLIT:
                case CQE_TYPE_RX_TCP:
-               case CQE_TYPE_SEND_PTP:
                        /* Ignore for now */
                break;
                }
@@ -1319,12 +1382,28 @@ int nicvf_stop(struct net_device *netdev)
 
        nicvf_free_cq_poll(nic);
 
+       /* Free any pending SKB saved to receive timestamp */
+       if (nic->ptp_skb) {
+               dev_kfree_skb_any(nic->ptp_skb);
+               nic->ptp_skb = NULL;
+       }
+
        /* Clear multiqset info */
        nic->pnicvf = nic;
 
        return 0;
 }
 
+static int nicvf_config_hw_rx_tstamp(struct nicvf *nic, bool enable)
+{
+       union nic_mbx mbx = {};
+
+       mbx.ptp.msg = NIC_MBOX_MSG_PTP_CFG;
+       mbx.ptp.enable = enable;
+
+       return nicvf_send_msg_to_pf(nic, &mbx);
+}
+
 static int nicvf_update_hw_max_frs(struct nicvf *nic, int mtu)
 {
        union nic_mbx mbx = {};
@@ -1394,6 +1473,12 @@ int nicvf_open(struct net_device *netdev)
        if (nic->sqs_mode)
                nicvf_get_primary_vf_struct(nic);
 
+       /* Configure PTP timestamp */
+       if (nic->ptp_clock)
+               nicvf_config_hw_rx_tstamp(nic, nic->hw_rx_tstamp);
+       atomic_set(&nic->tx_ptp_skbs, 0);
+       nic->ptp_skb = NULL;
+
        /* Configure receive side scaling and MTU */
        if (!nic->sqs_mode) {
                nicvf_rss_init(nic);
@@ -1820,6 +1905,77 @@ static void nicvf_xdp_flush(struct net_device *dev)
        return;
 }
 
+static int nicvf_config_hwtstamp(struct net_device *netdev, struct ifreq *ifr)
+{
+       struct hwtstamp_config config;
+       struct nicvf *nic = netdev_priv(netdev);
+
+       if (!nic->ptp_clock)
+               return -ENODEV;
+
+       if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+               return -EFAULT;
+
+       /* reserved for future extensions */
+       if (config.flags)
+               return -EINVAL;
+
+       switch (config.tx_type) {
+       case HWTSTAMP_TX_OFF:
+       case HWTSTAMP_TX_ON:
+               break;
+       default:
+               return -ERANGE;
+       }
+
+       switch (config.rx_filter) {
+       case HWTSTAMP_FILTER_NONE:
+               nic->hw_rx_tstamp = false;
+               break;
+       case HWTSTAMP_FILTER_ALL:
+       case HWTSTAMP_FILTER_SOME:
+       case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
+       case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
+       case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
+       case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+       case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+       case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+       case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+       case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+       case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+       case HWTSTAMP_FILTER_PTP_V2_EVENT:
+       case HWTSTAMP_FILTER_PTP_V2_SYNC:
+       case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+               nic->hw_rx_tstamp = true;
+               config.rx_filter = HWTSTAMP_FILTER_ALL;
+               break;
+       default:
+               return -ERANGE;
+       }
+
+       if (netif_running(netdev)) {
+               if (nic->hw_rx_tstamp)
+                       nicvf_config_hw_rx_tstamp(nic, true);
+               else
+                       nicvf_config_hw_rx_tstamp(nic, false);
+       }
+
+       if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
+               return -EFAULT;
+
+       return 0;
+}
+
+static int nicvf_ioctl(struct net_device *netdev, struct ifreq *req, int cmd)
+{
+       switch (cmd) {
+       case SIOCSHWTSTAMP:
+               return nicvf_config_hwtstamp(netdev, req);
+       default:
+               return -EOPNOTSUPP;
+       }
+}
+
 static const struct net_device_ops nicvf_netdev_ops = {
        .ndo_open               = nicvf_open,
        .ndo_stop               = nicvf_stop,
@@ -1833,6 +1989,7 @@ static const struct net_device_ops nicvf_netdev_ops = {
        .ndo_bpf                = nicvf_xdp,
        .ndo_xdp_xmit           = nicvf_xdp_xmit,
        .ndo_xdp_flush          = nicvf_xdp_flush,
+       .ndo_do_ioctl           = nicvf_ioctl,
 };
 
 static int nicvf_probe(struct pci_dev *pdev, const struct pci_device_id *ent)
@@ -1842,6 +1999,16 @@ static int nicvf_probe(struct pci_dev *pdev, const 
struct pci_device_id *ent)
        struct nicvf *nic;
        int    err, qcount;
        u16    sdevid;
+       struct cavium_ptp *ptp_clock;
+
+       ptp_clock = cavium_ptp_get();
+       if (IS_ERR(ptp_clock)) {
+               if (PTR_ERR(ptp_clock) == -ENODEV)
+                       /* In virtualized environment we proceed without ptp */
+                       ptp_clock = NULL;
+               else
+                       return PTR_ERR(ptp_clock);
+       }
 
        err = pci_enable_device(pdev);
        if (err) {
@@ -1896,6 +2063,7 @@ static int nicvf_probe(struct pci_dev *pdev, const struct 
pci_device_id *ent)
         */
        if (!nic->t88)
                nic->max_queues *= 2;
+       nic->ptp_clock = ptp_clock;
 
        /* MAP VF's configuration registers */
        nic->reg_base = pcim_iomap(pdev, PCI_CFG_REG_BAR_NUM, 0);
@@ -2009,6 +2177,7 @@ static void nicvf_remove(struct pci_dev *pdev)
        pci_set_drvdata(pdev, NULL);
        if (nic->drv_stats)
                free_percpu(nic->drv_stats);
+       cavium_ptp_put(nic->ptp_clock);
        free_netdev(netdev);
        pci_release_regions(pdev);
        pci_disable_device(pdev);
diff --git a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c 
b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
index 095c18aeb8d5..9a999c62d6ba 100644
--- a/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
+++ b/drivers/net/ethernet/cavium/thunder/nicvf_queues.c
@@ -978,6 +978,9 @@ void nicvf_qset_config(struct nicvf *nic, bool enable)
                qs_cfg->be = 1;
 #endif
                qs_cfg->vnic = qs->vnic_id;
+               /* Enable Tx timestamping capability */
+               if (nic->ptp_clock)
+                       qs_cfg->send_tstmp_ena = 1;
        }
        nicvf_send_msg_to_pf(nic, &mbx);
 }
@@ -1383,6 +1386,29 @@ nicvf_sq_add_hdr_subdesc(struct nicvf *nic, struct 
snd_queue *sq, int qentry,
                hdr->inner_l3_offset = skb_network_offset(skb) - 2;
                this_cpu_inc(nic->pnicvf->drv_stats->tx_tso);
        }
+
+       /* Check if timestamp is requested */
+       if (!(skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP)) {
+               skb_tx_timestamp(skb);
+               return;
+       }
+
+       /* Tx timestamping not supported along with TSO, so ignore request */
+       if (skb_shinfo(skb)->gso_size)
+               return;
+
+       /* HW supports only a single outstanding packet to timestamp */
+       if (!atomic_add_unless(&nic->pnicvf->tx_ptp_skbs, 1, 1))
+               return;
+
+       /* Mark the SKB for later reference */
+       skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+
+       /* Finally enable timestamp generation
+        * Since 'post_cqe' is also set, two CQEs will be posted
+        * for this packet i.e CQE_TYPE_SEND and CQE_TYPE_SEND_PTP.
+        */
+       hdr->tstmp = 1;
 }
 
 /* SQ GATHER subdescriptor
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c 
b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
index 5e5c4d7796b8..0f23999c5bcf 100644
--- a/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.c
@@ -245,6 +245,35 @@ void bgx_lmac_rx_tx_enable(int node, int bgx_idx, int 
lmacid, bool enable)
 }
 EXPORT_SYMBOL(bgx_lmac_rx_tx_enable);
 
+/* Enables or disables timestamp insertion by BGX for Rx packets */
+void bgx_config_timestamping(int node, int bgx_idx, int lmacid, bool enable)
+{
+       struct bgx *bgx = get_bgx(node, bgx_idx);
+       struct lmac *lmac;
+       u64 csr_offset, cfg;
+
+       if (!bgx)
+               return;
+
+       lmac = &bgx->lmac[lmacid];
+
+       if (lmac->lmac_type == BGX_MODE_SGMII ||
+           lmac->lmac_type == BGX_MODE_QSGMII ||
+           lmac->lmac_type == BGX_MODE_RGMII)
+               csr_offset = BGX_GMP_GMI_RXX_FRM_CTL;
+       else
+               csr_offset = BGX_SMUX_RX_FRM_CTL;
+
+       cfg = bgx_reg_read(bgx, lmacid, csr_offset);
+
+       if (enable)
+               cfg |= BGX_PKT_RX_PTP_EN;
+       else
+               cfg &= ~BGX_PKT_RX_PTP_EN;
+       bgx_reg_write(bgx, lmacid, csr_offset, cfg);
+}
+EXPORT_SYMBOL(bgx_config_timestamping);
+
 void bgx_lmac_get_pfc(int node, int bgx_idx, int lmacid, void *pause)
 {
        struct pfc *pfc = (struct pfc *)pause;
diff --git a/drivers/net/ethernet/cavium/thunder/thunder_bgx.h 
b/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
index 23acdc5ab896..5a7567d31138 100644
--- a/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
+++ b/drivers/net/ethernet/cavium/thunder/thunder_bgx.h
@@ -122,6 +122,8 @@
 #define  SPU_DBG_CTL_AN_NONCE_MCT_DIS          BIT_ULL(29)
 
 #define BGX_SMUX_RX_INT                        0x20000
+#define BGX_SMUX_RX_FRM_CTL            0x20020
+#define  BGX_PKT_RX_PTP_EN                     BIT_ULL(12)
 #define BGX_SMUX_RX_JABBER             0x20030
 #define BGX_SMUX_RX_CTL                        0x20048
 #define  SMU_RX_CTL_STATUS                     (3ull << 0)
@@ -172,6 +174,7 @@
 #define  GMI_PORT_CFG_SPEED_MSB                        BIT_ULL(8)
 #define  GMI_PORT_CFG_RX_IDLE                  BIT_ULL(12)
 #define  GMI_PORT_CFG_TX_IDLE                  BIT_ULL(13)
+#define BGX_GMP_GMI_RXX_FRM_CTL                0x38028
 #define BGX_GMP_GMI_RXX_JABBER         0x38038
 #define BGX_GMP_GMI_TXX_THRESH         0x38210
 #define BGX_GMP_GMI_TXX_APPEND         0x38218
@@ -223,6 +226,7 @@ void bgx_set_lmac_mac(int node, int bgx_idx, int lmacid, 
const u8 *mac);
 void bgx_get_lmac_link_state(int node, int bgx_idx, int lmacid, void *status);
 void bgx_lmac_internal_loopback(int node, int bgx_idx,
                                int lmac_idx, bool enable);
+void bgx_config_timestamping(int node, int bgx_idx, int lmacid, bool enable);
 void bgx_lmac_get_pfc(int node, int bgx_idx, int lmacid, void *pause);
 void bgx_lmac_set_pfc(int node, int bgx_idx, int lmacid, void *pause);
 
-- 
2.15.1

Reply via email to