// SPDX-License-Identifier: GPL-2.0+
/* Microchip Sparx5 Switch driver
*
* Copyright (c) 2021 Microchip Technology Inc. and its subsidiaries.
*
* The Sparx5 Chip Register Model can be browsed at this location:
* https://github.com/microchip-ung/sparx-5_reginfo
*/
#include <linux/ptp_classify.h>
#include "sparx5_main_regs.h"
#include "sparx5_main.h"
#define SPARX5_MAX_PTP_ID 512
#define TOD_ACC_PIN 0x4
enum {
PTP_PIN_ACTION_IDLE = 0,
PTP_PIN_ACTION_LOAD,
PTP_PIN_ACTION_SAVE,
PTP_PIN_ACTION_CLOCK,
PTP_PIN_ACTION_DELTA,
PTP_PIN_ACTION_TOD
};
static u64 sparx5_ptp_get_1ppm(struct sparx5 *sparx5)
{
/* Represents 1ppm adjustment in 2^59 format with 1.59687500000(625)
* 1.99609375000(500), 3.99218750000(250) as reference
* The value is calculated as following:
* (1/1000000)/((2^-59)/X)
*/
u64 res = 0;
switch (sparx5->coreclock) {
case SPX5_CORE_CLOCK_250MHZ:
res = 2301339409586;
break;
case SPX5_CORE_CLOCK_500MHZ:
res = 1150669704793;
break;
case SPX5_CORE_CLOCK_625MHZ:
res = 920535763834;
break;
default:
WARN(1, "Invalid core clock");
break;
}
return res;
}
static u64 sparx5_ptp_get_nominal_value(struct sparx5 *sparx5)
{
u64 res = 0;
switch (sparx5->coreclock) {
case SPX5_CORE_CLOCK_250MHZ:
res = 0x1FF0000000000000;
break;
case SPX5_CORE_CLOCK_500MHZ:
res = 0x0FF8000000000000;
break;
case SPX5_CORE_CLOCK_625MHZ:
res = 0x0CC6666666666666;
break;
default:
WARN(1, "Invalid core clock");
break;
}
return res;
}
int sparx5_ptp_hwtstamp_set(struct sparx5_port *port,
struct kernel_hwtstamp_config *cfg,
struct netlink_ext_ack *extack)
{
struct sparx5 *sparx5 = port->sparx5;
struct sparx5_phc *phc;
/* For now don't allow to run ptp on ports that are part of a bridge,
* because in case of transparent clock the HW will still forward the
* frames, so there would be duplicate frames
*/
if (test_bit(port->portno, sparx5->bridge_mask))
return -EINVAL;
switch (cfg->tx_type) {
case HWTSTAMP_TX_ON:
port->ptp_cmd = IFH_REW_OP_TWO_STEP_PTP;
break;
case HWTSTAMP_TX_ONESTEP_SYNC:
port->ptp_cmd = IFH_REW_OP_ONE_STEP_PTP;
break;
case HWTSTAMP_TX_OFF:
port->ptp_cmd = IFH_REW_OP_NOOP;
break;
default:
return -ERANGE;
}
switch (cfg->rx_filter) {
case HWTSTAMP_FILTER_NONE:
break;
case HWTSTAMP_FILTER_ALL:
case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
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:
case HWTSTAMP_FILTER_NTP_ALL:
cfg->rx_filter = HWTSTAMP_FILTER_ALL;
break;
default:
return -ERANGE;
}
/* Commit back the result & save it */
mutex_lock(&sparx5->ptp_lock);
phc = &sparx5->phc[SPARX5_PHC_PORT];
phc->hwtstamp_config = *cfg;
mutex_unlock(&sparx5->ptp_lock);
return 0;
}
void sparx5_ptp_hwtstamp_get(struct sparx5_port *port,
struct kernel_hwtstamp_config *cfg)
{
struct sparx5 *sparx5 = port->sparx5;
struct sparx5_phc *phc;
phc = &sparx5->phc[SPARX5_PHC_PORT];
*cfg = phc->hwtstamp_config;
}
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 void sparx5_get_hwtimestamp(struct sparx5 *sparx5,
struct timespec64 *ts,
u32 nsec)
{
/* Read current PTP time to get seconds */
unsigned long flags;
u32 curr_nsec;
spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(SPARX5_PHC_PORT) |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
PTP_PTP_PIN_CFG_PTP_PIN_DOM |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
ts->tv_sec = spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
curr_nsec = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
ts->tv_nsec = nsec;
/* Sec has incremented since the ts was registered */
if (curr_nsec < nsec)
ts->tv_sec--;
spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
}
irqreturn_t sparx5_ptp_irq_handler(int irq, void *args)
{
int budget = SPARX5_MAX_PTP_ID;
struct sparx5 *sparx5 = args;
while (budget--) {
struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
struct skb_shared_hwtstamps shhwtstamps;
struct sparx5_port *port;
struct timespec64 ts;
unsigned long flags;
u32 val, id, txport;
u32 delay;
val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
/* Check if a timestamp can be retrieved */
if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
break;
WARN_ON(val & REW_PTP_TWOSTEP_CTRL_PTP_OVFL);
if (!(val & REW_PTP_TWOSTEP_CTRL_STAMP_TX))
continue;
/* Retrieve the ts Tx port */
txport = REW_PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
/* Retrieve its associated skb */
port = sparx5->ports[txport];
/* Retrieve the delay */
delay = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
delay = REW_PTP_TWOSTEP_STAMP_STAMP_NSEC_GET(delay);
/* Get next timestamp from fifo, which needs to be the
* rx timestamp which represents the id of the frame
*/
spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
REW_PTP_TWOSTEP_CTRL_PTP_NXT,
sparx5, REW_PTP_TWOSTEP_CTRL);
val = spx5_rd(sparx5, REW_PTP_TWOSTEP_CTRL);
/* Check if a timestamp can be retried */
if (!(val & REW_PTP_TWOSTEP_CTRL_PTP_VLD))
break;
/* Read RX timestamping to get the ID */
id = spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP);
id <<= 8;
id |= spx5_rd(sparx5, REW_PTP_TWOSTEP_STAMP_SUBNS);
spin_lock_irqsave(&port->tx_skbs.lock, flags);
skb_queue_walk_safe(&port->tx_skbs, skb, skb_tmp) {
if (SPARX5_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);
/* Next ts */
spx5_rmw(REW_PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
REW_PTP_TWOSTEP_CTRL_PTP_NXT,
sparx5, REW_PTP_TWOSTEP_CTRL);
if (WARN_ON(!skb_match))
continue;
spin_lock(&sparx5->ptp_ts_id_lock);
sparx5->ptp_skbs--;
spin_unlock(&sparx5->ptp_ts_id_lock);
/* Get the h/w timestamp */
sparx5_get_hwtimestamp(sparx5, &ts, delay);
/* Set the timestamp into the skb */
shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
skb_tstamp_tx(skb_match, &shhwtstamps);
dev_kfree_skb_any(skb_match);
}
return IRQ_HANDLED;
}
static int sparx5_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
{
struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
struct sparx5 *sparx5 = phc->sparx5;
unsigned long flags;
bool neg_adj = 0;
u64 tod_inc;
u64 ref;
if (!scaled_ppm)
return 0;
if (scaled_ppm < 0) {
neg_adj = 1;
scaled_ppm = -scaled_ppm;
}
tod_inc = sparx5_ptp_get_nominal_value(sparx5);
/* The multiplication is split in 2 separate additions because of
* overflow issues. If scaled_ppm with 16bit fractional part was bigger
* than 20ppm then we got overflow.
*/
ref = sparx5_ptp_get_1ppm(sparx5) * (scaled_ppm >> 16);
ref += (sparx5_ptp_get_1ppm(sparx5) * (0xffff & scaled_ppm)) >> 16;
tod_inc = neg_adj ? tod_inc - ref : tod_inc + ref;
spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(1 << BIT(phc->index)),
PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
sparx5, PTP_PTP_DOM_CFG);
spx5_wr((u32)tod_inc & 0xFFFFFFFF, sparx5,
PTP_CLK_PER_CFG(phc->index, 0));
spx5_wr((u32)(tod_inc >> 32), sparx5,
PTP_CLK_PER_CFG(phc->index, 1));
spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS, sparx5,
PTP_PTP_DOM_CFG);
spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
return 0;
}
static int sparx5_ptp_settime64(struct ptp_clock_info *ptp,
const struct timespec64 *ts)
{
struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
struct sparx5 *sparx5 = phc->sparx5;
unsigned long flags;
spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
/* Must be in IDLE mode before the time can be loaded */
spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
PTP_PTP_PIN_CFG_PTP_PIN_DOM |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
/* Set new value */
spx5_wr(PTP_PTP_TOD_SEC_MSB_PTP_TOD_SEC_MSB_SET(upper_32_bits(ts->tv_sec)),
sparx5, PTP_PTP_TOD_SEC_MSB(TOD_ACC_PIN));
spx5_wr(lower_32_bits(ts->tv_sec),
sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
spx5_wr(ts->tv_nsec, sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
/* Apply new values */
spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_LOAD) |
PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
PTP_PTP_PIN_CFG_PTP_PIN_DOM |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
return 0;
}
int sparx5_ptp_gettime64(struct ptp_clock_info *ptp, struct timespec64 *ts)
{
struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
struct sparx5 *sparx5 = phc->sparx5;
unsigned long flags;
time64_t s;
s64 ns;
spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_SAVE) |
PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
PTP_PTP_PIN_CFG_PTP_PIN_DOM |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
s = spx5_rd(sparx5, PTP_PTP_TOD_SEC_MSB(TOD_ACC_PIN));
s <<= 32;
s |= spx5_rd(sparx5, PTP_PTP_TOD_SEC_LSB(TOD_ACC_PIN));
ns = spx5_rd(sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
ns &= PTP_PTP_TOD_NSEC_PTP_TOD_NSEC;
spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
/* Deal with negative values */
if ((ns & 0xFFFFFFF0) == 0x3FFFFFF0) {
s--;
ns &= 0xf;
ns += 999999984;
}
set_normalized_timespec64(ts, s, ns);
return 0;
}
static int sparx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
{
struct sparx5_phc *phc = container_of(ptp, struct sparx5_phc, info);
struct sparx5 *sparx5 = phc->sparx5;
if (delta > -(NSEC_PER_SEC / 2) && delta < (NSEC_PER_SEC / 2)) {
unsigned long flags;
spin_lock_irqsave(&sparx5->ptp_clock_lock, flags);
/* Must be in IDLE mode before the time can be loaded */
spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_IDLE) |
PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
PTP_PTP_PIN_CFG_PTP_PIN_DOM |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
spx5_wr(PTP_PTP_TOD_NSEC_PTP_TOD_NSEC_SET(delta),
sparx5, PTP_PTP_TOD_NSEC(TOD_ACC_PIN));
/* Adjust time with the value of PTP_TOD_NSEC */
spx5_rmw(PTP_PTP_PIN_CFG_PTP_PIN_ACTION_SET(PTP_PIN_ACTION_DELTA) |
PTP_PTP_PIN_CFG_PTP_PIN_DOM_SET(phc->index) |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC_SET(0),
PTP_PTP_PIN_CFG_PTP_PIN_ACTION |
PTP_PTP_PIN_CFG_PTP_PIN_DOM |
PTP_PTP_PIN_CFG_PTP_PIN_SYNC,
sparx5, PTP_PTP_PIN_CFG(TOD_ACC_PIN));
spin_unlock_irqrestore(&sparx5->ptp_clock_lock, flags);
} else {
/* Fall back using sparx5_ptp_settime64 which is not exact */
struct timespec64 ts;
u64 now;
sparx5_ptp_gettime64(ptp, &ts);
now = ktime_to_ns(timespec64_to_ktime(ts));
ts = ns_to_timespec64(now + delta);
sparx5_ptp_settime64(ptp, &ts);
}
return 0;
}
static struct ptp_clock_info sparx5_ptp_clock_info = {
.owner = THIS_MODULE,
.name = "sparx5 ptp",
.max_adj = 200000,
.gettime64 = sparx5_ptp_gettime64,
.settime64 = sparx5_ptp_settime64,
.adjtime = sparx5_ptp_adjtime,
.adjfine = sparx5_ptp_adjfine,
};
static int sparx5_ptp_phc_init(struct sparx5 *sparx5,
int index,
struct ptp_clock_info *clock_info)
{
struct sparx5_phc *phc = &sparx5->phc[index];
phc->info = *clock_info;
phc->clock = ptp_clock_register(&phc->info, sparx5->dev);
if (IS_ERR(phc->clock))
return PTR_ERR(phc->clock);
phc->index = index;
phc->sparx5 = sparx5;
/* PTP Rx stamping is always enabled. */
phc->hwtstamp_config.rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
return 0;
}
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)
return 0;
for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
err = sparx5_ptp_phc_init(sparx5, i, &sparx5_ptp_clock_info);
if (err)
return err;
}
spin_lock_init(&sparx5->ptp_clock_lock);
spin_lock_init(&sparx5->ptp_ts_id_lock);
mutex_init(&sparx5->ptp_lock);
/* Disable master counters */
spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0), sparx5, PTP_PTP_DOM_CFG);
/* Configure the nominal TOD increment per clock cycle */
spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0x7),
PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
sparx5, PTP_PTP_DOM_CFG);
for (i = 0; i < SPARX5_PHC_COUNT; ++i) {
spx5_wr((u32)tod_adj & 0xFFFFFFFF, sparx5,
PTP_CLK_PER_CFG(i, 0));
spx5_wr((u32)(tod_adj >> 32), sparx5,
PTP_CLK_PER_CFG(i, 1));
}
spx5_rmw(PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS_SET(0),
PTP_PTP_DOM_CFG_PTP_CLKCFG_DIS,
sparx5, PTP_PTP_DOM_CFG);
/* Enable master counters */
spx5_wr(PTP_PTP_DOM_CFG_PTP_ENA_SET(0x7), sparx5, PTP_PTP_DOM_CFG);
for (i = 0; i < SPX5_PORTS; 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 < SPX5_PORTS; 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;
}
Codebase Browser
linux