[linux-block.git] / drivers / net / ethernet / stmicro / stmmac / stmmac_ptp.c

// SPDX-License-Identifier: GPL-2.0-only
/*******************************************************************************
  PTP 1588 clock using the STMMAC.

  Copyright (C) 2013  Vayavya Labs Pvt Ltd


  Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
*******************************************************************************/
#include "stmmac.h"
#include "stmmac_ptp.h"

/**
 * stmmac_adjust_freq
 *
 * @ptp: pointer to ptp_clock_info structure
 * @ppb: desired period change in parts ber billion
 *
 * Description: this function will adjust the frequency of hardware clock.
 */
static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
{
	struct stmmac_priv *priv =
	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	unsigned long flags;
	u32 diff, addend;
	int neg_adj = 0;
	u64 adj;

	if (ppb < 0) {
		neg_adj = 1;
		ppb = -ppb;
	}

	addend = priv->default_addend;
	adj = addend;
	adj *= ppb;
	diff = div_u64(adj, 1000000000ULL);
	addend = neg_adj ? (addend - diff) : (addend + diff);

	spin_lock_irqsave(&priv->ptp_lock, flags);
	stmmac_config_addend(priv, priv->ptpaddr, addend);
	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return 0;
}

/**
 * stmmac_adjust_time
 *
 * @ptp: pointer to ptp_clock_info structure
 * @delta: desired change in nanoseconds
 *
 * Description: this function will shift/adjust the hardware clock time.
 */
static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
{
	struct stmmac_priv *priv =
	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	unsigned long flags;
	u32 sec, nsec;
	u32 quotient, reminder;
	int neg_adj = 0;
	bool xmac;

	xmac = priv->plat->has_gmac4 || priv->plat->has_xgmac;

	if (delta < 0) {
		neg_adj = 1;
		delta = -delta;
	}

	quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
	sec = quotient;
	nsec = reminder;

	spin_lock_irqsave(&priv->ptp_lock, flags);
	stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return 0;
}

/**
 * stmmac_get_time
 *
 * @ptp: pointer to ptp_clock_info structure
 * @ts: pointer to hold time/result
 *
 * Description: this function will read the current time from the
 * hardware clock and store it in @ts.
 */
static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts)
{
	struct stmmac_priv *priv =
	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	unsigned long flags;
	u64 ns = 0;

	spin_lock_irqsave(&priv->ptp_lock, flags);
	stmmac_get_systime(priv, priv->ptpaddr, &ns);
	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	*ts = ns_to_timespec64(ns);

	return 0;
}

/**
 * stmmac_set_time
 *
 * @ptp: pointer to ptp_clock_info structure
 * @ts: time value to set
 *
 * Description: this function will set the current time on the
 * hardware clock.
 */
static int stmmac_set_time(struct ptp_clock_info *ptp,
			   const struct timespec64 *ts)
{
	struct stmmac_priv *priv =
	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	unsigned long flags;

	spin_lock_irqsave(&priv->ptp_lock, flags);
	stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	return 0;
}

static int stmmac_enable(struct ptp_clock_info *ptp,
			 struct ptp_clock_request *rq, int on)
{
	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:
		/* Reject requests with unsupported flags */
		if (rq->perout.flags)
			return -EOPNOTSUPP;

		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 struct ptp_clock_info stmmac_ptp_clock_ops = {
	.owner = THIS_MODULE,
	.name = "stmmac ptp",
	.max_adj = 62500000,
	.n_alarm = 0,
	.n_ext_ts = 0,
	.n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
	.n_pins = 0,
	.pps = 0,
	.adjfreq = stmmac_adjust_freq,
	.adjtime = stmmac_adjust_time,
	.gettime64 = stmmac_get_time,
	.settime64 = stmmac_set_time,
	.enable = stmmac_enable,
};

/**
 * stmmac_ptp_register
 * @priv: driver private structure
 * Description: this function will register the ptp clock driver
 * to kernel. It also does some house keeping work.
 */
void stmmac_ptp_register(struct stmmac_priv *priv)
{
	int i;

	if (priv->plat->ptp_clk_freq_config)
		priv->plat->ptp_clk_freq_config(priv);

	for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
		if (i >= STMMAC_PPS_MAX)
			break;
		priv->pps[i].available = true;
	}

	if (priv->plat->ptp_max_adj)
		stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;

	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;

	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
					     priv->device);
	if (IS_ERR(priv->ptp_clock)) {
		netdev_err(priv->dev, "ptp_clock_register failed\n");
		priv->ptp_clock = NULL;
	} else if (priv->ptp_clock)
		netdev_info(priv->dev, "registered PTP clock\n");
}

/**
 * stmmac_ptp_unregister
 * @priv: driver private structure
 * Description: this function will remove/unregister the ptp clock driver
 * from the kernel.
 */
void stmmac_ptp_unregister(struct stmmac_priv *priv)
{
	if (priv->ptp_clock) {
		ptp_clock_unregister(priv->ptp_clock);
		priv->ptp_clock = NULL;
		pr_debug("Removed PTP HW clock successfully on %s\n",
			 priv->dev->name);
	}
}
Commit	Line	Data
4fa9c49f	1	// SPDX-License-Identifier: GPL-2.0-only
92ba6888 RK	2	/*******************************************************************************
	3	PTP 1588 clock using the STMMAC.
	4
	5	Copyright (C) 2013 Vayavya Labs Pvt Ltd
	6
92ba6888 RK	7
	8	Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
	9	*******************************************************************************/
	10	#include "stmmac.h"
	11	#include "stmmac_ptp.h"
	12
	13	/**
	14	* stmmac_adjust_freq
	15	*
	16	* @ptp: pointer to ptp_clock_info structure
	17	* @ppb: desired period change in parts ber billion
	18	*
	19	* Description: this function will adjust the frequency of hardware clock.
	20	*/
	21	static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
	22	{
	23	struct stmmac_priv *priv =
	24	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	25	unsigned long flags;
	26	u32 diff, addend;
	27	int neg_adj = 0;
	28	u64 adj;
	29
	30	if (ppb < 0) {
	31	neg_adj = 1;
	32	ppb = -ppb;
	33	}
	34
	35	addend = priv->default_addend;
	36	adj = addend;
	37	adj *= ppb;
	38	diff = div_u64(adj, 1000000000ULL);
	39	addend = neg_adj ? (addend - diff) : (addend + diff);
	40
	41	spin_lock_irqsave(&priv->ptp_lock, flags);
cc4c9001	42	stmmac_config_addend(priv, priv->ptpaddr, addend);
7cd01399	43	spin_unlock_irqrestore(&priv->ptp_lock, flags);
92ba6888 RK	44
	45	return 0;
	46	}
	47
	48	/**
	49	* stmmac_adjust_time
	50	*
	51	* @ptp: pointer to ptp_clock_info structure
	52	* @delta: desired change in nanoseconds
	53	*
	54	* Description: this function will shift/adjust the hardware clock time.
	55	*/
	56	static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
	57	{
	58	struct stmmac_priv *priv =
	59	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	60	unsigned long flags;
	61	u32 sec, nsec;
	62	u32 quotient, reminder;
	63	int neg_adj = 0;
4bb7aff9 JA	64	bool xmac;
	65
	66	xmac = priv->plat->has_gmac4 \|\| priv->plat->has_xgmac;
92ba6888 RK	67
	68	if (delta < 0) {
	69	neg_adj = 1;
	70	delta = -delta;
	71	}
	72
	73	quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
	74	sec = quotient;
	75	nsec = reminder;
	76
	77	spin_lock_irqsave(&priv->ptp_lock, flags);
4bb7aff9	78	stmmac_adjust_systime(priv, priv->ptpaddr, sec, nsec, neg_adj, xmac);
7cd01399	79	spin_unlock_irqrestore(&priv->ptp_lock, flags);
92ba6888 RK	80
	81	return 0;
	82	}
	83
	84	/**
	85	* stmmac_get_time
	86	*
	87	* @ptp: pointer to ptp_clock_info structure
	88	* @ts: pointer to hold time/result
	89	*
	90	* Description: this function will read the current time from the
	91	* hardware clock and store it in @ts.
	92	*/
3f6c4654	93	static int stmmac_get_time(struct ptp_clock_info ptp, struct timespec64 ts)
92ba6888 RK	94	{
	95	struct stmmac_priv *priv =
	96	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	97	unsigned long flags;
1f5d861f	98	u64 ns = 0;
92ba6888 RK	99
92ba6888 RK	100	spin_lock_irqsave(&priv->ptp_lock, flags);
cc4c9001	101	stmmac_get_systime(priv, priv->ptpaddr, &ns);
92ba6888 RK	102	spin_unlock_irqrestore(&priv->ptp_lock, flags);
92ba6888 RK	103
e7ea55be	104	*ts = ns_to_timespec64(ns);
92ba6888 RK	105
	106	return 0;
	107	}
	108
	109	/**
	110	* stmmac_set_time
	111	*
	112	* @ptp: pointer to ptp_clock_info structure
	113	* @ts: time value to set
	114	*
	115	* Description: this function will set the current time on the
	116	* hardware clock.
	117	*/
	118	static int stmmac_set_time(struct ptp_clock_info *ptp,
3f6c4654	119	const struct timespec64 *ts)
92ba6888 RK	120	{
	121	struct stmmac_priv *priv =
	122	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	123	unsigned long flags;
	124
	125	spin_lock_irqsave(&priv->ptp_lock, flags);
cc4c9001	126	stmmac_init_systime(priv, priv->ptpaddr, ts->tv_sec, ts->tv_nsec);
92ba6888 RK	127	spin_unlock_irqrestore(&priv->ptp_lock, flags);
	128
	129	return 0;
	130	}
	131
	132	static int stmmac_enable(struct ptp_clock_info *ptp,
	133	struct ptp_clock_request *rq, int on)
	134	{
9a8a02c9 JA	135	struct stmmac_priv *priv =
	136	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	137	struct stmmac_pps_cfg *cfg;
	138	int ret = -EOPNOTSUPP;
	139	unsigned long flags;
	140
	141	switch (rq->type) {
	142	case PTP_CLK_REQ_PEROUT:
7f9048f1 JK	143	/* Reject requests with unsupported flags */
	144	if (rq->perout.flags)
	145	return -EOPNOTSUPP;
	146
9a8a02c9 JA	147	cfg = &priv->pps[rq->perout.index];
	148
	149	cfg->start.tv_sec = rq->perout.start.sec;
	150	cfg->start.tv_nsec = rq->perout.start.nsec;
	151	cfg->period.tv_sec = rq->perout.period.sec;
	152	cfg->period.tv_nsec = rq->perout.period.nsec;
	153
	154	spin_lock_irqsave(&priv->ptp_lock, flags);
	155	ret = stmmac_flex_pps_config(priv, priv->ioaddr,
	156	rq->perout.index, cfg, on,
	157	priv->sub_second_inc,
	158	priv->systime_flags);
	159	spin_unlock_irqrestore(&priv->ptp_lock, flags);
	160	break;
	161	default:
	162	break;
	163	}
	164
	165	return ret;
92ba6888 RK	166	}
	167
	168	/* structure describing a PTP hardware clock */
9a8a02c9	169	static struct ptp_clock_info stmmac_ptp_clock_ops = {
92ba6888	170	.owner = THIS_MODULE,
5da202c8	171	.name = "stmmac ptp",
92ba6888 RK	172	.max_adj = 62500000,
	173	.n_alarm = 0,
	174	.n_ext_ts = 0,
9a8a02c9	175	.n_per_out = 0, /* will be overwritten in stmmac_ptp_register */
4986b4f0	176	.n_pins = 0,
92ba6888 RK	177	.pps = 0,
	178	.adjfreq = stmmac_adjust_freq,
	179	.adjtime = stmmac_adjust_time,
3f6c4654 RC	180	.gettime64 = stmmac_get_time,
3f6c4654 RC	181	.settime64 = stmmac_set_time,
92ba6888 RK	182	.enable = stmmac_enable,
	183	};
	184
	185	/**
	186	* stmmac_ptp_register
32ceabca	187	* @priv: driver private structure
92ba6888 RK	188	* Description: this function will register the ptp clock driver
	189	* to kernel. It also does some house keeping work.
	190	*/
c30a70d3	191	void stmmac_ptp_register(struct stmmac_priv *priv)
92ba6888	192	{
9a8a02c9 JA	193	int i;
9a8a02c9 JA	194
76da35dc WVK	195	if (priv->plat->ptp_clk_freq_config)
	196	priv->plat->ptp_clk_freq_config(priv);
	197
9a8a02c9 JA	198	for (i = 0; i < priv->dma_cap.pps_out_num; i++) {
	199	if (i >= STMMAC_PPS_MAX)
	200	break;
	201	priv->pps[i].available = true;
	202	}
	203
190f73ab VW	204	if (priv->plat->ptp_max_adj)
	205	stmmac_ptp_clock_ops.max_adj = priv->plat->ptp_max_adj;
	206
9a8a02c9 JA	207	stmmac_ptp_clock_ops.n_per_out = priv->dma_cap.pps_out_num;
9a8a02c9 JA	208
92ba6888 RK	209	spin_lock_init(&priv->ptp_lock);
	210	priv->ptp_clock_ops = stmmac_ptp_clock_ops;
	211
	212	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
	213	priv->device);
	214	if (IS_ERR(priv->ptp_clock)) {
c30a70d3	215	netdev_err(priv->dev, "ptp_clock_register failed\n");
92ba6888	216	priv->ptp_clock = NULL;
c30a70d3 GC	217	} else if (priv->ptp_clock)
c30a70d3 GC	218	netdev_info(priv->dev, "registered PTP clock\n");
92ba6888 RK	219	}
	220
	221	/**
	222	* stmmac_ptp_unregister
32ceabca	223	* @priv: driver private structure
92ba6888 RK	224	* Description: this function will remove/unregister the ptp clock driver
	225	* from the kernel.
	226	*/
	227	void stmmac_ptp_unregister(struct stmmac_priv *priv)
	228	{
	229	if (priv->ptp_clock) {
	230	ptp_clock_unregister(priv->ptp_clock);
f95f4045	231	priv->ptp_clock = NULL;
92ba6888 RK	232	pr_debug("Removed PTP HW clock successfully on %s\n",
	233	priv->dev->name);
	234	}
	235	}