[PATCH v2 3/6] igb: add additional ieee1588 support functions

[Daniel Mrzyglod <danielx.t.mrzyglod@intel.com>]

From: Pablo de Lara <pablo.de.lara.guarch@intel.com>

Add additional functions to support the existing IEEE1588
functionality and to enable getting, setting and adjusting
the device time.

Signed-off-by: Pablo de Lara <pablo.de.lara.guarch@intel.com>
Signed-off-by: Daniel Mrzyglod <danielx.t.mrzyglod@intel.com>
---
 drivers/net/e1000/e1000_ethdev.h |  22 +++
 drivers/net/e1000/igb_ethdev.c   | 338 +++++++++++++++++++++++++++++++++++++--
 2 files changed, 350 insertions(+), 10 deletions(-)

diff --git a/drivers/net/e1000/e1000_ethdev.h b/drivers/net/e1000/e1000_ethdev.h
index 4e69e44..c8772b7 100644
--- a/drivers/net/e1000/e1000_ethdev.h
+++ b/drivers/net/e1000/e1000_ethdev.h
@@ -220,6 +220,26 @@ struct e1000_filter_info {
 };
 
 /*
+ * Structure for cyclecounter IEEE1588 functionality.
+ */
+struct cyclecounter {
+	uint64_t (*read)(struct rte_eth_dev *dev);
+	uint64_t mask;
+	uint32_t shift;
+};
+
+/*
+ * Structure to hold and calculate Unix epoch time.
+ */
+struct timecounter {
+	struct cyclecounter *cc;
+	uint64_t cycle_last;
+	uint64_t nsec;
+	uint64_t mask;
+	uint64_t frac;
+};
+
+/*
  * Structure to store private data for each driver instance (for each port).
  */
 struct e1000_adapter {
@@ -230,6 +250,8 @@ struct e1000_adapter {
 	struct e1000_vf_info    *vfdata;
 	struct e1000_filter_info filter;
 	bool stopped;
+	struct cyclecounter cc;
+	struct timecounter tc;
 };
 
 #define E1000_DEV_PRIVATE(adapter) \
diff --git a/drivers/net/e1000/igb_ethdev.c b/drivers/net/e1000/igb_ethdev.c
index 3ab082e..fea11f8 100644
--- a/drivers/net/e1000/igb_ethdev.c
+++ b/drivers/net/e1000/igb_ethdev.c
@@ -78,10 +78,12 @@
 #define IGB_8_BIT_MASK   UINT8_MAX
 
 /* Additional timesync values. */
-#define E1000_ETQF_FILTER_1588 3
-#define E1000_TIMINCA_INCVALUE 16000000
-#define E1000_TIMINCA_INIT     ((0x02 << E1000_TIMINCA_16NS_SHIFT) \
-				| E1000_TIMINCA_INCVALUE)
+#define NSEC_PER_SEC                 1000000000L
+#define E1000_CYCLECOUNTER_MASK      0xffffffffffffffff
+#define E1000_ETQF_FILTER_1588       3
+#define IGB_82576_TSYNC_SHIFT        16
+#define E1000_INCPERIOD_82576        (1 << E1000_TIMINCA_16NS_SHIFT)
+#define E1000_INCVALUE_82576         (16 << IGB_82576_TSYNC_SHIFT)
 #define E1000_TSAUXC_DISABLE_SYSTIME 0x80000000
 
 static int  eth_igb_configure(struct rte_eth_dev *dev);
@@ -230,6 +232,11 @@ static int igb_timesync_read_rx_timestamp(struct rte_eth_dev *dev,
 					  uint32_t flags);
 static int igb_timesync_read_tx_timestamp(struct rte_eth_dev *dev,
 					  struct timespec *timestamp);
+static int igb_timesync_time_adjust(struct rte_eth_dev *dev, int64_t delta);
+static int igb_timesync_time_get(struct rte_eth_dev *dev,
+		struct timespec *timestamp);
+static int igb_timesync_time_set(struct rte_eth_dev *dev,
+		struct timespec *timestamp);
 static int eth_igb_rx_queue_intr_enable(struct rte_eth_dev *dev,
 					uint16_t queue_id);
 static int eth_igb_rx_queue_intr_disable(struct rte_eth_dev *dev,
@@ -327,6 +334,9 @@ static const struct eth_dev_ops eth_igb_ops = {
 	.get_eeprom_length    = eth_igb_get_eeprom_length,
 	.get_eeprom           = eth_igb_get_eeprom,
 	.set_eeprom           = eth_igb_set_eeprom,
+	.timesync_time_adjust  = igb_timesync_time_adjust,
+	.timesync_time_get     = igb_timesync_time_get,
+	.timesync_time_set     = igb_timesync_time_set,
 };
 
 /*
@@ -3883,20 +3893,286 @@ eth_igb_set_mc_addr_list(struct rte_eth_dev *dev,
 	return 0;
 }
 
+static inline uint64_t
+timespec_to_ns(const struct timespec *ts)
+{
+	return ((uint64_t) ts->tv_sec * NSEC_PER_SEC) + ts->tv_nsec;
+}
+
+static struct timespec
+ns_to_timespec(uint64_t nsec)
+{
+	struct timespec ts = {0, 0};
+
+	if (nsec == 0)
+		return ts;
+
+	ts.tv_sec = nsec / NSEC_PER_SEC;
+	ts.tv_nsec = nsec % NSEC_PER_SEC;
+
+	return ts;
+}
+
+/*
+ * Converts cycle counter cycles to nanoseconds.
+ */
+static inline uint64_t
+cyclecounter_cycles_to_ns(const struct cyclecounter *cc, uint64_t cycles,
+			uint64_t mask, uint64_t *frac)
+{
+	uint64_t ns;
+
+	/* Add fractional nanoseconds. */
+	ns = cycles + *frac;
+	*frac = ns & mask;
+
+	/* Shift to get only nanoseconds. */
+	return (ns >> cc->shift);
+}
+
+/*
+ * Like cyclecounter_cycles_to_ns(), but this is used when
+ * computing a time previous to the stored in the cycle counter.
+ */
+static uint64_t
+cyclecounter_cycles_to_ns_backwards(const struct cyclecounter *cc,
+			uint64_t cycles, uint64_t frac)
+{
+	return ((cycles - frac) >> cc->shift);
+}
+
+/*
+ * Register units might not be nanoseconds. This function converts
+ * these units into nanoseconds and adds to the previous time stored.
+ */
+static uint64_t
+timecounter_cycles_to_ns_time(struct timecounter *tc, uint64_t cycle_tstamp)
+{
+	uint64_t delta;
+	uint64_t nsec = tc->nsec, frac = tc->frac;
+
+	delta = (cycle_tstamp - tc->cycle_last) & tc->cc->mask;
+	/*
+	 * Cycle counts that are correctly converted as they
+	 * are between -1/2 max cycle count and +1/2 max cycle count.
+	 */
+	if (delta > (tc->cc->mask / 2)) {
+		delta = (tc->cycle_last - cycle_tstamp) & tc->cc->mask;
+		nsec -= cyclecounter_cycles_to_ns_backwards(tc->cc, delta, frac);
+	} else {
+		nsec += cyclecounter_cycles_to_ns(tc->cc, delta, tc->mask, &frac);
+	}
+
+	return nsec;
+}
+
+static uint64_t
+igb_read_timesync_cyclecounter(struct rte_eth_dev *dev)
+{
+	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	uint64_t systime_cycles = 0;
+
+	switch (hw->mac.type) {
+	case e1000_i210:
+	case e1000_i211:
+		/*
+		 * Need to read System Time Residue Register to be able
+		 * to read the other two registers.
+		 */
+		E1000_READ_REG(hw, E1000_SYSTIMR);
+		/* SYSTIMEL stores ns and SYSTIMEH stores seconds. */
+		systime_cycles = (uint64_t)E1000_READ_REG(hw, E1000_SYSTIML);
+		systime_cycles += (uint64_t)E1000_READ_REG(hw, E1000_SYSTIMH)
+				* NSEC_PER_SEC;
+		break;
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		/*
+		 * Need to read System Time Residue Register to be able
+		 * to read the other two registers.
+		 */
+		E1000_READ_REG(hw, E1000_SYSTIMR);
+		systime_cycles |= (uint64_t)E1000_READ_REG(hw, E1000_SYSTIML);
+		/* Only the 8 LSB are valid. */
+		systime_cycles |= (uint64_t)(E1000_READ_REG(hw, E1000_SYSTIMH)
+				& 0xff) << 32;
+		break;
+	default:
+		systime_cycles |= (uint64_t)E1000_READ_REG(hw, E1000_SYSTIML);
+		systime_cycles |= (uint64_t)E1000_READ_REG(hw, E1000_SYSTIMH)
+				<< 32;
+		break;
+	}
+
+	return systime_cycles;
+}
+
+/*
+ * Get nanoseconds since the last call of this function.
+ */
+static uint64_t
+timecounter_read_ns_delta(struct rte_eth_dev *dev)
+{
+	uint64_t cycle_now, cycle_delta;
+	uint64_t ns_offset;
+	struct e1000_adapter *adapter =
+				(struct e1000_adapter *)dev->data->dev_private;
+	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+
+	/* Read cycle counter. */
+	cycle_now = adapter->tc.cc->read(dev);
+
+	/* Calculate the delta since the last timecounter_read_delta(). */
+	cycle_delta = (cycle_now - adapter->tc.cycle_last) & adapter->tc.cc->mask;
+
+	/* Convert to nanoseconds. */
+	switch (hw->mac.type) {
+	case e1000_i210:
+	case e1000_i211:
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		/* Registers store directly nanoseconds, no need to convert. */
+		ns_offset = cycle_delta;
+		break;
+	default:
+		ns_offset = cyclecounter_cycles_to_ns(adapter->tc.cc, cycle_delta,
+					adapter->tc.mask, &adapter->tc.frac);
+	}
+
+	/* Store current cycle counter for next timecounter_read_ns_delta() call. */
+	adapter->tc.cycle_last = cycle_now;
+
+	return ns_offset;
+}
+
+static uint64_t
+timecounter_read(struct rte_eth_dev *dev)
+{
+	uint64_t nsec;
+	struct e1000_adapter *adapter =
+			(struct e1000_adapter *)dev->data->dev_private;
+
+	/* increment time by nanoseconds since last call */
+	nsec = timecounter_read_ns_delta(dev);
+	nsec += adapter->tc.nsec;
+	adapter->tc.nsec = nsec;
+
+	return nsec;
+}
+
+
+static void
+timecounter_init(struct rte_eth_dev *dev,
+		      uint64_t start_time)
+{
+	struct e1000_adapter *adapter =
+				(struct e1000_adapter *)dev->data->dev_private;
+	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+
+	adapter->tc.cc = &adapter->cc;
+	adapter->tc.cycle_last = adapter->tc.cc->read(dev);
+	adapter->tc.nsec = start_time;
+	switch (hw->mac.type) {
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		/* 32 LSB bits + 8 MSB bits = 40 bits */
+		adapter->tc.mask = (1ULL << 40) - 1;
+	default:
+		adapter->tc.mask = (1ULL << adapter->tc.cc->shift) - 1;
+	}
+	adapter->tc.frac = 0;
+}
+
+static void
+igb_start_cyclecounter(struct rte_eth_dev *dev)
+{
+	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	struct e1000_adapter *adapter =
+		(struct e1000_adapter *)dev->data->dev_private;
+	uint32_t incval = 1;
+	uint32_t shift = 0;
+
+	switch (hw->mac.type) {
+	case e1000_i210:
+	case e1000_i211:
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		/* Start incrementing the register used to timestamp PTP packets. */
+		E1000_WRITE_REG(hw, E1000_TIMINCA, incval);
+		break;
+	case e1000_82576:
+		incval = E1000_INCVALUE_82576;
+		shift = IGB_82576_TSYNC_SHIFT;
+		E1000_WRITE_REG(hw, E1000_TIMINCA, E1000_INCPERIOD_82576 | incval);
+		break;
+	default:
+		/* Not supported */
+		return;
+	}
+
+	memset(&adapter->cc, 0, sizeof(struct cyclecounter));
+	adapter->cc.read = igb_read_timesync_cyclecounter;
+	adapter->cc.mask = E1000_CYCLECOUNTER_MASK;
+	adapter->cc.shift = shift;
+}
+
+static int
+igb_timesync_time_adjust(struct rte_eth_dev *dev, int64_t delta)
+{
+	struct e1000_adapter *adapter =
+			(struct e1000_adapter *)dev->data->dev_private;
+
+	adapter->tc.nsec += delta;
+
+	return 0;
+}
+
+static int
+igb_timesync_time_set(struct rte_eth_dev *dev, struct timespec *ts)
+{
+	uint64_t ns;
+
+	ns = timespec_to_ns(ts);
+
+	/* Reset the timecounter. */
+	timecounter_init(dev, ns);
+
+	return 0;
+}
+
+static int
+igb_timesync_time_get(struct rte_eth_dev *dev, struct timespec *ts)
+{
+	uint64_t ns;
+
+	ns = timecounter_read(dev);
+	*ts = ns_to_timespec(ns);
+
+	return 0;
+}
+
 static int
 igb_timesync_enable(struct rte_eth_dev *dev)
 {
 	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
 	uint32_t tsync_ctl;
 	uint32_t tsauxc;
+	uint64_t ns;
+	struct timespec zerotime = {0, 0};
 
 	/* Enable system time for it isn't on by default. */
 	tsauxc = E1000_READ_REG(hw, E1000_TSAUXC);
 	tsauxc &= ~E1000_TSAUXC_DISABLE_SYSTIME;
 	E1000_WRITE_REG(hw, E1000_TSAUXC, tsauxc);
 
-	/* Start incrementing the register used to timestamp PTP packets. */
-	E1000_WRITE_REG(hw, E1000_TIMINCA, E1000_TIMINCA_INIT);
+	/* Set 0.0 epoch time to initialize timecounter. */
+	ns = timespec_to_ns(&zerotime);
+	igb_start_cyclecounter(dev);
+	timecounter_init(dev, ns);
 
 	/* Enable L2 filtering of IEEE1588/802.1AS Ethernet frame types. */
 	E1000_WRITE_REG(hw, E1000_ETQF(E1000_ETQF_FILTER_1588),
@@ -3948,9 +4224,13 @@ igb_timesync_read_rx_timestamp(struct rte_eth_dev *dev,
 			       uint32_t flags __rte_unused)
 {
 	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	struct e1000_adapter *adapter =
+			(struct e1000_adapter *)dev->data->dev_private;
+
 	uint32_t tsync_rxctl;
 	uint32_t rx_stmpl;
 	uint32_t rx_stmph;
+	uint64_t regival = 0;
 
 	tsync_rxctl = E1000_READ_REG(hw, E1000_TSYNCRXCTL);
 	if ((tsync_rxctl & E1000_TSYNCRXCTL_VALID) == 0)
@@ -3958,9 +4238,26 @@ igb_timesync_read_rx_timestamp(struct rte_eth_dev *dev,
 
 	rx_stmpl = E1000_READ_REG(hw, E1000_RXSTMPL);
 	rx_stmph = E1000_READ_REG(hw, E1000_RXSTMPH);
+	timecounter_read(dev);
+
+	switch (hw->mac.type) {
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		regival = (uint64_t)((((uint64_t)(rx_stmph & 0xff)) << 32)
+			| rx_stmpl);
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		regival = (uint64_t)((uint64_t)rx_stmph * NSEC_PER_SEC
+			+ rx_stmpl);
+		break;
+	default:
+		regival = (uint64_t)(((uint64_t)rx_stmph << 32) | rx_stmpl);
+	}
 
-	timestamp->tv_sec = (uint64_t)(((uint64_t)rx_stmph << 32) | rx_stmpl);
-	timestamp->tv_nsec = 0;
+	regival = timecounter_cycles_to_ns_time(&adapter->tc, regival);
+	*timestamp = ns_to_timespec(regival);
 
 	return  0;
 }
@@ -3970,6 +4267,10 @@ igb_timesync_read_tx_timestamp(struct rte_eth_dev *dev,
 			       struct timespec *timestamp)
 {
 	struct e1000_hw *hw = E1000_DEV_PRIVATE_TO_HW(dev->data->dev_private);
+	struct e1000_adapter *adapter =
+			(struct e1000_adapter *)dev->data->dev_private;
+	uint64_t regival = 0;
+
 	uint32_t tsync_txctl;
 	uint32_t tx_stmpl;
 	uint32_t tx_stmph;
@@ -3980,9 +4281,26 @@ igb_timesync_read_tx_timestamp(struct rte_eth_dev *dev,
 
 	tx_stmpl = E1000_READ_REG(hw, E1000_TXSTMPL);
 	tx_stmph = E1000_READ_REG(hw, E1000_TXSTMPH);
+	timecounter_read(dev);
+
+	switch (hw->mac.type) {
+	case e1000_82580:
+	case e1000_i350:
+	case e1000_i354:
+		regival = (uint64_t)((((uint64_t)(tx_stmph & 0xff)) << 32)
+			| tx_stmpl);
+		break;
+	case e1000_i210:
+	case e1000_i211:
+		regival = (uint64_t)((uint64_t)tx_stmph * NSEC_PER_SEC
+			+ tx_stmpl);
+		break;
+	default:
+		regival = (uint64_t)(((uint64_t)tx_stmph << 32) | tx_stmpl);
+	}
 
-	timestamp->tv_sec = (uint64_t)(((uint64_t)tx_stmph << 32) | tx_stmpl);
-	timestamp->tv_nsec = 0;
+	regival = timecounter_cycles_to_ns_time(&adapter->tc, regival);
+	*timestamp = ns_to_timespec(regival);
 
 	return  0;
 }
-- 
2.1.0