[PATCH net-next 5/9] mlxsw: spectrum_ptp: Add implementation for physical hardware clock operations

[Ido Schimmel <idosch@nvidia.com>]

From: Danielle Ratson <danieller@nvidia.com>

Implement physical hardware clock operations. The main difference between
the existing operations of Spectrum-1 and the new operations of Spectrum-2
is the usage of UTC hardware clock instead of FRC.

Add support for init() and fini() functions for PTP clock in Spectrum-2.

Signed-off-by: Danielle Ratson <danieller@nvidia.com>
Signed-off-by: Amit Cohen <amcohen@nvidia.com>
Reviewed-by: Petr Machata <petrm@nvidia.com>
Signed-off-by: Ido Schimmel <idosch@nvidia.com>
---
 .../ethernet/mellanox/mlxsw/spectrum_ptp.c    | 147 ++++++++++++++++++
 .../ethernet/mellanox/mlxsw/spectrum_ptp.h    |  19 ++-
 2 files changed, 159 insertions(+), 7 deletions(-)

diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
index 04110a12c855..dd7c94dd4c01 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.c
@@ -339,6 +339,153 @@ void mlxsw_sp1_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock_common)
 	kfree(clock);
 }
 
+static u64 mlxsw_sp2_ptp_read_utc(struct mlxsw_sp_ptp_clock *clock,
+				  struct ptp_system_timestamp *sts)
+{
+	struct mlxsw_core *mlxsw_core = clock->core;
+	u32 utc_sec1, utc_sec2, utc_nsec;
+
+	utc_sec1 = mlxsw_core_read_utc_sec(mlxsw_core);
+	ptp_read_system_prets(sts);
+	utc_nsec = mlxsw_core_read_utc_nsec(mlxsw_core);
+	ptp_read_system_postts(sts);
+	utc_sec2 = mlxsw_core_read_utc_sec(mlxsw_core);
+
+	if (utc_sec1 != utc_sec2) {
+		/* Wrap around. */
+		ptp_read_system_prets(sts);
+		utc_nsec = mlxsw_core_read_utc_nsec(mlxsw_core);
+		ptp_read_system_postts(sts);
+	}
+
+	return (u64)utc_sec2 * NSEC_PER_SEC + utc_nsec;
+}
+
+static int
+mlxsw_sp2_ptp_phc_settime(struct mlxsw_sp_ptp_clock *clock, u64 nsec)
+{
+	struct mlxsw_core *mlxsw_core = clock->core;
+	char mtutc_pl[MLXSW_REG_MTUTC_LEN];
+	u32 sec, nsec_rem;
+
+	sec = div_u64_rem(nsec, NSEC_PER_SEC, &nsec_rem);
+	mlxsw_reg_mtutc_pack(mtutc_pl,
+			     MLXSW_REG_MTUTC_OPERATION_SET_TIME_IMMEDIATE,
+			     0, sec, nsec_rem, 0);
+	return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
+}
+
+static int mlxsw_sp2_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
+{
+	struct mlxsw_sp_ptp_clock *clock =
+		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
+	s32 ppb = scaled_ppm_to_ppb(scaled_ppm);
+
+	/* In Spectrum-2 and newer ASICs, the frequency adjustment in MTUTC is
+	 * reversed, positive values mean to decrease the frequency. Adjust the
+	 * sign of PPB to this behavior.
+	 */
+	return mlxsw_sp_ptp_phc_adjfreq(clock, -ppb);
+}
+
+static int mlxsw_sp2_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
+{
+	struct mlxsw_sp_ptp_clock *clock =
+		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
+	struct mlxsw_core *mlxsw_core = clock->core;
+	char mtutc_pl[MLXSW_REG_MTUTC_LEN];
+
+	/* HW time adjustment range is s16. If out of range, set time instead. */
+	if (delta < S16_MIN || delta > S16_MAX) {
+		u64 nsec;
+
+		nsec = mlxsw_sp2_ptp_read_utc(clock, NULL);
+		nsec += delta;
+
+		return mlxsw_sp2_ptp_phc_settime(clock, nsec);
+	}
+
+	mlxsw_reg_mtutc_pack(mtutc_pl,
+			     MLXSW_REG_MTUTC_OPERATION_ADJUST_TIME,
+			     0, 0, 0, delta);
+	return mlxsw_reg_write(mlxsw_core, MLXSW_REG(mtutc), mtutc_pl);
+}
+
+static int mlxsw_sp2_ptp_gettimex(struct ptp_clock_info *ptp,
+				  struct timespec64 *ts,
+				  struct ptp_system_timestamp *sts)
+{
+	struct mlxsw_sp_ptp_clock *clock =
+		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
+	u64 nsec;
+
+	nsec = mlxsw_sp2_ptp_read_utc(clock, sts);
+	*ts = ns_to_timespec64(nsec);
+
+	return 0;
+}
+
+static int mlxsw_sp2_ptp_settime(struct ptp_clock_info *ptp,
+				 const struct timespec64 *ts)
+{
+	struct mlxsw_sp_ptp_clock *clock =
+		container_of(ptp, struct mlxsw_sp_ptp_clock, ptp_info);
+	u64 nsec = timespec64_to_ns(ts);
+
+	return mlxsw_sp2_ptp_phc_settime(clock, nsec);
+}
+
+static const struct ptp_clock_info mlxsw_sp2_ptp_clock_info = {
+	.owner		= THIS_MODULE,
+	.name		= "mlxsw_sp_clock",
+	.max_adj	= MLXSW_REG_MTUTC_MAX_FREQ_ADJ,
+	.adjfine	= mlxsw_sp2_ptp_adjfine,
+	.adjtime	= mlxsw_sp2_ptp_adjtime,
+	.gettimex64	= mlxsw_sp2_ptp_gettimex,
+	.settime64	= mlxsw_sp2_ptp_settime,
+};
+
+struct mlxsw_sp_ptp_clock *
+mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
+{
+	struct mlxsw_sp_ptp_clock *clock;
+	int err;
+
+	clock = kzalloc(sizeof(*clock), GFP_KERNEL);
+	if (!clock)
+		return ERR_PTR(-ENOMEM);
+
+	clock->core = mlxsw_sp->core;
+
+	clock->ptp_info = mlxsw_sp2_ptp_clock_info;
+
+	err = mlxsw_sp2_ptp_phc_settime(clock, 0);
+	if (err) {
+		dev_err(dev, "setting UTC time failed %d\n", err);
+		goto err_ptp_phc_settime;
+	}
+
+	clock->ptp = ptp_clock_register(&clock->ptp_info, dev);
+	if (IS_ERR(clock->ptp)) {
+		err = PTR_ERR(clock->ptp);
+		dev_err(dev, "ptp_clock_register failed %d\n", err);
+		goto err_ptp_clock_register;
+	}
+
+	return clock;
+
+err_ptp_clock_register:
+err_ptp_phc_settime:
+	kfree(clock);
+	return ERR_PTR(err);
+}
+
+void mlxsw_sp2_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
+{
+	ptp_clock_unregister(clock->ptp);
+	kfree(clock);
+}
+
 static int mlxsw_sp_ptp_parse(struct sk_buff *skb,
 			      u8 *p_domain_number,
 			      u8 *p_message_type,
diff --git a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h
index 87afc96a0ed6..d5871cb4ae50 100644
--- a/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h
+++ b/drivers/net/ethernet/mellanox/mlxsw/spectrum_ptp.h
@@ -57,6 +57,11 @@ void mlxsw_sp1_get_stats_strings(u8 **p);
 void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 			 u64 *data, int data_index);
 
+struct mlxsw_sp_ptp_clock *
+mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev);
+
+void mlxsw_sp2_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock);
+
 struct mlxsw_sp_ptp_state *mlxsw_sp2_ptp_init(struct mlxsw_sp *mlxsw_sp);
 
 void mlxsw_sp2_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state);
@@ -140,26 +145,26 @@ static inline void mlxsw_sp1_get_stats(struct mlxsw_sp_port *mlxsw_sp_port,
 {
 }
 
-static inline struct mlxsw_sp_ptp_state *
-mlxsw_sp2_ptp_init(struct mlxsw_sp *mlxsw_sp)
+static inline struct mlxsw_sp_ptp_clock *
+mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
 {
 	return NULL;
 }
 
-static inline void mlxsw_sp2_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state)
+static inline void mlxsw_sp2_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
 {
 }
-#endif
 
-static inline struct mlxsw_sp_ptp_clock *
-mlxsw_sp2_ptp_clock_init(struct mlxsw_sp *mlxsw_sp, struct device *dev)
+static inline struct mlxsw_sp_ptp_state *
+mlxsw_sp2_ptp_init(struct mlxsw_sp *mlxsw_sp)
 {
 	return NULL;
 }
 
-static inline void mlxsw_sp2_ptp_clock_fini(struct mlxsw_sp_ptp_clock *clock)
+static inline void mlxsw_sp2_ptp_fini(struct mlxsw_sp_ptp_state *ptp_state)
 {
 }
+#endif
 
 static inline void mlxsw_sp2_ptp_receive(struct mlxsw_sp *mlxsw_sp,
 					 struct sk_buff *skb, u16 local_port)
-- 
2.36.1