--- /dev/null
+// SPDX-License-Identifier: (GPL-2.0 OR MIT)
+/*
+ * DSA driver for:
+ * Hirschmann Hellcreek TSN switch.
+ *
+ * Copyright (C) 2019,2020 Hochschule Offenburg
+ * Copyright (C) 2019,2020 Linutronix GmbH
+ * Authors: Kamil Alkhouri <kamil.alkhouri@hs-offenburg.de>
+ * Kurt Kanzenbach <kurt@linutronix.de>
+ */
+
+#include <linux/ptp_classify.h>
+
+#include "hellcreek.h"
+#include "hellcreek_hwtstamp.h"
+#include "hellcreek_ptp.h"
+
+int hellcreek_get_ts_info(struct dsa_switch *ds, int port,
+ struct ethtool_ts_info *info)
+{
+ struct hellcreek *hellcreek = ds->priv;
+
+ info->phc_index = hellcreek->ptp_clock ?
+ ptp_clock_index(hellcreek->ptp_clock) : -1;
+ info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
+ SOF_TIMESTAMPING_RX_HARDWARE |
+ SOF_TIMESTAMPING_RAW_HARDWARE;
+
+ /* enabled tx timestamping */
+ info->tx_types = BIT(HWTSTAMP_TX_ON);
+
+ /* L2 & L4 PTPv2 event rx messages are timestamped */
+ info->rx_filters = BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
+
+ return 0;
+}
+
+/* Enabling/disabling TX and RX HW timestamping for different PTP messages is
+ * not available in the switch. Thus, this function only serves as a check if
+ * the user requested what is actually available or not
+ */
+static int hellcreek_set_hwtstamp_config(struct hellcreek *hellcreek, int port,
+ struct hwtstamp_config *config)
+{
+ struct hellcreek_port_hwtstamp *ps =
+ &hellcreek->ports[port].port_hwtstamp;
+ bool tx_tstamp_enable = false;
+ bool rx_tstamp_enable = false;
+
+ /* Interaction with the timestamp hardware is prevented here. It is
+ * enabled when this config function ends successfully
+ */
+ clear_bit_unlock(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
+
+ /* Reserved for future extensions */
+ if (config->flags)
+ return -EINVAL;
+
+ switch (config->tx_type) {
+ case HWTSTAMP_TX_ON:
+ tx_tstamp_enable = true;
+ break;
+
+ /* TX HW timestamping can't be disabled on the switch */
+ case HWTSTAMP_TX_OFF:
+ config->tx_type = HWTSTAMP_TX_ON;
+ break;
+
+ default:
+ return -ERANGE;
+ }
+
+ switch (config->rx_filter) {
+ /* RX HW timestamping can't be disabled on the switch */
+ case HWTSTAMP_FILTER_NONE:
+ config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+ break;
+
+ case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
+ case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
+ case HWTSTAMP_FILTER_PTP_V2_EVENT:
+ case HWTSTAMP_FILTER_PTP_V2_SYNC:
+ case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
+ config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+ rx_tstamp_enable = true;
+ break;
+
+ /* RX HW timestamping can't be enabled for all messages on the switch */
+ case HWTSTAMP_FILTER_ALL:
+ config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
+ break;
+
+ default:
+ return -ERANGE;
+ }
+
+ if (!tx_tstamp_enable)
+ return -ERANGE;
+
+ if (!rx_tstamp_enable)
+ return -ERANGE;
+
+ /* If this point is reached, then the requested hwtstamp config is
+ * compatible with the hwtstamp offered by the switch. Therefore,
+ * enable the interaction with the HW timestamping
+ */
+ set_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state);
+
+ return 0;
+}
+
+int hellcreek_port_hwtstamp_set(struct dsa_switch *ds, int port,
+ struct ifreq *ifr)
+{
+ struct hellcreek *hellcreek = ds->priv;
+ struct hellcreek_port_hwtstamp *ps;
+ struct hwtstamp_config config;
+ int err;
+
+ ps = &hellcreek->ports[port].port_hwtstamp;
+
+ if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
+ return -EFAULT;
+
+ err = hellcreek_set_hwtstamp_config(hellcreek, port, &config);
+ if (err)
+ return err;
+
+ /* Save the chosen configuration to be returned later */
+ memcpy(&ps->tstamp_config, &config, sizeof(config));
+
+ return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
+ -EFAULT : 0;
+}
+
+int hellcreek_port_hwtstamp_get(struct dsa_switch *ds, int port,
+ struct ifreq *ifr)
+{
+ struct hellcreek *hellcreek = ds->priv;
+ struct hellcreek_port_hwtstamp *ps;
+ struct hwtstamp_config *config;
+
+ ps = &hellcreek->ports[port].port_hwtstamp;
+ config = &ps->tstamp_config;
+
+ return copy_to_user(ifr->ifr_data, config, sizeof(*config)) ?
+ -EFAULT : 0;
+}
+
+/* Returns a pointer to the PTP header if the caller should time stamp, or NULL
+ * if the caller should not.
+ */
+static struct ptp_header *hellcreek_should_tstamp(struct hellcreek *hellcreek,
+ int port, struct sk_buff *skb,
+ unsigned int type)
+{
+ struct hellcreek_port_hwtstamp *ps =
+ &hellcreek->ports[port].port_hwtstamp;
+ struct ptp_header *hdr;
+
+ hdr = ptp_parse_header(skb, type);
+ if (!hdr)
+ return NULL;
+
+ if (!test_bit(HELLCREEK_HWTSTAMP_ENABLED, &ps->state))
+ return NULL;
+
+ return hdr;
+}
+
+static u64 hellcreek_get_reserved_field(const struct ptp_header *hdr)
+{
+ return be32_to_cpu(hdr->reserved2);
+}
+
+static void hellcreek_clear_reserved_field(struct ptp_header *hdr)
+{
+ hdr->reserved2 = 0;
+}
+
+static int hellcreek_ptp_hwtstamp_available(struct hellcreek *hellcreek,
+ unsigned int ts_reg)
+{
+ u16 status;
+
+ status = hellcreek_ptp_read(hellcreek, ts_reg);
+
+ if (status & PR_TS_STATUS_TS_LOST)
+ dev_err(hellcreek->dev,
+ "Tx time stamp lost! This should never happen!\n");
+
+ /* If hwtstamp is not available, this means the previous hwtstamp was
+ * successfully read, and the one we need is not yet available
+ */
+ return (status & PR_TS_STATUS_TS_AVAIL) ? 1 : 0;
+}
+
+/* Get nanoseconds timestamp from timestamping unit */
+static u64 hellcreek_ptp_hwtstamp_read(struct hellcreek *hellcreek,
+ unsigned int ts_reg)
+{
+ u16 nsl, nsh;
+
+ nsh = hellcreek_ptp_read(hellcreek, ts_reg);
+ nsh = hellcreek_ptp_read(hellcreek, ts_reg);
+ nsh = hellcreek_ptp_read(hellcreek, ts_reg);
+ nsh = hellcreek_ptp_read(hellcreek, ts_reg);
+ nsl = hellcreek_ptp_read(hellcreek, ts_reg);
+
+ return (u64)nsl | ((u64)nsh << 16);
+}
+
+static int hellcreek_txtstamp_work(struct hellcreek *hellcreek,
+ struct hellcreek_port_hwtstamp *ps, int port)
+{
+ struct skb_shared_hwtstamps shhwtstamps;
+ unsigned int status_reg, data_reg;
+ struct sk_buff *tmp_skb;
+ int ts_status;
+ u64 ns = 0;
+
+ if (!ps->tx_skb)
+ return 0;
+
+ switch (port) {
+ case 2:
+ status_reg = PR_TS_TX_P1_STATUS_C;
+ data_reg = PR_TS_TX_P1_DATA_C;
+ break;
+ case 3:
+ status_reg = PR_TS_TX_P2_STATUS_C;
+ data_reg = PR_TS_TX_P2_DATA_C;
+ break;
+ default:
+ dev_err(hellcreek->dev, "Wrong port for timestamping!\n");
+ return 0;
+ }
+
+ ts_status = hellcreek_ptp_hwtstamp_available(hellcreek, status_reg);
+
+ /* Not available yet? */
+ if (ts_status == 0) {
+ /* Check whether the operation of reading the tx timestamp has
+ * exceeded its allowed period
+ */
+ if (time_is_before_jiffies(ps->tx_tstamp_start +
+ TX_TSTAMP_TIMEOUT)) {
+ dev_err(hellcreek->dev,
+ "Timeout while waiting for Tx timestamp!\n");
+ goto free_and_clear_skb;
+ }
+
+ /* The timestamp should be available quickly, while getting it
+ * in high priority. Restart the work
+ */
+ return 1;
+ }
+
+ mutex_lock(&hellcreek->ptp_lock);
+ ns = hellcreek_ptp_hwtstamp_read(hellcreek, data_reg);
+ ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
+ mutex_unlock(&hellcreek->ptp_lock);
+
+ /* Now we have the timestamp in nanoseconds, store it in the correct
+ * structure in order to send it to the user
+ */
+ memset(&shhwtstamps, 0, sizeof(shhwtstamps));
+ shhwtstamps.hwtstamp = ns_to_ktime(ns);
+
+ tmp_skb = ps->tx_skb;
+ ps->tx_skb = NULL;
+
+ /* skb_complete_tx_timestamp() frees up the client to make another
+ * timestampable transmit. We have to be ready for it by clearing the
+ * ps->tx_skb "flag" beforehand
+ */
+ clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
+
+ /* Deliver a clone of the original outgoing tx_skb with tx hwtstamp */
+ skb_complete_tx_timestamp(tmp_skb, &shhwtstamps);
+
+ return 0;
+
+free_and_clear_skb:
+ dev_kfree_skb_any(ps->tx_skb);
+ ps->tx_skb = NULL;
+ clear_bit_unlock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state);
+
+ return 0;
+}
+
+static void hellcreek_get_rxts(struct hellcreek *hellcreek,
+ struct hellcreek_port_hwtstamp *ps,
+ struct sk_buff *skb, struct sk_buff_head *rxq,
+ int port)
+{
+ struct skb_shared_hwtstamps *shwt;
+ struct sk_buff_head received;
+ unsigned long flags;
+
+ /* The latched timestamp belongs to one of the received frames. */
+ __skb_queue_head_init(&received);
+
+ /* Lock & disable interrupts */
+ spin_lock_irqsave(&rxq->lock, flags);
+
+ /* Add the reception queue "rxq" to the "received" queue an reintialize
+ * "rxq". From now on, we deal with "received" not with "rxq"
+ */
+ skb_queue_splice_tail_init(rxq, &received);
+
+ spin_unlock_irqrestore(&rxq->lock, flags);
+
+ for (; skb; skb = __skb_dequeue(&received)) {
+ struct ptp_header *hdr;
+ unsigned int type;
+ u64 ns;
+
+ /* Get nanoseconds from ptp packet */
+ type = SKB_PTP_TYPE(skb);
+ hdr = ptp_parse_header(skb, type);
+ ns = hellcreek_get_reserved_field(hdr);
+ hellcreek_clear_reserved_field(hdr);
+
+ /* Add seconds part */
+ mutex_lock(&hellcreek->ptp_lock);
+ ns += hellcreek_ptp_gettime_seconds(hellcreek, ns);
+ mutex_unlock(&hellcreek->ptp_lock);
+
+ /* Save time stamp */
+ shwt = skb_hwtstamps(skb);
+ memset(shwt, 0, sizeof(*shwt));
+ shwt->hwtstamp = ns_to_ktime(ns);
+ netif_rx_ni(skb);
+ }
+}
+
+static void hellcreek_rxtstamp_work(struct hellcreek *hellcreek,
+ struct hellcreek_port_hwtstamp *ps,
+ int port)
+{
+ struct sk_buff *skb;
+
+ skb = skb_dequeue(&ps->rx_queue);
+ if (skb)
+ hellcreek_get_rxts(hellcreek, ps, skb, &ps->rx_queue, port);
+}
+
+long hellcreek_hwtstamp_work(struct ptp_clock_info *ptp)
+{
+ struct hellcreek *hellcreek = ptp_to_hellcreek(ptp);
+ struct dsa_switch *ds = hellcreek->ds;
+ int i, restart = 0;
+
+ for (i = 0; i < ds->num_ports; i++) {
+ struct hellcreek_port_hwtstamp *ps;
+
+ if (!dsa_is_user_port(ds, i))
+ continue;
+
+ ps = &hellcreek->ports[i].port_hwtstamp;
+
+ if (test_bit(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS, &ps->state))
+ restart |= hellcreek_txtstamp_work(hellcreek, ps, i);
+
+ hellcreek_rxtstamp_work(hellcreek, ps, i);
+ }
+
+ return restart ? 1 : -1;
+}
+
+bool hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
+ struct sk_buff *clone, unsigned int type)
+{
+ struct hellcreek *hellcreek = ds->priv;
+ struct hellcreek_port_hwtstamp *ps;
+ struct ptp_header *hdr;
+
+ ps = &hellcreek->ports[port].port_hwtstamp;
+
+ /* Check if the driver is expected to do HW timestamping */
+ if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
+ return false;
+
+ /* Make sure the message is a PTP message that needs to be timestamped
+ * and the interaction with the HW timestamping is enabled. If not, stop
+ * here
+ */
+ hdr = hellcreek_should_tstamp(hellcreek, port, clone, type);
+ if (!hdr)
+ return false;
+
+ if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
+ &ps->state))
+ return false;
+
+ ps->tx_skb = clone;
+
+ /* store the number of ticks occurred since system start-up till this
+ * moment
+ */
+ ps->tx_tstamp_start = jiffies;
+
+ ptp_schedule_worker(hellcreek->ptp_clock, 0);
+
+ return true;
+}
+
+bool hellcreek_port_rxtstamp(struct dsa_switch *ds, int port,
+ struct sk_buff *skb, unsigned int type)
+{
+ struct hellcreek *hellcreek = ds->priv;
+ struct hellcreek_port_hwtstamp *ps;
+ struct ptp_header *hdr;
+
+ ps = &hellcreek->ports[port].port_hwtstamp;
+
+ /* This check only fails if the user did not initialize hardware
+ * timestamping beforehand.
+ */
+ if (ps->tstamp_config.rx_filter != HWTSTAMP_FILTER_PTP_V2_EVENT)
+ return false;
+
+ /* Make sure the message is a PTP message that needs to be timestamped
+ * and the interaction with the HW timestamping is enabled. If not, stop
+ * here
+ */
+ hdr = hellcreek_should_tstamp(hellcreek, port, skb, type);
+ if (!hdr)
+ return false;
+
+ SKB_PTP_TYPE(skb) = type;
+
+ skb_queue_tail(&ps->rx_queue, skb);
+
+ ptp_schedule_worker(hellcreek->ptp_clock, 0);
+
+ return true;
+}
+
+static void hellcreek_hwtstamp_port_setup(struct hellcreek *hellcreek, int port)
+{
+ struct hellcreek_port_hwtstamp *ps =
+ &hellcreek->ports[port].port_hwtstamp;
+
+ skb_queue_head_init(&ps->rx_queue);
+}
+
+int hellcreek_hwtstamp_setup(struct hellcreek *hellcreek)
+{
+ struct dsa_switch *ds = hellcreek->ds;
+ int i;
+
+ /* Initialize timestamping ports. */
+ for (i = 0; i < ds->num_ports; ++i) {
+ if (!dsa_is_user_port(ds, i))
+ continue;
+
+ hellcreek_hwtstamp_port_setup(hellcreek, i);
+ }
+
+ /* Select the synchronized clock as the source timekeeper for the
+ * timestamps and enable inline timestamping.
+ */
+ hellcreek_ptp_write(hellcreek, PR_SETTINGS_C_TS_SRC_TK_MASK |
+ PR_SETTINGS_C_RES3TS,
+ PR_SETTINGS_C);
+
+ return 0;
+}
+
+void hellcreek_hwtstamp_free(struct hellcreek *hellcreek)
+{
+ /* Nothing todo */
+}
projects / users / hch / configfs.git / commitdiff