WSL2-Linux-Kernel/drivers/net/dsa/hirschmann/hellcreek_hwtstamp.c

486 строки
12 KiB
C

// 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;
}
void hellcreek_port_txtstamp(struct dsa_switch *ds, int port,
struct sk_buff *skb)
{
struct hellcreek *hellcreek = ds->priv;
struct hellcreek_port_hwtstamp *ps;
struct ptp_header *hdr;
struct sk_buff *clone;
unsigned int type;
ps = &hellcreek->ports[port].port_hwtstamp;
type = ptp_classify_raw(skb);
if (type == PTP_CLASS_NONE)
return;
/* 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;
clone = skb_clone_sk(skb);
if (!clone)
return;
if (test_and_set_bit_lock(HELLCREEK_HWTSTAMP_TX_IN_PROGRESS,
&ps->state)) {
kfree_skb(clone);
return;
}
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);
}
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 */
}