#define SETTIME_CORRECTION (0)
+static int _idtcm_adjfine(struct idtcm_channel *channel, long scaled_ppm);
+
static int contains_full_configuration(struct idtcm *idtcm,
const struct firmware *fw)
{
}
static int _idtcm_set_dpll_hw_tod(struct idtcm_channel *channel,
- struct timespec64 const *ts,
- enum hw_tod_write_trig_sel wr_trig)
+ struct timespec64 const *ts,
+ enum hw_tod_write_trig_sel wr_trig)
{
struct idtcm *idtcm = channel->idtcm;
u8 buf[TOD_BYTE_COUNT];
return err;
}
-static int idtcm_do_phase_pull_in(struct idtcm_channel *channel,
- s32 offset_ns,
- u32 max_ffo_ppb)
+static int do_phase_pull_in_fw(struct idtcm_channel *channel,
+ s32 offset_ns,
+ u32 max_ffo_ppb)
{
int err;
s64 now;
if (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS_DEPRECATED) {
- err = idtcm_do_phase_pull_in(channel, delta, 0);
+ err = channel->do_phase_pull_in(channel, delta, 0);
} else {
idtcm->calculate_overhead_flag = 1;
if (firmware) /* module parameter */
snprintf(fname, sizeof(fname), "%s", firmware);
- dev_dbg(&idtcm->client->dev, "requesting firmware '%s'", fname);
+ dev_info(&idtcm->client->dev, "firmware '%s'", fname);
err = request_firmware(&fw, fname, dev);
if (err) {
}
static int idtcm_get_pll_mode(struct idtcm_channel *channel,
- enum pll_mode *pll_mode)
+ enum pll_mode *mode)
{
struct idtcm *idtcm = channel->idtcm;
int err;
if (err)
return err;
- *pll_mode = (dpll_mode >> PLL_MODE_SHIFT) & PLL_MODE_MASK;
+ *mode = (dpll_mode >> PLL_MODE_SHIFT) & PLL_MODE_MASK;
return 0;
}
static int idtcm_set_pll_mode(struct idtcm_channel *channel,
- enum pll_mode pll_mode)
+ enum pll_mode mode)
{
struct idtcm *idtcm = channel->idtcm;
int err;
dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
- dpll_mode |= (pll_mode << PLL_MODE_SHIFT);
-
- channel->pll_mode = pll_mode;
+ dpll_mode |= (mode << PLL_MODE_SHIFT);
err = idtcm_write(idtcm, channel->dpll_n,
IDTCM_FW_REG(idtcm->fw_ver, V520, DPLL_MODE),
&dpll_mode, sizeof(dpll_mode));
+ return err;
+}
+
+static int idtcm_get_manual_reference(struct idtcm_channel *channel,
+ enum manual_reference *ref)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ u8 dpll_manu_ref_cfg;
+ int err;
+
+ err = idtcm_read(idtcm, channel->dpll_ctrl_n,
+ DPLL_CTRL_DPLL_MANU_REF_CFG,
+ &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
+ if (err)
+ return err;
+
+ dpll_manu_ref_cfg &= (MANUAL_REFERENCE_MASK << MANUAL_REFERENCE_SHIFT);
+
+ *ref = dpll_manu_ref_cfg >> MANUAL_REFERENCE_SHIFT;
+
+ return 0;
+}
+
+static int idtcm_set_manual_reference(struct idtcm_channel *channel,
+ enum manual_reference ref)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ u8 dpll_manu_ref_cfg;
+ int err;
+
+ err = idtcm_read(idtcm, channel->dpll_ctrl_n,
+ DPLL_CTRL_DPLL_MANU_REF_CFG,
+ &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
+ if (err)
+ return err;
+
+ dpll_manu_ref_cfg &= ~(MANUAL_REFERENCE_MASK << MANUAL_REFERENCE_SHIFT);
+
+ dpll_manu_ref_cfg |= (ref << MANUAL_REFERENCE_SHIFT);
+
+ err = idtcm_write(idtcm, channel->dpll_ctrl_n,
+ DPLL_CTRL_DPLL_MANU_REF_CFG,
+ &dpll_manu_ref_cfg, sizeof(dpll_manu_ref_cfg));
+
+ return err;
+}
+
+static int configure_dpll_mode_write_frequency(struct idtcm_channel *channel)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ int err;
+
+ err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_FREQUENCY);
+
+ if (err)
+ dev_err(&idtcm->client->dev, "Failed to set pll mode to write frequency");
+ else
+ channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+
+ return err;
+}
+
+static int configure_dpll_mode_write_phase(struct idtcm_channel *channel)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ int err;
+
+ err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_PHASE);
+
+ if (err)
+ dev_err(&idtcm->client->dev, "Failed to set pll mode to write phase");
+ else
+ channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+
+ return err;
+}
+
+static int configure_manual_reference_write_frequency(struct idtcm_channel *channel)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ int err;
+
+ err = idtcm_set_manual_reference(channel, MANU_REF_WRITE_FREQUENCY);
+
+ if (err)
+ dev_err(&idtcm->client->dev, "Failed to set manual reference to write frequency");
+ else
+ channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+
+ return err;
+}
+
+static int configure_manual_reference_write_phase(struct idtcm_channel *channel)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ int err;
+
+ err = idtcm_set_manual_reference(channel, MANU_REF_WRITE_PHASE);
+
+ if (err)
+ dev_err(&idtcm->client->dev, "Failed to set manual reference to write phase");
+ else
+ channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+
+ return err;
+}
+
+static int idtcm_stop_phase_pull_in(struct idtcm_channel *channel)
+{
+ int err;
+
+ err = _idtcm_adjfine(channel, channel->current_freq_scaled_ppm);
if (err)
return err;
+ channel->phase_pull_in = false;
+
return 0;
}
+static long idtcm_work_handler(struct ptp_clock_info *ptp)
+{
+ struct idtcm_channel *channel = container_of(ptp, struct idtcm_channel, caps);
+ struct idtcm *idtcm = channel->idtcm;
+
+ mutex_lock(&idtcm->reg_lock);
+
+ (void)idtcm_stop_phase_pull_in(channel);
+
+ mutex_unlock(&idtcm->reg_lock);
+
+ /* Return a negative value here to not reschedule */
+ return -1;
+}
+
+static s32 phase_pull_in_scaled_ppm(s32 current_ppm, s32 phase_pull_in_ppb)
+{
+ /* ppb = scaled_ppm * 125 / 2^13 */
+ /* scaled_ppm = ppb * 2^13 / 125 */
+
+ s64 max_scaled_ppm = (PHASE_PULL_IN_MAX_PPB << 13) / 125;
+ s64 scaled_ppm = (phase_pull_in_ppb << 13) / 125;
+
+ current_ppm += scaled_ppm;
+
+ if (current_ppm > max_scaled_ppm)
+ current_ppm = max_scaled_ppm;
+ else if (current_ppm < -max_scaled_ppm)
+ current_ppm = -max_scaled_ppm;
+
+ return current_ppm;
+}
+
+static int do_phase_pull_in_sw(struct idtcm_channel *channel,
+ s32 delta_ns,
+ u32 max_ffo_ppb)
+{
+ s32 current_ppm = channel->current_freq_scaled_ppm;
+ u32 duration_ms = MSEC_PER_SEC;
+ s32 delta_ppm;
+ s32 ppb;
+ int err;
+
+ /* If the ToD correction is less than PHASE_PULL_IN_MIN_THRESHOLD_NS,
+ * skip. The error introduced by the ToD adjustment procedure would
+ * be bigger than the required ToD correction
+ */
+ if (abs(delta_ns) < PHASE_PULL_IN_MIN_THRESHOLD_NS)
+ return 0;
+
+ if (max_ffo_ppb == 0)
+ max_ffo_ppb = PHASE_PULL_IN_MAX_PPB;
+
+ /* For most cases, keep phase pull-in duration 1 second */
+ ppb = delta_ns;
+ while (abs(ppb) > max_ffo_ppb) {
+ duration_ms *= 2;
+ ppb /= 2;
+ }
+
+ delta_ppm = phase_pull_in_scaled_ppm(current_ppm, ppb);
+
+ err = _idtcm_adjfine(channel, delta_ppm);
+
+ if (err)
+ return err;
+
+ /* schedule the worker to cancel phase pull-in */
+ ptp_schedule_worker(channel->ptp_clock,
+ msecs_to_jiffies(duration_ms) - 1);
+
+ channel->phase_pull_in = true;
+
+ return 0;
+}
+
+static int initialize_operating_mode_with_manual_reference(struct idtcm_channel *channel,
+ enum manual_reference ref)
+{
+ struct idtcm *idtcm = channel->idtcm;
+
+ channel->mode = PTP_PLL_MODE_UNSUPPORTED;
+ channel->configure_write_frequency = configure_manual_reference_write_frequency;
+ channel->configure_write_phase = configure_manual_reference_write_phase;
+ channel->do_phase_pull_in = do_phase_pull_in_sw;
+
+ switch (ref) {
+ case MANU_REF_WRITE_PHASE:
+ channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+ break;
+ case MANU_REF_WRITE_FREQUENCY:
+ channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+ break;
+ default:
+ dev_warn(&idtcm->client->dev,
+ "Unsupported MANUAL_REFERENCE: 0x%02x", ref);
+ }
+
+ return 0;
+}
+
+static int initialize_operating_mode_with_pll_mode(struct idtcm_channel *channel,
+ enum pll_mode mode)
+{
+ struct idtcm *idtcm = channel->idtcm;
+ int err = 0;
+
+ channel->mode = PTP_PLL_MODE_UNSUPPORTED;
+ channel->configure_write_frequency = configure_dpll_mode_write_frequency;
+ channel->configure_write_phase = configure_dpll_mode_write_phase;
+ channel->do_phase_pull_in = do_phase_pull_in_fw;
+
+ switch (mode) {
+ case PLL_MODE_WRITE_PHASE:
+ channel->mode = PTP_PLL_MODE_WRITE_PHASE;
+ break;
+ case PLL_MODE_WRITE_FREQUENCY:
+ channel->mode = PTP_PLL_MODE_WRITE_FREQUENCY;
+ break;
+ default:
+ dev_err(&idtcm->client->dev,
+ "Unsupported PLL_MODE: 0x%02x", mode);
+ err = -EINVAL;
+ }
+
+ return err;
+}
+
+static int initialize_dco_operating_mode(struct idtcm_channel *channel)
+{
+ enum manual_reference ref = MANU_REF_XO_DPLL;
+ enum pll_mode mode = PLL_MODE_DISABLED;
+ struct idtcm *idtcm = channel->idtcm;
+ int err;
+
+ channel->mode = PTP_PLL_MODE_UNSUPPORTED;
+
+ err = idtcm_get_pll_mode(channel, &mode);
+ if (err) {
+ dev_err(&idtcm->client->dev, "Unable to read pll mode!");
+ return err;
+ }
+
+ if (mode == PLL_MODE_PLL) {
+ err = idtcm_get_manual_reference(channel, &ref);
+ if (err) {
+ dev_err(&idtcm->client->dev, "Unable to read manual reference!");
+ return err;
+ }
+ err = initialize_operating_mode_with_manual_reference(channel, ref);
+ } else {
+ err = initialize_operating_mode_with_pll_mode(channel, mode);
+ }
+
+ if (channel->mode == PTP_PLL_MODE_WRITE_PHASE)
+ channel->configure_write_frequency(channel);
+
+ return err;
+}
+
/* PTP Hardware Clock interface */
/**
- * @brief Maximum absolute value for write phase offset in picoseconds
+ * Maximum absolute value for write phase offset in picoseconds
+ *
+ * @channel: channel
+ * @delta_ns: delta in nanoseconds
*
* Destination signed register is 32-bit register in resolution of 50ps
*
s32 phase_50ps;
s64 offset_ps;
- if (channel->pll_mode != PLL_MODE_WRITE_PHASE) {
- err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_PHASE);
+ if (channel->mode != PTP_PLL_MODE_WRITE_PHASE) {
+ err = channel->configure_write_phase(channel);
if (err)
return err;
}
u8 buf[6] = {0};
s64 fcw;
- if (channel->pll_mode != PLL_MODE_WRITE_FREQUENCY) {
- err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_FREQUENCY);
+ if (channel->mode != PTP_PLL_MODE_WRITE_FREQUENCY) {
+ err = channel->configure_write_frequency(channel);
if (err)
return err;
}
enum scsr_tod_write_type_sel type;
int err;
+ if (channel->phase_pull_in == true)
+ return 0;
+
+ mutex_lock(&idtcm->reg_lock);
+
if (abs(delta) < PHASE_PULL_IN_THRESHOLD_NS) {
- err = idtcm_do_phase_pull_in(channel, delta, 0);
+ err = channel->do_phase_pull_in(channel, delta, 0);
if (err)
dev_err(&idtcm->client->dev,
"Failed at line %d in %s!", __LINE__, __func__);
- return err;
- }
-
- if (delta >= 0) {
- ts = ns_to_timespec64(delta);
- type = SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS;
} else {
- ts = ns_to_timespec64(-delta);
- type = SCSR_TOD_WR_TYPE_SEL_DELTA_MINUS;
+ if (delta >= 0) {
+ ts = ns_to_timespec64(delta);
+ type = SCSR_TOD_WR_TYPE_SEL_DELTA_PLUS;
+ } else {
+ ts = ns_to_timespec64(-delta);
+ type = SCSR_TOD_WR_TYPE_SEL_DELTA_MINUS;
+ }
+ err = _idtcm_settime(channel, &ts, type);
+ if (err)
+ dev_err(&idtcm->client->dev,
+ "Failed at line %d in %s!", __LINE__, __func__);
}
-
- mutex_lock(&idtcm->reg_lock);
-
- err = _idtcm_settime(channel, &ts, type);
- if (err)
- dev_err(&idtcm->client->dev,
- "Failed at line %d in %s!", __LINE__, __func__);
-
mutex_unlock(&idtcm->reg_lock);
return err;
struct idtcm *idtcm = channel->idtcm;
int err;
+ if (channel->phase_pull_in == true)
+ return 0;
+
+ if (scaled_ppm == channel->current_freq_scaled_ppm)
+ return 0;
+
mutex_lock(&idtcm->reg_lock);
err = _idtcm_adjfine(channel, scaled_ppm);
- if (err)
- dev_err(&idtcm->client->dev,
- "Failed at line %d in %s!", __LINE__, __func__);
mutex_unlock(&idtcm->reg_lock);
+ if (!err)
+ channel->current_freq_scaled_ppm = scaled_ppm;
+
return err;
}
.gettime64 = &idtcm_gettime,
.settime64 = &idtcm_settime,
.enable = &idtcm_enable,
+ .do_aux_work = &idtcm_work_handler,
};
static const struct ptp_clock_info idtcm_caps_deprecated = {
.gettime64 = &idtcm_gettime,
.settime64 = &idtcm_settime_deprecated,
.enable = &idtcm_enable,
+ .do_aux_work = &idtcm_work_handler,
};
static int configure_channel_pll(struct idtcm_channel *channel)
return -EINVAL;
channel = &idtcm->channel[index];
+
channel->idtcm = idtcm;
+ channel->current_freq_scaled_ppm = 0;
/* Set pll addresses */
err = configure_channel_pll(channel);
snprintf(channel->caps.name, sizeof(channel->caps.name),
"IDT CM TOD%u", index);
- /* Sync pll mode with hardware */
- err = idtcm_get_pll_mode(channel, &channel->pll_mode);
- if (err) {
- dev_err(&idtcm->client->dev,
- "Error: %s - Unable to read pll mode", __func__);
+ err = initialize_dco_operating_mode(channel);
+ if (err)
return err;
- }
err = idtcm_enable_tod(channel);
if (err) {
for (i = 0; i < MAX_TOD; i++) {
channel = &idtcm->channel[i];
-
if (channel->ptp_clock)
ptp_clock_unregister(channel->ptp_clock);
}
projects / linux-2.6-microblaze.git / blobdiff