forked from Minki/linux
net: mscc: ocelot: move more PTP code from the lib to ocelot_ptp.c
Decongest ocelot.c a bit more by moving all PTP related logic (including timestamp processing and PTP packet traps) to ocelot_ptp.c. Signed-off-by: Vladimir Oltean <vladimir.oltean@nxp.com> Signed-off-by: David S. Miller <davem@davemloft.net>
This commit is contained in:
Vladimir Oltean
David S. Miller
parent
97076c3cc9
commit
d50e41bf02
@ -6,7 +6,6 @@
|
||||
*/
|
||||
#include <linux/dsa/ocelot.h>
|
||||
#include <linux/if_bridge.h>
|
||||
#include <linux/ptp_classify.h>
|
||||
#include <soc/mscc/ocelot_vcap.h>
|
||||
#include "ocelot.h"
|
||||
#include "ocelot_vcap.h"
|
||||
@ -910,211 +909,6 @@ void ocelot_phylink_mac_link_up(struct ocelot *ocelot, int port,
|
||||
}
|
||||
EXPORT_SYMBOL_GPL(ocelot_phylink_mac_link_up);
|
||||
|
||||
static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
|
||||
struct sk_buff *clone)
|
||||
{
|
||||
struct ocelot_port *ocelot_port = ocelot->ports[port];
|
||||
unsigned long flags;
|
||||
|
||||
spin_lock_irqsave(&ocelot->ts_id_lock, flags);
|
||||
|
||||
if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
|
||||
ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
|
||||
spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
|
||||
/* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
|
||||
OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
|
||||
|
||||
ocelot_port->ts_id++;
|
||||
if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
|
||||
ocelot_port->ts_id = 0;
|
||||
|
||||
ocelot_port->ptp_skbs_in_flight++;
|
||||
ocelot->ptp_skbs_in_flight++;
|
||||
|
||||
skb_queue_tail(&ocelot_port->tx_skbs, clone);
|
||||
|
||||
spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
|
||||
unsigned int ptp_class)
|
||||
{
|
||||
struct ptp_header *hdr;
|
||||
u8 msgtype, twostep;
|
||||
|
||||
hdr = ptp_parse_header(skb, ptp_class);
|
||||
if (!hdr)
|
||||
return false;
|
||||
|
||||
msgtype = ptp_get_msgtype(hdr, ptp_class);
|
||||
twostep = hdr->flag_field[0] & 0x2;
|
||||
|
||||
if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
|
||||
struct sk_buff *skb,
|
||||
struct sk_buff **clone)
|
||||
{
|
||||
struct ocelot_port *ocelot_port = ocelot->ports[port];
|
||||
u8 ptp_cmd = ocelot_port->ptp_cmd;
|
||||
unsigned int ptp_class;
|
||||
int err;
|
||||
|
||||
/* Don't do anything if PTP timestamping not enabled */
|
||||
if (!ptp_cmd)
|
||||
return 0;
|
||||
|
||||
ptp_class = ptp_classify_raw(skb);
|
||||
if (ptp_class == PTP_CLASS_NONE)
|
||||
return -EINVAL;
|
||||
|
||||
/* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
|
||||
if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
|
||||
if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
|
||||
OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Fall back to two-step timestamping */
|
||||
ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
|
||||
}
|
||||
|
||||
if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
|
||||
*clone = skb_clone_sk(skb);
|
||||
if (!(*clone))
|
||||
return -ENOMEM;
|
||||
|
||||
err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
|
||||
OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_port_txtstamp_request);
|
||||
|
||||
static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
|
||||
struct timespec64 *ts)
|
||||
{
|
||||
unsigned long flags;
|
||||
u32 val;
|
||||
|
||||
spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
|
||||
|
||||
/* Read current PTP time to get seconds */
|
||||
val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
|
||||
|
||||
val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
|
||||
val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
|
||||
ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
|
||||
ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
|
||||
|
||||
/* Read packet HW timestamp from FIFO */
|
||||
val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
|
||||
ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
|
||||
|
||||
/* Sec has incremented since the ts was registered */
|
||||
if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
|
||||
ts->tv_sec--;
|
||||
|
||||
spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
|
||||
}
|
||||
|
||||
static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
|
||||
{
|
||||
struct ptp_header *hdr;
|
||||
|
||||
hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
|
||||
if (WARN_ON(!hdr))
|
||||
return false;
|
||||
|
||||
return seqid == ntohs(hdr->sequence_id);
|
||||
}
|
||||
|
||||
void ocelot_get_txtstamp(struct ocelot *ocelot)
|
||||
{
|
||||
int budget = OCELOT_PTP_QUEUE_SZ;
|
||||
|
||||
while (budget--) {
|
||||
struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
|
||||
struct skb_shared_hwtstamps shhwtstamps;
|
||||
u32 val, id, seqid, txport;
|
||||
struct ocelot_port *port;
|
||||
struct timespec64 ts;
|
||||
unsigned long flags;
|
||||
|
||||
val = ocelot_read(ocelot, SYS_PTP_STATUS);
|
||||
|
||||
/* Check if a timestamp can be retrieved */
|
||||
if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
|
||||
break;
|
||||
|
||||
WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
|
||||
|
||||
/* Retrieve the ts ID and Tx port */
|
||||
id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
|
||||
txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
|
||||
seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
|
||||
|
||||
port = ocelot->ports[txport];
|
||||
|
||||
spin_lock(&ocelot->ts_id_lock);
|
||||
port->ptp_skbs_in_flight--;
|
||||
ocelot->ptp_skbs_in_flight--;
|
||||
spin_unlock(&ocelot->ts_id_lock);
|
||||
|
||||
/* Retrieve its associated skb */
|
||||
try_again:
|
||||
spin_lock_irqsave(&port->tx_skbs.lock, flags);
|
||||
|
||||
skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
|
||||
if (OCELOT_SKB_CB(skb)->ts_id != id)
|
||||
continue;
|
||||
__skb_unlink(skb, &port->tx_skbs);
|
||||
skb_match = skb;
|
||||
break;
|
||||
}
|
||||
|
||||
spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
|
||||
|
||||
if (WARN_ON(!skb_match))
|
||||
continue;
|
||||
|
||||
if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
|
||||
dev_err_ratelimited(ocelot->dev,
|
||||
"port %d received stale TX timestamp for seqid %d, discarding\n",
|
||||
txport, seqid);
|
||||
dev_kfree_skb_any(skb);
|
||||
goto try_again;
|
||||
}
|
||||
|
||||
/* Get the h/w timestamp */
|
||||
ocelot_get_hwtimestamp(ocelot, &ts);
|
||||
|
||||
/* Set the timestamp into the skb */
|
||||
memset(&shhwtstamps, 0, sizeof(shhwtstamps));
|
||||
shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
|
||||
skb_complete_tx_timestamp(skb_match, &shhwtstamps);
|
||||
|
||||
/* Next ts */
|
||||
ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
|
||||
}
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_get_txtstamp);
|
||||
|
||||
static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
|
||||
u32 *rval)
|
||||
{
|
||||
@ -1497,53 +1291,6 @@ int ocelot_fdb_dump(struct ocelot *ocelot, int port,
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_fdb_dump);
|
||||
|
||||
static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_ETYPE;
|
||||
*(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588);
|
||||
*(__be16 *)trap->key.etype.etype.mask = htons(0xffff);
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV4;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv4.dport.value = PTP_EV_PORT;
|
||||
trap->key.ipv4.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV6;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv6.dport.value = PTP_EV_PORT;
|
||||
trap->key.ipv6.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV4;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv4.dport.value = PTP_GEN_PORT;
|
||||
trap->key.ipv4.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV6;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv6.dport.value = PTP_GEN_PORT;
|
||||
trap->key.ipv6.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
int ocelot_trap_add(struct ocelot *ocelot, int port,
|
||||
unsigned long cookie, bool take_ts,
|
||||
void (*populate)(struct ocelot_vcap_filter *f))
|
||||
@ -1612,231 +1359,6 @@ int ocelot_trap_del(struct ocelot *ocelot, int port, unsigned long cookie)
|
||||
return ocelot_vcap_filter_replace(ocelot, trap);
|
||||
}
|
||||
|
||||
static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
|
||||
|
||||
return ocelot_trap_add(ocelot, port, l2_cookie, true,
|
||||
ocelot_populate_l2_ptp_trap_key);
|
||||
}
|
||||
|
||||
static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
|
||||
|
||||
return ocelot_trap_del(ocelot, port, l2_cookie);
|
||||
}
|
||||
|
||||
static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true,
|
||||
ocelot_populate_ipv4_ptp_event_trap_key);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false,
|
||||
ocelot_populate_ipv4_ptp_general_trap_key);
|
||||
if (err)
|
||||
ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
|
||||
err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie);
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true,
|
||||
ocelot_populate_ipv6_ptp_event_trap_key);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false,
|
||||
ocelot_populate_ipv6_ptp_general_trap_key);
|
||||
if (err)
|
||||
ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
|
||||
err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie);
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
|
||||
bool l2, bool l4)
|
||||
{
|
||||
int err;
|
||||
|
||||
if (l2)
|
||||
err = ocelot_l2_ptp_trap_add(ocelot, port);
|
||||
else
|
||||
err = ocelot_l2_ptp_trap_del(ocelot, port);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
if (l4) {
|
||||
err = ocelot_ipv4_ptp_trap_add(ocelot, port);
|
||||
if (err)
|
||||
goto err_ipv4;
|
||||
|
||||
err = ocelot_ipv6_ptp_trap_add(ocelot, port);
|
||||
if (err)
|
||||
goto err_ipv6;
|
||||
} else {
|
||||
err = ocelot_ipv4_ptp_trap_del(ocelot, port);
|
||||
|
||||
err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
|
||||
}
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
return 0;
|
||||
|
||||
err_ipv6:
|
||||
ocelot_ipv4_ptp_trap_del(ocelot, port);
|
||||
err_ipv4:
|
||||
if (l2)
|
||||
ocelot_l2_ptp_trap_del(ocelot, port);
|
||||
return err;
|
||||
}
|
||||
|
||||
int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
|
||||
{
|
||||
return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
|
||||
sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_hwstamp_get);
|
||||
|
||||
int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
|
||||
{
|
||||
struct ocelot_port *ocelot_port = ocelot->ports[port];
|
||||
bool l2 = false, l4 = false;
|
||||
struct hwtstamp_config cfg;
|
||||
int err;
|
||||
|
||||
if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
|
||||
return -EFAULT;
|
||||
|
||||
/* Tx type sanity check */
|
||||
switch (cfg.tx_type) {
|
||||
case HWTSTAMP_TX_ON:
|
||||
ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
|
||||
break;
|
||||
case HWTSTAMP_TX_ONESTEP_SYNC:
|
||||
/* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
|
||||
* need to update the origin time.
|
||||
*/
|
||||
ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
|
||||
break;
|
||||
case HWTSTAMP_TX_OFF:
|
||||
ocelot_port->ptp_cmd = 0;
|
||||
break;
|
||||
default:
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
mutex_lock(&ocelot->ptp_lock);
|
||||
|
||||
switch (cfg.rx_filter) {
|
||||
case HWTSTAMP_FILTER_NONE:
|
||||
break;
|
||||
case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
|
||||
l4 = true;
|
||||
break;
|
||||
case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
|
||||
l2 = true;
|
||||
break;
|
||||
case HWTSTAMP_FILTER_PTP_V2_EVENT:
|
||||
case HWTSTAMP_FILTER_PTP_V2_SYNC:
|
||||
case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
|
||||
l2 = true;
|
||||
l4 = true;
|
||||
break;
|
||||
default:
|
||||
mutex_unlock(&ocelot->ptp_lock);
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
|
||||
if (err) {
|
||||
mutex_unlock(&ocelot->ptp_lock);
|
||||
return err;
|
||||
}
|
||||
|
||||
if (l2 && l4)
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
|
||||
else if (l2)
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
|
||||
else if (l4)
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
|
||||
else
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_NONE;
|
||||
|
||||
/* Commit back the result & save it */
|
||||
memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
|
||||
mutex_unlock(&ocelot->ptp_lock);
|
||||
|
||||
return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_hwstamp_set);
|
||||
|
||||
int ocelot_get_ts_info(struct ocelot *ocelot, int port,
|
||||
struct ethtool_ts_info *info)
|
||||
{
|
||||
info->phc_index = ocelot->ptp_clock ?
|
||||
ptp_clock_index(ocelot->ptp_clock) : -1;
|
||||
if (info->phc_index == -1) {
|
||||
info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_RX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_SOFTWARE;
|
||||
return 0;
|
||||
}
|
||||
info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_RX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_SOFTWARE |
|
||||
SOF_TIMESTAMPING_TX_HARDWARE |
|
||||
SOF_TIMESTAMPING_RX_HARDWARE |
|
||||
SOF_TIMESTAMPING_RAW_HARDWARE;
|
||||
info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
|
||||
BIT(HWTSTAMP_TX_ONESTEP_SYNC);
|
||||
info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
|
||||
BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
|
||||
BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
|
||||
BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
|
||||
|
||||
return 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_get_ts_info);
|
||||
|
||||
static u32 ocelot_get_bond_mask(struct ocelot *ocelot, struct net_device *bond)
|
||||
{
|
||||
u32 mask = 0;
|
||||
|
||||
@ -6,9 +6,13 @@
|
||||
*/
|
||||
#include <linux/time64.h>
|
||||
|
||||
#include <linux/dsa/ocelot.h>
|
||||
#include <linux/ptp_classify.h>
|
||||
#include <soc/mscc/ocelot_ptp.h>
|
||||
#include <soc/mscc/ocelot_sys.h>
|
||||
#include <soc/mscc/ocelot_vcap.h>
|
||||
#include <soc/mscc/ocelot.h>
|
||||
#include "ocelot.h"
|
||||
|
||||
int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
|
||||
{
|
||||
@ -310,6 +314,483 @@ int ocelot_ptp_enable(struct ptp_clock_info *ptp,
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_ptp_enable);
|
||||
|
||||
static void ocelot_populate_l2_ptp_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_ETYPE;
|
||||
*(__be16 *)trap->key.etype.etype.value = htons(ETH_P_1588);
|
||||
*(__be16 *)trap->key.etype.etype.mask = htons(0xffff);
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv4_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV4;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv4.dport.value = PTP_EV_PORT;
|
||||
trap->key.ipv4.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv6_ptp_event_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV6;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv6.dport.value = PTP_EV_PORT;
|
||||
trap->key.ipv6.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv4_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV4;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv4.dport.value = PTP_GEN_PORT;
|
||||
trap->key.ipv4.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static void
|
||||
ocelot_populate_ipv6_ptp_general_trap_key(struct ocelot_vcap_filter *trap)
|
||||
{
|
||||
trap->key_type = OCELOT_VCAP_KEY_IPV6;
|
||||
trap->key.ipv4.proto.value[0] = IPPROTO_UDP;
|
||||
trap->key.ipv4.proto.mask[0] = 0xff;
|
||||
trap->key.ipv6.dport.value = PTP_GEN_PORT;
|
||||
trap->key.ipv6.dport.mask = 0xffff;
|
||||
}
|
||||
|
||||
static int ocelot_l2_ptp_trap_add(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
|
||||
|
||||
return ocelot_trap_add(ocelot, port, l2_cookie, true,
|
||||
ocelot_populate_l2_ptp_trap_key);
|
||||
}
|
||||
|
||||
static int ocelot_l2_ptp_trap_del(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long l2_cookie = OCELOT_VCAP_IS2_L2_PTP_TRAP(ocelot);
|
||||
|
||||
return ocelot_trap_del(ocelot, port, l2_cookie);
|
||||
}
|
||||
|
||||
static int ocelot_ipv4_ptp_trap_add(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv4_ev_cookie, true,
|
||||
ocelot_populate_ipv4_ptp_event_trap_key);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv4_gen_cookie, false,
|
||||
ocelot_populate_ipv4_ptp_general_trap_key);
|
||||
if (err)
|
||||
ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_ipv4_ptp_trap_del(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv4_gen_cookie = OCELOT_VCAP_IS2_IPV4_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv4_ev_cookie = OCELOT_VCAP_IS2_IPV4_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_del(ocelot, port, ipv4_ev_cookie);
|
||||
err |= ocelot_trap_del(ocelot, port, ipv4_gen_cookie);
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_ipv6_ptp_trap_add(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv6_ev_cookie, true,
|
||||
ocelot_populate_ipv6_ptp_event_trap_key);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
err = ocelot_trap_add(ocelot, port, ipv6_gen_cookie, false,
|
||||
ocelot_populate_ipv6_ptp_general_trap_key);
|
||||
if (err)
|
||||
ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
|
||||
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_ipv6_ptp_trap_del(struct ocelot *ocelot, int port)
|
||||
{
|
||||
unsigned long ipv6_gen_cookie = OCELOT_VCAP_IS2_IPV6_GEN_PTP_TRAP(ocelot);
|
||||
unsigned long ipv6_ev_cookie = OCELOT_VCAP_IS2_IPV6_EV_PTP_TRAP(ocelot);
|
||||
int err;
|
||||
|
||||
err = ocelot_trap_del(ocelot, port, ipv6_ev_cookie);
|
||||
err |= ocelot_trap_del(ocelot, port, ipv6_gen_cookie);
|
||||
return err;
|
||||
}
|
||||
|
||||
static int ocelot_setup_ptp_traps(struct ocelot *ocelot, int port,
|
||||
bool l2, bool l4)
|
||||
{
|
||||
int err;
|
||||
|
||||
if (l2)
|
||||
err = ocelot_l2_ptp_trap_add(ocelot, port);
|
||||
else
|
||||
err = ocelot_l2_ptp_trap_del(ocelot, port);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
if (l4) {
|
||||
err = ocelot_ipv4_ptp_trap_add(ocelot, port);
|
||||
if (err)
|
||||
goto err_ipv4;
|
||||
|
||||
err = ocelot_ipv6_ptp_trap_add(ocelot, port);
|
||||
if (err)
|
||||
goto err_ipv6;
|
||||
} else {
|
||||
err = ocelot_ipv4_ptp_trap_del(ocelot, port);
|
||||
|
||||
err |= ocelot_ipv6_ptp_trap_del(ocelot, port);
|
||||
}
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
return 0;
|
||||
|
||||
err_ipv6:
|
||||
ocelot_ipv4_ptp_trap_del(ocelot, port);
|
||||
err_ipv4:
|
||||
if (l2)
|
||||
ocelot_l2_ptp_trap_del(ocelot, port);
|
||||
return err;
|
||||
}
|
||||
|
||||
int ocelot_hwstamp_get(struct ocelot *ocelot, int port, struct ifreq *ifr)
|
||||
{
|
||||
return copy_to_user(ifr->ifr_data, &ocelot->hwtstamp_config,
|
||||
sizeof(ocelot->hwtstamp_config)) ? -EFAULT : 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_hwstamp_get);
|
||||
|
||||
int ocelot_hwstamp_set(struct ocelot *ocelot, int port, struct ifreq *ifr)
|
||||
{
|
||||
struct ocelot_port *ocelot_port = ocelot->ports[port];
|
||||
bool l2 = false, l4 = false;
|
||||
struct hwtstamp_config cfg;
|
||||
int err;
|
||||
|
||||
if (copy_from_user(&cfg, ifr->ifr_data, sizeof(cfg)))
|
||||
return -EFAULT;
|
||||
|
||||
/* Tx type sanity check */
|
||||
switch (cfg.tx_type) {
|
||||
case HWTSTAMP_TX_ON:
|
||||
ocelot_port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
|
||||
break;
|
||||
case HWTSTAMP_TX_ONESTEP_SYNC:
|
||||
/* IFH_REW_OP_ONE_STEP_PTP updates the correctional field, we
|
||||
* need to update the origin time.
|
||||
*/
|
||||
ocelot_port->ptp_cmd = IFH_REW_OP_ORIGIN_PTP;
|
||||
break;
|
||||
case HWTSTAMP_TX_OFF:
|
||||
ocelot_port->ptp_cmd = 0;
|
||||
break;
|
||||
default:
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
mutex_lock(&ocelot->ptp_lock);
|
||||
|
||||
switch (cfg.rx_filter) {
|
||||
case HWTSTAMP_FILTER_NONE:
|
||||
break;
|
||||
case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
|
||||
l4 = true;
|
||||
break;
|
||||
case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
|
||||
case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
|
||||
l2 = true;
|
||||
break;
|
||||
case HWTSTAMP_FILTER_PTP_V2_EVENT:
|
||||
case HWTSTAMP_FILTER_PTP_V2_SYNC:
|
||||
case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
|
||||
l2 = true;
|
||||
l4 = true;
|
||||
break;
|
||||
default:
|
||||
mutex_unlock(&ocelot->ptp_lock);
|
||||
return -ERANGE;
|
||||
}
|
||||
|
||||
err = ocelot_setup_ptp_traps(ocelot, port, l2, l4);
|
||||
if (err) {
|
||||
mutex_unlock(&ocelot->ptp_lock);
|
||||
return err;
|
||||
}
|
||||
|
||||
if (l2 && l4)
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
|
||||
else if (l2)
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
|
||||
else if (l4)
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
|
||||
else
|
||||
cfg.rx_filter = HWTSTAMP_FILTER_NONE;
|
||||
|
||||
/* Commit back the result & save it */
|
||||
memcpy(&ocelot->hwtstamp_config, &cfg, sizeof(cfg));
|
||||
mutex_unlock(&ocelot->ptp_lock);
|
||||
|
||||
return copy_to_user(ifr->ifr_data, &cfg, sizeof(cfg)) ? -EFAULT : 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_hwstamp_set);
|
||||
|
||||
int ocelot_get_ts_info(struct ocelot *ocelot, int port,
|
||||
struct ethtool_ts_info *info)
|
||||
{
|
||||
info->phc_index = ocelot->ptp_clock ?
|
||||
ptp_clock_index(ocelot->ptp_clock) : -1;
|
||||
if (info->phc_index == -1) {
|
||||
info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_RX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_SOFTWARE;
|
||||
return 0;
|
||||
}
|
||||
info->so_timestamping |= SOF_TIMESTAMPING_TX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_RX_SOFTWARE |
|
||||
SOF_TIMESTAMPING_SOFTWARE |
|
||||
SOF_TIMESTAMPING_TX_HARDWARE |
|
||||
SOF_TIMESTAMPING_RX_HARDWARE |
|
||||
SOF_TIMESTAMPING_RAW_HARDWARE;
|
||||
info->tx_types = BIT(HWTSTAMP_TX_OFF) | BIT(HWTSTAMP_TX_ON) |
|
||||
BIT(HWTSTAMP_TX_ONESTEP_SYNC);
|
||||
info->rx_filters = BIT(HWTSTAMP_FILTER_NONE) |
|
||||
BIT(HWTSTAMP_FILTER_PTP_V2_EVENT) |
|
||||
BIT(HWTSTAMP_FILTER_PTP_V2_L2_EVENT) |
|
||||
BIT(HWTSTAMP_FILTER_PTP_V2_L4_EVENT);
|
||||
|
||||
return 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_get_ts_info);
|
||||
|
||||
static int ocelot_port_add_txtstamp_skb(struct ocelot *ocelot, int port,
|
||||
struct sk_buff *clone)
|
||||
{
|
||||
struct ocelot_port *ocelot_port = ocelot->ports[port];
|
||||
unsigned long flags;
|
||||
|
||||
spin_lock_irqsave(&ocelot->ts_id_lock, flags);
|
||||
|
||||
if (ocelot_port->ptp_skbs_in_flight == OCELOT_MAX_PTP_ID ||
|
||||
ocelot->ptp_skbs_in_flight == OCELOT_PTP_FIFO_SIZE) {
|
||||
spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
|
||||
return -EBUSY;
|
||||
}
|
||||
|
||||
skb_shinfo(clone)->tx_flags |= SKBTX_IN_PROGRESS;
|
||||
/* Store timestamp ID in OCELOT_SKB_CB(clone)->ts_id */
|
||||
OCELOT_SKB_CB(clone)->ts_id = ocelot_port->ts_id;
|
||||
|
||||
ocelot_port->ts_id++;
|
||||
if (ocelot_port->ts_id == OCELOT_MAX_PTP_ID)
|
||||
ocelot_port->ts_id = 0;
|
||||
|
||||
ocelot_port->ptp_skbs_in_flight++;
|
||||
ocelot->ptp_skbs_in_flight++;
|
||||
|
||||
skb_queue_tail(&ocelot_port->tx_skbs, clone);
|
||||
|
||||
spin_unlock_irqrestore(&ocelot->ts_id_lock, flags);
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
static bool ocelot_ptp_is_onestep_sync(struct sk_buff *skb,
|
||||
unsigned int ptp_class)
|
||||
{
|
||||
struct ptp_header *hdr;
|
||||
u8 msgtype, twostep;
|
||||
|
||||
hdr = ptp_parse_header(skb, ptp_class);
|
||||
if (!hdr)
|
||||
return false;
|
||||
|
||||
msgtype = ptp_get_msgtype(hdr, ptp_class);
|
||||
twostep = hdr->flag_field[0] & 0x2;
|
||||
|
||||
if (msgtype == PTP_MSGTYPE_SYNC && twostep == 0)
|
||||
return true;
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
int ocelot_port_txtstamp_request(struct ocelot *ocelot, int port,
|
||||
struct sk_buff *skb,
|
||||
struct sk_buff **clone)
|
||||
{
|
||||
struct ocelot_port *ocelot_port = ocelot->ports[port];
|
||||
u8 ptp_cmd = ocelot_port->ptp_cmd;
|
||||
unsigned int ptp_class;
|
||||
int err;
|
||||
|
||||
/* Don't do anything if PTP timestamping not enabled */
|
||||
if (!ptp_cmd)
|
||||
return 0;
|
||||
|
||||
ptp_class = ptp_classify_raw(skb);
|
||||
if (ptp_class == PTP_CLASS_NONE)
|
||||
return -EINVAL;
|
||||
|
||||
/* Store ptp_cmd in OCELOT_SKB_CB(skb)->ptp_cmd */
|
||||
if (ptp_cmd == IFH_REW_OP_ORIGIN_PTP) {
|
||||
if (ocelot_ptp_is_onestep_sync(skb, ptp_class)) {
|
||||
OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
|
||||
return 0;
|
||||
}
|
||||
|
||||
/* Fall back to two-step timestamping */
|
||||
ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
|
||||
}
|
||||
|
||||
if (ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
|
||||
*clone = skb_clone_sk(skb);
|
||||
if (!(*clone))
|
||||
return -ENOMEM;
|
||||
|
||||
err = ocelot_port_add_txtstamp_skb(ocelot, port, *clone);
|
||||
if (err)
|
||||
return err;
|
||||
|
||||
OCELOT_SKB_CB(skb)->ptp_cmd = ptp_cmd;
|
||||
OCELOT_SKB_CB(*clone)->ptp_class = ptp_class;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_port_txtstamp_request);
|
||||
|
||||
static void ocelot_get_hwtimestamp(struct ocelot *ocelot,
|
||||
struct timespec64 *ts)
|
||||
{
|
||||
unsigned long flags;
|
||||
u32 val;
|
||||
|
||||
spin_lock_irqsave(&ocelot->ptp_clock_lock, flags);
|
||||
|
||||
/* Read current PTP time to get seconds */
|
||||
val = ocelot_read_rix(ocelot, PTP_PIN_CFG, TOD_ACC_PIN);
|
||||
|
||||
val &= ~(PTP_PIN_CFG_SYNC | PTP_PIN_CFG_ACTION_MASK | PTP_PIN_CFG_DOM);
|
||||
val |= PTP_PIN_CFG_ACTION(PTP_PIN_ACTION_SAVE);
|
||||
ocelot_write_rix(ocelot, val, PTP_PIN_CFG, TOD_ACC_PIN);
|
||||
ts->tv_sec = ocelot_read_rix(ocelot, PTP_PIN_TOD_SEC_LSB, TOD_ACC_PIN);
|
||||
|
||||
/* Read packet HW timestamp from FIFO */
|
||||
val = ocelot_read(ocelot, SYS_PTP_TXSTAMP);
|
||||
ts->tv_nsec = SYS_PTP_TXSTAMP_PTP_TXSTAMP(val);
|
||||
|
||||
/* Sec has incremented since the ts was registered */
|
||||
if ((ts->tv_sec & 0x1) != !!(val & SYS_PTP_TXSTAMP_PTP_TXSTAMP_SEC))
|
||||
ts->tv_sec--;
|
||||
|
||||
spin_unlock_irqrestore(&ocelot->ptp_clock_lock, flags);
|
||||
}
|
||||
|
||||
static bool ocelot_validate_ptp_skb(struct sk_buff *clone, u16 seqid)
|
||||
{
|
||||
struct ptp_header *hdr;
|
||||
|
||||
hdr = ptp_parse_header(clone, OCELOT_SKB_CB(clone)->ptp_class);
|
||||
if (WARN_ON(!hdr))
|
||||
return false;
|
||||
|
||||
return seqid == ntohs(hdr->sequence_id);
|
||||
}
|
||||
|
||||
void ocelot_get_txtstamp(struct ocelot *ocelot)
|
||||
{
|
||||
int budget = OCELOT_PTP_QUEUE_SZ;
|
||||
|
||||
while (budget--) {
|
||||
struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
|
||||
struct skb_shared_hwtstamps shhwtstamps;
|
||||
u32 val, id, seqid, txport;
|
||||
struct ocelot_port *port;
|
||||
struct timespec64 ts;
|
||||
unsigned long flags;
|
||||
|
||||
val = ocelot_read(ocelot, SYS_PTP_STATUS);
|
||||
|
||||
/* Check if a timestamp can be retrieved */
|
||||
if (!(val & SYS_PTP_STATUS_PTP_MESS_VLD))
|
||||
break;
|
||||
|
||||
WARN_ON(val & SYS_PTP_STATUS_PTP_OVFL);
|
||||
|
||||
/* Retrieve the ts ID and Tx port */
|
||||
id = SYS_PTP_STATUS_PTP_MESS_ID_X(val);
|
||||
txport = SYS_PTP_STATUS_PTP_MESS_TXPORT_X(val);
|
||||
seqid = SYS_PTP_STATUS_PTP_MESS_SEQ_ID(val);
|
||||
|
||||
port = ocelot->ports[txport];
|
||||
|
||||
spin_lock(&ocelot->ts_id_lock);
|
||||
port->ptp_skbs_in_flight--;
|
||||
ocelot->ptp_skbs_in_flight--;
|
||||
spin_unlock(&ocelot->ts_id_lock);
|
||||
|
||||
/* Retrieve its associated skb */
|
||||
try_again:
|
||||
spin_lock_irqsave(&port->tx_skbs.lock, flags);
|
||||
|
||||
skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
|
||||
if (OCELOT_SKB_CB(skb)->ts_id != id)
|
||||
continue;
|
||||
__skb_unlink(skb, &port->tx_skbs);
|
||||
skb_match = skb;
|
||||
break;
|
||||
}
|
||||
|
||||
spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
|
||||
|
||||
if (WARN_ON(!skb_match))
|
||||
continue;
|
||||
|
||||
if (!ocelot_validate_ptp_skb(skb_match, seqid)) {
|
||||
dev_err_ratelimited(ocelot->dev,
|
||||
"port %d received stale TX timestamp for seqid %d, discarding\n",
|
||||
txport, seqid);
|
||||
dev_kfree_skb_any(skb);
|
||||
goto try_again;
|
||||
}
|
||||
|
||||
/* Get the h/w timestamp */
|
||||
ocelot_get_hwtimestamp(ocelot, &ts);
|
||||
|
||||
/* Set the timestamp into the skb */
|
||||
memset(&shhwtstamps, 0, sizeof(shhwtstamps));
|
||||
shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
|
||||
skb_complete_tx_timestamp(skb_match, &shhwtstamps);
|
||||
|
||||
/* Next ts */
|
||||
ocelot_write(ocelot, SYS_PTP_NXT_PTP_NXT, SYS_PTP_NXT);
|
||||
}
|
||||
}
|
||||
EXPORT_SYMBOL(ocelot_get_txtstamp);
|
||||
|
||||
int ocelot_init_timestamp(struct ocelot *ocelot,
|
||||
const struct ptp_clock_info *info)
|
||||
{
|
||||
|
||||
Loading…
Reference in New Issue
Block a user