net: microchip: sparx5: add function for calculating PTP basetime

Add a new function for calculating PTP basetime, required by the stream
gate scheduler to calculate gate state (open / close).

Signed-off-by: Daniel Machon <daniel.machon@microchip.com>
Reviewed-by: Simon Horman <simon.horman@corigine.com>
Signed-off-by: David S. Miller <davem@davemloft.net>

diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_main.h b/drivers/net/ethernet/microchip/sparx5/sparx5_main.h
index 709cad5..fd71b2e 100644 (file)
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_main.h
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_main.h
@@ -396,6 +396,7 @@ int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
 void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
                                 struct sk_buff *skb);
 irqreturn_t sparx5_ptp_irq_handler(int irq, void *args);
+int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts);
 
 /* sparx5_vcap_impl.c */
 int sparx5_vcap_init(struct sparx5 *sparx5);
@@ -481,6 +482,10 @@ int sparx5_psfp_fm_add(struct sparx5 *sparx5, u32 uidx,
                       struct sparx5_psfp_fm *fm, u32 *id);
 int sparx5_psfp_fm_del(struct sparx5 *sparx5, u32 id);
 
+/* sparx5_qos.c */
+void sparx5_new_base_time(struct sparx5 *sparx5, const u32 cycle_time,
+                         const ktime_t org_base_time, ktime_t *new_base_time);
+
 /* Clock period in picoseconds */
 static inline u32 sparx5_clk_period(enum sparx5_core_clockfreq cclock)
 {

diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c b/drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c
index 0ed1ea7..af85d66 100644 (file)
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c
@@ -476,8 +476,7 @@ static int sparx5_ptp_settime64(struct ptp_clock_info *ptp,
        return 0;
 }
 
-static int sparx5_ptp_gettime64(struct ptp_clock_info *ptp,
-                               struct timespec64 *ts)
+int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
 {
        struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
        struct sparx5 *sparx5 = phc->sparx5;

diff --git a/drivers/net/ethernet/microchip/sparx5/sparx5_qos.c b/drivers/net/ethernet/microchip/sparx5/sparx5_qos.c
index 379e540..ebfdbbf 100644 (file)
--- a/drivers/net/ethernet/microchip/sparx5/sparx5_qos.c
+++ b/drivers/net/ethernet/microchip/sparx5/sparx5_qos.c
@@ -9,6 +9,63 @@
 #include "sparx5_main.h"
 #include "sparx5_qos.h"
 
+/* Calculate new base_time based on cycle_time.
+ *
+ * The hardware requires a base_time that is always in the future.
+ * We define threshold_time as current_time + (2 * cycle_time).
+ * If base_time is below threshold_time this function recalculates it to be in
+ * the interval:
+ * threshold_time <= base_time < (threshold_time + cycle_time)
+ *
+ * A very simple algorithm could be like this:
+ * new_base_time = org_base_time + N * cycle_time
+ * using the lowest N so (new_base_time >= threshold_time
+ */
+void sparx5_new_base_time(struct sparx5 *sparx5, const u32 cycle_time,
+                         const ktime_t org_base_time, ktime_t *new_base_time)
+{
+       ktime_t current_time, threshold_time, new_time;
+       struct timespec64 ts;
+       u64 nr_of_cycles_p2;
+       u64 nr_of_cycles;
+       u64 diff_time;
+
+       new_time = org_base_time;
+
+       sparx5_ptp_gettime64(&sparx5->phc[SPARX5_PHC_PORT].info, &ts);
+       current_time = timespec64_to_ktime(ts);
+       threshold_time = current_time + (2 * cycle_time);
+       diff_time = threshold_time - new_time;
+       nr_of_cycles = div_u64(diff_time, cycle_time);
+       nr_of_cycles_p2 = 1; /* Use 2^0 as start value */
+
+       if (new_time >= threshold_time) {
+               *new_base_time = new_time;
+               return;
+       }
+
+       /* Calculate the smallest power of 2 (nr_of_cycles_p2)
+        * that is larger than nr_of_cycles.
+        */
+       while (nr_of_cycles_p2 < nr_of_cycles)
+               nr_of_cycles_p2 <<= 1; /* Next (higher) power of 2 */
+
+       /* Add as big chunks (power of 2 * cycle_time)
+        * as possible for each power of 2
+        */
+       while (nr_of_cycles_p2) {
+               if (new_time < threshold_time) {
+                       new_time += cycle_time * nr_of_cycles_p2;
+                       while (new_time < threshold_time)
+                               new_time += cycle_time * nr_of_cycles_p2;
+                       new_time -= cycle_time * nr_of_cycles_p2;
+               }
+               nr_of_cycles_p2 >>= 1; /* Next (lower) power of 2 */
+       }
+       new_time += cycle_time;
+       *new_base_time = new_time;
+}
+
 /* Max rates for leak groups */
 static const u32 spx5_hsch_max_group_rate[SPX5_HSCH_LEAK_GRP_CNT] = {
        1048568, /*  1.049 Gbps */