Commit e014ae39 authored by Min Li's avatar Min Li Committed by Jakub Kicinski
Browse files

ptp: idt82p33: add adjphase support 



Add idt82p33_adjphase() to support PHC write phase mode.

Signed-off-by: default avatarMin Li <min.li.xe@renesas.com>
Acked-by: default avatarRichard Cochran <richardcochran@gmail.com>
Link: https://lore.kernel.org/r/1604634729-24960-1-git-send-email-min.li.xe@renesas.com


Signed-off-by: default avatarJakub Kicinski <kuba@kernel.org>
parent 419a38ce

drivers/ptp/ptp_idt82p33.c

+151 −66
Original line number Diff line number Diff line
@@ -21,6 +21,7 @@ MODULE_DESCRIPTION("Driver for IDT 82p33xxx clock devices"); 
MODULE_AUTHOR("IDT support-1588 <IDT-support-1588@lm.renesas.com>");

MODULE_VERSION("1.0");

MODULE_LICENSE("GPL");

MODULE_FIRMWARE(FW_FILENAME);



/* Module Parameters */

static u32 sync_tod_timeout = SYNC_TOD_TIMEOUT_SEC;
@@ -448,8 +449,11 @@ static int idt82p33_measure_tod_write_overhead(struct idt82p33_channel *channel) 


	err = idt82p33_measure_settime_gettime_gap_overhead(channel, &gap_ns);



	if (err)

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

		return err;

	}



	err = idt82p33_measure_one_byte_write_overhead(channel,

						       &one_byte_write_ns);
@@ -518,13 +522,10 @@ static int idt82p33_sync_tod(struct idt82p33_channel *channel, bool enable) 
	u8 sync_cnfg;

	int err;



	if (enable == channel->sync_tod_on) {

		if (enable && sync_tod_timeout) {

			mod_delayed_work(system_wq, &channel->sync_tod_work,

	/* Turn it off after sync_tod_timeout seconds */

	if (enable && sync_tod_timeout)

		ptp_schedule_worker(channel->ptp_clock,

				    sync_tod_timeout * HZ);

		}

		return 0;

	}



	err = idt82p33_read(idt82p33, channel->dpll_sync_cnfg,

			    &sync_cnfg, sizeof(sync_cnfg));
@@ -532,29 +533,17 @@ static int idt82p33_sync_tod(struct idt82p33_channel *channel, bool enable) 
		return err;



	sync_cnfg &= ~SYNC_TOD;



	if (enable)

		sync_cnfg |= SYNC_TOD;



	err = idt82p33_write(idt82p33, channel->dpll_sync_cnfg,

	return idt82p33_write(idt82p33, channel->dpll_sync_cnfg,

			      &sync_cnfg, sizeof(sync_cnfg));

	if (err)

		return err;



	channel->sync_tod_on = enable;



	if (enable && sync_tod_timeout) {

		mod_delayed_work(system_wq, &channel->sync_tod_work,

				 sync_tod_timeout * HZ);

}



	return 0;

}



static void idt82p33_sync_tod_work_handler(struct work_struct *work)

static long idt82p33_sync_tod_work_handler(struct ptp_clock_info *ptp)

{

	struct idt82p33_channel *channel =

		container_of(work, struct idt82p33_channel, sync_tod_work.work);

			container_of(ptp, struct idt82p33_channel, caps);

	struct idt82p33 *idt82p33 = channel->idt82p33;



	mutex_lock(&idt82p33->reg_lock);
@@ -562,35 +551,46 @@ static void idt82p33_sync_tod_work_handler(struct work_struct *work) 
	(void)idt82p33_sync_tod(channel, false);



	mutex_unlock(&idt82p33->reg_lock);



	/* Return a negative value here to not reschedule */

	return -1;

}



static int idt82p33_pps_enable(struct idt82p33_channel *channel, bool enable)

static int idt82p33_output_enable(struct idt82p33_channel *channel,

				  bool enable, unsigned int outn)

{

	struct idt82p33 *idt82p33 = channel->idt82p33;

	u8 mask, outn, val;

	int err;

	u8 val;



	mask = channel->output_mask;

	outn = 0;



	while (mask) {

		if (mask & 0x1) {

			err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn),

					    &val, sizeof(val));

	err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));

	if (err)

		return err;



	if (enable)

		val &= ~SQUELCH_ENABLE;

	else

		val |= SQUELCH_ENABLE;



			err = idt82p33_write(idt82p33, OUT_MUX_CNFG(outn),

					     &val, sizeof(val));

	return idt82p33_write(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));

}



static int idt82p33_output_mask_enable(struct idt82p33_channel *channel,

				       bool enable)

{

	u16 mask;

	int err;

	u8 outn;



	mask = channel->output_mask;

	outn = 0;



	while (mask) {

		if (mask & 0x1) {

			err = idt82p33_output_enable(channel, enable, outn);

			if (err)

				return err;

		}



		mask >>= 0x1;

		outn++;

	}
@@ -598,6 +598,20 @@ static int idt82p33_pps_enable(struct idt82p33_channel *channel, bool enable) 
	return 0;

}



static int idt82p33_perout_enable(struct idt82p33_channel *channel,

				  bool enable,

				  struct ptp_perout_request *perout)

{

	unsigned int flags = perout->flags;



	/* Enable/disable output based on output_mask */

	if (flags == PEROUT_ENABLE_OUTPUT_MASK)

		return idt82p33_output_mask_enable(channel, enable);



	/* Enable/disable individual output instead */

	return idt82p33_output_enable(channel, enable, perout->index);

}



static int idt82p33_enable_tod(struct idt82p33_channel *channel)

{

	struct idt82p33 *idt82p33 = channel->idt82p33;
@@ -611,15 +625,13 @@ static int idt82p33_enable_tod(struct idt82p33_channel *channel) 
	if (err)

		return err;



	err = idt82p33_pps_enable(channel, false);



	if (err)

		return err;



	err = idt82p33_measure_tod_write_overhead(channel);



	if (err)

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

		return err;

	}



	err = _idt82p33_settime(channel, &ts);
@@ -638,10 +650,8 @@ static void idt82p33_ptp_clock_unregister_all(struct idt82p33 *idt82p33) 


		channel = &idt82p33->channel[i];



		if (channel->ptp_clock) {

		if (channel->ptp_clock)

			ptp_clock_unregister(channel->ptp_clock);

			cancel_delayed_work_sync(&channel->sync_tod_work);

		}

	}

}
@@ -659,14 +669,15 @@ static int idt82p33_enable(struct ptp_clock_info *ptp, 


	if (rq->type == PTP_CLK_REQ_PEROUT) {

		if (!on)

			err = idt82p33_pps_enable(channel, false);



			err = idt82p33_perout_enable(channel, false,

						     &rq->perout);

		/* Only accept a 1-PPS aligned to the second. */

		else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||

		    rq->perout.period.nsec) {

			err = -ERANGE;

		} else

			err = idt82p33_pps_enable(channel, true);

			err = idt82p33_perout_enable(channel, true,

						     &rq->perout);

	}



	mutex_unlock(&idt82p33->reg_lock);
@@ -674,6 +685,48 @@ static int idt82p33_enable(struct ptp_clock_info *ptp, 
	return err;

}



static int idt82p33_adjwritephase(struct ptp_clock_info *ptp, s32 offset_ns)

{

	struct idt82p33_channel *channel =

		container_of(ptp, struct idt82p33_channel, caps);

	struct idt82p33 *idt82p33 = channel->idt82p33;

	s64 offset_regval, offset_fs;

	u8 val[4] = {0};

	int err;



	offset_fs = (s64)(-offset_ns) * 1000000;



	if (offset_fs > WRITE_PHASE_OFFSET_LIMIT)

		offset_fs = WRITE_PHASE_OFFSET_LIMIT;

	else if (offset_fs < -WRITE_PHASE_OFFSET_LIMIT)

		offset_fs = -WRITE_PHASE_OFFSET_LIMIT;



	/* Convert from phaseoffset_fs to register value */

	offset_regval = div_s64(offset_fs * 1000, IDT_T0DPLL_PHASE_RESOL);



	val[0] = offset_regval & 0xFF;

	val[1] = (offset_regval >> 8) & 0xFF;

	val[2] = (offset_regval >> 16) & 0xFF;

	val[3] = (offset_regval >> 24) & 0x1F;

	val[3] |= PH_OFFSET_EN;



	mutex_lock(&idt82p33->reg_lock);



	err = idt82p33_dpll_set_mode(channel, PLL_MODE_WPH);

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

		goto out;

	}



	err = idt82p33_write(idt82p33, channel->dpll_phase_cnfg, val,

			     sizeof(val));



out:

	mutex_unlock(&idt82p33->reg_lock);

	return err;

}



static int idt82p33_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)

{

	struct idt82p33_channel *channel =
@@ -683,6 +736,9 @@ static int idt82p33_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) 


	mutex_lock(&idt82p33->reg_lock);

	err = _idt82p33_adjfine(channel, scaled_ppm);

	if (err)

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

	mutex_unlock(&idt82p33->reg_lock);



	return err;
@@ -706,10 +762,15 @@ static int idt82p33_adjtime(struct ptp_clock_info *ptp, s64 delta_ns) 


	if (err) {

		mutex_unlock(&idt82p33->reg_lock);

		dev_err(&idt82p33->client->dev,

			"Adjtime failed in %s with err %d!\n", __func__, err);

		return err;

	}



	err = idt82p33_sync_tod(channel, true);

	if (err)

		dev_err(&idt82p33->client->dev,

			"Sync_tod failed in %s with err %d!\n", __func__, err);



	mutex_unlock(&idt82p33->reg_lock);
@@ -725,6 +786,9 @@ static int idt82p33_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) 


	mutex_lock(&idt82p33->reg_lock);

	err = _idt82p33_gettime(channel, ts);

	if (err)

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

	mutex_unlock(&idt82p33->reg_lock);



	return err;
@@ -740,6 +804,9 @@ static int idt82p33_settime(struct ptp_clock_info *ptp, 


	mutex_lock(&idt82p33->reg_lock);

	err = _idt82p33_settime(channel, ts);

	if (err)

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

	mutex_unlock(&idt82p33->reg_lock);



	return err;
@@ -772,9 +839,6 @@ static int idt82p33_channel_init(struct idt82p33_channel *channel, int index) 
		return -EINVAL;

	}



	INIT_DELAYED_WORK(&channel->sync_tod_work,

			  idt82p33_sync_tod_work_handler);

	channel->sync_tod_on = false;

	channel->current_freq_ppb = 0;



	return 0;
@@ -784,11 +848,14 @@ static void idt82p33_caps_init(struct ptp_clock_info *caps) 
{

	caps->owner = THIS_MODULE;

	caps->max_adj = 92000;

	caps->n_per_out = 11;

	caps->adjphase = idt82p33_adjwritephase;

	caps->adjfine = idt82p33_adjfine;

	caps->adjtime = idt82p33_adjtime;

	caps->gettime64 = idt82p33_gettime;

	caps->settime64 = idt82p33_settime;

	caps->enable = idt82p33_enable;

	caps->do_aux_work = idt82p33_sync_tod_work_handler;

}



static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index)
@@ -802,23 +869,18 @@ static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index) 
	channel = &idt82p33->channel[index];



	err = idt82p33_channel_init(channel, index);

	if (err)

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Channel_init failed in %s with err %d!\n",

			__func__, err);

		return err;

	}



	channel->idt82p33 = idt82p33;



	idt82p33_caps_init(&channel->caps);

	snprintf(channel->caps.name, sizeof(channel->caps.name),

		 "IDT 82P33 PLL%u", index);

	channel->caps.n_per_out = hweight8(channel->output_mask);



	err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);

	if (err)

		return err;



	err = idt82p33_enable_tod(channel);

	if (err)

		return err;



	channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
@@ -831,6 +893,22 @@ static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index) 
	if (!channel->ptp_clock)

		return -ENOTSUPP;



	err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Dpll_set_mode failed in %s with err %d!\n",

			__func__, err);

		return err;

	}



	err = idt82p33_enable_tod(channel);

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Enable_tod failed in %s with err %d!\n",

			__func__, err);

		return err;

	}



	dev_info(&idt82p33->client->dev, "PLL%d registered as ptp%d\n",

		 index, channel->ptp_clock->index);
@@ -850,8 +928,11 @@ static int idt82p33_load_firmware(struct idt82p33 *idt82p33) 


	err = request_firmware(&fw, FW_FILENAME, &idt82p33->client->dev);



	if (err)

	if (err) {

		dev_err(&idt82p33->client->dev,

			"Failed in %s with err %d!\n", __func__, err);

		return err;

	}



	dev_dbg(&idt82p33->client->dev, "firmware size %zu bytes\n", fw->size);
@@ -935,10 +1016,14 @@ static int idt82p33_probe(struct i2c_client *client, 
		for (i = 0; i < MAX_PHC_PLL; i++) {

			if (idt82p33->pll_mask & (1 << i)) {

				err = idt82p33_enable_channel(idt82p33, i);

				if (err)

				if (err) {

					dev_err(&idt82p33->client->dev,

						"Failed in %s with err %d!\n",

						__func__, err);

					break;

				}

			}

		}

	} else {

		dev_err(&idt82p33->client->dev,

			"no PLLs flagged as PHCs, nothing to do\n");

drivers/ptp/ptp_idt82p33.h

+2 −0
Original line number Diff line number Diff line
@@ -56,6 +56,8 @@ 
#define PLL_MODE_SHIFT                    (0)

#define PLL_MODE_MASK                     (0x1F)



#define PEROUT_ENABLE_OUTPUT_MASK         (0xdeadbeef)



enum pll_mode {

	PLL_MODE_MIN = 0,

	PLL_MODE_AUTOMATIC = PLL_MODE_MIN,