[linux-2.6-block.git] / drivers / net / ethernet / aquantia / atlantic / aq_ptp.c

// SPDX-License-Identifier: GPL-2.0-only
/* Aquantia Corporation Network Driver
 * Copyright (C) 2014-2019 Aquantia Corporation. All rights reserved
 */

/* File aq_ptp.c:
 * Definition of functions for Linux PTP support.
 */

#include <linux/ptp_clock_kernel.h>
#include <linux/clocksource.h>

#include "aq_nic.h"
#include "aq_ptp.h"
#include "aq_ring.h"

struct ptp_skb_ring {
	struct sk_buff **buff;
	spinlock_t lock;
	unsigned int size;
	unsigned int head;
	unsigned int tail;
};

struct aq_ptp_s {
	struct aq_nic_s *aq_nic;
	spinlock_t ptp_lock;
	spinlock_t ptp_ring_lock;
	struct ptp_clock *ptp_clock;
	struct ptp_clock_info ptp_info;

	struct aq_ring_param_s ptp_ring_param;

	struct aq_ring_s ptp_tx;
	struct aq_ring_s ptp_rx;
	struct aq_ring_s hwts_rx;

	struct ptp_skb_ring skb_ring;
};

static int aq_ptp_skb_ring_init(struct ptp_skb_ring *ring, unsigned int size)
{
	struct sk_buff **buff = kmalloc(sizeof(*buff) * size, GFP_KERNEL);

	if (!buff)
		return -ENOMEM;

	spin_lock_init(&ring->lock);

	ring->buff = buff;
	ring->size = size;
	ring->head = 0;
	ring->tail = 0;

	return 0;
}

static void aq_ptp_skb_ring_release(struct ptp_skb_ring *ring)
{
	kfree(ring->buff);
	ring->buff = NULL;
}

/* aq_ptp_adjfine
 * @ptp: the ptp clock structure
 * @ppb: parts per billion adjustment from base
 *
 * adjust the frequency of the ptp cycle counter by the
 * indicated ppb from the base frequency.
 */
static int aq_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
{
	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;

	mutex_lock(&aq_nic->fwreq_mutex);
	aq_nic->aq_hw_ops->hw_adj_clock_freq(aq_nic->aq_hw,
					     scaled_ppm_to_ppb(scaled_ppm));
	mutex_unlock(&aq_nic->fwreq_mutex);

	return 0;
}

/* aq_ptp_adjtime
 * @ptp: the ptp clock structure
 * @delta: offset to adjust the cycle counter by
 *
 * adjust the timer by resetting the timecounter structure.
 */
static int aq_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
{
	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
	unsigned long flags;

	spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
	aq_nic->aq_hw_ops->hw_adj_sys_clock(aq_nic->aq_hw, delta);
	spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);

	return 0;
}

/* aq_ptp_gettime
 * @ptp: the ptp clock structure
 * @ts: timespec structure to hold the current time value
 *
 * read the timecounter and return the correct value on ns,
 * after converting it into a struct timespec.
 */
static int aq_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
{
	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
	unsigned long flags;
	u64 ns;

	spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
	aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &ns);
	spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);

	*ts = ns_to_timespec64(ns);

	return 0;
}

/* aq_ptp_settime
 * @ptp: the ptp clock structure
 * @ts: the timespec containing the new time for the cycle counter
 *
 * reset the timecounter to use a new base value instead of the kernel
 * wall timer value.
 */
static int aq_ptp_settime(struct ptp_clock_info *ptp,
			  const struct timespec64 *ts)
{
	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
	unsigned long flags;
	u64 ns = timespec64_to_ns(ts);
	u64 now;

	spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
	aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &now);
	aq_nic->aq_hw_ops->hw_adj_sys_clock(aq_nic->aq_hw, (s64)ns - (s64)now);

	spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);

	return 0;
}

int aq_ptp_ring_init(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	int err = 0;

	if (!aq_ptp)
		return 0;

	err = aq_ring_init(&aq_ptp->ptp_tx);
	if (err < 0)
		goto err_exit;
	err = aq_nic->aq_hw_ops->hw_ring_tx_init(aq_nic->aq_hw,
						 &aq_ptp->ptp_tx,
						 &aq_ptp->ptp_ring_param);
	if (err < 0)
		goto err_exit;

	err = aq_ring_init(&aq_ptp->ptp_rx);
	if (err < 0)
		goto err_exit;
	err = aq_nic->aq_hw_ops->hw_ring_rx_init(aq_nic->aq_hw,
						 &aq_ptp->ptp_rx,
						 &aq_ptp->ptp_ring_param);
	if (err < 0)
		goto err_exit;

	err = aq_ring_rx_fill(&aq_ptp->ptp_rx);
	if (err < 0)
		goto err_rx_free;
	err = aq_nic->aq_hw_ops->hw_ring_rx_fill(aq_nic->aq_hw,
						 &aq_ptp->ptp_rx,
						 0U);
	if (err < 0)
		goto err_rx_free;

	err = aq_ring_init(&aq_ptp->hwts_rx);
	if (err < 0)
		goto err_rx_free;
	err = aq_nic->aq_hw_ops->hw_ring_rx_init(aq_nic->aq_hw,
						 &aq_ptp->hwts_rx,
						 &aq_ptp->ptp_ring_param);

	return err;

err_rx_free:
	aq_ring_rx_deinit(&aq_ptp->ptp_rx);
err_exit:
	return err;
}

int aq_ptp_ring_start(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	int err = 0;

	if (!aq_ptp)
		return 0;

	err = aq_nic->aq_hw_ops->hw_ring_tx_start(aq_nic->aq_hw, &aq_ptp->ptp_tx);
	if (err < 0)
		goto err_exit;

	err = aq_nic->aq_hw_ops->hw_ring_rx_start(aq_nic->aq_hw, &aq_ptp->ptp_rx);
	if (err < 0)
		goto err_exit;

	err = aq_nic->aq_hw_ops->hw_ring_rx_start(aq_nic->aq_hw,
						  &aq_ptp->hwts_rx);
	if (err < 0)
		goto err_exit;

err_exit:
	return err;
}

void aq_ptp_ring_stop(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;

	if (!aq_ptp)
		return;

	aq_nic->aq_hw_ops->hw_ring_tx_stop(aq_nic->aq_hw, &aq_ptp->ptp_tx);
	aq_nic->aq_hw_ops->hw_ring_rx_stop(aq_nic->aq_hw, &aq_ptp->ptp_rx);

	aq_nic->aq_hw_ops->hw_ring_rx_stop(aq_nic->aq_hw, &aq_ptp->hwts_rx);
}

void aq_ptp_ring_deinit(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;

	if (!aq_ptp || !aq_ptp->ptp_tx.aq_nic || !aq_ptp->ptp_rx.aq_nic)
		return;

	aq_ring_tx_clean(&aq_ptp->ptp_tx);
	aq_ring_rx_deinit(&aq_ptp->ptp_rx);
}

#define PTP_8TC_RING_IDX             8
#define PTP_4TC_RING_IDX            16
#define PTP_HWST_RING_IDX           31

int aq_ptp_ring_alloc(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	unsigned int tx_ring_idx, rx_ring_idx;
	struct aq_ring_s *hwts = 0;
	u32 tx_tc_mode, rx_tc_mode;
	struct aq_ring_s *ring;
	int err;

	if (!aq_ptp)
		return 0;

	/* Index must to be 8 (8 TCs) or 16 (4 TCs).
	 * It depends from Traffic Class mode.
	 */
	aq_nic->aq_hw_ops->hw_tx_tc_mode_get(aq_nic->aq_hw, &tx_tc_mode);
	if (tx_tc_mode == 0)
		tx_ring_idx = PTP_8TC_RING_IDX;
	else
		tx_ring_idx = PTP_4TC_RING_IDX;

	ring = aq_ring_tx_alloc(&aq_ptp->ptp_tx, aq_nic,
				tx_ring_idx, &aq_nic->aq_nic_cfg);
	if (!ring) {
		err = -ENOMEM;
		goto err_exit;
	}

	aq_nic->aq_hw_ops->hw_rx_tc_mode_get(aq_nic->aq_hw, &rx_tc_mode);
	if (rx_tc_mode == 0)
		rx_ring_idx = PTP_8TC_RING_IDX;
	else
		rx_ring_idx = PTP_4TC_RING_IDX;

	ring = aq_ring_rx_alloc(&aq_ptp->ptp_rx, aq_nic,
				rx_ring_idx, &aq_nic->aq_nic_cfg);
	if (!ring) {
		err = -ENOMEM;
		goto err_exit_ptp_tx;
	}

	hwts = aq_ring_hwts_rx_alloc(&aq_ptp->hwts_rx, aq_nic, PTP_HWST_RING_IDX,
				     aq_nic->aq_nic_cfg.rxds,
				     aq_nic->aq_nic_cfg.aq_hw_caps->rxd_size);
	if (!hwts) {
		err = -ENOMEM;
		goto err_exit_ptp_rx;
	}

	err = aq_ptp_skb_ring_init(&aq_ptp->skb_ring, aq_nic->aq_nic_cfg.rxds);
	if (err != 0) {
		err = -ENOMEM;
		goto err_exit_hwts_rx;
	}

	return 0;

err_exit_hwts_rx:
	aq_ring_free(&aq_ptp->hwts_rx);
err_exit_ptp_rx:
	aq_ring_free(&aq_ptp->ptp_rx);
err_exit_ptp_tx:
	aq_ring_free(&aq_ptp->ptp_tx);
err_exit:
	return err;
}

void aq_ptp_ring_free(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;

	if (!aq_ptp)
		return;

	aq_ring_free(&aq_ptp->ptp_tx);
	aq_ring_free(&aq_ptp->ptp_rx);
	aq_ring_free(&aq_ptp->hwts_rx);

	aq_ptp_skb_ring_release(&aq_ptp->skb_ring);
}

static struct ptp_clock_info aq_ptp_clock = {
	.owner		= THIS_MODULE,
	.name		= "atlantic ptp",
	.max_adj	= 999999999,
	.n_ext_ts	= 0,
	.pps		= 0,
	.adjfine	= aq_ptp_adjfine,
	.adjtime	= aq_ptp_adjtime,
	.gettime64	= aq_ptp_gettime,
	.settime64	= aq_ptp_settime,
	.n_per_out	= 0,
	.n_pins		= 0,
	.pin_config	= NULL,
};

void aq_ptp_clock_init(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	struct timespec64 ts;

	ktime_get_real_ts64(&ts);
	aq_ptp_settime(&aq_ptp->ptp_info, &ts);
}

int aq_ptp_init(struct aq_nic_s *aq_nic, unsigned int idx_vec)
{
	struct hw_atl_utils_mbox mbox;
	struct ptp_clock *clock;
	struct aq_ptp_s *aq_ptp;
	int err = 0;

	if (!aq_nic->aq_hw_ops->hw_get_ptp_ts) {
		aq_nic->aq_ptp = NULL;
		return 0;
	}

	if (!aq_nic->aq_fw_ops->enable_ptp) {
		aq_nic->aq_ptp = NULL;
		return 0;
	}

	hw_atl_utils_mpi_read_stats(aq_nic->aq_hw, &mbox);

	if (!(mbox.info.caps_ex & BIT(CAPS_EX_PHY_PTP_EN))) {
		aq_nic->aq_ptp = NULL;
		return 0;
	}

	aq_ptp = kzalloc(sizeof(*aq_ptp), GFP_KERNEL);
	if (!aq_ptp) {
		err = -ENOMEM;
		goto err_exit;
	}

	aq_ptp->aq_nic = aq_nic;

	spin_lock_init(&aq_ptp->ptp_lock);
	spin_lock_init(&aq_ptp->ptp_ring_lock);

	aq_ptp->ptp_info = aq_ptp_clock;
	clock = ptp_clock_register(&aq_ptp->ptp_info, &aq_nic->ndev->dev);
	if (!clock || IS_ERR(clock)) {
		netdev_err(aq_nic->ndev, "ptp_clock_register failed\n");
		err = PTR_ERR(clock);
		goto err_exit;
	}
	aq_ptp->ptp_clock = clock;

	aq_nic->aq_ptp = aq_ptp;

	/* enable ptp counter */
	aq_utils_obj_set(&aq_nic->aq_hw->flags, AQ_HW_PTP_AVAILABLE);
	mutex_lock(&aq_nic->fwreq_mutex);
	aq_nic->aq_fw_ops->enable_ptp(aq_nic->aq_hw, 1);
	aq_ptp_clock_init(aq_nic);
	mutex_unlock(&aq_nic->fwreq_mutex);

	return 0;

err_exit:
	kfree(aq_ptp);
	aq_nic->aq_ptp = NULL;
	return err;
}

void aq_ptp_unregister(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;

	if (!aq_ptp)
		return;

	ptp_clock_unregister(aq_ptp->ptp_clock);
}

void aq_ptp_free(struct aq_nic_s *aq_nic)
{
	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;

	if (!aq_ptp)
		return;

	/* disable ptp */
	mutex_lock(&aq_nic->fwreq_mutex);
	aq_nic->aq_fw_ops->enable_ptp(aq_nic->aq_hw, 0);
	mutex_unlock(&aq_nic->fwreq_mutex);

	kfree(aq_ptp);
	aq_nic->aq_ptp = NULL;
}
Commit	Line	Data
1a64f8dc EP	1	// SPDX-License-Identifier: GPL-2.0-only
	2	/* Aquantia Corporation Network Driver
	3	* Copyright (C) 2014-2019 Aquantia Corporation. All rights reserved
	4	*/
	5
	6	/* File aq_ptp.c:
	7	* Definition of functions for Linux PTP support.
	8	*/
	9
	10	#include <linux/ptp_clock_kernel.h>
	11	#include <linux/clocksource.h>
	12
	13	#include "aq_nic.h"
	14	#include "aq_ptp.h"
94ad9455 EP	15	#include "aq_ring.h"
	16
	17	struct ptp_skb_ring {
	18	struct sk_buff **buff;
	19	spinlock_t lock;
	20	unsigned int size;
	21	unsigned int head;
	22	unsigned int tail;
	23	};
1a64f8dc EP	24
	25	struct aq_ptp_s {
	26	struct aq_nic_s *aq_nic;
910479a9	27	spinlock_t ptp_lock;
94ad9455	28	spinlock_t ptp_ring_lock;
1a64f8dc EP	29	struct ptp_clock *ptp_clock;
1a64f8dc EP	30	struct ptp_clock_info ptp_info;
94ad9455 EP	31
	32	struct aq_ring_param_s ptp_ring_param;
	33
	34	struct aq_ring_s ptp_tx;
	35	struct aq_ring_s ptp_rx;
	36	struct aq_ring_s hwts_rx;
	37
	38	struct ptp_skb_ring skb_ring;
1a64f8dc EP	39	};
1a64f8dc EP	40
94ad9455 EP	41	static int aq_ptp_skb_ring_init(struct ptp_skb_ring *ring, unsigned int size)
	42	{
	43	struct sk_buff *buff = kmalloc(sizeof(buff) * size, GFP_KERNEL);
	44
	45	if (!buff)
	46	return -ENOMEM;
	47
	48	spin_lock_init(&ring->lock);
	49
	50	ring->buff = buff;
	51	ring->size = size;
	52	ring->head = 0;
	53	ring->tail = 0;
	54
	55	return 0;
	56	}
	57
	58	static void aq_ptp_skb_ring_release(struct ptp_skb_ring *ring)
	59	{
	60	kfree(ring->buff);
	61	ring->buff = NULL;
	62	}
	63
910479a9 EP	64	/* aq_ptp_adjfine
	65	* @ptp: the ptp clock structure
	66	* @ppb: parts per billion adjustment from base
	67	*
	68	* adjust the frequency of the ptp cycle counter by the
	69	* indicated ppb from the base frequency.
	70	*/
	71	static int aq_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
	72	{
	73	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	74	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
	75
	76	mutex_lock(&aq_nic->fwreq_mutex);
	77	aq_nic->aq_hw_ops->hw_adj_clock_freq(aq_nic->aq_hw,
	78	scaled_ppm_to_ppb(scaled_ppm));
	79	mutex_unlock(&aq_nic->fwreq_mutex);
	80
	81	return 0;
	82	}
	83
	84	/* aq_ptp_adjtime
	85	* @ptp: the ptp clock structure
	86	* @delta: offset to adjust the cycle counter by
	87	*
	88	* adjust the timer by resetting the timecounter structure.
	89	*/
	90	static int aq_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
	91	{
	92	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	93	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
	94	unsigned long flags;
	95
	96	spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
	97	aq_nic->aq_hw_ops->hw_adj_sys_clock(aq_nic->aq_hw, delta);
	98	spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);
	99
	100	return 0;
	101	}
	102
	103	/* aq_ptp_gettime
	104	* @ptp: the ptp clock structure
	105	* @ts: timespec structure to hold the current time value
	106	*
	107	* read the timecounter and return the correct value on ns,
	108	* after converting it into a struct timespec.
	109	*/
	110	static int aq_ptp_gettime(struct ptp_clock_info ptp, struct timespec64 ts)
	111	{
	112	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
	113	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
	114	unsigned long flags;
	115	u64 ns;
	116
	117	spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
	118	aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &ns);
	119	spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);
	120
	121	*ts = ns_to_timespec64(ns);
	122
	123	return 0;
	124	}
	125
	126	/* aq_ptp_settime
	127	* @ptp: the ptp clock structure
128	* @ts: the timespec containing the new time for the cycle counter
129	*
130	* reset the timecounter to use a new base value instead of the kernel
131	* wall timer value.
132	*/
133	static int aq_ptp_settime(struct ptp_clock_info *ptp,
134	const struct timespec64 *ts)
135	{
136	struct aq_ptp_s *aq_ptp = container_of(ptp, struct aq_ptp_s, ptp_info);
137	struct aq_nic_s *aq_nic = aq_ptp->aq_nic;
138	unsigned long flags;
139	u64 ns = timespec64_to_ns(ts);
140	u64 now;
141
142	spin_lock_irqsave(&aq_ptp->ptp_lock, flags);
143	aq_nic->aq_hw_ops->hw_get_ptp_ts(aq_nic->aq_hw, &now);
144	aq_nic->aq_hw_ops->hw_adj_sys_clock(aq_nic->aq_hw, (s64)ns - (s64)now);
145
146	spin_unlock_irqrestore(&aq_ptp->ptp_lock, flags);
147
148	return 0;
149	}
150
94ad9455 EP	151	int aq_ptp_ring_init(struct aq_nic_s *aq_nic)
	152	{
	153	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	154	int err = 0;
	155
	156	if (!aq_ptp)
	157	return 0;
	158
	159	err = aq_ring_init(&aq_ptp->ptp_tx);
	160	if (err < 0)
	161	goto err_exit;
	162	err = aq_nic->aq_hw_ops->hw_ring_tx_init(aq_nic->aq_hw,
	163	&aq_ptp->ptp_tx,
	164	&aq_ptp->ptp_ring_param);
	165	if (err < 0)
	166	goto err_exit;
	167
	168	err = aq_ring_init(&aq_ptp->ptp_rx);
	169	if (err < 0)
	170	goto err_exit;
	171	err = aq_nic->aq_hw_ops->hw_ring_rx_init(aq_nic->aq_hw,
	172	&aq_ptp->ptp_rx,
	173	&aq_ptp->ptp_ring_param);
	174	if (err < 0)
	175	goto err_exit;
	176
	177	err = aq_ring_rx_fill(&aq_ptp->ptp_rx);
	178	if (err < 0)
	179	goto err_rx_free;
	180	err = aq_nic->aq_hw_ops->hw_ring_rx_fill(aq_nic->aq_hw,
	181	&aq_ptp->ptp_rx,
	182	0U);
	183	if (err < 0)
	184	goto err_rx_free;
	185
	186	err = aq_ring_init(&aq_ptp->hwts_rx);
	187	if (err < 0)
	188	goto err_rx_free;
	189	err = aq_nic->aq_hw_ops->hw_ring_rx_init(aq_nic->aq_hw,
	190	&aq_ptp->hwts_rx,
	191	&aq_ptp->ptp_ring_param);
	192
	193	return err;
	194
	195	err_rx_free:
	196	aq_ring_rx_deinit(&aq_ptp->ptp_rx);
	197	err_exit:
	198	return err;
	199	}
	200
	201	int aq_ptp_ring_start(struct aq_nic_s *aq_nic)
	202	{
	203	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	204	int err = 0;
	205
	206	if (!aq_ptp)
	207	return 0;
	208
	209	err = aq_nic->aq_hw_ops->hw_ring_tx_start(aq_nic->aq_hw, &aq_ptp->ptp_tx);
	210	if (err < 0)
	211	goto err_exit;
	212
	213	err = aq_nic->aq_hw_ops->hw_ring_rx_start(aq_nic->aq_hw, &aq_ptp->ptp_rx);
	214	if (err < 0)
215	goto err_exit;
216
217	err = aq_nic->aq_hw_ops->hw_ring_rx_start(aq_nic->aq_hw,
218	&aq_ptp->hwts_rx);
219	if (err < 0)
220	goto err_exit;
221
222	err_exit:
223	return err;
224	}
225
226	void aq_ptp_ring_stop(struct aq_nic_s *aq_nic)
227	{
228	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
229
230	if (!aq_ptp)
231	return;
232
233	aq_nic->aq_hw_ops->hw_ring_tx_stop(aq_nic->aq_hw, &aq_ptp->ptp_tx);
234	aq_nic->aq_hw_ops->hw_ring_rx_stop(aq_nic->aq_hw, &aq_ptp->ptp_rx);
235
236	aq_nic->aq_hw_ops->hw_ring_rx_stop(aq_nic->aq_hw, &aq_ptp->hwts_rx);
237	}
238
239	void aq_ptp_ring_deinit(struct aq_nic_s *aq_nic)
240	{
241	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
242
243	if (!aq_ptp \|\| !aq_ptp->ptp_tx.aq_nic \|\| !aq_ptp->ptp_rx.aq_nic)
244	return;
245
246	aq_ring_tx_clean(&aq_ptp->ptp_tx);
247	aq_ring_rx_deinit(&aq_ptp->ptp_rx);
248	}
249
250	#define PTP_8TC_RING_IDX 8
251	#define PTP_4TC_RING_IDX 16
252	#define PTP_HWST_RING_IDX 31
253
254	int aq_ptp_ring_alloc(struct aq_nic_s *aq_nic)
255	{
256	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
257	unsigned int tx_ring_idx, rx_ring_idx;
258	struct aq_ring_s *hwts = 0;
259	u32 tx_tc_mode, rx_tc_mode;
260	struct aq_ring_s *ring;
261	int err;
262
263	if (!aq_ptp)
264	return 0;
265
266	/* Index must to be 8 (8 TCs) or 16 (4 TCs).
267	* It depends from Traffic Class mode.
268	*/
269	aq_nic->aq_hw_ops->hw_tx_tc_mode_get(aq_nic->aq_hw, &tx_tc_mode);
270	if (tx_tc_mode == 0)
271	tx_ring_idx = PTP_8TC_RING_IDX;
272	else
273	tx_ring_idx = PTP_4TC_RING_IDX;
274
275	ring = aq_ring_tx_alloc(&aq_ptp->ptp_tx, aq_nic,
276	tx_ring_idx, &aq_nic->aq_nic_cfg);
277	if (!ring) {
278	err = -ENOMEM;
279	goto err_exit;
280	}
281
282	aq_nic->aq_hw_ops->hw_rx_tc_mode_get(aq_nic->aq_hw, &rx_tc_mode);
283	if (rx_tc_mode == 0)
284	rx_ring_idx = PTP_8TC_RING_IDX;
285	else
286	rx_ring_idx = PTP_4TC_RING_IDX;
287
288	ring = aq_ring_rx_alloc(&aq_ptp->ptp_rx, aq_nic,
289	rx_ring_idx, &aq_nic->aq_nic_cfg);
290	if (!ring) {
291	err = -ENOMEM;
292	goto err_exit_ptp_tx;
293	}
294
295	hwts = aq_ring_hwts_rx_alloc(&aq_ptp->hwts_rx, aq_nic, PTP_HWST_RING_IDX,
296	aq_nic->aq_nic_cfg.rxds,
297	aq_nic->aq_nic_cfg.aq_hw_caps->rxd_size);
298	if (!hwts) {
299	err = -ENOMEM;
300	goto err_exit_ptp_rx;
301	}
302
303	err = aq_ptp_skb_ring_init(&aq_ptp->skb_ring, aq_nic->aq_nic_cfg.rxds);
304	if (err != 0) {
305	err = -ENOMEM;
306	goto err_exit_hwts_rx;
307	}
308
309	return 0;
310
311	err_exit_hwts_rx:
312	aq_ring_free(&aq_ptp->hwts_rx);
313	err_exit_ptp_rx:
314	aq_ring_free(&aq_ptp->ptp_rx);
315	err_exit_ptp_tx:
316	aq_ring_free(&aq_ptp->ptp_tx);
317	err_exit:
318	return err;
319	}
320
321	void aq_ptp_ring_free(struct aq_nic_s *aq_nic)
322	{
323	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
324
325	if (!aq_ptp)
326	return;
327
328	aq_ring_free(&aq_ptp->ptp_tx);
329	aq_ring_free(&aq_ptp->ptp_rx);
330	aq_ring_free(&aq_ptp->hwts_rx);
331
332	aq_ptp_skb_ring_release(&aq_ptp->skb_ring);
333	}
334
1a64f8dc EP	335	static struct ptp_clock_info aq_ptp_clock = {
	336	.owner = THIS_MODULE,
	337	.name = "atlantic ptp",
910479a9	338	.max_adj = 999999999,
1a64f8dc EP	339	.n_ext_ts = 0,
1a64f8dc EP	340	.pps = 0,
910479a9 EP	341	.adjfine = aq_ptp_adjfine,
	342	.adjtime = aq_ptp_adjtime,
	343	.gettime64 = aq_ptp_gettime,
	344	.settime64 = aq_ptp_settime,
1a64f8dc EP	345	.n_per_out = 0,
	346	.n_pins = 0,
	347	.pin_config = NULL,
	348	};
	349
94ad9455 EP	350	void aq_ptp_clock_init(struct aq_nic_s *aq_nic)
	351	{
	352	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	353	struct timespec64 ts;
	354
	355	ktime_get_real_ts64(&ts);
	356	aq_ptp_settime(&aq_ptp->ptp_info, &ts);
	357	}
	358
1a64f8dc EP	359	int aq_ptp_init(struct aq_nic_s *aq_nic, unsigned int idx_vec)
	360	{
	361	struct hw_atl_utils_mbox mbox;
910479a9	362	struct ptp_clock *clock;
1a64f8dc EP	363	struct aq_ptp_s *aq_ptp;
	364	int err = 0;
	365
910479a9 EP	366	if (!aq_nic->aq_hw_ops->hw_get_ptp_ts) {
	367	aq_nic->aq_ptp = NULL;
	368	return 0;
	369	}
	370
	371	if (!aq_nic->aq_fw_ops->enable_ptp) {
	372	aq_nic->aq_ptp = NULL;
	373	return 0;
	374	}
	375
1a64f8dc EP	376	hw_atl_utils_mpi_read_stats(aq_nic->aq_hw, &mbox);
	377
	378	if (!(mbox.info.caps_ex & BIT(CAPS_EX_PHY_PTP_EN))) {
	379	aq_nic->aq_ptp = NULL;
	380	return 0;
	381	}
	382
	383	aq_ptp = kzalloc(sizeof(*aq_ptp), GFP_KERNEL);
	384	if (!aq_ptp) {
	385	err = -ENOMEM;
	386	goto err_exit;
	387	}
	388
	389	aq_ptp->aq_nic = aq_nic;
	390
910479a9	391	spin_lock_init(&aq_ptp->ptp_lock);
94ad9455	392	spin_lock_init(&aq_ptp->ptp_ring_lock);
910479a9	393
1a64f8dc	394	aq_ptp->ptp_info = aq_ptp_clock;
910479a9 EP	395	clock = ptp_clock_register(&aq_ptp->ptp_info, &aq_nic->ndev->dev);
	396	if (!clock \|\| IS_ERR(clock)) {
	397	netdev_err(aq_nic->ndev, "ptp_clock_register failed\n");
	398	err = PTR_ERR(clock);
	399	goto err_exit;
	400	}
	401	aq_ptp->ptp_clock = clock;
1a64f8dc EP	402
	403	aq_nic->aq_ptp = aq_ptp;
	404
910479a9 EP	405	/* enable ptp counter */
	406	aq_utils_obj_set(&aq_nic->aq_hw->flags, AQ_HW_PTP_AVAILABLE);
	407	mutex_lock(&aq_nic->fwreq_mutex);
	408	aq_nic->aq_fw_ops->enable_ptp(aq_nic->aq_hw, 1);
	409	aq_ptp_clock_init(aq_nic);
	410	mutex_unlock(&aq_nic->fwreq_mutex);
	411
1a64f8dc EP	412	return 0;
	413
	414	err_exit:
	415	kfree(aq_ptp);
	416	aq_nic->aq_ptp = NULL;
	417	return err;
	418	}
	419
	420	void aq_ptp_unregister(struct aq_nic_s *aq_nic)
	421	{
	422	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	423
	424	if (!aq_ptp)
	425	return;
	426
	427	ptp_clock_unregister(aq_ptp->ptp_clock);
	428	}
	429
	430	void aq_ptp_free(struct aq_nic_s *aq_nic)
	431	{
	432	struct aq_ptp_s *aq_ptp = aq_nic->aq_ptp;
	433
	434	if (!aq_ptp)
	435	return;
	436
910479a9 EP	437	/* disable ptp */
	438	mutex_lock(&aq_nic->fwreq_mutex);
	439	aq_nic->aq_fw_ops->enable_ptp(aq_nic->aq_hw, 0);
	440	mutex_unlock(&aq_nic->fwreq_mutex);
	441
1a64f8dc EP	442	kfree(aq_ptp);
	443	aq_nic->aq_ptp = NULL;
	444	}