From: Vincent Cheng
Add idtcm_adjphase() to support PHC write phase mode.
Signed-off-by: Vincent Cheng
---
drivers/ptp/ptp_clockmatrix.c | 123 ++
drivers/ptp/ptp_clockmatrix.h | 11 +++-
2 files changed, 132 insertions(+), 2 deletions(-)
diff --git a/drivers/ptp/ptp_clockmatrix.c b/drivers/ptp/ptp_clockmatrix.c
index a3f6088..07b13c3 100644
--- a/drivers/ptp/ptp_clockmatrix.c
+++ b/drivers/ptp/ptp_clockmatrix.c
@@ -24,6 +24,15 @@ MODULE_LICENSE("GPL");
#define SETTIME_CORRECTION (0)
+static void set_write_phase_ready(struct kthread_work *work)
+{
+ struct idtcm_channel *ch = container_of(work,
+ struct idtcm_channel,
+ write_phase_ready_work.work);
+
+ ch->write_phase_ready = 1;
+}
+
static int char_array_to_timespec(u8 *buf,
u8 count,
struct timespec64 *ts)
@@ -871,6 +880,69 @@ static int idtcm_set_pll_mode(struct idtcm_channel
*channel,
/* PTP Hardware Clock interface */
+/**
+ * @brief Maximum absolute value for write phase offset in picoseconds
+ *
+ * Destination signed register is 32-bit register in resolution of 50ps
+ *
+ * 0x7fff * 50 = 2147483647 * 50 = 107374182350
+ */
+static int _idtcm_adjphase(struct idtcm_channel *channel, s32 deltaNs)
+{
+ struct idtcm *idtcm = channel->idtcm;
+
+ int err;
+ u8 i;
+ u8 buf[4] = {0};
+ s32 phaseIn50Picoseconds;
+ s64 phaseOffsetInPs;
+
+ if (channel->pll_mode != PLL_MODE_WRITE_PHASE) {
+
+ kthread_cancel_delayed_work_sync(
+ >write_phase_ready_work);
+
+ err = idtcm_set_pll_mode(channel, PLL_MODE_WRITE_PHASE);
+
+ if (err)
+ return err;
+
+ channel->write_phase_ready = 0;
+
+ kthread_queue_delayed_work(channel->kworker,
+ >write_phase_ready_work,
+ msecs_to_jiffies(WR_PHASE_SETUP_MS));
+ }
+
+ if (!channel->write_phase_ready)
+ deltaNs = 0;
+
+ phaseOffsetInPs = (s64)deltaNs * 1000;
+
+ /*
+* Check for 32-bit signed max * 50:
+*
+* 0x7fff * 50 = 2147483647 * 50 = 107374182350
+*/
+ if (phaseOffsetInPs > MAX_ABS_WRITE_PHASE_PICOSECONDS)
+ phaseOffsetInPs = MAX_ABS_WRITE_PHASE_PICOSECONDS;
+ else if (phaseOffsetInPs < -MAX_ABS_WRITE_PHASE_PICOSECONDS)
+ phaseOffsetInPs = -MAX_ABS_WRITE_PHASE_PICOSECONDS;
+
+ phaseIn50Picoseconds = DIV_ROUND_CLOSEST(div64_s64(phaseOffsetInPs, 50),
+1);
+
+ for (i = 0; i < 4; i++) {
+ buf[i] = phaseIn50Picoseconds & 0xff;
+ phaseIn50Picoseconds >>= 8;
+ }
+
+ err = idtcm_write(idtcm, channel->dpll_phase, DPLL_WR_PHASE,
+ buf, sizeof(buf));
+
+ return err;
+}
+
static int idtcm_adjfreq(struct ptp_clock_info *ptp, s32 ppb)
{
struct idtcm_channel *channel =
@@ -977,6 +1049,24 @@ static int idtcm_adjtime(struct ptp_clock_info *ptp, s64
delta)
return err;
}
+static int idtcm_adjphase(struct ptp_clock_info *ptp, s32 delta)
+{
+ struct idtcm_channel *channel =
+ container_of(ptp, struct idtcm_channel, caps);
+
+ struct idtcm *idtcm = channel->idtcm;
+
+ int err;
+
+ mutex_lock(>reg_lock);
+
+ err = _idtcm_adjphase(channel, delta);
+
+ mutex_unlock(>reg_lock);
+
+ return err;
+}
+
static int idtcm_enable(struct ptp_clock_info *ptp,
struct ptp_clock_request *rq, int on)
{
@@ -1055,6 +1145,7 @@ static const struct ptp_clock_info idtcm_caps = {
.owner = THIS_MODULE,
.max_adj= 244000,
.n_per_out = 1,
+ .adjphase = _adjphase,
.adjfreq= _adjfreq,
.adjtime= _adjtime,
.gettime64 = _gettime,
@@ -1062,6 +1153,21 @@ static const struct ptp_clock_info idtcm_caps = {
.enable = _enable,
};
+static int write_phase_worker_setup(struct idtcm_channel *channel, int index)
+{
+ channel->kworker = kthread_create_worker(0, "channel%d", index);
+
+ if (IS_ERR(channel->kworker))
+ return PTR_ERR(channel->kworker);
+
+ channel->write_phase_ready = 0;
+
+ kthread_init_delayed_work(>write_phase_ready_work,
+ set_write_phase_ready);
+
+ return 0;
+}
+
static int idtcm_enable_channel(struct idtcm *idtcm, u32 index)
{
struct idtcm_channel *channel;
@@ -1146,6 +1252,10 @@ static int idtcm_enable_channel(struct idtcm *idtcm, u32
index)
if (!channel->ptp_clock)
return -ENOTSUPP;
+ err = write_phase_worker_setup(channel,
