[PATCH net-next v2 10/15] net: lan969x: add PTP handler function

[Vadim Fedorenko]

On 23/10/2024 23:01, Daniel Machon wrote:
> Add PTP IRQ handler for lan969x. This is required, as the PTP registers
> are placed in two different targets on Sparx5 and lan969x. The
> implementation is otherwise the same as on Sparx5.
> 
> Also, expose sparx5_get_hwtimestamp() for use by lan969x.
> 
> Reviewed-by: Steen Hegelund <Steen.Hegelund at microchip.com>
> Signed-off-by: Daniel Machon <daniel.machon at microchip.com>
> ---
>   drivers/net/ethernet/microchip/lan969x/lan969x.c   | 90 ++++++++++++++++++++++
>   .../net/ethernet/microchip/sparx5/sparx5_main.h    |  5 ++
>   drivers/net/ethernet/microchip/sparx5/sparx5_ptp.c |  9 +--
>   3 files changed, 99 insertions(+), 5 deletions(-)
> 
> diff --git a/drivers/net/ethernet/microchip/lan969x/lan969x.c b/drivers/net/ethernet/microchip/lan969x/lan969x.c
> index 2c2b86f9144e..a3b40e09b947 100644
> --- a/drivers/net/ethernet/microchip/lan969x/lan969x.c
> +++ b/drivers/net/ethernet/microchip/lan969x/lan969x.c
> @@ -201,6 +201,95 @@ static int lan969x_port_mux_set(struct sparx5 *sparx5, struct sparx5_port *port,
>   	return 0;
>   }
>   
> +static irqreturn_t lan969x_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, PTP_TWOSTEP_CTRL);
> +
> +		/* Check if a timestamp can be retrieved */
> +		if (!(val & PTP_TWOSTEP_CTRL_PTP_VLD))
> +			break;
> +
> +		WARN_ON(val & PTP_TWOSTEP_CTRL_PTP_OVFL);
> +
> +		if (!(val & PTP_TWOSTEP_CTRL_STAMP_TX))
> +			continue;
> +
> +		/* Retrieve the ts Tx port */
> +		txport = PTP_TWOSTEP_CTRL_STAMP_PORT_GET(val);
> +
> +		/* Retrieve its associated skb */
> +		port = sparx5->ports[txport];
> +
> +		/* Retrieve the delay */
> +		delay = spx5_rd(sparx5, PTP_TWOSTEP_STAMP_NSEC);
> +		delay = PTP_TWOSTEP_STAMP_NSEC_NS_GET(delay);
> +
> +		/* Get next timestamp from fifo, which needs to be the
> +		 * rx timestamp which represents the id of the frame
> +		 */
> +		spx5_rmw(PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
> +			 PTP_TWOSTEP_CTRL_PTP_NXT,
> +			 sparx5, PTP_TWOSTEP_CTRL);
> +
> +		val = spx5_rd(sparx5, PTP_TWOSTEP_CTRL);
> +
> +		/* Check if a timestamp can be retrieved */
> +		if (!(val & PTP_TWOSTEP_CTRL_PTP_VLD))
> +			break;
> +
> +		/* Read RX timestamping to get the ID */
> +		id = spx5_rd(sparx5, PTP_TWOSTEP_STAMP_NSEC);
> +		id <<= 8;
> +		id |= spx5_rd(sparx5, 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(PTP_TWOSTEP_CTRL_PTP_NXT_SET(1),
> +			 PTP_TWOSTEP_CTRL_PTP_NXT,
> +			 sparx5, 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 in 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;
> +}
> +

This handler looks like an absolute copy of sparx5_ptp_irq_handler()
with the difference in registers only. Did you consider keep one
function but substitute ptp register sets?

>   static const struct sparx5_regs lan969x_regs = {
>   	.tsize = lan969x_tsize,
>   	.gaddr = lan969x_gaddr,
> @@ -242,6 +331,7 @@ static const struct sparx5_ops lan969x_ops = {
>   	.get_hsch_max_group_rate = &lan969x_get_hsch_max_group_rate,
>   	.get_sdlb_group          = &lan969x_get_sdlb_group,
>   	.set_port_mux            = &lan969x_port_mux_set,
> +	.ptp_irq_handler         = &lan969x_ptp_irq_handler,
>   };
>