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 */
35static 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
59	spin_unlock_irqrestore(&priv->ptp_lock, flags);
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 */
72static 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
94	spin_unlock_irqrestore(&priv->ptp_lock, flags);
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 */
108static int stmmac_get_time(struct ptp_clock_info *ptp, struct timespec64 *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
115	spin_lock_irqsave(&priv->ptp_lock, flags);
116
117	ns = priv->hw->ptp->get_systime(priv->ioaddr);
118
119	spin_unlock_irqrestore(&priv->ptp_lock, flags);
120
121	*ts = ns_to_timespec64(ns);
122
123	return 0;
124}
125
126/**
127 * stmmac_set_time
128 *
129 * @ptp: pointer to ptp_clock_info structure
130 * @ts: time value to set
131 *
132 * Description: this function will set the current time on the
133 * hardware clock.
134 */
135static int stmmac_set_time(struct ptp_clock_info *ptp,
136			   const struct timespec64 *ts)
137{
138	struct stmmac_priv *priv =
139	    container_of(ptp, struct stmmac_priv, ptp_clock_ops);
140	unsigned long flags;
141
142	spin_lock_irqsave(&priv->ptp_lock, flags);
143
144	priv->hw->ptp->init_systime(priv->ioaddr, ts->tv_sec, ts->tv_nsec);
145
146	spin_unlock_irqrestore(&priv->ptp_lock, flags);
147
148	return 0;
149}
150
151static int stmmac_enable(struct ptp_clock_info *ptp,
152			 struct ptp_clock_request *rq, int on)
153{
154	return -EOPNOTSUPP;
155}
156
157/* structure describing a PTP hardware clock */
158static struct ptp_clock_info stmmac_ptp_clock_ops = {
159	.owner = THIS_MODULE,
160	.name = "stmmac_ptp_clock",
161	.max_adj = 62500000,
162	.n_alarm = 0,
163	.n_ext_ts = 0,
164	.n_per_out = 0,
165	.n_pins = 0,
166	.pps = 0,
167	.adjfreq = stmmac_adjust_freq,
168	.adjtime = stmmac_adjust_time,
169	.gettime64 = stmmac_get_time,
170	.settime64 = stmmac_set_time,
171	.enable = stmmac_enable,
172};
173
174/**
175 * stmmac_ptp_register
176 * @priv: driver private structure
177 * Description: this function will register the ptp clock driver
178 * to kernel. It also does some house keeping work.
179 */
180int stmmac_ptp_register(struct stmmac_priv *priv)
181{
182	spin_lock_init(&priv->ptp_lock);
183	priv->ptp_clock_ops = stmmac_ptp_clock_ops;
184
185	priv->ptp_clock = ptp_clock_register(&priv->ptp_clock_ops,
186					     priv->device);
187	if (IS_ERR(priv->ptp_clock)) {
188		priv->ptp_clock = NULL;
189		pr_err("ptp_clock_register() failed on %s\n", priv->dev->name);
190	} else
191		pr_debug("Added PTP HW clock successfully on %s\n",
192			 priv->dev->name);
193
194	return 0;
195}
196
197/**
198 * stmmac_ptp_unregister
199 * @priv: driver private structure
200 * Description: this function will remove/unregister the ptp clock driver
201 * from the kernel.
202 */
203void stmmac_ptp_unregister(struct stmmac_priv *priv)
204{
205	if (priv->ptp_clock) {
206		ptp_clock_unregister(priv->ptp_clock);
207		priv->ptp_clock = NULL;
208		pr_debug("Removed PTP HW clock successfully on %s\n",
209			 priv->dev->name);
210	}
211}
212