drivers/ptp/ptp_fc3.c

38 static s64 ns2counters(struct idtfc3 *idtfc3, s64 nsec, u32 *sub_ns)
44 		sync = div_u64_rem(nsec, idtfc3->ns_per_sync, &rem);
47 		sync = -div_u64_rem(-nsec - 1, idtfc3->ns_per_sync, &rem) - 1;
48 		*sub_ns = idtfc3->ns_per_sync - rem - 1;
51 	return sync * idtfc3->ns_per_sync;
54 static s64 tdc_meas2offset(struct idtfc3 *idtfc3, u64 meas_read)
61 	fine = div64_s64(fine * NSEC_PER_SEC, idtfc3->tdc_apll_freq * 62LL);
62 	coarse = div64_s64(coarse * NSEC_PER_SEC, idtfc3->time_ref_freq);
67 static s64 tdc_offset2phase(struct idtfc3 *idtfc3, s64 offset_ns)
69 	if (offset_ns > idtfc3->ns_per_sync / 2)
70 		offset_ns -= idtfc3->ns_per_sync;
72 	return offset_ns * idtfc3->tdc_offset_sign;
75 static int idtfc3_set_lpf_mode(struct idtfc3 *idtfc3, u8 mode)
82 	if (idtfc3->lpf_mode == mode)
85 	err = regmap_bulk_write(idtfc3->regmap, LPF_MODE_CNFG, &mode, sizeof(mode));
89 	idtfc3->lpf_mode = mode;
94 static int idtfc3_enable_lpf(struct idtfc3 *idtfc3, bool enable)
99 	err = regmap_bulk_read(idtfc3->regmap, LPF_CTRL, &val, sizeof(val));
108 	return regmap_bulk_write(idtfc3->regmap, LPF_CTRL, &val, sizeof(val));
111 static int idtfc3_get_time_ref_freq(struct idtfc3 *idtfc3)
118 	err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_MEAS_DIV_CNFG, buf, sizeof(buf));
123 	err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_COUNT, buf, 1);
127 	idtfc3->time_ref_freq = idtfc3->hw_param.time_clk_freq *
133 static int idtfc3_get_tdc_offset_sign(struct idtfc3 *idtfc3)
140 	err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_TDC_FANOUT_CNFG, buf, sizeof(buf));
146 		dev_err(idtfc3->dev, "TIME_SYNC_TO_TDC_EN is off !!!");
154 		dev_err(idtfc3->dev, "Invalid tdc_mux_sel sig1=%d sig2=%d", sig1, sig2);
157 		idtfc3->tdc_offset_sign = 1;
159 		idtfc3->tdc_offset_sign = -1;
165 static int idtfc3_lpf_bw(struct idtfc3 *idtfc3, u8 shift, u8 mult)
169 	return regmap_bulk_write(idtfc3->regmap, LPF_BW_CNFG, &val, sizeof(val));
172 static int idtfc3_enable_tdc(struct idtfc3 *idtfc3, bool enable, u8 meas_mode)
178 	err = regmap_bulk_write(idtfc3->regmap, TIME_CLOCK_MEAS_CTRL, &val, sizeof(val));
183 		return idtfc3_lpf_bw(idtfc3, LPF_BW_SHIFT_DEFAULT, LPF_BW_MULT_DEFAULT);
189 	err = regmap_bulk_write(idtfc3->regmap, TIME_CLOCK_MEAS_CNFG,
198 	err = regmap_bulk_write(idtfc3->regmap, TIME_CLOCK_MEAS_CTRL, &val, sizeof(val));
202 	return idtfc3_lpf_bw(idtfc3, LPF_BW_SHIFT_1PPS, LPF_BW_MULT_DEFAULT);
205 static bool get_tdc_meas(struct idtfc3 *idtfc3, s64 *offset_ns)
213 		err = regmap_bulk_read(idtfc3->regmap, TDC_FIFO_STS,
221 		err = regmap_bulk_read(idtfc3->regmap, TDC_FIFO_READ_REQ,
230 		*offset_ns = tdc_meas2offset(idtfc3, get_unaligned_le64(&buf[1]));
235 static int check_tdc_fifo_overrun(struct idtfc3 *idtfc3)
241 	err = regmap_bulk_read(idtfc3->regmap, TDC_FIFO_STS, &val, sizeof(val));
248 	dev_warn(idtfc3->dev, "TDC FIFO overrun !!!");
250 	err = idtfc3_enable_tdc(idtfc3, true, CONTINUOUS);
257 static int get_tdc_meas_continuous(struct idtfc3 *idtfc3)
263 	err = check_tdc_fifo_overrun(idtfc3);
267 	if (get_tdc_meas(idtfc3, &offset_ns) && offset_ns >= 0) {
269 		event.offset = tdc_offset2phase(idtfc3, offset_ns);
271 		ptp_clock_event(idtfc3->ptp_clock, &event);
277 static int idtfc3_read_subcounter(struct idtfc3 *idtfc3)
282 	err = regmap_bulk_read(idtfc3->regmap, TOD_COUNTER_READ_REQ,
291 static int idtfc3_tod_update_is_done(struct idtfc3 *idtfc3)
297 				       idtfc3->tc_write_timeout, true, idtfc3->regmap,
300 		dev_err(idtfc3->dev, "TOD counter write timeout !!!");
305 static int idtfc3_write_subcounter(struct idtfc3 *idtfc3, u32 counter)
316 	err = regmap_bulk_write(idtfc3->regmap, TOD_SYNC_LOAD_VAL_CTRL,
321 	return idtfc3_tod_update_is_done(idtfc3);
324 static int idtfc3_timecounter_update(struct idtfc3 *idtfc3, u32 counter, s64 ns)
328 	err = idtfc3_write_subcounter(idtfc3, counter);
333 	idtfc3->ns = ns;
334 	idtfc3->last_counter = counter;
339 static int idtfc3_timecounter_read(struct idtfc3 *idtfc3)
343 	now = idtfc3_read_subcounter(idtfc3);
348 	if (now >= idtfc3->last_counter)
349 		delta = now - idtfc3->last_counter;
351 		delta = idtfc3->sub_sync_count - idtfc3->last_counter + now;
354 	idtfc3->ns += delta * idtfc3->ns_per_counter;
355 	idtfc3->last_counter = now;
360 static int _idtfc3_gettime(struct idtfc3 *idtfc3, struct timespec64 *ts)
364 	err = idtfc3_timecounter_read(idtfc3);
368 	*ts = ns_to_timespec64(idtfc3->ns);
375 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
378 	mutex_lock(idtfc3->lock);
379 	err = _idtfc3_gettime(idtfc3, ts);
380 	mutex_unlock(idtfc3->lock);
385 static int _idtfc3_settime(struct idtfc3 *idtfc3, const struct timespec64 *ts)
392 		dev_err(idtfc3->dev, "%s: invalid timespec", __func__);
396 	now = idtfc3_read_subcounter(idtfc3);
400 	offset_ns = (idtfc3->sub_sync_count - now) * idtfc3->ns_per_counter;
402 	(void)ns2counters(idtfc3, offset_ns + now_ns, &sub_ns);
404 	counter = sub_ns / idtfc3->ns_per_counter;
405 	return idtfc3_timecounter_update(idtfc3, counter, now_ns);
410 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
413 	mutex_lock(idtfc3->lock);
414 	err = _idtfc3_settime(idtfc3, ts);
415 	mutex_unlock(idtfc3->lock);
420 static int _idtfc3_adjtime(struct idtfc3 *idtfc3, s64 delta)
430 	if (idtfc3->ns + delta < 0) {
431 		dev_err(idtfc3->dev, "%lld ns adj is too large", delta);
435 	sync_ns = ns2counters(idtfc3, delta + idtfc3->ns_per_sync, &sub_ns);
437 	counter = sub_ns / idtfc3->ns_per_counter;
438 	return idtfc3_timecounter_update(idtfc3, counter, idtfc3->ns + sync_ns +
439 									counter * idtfc3->ns_per_counter);
444 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
447 	mutex_lock(idtfc3->lock);
448 	err = _idtfc3_adjtime(idtfc3, delta);
449 	mutex_unlock(idtfc3->lock);
454 static int _idtfc3_adjphase(struct idtfc3 *idtfc3, s32 delta)
460 	err = idtfc3_set_lpf_mode(idtfc3, LPF_WP);
472 	pcw = div_s64((s64)delta * idtfc3->tdc_apll_freq * 124, NSEC_PER_SEC);
476 	return regmap_bulk_write(idtfc3->regmap, LPF_WR_PHASE_CTRL, buf, sizeof(buf));
481 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
484 	mutex_lock(idtfc3->lock);
485 	err = _idtfc3_adjphase(idtfc3, delta);
486 	mutex_unlock(idtfc3->lock);
491 static int _idtfc3_adjfine(struct idtfc3 *idtfc3, long scaled_ppm)
497 	err = idtfc3_set_lpf_mode(idtfc3, LPF_WF);
519 	return regmap_bulk_write(idtfc3->regmap, LPF_WR_FREQ_CTRL, buf, sizeof(buf));
524 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
527 	mutex_lock(idtfc3->lock);
528 	err = _idtfc3_adjfine(idtfc3, scaled_ppm);
529 	mutex_unlock(idtfc3->lock);
537 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
540 	mutex_lock(idtfc3->lock);
558 				err = idtfc3_enable_tdc(idtfc3, true, CONTINUOUS);
560 			err = idtfc3_enable_tdc(idtfc3, false, MEAS_MODE_INVALID);
566 	mutex_unlock(idtfc3->lock);
569 		dev_err(idtfc3->dev, "Failed in %s with err %d!", __func__, err);
576 	struct idtfc3 *idtfc3 = container_of(ptp, struct idtfc3, caps);
579 	mutex_lock(idtfc3->lock);
582 		idtfc3_timecounter_read(idtfc3);
583 	get_tdc_meas_continuous(idtfc3);
585 	mutex_unlock(idtfc3->lock);
587 	return idtfc3->tc_update_period;
604 static int idtfc3_hw_calibrate(struct idtfc3 *idtfc3)
617 	err = regmap_bulk_write(idtfc3->regmap, TDC_CTRL,
622 	err = regmap_bulk_write(idtfc3->regmap, TDC_CTRL,
634 	err = regmap_bulk_write(idtfc3->regmap, SOFT_RESET_CTRL,
639 	err = regmap_bulk_write(idtfc3->regmap, SOFT_RESET_CTRL,
648 static int idtfc3_init_timecounter(struct idtfc3 *idtfc3)
653 	period_ms = idtfc3->sub_sync_count * MSEC_PER_SEC /
654 			idtfc3->hw_param.time_clk_freq;
656 	idtfc3->tc_update_period = msecs_to_jiffies(period_ms / TDC_GET_PERIOD);
657 	idtfc3->tc_write_timeout = period_ms * USEC_PER_MSEC;
659 	err = idtfc3_timecounter_update(idtfc3, 0, 0);
663 	err = idtfc3_timecounter_read(idtfc3);
667 	ptp_schedule_worker(idtfc3->ptp_clock, idtfc3->tc_update_period);
672 static int idtfc3_get_tdc_apll_freq(struct idtfc3 *idtfc3)
677 	struct idtfc3_hw_param *param = &idtfc3->hw_param;
679 	err = regmap_bulk_read(idtfc3->regmap, TDC_REF_DIV_CNFG,
684 	err = regmap_bulk_read(idtfc3->regmap, TDC_FB_DIV_INT_CNFG,
692 	idtfc3->tdc_apll_freq = div_u64(param->xtal_freq * (u64)tdc_fb_div_int,
698 static int idtfc3_get_fod(struct idtfc3 *idtfc3)
703 	err = regmap_bulk_read(idtfc3->regmap, TIME_CLOCK_SRC, &fod, sizeof(fod));
709 		idtfc3->fod_n = FOD_0;
712 		idtfc3->fod_n = FOD_1;
715 		idtfc3->fod_n = FOD_2;
724 static int idtfc3_get_sync_count(struct idtfc3 *idtfc3)
729 	err = regmap_bulk_read(idtfc3->regmap, SUB_SYNC_GEN_CNFG, buf, sizeof(buf));
733 	idtfc3->sub_sync_count = (get_unaligned_le32(buf) & SUB_SYNC_COUNTER_MASK) + 1;
734 	idtfc3->ns_per_counter = NSEC_PER_SEC / idtfc3->hw_param.time_clk_freq;
735 	idtfc3->ns_per_sync = idtfc3->sub_sync_count * idtfc3->ns_per_counter;
740 static int idtfc3_setup_hw_param(struct idtfc3 *idtfc3)
744 	err = idtfc3_get_fod(idtfc3);
748 	err = idtfc3_get_sync_count(idtfc3);
752 	err = idtfc3_get_time_ref_freq(idtfc3);
756 	return idtfc3_get_tdc_apll_freq(idtfc3);
759 static int idtfc3_configure_hw(struct idtfc3 *idtfc3)
763 	err = idtfc3_hw_calibrate(idtfc3);
767 	err = idtfc3_enable_lpf(idtfc3, true);
771 	err = idtfc3_enable_tdc(idtfc3, false, MEAS_MODE_INVALID);
775 	err = idtfc3_get_tdc_offset_sign(idtfc3);
779 	return idtfc3_setup_hw_param(idtfc3);
782 static int idtfc3_set_overhead(struct idtfc3 *idtfc3)
797 		err = regmap_bulk_write(idtfc3->regmap, TOD_SYNC_LOAD_VAL_CTRL,
816 	idtfc3->tod_write_overhead = lowest_ns;
821 static int idtfc3_enable_ptp(struct idtfc3 *idtfc3)
825 	idtfc3->caps = idtfc3_caps;
826 	snprintf(idtfc3->caps.name, sizeof(idtfc3->caps.name), "IDT FC3W");
827 	idtfc3->ptp_clock = ptp_clock_register(&idtfc3->caps, NULL);
829 	if (IS_ERR(idtfc3->ptp_clock)) {
830 		err = PTR_ERR(idtfc3->ptp_clock);
831 		idtfc3->ptp_clock = NULL;
835 	err = idtfc3_set_overhead(idtfc3);
839 	err = idtfc3_init_timecounter(idtfc3);
843 	dev_info(idtfc3->dev, "TIME_SYNC_CHANNEL registered as ptp%d",
844 		 idtfc3->ptp_clock->index);
849 static int idtfc3_load_firmware(struct idtfc3 *idtfc3)
859 	idtfc3_default_hw_param(&idtfc3->hw_param);
864 	dev_info(idtfc3->dev, "requesting firmware '%s'\n", fname);
866 	err = request_firmware(&fw, fname, idtfc3->dev);
869 		dev_err(idtfc3->dev,
874 	dev_dbg(idtfc3->dev, "firmware size %zu bytes\n", fw->size);
880 			dev_err(idtfc3->dev,
889 			err = idtfc3_set_hw_param(&idtfc3->hw_param, addr, val);
899 			err = regmap_bulk_write(idtfc3->regmap, addr,
907 	err = idtfc3_configure_hw(idtfc3);
913 static int idtfc3_read_device_id(struct idtfc3 *idtfc3, u16 *device_id)
918 	err = regmap_bulk_read(idtfc3->regmap, DEVICE_ID,
921 		dev_err(idtfc3->dev, "%s failed with %d", __func__, err);
930 static int idtfc3_check_device_compatibility(struct idtfc3 *idtfc3)
935 	err = idtfc3_read_device_id(idtfc3, &device_id);
940 		dev_err(idtfc3->dev, "invalid device");
950 	struct idtfc3 *idtfc3;
953 	idtfc3 = devm_kzalloc(&pdev->dev, sizeof(struct idtfc3), GFP_KERNEL);
955 	if (!idtfc3)
958 	idtfc3->dev = &pdev->dev;
959 	idtfc3->mfd = pdev->dev.parent;
960 	idtfc3->lock = &ddata->lock;
961 	idtfc3->regmap = ddata->regmap;
963 	mutex_lock(idtfc3->lock);
965 	err = idtfc3_check_device_compatibility(idtfc3);
967 		mutex_unlock(idtfc3->lock);
971 	err = idtfc3_load_firmware(idtfc3);
974 			mutex_unlock(idtfc3->lock);
977 		dev_warn(idtfc3->dev, "loading firmware failed with %d", err);
980 	err = idtfc3_enable_ptp(idtfc3);
982 		dev_err(idtfc3->dev, "idtfc3_enable_ptp failed with %d", err);
983 		mutex_unlock(idtfc3->lock);
987 	mutex_unlock(idtfc3->lock);
990 		ptp_clock_unregister(idtfc3->ptp_clock);
994 	platform_set_drvdata(pdev, idtfc3);
1001 	struct idtfc3 *idtfc3 = platform_get_drvdata(pdev);
1003 	ptp_clock_unregister(idtfc3->ptp_clock);