1f0d4ba9eSKamil Alkhouri // SPDX-License-Identifier: (GPL-2.0 OR MIT)
2f0d4ba9eSKamil Alkhouri /*
3f0d4ba9eSKamil Alkhouri * DSA driver for:
4f0d4ba9eSKamil Alkhouri * Hirschmann Hellcreek TSN switch.
5f0d4ba9eSKamil Alkhouri *
6f0d4ba9eSKamil Alkhouri * Copyright (C) 2019,2020 Hochschule Offenburg
7f0d4ba9eSKamil Alkhouri * Copyright (C) 2019,2020 Linutronix GmbH
8f0d4ba9eSKamil Alkhouri * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
9f0d4ba9eSKamil Alkhouri * Kurt Kanzenbach <kurt@linutronix.de>
10f0d4ba9eSKamil Alkhouri */
11f0d4ba9eSKamil Alkhouri
12f0d4ba9eSKamil Alkhouri #include <linux/ptp_classify.h>
13f0d4ba9eSKamil Alkhouri
14f0d4ba9eSKamil Alkhouri #include "hellcreek.h"
15f0d4ba9eSKamil Alkhouri #include "hellcreek_hwtstamp.h"
16f0d4ba9eSKamil Alkhouri #include "hellcreek_ptp.h"
17f0d4ba9eSKamil Alkhouri
hellcreek_get_ts_info(struct dsa_switch * ds,int port,struct ethtool_ts_info * info)18f0d4ba9eSKamil Alkhouri int hellcreek_get_ts_info(struct dsa_switch *ds, int port,
19f0d4ba9eSKamil Alkhouri struct ethtool_ts_info *info)
20f0d4ba9eSKamil Alkhouri {
21f0d4ba9eSKamil Alkhouri struct hellcreek *hellcreek = ds->priv;
22f0d4ba9eSKamil Alkhouri
23f0d4ba9eSKamil Alkhouri info->phc_index = hellcreek->ptp_clock ?
24f0d4ba9eSKamil Alkhouri ptp_clock_index(hellcreek->ptp_clock) : -1;
25f0d4ba9eSKamil Alkhouri info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
26f0d4ba9eSKamil Alkhouri SOF_TIMESTAMPING_RX_HARDWARE |
27f0d4ba9eSKamil Alkhouri SOF_TIMESTAMPING_RAW_HARDWARE;
28f0d4ba9eSKamil Alkhouri
29f0d4ba9eSKamil Alkhouri /* enabled tx timestamping */
30f0d4ba9eSKamil Alkhouri info->tx_types = BIT(HWTSTAMP_TX_ON);
31f0d4ba9eSKamil Alkhouri
32f0d4ba9eSKamil Alkhouri /* L2 & L4 PTPv2 event rx messages are timestamped */
33f0d4ba9eSKamil Alkhouri info->rx_filters = BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
34f0d4ba9eSKamil Alkhouri
35f0d4ba9eSKamil Alkhouri return 0;
36f0d4ba9eSKamil Alkhouri }
37f0d4ba9eSKamil Alkhouri
38f0d4ba9eSKamil Alkhouri /* Enabling/disabling TX and RX HW timestamping for different PTP messages is
39f0d4ba9eSKamil Alkhouri * not available in the switch. Thus, this function only serves as a check if
40f0d4ba9eSKamil Alkhouri * the user requested what is actually available or not
41f0d4ba9eSKamil Alkhouri */
hellcreek_set_hwtstamp_config(struct hellcreek * hellcreek,int port,struct hwtstamp_config * config)42f0d4ba9eSKamil Alkhouri static int hellcreek_set_hwtstamp_config(struct hellcreek *hellcreek, int port,
43f0d4ba9eSKamil Alkhouri struct hwtstamp_config *config)
44f0d4ba9eSKamil Alkhouri {
45f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps =
46f0d4ba9eSKamil Alkhouri &hellcreek->ports[port].port_hwtstamp;
47f0d4ba9eSKamil Alkhouri bool tx_tstamp_enable = false;
48f0d4ba9eSKamil Alkhouri bool rx_tstamp_enable = false;
49f0d4ba9eSKamil Alkhouri
50f0d4ba9eSKamil Alkhouri /* Interaction with the timestamp hardware is prevented here. It is
51f0d4ba9eSKamil Alkhouri * enabled when this config function ends successfully
52f0d4ba9eSKamil Alkhouri */
53f0d4ba9eSKamil Alkhouri clear_bit_unlock(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
54f0d4ba9eSKamil Alkhouri
55f0d4ba9eSKamil Alkhouri switch (config->tx_type) {
56f0d4ba9eSKamil Alkhouri case HWTSTAMP_TX_ON:
57f0d4ba9eSKamil Alkhouri tx_tstamp_enable = true;
58f0d4ba9eSKamil Alkhouri break;
59f0d4ba9eSKamil Alkhouri
60f0d4ba9eSKamil Alkhouri /* TX HW timestamping can't be disabled on the switch */
61f0d4ba9eSKamil Alkhouri case HWTSTAMP_TX_OFF:
62f0d4ba9eSKamil Alkhouri config->tx_type = HWTSTAMP_TX_ON;
63f0d4ba9eSKamil Alkhouri break;
64f0d4ba9eSKamil Alkhouri
65f0d4ba9eSKamil Alkhouri default:
66f0d4ba9eSKamil Alkhouri return -ERANGE;
67f0d4ba9eSKamil Alkhouri }
68f0d4ba9eSKamil Alkhouri
69f0d4ba9eSKamil Alkhouri switch (config->rx_filter) {
70f0d4ba9eSKamil Alkhouri /* RX HW timestamping can't be disabled on the switch */
71f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_NONE:
72f0d4ba9eSKamil Alkhouri config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
73f0d4ba9eSKamil Alkhouri break;
74f0d4ba9eSKamil Alkhouri
75f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
76f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
77f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
78f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
79f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
80f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
81f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_EVENT:
82f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_SYNC:
83f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
84f0d4ba9eSKamil Alkhouri config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
85f0d4ba9eSKamil Alkhouri rx_tstamp_enable = true;
86f0d4ba9eSKamil Alkhouri break;
87f0d4ba9eSKamil Alkhouri
88f0d4ba9eSKamil Alkhouri /* RX HW timestamping can't be enabled for all messages on the switch */
89f0d4ba9eSKamil Alkhouri case HWTSTAMP_FILTER_ALL:
90f0d4ba9eSKamil Alkhouri config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
91f0d4ba9eSKamil Alkhouri break;
92f0d4ba9eSKamil Alkhouri
93f0d4ba9eSKamil Alkhouri default:
94f0d4ba9eSKamil Alkhouri return -ERANGE;
95f0d4ba9eSKamil Alkhouri }
96f0d4ba9eSKamil Alkhouri
97f0d4ba9eSKamil Alkhouri if (!tx_tstamp_enable)
98f0d4ba9eSKamil Alkhouri return -ERANGE;
99f0d4ba9eSKamil Alkhouri
100f0d4ba9eSKamil Alkhouri if (!rx_tstamp_enable)
101f0d4ba9eSKamil Alkhouri return -ERANGE;
102f0d4ba9eSKamil Alkhouri
103f0d4ba9eSKamil Alkhouri /* If this point is reached, then the requested hwtstamp config is
104f0d4ba9eSKamil Alkhouri * compatible with the hwtstamp offered by the switch. Therefore,
105f0d4ba9eSKamil Alkhouri * enable the interaction with the HW timestamping
106f0d4ba9eSKamil Alkhouri */
107f0d4ba9eSKamil Alkhouri set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
108f0d4ba9eSKamil Alkhouri
109f0d4ba9eSKamil Alkhouri return 0;
110f0d4ba9eSKamil Alkhouri }
111f0d4ba9eSKamil Alkhouri
hellcreek_port_hwtstamp_set(struct dsa_switch * ds,int port,struct ifreq * ifr)112f0d4ba9eSKamil Alkhouri int hellcreek_port_hwtstamp_set(struct dsa_switch *ds, int port,
113f0d4ba9eSKamil Alkhouri struct ifreq *ifr)
114f0d4ba9eSKamil Alkhouri {
115f0d4ba9eSKamil Alkhouri struct hellcreek *hellcreek = ds->priv;
116f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps;
117f0d4ba9eSKamil Alkhouri struct hwtstamp_config config;
118f0d4ba9eSKamil Alkhouri int err;
119f0d4ba9eSKamil Alkhouri
120f0d4ba9eSKamil Alkhouri ps = &hellcreek->ports[port].port_hwtstamp;
121f0d4ba9eSKamil Alkhouri
122f0d4ba9eSKamil Alkhouri if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
123f0d4ba9eSKamil Alkhouri return -EFAULT;
124f0d4ba9eSKamil Alkhouri
125f0d4ba9eSKamil Alkhouri err = hellcreek_set_hwtstamp_config(hellcreek, port, &config);
126f0d4ba9eSKamil Alkhouri if (err)
127f0d4ba9eSKamil Alkhouri return err;
128f0d4ba9eSKamil Alkhouri
129f0d4ba9eSKamil Alkhouri /* Save the chosen configuration to be returned later */
130f0d4ba9eSKamil Alkhouri memcpy(&ps->tstamp_config, &config, sizeof(config));
131f0d4ba9eSKamil Alkhouri
132f0d4ba9eSKamil Alkhouri return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
133f0d4ba9eSKamil Alkhouri -EFAULT : 0;
134f0d4ba9eSKamil Alkhouri }
135f0d4ba9eSKamil Alkhouri
hellcreek_port_hwtstamp_get(struct dsa_switch * ds,int port,struct ifreq * ifr)136f0d4ba9eSKamil Alkhouri int hellcreek_port_hwtstamp_get(struct dsa_switch *ds, int port,
137f0d4ba9eSKamil Alkhouri struct ifreq *ifr)
138f0d4ba9eSKamil Alkhouri {
139f0d4ba9eSKamil Alkhouri struct hellcreek *hellcreek = ds->priv;
140f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps;
141f0d4ba9eSKamil Alkhouri struct hwtstamp_config *config;
142f0d4ba9eSKamil Alkhouri
143f0d4ba9eSKamil Alkhouri ps = &hellcreek->ports[port].port_hwtstamp;
144f0d4ba9eSKamil Alkhouri config = &ps->tstamp_config;
145f0d4ba9eSKamil Alkhouri
146f0d4ba9eSKamil Alkhouri return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
147f0d4ba9eSKamil Alkhouri -EFAULT : 0;
148f0d4ba9eSKamil Alkhouri }
149f0d4ba9eSKamil Alkhouri
150f0d4ba9eSKamil Alkhouri /* Returns a pointer to the PTP header if the caller should time stamp, or NULL
151f0d4ba9eSKamil Alkhouri * if the caller should not.
152f0d4ba9eSKamil Alkhouri */
hellcreek_should_tstamp(struct hellcreek * hellcreek,int port,struct sk_buff * skb,unsigned int type)153f0d4ba9eSKamil Alkhouri static struct ptp_header *hellcreek_should_tstamp(struct hellcreek *hellcreek,
154f0d4ba9eSKamil Alkhouri int port, struct sk_buff *skb,
155f0d4ba9eSKamil Alkhouri unsigned int type)
156f0d4ba9eSKamil Alkhouri {
157f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps =
158f0d4ba9eSKamil Alkhouri &hellcreek->ports[port].port_hwtstamp;
159f0d4ba9eSKamil Alkhouri struct ptp_header *hdr;
160f0d4ba9eSKamil Alkhouri
161f0d4ba9eSKamil Alkhouri hdr = ptp_parse_header(skb, type);
162f0d4ba9eSKamil Alkhouri if (!hdr)
163f0d4ba9eSKamil Alkhouri return NULL;
164f0d4ba9eSKamil Alkhouri
165f0d4ba9eSKamil Alkhouri if (!test_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state))
166f0d4ba9eSKamil Alkhouri return NULL;
167f0d4ba9eSKamil Alkhouri
168f0d4ba9eSKamil Alkhouri return hdr;
169f0d4ba9eSKamil Alkhouri }
170f0d4ba9eSKamil Alkhouri
hellcreek_get_reserved_field(const struct ptp_header * hdr)171f0d4ba9eSKamil Alkhouri static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
172f0d4ba9eSKamil Alkhouri {
173f0d4ba9eSKamil Alkhouri return be32_to_cpu(hdr->reserved2);
174f0d4ba9eSKamil Alkhouri }
175f0d4ba9eSKamil Alkhouri
hellcreek_clear_reserved_field(struct ptp_header * hdr)176f0d4ba9eSKamil Alkhouri static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
177f0d4ba9eSKamil Alkhouri {
178f0d4ba9eSKamil Alkhouri hdr->reserved2 = 0;
179f0d4ba9eSKamil Alkhouri }
180f0d4ba9eSKamil Alkhouri
hellcreek_ptp_hwtstamp_available(struct hellcreek * hellcreek,unsigned int ts_reg)181f0d4ba9eSKamil Alkhouri static int hellcreek_ptp_hwtstamp_available(struct hellcreek *hellcreek,
182f0d4ba9eSKamil Alkhouri unsigned int ts_reg)
183f0d4ba9eSKamil Alkhouri {
184f0d4ba9eSKamil Alkhouri u16 status;
185f0d4ba9eSKamil Alkhouri
186f0d4ba9eSKamil Alkhouri status = hellcreek_ptp_read(hellcreek, ts_reg);
187f0d4ba9eSKamil Alkhouri
188f0d4ba9eSKamil Alkhouri if (status & PR_TS_STATUS_TS_LOST)
189f0d4ba9eSKamil Alkhouri dev_err(hellcreek->dev,
190f0d4ba9eSKamil Alkhouri "Tx time stamp lost! This should never happen!\n");
191f0d4ba9eSKamil Alkhouri
192f0d4ba9eSKamil Alkhouri /* If hwtstamp is not available, this means the previous hwtstamp was
193f0d4ba9eSKamil Alkhouri * successfully read, and the one we need is not yet available
194f0d4ba9eSKamil Alkhouri */
195f0d4ba9eSKamil Alkhouri return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0;
196f0d4ba9eSKamil Alkhouri }
197f0d4ba9eSKamil Alkhouri
198f0d4ba9eSKamil Alkhouri /* Get nanoseconds timestamp from timestamping unit */
hellcreek_ptp_hwtstamp_read(struct hellcreek * hellcreek,unsigned int ts_reg)199f0d4ba9eSKamil Alkhouri static u64 hellcreek_ptp_hwtstamp_read(struct hellcreek *hellcreek,
200f0d4ba9eSKamil Alkhouri unsigned int ts_reg)
201f0d4ba9eSKamil Alkhouri {
202f0d4ba9eSKamil Alkhouri u16 nsl, nsh;
203f0d4ba9eSKamil Alkhouri
204f0d4ba9eSKamil Alkhouri nsh = hellcreek_ptp_read(hellcreek, ts_reg);
205f0d4ba9eSKamil Alkhouri nsh = hellcreek_ptp_read(hellcreek, ts_reg);
206f0d4ba9eSKamil Alkhouri nsh = hellcreek_ptp_read(hellcreek, ts_reg);
207f0d4ba9eSKamil Alkhouri nsh = hellcreek_ptp_read(hellcreek, ts_reg);
208f0d4ba9eSKamil Alkhouri nsl = hellcreek_ptp_read(hellcreek, ts_reg);
209f0d4ba9eSKamil Alkhouri
210f0d4ba9eSKamil Alkhouri return (u64)nsl | ((u64)nsh << 16);
211f0d4ba9eSKamil Alkhouri }
212f0d4ba9eSKamil Alkhouri
hellcreek_txtstamp_work(struct hellcreek * hellcreek,struct hellcreek_port_hwtstamp * ps,int port)213f0d4ba9eSKamil Alkhouri static int hellcreek_txtstamp_work(struct hellcreek *hellcreek,
214f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps, int port)
215f0d4ba9eSKamil Alkhouri {
216f0d4ba9eSKamil Alkhouri struct skb_shared_hwtstamps shhwtstamps;
217f0d4ba9eSKamil Alkhouri unsigned int status_reg, data_reg;
218f0d4ba9eSKamil Alkhouri struct sk_buff *tmp_skb;
219f0d4ba9eSKamil Alkhouri int ts_status;
220f0d4ba9eSKamil Alkhouri u64 ns = 0;
221f0d4ba9eSKamil Alkhouri
222f0d4ba9eSKamil Alkhouri if (!ps->tx_skb)
223f0d4ba9eSKamil Alkhouri return 0;
224f0d4ba9eSKamil Alkhouri
225f0d4ba9eSKamil Alkhouri switch (port) {
226f0d4ba9eSKamil Alkhouri case 2:
227f0d4ba9eSKamil Alkhouri status_reg = PR_TS_TX_P1_STATUS_C;
228f0d4ba9eSKamil Alkhouri data_reg = PR_TS_TX_P1_DATA_C;
229f0d4ba9eSKamil Alkhouri break;
230f0d4ba9eSKamil Alkhouri case 3:
231f0d4ba9eSKamil Alkhouri status_reg = PR_TS_TX_P2_STATUS_C;
232f0d4ba9eSKamil Alkhouri data_reg = PR_TS_TX_P2_DATA_C;
233f0d4ba9eSKamil Alkhouri break;
234f0d4ba9eSKamil Alkhouri default:
235f0d4ba9eSKamil Alkhouri dev_err(hellcreek->dev, "Wrong port for timestamping!\n");
236f0d4ba9eSKamil Alkhouri return 0;
237f0d4ba9eSKamil Alkhouri }
238f0d4ba9eSKamil Alkhouri
239f0d4ba9eSKamil Alkhouri ts_status = hellcreek_ptp_hwtstamp_available(hellcreek, status_reg);
240f0d4ba9eSKamil Alkhouri
241f0d4ba9eSKamil Alkhouri /* Not available yet? */
242f0d4ba9eSKamil Alkhouri if (ts_status == 0) {
243f0d4ba9eSKamil Alkhouri /* Check whether the operation of reading the tx timestamp has
244f0d4ba9eSKamil Alkhouri * exceeded its allowed period
245f0d4ba9eSKamil Alkhouri */
246f0d4ba9eSKamil Alkhouri if (time_is_before_jiffies(ps->tx_tstamp_start +
247f0d4ba9eSKamil Alkhouri TX_TSTAMP_TIMEOUT)) {
248f0d4ba9eSKamil Alkhouri dev_err(hellcreek->dev,
249f0d4ba9eSKamil Alkhouri "Timeout while waiting for Tx timestamp!\n");
250f0d4ba9eSKamil Alkhouri goto free_and_clear_skb;
251f0d4ba9eSKamil Alkhouri }
252f0d4ba9eSKamil Alkhouri
253f0d4ba9eSKamil Alkhouri /* The timestamp should be available quickly, while getting it
254f0d4ba9eSKamil Alkhouri * in high priority. Restart the work
255f0d4ba9eSKamil Alkhouri */
256f0d4ba9eSKamil Alkhouri return 1;
257f0d4ba9eSKamil Alkhouri }
258f0d4ba9eSKamil Alkhouri
259f0d4ba9eSKamil Alkhouri mutex_lock(&hellcreek->ptp_lock);
260f0d4ba9eSKamil Alkhouri ns = hellcreek_ptp_hwtstamp_read(hellcreek, data_reg);
261f0d4ba9eSKamil Alkhouri ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
262f0d4ba9eSKamil Alkhouri mutex_unlock(&hellcreek->ptp_lock);
263f0d4ba9eSKamil Alkhouri
264f0d4ba9eSKamil Alkhouri /* Now we have the timestamp in nanoseconds, store it in the correct
265f0d4ba9eSKamil Alkhouri * structure in order to send it to the user
266f0d4ba9eSKamil Alkhouri */
267f0d4ba9eSKamil Alkhouri memset(&shhwtstamps, 0, sizeof(shhwtstamps));
268f0d4ba9eSKamil Alkhouri shhwtstamps.hwtstamp = ns_to_ktime(ns);
269f0d4ba9eSKamil Alkhouri
270f0d4ba9eSKamil Alkhouri tmp_skb = ps->tx_skb;
271f0d4ba9eSKamil Alkhouri ps->tx_skb = NULL;
272f0d4ba9eSKamil Alkhouri
273f0d4ba9eSKamil Alkhouri /* skb_complete_tx_timestamp() frees up the client to make another
274f0d4ba9eSKamil Alkhouri * timestampable transmit. We have to be ready for it by clearing the
275f0d4ba9eSKamil Alkhouri * ps->tx_skb "flag" beforehand
276f0d4ba9eSKamil Alkhouri */
277f0d4ba9eSKamil Alkhouri clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
278f0d4ba9eSKamil Alkhouri
279f0d4ba9eSKamil Alkhouri /* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */
280f0d4ba9eSKamil Alkhouri skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
281f0d4ba9eSKamil Alkhouri
282f0d4ba9eSKamil Alkhouri return 0;
283f0d4ba9eSKamil Alkhouri
284f0d4ba9eSKamil Alkhouri free_and_clear_skb:
285f0d4ba9eSKamil Alkhouri dev_kfree_skb_any(ps->tx_skb);
286f0d4ba9eSKamil Alkhouri ps->tx_skb = NULL;
287f0d4ba9eSKamil Alkhouri clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
288f0d4ba9eSKamil Alkhouri
289f0d4ba9eSKamil Alkhouri return 0;
290f0d4ba9eSKamil Alkhouri }
291f0d4ba9eSKamil Alkhouri
hellcreek_get_rxts(struct hellcreek * hellcreek,struct hellcreek_port_hwtstamp * ps,struct sk_buff * skb,struct sk_buff_head * rxq,int port)292f0d4ba9eSKamil Alkhouri static void hellcreek_get_rxts(struct hellcreek *hellcreek,
293f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps,
294f0d4ba9eSKamil Alkhouri struct sk_buff *skb, struct sk_buff_head *rxq,
295f0d4ba9eSKamil Alkhouri int port)
296f0d4ba9eSKamil Alkhouri {
297f0d4ba9eSKamil Alkhouri struct skb_shared_hwtstamps *shwt;
298f0d4ba9eSKamil Alkhouri struct sk_buff_head received;
299f0d4ba9eSKamil Alkhouri unsigned long flags;
300f0d4ba9eSKamil Alkhouri
301*ae3683a3SKurt Kanzenbach /* Construct Rx timestamps for all received PTP packets. */
302f0d4ba9eSKamil Alkhouri __skb_queue_head_init(&received);
303f0d4ba9eSKamil Alkhouri spin_lock_irqsave(&rxq->lock, flags);
304f0d4ba9eSKamil Alkhouri skb_queue_splice_tail_init(rxq, &received);
305f0d4ba9eSKamil Alkhouri spin_unlock_irqrestore(&rxq->lock, flags);
306f0d4ba9eSKamil Alkhouri
307f0d4ba9eSKamil Alkhouri for (; skb; skb = __skb_dequeue(&received)) {
308f0d4ba9eSKamil Alkhouri struct ptp_header *hdr;
309f0d4ba9eSKamil Alkhouri unsigned int type;
310f0d4ba9eSKamil Alkhouri u64 ns;
311f0d4ba9eSKamil Alkhouri
312f0d4ba9eSKamil Alkhouri /* Get nanoseconds from ptp packet */
313f0d4ba9eSKamil Alkhouri type = SKB_PTP_TYPE(skb);
314f0d4ba9eSKamil Alkhouri hdr = ptp_parse_header(skb, type);
315f0d4ba9eSKamil Alkhouri ns = hellcreek_get_reserved_field(hdr);
316f0d4ba9eSKamil Alkhouri hellcreek_clear_reserved_field(hdr);
317f0d4ba9eSKamil Alkhouri
318f0d4ba9eSKamil Alkhouri /* Add seconds part */
319f0d4ba9eSKamil Alkhouri mutex_lock(&hellcreek->ptp_lock);
320f0d4ba9eSKamil Alkhouri ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
321f0d4ba9eSKamil Alkhouri mutex_unlock(&hellcreek->ptp_lock);
322f0d4ba9eSKamil Alkhouri
323f0d4ba9eSKamil Alkhouri /* Save time stamp */
324f0d4ba9eSKamil Alkhouri shwt = skb_hwtstamps(skb);
325f0d4ba9eSKamil Alkhouri memset(shwt, 0, sizeof(*shwt));
326f0d4ba9eSKamil Alkhouri shwt->hwtstamp = ns_to_ktime(ns);
327db00cc9dSSebastian Andrzej Siewior netif_rx(skb);
328f0d4ba9eSKamil Alkhouri }
329f0d4ba9eSKamil Alkhouri }
330f0d4ba9eSKamil Alkhouri
hellcreek_rxtstamp_work(struct hellcreek * hellcreek,struct hellcreek_port_hwtstamp * ps,int port)331f0d4ba9eSKamil Alkhouri static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek,
332f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps,
333f0d4ba9eSKamil Alkhouri int port)
334f0d4ba9eSKamil Alkhouri {
335f0d4ba9eSKamil Alkhouri struct sk_buff *skb;
336f0d4ba9eSKamil Alkhouri
337f0d4ba9eSKamil Alkhouri skb = skb_dequeue(&ps->rx_queue);
338f0d4ba9eSKamil Alkhouri if (skb)
339f0d4ba9eSKamil Alkhouri hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port);
340f0d4ba9eSKamil Alkhouri }
341f0d4ba9eSKamil Alkhouri
hellcreek_hwtstamp_work(struct ptp_clock_info * ptp)342f0d4ba9eSKamil Alkhouri long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp)
343f0d4ba9eSKamil Alkhouri {
344f0d4ba9eSKamil Alkhouri struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
345f0d4ba9eSKamil Alkhouri struct dsa_switch *ds = hellcreek->ds;
346f0d4ba9eSKamil Alkhouri int i, restart = 0;
347f0d4ba9eSKamil Alkhouri
348f0d4ba9eSKamil Alkhouri for (i = 0; i < ds->num_ports; i++) {
349f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps;
350f0d4ba9eSKamil Alkhouri
351f0d4ba9eSKamil Alkhouri if (!dsa_is_user_port(ds, i))
352f0d4ba9eSKamil Alkhouri continue;
353f0d4ba9eSKamil Alkhouri
354f0d4ba9eSKamil Alkhouri ps = &hellcreek->ports[i].port_hwtstamp;
355f0d4ba9eSKamil Alkhouri
356f0d4ba9eSKamil Alkhouri if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
357f0d4ba9eSKamil Alkhouri restart |= hellcreek_txtstamp_work(hellcreek, ps, i);
358f0d4ba9eSKamil Alkhouri
359f0d4ba9eSKamil Alkhouri hellcreek_rxtstamp_work(hellcreek, ps, i);
360f0d4ba9eSKamil Alkhouri }
361f0d4ba9eSKamil Alkhouri
362f0d4ba9eSKamil Alkhouri return restart ? 1 : -1;
363f0d4ba9eSKamil Alkhouri }
364f0d4ba9eSKamil Alkhouri
hellcreek_port_txtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb)3655c5416f5SYangbo Lu void hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
3665c5416f5SYangbo Lu struct sk_buff *skb)
367f0d4ba9eSKamil Alkhouri {
368f0d4ba9eSKamil Alkhouri struct hellcreek *hellcreek = ds->priv;
369f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps;
370f0d4ba9eSKamil Alkhouri struct ptp_header *hdr;
3715c5416f5SYangbo Lu struct sk_buff *clone;
372cf536ea3SYangbo Lu unsigned int type;
373f0d4ba9eSKamil Alkhouri
374f0d4ba9eSKamil Alkhouri ps = &hellcreek->ports[port].port_hwtstamp;
375f0d4ba9eSKamil Alkhouri
3765c5416f5SYangbo Lu type = ptp_classify_raw(skb);
377cf536ea3SYangbo Lu if (type == PTP_CLASS_NONE)
3785c5416f5SYangbo Lu return;
379cf536ea3SYangbo Lu
380f0d4ba9eSKamil Alkhouri /* Make sure the message is a PTP message that needs to be timestamped
381f0d4ba9eSKamil Alkhouri * and the interaction with the HW timestamping is enabled. If not, stop
382f0d4ba9eSKamil Alkhouri * here
383f0d4ba9eSKamil Alkhouri */
3845c5416f5SYangbo Lu hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
385f0d4ba9eSKamil Alkhouri if (!hdr)
3865c5416f5SYangbo Lu return;
3875c5416f5SYangbo Lu
3885c5416f5SYangbo Lu clone = skb_clone_sk(skb);
3895c5416f5SYangbo Lu if (!clone)
3905c5416f5SYangbo Lu return;
391f0d4ba9eSKamil Alkhouri
392f0d4ba9eSKamil Alkhouri if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
3935c5416f5SYangbo Lu &ps->state)) {
3945c5416f5SYangbo Lu kfree_skb(clone);
3955c5416f5SYangbo Lu return;
3965c5416f5SYangbo Lu }
397f0d4ba9eSKamil Alkhouri
398f0d4ba9eSKamil Alkhouri ps->tx_skb = clone;
399f0d4ba9eSKamil Alkhouri
400f0d4ba9eSKamil Alkhouri /* store the number of ticks occurred since system start-up till this
401f0d4ba9eSKamil Alkhouri * moment
402f0d4ba9eSKamil Alkhouri */
403f0d4ba9eSKamil Alkhouri ps->tx_tstamp_start = jiffies;
404f0d4ba9eSKamil Alkhouri
405f0d4ba9eSKamil Alkhouri ptp_schedule_worker(hellcreek->ptp_clock, 0);
406f0d4ba9eSKamil Alkhouri }
407f0d4ba9eSKamil Alkhouri
hellcreek_port_rxtstamp(struct dsa_switch * ds,int port,struct sk_buff * skb,unsigned int type)408f0d4ba9eSKamil Alkhouri bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port,
409f0d4ba9eSKamil Alkhouri struct sk_buff *skb, unsigned int type)
410f0d4ba9eSKamil Alkhouri {
411f0d4ba9eSKamil Alkhouri struct hellcreek *hellcreek = ds->priv;
412f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps;
413f0d4ba9eSKamil Alkhouri struct ptp_header *hdr;
414f0d4ba9eSKamil Alkhouri
415f0d4ba9eSKamil Alkhouri ps = &hellcreek->ports[port].port_hwtstamp;
416f0d4ba9eSKamil Alkhouri
417f0d4ba9eSKamil Alkhouri /* This check only fails if the user did not initialize hardware
418f0d4ba9eSKamil Alkhouri * timestamping beforehand.
419f0d4ba9eSKamil Alkhouri */
420f0d4ba9eSKamil Alkhouri if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
421f0d4ba9eSKamil Alkhouri return false;
422f0d4ba9eSKamil Alkhouri
423f0d4ba9eSKamil Alkhouri /* Make sure the message is a PTP message that needs to be timestamped
424f0d4ba9eSKamil Alkhouri * and the interaction with the HW timestamping is enabled. If not, stop
425f0d4ba9eSKamil Alkhouri * here
426f0d4ba9eSKamil Alkhouri */
427f0d4ba9eSKamil Alkhouri hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
428f0d4ba9eSKamil Alkhouri if (!hdr)
429f0d4ba9eSKamil Alkhouri return false;
430f0d4ba9eSKamil Alkhouri
431f0d4ba9eSKamil Alkhouri SKB_PTP_TYPE(skb) = type;
432f0d4ba9eSKamil Alkhouri
433f0d4ba9eSKamil Alkhouri skb_queue_tail(&ps->rx_queue, skb);
434f0d4ba9eSKamil Alkhouri
435f0d4ba9eSKamil Alkhouri ptp_schedule_worker(hellcreek->ptp_clock, 0);
436f0d4ba9eSKamil Alkhouri
437f0d4ba9eSKamil Alkhouri return true;
438f0d4ba9eSKamil Alkhouri }
439f0d4ba9eSKamil Alkhouri
hellcreek_hwtstamp_port_setup(struct hellcreek * hellcreek,int port)440f0d4ba9eSKamil Alkhouri static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port)
441f0d4ba9eSKamil Alkhouri {
442f0d4ba9eSKamil Alkhouri struct hellcreek_port_hwtstamp *ps =
443f0d4ba9eSKamil Alkhouri &hellcreek->ports[port].port_hwtstamp;
444f0d4ba9eSKamil Alkhouri
445f0d4ba9eSKamil Alkhouri skb_queue_head_init(&ps->rx_queue);
446f0d4ba9eSKamil Alkhouri }
447f0d4ba9eSKamil Alkhouri
hellcreek_hwtstamp_setup(struct hellcreek * hellcreek)448f0d4ba9eSKamil Alkhouri int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek)
449f0d4ba9eSKamil Alkhouri {
450f0d4ba9eSKamil Alkhouri struct dsa_switch *ds = hellcreek->ds;
451f0d4ba9eSKamil Alkhouri int i;
452f0d4ba9eSKamil Alkhouri
453f0d4ba9eSKamil Alkhouri /* Initialize timestamping ports. */
454f0d4ba9eSKamil Alkhouri for (i = 0; i < ds->num_ports; ++i) {
455f0d4ba9eSKamil Alkhouri if (!dsa_is_user_port(ds, i))
456f0d4ba9eSKamil Alkhouri continue;
457f0d4ba9eSKamil Alkhouri
458f0d4ba9eSKamil Alkhouri hellcreek_hwtstamp_port_setup(hellcreek, i);
459f0d4ba9eSKamil Alkhouri }
460f0d4ba9eSKamil Alkhouri
461f0d4ba9eSKamil Alkhouri /* Select the synchronized clock as the source timekeeper for the
462f0d4ba9eSKamil Alkhouri * timestamps and enable inline timestamping.
463f0d4ba9eSKamil Alkhouri */
464f0d4ba9eSKamil Alkhouri hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK |
465f0d4ba9eSKamil Alkhouri PR_SETTINGS_C_RES3TS,
466f0d4ba9eSKamil Alkhouri PR_SETTINGS_C);
467f0d4ba9eSKamil Alkhouri
468f0d4ba9eSKamil Alkhouri return 0;
469f0d4ba9eSKamil Alkhouri }
470f0d4ba9eSKamil Alkhouri
hellcreek_hwtstamp_free(struct hellcreek * hellcreek)471f0d4ba9eSKamil Alkhouri void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
472f0d4ba9eSKamil Alkhouri {
473f0d4ba9eSKamil Alkhouri /* Nothing todo */
474f0d4ba9eSKamil Alkhouri }
475