static int ksz_ptp_enable_mode(struct ksz_device *dev)
{
struct ksz_tagger_data *tagger_data = ksz_tagger_data(dev->ds);
+ struct ksz_ptp_data *ptp_data = &dev->ptp_data;
struct ksz_port *prt;
struct dsa_port *dp;
bool tag_en = false;
+ int ret;
dsa_switch_for_each_user_port(dp, dev->ds) {
prt = &dev->ports[dp->index];
}
}
+ if (tag_en) {
+ ret = ptp_schedule_worker(ptp_data->clock, 0);
+ if (ret)
+ return ret;
+ } else {
+ ptp_cancel_worker_sync(ptp_data->clock);
+ }
+
tagger_data->hwtstamp_set_state(dev->ds, tag_en);
return ksz_rmw16(dev, REG_PTP_MSG_CONF1, PTP_ENABLE,
goto unlock;
ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_LOAD_TIME, PTP_LOAD_TIME);
+ if (ret)
+ goto unlock;
+
+ spin_lock_bh(&ptp_data->clock_lock);
+ ptp_data->clock_time = *ts;
+ spin_unlock_bh(&ptp_data->clock_lock);
unlock:
mutex_unlock(&ptp_data->lock);
{
struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
+ struct timespec64 delta64 = ns_to_timespec64(delta);
s32 sec, nsec;
u16 data16;
int ret;
data16 |= PTP_STEP_DIR;
ret = ksz_write16(dev, REG_PTP_CLK_CTRL, data16);
+ if (ret)
+ goto unlock;
+
+ spin_lock_bh(&ptp_data->clock_lock);
+ ptp_data->clock_time = timespec64_add(ptp_data->clock_time, delta64);
+ spin_unlock_bh(&ptp_data->clock_lock);
unlock:
mutex_unlock(&ptp_data->lock);
return ret;
}
+/* Function is pointer to the do_aux_work in the ptp_clock capability */
+static long ksz_ptp_do_aux_work(struct ptp_clock_info *ptp)
+{
+ struct ksz_ptp_data *ptp_data = ptp_caps_to_data(ptp);
+ struct ksz_device *dev = ptp_data_to_ksz_dev(ptp_data);
+ struct timespec64 ts;
+ int ret;
+
+ mutex_lock(&ptp_data->lock);
+ ret = _ksz_ptp_gettime(dev, &ts);
+ if (ret)
+ goto out;
+
+ spin_lock_bh(&ptp_data->clock_lock);
+ ptp_data->clock_time = ts;
+ spin_unlock_bh(&ptp_data->clock_lock);
+
+out:
+ mutex_unlock(&ptp_data->lock);
+
+ return HZ; /* reschedule in 1 second */
+}
+
static int ksz_ptp_start_clock(struct ksz_device *dev)
{
- return ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE);
+ struct ksz_ptp_data *ptp_data = &dev->ptp_data;
+ int ret;
+
+ ret = ksz_rmw16(dev, REG_PTP_CLK_CTRL, PTP_CLK_ENABLE, PTP_CLK_ENABLE);
+ if (ret)
+ return ret;
+
+ ptp_data->clock_time.tv_sec = 0;
+ ptp_data->clock_time.tv_nsec = 0;
+
+ return 0;
}
int ksz_ptp_clock_register(struct dsa_switch *ds)
ptp_data = &dev->ptp_data;
mutex_init(&ptp_data->lock);
+ spin_lock_init(&ptp_data->clock_lock);
ptp_data->caps.owner = THIS_MODULE;
snprintf(ptp_data->caps.name, 16, "Microchip Clock");
ptp_data->caps.settime64 = ksz_ptp_settime;
ptp_data->caps.adjfine = ksz_ptp_adjfine;
ptp_data->caps.adjtime = ksz_ptp_adjtime;
+ ptp_data->caps.do_aux_work = ksz_ptp_do_aux_work;
ret = ksz_ptp_start_clock(dev);
if (ret)