#define IFH_REW_OP_ONE_STEP_PTP 0x3
#define IFH_REW_OP_TWO_STEP_PTP 0x4
+#define IFH_PDU_TYPE_NONE 0x0
+#define IFH_PDU_TYPE_PTP 0x5
+#define IFH_PDU_TYPE_IPV4_UDP_PTP 0x6
+#define IFH_PDU_TYPE_IPV6_UDP_PTP 0x7
+
struct sparx5;
struct sparx5_db_hw {
struct hrtimer inj_timer;
/* ptp */
u8 ptp_cmd;
+ u16 ts_id;
+ struct sk_buff_head tx_skbs;
};
enum sparx5_core_clockfreq {
u8 index;
};
+struct sparx5_skb_cb {
+ u8 rew_op;
+ u8 pdu_type;
+ u8 pdu_w16_offset;
+ u16 ts_id;
+ unsigned long jiffies;
+};
+
+#define SPARX5_PTP_TIMEOUT msecs_to_jiffies(10)
+#define SPARX5_SKB_CB(skb) \
+ ((struct sparx5_skb_cb *)((skb)->cb))
+
struct sparx5 {
struct platform_device *pdev;
struct device *dev;
bool ptp;
struct sparx5_phc phc[SPARX5_PHC_COUNT];
spinlock_t ptp_clock_lock; /* lock for phc */
+ spinlock_t ptp_ts_id_lock; /* lock for ts_id */
struct mutex ptp_lock; /* lock for ptp interface state */
+ u16 ptp_skbs;
};
/* sparx5_switchdev.c */
/* sparx5_packet.c */
struct frame_info {
int src_port;
+ u32 timestamp;
};
void sparx5_xtr_flush(struct sparx5 *sparx5, u8 grp);
int sparx_stats_init(struct sparx5 *sparx5);
/* sparx5_netdev.c */
+void sparx5_set_port_ifh_timestamp(void *ifh_hdr, u64 timestamp);
+void sparx5_set_port_ifh_rew_op(void *ifh_hdr, u32 rew_op);
+void sparx5_set_port_ifh_pdu_type(void *ifh_hdr, u32 pdu_type);
+void sparx5_set_port_ifh_pdu_w16_offset(void *ifh_hdr, u32 pdu_w16_offset);
void sparx5_set_port_ifh(void *ifh_hdr, u16 portno);
bool sparx5_netdevice_check(const struct net_device *dev);
struct net_device *sparx5_create_netdev(struct sparx5 *sparx5, u32 portno);
void sparx5_ptp_deinit(struct sparx5 *sparx5);
int sparx5_ptp_hwtstamp_set(struct sparx5_port *port, struct ifreq *ifr);
int sparx5_ptp_hwtstamp_get(struct sparx5_port *port, struct ifreq *ifr);
+void sparx5_ptp_rxtstamp(struct sparx5 *sparx5, struct sk_buff *skb,
+ u64 timestamp);
+int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
+ struct sk_buff *skb);
+void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
+ struct sk_buff *skb);
/* Clock period in picoseconds */
static inline u32 sparx5_clk_period(enum sparx5_core_clockfreq cclock)
((u32)xtr_hdr[30] << 0);
fwd = (fwd >> 5);
info->src_port = FIELD_GET(GENMASK(7, 1), fwd);
+
+ info->timestamp =
+ ((u64)xtr_hdr[2] << 24) |
+ ((u64)xtr_hdr[3] << 16) |
+ ((u64)xtr_hdr[4] << 8) |
+ ((u64)xtr_hdr[5] << 0);
}
static void sparx5_xtr_grp(struct sparx5 *sparx5, u8 grp, bool byte_swap)
/* Finish up skb */
skb_put(skb, byte_cnt - ETH_FCS_LEN);
eth_skb_pad(skb);
+ sparx5_ptp_rxtstamp(sparx5, skb, fi.timestamp);
skb->protocol = eth_type_trans(skb, netdev);
netdev->stats.rx_bytes += skb->len;
netdev->stats.rx_packets++;
memset(ifh, 0, IFH_LEN * 4);
sparx5_set_port_ifh(ifh, port->portno);
+ if (sparx5->ptp && skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP) {
+ ret = sparx5_ptp_txtstamp_request(port, skb);
+ if (ret)
+ return ret;
+
+ sparx5_set_port_ifh_rew_op(ifh, SPARX5_SKB_CB(skb)->rew_op);
+ sparx5_set_port_ifh_pdu_type(ifh, SPARX5_SKB_CB(skb)->pdu_type);
+ sparx5_set_port_ifh_pdu_w16_offset(ifh, SPARX5_SKB_CB(skb)->pdu_w16_offset);
+ sparx5_set_port_ifh_timestamp(ifh, SPARX5_SKB_CB(skb)->ts_id);
+ }
+
+ skb_tx_timestamp(skb);
if (sparx5->fdma_irq > 0)
ret = sparx5_fdma_xmit(sparx5, ifh, skb);
else
if (ret == NETDEV_TX_OK) {
stats->tx_bytes += skb->len;
stats->tx_packets++;
- skb_tx_timestamp(skb);
+
+ if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
+ SPARX5_SKB_CB(skb)->rew_op == IFH_REW_OP_TWO_STEP_PTP)
+ return ret;
+
dev_kfree_skb_any(skb);
} else {
stats->tx_dropped++;
+
+ if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP &&
+ SPARX5_SKB_CB(skb)->rew_op == IFH_REW_OP_TWO_STEP_PTP)
+ sparx5_ptp_txtstamp_release(port, skb);
}
return ret;
}
sizeof(phc->hwtstamp_config)) ? -EFAULT : 0;
}
+static void sparx5_ptp_classify(struct sparx5_port *port, struct sk_buff *skb,
+ u8 *rew_op, u8 *pdu_type, u8 *pdu_w16_offset)
+{
+ struct ptp_header *header;
+ u8 msgtype;
+ int type;
+
+ if (port->ptp_cmd == IFH_REW_OP_NOOP) {
+ *rew_op = IFH_REW_OP_NOOP;
+ *pdu_type = IFH_PDU_TYPE_NONE;
+ *pdu_w16_offset = 0;
+ return;
+ }
+
+ type = ptp_classify_raw(skb);
+ if (type == PTP_CLASS_NONE) {
+ *rew_op = IFH_REW_OP_NOOP;
+ *pdu_type = IFH_PDU_TYPE_NONE;
+ *pdu_w16_offset = 0;
+ return;
+ }
+
+ header = ptp_parse_header(skb, type);
+ if (!header) {
+ *rew_op = IFH_REW_OP_NOOP;
+ *pdu_type = IFH_PDU_TYPE_NONE;
+ *pdu_w16_offset = 0;
+ return;
+ }
+
+ *pdu_w16_offset = 7;
+ if (type & PTP_CLASS_L2)
+ *pdu_type = IFH_PDU_TYPE_PTP;
+ if (type & PTP_CLASS_IPV4)
+ *pdu_type = IFH_PDU_TYPE_IPV4_UDP_PTP;
+ if (type & PTP_CLASS_IPV6)
+ *pdu_type = IFH_PDU_TYPE_IPV6_UDP_PTP;
+
+ if (port->ptp_cmd == IFH_REW_OP_TWO_STEP_PTP) {
+ *rew_op = IFH_REW_OP_TWO_STEP_PTP;
+ return;
+ }
+
+ /* If it is sync and run 1 step then set the correct operation,
+ * otherwise run as 2 step
+ */
+ msgtype = ptp_get_msgtype(header, type);
+ if ((msgtype & 0xf) == 0) {
+ *rew_op = IFH_REW_OP_ONE_STEP_PTP;
+ return;
+ }
+
+ *rew_op = IFH_REW_OP_TWO_STEP_PTP;
+}
+
+static void sparx5_ptp_txtstamp_old_release(struct sparx5_port *port)
+{
+ struct sk_buff *skb, *skb_tmp;
+ unsigned long flags;
+
+ spin_lock_irqsave(&port->tx_skbs.lock, flags);
+ skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
+ if time_after(SPARX5_SKB_CB(skb)->jiffies + SPARX5_PTP_TIMEOUT,
+ jiffies)
+ break;
+
+ __skb_unlink(skb, &port->tx_skbs);
+ dev_kfree_skb_any(skb);
+ }
+ spin_unlock_irqrestore(&port->tx_skbs.lock, flags);
+}
+
+int sparx5_ptp_txtstamp_request(struct sparx5_port *port,
+ struct sk_buff *skb)
+{
+ struct sparx5 *sparx5 = port->sparx5;
+ u8 rew_op, pdu_type, pdu_w16_offset;
+ unsigned long flags;
+
+ sparx5_ptp_classify(port, skb, &rew_op, &pdu_type, &pdu_w16_offset);
+ SPARX5_SKB_CB(skb)->rew_op = rew_op;
+ SPARX5_SKB_CB(skb)->pdu_type = pdu_type;
+ SPARX5_SKB_CB(skb)->pdu_w16_offset = pdu_w16_offset;
+
+ if (rew_op != IFH_REW_OP_TWO_STEP_PTP)
+ return 0;
+
+ sparx5_ptp_txtstamp_old_release(port);
+
+ spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
+ if (sparx5->ptp_skbs == SPARX5_MAX_PTP_ID) {
+ spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
+ return -EBUSY;
+ }
+
+ skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
+
+ skb_queue_tail(&port->tx_skbs, skb);
+ SPARX5_SKB_CB(skb)->ts_id = port->ts_id;
+ SPARX5_SKB_CB(skb)->jiffies = jiffies;
+
+ sparx5->ptp_skbs++;
+ port->ts_id++;
+ if (port->ts_id == SPARX5_MAX_PTP_ID)
+ port->ts_id = 0;
+
+ spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
+
+ return 0;
+}
+
+void sparx5_ptp_txtstamp_release(struct sparx5_port *port,
+ struct sk_buff *skb)
+{
+ struct sparx5 *sparx5 = port->sparx5;
+ unsigned long flags;
+
+ spin_lock_irqsave(&sparx5->ptp_ts_id_lock, flags);
+ port->ts_id--;
+ sparx5->ptp_skbs--;
+ skb_unlink(skb, &port->tx_skbs);
+ spin_unlock_irqrestore(&sparx5->ptp_ts_id_lock, flags);
+}
+
static int sparx5_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
{
struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
int sparx5_ptp_init(struct sparx5 *sparx5)
{
u64 tod_adj = sparx5_ptp_get_nominal_value(sparx5);
+ struct sparx5_port *port;
int err, i;
if (!sparx5->ptp)
}
spin_lock_init(&sparx5->ptp_clock_lock);
+ spin_lock_init(&sparx5->ptp_ts_id_lock);
mutex_init(&sparx5->ptp_lock);
/* Disable master counters */
/* Enable master counters */
spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0x7), sparx5, PTP_PTP_DOM_CFG);
+ for (i = 0; i < sparx5->port_count; i++) {
+ port = sparx5->ports[i];
+ if (!port)
+ continue;
+
+ skb_queue_head_init(&port->tx_skbs);
+ }
+
return 0;
}
void sparx5_ptp_deinit(struct sparx5 *sparx5)
{
+ struct sparx5_port *port;
int i;
+ for (i = 0; i < sparx5->port_count; i++) {
+ port = sparx5->ports[i];
+ if (!port)
+ continue;
+
+ skb_queue_purge(&port->tx_skbs);
+ }
+
for (i = 0; i < SPARX5_PHC_COUNT; ++i)
ptp_clock_unregister(sparx5->phc[i].clock);
}
+
+void sparx5_ptp_rxtstamp(struct sparx5 *sparx5, struct sk_buff *skb,
+ u64 timestamp)
+{
+ struct skb_shared_hwtstamps *shhwtstamps;
+ struct sparx5_phc *phc;
+ struct timespec64 ts;
+ u64 full_ts_in_ns;
+
+ if (!sparx5->ptp)
+ return;
+
+ phc = &sparx5->phc[SPARX5_PHC_PORT];
+ sparx5_ptp_gettime64(&phc->info, &ts);
+
+ if (ts.tv_nsec < timestamp)
+ ts.tv_sec--;
+ ts.tv_nsec = timestamp;
+ full_ts_in_ns = ktime_set(ts.tv_sec, ts.tv_nsec);
+
+ shhwtstamps = skb_hwtstamps(skb);
+ shhwtstamps->hwtstamp = full_ts_in_ns;
+}