ptp: idt82p33: add adjphase support
Add idt82p33_adjphase() to support PHC write phase mode. Signed-off-by: Min Li <min.li.xe@renesas.com> Acked-by: Richard Cochran <richardcochran@gmail.com> Link: https://lore.kernel.org/r/1604634729-24960-1-git-send-email-min.li.xe@renesas.com Signed-off-by: Jakub Kicinski <kuba@kernel.org>
This commit is contained in:
Min Li
Jakub Kicinski
parent
419a38cecf
commit
e014ae3949
@ -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,
|
||||
sync_tod_timeout * HZ);
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
/* Turn it off after sync_tod_timeout seconds */
|
||||
if (enable && sync_tod_timeout)
|
||||
ptp_schedule_worker(channel->ptp_clock,
|
||||
sync_tod_timeout * HZ);
|
||||
|
||||
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,
|
||||
&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;
|
||||
return idt82p33_write(idt82p33, channel->dpll_sync_cnfg,
|
||||
&sync_cnfg, sizeof(sync_cnfg));
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
|
||||
if (err)
|
||||
return err;
|
||||
if (enable)
|
||||
val &= ~SQUELCH_ENABLE;
|
||||
else
|
||||
val |= SQUELCH_ENABLE;
|
||||
|
||||
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_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));
|
||||
|
||||
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,8 +1016,12 @@ 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 {
|
||||
|
||||
@ -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,
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user