From 9a8a02c9d46dcd4c663dac39e6518b6bb7ac1631 Mon Sep 17 00:00:00 2001 From: Jose Abreu Date: Thu, 31 May 2018 18:01:27 +0100 Subject: [PATCH] net: stmmac: Add Flexible PPS support This adds support for Flexible PPS output (which is equivalent to per_out output of PTP subsystem). Tested using an oscilloscope and the following commands: 1) Start PTP4L: # ptp4l -A -4 -H -m -i eth0 & 2) Set Flexible PPS frequency: # echo > /sys/class/ptp/ptpX/period Where, ts/tns is start time and ps/pns is period time, and ptpX is ptp of eth0. Signed-off-by: Jose Abreu Cc: David S. Miller Cc: Joao Pinto Cc: Vitor Soares Cc: Giuseppe Cavallaro Cc: Alexandre Torgue Cc: Richard Cochran Signed-off-by: David S. Miller --- drivers/net/ethernet/stmicro/stmmac/common.h | 2 + drivers/net/ethernet/stmicro/stmmac/dwmac4.h | 1 + .../net/ethernet/stmicro/stmmac/dwmac4_core.c | 1 + .../net/ethernet/stmicro/stmmac/dwmac4_dma.c | 2 + drivers/net/ethernet/stmicro/stmmac/dwmac5.c | 55 +++++++++++++++++++ drivers/net/ethernet/stmicro/stmmac/dwmac5.h | 22 ++++++++ drivers/net/ethernet/stmicro/stmmac/hwif.h | 7 +++ drivers/net/ethernet/stmicro/stmmac/stmmac.h | 12 ++++ .../net/ethernet/stmicro/stmmac/stmmac_main.c | 4 ++ .../net/ethernet/stmicro/stmmac/stmmac_ptp.c | 42 +++++++++++++- 10 files changed, 145 insertions(+), 3 deletions(-) diff --git a/drivers/net/ethernet/stmicro/stmmac/common.h b/drivers/net/ethernet/stmicro/stmmac/common.h index a679cb729d1de..78fd0f8b8e81e 100644 --- a/drivers/net/ethernet/stmicro/stmmac/common.h +++ b/drivers/net/ethernet/stmicro/stmmac/common.h @@ -346,6 +346,8 @@ struct dma_features { /* TX and RX number of queues */ unsigned int number_rx_queues; unsigned int number_tx_queues; + /* PPS output */ + unsigned int pps_out_num; /* Alternate (enhanced) DESC mode */ unsigned int enh_desc; /* TX and RX FIFO sizes */ diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4.h b/drivers/net/ethernet/stmicro/stmmac/dwmac4.h index 6330a55953df5..eb013d54025af 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac4.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4.h @@ -187,6 +187,7 @@ enum power_event { #define GMAC_HW_RXFIFOSIZE GENMASK(4, 0) /* MAC HW features2 bitmap */ +#define GMAC_HW_FEAT_PPSOUTNUM GENMASK(26, 24) #define GMAC_HW_FEAT_TXCHCNT GENMASK(21, 18) #define GMAC_HW_FEAT_RXCHCNT GENMASK(15, 12) #define GMAC_HW_FEAT_TXQCNT GENMASK(9, 6) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c index a7121a7d9391c..7e5d5db0d5165 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_core.c @@ -796,6 +796,7 @@ const struct stmmac_ops dwmac510_ops = { .safety_feat_irq_status = dwmac5_safety_feat_irq_status, .safety_feat_dump = dwmac5_safety_feat_dump, .rxp_config = dwmac5_rxp_config, + .flex_pps_config = dwmac5_flex_pps_config, }; int dwmac4_setup(struct stmmac_priv *priv) diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c index bf8e5a16f11c8..d37f17ca62fec 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac4_dma.c @@ -373,6 +373,8 @@ static void dwmac4_get_hw_feature(void __iomem *ioaddr, ((hw_cap & GMAC_HW_FEAT_RXQCNT) >> 0) + 1; dma_cap->number_tx_queues = ((hw_cap & GMAC_HW_FEAT_TXQCNT) >> 6) + 1; + /* PPS output */ + dma_cap->pps_out_num = (hw_cap & GMAC_HW_FEAT_PPSOUTNUM) >> 24; /* IEEE 1588-2002 */ dma_cap->time_stamp = 0; diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac5.c b/drivers/net/ethernet/stmicro/stmmac/dwmac5.c index b2becb80a697c..3f4f3132e16b3 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac5.c +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac5.c @@ -8,6 +8,7 @@ #include "dwmac4.h" #include "dwmac5.h" #include "stmmac.h" +#include "stmmac_ptp.h" struct dwmac5_error_desc { bool valid; @@ -494,3 +495,57 @@ re_enable: writel(old_val, ioaddr + GMAC_CONFIG); return ret; } + +int dwmac5_flex_pps_config(void __iomem *ioaddr, int index, + struct stmmac_pps_cfg *cfg, bool enable, + u32 sub_second_inc, u32 systime_flags) +{ + u32 tnsec = readl(ioaddr + MAC_PPSx_TARGET_TIME_NSEC(index)); + u32 val = readl(ioaddr + MAC_PPS_CONTROL); + u64 period; + + if (!cfg->available) + return -EINVAL; + if (tnsec & TRGTBUSY0) + return -EBUSY; + if (!sub_second_inc || !systime_flags) + return -EINVAL; + + val &= ~PPSx_MASK(index); + + if (!enable) { + val |= PPSCMDx(index, 0x5); + writel(val, ioaddr + MAC_PPS_CONTROL); + return 0; + } + + val |= PPSCMDx(index, 0x2); + val |= TRGTMODSELx(index, 0x2); + val |= PPSEN0; + + writel(cfg->start.tv_sec, ioaddr + MAC_PPSx_TARGET_TIME_SEC(index)); + + if (!(systime_flags & PTP_TCR_TSCTRLSSR)) + cfg->start.tv_nsec = (cfg->start.tv_nsec * 1000) / 465; + writel(cfg->start.tv_nsec, ioaddr + MAC_PPSx_TARGET_TIME_NSEC(index)); + + period = cfg->period.tv_sec * 1000000000; + period += cfg->period.tv_nsec; + + do_div(period, sub_second_inc); + + if (period <= 1) + return -EINVAL; + + writel(period - 1, ioaddr + MAC_PPSx_INTERVAL(index)); + + period >>= 1; + if (period <= 1) + return -EINVAL; + + writel(period - 1, ioaddr + MAC_PPSx_WIDTH(index)); + + /* Finally, activate it */ + writel(val, ioaddr + MAC_PPS_CONTROL); + return 0; +} diff --git a/drivers/net/ethernet/stmicro/stmmac/dwmac5.h b/drivers/net/ethernet/stmicro/stmmac/dwmac5.h index cc810aff71007..775db776b3cc9 100644 --- a/drivers/net/ethernet/stmicro/stmmac/dwmac5.h +++ b/drivers/net/ethernet/stmicro/stmmac/dwmac5.h @@ -11,6 +11,25 @@ #define PRTYEN BIT(1) #define TMOUTEN BIT(0) +#define MAC_PPS_CONTROL 0x00000b70 +#define PPS_MAXIDX(x) ((((x) + 1) * 8) - 1) +#define PPS_MINIDX(x) ((x) * 8) +#define PPSx_MASK(x) GENMASK(PPS_MAXIDX(x), PPS_MINIDX(x)) +#define MCGRENx(x) BIT(PPS_MAXIDX(x)) +#define TRGTMODSELx(x, val) \ + GENMASK(PPS_MAXIDX(x) - 1, PPS_MAXIDX(x) - 2) & \ + ((val) << (PPS_MAXIDX(x) - 2)) +#define PPSCMDx(x, val) \ + GENMASK(PPS_MINIDX(x) + 3, PPS_MINIDX(x)) & \ + ((val) << PPS_MINIDX(x)) +#define PPSEN0 BIT(4) +#define MAC_PPSx_TARGET_TIME_SEC(x) (0x00000b80 + ((x) * 0x10)) +#define MAC_PPSx_TARGET_TIME_NSEC(x) (0x00000b84 + ((x) * 0x10)) +#define TRGTBUSY0 BIT(31) +#define TTSL0 GENMASK(30, 0) +#define MAC_PPSx_INTERVAL(x) (0x00000b88 + ((x) * 0x10)) +#define MAC_PPSx_WIDTH(x) (0x00000b8c + ((x) * 0x10)) + #define MTL_RXP_CONTROL_STATUS 0x00000ca0 #define RXPI BIT(31) #define NPE GENMASK(23, 16) @@ -61,5 +80,8 @@ int dwmac5_safety_feat_dump(struct stmmac_safety_stats *stats, int index, unsigned long *count, const char **desc); int dwmac5_rxp_config(void __iomem *ioaddr, struct stmmac_tc_entry *entries, unsigned int count); +int dwmac5_flex_pps_config(void __iomem *ioaddr, int index, + struct stmmac_pps_cfg *cfg, bool enable, + u32 sub_second_inc, u32 systime_flags); #endif /* __DWMAC5_H__ */ diff --git a/drivers/net/ethernet/stmicro/stmmac/hwif.h b/drivers/net/ethernet/stmicro/stmmac/hwif.h index f499a7fad6f03..e44e7b26ce829 100644 --- a/drivers/net/ethernet/stmicro/stmmac/hwif.h +++ b/drivers/net/ethernet/stmicro/stmmac/hwif.h @@ -241,6 +241,7 @@ struct net_device; struct rgmii_adv; struct stmmac_safety_stats; struct stmmac_tc_entry; +struct stmmac_pps_cfg; /* Helpers to program the MAC core */ struct stmmac_ops { @@ -313,6 +314,10 @@ struct stmmac_ops { /* Flexible RX Parser */ int (*rxp_config)(void __iomem *ioaddr, struct stmmac_tc_entry *entries, unsigned int count); + /* Flexible PPS */ + int (*flex_pps_config)(void __iomem *ioaddr, int index, + struct stmmac_pps_cfg *cfg, bool enable, + u32 sub_second_inc, u32 systime_flags); }; #define stmmac_core_init(__priv, __args...) \ @@ -379,6 +384,8 @@ struct stmmac_ops { stmmac_do_callback(__priv, mac, safety_feat_dump, __args) #define stmmac_rxp_config(__priv, __args...) \ stmmac_do_callback(__priv, mac, rxp_config, __args) +#define stmmac_flex_pps_config(__priv, __args...) \ + stmmac_do_callback(__priv, mac, flex_pps_config, __args) /* PTP and HW Timer helpers */ struct stmmac_hwtimestamp { diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac.h b/drivers/net/ethernet/stmicro/stmmac/stmmac.h index fbfe5dcefa87f..025efbf6145cc 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac.h +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac.h @@ -100,6 +100,13 @@ struct stmmac_tc_entry { } __packed val; }; +#define STMMAC_PPS_MAX 4 +struct stmmac_pps_cfg { + bool available; + struct timespec64 start; + struct timespec64 period; +}; + struct stmmac_priv { /* Frequently used values are kept adjacent for cache effect */ u32 tx_count_frames; @@ -160,6 +167,8 @@ struct stmmac_priv { struct ptp_clock *ptp_clock; struct ptp_clock_info ptp_clock_ops; unsigned int default_addend; + u32 sub_second_inc; + u32 systime_flags; u32 adv_ts; int use_riwt; int irq_wake; @@ -181,6 +190,9 @@ struct stmmac_priv { unsigned int tc_entries_max; unsigned int tc_off_max; struct stmmac_tc_entry *tc_entries; + + /* Pulse Per Second output */ + struct stmmac_pps_cfg pps[STMMAC_PPS_MAX]; }; enum stmmac_state { diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c index 77af85c981db1..11fb7c777d89b 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_main.c @@ -721,6 +721,10 @@ static int stmmac_hwtstamp_ioctl(struct net_device *dev, struct ifreq *ifr) priv->plat->has_gmac4, &sec_inc); temp = div_u64(1000000000ULL, sec_inc); + /* Store sub second increment and flags for later use */ + priv->sub_second_inc = sec_inc; + priv->systime_flags = value; + /* calculate default added value: * formula is : * addend = (2^32)/freq_div_ratio; diff --git a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c index 7d3a5c7f5db6b..0cb0e39a2be93 100644 --- a/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c +++ b/drivers/net/ethernet/stmicro/stmmac/stmmac_ptp.c @@ -140,17 +140,43 @@ static int stmmac_set_time(struct ptp_clock_info *ptp, static int stmmac_enable(struct ptp_clock_info *ptp, struct ptp_clock_request *rq, int on) { - return -EOPNOTSUPP; + struct stmmac_priv *priv = + container_of(ptp, struct stmmac_priv, ptp_clock_ops); + struct stmmac_pps_cfg *cfg; + int ret = -EOPNOTSUPP; + unsigned long flags; + + switch (rq->type) { + case PTP_CLK_REQ_PEROUT: + cfg = &priv->pps[rq->perout.index]; + + cfg->start.tv_sec = rq->perout.start.sec; + cfg->start.tv_nsec = rq->perout.start.nsec; + cfg->period.tv_sec = rq->perout.period.sec; + cfg->period.tv_nsec = rq->perout.period.nsec; + + spin_lock_irqsave(&priv->ptp_lock, flags); + ret = stmmac_flex_pps_config(priv, priv->ioaddr, + rq->perout.index, cfg, on, + priv->sub_second_inc, + priv->systime_flags); + spin_unlock_irqrestore(&priv->ptp_lock, flags); + break; + default: + break; + } + + return ret; } /* structure describing a PTP hardware clock */ -static const struct ptp_clock_info stmmac_ptp_clock_ops = { +static struct ptp_clock_info stmmac_ptp_clock_ops = { .owner = THIS_MODULE, .name = "stmmac_ptp_clock", .max_adj = 62500000, .n_alarm = 0, .n_ext_ts = 0, - .n_per_out = 0, + .n_per_out = 0, /* will be overwritten in stmmac_ptp_register */ .n_pins = 0, .pps = 0, .adjfreq = stmmac_adjust_freq, @@ -168,6 +194,16 @@ static const struct ptp_clock_info stmmac_ptp_clock_ops = { */ void stmmac_ptp_register(struct stmmac_priv *priv) { + int i; + + for (i = 0; i < priv->dma_cap.pps_out_num; i++) { + if (i >= STMMAC_PPS_MAX) + break; + priv->pps[i].available = true; + } + + stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num; + spin_lock_init(&priv->ptp_lock); priv->ptp_clock_ops = stmmac_ptp_clock_ops; -- 2.39.2