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/dsa/ocelot.h>
|
||||||
#include <linux/if_bridge.h>
|
#include <linux/if_bridge.h>
|
||||||
#include <linux/ptp_classify.h>
|
|
||||||
#include <soc/mscc/ocelot_vcap.h>
|
#include <soc/mscc/ocelot_vcap.h>
|
||||||
#include "ocelot.h"
|
#include "ocelot.h"
|
||||||
#include "ocelot_vcap.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);
|
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,
|
static int ocelot_rx_frame_word(struct ocelot *ocelot, u8 grp, bool ifh,
|
||||||
u32 *rval)
|
u32 *rval)
|
||||||
{
|
{
|
||||||
@ -1497,53 +1291,6 @@ int ocelot_fdb_dump(struct ocelot *ocelot, int port,
|
|||||||
}
|
}
|
||||||
EXPORT_SYMBOL(ocelot_fdb_dump);
|
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,
|
int ocelot_trap_add(struct ocelot *ocelot, int port,
|
||||||
unsigned long cookie, bool take_ts,
|
unsigned long cookie, bool take_ts,
|
||||||
void (*populate)(struct ocelot_vcap_filter *f))
|
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);
|
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)
|
static u32 ocelot_get_bond_mask(struct ocelot *ocelot, struct net_device *bond)
|
||||||
{
|
{
|
||||||
u32 mask = 0;
|
u32 mask = 0;
|
||||||
|
|||||||
@ -6,9 +6,13 @@
|
|||||||
*/
|
*/
|
||||||
#include <linux/time64.h>
|
#include <linux/time64.h>
|
||||||
|
|
||||||
|
#include <linux/dsa/ocelot.h>
|
||||||
|
#include <linux/ptp_classify.h>
|
||||||
#include <soc/mscc/ocelot_ptp.h>
|
#include <soc/mscc/ocelot_ptp.h>
|
||||||
#include <soc/mscc/ocelot_sys.h>
|
#include <soc/mscc/ocelot_sys.h>
|
||||||
|
#include <soc/mscc/ocelot_vcap.h>
|
||||||
#include <soc/mscc/ocelot.h>
|
#include <soc/mscc/ocelot.h>
|
||||||
|
#include "ocelot.h"
|
||||||
|
|
||||||
int ocelot_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
|
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);
|
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,
|
int ocelot_init_timestamp(struct ocelot *ocelot,
|
||||||
const struct ptp_clock_info *info)
|
const struct ptp_clock_info *info)
|
||||||
{
|
{
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user