[linux-block.git] / drivers / net / ethernet / mellanox / mlx5 / core / lib / clock.c

/*
 * Copyright (c) 2015, Mellanox Technologies. All rights reserved.
 *
 * This software is available to you under a choice of one of two
 * licenses.  You may choose to be licensed under the terms of the GNU
 * General Public License (GPL) Version 2, available from the file
 * COPYING in the main directory of this source tree, or the
 * OpenIB.org BSD license below:
 *
 *     Redistribution and use in source and binary forms, with or
 *     without modification, are permitted provided that the following
 *     conditions are met:
 *
 *      - Redistributions of source code must retain the above
 *        copyright notice, this list of conditions and the following
 *        disclaimer.
 *
 *      - Redistributions in binary form must reproduce the above
 *        copyright notice, this list of conditions and the following
 *        disclaimer in the documentation and/or other materials
 *        provided with the distribution.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
 * EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
 * NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
 * BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
 * ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
 * CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

#include <linux/clocksource.h>
#include <linux/highmem.h>
#include <rdma/mlx5-abi.h>
#include "en.h"
#include "clock.h"

enum {
	MLX5_CYCLES_SHIFT	= 23
};

enum {
	MLX5_PIN_MODE_IN		= 0x0,
	MLX5_PIN_MODE_OUT		= 0x1,
};

enum {
	MLX5_OUT_PATTERN_PULSE		= 0x0,
	MLX5_OUT_PATTERN_PERIODIC	= 0x1,
};

enum {
	MLX5_EVENT_MODE_DISABLE	= 0x0,
	MLX5_EVENT_MODE_REPETETIVE	= 0x1,
	MLX5_EVENT_MODE_ONCE_TILL_ARM	= 0x2,
};

enum {
	MLX5_MTPPS_FS_ENABLE			= BIT(0x0),
	MLX5_MTPPS_FS_PATTERN			= BIT(0x2),
	MLX5_MTPPS_FS_PIN_MODE			= BIT(0x3),
	MLX5_MTPPS_FS_TIME_STAMP		= BIT(0x4),
	MLX5_MTPPS_FS_OUT_PULSE_DURATION	= BIT(0x5),
	MLX5_MTPPS_FS_ENH_OUT_PER_ADJ		= BIT(0x7),
};

static u64 read_internal_timer(const struct cyclecounter *cc)
{
	struct mlx5_clock *clock = container_of(cc, struct mlx5_clock, cycles);
	struct mlx5_core_dev *mdev = container_of(clock, struct mlx5_core_dev,
						  clock);

	return mlx5_read_internal_timer(mdev) & cc->mask;
}

static void mlx5_update_clock_info_page(struct mlx5_core_dev *mdev)
{
	struct mlx5_ib_clock_info *clock_info = mdev->clock_info;
	struct mlx5_clock *clock = &mdev->clock;
	u32 sign;

	if (!clock_info)
		return;

	sign = smp_load_acquire(&clock_info->sign);
	smp_store_mb(clock_info->sign,
		     sign | MLX5_IB_CLOCK_INFO_KERNEL_UPDATING);

	clock_info->cycles = clock->tc.cycle_last;
	clock_info->mult   = clock->cycles.mult;
	clock_info->nsec   = clock->tc.nsec;
	clock_info->frac   = clock->tc.frac;

	smp_store_release(&clock_info->sign,
			  sign + MLX5_IB_CLOCK_INFO_KERNEL_UPDATING * 2);
}

static void mlx5_pps_out(struct work_struct *work)
{
	struct mlx5_pps *pps_info = container_of(work, struct mlx5_pps,
						 out_work);
	struct mlx5_clock *clock = container_of(pps_info, struct mlx5_clock,
						pps_info);
	struct mlx5_core_dev *mdev = container_of(clock, struct mlx5_core_dev,
						  clock);
	u32 in[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
	unsigned long flags;
	int i;

	for (i = 0; i < clock->ptp_info.n_pins; i++) {
		u64 tstart;

		write_lock_irqsave(&clock->lock, flags);
		tstart = clock->pps_info.start[i];
		clock->pps_info.start[i] = 0;
		write_unlock_irqrestore(&clock->lock, flags);
		if (!tstart)
			continue;

		MLX5_SET(mtpps_reg, in, pin, i);
		MLX5_SET64(mtpps_reg, in, time_stamp, tstart);
		MLX5_SET(mtpps_reg, in, field_select, MLX5_MTPPS_FS_TIME_STAMP);
		mlx5_set_mtpps(mdev, in, sizeof(in));
	}
}

static void mlx5_timestamp_overflow(struct work_struct *work)
{
	struct delayed_work *dwork = to_delayed_work(work);
	struct mlx5_clock *clock = container_of(dwork, struct mlx5_clock,
						overflow_work);
	unsigned long flags;

	write_lock_irqsave(&clock->lock, flags);
	timecounter_read(&clock->tc);
	mlx5_update_clock_info_page(clock->mdev);
	write_unlock_irqrestore(&clock->lock, flags);
	schedule_delayed_work(&clock->overflow_work, clock->overflow_period);
}

static int mlx5_ptp_settime(struct ptp_clock_info *ptp,
			    const struct timespec64 *ts)
{
	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
						 ptp_info);
	u64 ns = timespec64_to_ns(ts);
	unsigned long flags;

	write_lock_irqsave(&clock->lock, flags);
	timecounter_init(&clock->tc, &clock->cycles, ns);
	mlx5_update_clock_info_page(clock->mdev);
	write_unlock_irqrestore(&clock->lock, flags);

	return 0;
}

static int mlx5_ptp_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
{
	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
						ptp_info);
	u64 ns;
	unsigned long flags;

	write_lock_irqsave(&clock->lock, flags);
	ns = timecounter_read(&clock->tc);
	write_unlock_irqrestore(&clock->lock, flags);

	*ts = ns_to_timespec64(ns);

	return 0;
}

static int mlx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
{
	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
						ptp_info);
	unsigned long flags;

	write_lock_irqsave(&clock->lock, flags);
	timecounter_adjtime(&clock->tc, delta);
	mlx5_update_clock_info_page(clock->mdev);
	write_unlock_irqrestore(&clock->lock, flags);

	return 0;
}

static int mlx5_ptp_adjfreq(struct ptp_clock_info *ptp, s32 delta)
{
	u64 adj;
	u32 diff;
	unsigned long flags;
	int neg_adj = 0;
	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
						ptp_info);

	if (delta < 0) {
		neg_adj = 1;
		delta = -delta;
	}

	adj = clock->nominal_c_mult;
	adj *= delta;
	diff = div_u64(adj, 1000000000ULL);

	write_lock_irqsave(&clock->lock, flags);
	timecounter_read(&clock->tc);
	clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff :
				       clock->nominal_c_mult + diff;
	mlx5_update_clock_info_page(clock->mdev);
	write_unlock_irqrestore(&clock->lock, flags);

	return 0;
}

static int mlx5_extts_configure(struct ptp_clock_info *ptp,
				struct ptp_clock_request *rq,
				int on)
{
	struct mlx5_clock *clock =
			container_of(ptp, struct mlx5_clock, ptp_info);
	struct mlx5_core_dev *mdev =
			container_of(clock, struct mlx5_core_dev, clock);
	u32 in[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
	u32 field_select = 0;
	u8 pin_mode = 0;
	u8 pattern = 0;
	int pin = -1;
	int err = 0;

	if (!MLX5_PPS_CAP(mdev))
		return -EOPNOTSUPP;

	if (rq->extts.index >= clock->ptp_info.n_pins)
		return -EINVAL;

	if (on) {
		pin = ptp_find_pin(clock->ptp, PTP_PF_EXTTS, rq->extts.index);
		if (pin < 0)
			return -EBUSY;
		pin_mode = MLX5_PIN_MODE_IN;
		pattern = !!(rq->extts.flags & PTP_FALLING_EDGE);
		field_select = MLX5_MTPPS_FS_PIN_MODE |
			       MLX5_MTPPS_FS_PATTERN |
			       MLX5_MTPPS_FS_ENABLE;
	} else {
		pin = rq->extts.index;
		field_select = MLX5_MTPPS_FS_ENABLE;
	}

	MLX5_SET(mtpps_reg, in, pin, pin);
	MLX5_SET(mtpps_reg, in, pin_mode, pin_mode);
	MLX5_SET(mtpps_reg, in, pattern, pattern);
	MLX5_SET(mtpps_reg, in, enable, on);
	MLX5_SET(mtpps_reg, in, field_select, field_select);

	err = mlx5_set_mtpps(mdev, in, sizeof(in));
	if (err)
		return err;

	return mlx5_set_mtppse(mdev, pin, 0,
			       MLX5_EVENT_MODE_REPETETIVE & on);
}

static int mlx5_perout_configure(struct ptp_clock_info *ptp,
				 struct ptp_clock_request *rq,
				 int on)
{
	struct mlx5_clock *clock =
			container_of(ptp, struct mlx5_clock, ptp_info);
	struct mlx5_core_dev *mdev =
			container_of(clock, struct mlx5_core_dev, clock);
	u32 in[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
	u64 nsec_now, nsec_delta, time_stamp = 0;
	u64 cycles_now, cycles_delta;
	struct timespec64 ts;
	unsigned long flags;
	u32 field_select = 0;
	u8 pin_mode = 0;
	u8 pattern = 0;
	int pin = -1;
	int err = 0;
	s64 ns;

	if (!MLX5_PPS_CAP(mdev))
		return -EOPNOTSUPP;

	if (rq->perout.index >= clock->ptp_info.n_pins)
		return -EINVAL;

	if (on) {
		pin = ptp_find_pin(clock->ptp, PTP_PF_PEROUT,
				   rq->perout.index);
		if (pin < 0)
			return -EBUSY;

		pin_mode = MLX5_PIN_MODE_OUT;
		pattern = MLX5_OUT_PATTERN_PERIODIC;
		ts.tv_sec = rq->perout.period.sec;
		ts.tv_nsec = rq->perout.period.nsec;
		ns = timespec64_to_ns(&ts);

		if ((ns >> 1) != 500000000LL)
			return -EINVAL;

		ts.tv_sec = rq->perout.start.sec;
		ts.tv_nsec = rq->perout.start.nsec;
		ns = timespec64_to_ns(&ts);
		cycles_now = mlx5_read_internal_timer(mdev);
		write_lock_irqsave(&clock->lock, flags);
		nsec_now = timecounter_cyc2time(&clock->tc, cycles_now);
		nsec_delta = ns - nsec_now;
		cycles_delta = div64_u64(nsec_delta << clock->cycles.shift,
					 clock->cycles.mult);
		write_unlock_irqrestore(&clock->lock, flags);
		time_stamp = cycles_now + cycles_delta;
		field_select = MLX5_MTPPS_FS_PIN_MODE |
			       MLX5_MTPPS_FS_PATTERN |
			       MLX5_MTPPS_FS_ENABLE |
			       MLX5_MTPPS_FS_TIME_STAMP;
	} else {
		pin = rq->perout.index;
		field_select = MLX5_MTPPS_FS_ENABLE;
	}

	MLX5_SET(mtpps_reg, in, pin, pin);
	MLX5_SET(mtpps_reg, in, pin_mode, pin_mode);
	MLX5_SET(mtpps_reg, in, pattern, pattern);
	MLX5_SET(mtpps_reg, in, enable, on);
	MLX5_SET64(mtpps_reg, in, time_stamp, time_stamp);
	MLX5_SET(mtpps_reg, in, field_select, field_select);

	err = mlx5_set_mtpps(mdev, in, sizeof(in));
	if (err)
		return err;

	return mlx5_set_mtppse(mdev, pin, 0,
			       MLX5_EVENT_MODE_REPETETIVE & on);
}

static int mlx5_pps_configure(struct ptp_clock_info *ptp,
			      struct ptp_clock_request *rq,
			      int on)
{
	struct mlx5_clock *clock =
			container_of(ptp, struct mlx5_clock, ptp_info);

	clock->pps_info.enabled = !!on;
	return 0;
}

static int mlx5_ptp_enable(struct ptp_clock_info *ptp,
			   struct ptp_clock_request *rq,
			   int on)
{
	switch (rq->type) {
	case PTP_CLK_REQ_EXTTS:
		return mlx5_extts_configure(ptp, rq, on);
	case PTP_CLK_REQ_PEROUT:
		return mlx5_perout_configure(ptp, rq, on);
	case PTP_CLK_REQ_PPS:
		return mlx5_pps_configure(ptp, rq, on);
	default:
		return -EOPNOTSUPP;
	}
	return 0;
}

static int mlx5_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
			   enum ptp_pin_function func, unsigned int chan)
{
	return (func == PTP_PF_PHYSYNC) ? -EOPNOTSUPP : 0;
}

static const struct ptp_clock_info mlx5_ptp_clock_info = {
	.owner		= THIS_MODULE,
	.name		= "mlx5_p2p",
	.max_adj	= 100000000,
	.n_alarm	= 0,
	.n_ext_ts	= 0,
	.n_per_out	= 0,
	.n_pins		= 0,
	.pps		= 0,
	.adjfreq	= mlx5_ptp_adjfreq,
	.adjtime	= mlx5_ptp_adjtime,
	.gettime64	= mlx5_ptp_gettime,
	.settime64	= mlx5_ptp_settime,
	.enable		= NULL,
	.verify		= NULL,
};

static int mlx5_init_pin_config(struct mlx5_clock *clock)
{
	int i;

	clock->ptp_info.pin_config =
			kzalloc(sizeof(*clock->ptp_info.pin_config) *
				clock->ptp_info.n_pins, GFP_KERNEL);
	if (!clock->ptp_info.pin_config)
		return -ENOMEM;
	clock->ptp_info.enable = mlx5_ptp_enable;
	clock->ptp_info.verify = mlx5_ptp_verify;
	clock->ptp_info.pps = 1;

	for (i = 0; i < clock->ptp_info.n_pins; i++) {
		snprintf(clock->ptp_info.pin_config[i].name,
			 sizeof(clock->ptp_info.pin_config[i].name),
			 "mlx5_pps%d", i);
		clock->ptp_info.pin_config[i].index = i;
		clock->ptp_info.pin_config[i].func = PTP_PF_NONE;
		clock->ptp_info.pin_config[i].chan = i;
	}

	return 0;
}

static void mlx5_get_pps_caps(struct mlx5_core_dev *mdev)
{
	struct mlx5_clock *clock = &mdev->clock;
	u32 out[MLX5_ST_SZ_DW(mtpps_reg)] = {0};

	mlx5_query_mtpps(mdev, out, sizeof(out));

	clock->ptp_info.n_pins = MLX5_GET(mtpps_reg, out,
					  cap_number_of_pps_pins);
	clock->ptp_info.n_ext_ts = MLX5_GET(mtpps_reg, out,
					    cap_max_num_of_pps_in_pins);
	clock->ptp_info.n_per_out = MLX5_GET(mtpps_reg, out,
					     cap_max_num_of_pps_out_pins);

	clock->pps_info.pin_caps[0] = MLX5_GET(mtpps_reg, out, cap_pin_0_mode);
	clock->pps_info.pin_caps[1] = MLX5_GET(mtpps_reg, out, cap_pin_1_mode);
	clock->pps_info.pin_caps[2] = MLX5_GET(mtpps_reg, out, cap_pin_2_mode);
	clock->pps_info.pin_caps[3] = MLX5_GET(mtpps_reg, out, cap_pin_3_mode);
	clock->pps_info.pin_caps[4] = MLX5_GET(mtpps_reg, out, cap_pin_4_mode);
	clock->pps_info.pin_caps[5] = MLX5_GET(mtpps_reg, out, cap_pin_5_mode);
	clock->pps_info.pin_caps[6] = MLX5_GET(mtpps_reg, out, cap_pin_6_mode);
	clock->pps_info.pin_caps[7] = MLX5_GET(mtpps_reg, out, cap_pin_7_mode);
}

void mlx5_pps_event(struct mlx5_core_dev *mdev,
		    struct mlx5_eqe *eqe)
{
	struct mlx5_clock *clock = &mdev->clock;
	struct ptp_clock_event ptp_event;
	struct timespec64 ts;
	u64 nsec_now, nsec_delta;
	u64 cycles_now, cycles_delta;
	int pin = eqe->data.pps.pin;
	s64 ns;
	unsigned long flags;

	switch (clock->ptp_info.pin_config[pin].func) {
	case PTP_PF_EXTTS:
		ptp_event.index = pin;
		ptp_event.timestamp = timecounter_cyc2time(&clock->tc,
					be64_to_cpu(eqe->data.pps.time_stamp));
		if (clock->pps_info.enabled) {
			ptp_event.type = PTP_CLOCK_PPSUSR;
			ptp_event.pps_times.ts_real =
					ns_to_timespec64(ptp_event.timestamp);
		} else {
			ptp_event.type = PTP_CLOCK_EXTTS;
		}
		ptp_clock_event(clock->ptp, &ptp_event);
		break;
	case PTP_PF_PEROUT:
		mlx5_ptp_gettime(&clock->ptp_info, &ts);
		cycles_now = mlx5_read_internal_timer(mdev);
		ts.tv_sec += 1;
		ts.tv_nsec = 0;
		ns = timespec64_to_ns(&ts);
		write_lock_irqsave(&clock->lock, flags);
		nsec_now = timecounter_cyc2time(&clock->tc, cycles_now);
		nsec_delta = ns - nsec_now;
		cycles_delta = div64_u64(nsec_delta << clock->cycles.shift,
					 clock->cycles.mult);
		clock->pps_info.start[pin] = cycles_now + cycles_delta;
		schedule_work(&clock->pps_info.out_work);
		write_unlock_irqrestore(&clock->lock, flags);
		break;
	default:
		mlx5_core_err(mdev, " Unhandled event\n");
	}
}

void mlx5_init_clock(struct mlx5_core_dev *mdev)
{
	struct mlx5_clock *clock = &mdev->clock;
	u64 ns;
	u64 frac = 0;
	u32 dev_freq;

	dev_freq = MLX5_CAP_GEN(mdev, device_frequency_khz);
	if (!dev_freq) {
		mlx5_core_warn(mdev, "invalid device_frequency_khz, aborting HW clock init\n");
		return;
	}
	rwlock_init(&clock->lock);
	clock->cycles.read = read_internal_timer;
	clock->cycles.shift = MLX5_CYCLES_SHIFT;
	clock->cycles.mult = clocksource_khz2mult(dev_freq,
						  clock->cycles.shift);
	clock->nominal_c_mult = clock->cycles.mult;
	clock->cycles.mask = CLOCKSOURCE_MASK(41);
	clock->mdev = mdev;

	timecounter_init(&clock->tc, &clock->cycles,
			 ktime_to_ns(ktime_get_real()));

	/* Calculate period in seconds to call the overflow watchdog - to make
	 * sure counter is checked at least once every wrap around.
	 */
	ns = cyclecounter_cyc2ns(&clock->cycles, clock->cycles.mask,
				 frac, &frac);
	do_div(ns, NSEC_PER_SEC / 2 / HZ);
	clock->overflow_period = ns;

	mdev->clock_info_page = alloc_page(GFP_KERNEL);
	if (mdev->clock_info_page) {
		mdev->clock_info = kmap(mdev->clock_info_page);
		if (!mdev->clock_info) {
			__free_page(mdev->clock_info_page);
			mlx5_core_warn(mdev, "failed to map clock page\n");
		} else {
			mdev->clock_info->sign   = 0;
			mdev->clock_info->nsec   = clock->tc.nsec;
			mdev->clock_info->cycles = clock->tc.cycle_last;
			mdev->clock_info->mask   = clock->cycles.mask;
			mdev->clock_info->mult   = clock->nominal_c_mult;
			mdev->clock_info->shift  = clock->cycles.shift;
			mdev->clock_info->frac   = clock->tc.frac;
			mdev->clock_info->overflow_period =
						clock->overflow_period;
		}
	}

	INIT_WORK(&clock->pps_info.out_work, mlx5_pps_out);
	INIT_DELAYED_WORK(&clock->overflow_work, mlx5_timestamp_overflow);
	if (clock->overflow_period)
		schedule_delayed_work(&clock->overflow_work, 0);
	else
		mlx5_core_warn(mdev, "invalid overflow period, overflow_work is not scheduled\n");

	/* Configure the PHC */
	clock->ptp_info = mlx5_ptp_clock_info;

	/* Initialize 1PPS data structures */
	if (MLX5_PPS_CAP(mdev))
		mlx5_get_pps_caps(mdev);
	if (clock->ptp_info.n_pins)
		mlx5_init_pin_config(clock);

	clock->ptp = ptp_clock_register(&clock->ptp_info,
					&mdev->pdev->dev);
	if (IS_ERR(clock->ptp)) {
		mlx5_core_warn(mdev, "ptp_clock_register failed %ld\n",
			       PTR_ERR(clock->ptp));
		clock->ptp = NULL;
	}
}

void mlx5_cleanup_clock(struct mlx5_core_dev *mdev)
{
	struct mlx5_clock *clock = &mdev->clock;

	if (!MLX5_CAP_GEN(mdev, device_frequency_khz))
		return;

	if (clock->ptp) {
		ptp_clock_unregister(clock->ptp);
		clock->ptp = NULL;
	}

	cancel_work_sync(&clock->pps_info.out_work);
	cancel_delayed_work_sync(&clock->overflow_work);

	if (mdev->clock_info) {
		kunmap(mdev->clock_info_page);
		__free_page(mdev->clock_info_page);
		mdev->clock_info = NULL;
	}

	kfree(clock->ptp_info.pin_config);
}
Commit	Line	Data
ef9814de EBE	1	/*
	2	* Copyright (c) 2015, Mellanox Technologies. All rights reserved.
	3	*
	4	* This software is available to you under a choice of one of two
	5	* licenses. You may choose to be licensed under the terms of the GNU
	6	* General Public License (GPL) Version 2, available from the file
	7	* COPYING in the main directory of this source tree, or the
	8	* OpenIB.org BSD license below:
	9	*
	10	* Redistribution and use in source and binary forms, with or
	11	* without modification, are permitted provided that the following
	12	* conditions are met:
	13	*
	14	* - Redistributions of source code must retain the above
	15	* copyright notice, this list of conditions and the following
	16	* disclaimer.
	17	*
	18	* - Redistributions in binary form must reproduce the above
	19	* copyright notice, this list of conditions and the following
	20	* disclaimer in the documentation and/or other materials
	21	* provided with the distribution.
	22	*
	23	* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
	24	* EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
	25	* MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
	26	* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS
	27	* BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN
	28	* ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
	29	* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
	30	* SOFTWARE.
	31	*/
	32
	33	#include <linux/clocksource.h>
24d33d2c FD	34	#include <linux/highmem.h>
24d33d2c FD	35	#include <rdma/mlx5-abi.h>
ef9814de	36	#include "en.h"
9afe9a53	37	#include "clock.h"
ef9814de EBE	38
ef9814de EBE	39	enum {
7c39afb3	40	MLX5_CYCLES_SHIFT = 23
ef9814de EBE	41	};
ef9814de EBE	42
ee7f1220	43	enum {
7c39afb3 FD	44	MLX5_PIN_MODE_IN = 0x0,
7c39afb3 FD	45	MLX5_PIN_MODE_OUT = 0x1,
ee7f1220 EE	46	};
	47
	48	enum {
7c39afb3 FD	49	MLX5_OUT_PATTERN_PULSE = 0x0,
7c39afb3 FD	50	MLX5_OUT_PATTERN_PERIODIC = 0x1,
ee7f1220 EE	51	};
	52
	53	enum {
7c39afb3 FD	54	MLX5_EVENT_MODE_DISABLE = 0x0,
	55	MLX5_EVENT_MODE_REPETETIVE = 0x1,
	56	MLX5_EVENT_MODE_ONCE_TILL_ARM = 0x2,
ee7f1220 EE	57	};
ee7f1220 EE	58
fa367688	59	enum {
7c39afb3 FD	60	MLX5_MTPPS_FS_ENABLE = BIT(0x0),
	61	MLX5_MTPPS_FS_PATTERN = BIT(0x2),
	62	MLX5_MTPPS_FS_PIN_MODE = BIT(0x3),
	63	MLX5_MTPPS_FS_TIME_STAMP = BIT(0x4),
	64	MLX5_MTPPS_FS_OUT_PULSE_DURATION = BIT(0x5),
	65	MLX5_MTPPS_FS_ENH_OUT_PER_ADJ = BIT(0x7),
fa367688 EE	66	};
fa367688 EE	67
7c39afb3	68	static u64 read_internal_timer(const struct cyclecounter *cc)
ef9814de	69	{
7c39afb3 FD	70	struct mlx5_clock *clock = container_of(cc, struct mlx5_clock, cycles);
	71	struct mlx5_core_dev *mdev = container_of(clock, struct mlx5_core_dev,
	72	clock);
ef9814de	73
7c39afb3	74	return mlx5_read_internal_timer(mdev) & cc->mask;
ef9814de EBE	75	}
ef9814de EBE	76
24d33d2c FD	77	static void mlx5_update_clock_info_page(struct mlx5_core_dev *mdev)
	78	{
	79	struct mlx5_ib_clock_info *clock_info = mdev->clock_info;
	80	struct mlx5_clock *clock = &mdev->clock;
	81	u32 sign;
	82
	83	if (!clock_info)
	84	return;
	85
	86	sign = smp_load_acquire(&clock_info->sign);
	87	smp_store_mb(clock_info->sign,
	88	sign \| MLX5_IB_CLOCK_INFO_KERNEL_UPDATING);
	89
	90	clock_info->cycles = clock->tc.cycle_last;
	91	clock_info->mult = clock->cycles.mult;
	92	clock_info->nsec = clock->tc.nsec;
	93	clock_info->frac = clock->tc.frac;
	94
	95	smp_store_release(&clock_info->sign,
	96	sign + MLX5_IB_CLOCK_INFO_KERNEL_UPDATING * 2);
	97	}
	98
7c39afb3	99	static void mlx5_pps_out(struct work_struct *work)
4272f9b8	100	{
7c39afb3 FD	101	struct mlx5_pps *pps_info = container_of(work, struct mlx5_pps,
	102	out_work);
	103	struct mlx5_clock *clock = container_of(pps_info, struct mlx5_clock,
	104	pps_info);
	105	struct mlx5_core_dev *mdev = container_of(clock, struct mlx5_core_dev,
	106	clock);
4272f9b8 EE	107	u32 in[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
	108	unsigned long flags;
	109	int i;
	110
7c39afb3	111	for (i = 0; i < clock->ptp_info.n_pins; i++) {
4272f9b8 EE	112	u64 tstart;
4272f9b8 EE	113
7c39afb3 FD	114	write_lock_irqsave(&clock->lock, flags);
	115	tstart = clock->pps_info.start[i];
	116	clock->pps_info.start[i] = 0;
	117	write_unlock_irqrestore(&clock->lock, flags);
4272f9b8 EE	118	if (!tstart)
	119	continue;
	120
	121	MLX5_SET(mtpps_reg, in, pin, i);
	122	MLX5_SET64(mtpps_reg, in, time_stamp, tstart);
7c39afb3 FD	123	MLX5_SET(mtpps_reg, in, field_select, MLX5_MTPPS_FS_TIME_STAMP);
7c39afb3 FD	124	mlx5_set_mtpps(mdev, in, sizeof(in));
4272f9b8 EE	125	}
	126	}
	127
7c39afb3	128	static void mlx5_timestamp_overflow(struct work_struct *work)
ef9814de EBE	129	{
ef9814de EBE	130	struct delayed_work *dwork = to_delayed_work(work);
7c39afb3 FD	131	struct mlx5_clock *clock = container_of(dwork, struct mlx5_clock,
7c39afb3 FD	132	overflow_work);
0ad9b204	133	unsigned long flags;
ef9814de	134
7c39afb3 FD	135	write_lock_irqsave(&clock->lock, flags);
7c39afb3 FD	136	timecounter_read(&clock->tc);
24d33d2c	137	mlx5_update_clock_info_page(clock->mdev);
7c39afb3 FD	138	write_unlock_irqrestore(&clock->lock, flags);
7c39afb3 FD	139	schedule_delayed_work(&clock->overflow_work, clock->overflow_period);
ef9814de EBE	140	}
ef9814de EBE	141
7c39afb3 FD	142	static int mlx5_ptp_settime(struct ptp_clock_info *ptp,
7c39afb3 FD	143	const struct timespec64 *ts)
3d8c38af	144	{
7c39afb3 FD	145	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
7c39afb3 FD	146	ptp_info);
3d8c38af	147	u64 ns = timespec64_to_ns(ts);
0ad9b204	148	unsigned long flags;
3d8c38af	149
7c39afb3 FD	150	write_lock_irqsave(&clock->lock, flags);
7c39afb3 FD	151	timecounter_init(&clock->tc, &clock->cycles, ns);
24d33d2c	152	mlx5_update_clock_info_page(clock->mdev);
7c39afb3	153	write_unlock_irqrestore(&clock->lock, flags);
3d8c38af EBE	154
	155	return 0;
	156	}
	157
7c39afb3	158	static int mlx5_ptp_gettime(struct ptp_clock_info ptp, struct timespec64 ts)
3d8c38af	159	{
7c39afb3 FD	160	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
7c39afb3 FD	161	ptp_info);
3d8c38af	162	u64 ns;
0ad9b204	163	unsigned long flags;
3d8c38af	164
7c39afb3 FD	165	write_lock_irqsave(&clock->lock, flags);
	166	ns = timecounter_read(&clock->tc);
	167	write_unlock_irqrestore(&clock->lock, flags);
3d8c38af EBE	168
	169	*ts = ns_to_timespec64(ns);
	170
	171	return 0;
	172	}
	173
7c39afb3	174	static int mlx5_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
3d8c38af	175	{
7c39afb3 FD	176	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
7c39afb3 FD	177	ptp_info);
0ad9b204	178	unsigned long flags;
3d8c38af	179
7c39afb3 FD	180	write_lock_irqsave(&clock->lock, flags);
7c39afb3 FD	181	timecounter_adjtime(&clock->tc, delta);
24d33d2c	182	mlx5_update_clock_info_page(clock->mdev);
7c39afb3	183	write_unlock_irqrestore(&clock->lock, flags);
3d8c38af EBE	184
	185	return 0;
	186	}
	187
7c39afb3	188	static int mlx5_ptp_adjfreq(struct ptp_clock_info *ptp, s32 delta)
3d8c38af EBE	189	{
	190	u64 adj;
	191	u32 diff;
0ad9b204	192	unsigned long flags;
3d8c38af	193	int neg_adj = 0;
7c39afb3 FD	194	struct mlx5_clock *clock = container_of(ptp, struct mlx5_clock,
7c39afb3 FD	195	ptp_info);
3d8c38af EBE	196
	197	if (delta < 0) {
	198	neg_adj = 1;
	199	delta = -delta;
	200	}
	201
7c39afb3	202	adj = clock->nominal_c_mult;
3d8c38af EBE	203	adj *= delta;
	204	diff = div_u64(adj, 1000000000ULL);
	205
7c39afb3 FD	206	write_lock_irqsave(&clock->lock, flags);
	207	timecounter_read(&clock->tc);
	208	clock->cycles.mult = neg_adj ? clock->nominal_c_mult - diff :
	209	clock->nominal_c_mult + diff;
24d33d2c	210	mlx5_update_clock_info_page(clock->mdev);
7c39afb3	211	write_unlock_irqrestore(&clock->lock, flags);
3d8c38af EBE	212
	213	return 0;
	214	}
	215
7c39afb3 FD	216	static int mlx5_extts_configure(struct ptp_clock_info *ptp,
	217	struct ptp_clock_request *rq,
	218	int on)
ee7f1220	219	{
7c39afb3 FD	220	struct mlx5_clock *clock =
	221	container_of(ptp, struct mlx5_clock, ptp_info);
	222	struct mlx5_core_dev *mdev =
	223	container_of(clock, struct mlx5_core_dev, clock);
ee7f1220	224	u32 in[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
49c5031c EE	225	u32 field_select = 0;
49c5031c EE	226	u8 pin_mode = 0;
ee7f1220 EE	227	u8 pattern = 0;
	228	int pin = -1;
	229	int err = 0;
	230
7c39afb3	231	if (!MLX5_PPS_CAP(mdev))
ee7f1220 EE	232	return -EOPNOTSUPP;
ee7f1220 EE	233
7c39afb3	234	if (rq->extts.index >= clock->ptp_info.n_pins)
ee7f1220 EE	235	return -EINVAL;
	236
	237	if (on) {
7c39afb3	238	pin = ptp_find_pin(clock->ptp, PTP_PF_EXTTS, rq->extts.index);
ee7f1220 EE	239	if (pin < 0)
ee7f1220 EE	240	return -EBUSY;
7c39afb3	241	pin_mode = MLX5_PIN_MODE_IN;
49c5031c	242	pattern = !!(rq->extts.flags & PTP_FALLING_EDGE);
7c39afb3 FD	243	field_select = MLX5_MTPPS_FS_PIN_MODE \|
	244	MLX5_MTPPS_FS_PATTERN \|
	245	MLX5_MTPPS_FS_ENABLE;
49c5031c EE	246	} else {
49c5031c EE	247	pin = rq->extts.index;
7c39afb3	248	field_select = MLX5_MTPPS_FS_ENABLE;
ee7f1220 EE	249	}
ee7f1220 EE	250
ee7f1220	251	MLX5_SET(mtpps_reg, in, pin, pin);
49c5031c	252	MLX5_SET(mtpps_reg, in, pin_mode, pin_mode);
ee7f1220 EE	253	MLX5_SET(mtpps_reg, in, pattern, pattern);
ee7f1220 EE	254	MLX5_SET(mtpps_reg, in, enable, on);
49c5031c	255	MLX5_SET(mtpps_reg, in, field_select, field_select);
ee7f1220	256
7c39afb3	257	err = mlx5_set_mtpps(mdev, in, sizeof(in));
ee7f1220 EE	258	if (err)
	259	return err;
	260
7c39afb3 FD	261	return mlx5_set_mtppse(mdev, pin, 0,
7c39afb3 FD	262	MLX5_EVENT_MODE_REPETETIVE & on);
ee7f1220 EE	263	}
ee7f1220 EE	264
7c39afb3 FD	265	static int mlx5_perout_configure(struct ptp_clock_info *ptp,
	266	struct ptp_clock_request *rq,
	267	int on)
ee7f1220	268	{
7c39afb3 FD	269	struct mlx5_clock *clock =
	270	container_of(ptp, struct mlx5_clock, ptp_info);
	271	struct mlx5_core_dev *mdev =
	272	container_of(clock, struct mlx5_core_dev, clock);
ee7f1220	273	u32 in[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
4272f9b8	274	u64 nsec_now, nsec_delta, time_stamp = 0;
ee7f1220 EE	275	u64 cycles_now, cycles_delta;
	276	struct timespec64 ts;
	277	unsigned long flags;
49c5031c EE	278	u32 field_select = 0;
	279	u8 pin_mode = 0;
	280	u8 pattern = 0;
ee7f1220	281	int pin = -1;
4272f9b8	282	int err = 0;
ee7f1220 EE	283	s64 ns;
ee7f1220 EE	284
7c39afb3	285	if (!MLX5_PPS_CAP(mdev))
ee7f1220 EE	286	return -EOPNOTSUPP;
ee7f1220 EE	287
7c39afb3	288	if (rq->perout.index >= clock->ptp_info.n_pins)
ee7f1220 EE	289	return -EINVAL;
	290
	291	if (on) {
7c39afb3	292	pin = ptp_find_pin(clock->ptp, PTP_PF_PEROUT,
ee7f1220 EE	293	rq->perout.index);
	294	if (pin < 0)
	295	return -EBUSY;
ee7f1220	296
7c39afb3 FD	297	pin_mode = MLX5_PIN_MODE_OUT;
7c39afb3 FD	298	pattern = MLX5_OUT_PATTERN_PERIODIC;
49c5031c EE	299	ts.tv_sec = rq->perout.period.sec;
	300	ts.tv_nsec = rq->perout.period.nsec;
	301	ns = timespec64_to_ns(&ts);
	302
ee7f1220 EE	303	if ((ns >> 1) != 500000000LL)
ee7f1220 EE	304	return -EINVAL;
49c5031c EE	305
	306	ts.tv_sec = rq->perout.start.sec;
	307	ts.tv_nsec = rq->perout.start.nsec;
	308	ns = timespec64_to_ns(&ts);
7c39afb3 FD	309	cycles_now = mlx5_read_internal_timer(mdev);
	310	write_lock_irqsave(&clock->lock, flags);
	311	nsec_now = timecounter_cyc2time(&clock->tc, cycles_now);
49c5031c	312	nsec_delta = ns - nsec_now;
7c39afb3 FD	313	cycles_delta = div64_u64(nsec_delta << clock->cycles.shift,
	314	clock->cycles.mult);
	315	write_unlock_irqrestore(&clock->lock, flags);
49c5031c	316	time_stamp = cycles_now + cycles_delta;
7c39afb3 FD	317	field_select = MLX5_MTPPS_FS_PIN_MODE \|
	318	MLX5_MTPPS_FS_PATTERN \|
	319	MLX5_MTPPS_FS_ENABLE \|
	320	MLX5_MTPPS_FS_TIME_STAMP;
49c5031c EE	321	} else {
49c5031c EE	322	pin = rq->perout.index;
7c39afb3	323	field_select = MLX5_MTPPS_FS_ENABLE;
49c5031c EE	324	}
49c5031c EE	325
ee7f1220	326	MLX5_SET(mtpps_reg, in, pin, pin);
49c5031c EE	327	MLX5_SET(mtpps_reg, in, pin_mode, pin_mode);
49c5031c EE	328	MLX5_SET(mtpps_reg, in, pattern, pattern);
ee7f1220 EE	329	MLX5_SET(mtpps_reg, in, enable, on);
ee7f1220 EE	330	MLX5_SET64(mtpps_reg, in, time_stamp, time_stamp);
49c5031c EE	331	MLX5_SET(mtpps_reg, in, field_select, field_select);
49c5031c EE	332
7c39afb3	333	err = mlx5_set_mtpps(mdev, in, sizeof(in));
4272f9b8 EE	334	if (err)
	335	return err;
	336
7c39afb3 FD	337	return mlx5_set_mtppse(mdev, pin, 0,
7c39afb3 FD	338	MLX5_EVENT_MODE_REPETETIVE & on);
ee7f1220 EE	339	}
ee7f1220 EE	340
7c39afb3 FD	341	static int mlx5_pps_configure(struct ptp_clock_info *ptp,
	342	struct ptp_clock_request *rq,
	343	int on)
cf503308	344	{
7c39afb3 FD	345	struct mlx5_clock *clock =
7c39afb3 FD	346	container_of(ptp, struct mlx5_clock, ptp_info);
cf503308	347
7c39afb3	348	clock->pps_info.enabled = !!on;
cf503308 EE	349	return 0;
	350	}
	351
7c39afb3 FD	352	static int mlx5_ptp_enable(struct ptp_clock_info *ptp,
	353	struct ptp_clock_request *rq,
	354	int on)
ee7f1220 EE	355	{
	356	switch (rq->type) {
	357	case PTP_CLK_REQ_EXTTS:
7c39afb3	358	return mlx5_extts_configure(ptp, rq, on);
ee7f1220	359	case PTP_CLK_REQ_PEROUT:
7c39afb3	360	return mlx5_perout_configure(ptp, rq, on);
cf503308	361	case PTP_CLK_REQ_PPS:
7c39afb3	362	return mlx5_pps_configure(ptp, rq, on);
ee7f1220 EE	363	default:
	364	return -EOPNOTSUPP;
	365	}
	366	return 0;
	367	}
	368
7c39afb3 FD	369	static int mlx5_ptp_verify(struct ptp_clock_info *ptp, unsigned int pin,
7c39afb3 FD	370	enum ptp_pin_function func, unsigned int chan)
ee7f1220 EE	371	{
	372	return (func == PTP_PF_PHYSYNC) ? -EOPNOTSUPP : 0;
	373	}
	374
7c39afb3	375	static const struct ptp_clock_info mlx5_ptp_clock_info = {
3d8c38af	376	.owner = THIS_MODULE,
7c39afb3	377	.name = "mlx5_p2p",
3d8c38af EBE	378	.max_adj = 100000000,
	379	.n_alarm = 0,
	380	.n_ext_ts = 0,
	381	.n_per_out = 0,
	382	.n_pins = 0,
	383	.pps = 0,
7c39afb3 FD	384	.adjfreq = mlx5_ptp_adjfreq,
	385	.adjtime = mlx5_ptp_adjtime,
	386	.gettime64 = mlx5_ptp_gettime,
	387	.settime64 = mlx5_ptp_settime,
3d8c38af	388	.enable = NULL,
ee7f1220	389	.verify = NULL,
3d8c38af EBE	390	};
3d8c38af EBE	391
7c39afb3	392	static int mlx5_init_pin_config(struct mlx5_clock *clock)
ee7f1220 EE	393	{
	394	int i;
	395
7c39afb3 FD	396	clock->ptp_info.pin_config =
	397	kzalloc(sizeof(clock->ptp_info.pin_config)
	398	clock->ptp_info.n_pins, GFP_KERNEL);
	399	if (!clock->ptp_info.pin_config)
ee7f1220	400	return -ENOMEM;
7c39afb3 FD	401	clock->ptp_info.enable = mlx5_ptp_enable;
	402	clock->ptp_info.verify = mlx5_ptp_verify;
	403	clock->ptp_info.pps = 1;
ee7f1220	404
7c39afb3 FD	405	for (i = 0; i < clock->ptp_info.n_pins; i++) {
	406	snprintf(clock->ptp_info.pin_config[i].name,
	407	sizeof(clock->ptp_info.pin_config[i].name),
ee7f1220	408	"mlx5_pps%d", i);
7c39afb3 FD	409	clock->ptp_info.pin_config[i].index = i;
	410	clock->ptp_info.pin_config[i].func = PTP_PF_NONE;
	411	clock->ptp_info.pin_config[i].chan = i;
ee7f1220 EE	412	}
	413
	414	return 0;
	415	}
	416
7c39afb3	417	static void mlx5_get_pps_caps(struct mlx5_core_dev *mdev)
ee7f1220	418	{
7c39afb3	419	struct mlx5_clock *clock = &mdev->clock;
ee7f1220 EE	420	u32 out[MLX5_ST_SZ_DW(mtpps_reg)] = {0};
ee7f1220 EE	421
7c39afb3 FD	422	mlx5_query_mtpps(mdev, out, sizeof(out));
	423
	424	clock->ptp_info.n_pins = MLX5_GET(mtpps_reg, out,
	425	cap_number_of_pps_pins);
	426	clock->ptp_info.n_ext_ts = MLX5_GET(mtpps_reg, out,
	427	cap_max_num_of_pps_in_pins);
	428	clock->ptp_info.n_per_out = MLX5_GET(mtpps_reg, out,
	429	cap_max_num_of_pps_out_pins);
	430
	431	clock->pps_info.pin_caps[0] = MLX5_GET(mtpps_reg, out, cap_pin_0_mode);
	432	clock->pps_info.pin_caps[1] = MLX5_GET(mtpps_reg, out, cap_pin_1_mode);
	433	clock->pps_info.pin_caps[2] = MLX5_GET(mtpps_reg, out, cap_pin_2_mode);
	434	clock->pps_info.pin_caps[3] = MLX5_GET(mtpps_reg, out, cap_pin_3_mode);
	435	clock->pps_info.pin_caps[4] = MLX5_GET(mtpps_reg, out, cap_pin_4_mode);
	436	clock->pps_info.pin_caps[5] = MLX5_GET(mtpps_reg, out, cap_pin_5_mode);
	437	clock->pps_info.pin_caps[6] = MLX5_GET(mtpps_reg, out, cap_pin_6_mode);
	438	clock->pps_info.pin_caps[7] = MLX5_GET(mtpps_reg, out, cap_pin_7_mode);
ee7f1220 EE	439	}
ee7f1220 EE	440
7c39afb3 FD	441	void mlx5_pps_event(struct mlx5_core_dev *mdev,
7c39afb3 FD	442	struct mlx5_eqe *eqe)
ee7f1220	443	{
7c39afb3 FD	444	struct mlx5_clock *clock = &mdev->clock;
7c39afb3 FD	445	struct ptp_clock_event ptp_event;
4272f9b8 EE	446	struct timespec64 ts;
	447	u64 nsec_now, nsec_delta;
	448	u64 cycles_now, cycles_delta;
7c39afb3	449	int pin = eqe->data.pps.pin;
4272f9b8 EE	450	s64 ns;
4272f9b8 EE	451	unsigned long flags;
ee7f1220	452
7c39afb3	453	switch (clock->ptp_info.pin_config[pin].func) {
4272f9b8	454	case PTP_PF_EXTTS:
afc98a0b FD	455	ptp_event.index = pin;
	456	ptp_event.timestamp = timecounter_cyc2time(&clock->tc,
	457	be64_to_cpu(eqe->data.pps.time_stamp));
7c39afb3 FD	458	if (clock->pps_info.enabled) {
7c39afb3 FD	459	ptp_event.type = PTP_CLOCK_PPSUSR;
afc98a0b FD	460	ptp_event.pps_times.ts_real =
afc98a0b FD	461	ns_to_timespec64(ptp_event.timestamp);
cf503308	462	} else {
7c39afb3	463	ptp_event.type = PTP_CLOCK_EXTTS;
cf503308	464	}
7c39afb3	465	ptp_clock_event(clock->ptp, &ptp_event);
4272f9b8 EE	466	break;
4272f9b8 EE	467	case PTP_PF_PEROUT:
7c39afb3 FD	468	mlx5_ptp_gettime(&clock->ptp_info, &ts);
7c39afb3 FD	469	cycles_now = mlx5_read_internal_timer(mdev);
4272f9b8 EE	470	ts.tv_sec += 1;
	471	ts.tv_nsec = 0;
	472	ns = timespec64_to_ns(&ts);
7c39afb3 FD	473	write_lock_irqsave(&clock->lock, flags);
7c39afb3 FD	474	nsec_now = timecounter_cyc2time(&clock->tc, cycles_now);
4272f9b8	475	nsec_delta = ns - nsec_now;
7c39afb3 FD	476	cycles_delta = div64_u64(nsec_delta << clock->cycles.shift,
	477	clock->cycles.mult);
	478	clock->pps_info.start[pin] = cycles_now + cycles_delta;
	479	schedule_work(&clock->pps_info.out_work);
	480	write_unlock_irqrestore(&clock->lock, flags);
4272f9b8 EE	481	break;
4272f9b8 EE	482	default:
7c39afb3	483	mlx5_core_err(mdev, " Unhandled event\n");
4272f9b8	484	}
ee7f1220 EE	485	}
ee7f1220 EE	486
7c39afb3	487	void mlx5_init_clock(struct mlx5_core_dev *mdev)
ef9814de	488	{
7c39afb3	489	struct mlx5_clock *clock = &mdev->clock;
ef9814de EBE	490	u64 ns;
	491	u64 frac = 0;
	492	u32 dev_freq;
	493
7c39afb3	494	dev_freq = MLX5_CAP_GEN(mdev, device_frequency_khz);
ef9814de	495	if (!dev_freq) {
7c39afb3	496	mlx5_core_warn(mdev, "invalid device_frequency_khz, aborting HW clock init\n");
ef9814de EBE	497	return;
ef9814de EBE	498	}
7c39afb3 FD	499	rwlock_init(&clock->lock);
	500	clock->cycles.read = read_internal_timer;
	501	clock->cycles.shift = MLX5_CYCLES_SHIFT;
	502	clock->cycles.mult = clocksource_khz2mult(dev_freq,
	503	clock->cycles.shift);
	504	clock->nominal_c_mult = clock->cycles.mult;
	505	clock->cycles.mask = CLOCKSOURCE_MASK(41);
24d33d2c	506	clock->mdev = mdev;
7c39afb3 FD	507
7c39afb3 FD	508	timecounter_init(&clock->tc, &clock->cycles,
ef9814de EBE	509	ktime_to_ns(ktime_get_real()));
	510
	511	/* Calculate period in seconds to call the overflow watchdog - to make
	512	* sure counter is checked at least once every wrap around.
	513	*/
7c39afb3	514	ns = cyclecounter_cyc2ns(&clock->cycles, clock->cycles.mask,
ef9814de EBE	515	frac, &frac);
ef9814de EBE	516	do_div(ns, NSEC_PER_SEC / 2 / HZ);
7c39afb3	517	clock->overflow_period = ns;
ef9814de	518
24d33d2c FD	519	mdev->clock_info_page = alloc_page(GFP_KERNEL);
	520	if (mdev->clock_info_page) {
	521	mdev->clock_info = kmap(mdev->clock_info_page);
	522	if (!mdev->clock_info) {
	523	__free_page(mdev->clock_info_page);
	524	mlx5_core_warn(mdev, "failed to map clock page\n");
	525	} else {
	526	mdev->clock_info->sign = 0;
	527	mdev->clock_info->nsec = clock->tc.nsec;
	528	mdev->clock_info->cycles = clock->tc.cycle_last;
	529	mdev->clock_info->mask = clock->cycles.mask;
	530	mdev->clock_info->mult = clock->nominal_c_mult;
	531	mdev->clock_info->shift = clock->cycles.shift;
	532	mdev->clock_info->frac = clock->tc.frac;
	533	mdev->clock_info->overflow_period =
	534	clock->overflow_period;
	535	}
	536	}
	537
7c39afb3 FD	538	INIT_WORK(&clock->pps_info.out_work, mlx5_pps_out);
	539	INIT_DELAYED_WORK(&clock->overflow_work, mlx5_timestamp_overflow);
	540	if (clock->overflow_period)
	541	schedule_delayed_work(&clock->overflow_work, 0);
ef9814de	542	else
7c39afb3	543	mlx5_core_warn(mdev, "invalid overflow period, overflow_work is not scheduled\n");
3d8c38af EBE	544
3d8c38af EBE	545	/* Configure the PHC */
7c39afb3	546	clock->ptp_info = mlx5_ptp_clock_info;
3d8c38af	547
ee7f1220	548	/* Initialize 1PPS data structures */
7c39afb3 FD	549	if (MLX5_PPS_CAP(mdev))
	550	mlx5_get_pps_caps(mdev);
	551	if (clock->ptp_info.n_pins)
	552	mlx5_init_pin_config(clock);
	553
	554	clock->ptp = ptp_clock_register(&clock->ptp_info,
	555	&mdev->pdev->dev);
	556	if (IS_ERR(clock->ptp)) {
	557	mlx5_core_warn(mdev, "ptp_clock_register failed %ld\n",
	558	PTR_ERR(clock->ptp));
	559	clock->ptp = NULL;
3d8c38af	560	}
ef9814de EBE	561	}
ef9814de EBE	562
7c39afb3	563	void mlx5_cleanup_clock(struct mlx5_core_dev *mdev)
ef9814de	564	{
7c39afb3	565	struct mlx5_clock *clock = &mdev->clock;
ef9814de	566
7c39afb3	567	if (!MLX5_CAP_GEN(mdev, device_frequency_khz))
ef9814de EBE	568	return;
ef9814de EBE	569
7c39afb3 FD	570	if (clock->ptp) {
	571	ptp_clock_unregister(clock->ptp);
	572	clock->ptp = NULL;
3d8c38af EBE	573	}
3d8c38af EBE	574
7c39afb3 FD	575	cancel_work_sync(&clock->pps_info.out_work);
7c39afb3 FD	576	cancel_delayed_work_sync(&clock->overflow_work);
24d33d2c FD	577
	578	if (mdev->clock_info) {
	579	kunmap(mdev->clock_info_page);
	580	__free_page(mdev->clock_info_page);
	581	mdev->clock_info = NULL;
	582	}
	583
7c39afb3	584	kfree(clock->ptp_info.pin_config);
ef9814de	585	}