#include <linux/cleanup.h>
#include <linux/delay.h>
#include <linux/dev_printk.h>
#include <linux/ptp_clock_kernel.h>
#include <linux/string.h>
#include <linux/types.h>
#include "chan.h"
#include "core.h"
int zl3073x_chan_state_update(struct zl3073x_dev *zldev, u8 index)
{
struct zl3073x_chan *chan = &zldev->chan[index];
u64 val;
int rc;
rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_MON_STATUS(index),
&chan->mon_status);
if (rc)
return rc;
rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_REFSEL_STATUS(index),
&chan->refsel_status);
if (rc)
return rc;
if (!zl3073x_chan_is_locked(chan)) {
if (!zl3073x_chan_mode_is_nco(chan))
chan->df_offset = ZL_DPLL_DF_OFFSET_UNKNOWN;
return 0;
}
rc = zl3073x_poll_zero_u8(zldev, ZL_REG_DPLL_DF_READ(index),
ZL_DPLL_DF_READ_SEM,
ZL_POLL_DF_READ_TIMEOUT_US);
if (rc)
return rc;
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_DF_READ(index),
ZL_DPLL_DF_READ_SEM | ZL_DPLL_DF_READ_REF_OFST);
if (rc)
return rc;
rc = zl3073x_poll_zero_u8(zldev, ZL_REG_DPLL_DF_READ(index),
ZL_DPLL_DF_READ_SEM,
ZL_POLL_DF_READ_TIMEOUT_US);
if (rc)
return rc;
rc = zl3073x_read_u48(zldev, ZL_REG_DPLL_DF_OFFSET(index), &val);
if (rc)
return rc;
chan->df_offset = sign_extend64(val, 47);
return 0;
}
int zl3073x_chan_nco_mode_set(struct zl3073x_dev *zldev, u8 index)
{
struct zl3073x_chan *chan = &zldev->chan[index];
u8 prev_mode, df_read;
u64 val;
int rc;
prev_mode = zl3073x_chan_mode_get(chan);
if (prev_mode == ZL_DPLL_MODE_REFSEL_MODE_FREERUN) {
zl3073x_chan_mode_set(chan, ZL_DPLL_MODE_REFSEL_MODE_NCO);
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_MODE_REFSEL(index),
chan->mode_refsel);
if (rc) {
zl3073x_chan_mode_set(chan, prev_mode);
return rc;
}
chan->df_offset = ZL_DPLL_DF_OFFSET_UNKNOWN;
return 0;
}
df_read = FIELD_PREP(ZL_DPLL_DF_READ_REF_OFST, 0) |
FIELD_PREP(ZL_DPLL_DF_READ_CMD, ZL_DPLL_DF_READ_CMD_ACC_I);
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_DF_READ(index), df_read);
if (rc)
return rc;
fsleep(25000);
zl3073x_chan_mode_set(chan, ZL_DPLL_MODE_REFSEL_MODE_NCO);
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_MODE_REFSEL(index),
chan->mode_refsel);
if (rc) {
zl3073x_chan_mode_set(chan, prev_mode);
return rc;
}
fsleep(25000);
rc = zl3073x_read_u48(zldev, ZL_REG_DPLL_DF_OFFSET(index), &val);
if (rc) {
dev_warn(zldev->dev,
"Failed to read DPLL%u df_offset: %pe\n",
index, ERR_PTR(rc));
chan->df_offset = ZL_DPLL_DF_OFFSET_UNKNOWN;
} else {
chan->df_offset = sign_extend64(val, 47);
}
return 0;
}
int zl3073x_chan_state_fetch(struct zl3073x_dev *zldev, u8 index)
{
struct zl3073x_chan *chan = &zldev->chan[index];
int rc, i;
rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_CTRL(index), &chan->ctrl);
if (rc)
return rc;
rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_MODE_REFSEL(index),
&chan->mode_refsel);
if (rc)
return rc;
dev_dbg(zldev->dev, "DPLL%u mode: %u, ref: %u\n", index,
zl3073x_chan_mode_get(chan), zl3073x_chan_ref_get(chan));
rc = zl3073x_chan_state_update(zldev, index);
if (rc)
return rc;
if (zl3073x_chan_mode_is_nco(chan))
chan->df_offset = ZL_DPLL_DF_OFFSET_UNKNOWN;
dev_dbg(zldev->dev,
"DPLL%u lock_state: %u, ho: %u, sel_state: %u, sel_ref: %u\n",
index, zl3073x_chan_lock_state_get(chan),
zl3073x_chan_is_ho_ready(chan) ? 1 : 0,
zl3073x_chan_refsel_state_get(chan),
zl3073x_chan_refsel_ref_get(chan));
guard(mutex)(&zldev->multiop_lock);
rc = zl3073x_mb_op(zldev, ZL_REG_DPLL_MB_SEM, ZL_DPLL_MB_SEM_RD,
ZL_REG_DPLL_MB_MASK, BIT(index));
if (rc)
return rc;
for (i = 0; i < ARRAY_SIZE(chan->ref_prio); i++) {
rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_REF_PRIO(i),
&chan->ref_prio[i]);
if (rc)
return rc;
}
return 0;
}
const struct zl3073x_chan *zl3073x_chan_state_get(struct zl3073x_dev *zldev,
u8 index)
{
return &zldev->chan[index];
}
int zl3073x_chan_tod_ready_wait(struct zl3073x_dev *zldev, u8 ch)
{
unsigned int timeout;
u8 tod_ctrl;
int rc;
rc = zl3073x_read_u8(zldev, ZL_REG_DPLL_TOD_CTRL(ch), &tod_ctrl);
if (rc)
return rc;
if (!(tod_ctrl & ZL_DPLL_TOD_CTRL_SEM))
return 0;
switch (FIELD_GET(ZL_DPLL_TOD_CTRL_CMD, tod_ctrl)) {
case ZL_DPLL_TOD_CTRL_CMD_WR_NEXT_1HZ:
timeout = ZL_POLL_TOD_WR_TIMEOUT_US;
break;
case ZL_DPLL_TOD_CTRL_CMD_RD_CURRENT:
case ZL_DPLL_TOD_CTRL_CMD_RD_NEXT_1HZ:
timeout = ZL_POLL_TOD_RD_TIMEOUT_US;
break;
default:
return -EOPNOTSUPP;
}
rc = zl3073x_poll_zero_u8(zldev, ZL_REG_DPLL_TOD_CTRL(ch),
ZL_DPLL_TOD_CTRL_SEM, timeout);
return rc == -ETIMEDOUT ? -EBUSY : rc;
}
static int zl3073x_chan_tod_ctrl(struct zl3073x_dev *zldev, u8 ch, u8 cmd)
{
return zl3073x_write_u8(zldev, ZL_REG_DPLL_TOD_CTRL(ch),
ZL_DPLL_TOD_CTRL_SEM | cmd);
}
int zl3073x_chan_tod_read(struct zl3073x_dev *zldev, u8 ch,
bool next_hz, struct timespec64 *ts,
struct ptp_system_timestamp *sts)
{
u32 nsec;
u64 sec;
u8 cmd;
int rc;
if (next_hz)
cmd = ZL_DPLL_TOD_CTRL_CMD_RD_NEXT_1HZ;
else
cmd = ZL_DPLL_TOD_CTRL_CMD_RD_CURRENT;
rc = zl3073x_chan_tod_ready_wait(zldev, ch);
if (rc)
return rc;
ptp_read_system_prets(sts);
rc = zl3073x_chan_tod_ctrl(zldev, ch, cmd);
if (rc)
return rc;
rc = zl3073x_chan_tod_ready_wait(zldev, ch);
if (rc)
return rc;
ptp_read_system_postts(sts);
rc = zl3073x_read_u48(zldev, ZL_REG_DPLL_TOD_SEC(ch), &sec);
if (rc)
return rc;
rc = zl3073x_read_u32(zldev, ZL_REG_DPLL_TOD_NS(ch), &nsec);
if (rc)
return rc;
ts->tv_sec = sec;
ts->tv_nsec = nsec;
return 0;
}
int zl3073x_chan_tod_write(struct zl3073x_dev *zldev, u8 ch,
struct timespec64 ts)
{
int rc;
rc = zl3073x_chan_tod_ready_wait(zldev, ch);
if (rc)
return rc;
rc = zl3073x_write_u48(zldev, ZL_REG_DPLL_TOD_SEC(ch), ts.tv_sec);
if (rc)
return rc;
rc = zl3073x_write_u32(zldev, ZL_REG_DPLL_TOD_NS(ch), ts.tv_nsec);
if (rc)
return rc;
return zl3073x_chan_tod_ctrl(zldev, ch,
ZL_DPLL_TOD_CTRL_CMD_WR_NEXT_1HZ);
}
int zl3073x_chan_tod_adjust(struct zl3073x_dev *zldev, u8 ch,
struct timespec64 delta)
{
#define ZL_TOD_MAX_RETRIES 20
static const long threshold_ns = 20 * NSEC_PER_MSEC;
struct timespec64 ts_next, ts_cur, diff;
int rc, i;
for (i = 0; i < ZL_TOD_MAX_RETRIES; i++) {
rc = zl3073x_chan_tod_read(zldev, ch, true, &ts_next, NULL);
if (rc)
return rc;
rc = zl3073x_chan_tod_read(zldev, ch, false, &ts_cur, NULL);
if (rc)
return rc;
diff = timespec64_sub(ts_next, ts_cur);
if (diff.tv_sec > 0 ||
(!diff.tv_sec && diff.tv_nsec >= threshold_ns))
break;
}
if (i == ZL_TOD_MAX_RETRIES) {
dev_warn(zldev->dev,
"DPLL%u ToD adjust failed to get stable margin\n",
ch);
return -EBUSY;
}
ts_next = timespec64_add(ts_next, delta);
if (!timespec64_valid_settod(&ts_next))
return -EINVAL;
return zl3073x_chan_tod_write(zldev, ch, ts_next);
#undef ZL_TOD_MAX_RETRIES
}
int zl3073x_chan_df_offset_set(struct zl3073x_dev *zldev, u8 ch, s64 offset)
{
int rc;
rc = zl3073x_write_u48(zldev, ZL_REG_DPLL_DF_OFFSET(ch), offset);
if (!rc)
zldev->chan[ch].df_offset = offset;
return rc;
}
int zl3073x_chan_tie_write(struct zl3073x_dev *zldev, u8 ch, s64 delta_ns)
{
s64 tie_data;
int rc;
guard(mutex)(&zldev->tie_lock);
rc = zl3073x_poll_zero_u8(zldev, ZL_REG_DPLL_TIE_CTRL,
ZL_DPLL_TIE_CTRL_OP,
ZL_POLL_TIE_WR_TIMEOUT_US);
if (rc)
return rc;
tie_data = delta_ns * 100000LL;
rc = zl3073x_write_u48(zldev, ZL_REG_DPLL_TIE_DATA(ch), tie_data);
if (rc)
return rc;
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_TIE_CTRL_MASK, BIT(ch));
if (rc)
return rc;
return zl3073x_write_u8(zldev, ZL_REG_DPLL_TIE_CTRL,
ZL_DPLL_TIE_CTRL_OP_WR);
}
int zl3073x_chan_phase_step(struct zl3073x_dev *zldev, u8 ch,
u16 out_mask, s32 step_cycles,
bool tod_step)
{
u8 ctrl;
int rc;
guard(mutex)(&zldev->phase_step_lock);
rc = zl3073x_poll_zero_u8(zldev, ZL_REG_OUTPUT_PHASE_STEP_CTRL,
ZL_OUTPUT_PHASE_STEP_CTRL_OP,
ZL_POLL_PHASE_STEP_TIMEOUT_US);
if (rc)
return rc;
rc = zl3073x_write_u32(zldev, ZL_REG_OUTPUT_PHASE_STEP_DATA,
step_cycles);
if (rc)
return rc;
rc = zl3073x_write_u16(zldev, ZL_REG_OUTPUT_PHASE_STEP_MASK, out_mask);
if (rc)
return rc;
rc = zl3073x_write_u8(zldev, ZL_REG_OUTPUT_PHASE_STEP_NUMBER, 1);
if (rc)
return rc;
ctrl = FIELD_PREP(ZL_OUTPUT_PHASE_STEP_CTRL_DPLL, ch) |
FIELD_PREP(ZL_OUTPUT_PHASE_STEP_CTRL_OP,
ZL_OUTPUT_PHASE_STEP_CTRL_OP_WRITE);
if (tod_step)
ctrl |= ZL_OUTPUT_PHASE_STEP_CTRL_TOD_STEP;
return zl3073x_write_u8(zldev, ZL_REG_OUTPUT_PHASE_STEP_CTRL, ctrl);
}
int zl3073x_chan_state_set(struct zl3073x_dev *zldev, u8 index,
const struct zl3073x_chan *chan)
{
struct zl3073x_chan *dchan = &zldev->chan[index];
int rc, i;
if (!memcmp(&dchan->cfg, &chan->cfg, sizeof(chan->cfg)))
return 0;
if (dchan->ctrl != chan->ctrl) {
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_CTRL(index),
chan->ctrl);
if (rc)
return rc;
dchan->ctrl = chan->ctrl;
}
if (dchan->mode_refsel != chan->mode_refsel) {
rc = zl3073x_write_u8(zldev, ZL_REG_DPLL_MODE_REFSEL(index),
chan->mode_refsel);
if (rc)
return rc;
dchan->mode_refsel = chan->mode_refsel;
}
if (!memcmp(dchan->ref_prio, chan->ref_prio, sizeof(chan->ref_prio))) {
dchan->cfg = chan->cfg;
return 0;
}
guard(mutex)(&zldev->multiop_lock);
rc = zl3073x_mb_op(zldev, ZL_REG_DPLL_MB_SEM, ZL_DPLL_MB_SEM_RD,
ZL_REG_DPLL_MB_MASK, BIT(index));
if (rc)
return rc;
for (i = 0; i < ARRAY_SIZE(chan->ref_prio); i++) {
if (dchan->ref_prio[i] != chan->ref_prio[i]) {
rc = zl3073x_write_u8(zldev,
ZL_REG_DPLL_REF_PRIO(i),
chan->ref_prio[i]);
if (rc)
return rc;
}
}
rc = zl3073x_mb_op(zldev, ZL_REG_DPLL_MB_SEM, ZL_DPLL_MB_SEM_WR,
ZL_REG_DPLL_MB_MASK, BIT(index));
if (rc)
return rc;
dchan->cfg = chan->cfg;
return 0;
}