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

/*******************************************************************************
  PTP 1588 clock using the STMMAC.

  Copyright (C) 2013  Vayavya Labs Pvt Ltd

  This program is free software; you can redistribute it and/or modify it
  under the terms and conditions of the GNU General Public License,
  version 2, as published by the Free Software Foundation.

  This program is distributed in the hope it will be useful, but WITHOUT
  ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
  more details.

  You should have received a copy of the GNU General Public License along with
  this program; if not, write to the Free Software Foundation, Inc.,
  51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.

  The full GNU General Public License is included in this distribution in
  the file called "COPYING".

  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);

	priv->hw->ptp->config_addend(priv->ioaddr, 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;

	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);

	priv->hw->ptp->adjust_systime(priv->ioaddr, sec, nsec, neg_adj);

	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 timespec *ts)
{
	struct stmmac_priv *priv =
	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	unsigned long flags;
	u64 ns;
	u32 reminder;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	ns = priv->hw->ptp->get_systime(priv->ioaddr);

	spin_unlock_irqrestore(&priv->ptp_lock, flags);

	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder);
	ts->tv_nsec = reminder;

	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 timespec *ts)
{
	struct stmmac_priv *priv =
	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	unsigned long flags;

	spin_lock_irqsave(&priv->ptp_lock, flags);

	priv->hw->ptp->init_systime(priv->ioaddr, 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)
{
	return -EOPNOTSUPP;
}

/* structure describing a PTP hardware clock */
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_pins = 0,
	.pps = 0,
	.adjfreq = stmmac_adjust_freq,
	.adjtime = stmmac_adjust_time,
	.gettime = stmmac_get_time,
	.settime = 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.
 */
int stmmac_ptp_register(struct stmmac_priv *priv)
{
	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)) {
		priv->ptp_clock = NULL;
		pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
	} else
		pr_debug("Added PTP HW clock successfully on %s\n",
			 priv->dev->name);

	return 0;
}

/**
 * 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);
		pr_debug("Removed PTP HW clock successfully on %s\n",
			 priv->dev->name);
	}
}
Commit	Line	Data
92ba6888 RK	1	/*******************************************************************************
	2	PTP 1588 clock using the STMMAC.
	3
	4	Copyright (C) 2013 Vayavya Labs Pvt Ltd
	5
	6	This program is free software; you can redistribute it and/or modify it
	7	under the terms and conditions of the GNU General Public License,
	8	version 2, as published by the Free Software Foundation.
	9
	10	This program is distributed in the hope it will be useful, but WITHOUT
	11	ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
	12	FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for
	13	more details.
	14
	15	You should have received a copy of the GNU General Public License along with
	16	this program; if not, write to the Free Software Foundation, Inc.,
	17	51 Franklin St - Fifth Floor, Boston, MA 02110-1301 USA.
	18
	19	The full GNU General Public License is included in this distribution in
	20	the file called "COPYING".
	21
	22	Author: Rayagond Kokatanur <rayagond@vayavyalabs.com>
	23	*******************************************************************************/
	24	#include "stmmac.h"
	25	#include "stmmac_ptp.h"
	26
	27	/**
	28	* stmmac_adjust_freq
	29	*
	30	* @ptp: pointer to ptp_clock_info structure
	31	* @ppb: desired period change in parts ber billion
	32	*
	33	* Description: this function will adjust the frequency of hardware clock.
	34	*/
	35	static int stmmac_adjust_freq(struct ptp_clock_info *ptp, s32 ppb)
	36	{
	37	struct stmmac_priv *priv =
	38	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	39	unsigned long flags;
	40	u32 diff, addend;
	41	int neg_adj = 0;
	42	u64 adj;
	43
	44	if (ppb < 0) {
	45	neg_adj = 1;
	46	ppb = -ppb;
	47	}
	48
	49	addend = priv->default_addend;
	50	adj = addend;
	51	adj *= ppb;
	52	diff = div_u64(adj, 1000000000ULL);
	53	addend = neg_adj ? (addend - diff) : (addend + diff);
	54
	55	spin_lock_irqsave(&priv->ptp_lock, flags);
	56
	57	priv->hw->ptp->config_addend(priv->ioaddr, addend);
	58
7cd01399	59	spin_unlock_irqrestore(&priv->ptp_lock, flags);
92ba6888 RK	60
	61	return 0;
	62	}
	63
	64	/**
	65	* stmmac_adjust_time
	66	*
	67	* @ptp: pointer to ptp_clock_info structure
	68	* @delta: desired change in nanoseconds
	69	*
	70	* Description: this function will shift/adjust the hardware clock time.
	71	*/
	72	static int stmmac_adjust_time(struct ptp_clock_info *ptp, s64 delta)
	73	{
	74	struct stmmac_priv *priv =
	75	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	76	unsigned long flags;
	77	u32 sec, nsec;
	78	u32 quotient, reminder;
	79	int neg_adj = 0;
	80
	81	if (delta < 0) {
	82	neg_adj = 1;
	83	delta = -delta;
	84	}
	85
	86	quotient = div_u64_rem(delta, 1000000000ULL, &reminder);
	87	sec = quotient;
	88	nsec = reminder;
	89
	90	spin_lock_irqsave(&priv->ptp_lock, flags);
	91
	92	priv->hw->ptp->adjust_systime(priv->ioaddr, sec, nsec, neg_adj);
	93
7cd01399	94	spin_unlock_irqrestore(&priv->ptp_lock, flags);
92ba6888 RK	95
	96	return 0;
	97	}
	98
	99	/**
	100	* stmmac_get_time
	101	*
	102	* @ptp: pointer to ptp_clock_info structure
	103	* @ts: pointer to hold time/result
	104	*
	105	* Description: this function will read the current time from the
	106	* hardware clock and store it in @ts.
	107	*/
	108	static int stmmac_get_time(struct ptp_clock_info ptp, struct timespec ts)
	109	{
	110	struct stmmac_priv *priv =
	111	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	112	unsigned long flags;
	113	u64 ns;
	114	u32 reminder;
	115
	116	spin_lock_irqsave(&priv->ptp_lock, flags);
	117
	118	ns = priv->hw->ptp->get_systime(priv->ioaddr);
	119
	120	spin_unlock_irqrestore(&priv->ptp_lock, flags);
	121
	122	ts->tv_sec = div_u64_rem(ns, 1000000000ULL, &reminder);
	123	ts->tv_nsec = reminder;
	124
	125	return 0;
	126	}
	127
	128	/**
	129	* stmmac_set_time
	130	*
	131	* @ptp: pointer to ptp_clock_info structure
	132	* @ts: time value to set
	133	*
	134	* Description: this function will set the current time on the
	135	* hardware clock.
	136	*/
	137	static int stmmac_set_time(struct ptp_clock_info *ptp,
	138	const struct timespec *ts)
	139	{
	140	struct stmmac_priv *priv =
	141	container_of(ptp, struct stmmac_priv, ptp_clock_ops);
	142	unsigned long flags;
	143
	144	spin_lock_irqsave(&priv->ptp_lock, flags);
	145
	146	priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec);
	147
	148	spin_unlock_irqrestore(&priv->ptp_lock, flags);
	149
	150	return 0;
	151	}
	152
	153	static int stmmac_enable(struct ptp_clock_info *ptp,
	154	struct ptp_clock_request *rq, int on)
	155	{
	156	return -EOPNOTSUPP;
	157	}
	158
159	/* structure describing a PTP hardware clock */
160	static struct ptp_clock_info stmmac_ptp_clock_ops = {
161	.owner = THIS_MODULE,
162	.name = "stmmac_ptp_clock",
163	.max_adj = 62500000,
164	.n_alarm = 0,
165	.n_ext_ts = 0,
166	.n_per_out = 0,
4986b4f0	167	.n_pins = 0,
92ba6888 RK	168	.pps = 0,
	169	.adjfreq = stmmac_adjust_freq,
	170	.adjtime = stmmac_adjust_time,
	171	.gettime = stmmac_get_time,
	172	.settime = stmmac_set_time,
	173	.enable = stmmac_enable,
	174	};
	175
	176	/**
	177	* stmmac_ptp_register
32ceabca	178	* @priv: driver private structure
92ba6888 RK	179	* Description: this function will register the ptp clock driver
	180	* to kernel. It also does some house keeping work.
	181	*/
	182	int stmmac_ptp_register(struct stmmac_priv *priv)
	183	{
	184	spin_lock_init(&priv->ptp_lock);
	185	priv->ptp_clock_ops = stmmac_ptp_clock_ops;
	186
	187	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
	188	priv->device);
	189	if (IS_ERR(priv->ptp_clock)) {
	190	priv->ptp_clock = NULL;
	191	pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
	192	} else
	193	pr_debug("Added PTP HW clock successfully on %s\n",
	194	priv->dev->name);
	195
	196	return 0;
	197	}
	198
	199	/**
	200	* stmmac_ptp_unregister
32ceabca	201	* @priv: driver private structure
92ba6888 RK	202	* Description: this function will remove/unregister the ptp clock driver
	203	* from the kernel.
	204	*/
	205	void stmmac_ptp_unregister(struct stmmac_priv *priv)
	206	{
	207	if (priv->ptp_clock) {
	208	ptp_clock_unregister(priv->ptp_clock);
	209	pr_debug("Removed PTP HW clock successfully on %s\n",
	210	priv->dev->name);
	211	}
	212	}