cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

ptp_idt82p33.c (20861B)


      1// SPDX-License-Identifier: GPL-2.0
      2//
      3// Copyright (C) 2018 Integrated Device Technology, Inc
      4//
      5
      6#define pr_fmt(fmt) "IDT_82p33xxx: " fmt
      7
      8#include <linux/firmware.h>
      9#include <linux/platform_device.h>
     10#include <linux/module.h>
     11#include <linux/ptp_clock_kernel.h>
     12#include <linux/delay.h>
     13#include <linux/jiffies.h>
     14#include <linux/kernel.h>
     15#include <linux/timekeeping.h>
     16#include <linux/bitops.h>
     17#include <linux/of.h>
     18#include <linux/mfd/rsmu.h>
     19#include <linux/mfd/idt82p33_reg.h>
     20
     21#include "ptp_private.h"
     22#include "ptp_idt82p33.h"
     23
     24MODULE_DESCRIPTION("Driver for IDT 82p33xxx clock devices");
     25MODULE_AUTHOR("IDT support-1588 <IDT-support-1588@lm.renesas.com>");
     26MODULE_VERSION("1.0");
     27MODULE_LICENSE("GPL");
     28MODULE_FIRMWARE(FW_FILENAME);
     29
     30/* Module Parameters */
     31static u32 phase_snap_threshold = SNAP_THRESHOLD_NS;
     32module_param(phase_snap_threshold, uint, 0);
     33MODULE_PARM_DESC(phase_snap_threshold,
     34"threshold (10000ns by default) below which adjtime would use double dco");
     35
     36static char *firmware;
     37module_param(firmware, charp, 0);
     38
     39static inline int idt82p33_read(struct idt82p33 *idt82p33, u16 regaddr,
     40				u8 *buf, u16 count)
     41{
     42	return regmap_bulk_read(idt82p33->regmap, regaddr, buf, count);
     43}
     44
     45static inline int idt82p33_write(struct idt82p33 *idt82p33, u16 regaddr,
     46				 u8 *buf, u16 count)
     47{
     48	return regmap_bulk_write(idt82p33->regmap, regaddr, buf, count);
     49}
     50
     51static void idt82p33_byte_array_to_timespec(struct timespec64 *ts,
     52					    u8 buf[TOD_BYTE_COUNT])
     53{
     54	time64_t sec;
     55	s32 nsec;
     56	u8 i;
     57
     58	nsec = buf[3];
     59	for (i = 0; i < 3; i++) {
     60		nsec <<= 8;
     61		nsec |= buf[2 - i];
     62	}
     63
     64	sec = buf[9];
     65	for (i = 0; i < 5; i++) {
     66		sec <<= 8;
     67		sec |= buf[8 - i];
     68	}
     69
     70	ts->tv_sec = sec;
     71	ts->tv_nsec = nsec;
     72}
     73
     74static void idt82p33_timespec_to_byte_array(struct timespec64 const *ts,
     75					    u8 buf[TOD_BYTE_COUNT])
     76{
     77	time64_t sec;
     78	s32 nsec;
     79	u8 i;
     80
     81	nsec = ts->tv_nsec;
     82	sec = ts->tv_sec;
     83
     84	for (i = 0; i < 4; i++) {
     85		buf[i] = nsec & 0xff;
     86		nsec >>= 8;
     87	}
     88
     89	for (i = 4; i < TOD_BYTE_COUNT; i++) {
     90		buf[i] = sec & 0xff;
     91		sec >>= 8;
     92	}
     93}
     94
     95static int idt82p33_dpll_set_mode(struct idt82p33_channel *channel,
     96				  enum pll_mode mode)
     97{
     98	struct idt82p33 *idt82p33 = channel->idt82p33;
     99	u8 dpll_mode;
    100	int err;
    101
    102	if (channel->pll_mode == mode)
    103		return 0;
    104
    105	err = idt82p33_read(idt82p33, channel->dpll_mode_cnfg,
    106			    &dpll_mode, sizeof(dpll_mode));
    107	if (err)
    108		return err;
    109
    110	dpll_mode &= ~(PLL_MODE_MASK << PLL_MODE_SHIFT);
    111
    112	dpll_mode |= (mode << PLL_MODE_SHIFT);
    113
    114	err = idt82p33_write(idt82p33, channel->dpll_mode_cnfg,
    115			     &dpll_mode, sizeof(dpll_mode));
    116	if (err)
    117		return err;
    118
    119	channel->pll_mode = mode;
    120
    121	return 0;
    122}
    123
    124static int _idt82p33_gettime(struct idt82p33_channel *channel,
    125			     struct timespec64 *ts)
    126{
    127	struct idt82p33 *idt82p33 = channel->idt82p33;
    128	u8 buf[TOD_BYTE_COUNT];
    129	u8 trigger;
    130	int err;
    131
    132	trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
    133			      HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
    134
    135
    136	err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
    137			     &trigger, sizeof(trigger));
    138
    139	if (err)
    140		return err;
    141
    142	if (idt82p33->calculate_overhead_flag)
    143		idt82p33->start_time = ktime_get_raw();
    144
    145	err = idt82p33_read(idt82p33, channel->dpll_tod_sts, buf, sizeof(buf));
    146
    147	if (err)
    148		return err;
    149
    150	idt82p33_byte_array_to_timespec(ts, buf);
    151
    152	return 0;
    153}
    154
    155/*
    156 *   TOD Trigger:
    157 *   Bits[7:4] Write 0x9, MSB write
    158 *   Bits[3:0] Read 0x9, LSB read
    159 */
    160
    161static int _idt82p33_settime(struct idt82p33_channel *channel,
    162			     struct timespec64 const *ts)
    163{
    164	struct idt82p33 *idt82p33 = channel->idt82p33;
    165	struct timespec64 local_ts = *ts;
    166	char buf[TOD_BYTE_COUNT];
    167	s64 dynamic_overhead_ns;
    168	unsigned char trigger;
    169	int err;
    170	u8 i;
    171
    172	trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
    173			      HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
    174
    175	err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
    176			&trigger, sizeof(trigger));
    177
    178	if (err)
    179		return err;
    180
    181	if (idt82p33->calculate_overhead_flag) {
    182		dynamic_overhead_ns = ktime_to_ns(ktime_get_raw())
    183					- ktime_to_ns(idt82p33->start_time);
    184
    185		timespec64_add_ns(&local_ts, dynamic_overhead_ns);
    186
    187		idt82p33->calculate_overhead_flag = 0;
    188	}
    189
    190	idt82p33_timespec_to_byte_array(&local_ts, buf);
    191
    192	/*
    193	 * Store the new time value.
    194	 */
    195	for (i = 0; i < TOD_BYTE_COUNT; i++) {
    196		err = idt82p33_write(idt82p33, channel->dpll_tod_cnfg + i,
    197				     &buf[i], sizeof(buf[i]));
    198		if (err)
    199			return err;
    200	}
    201
    202	return err;
    203}
    204
    205static int _idt82p33_adjtime(struct idt82p33_channel *channel, s64 delta_ns)
    206{
    207	struct idt82p33 *idt82p33 = channel->idt82p33;
    208	struct timespec64 ts;
    209	s64 now_ns;
    210	int err;
    211
    212	idt82p33->calculate_overhead_flag = 1;
    213
    214	err = _idt82p33_gettime(channel, &ts);
    215
    216	if (err)
    217		return err;
    218
    219	now_ns = timespec64_to_ns(&ts);
    220	now_ns += delta_ns + idt82p33->tod_write_overhead_ns;
    221
    222	ts = ns_to_timespec64(now_ns);
    223
    224	err = _idt82p33_settime(channel, &ts);
    225
    226	return err;
    227}
    228
    229static int _idt82p33_adjfine(struct idt82p33_channel *channel, long scaled_ppm)
    230{
    231	struct idt82p33 *idt82p33 = channel->idt82p33;
    232	unsigned char buf[5] = {0};
    233	int err, i;
    234	s64 fcw;
    235
    236	if (scaled_ppm == channel->current_freq_ppb)
    237		return 0;
    238
    239	/*
    240	 * Frequency Control Word unit is: 1.68 * 10^-10 ppm
    241	 *
    242	 * adjfreq:
    243	 *       ppb * 10^9
    244	 * FCW = ----------
    245	 *          168
    246	 *
    247	 * adjfine:
    248	 *       scaled_ppm * 5^12
    249	 * FCW = -------------
    250	 *         168 * 2^4
    251	 */
    252
    253	fcw = scaled_ppm * 244140625ULL;
    254	fcw = div_s64(fcw, 2688);
    255
    256	for (i = 0; i < 5; i++) {
    257		buf[i] = fcw & 0xff;
    258		fcw >>= 8;
    259	}
    260
    261	err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
    262
    263	if (err)
    264		return err;
    265
    266	err = idt82p33_write(idt82p33, channel->dpll_freq_cnfg,
    267			     buf, sizeof(buf));
    268
    269	if (err == 0)
    270		channel->current_freq_ppb = scaled_ppm;
    271
    272	return err;
    273}
    274
    275static int idt82p33_measure_one_byte_write_overhead(
    276		struct idt82p33_channel *channel, s64 *overhead_ns)
    277{
    278	struct idt82p33 *idt82p33 = channel->idt82p33;
    279	ktime_t start, stop;
    280	s64 total_ns;
    281	u8 trigger;
    282	int err;
    283	u8 i;
    284
    285	total_ns = 0;
    286	*overhead_ns = 0;
    287	trigger = TOD_TRIGGER(HW_TOD_WR_TRIG_SEL_MSB_TOD_CNFG,
    288			      HW_TOD_RD_TRIG_SEL_LSB_TOD_STS);
    289
    290	for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
    291
    292		start = ktime_get_raw();
    293
    294		err = idt82p33_write(idt82p33, channel->dpll_tod_trigger,
    295				     &trigger, sizeof(trigger));
    296
    297		stop = ktime_get_raw();
    298
    299		if (err)
    300			return err;
    301
    302		total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
    303	}
    304
    305	*overhead_ns = div_s64(total_ns, MAX_MEASURMENT_COUNT);
    306
    307	return err;
    308}
    309
    310static int idt82p33_measure_tod_write_9_byte_overhead(
    311			struct idt82p33_channel *channel)
    312{
    313	struct idt82p33 *idt82p33 = channel->idt82p33;
    314	u8 buf[TOD_BYTE_COUNT];
    315	ktime_t start, stop;
    316	s64 total_ns;
    317	int err = 0;
    318	u8 i, j;
    319
    320	total_ns = 0;
    321	idt82p33->tod_write_overhead_ns = 0;
    322
    323	for (i = 0; i < MAX_MEASURMENT_COUNT; i++) {
    324
    325		start = ktime_get_raw();
    326
    327		/* Need one less byte for applicable overhead */
    328		for (j = 0; j < (TOD_BYTE_COUNT - 1); j++) {
    329			err = idt82p33_write(idt82p33,
    330					     channel->dpll_tod_cnfg + i,
    331					     &buf[i], sizeof(buf[i]));
    332			if (err)
    333				return err;
    334		}
    335
    336		stop = ktime_get_raw();
    337
    338		total_ns += ktime_to_ns(stop) - ktime_to_ns(start);
    339	}
    340
    341	idt82p33->tod_write_overhead_ns = div_s64(total_ns,
    342						  MAX_MEASURMENT_COUNT);
    343
    344	return err;
    345}
    346
    347static int idt82p33_measure_settime_gettime_gap_overhead(
    348		struct idt82p33_channel *channel, s64 *overhead_ns)
    349{
    350	struct timespec64 ts1 = {0, 0};
    351	struct timespec64 ts2;
    352	int err;
    353
    354	*overhead_ns = 0;
    355
    356	err = _idt82p33_settime(channel, &ts1);
    357
    358	if (err)
    359		return err;
    360
    361	err = _idt82p33_gettime(channel, &ts2);
    362
    363	if (!err)
    364		*overhead_ns = timespec64_to_ns(&ts2) - timespec64_to_ns(&ts1);
    365
    366	return err;
    367}
    368
    369static int idt82p33_measure_tod_write_overhead(struct idt82p33_channel *channel)
    370{
    371	s64 trailing_overhead_ns, one_byte_write_ns, gap_ns;
    372	struct idt82p33 *idt82p33 = channel->idt82p33;
    373	int err;
    374
    375	idt82p33->tod_write_overhead_ns = 0;
    376
    377	err = idt82p33_measure_settime_gettime_gap_overhead(channel, &gap_ns);
    378
    379	if (err) {
    380		dev_err(idt82p33->dev,
    381			"Failed in %s with err %d!\n", __func__, err);
    382		return err;
    383	}
    384
    385	err = idt82p33_measure_one_byte_write_overhead(channel,
    386						       &one_byte_write_ns);
    387
    388	if (err)
    389		return err;
    390
    391	err = idt82p33_measure_tod_write_9_byte_overhead(channel);
    392
    393	if (err)
    394		return err;
    395
    396	trailing_overhead_ns = gap_ns - (2 * one_byte_write_ns);
    397
    398	idt82p33->tod_write_overhead_ns -= trailing_overhead_ns;
    399
    400	return err;
    401}
    402
    403static int idt82p33_check_and_set_masks(struct idt82p33 *idt82p33,
    404					u8 page,
    405					u8 offset,
    406					u8 val)
    407{
    408	int err = 0;
    409
    410	if (page == PLLMASK_ADDR_HI && offset == PLLMASK_ADDR_LO) {
    411		if ((val & 0xfc) || !(val & 0x3)) {
    412			dev_err(idt82p33->dev,
    413				"Invalid PLL mask 0x%x\n", val);
    414			err = -EINVAL;
    415		} else {
    416			idt82p33->pll_mask = val;
    417		}
    418	} else if (page == PLL0_OUTMASK_ADDR_HI &&
    419		offset == PLL0_OUTMASK_ADDR_LO) {
    420		idt82p33->channel[0].output_mask = val;
    421	} else if (page == PLL1_OUTMASK_ADDR_HI &&
    422		offset == PLL1_OUTMASK_ADDR_LO) {
    423		idt82p33->channel[1].output_mask = val;
    424	}
    425
    426	return err;
    427}
    428
    429static void idt82p33_display_masks(struct idt82p33 *idt82p33)
    430{
    431	u8 mask, i;
    432
    433	dev_info(idt82p33->dev,
    434		 "pllmask = 0x%02x\n", idt82p33->pll_mask);
    435
    436	for (i = 0; i < MAX_PHC_PLL; i++) {
    437		mask = 1 << i;
    438
    439		if (mask & idt82p33->pll_mask)
    440			dev_info(idt82p33->dev,
    441				 "PLL%d output_mask = 0x%04x\n",
    442				 i, idt82p33->channel[i].output_mask);
    443	}
    444}
    445
    446static int idt82p33_sync_tod(struct idt82p33_channel *channel, bool enable)
    447{
    448	struct idt82p33 *idt82p33 = channel->idt82p33;
    449	u8 sync_cnfg;
    450	int err;
    451
    452	err = idt82p33_read(idt82p33, channel->dpll_sync_cnfg,
    453			    &sync_cnfg, sizeof(sync_cnfg));
    454	if (err)
    455		return err;
    456
    457	sync_cnfg &= ~SYNC_TOD;
    458	if (enable)
    459		sync_cnfg |= SYNC_TOD;
    460
    461	return idt82p33_write(idt82p33, channel->dpll_sync_cnfg,
    462			      &sync_cnfg, sizeof(sync_cnfg));
    463}
    464
    465static int idt82p33_output_enable(struct idt82p33_channel *channel,
    466				  bool enable, unsigned int outn)
    467{
    468	struct idt82p33 *idt82p33 = channel->idt82p33;
    469	int err;
    470	u8 val;
    471
    472	err = idt82p33_read(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
    473	if (err)
    474		return err;
    475	if (enable)
    476		val &= ~SQUELCH_ENABLE;
    477	else
    478		val |= SQUELCH_ENABLE;
    479
    480	return idt82p33_write(idt82p33, OUT_MUX_CNFG(outn), &val, sizeof(val));
    481}
    482
    483static int idt82p33_output_mask_enable(struct idt82p33_channel *channel,
    484				       bool enable)
    485{
    486	u16 mask;
    487	int err;
    488	u8 outn;
    489
    490	mask = channel->output_mask;
    491	outn = 0;
    492
    493	while (mask) {
    494		if (mask & 0x1) {
    495			err = idt82p33_output_enable(channel, enable, outn);
    496			if (err)
    497				return err;
    498		}
    499
    500		mask >>= 0x1;
    501		outn++;
    502	}
    503
    504	return 0;
    505}
    506
    507static int idt82p33_perout_enable(struct idt82p33_channel *channel,
    508				  bool enable,
    509				  struct ptp_perout_request *perout)
    510{
    511	unsigned int flags = perout->flags;
    512
    513	/* Enable/disable output based on output_mask */
    514	if (flags == PEROUT_ENABLE_OUTPUT_MASK)
    515		return idt82p33_output_mask_enable(channel, enable);
    516
    517	/* Enable/disable individual output instead */
    518	return idt82p33_output_enable(channel, enable, perout->index);
    519}
    520
    521static int idt82p33_enable_tod(struct idt82p33_channel *channel)
    522{
    523	struct idt82p33 *idt82p33 = channel->idt82p33;
    524	struct timespec64 ts = {0, 0};
    525	int err;
    526
    527	err = idt82p33_measure_tod_write_overhead(channel);
    528
    529	if (err) {
    530		dev_err(idt82p33->dev,
    531			"Failed in %s with err %d!\n", __func__, err);
    532		return err;
    533	}
    534
    535	err = _idt82p33_settime(channel, &ts);
    536
    537	if (err)
    538		return err;
    539
    540	return idt82p33_sync_tod(channel, true);
    541}
    542
    543static void idt82p33_ptp_clock_unregister_all(struct idt82p33 *idt82p33)
    544{
    545	struct idt82p33_channel *channel;
    546	u8 i;
    547
    548	for (i = 0; i < MAX_PHC_PLL; i++) {
    549
    550		channel = &idt82p33->channel[i];
    551
    552		if (channel->ptp_clock)
    553			ptp_clock_unregister(channel->ptp_clock);
    554	}
    555}
    556
    557static int idt82p33_enable(struct ptp_clock_info *ptp,
    558			   struct ptp_clock_request *rq, int on)
    559{
    560	struct idt82p33_channel *channel =
    561			container_of(ptp, struct idt82p33_channel, caps);
    562	struct idt82p33 *idt82p33 = channel->idt82p33;
    563	int err = -EOPNOTSUPP;
    564
    565	mutex_lock(idt82p33->lock);
    566
    567	if (rq->type == PTP_CLK_REQ_PEROUT) {
    568		if (!on)
    569			err = idt82p33_perout_enable(channel, false,
    570						     &rq->perout);
    571		/* Only accept a 1-PPS aligned to the second. */
    572		else if (rq->perout.start.nsec || rq->perout.period.sec != 1 ||
    573			 rq->perout.period.nsec)
    574			err = -ERANGE;
    575		else
    576			err = idt82p33_perout_enable(channel, true,
    577						     &rq->perout);
    578	}
    579
    580	mutex_unlock(idt82p33->lock);
    581
    582	if (err)
    583		dev_err(idt82p33->dev,
    584			"Failed in %s with err %d!\n", __func__, err);
    585	return err;
    586}
    587
    588static int idt82p33_adjwritephase(struct ptp_clock_info *ptp, s32 offset_ns)
    589{
    590	struct idt82p33_channel *channel =
    591		container_of(ptp, struct idt82p33_channel, caps);
    592	struct idt82p33 *idt82p33 = channel->idt82p33;
    593	s64 offset_regval, offset_fs;
    594	u8 val[4] = {0};
    595	int err;
    596
    597	offset_fs = (s64)(-offset_ns) * 1000000;
    598
    599	if (offset_fs > WRITE_PHASE_OFFSET_LIMIT)
    600		offset_fs = WRITE_PHASE_OFFSET_LIMIT;
    601	else if (offset_fs < -WRITE_PHASE_OFFSET_LIMIT)
    602		offset_fs = -WRITE_PHASE_OFFSET_LIMIT;
    603
    604	/* Convert from phaseoffset_fs to register value */
    605	offset_regval = div_s64(offset_fs * 1000, IDT_T0DPLL_PHASE_RESOL);
    606
    607	val[0] = offset_regval & 0xFF;
    608	val[1] = (offset_regval >> 8) & 0xFF;
    609	val[2] = (offset_regval >> 16) & 0xFF;
    610	val[3] = (offset_regval >> 24) & 0x1F;
    611	val[3] |= PH_OFFSET_EN;
    612
    613	mutex_lock(idt82p33->lock);
    614
    615	err = idt82p33_dpll_set_mode(channel, PLL_MODE_WPH);
    616	if (err) {
    617		dev_err(idt82p33->dev,
    618			"Failed in %s with err %d!\n", __func__, err);
    619		goto out;
    620	}
    621
    622	err = idt82p33_write(idt82p33, channel->dpll_phase_cnfg, val,
    623			     sizeof(val));
    624
    625out:
    626	mutex_unlock(idt82p33->lock);
    627	return err;
    628}
    629
    630static int idt82p33_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
    631{
    632	struct idt82p33_channel *channel =
    633			container_of(ptp, struct idt82p33_channel, caps);
    634	struct idt82p33 *idt82p33 = channel->idt82p33;
    635	int err;
    636
    637	mutex_lock(idt82p33->lock);
    638	err = _idt82p33_adjfine(channel, scaled_ppm);
    639	mutex_unlock(idt82p33->lock);
    640	if (err)
    641		dev_err(idt82p33->dev,
    642			"Failed in %s with err %d!\n", __func__, err);
    643
    644	return err;
    645}
    646
    647static int idt82p33_adjtime(struct ptp_clock_info *ptp, s64 delta_ns)
    648{
    649	struct idt82p33_channel *channel =
    650			container_of(ptp, struct idt82p33_channel, caps);
    651	struct idt82p33 *idt82p33 = channel->idt82p33;
    652	int err;
    653
    654	mutex_lock(idt82p33->lock);
    655
    656	if (abs(delta_ns) < phase_snap_threshold) {
    657		mutex_unlock(idt82p33->lock);
    658		return 0;
    659	}
    660
    661	err = _idt82p33_adjtime(channel, delta_ns);
    662
    663	mutex_unlock(idt82p33->lock);
    664
    665	if (err)
    666		dev_err(idt82p33->dev,
    667			"Failed in %s with err %d!\n", __func__, err);
    668	return err;
    669}
    670
    671static int idt82p33_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
    672{
    673	struct idt82p33_channel *channel =
    674			container_of(ptp, struct idt82p33_channel, caps);
    675	struct idt82p33 *idt82p33 = channel->idt82p33;
    676	int err;
    677
    678	mutex_lock(idt82p33->lock);
    679	err = _idt82p33_gettime(channel, ts);
    680	mutex_unlock(idt82p33->lock);
    681
    682	if (err)
    683		dev_err(idt82p33->dev,
    684			"Failed in %s with err %d!\n", __func__, err);
    685	return err;
    686}
    687
    688static int idt82p33_settime(struct ptp_clock_info *ptp,
    689			    const struct timespec64 *ts)
    690{
    691	struct idt82p33_channel *channel =
    692			container_of(ptp, struct idt82p33_channel, caps);
    693	struct idt82p33 *idt82p33 = channel->idt82p33;
    694	int err;
    695
    696	mutex_lock(idt82p33->lock);
    697	err = _idt82p33_settime(channel, ts);
    698	mutex_unlock(idt82p33->lock);
    699
    700	if (err)
    701		dev_err(idt82p33->dev,
    702			"Failed in %s with err %d!\n", __func__, err);
    703	return err;
    704}
    705
    706static int idt82p33_channel_init(struct idt82p33_channel *channel, int index)
    707{
    708	switch (index) {
    709	case 0:
    710		channel->dpll_tod_cnfg = DPLL1_TOD_CNFG;
    711		channel->dpll_tod_trigger = DPLL1_TOD_TRIGGER;
    712		channel->dpll_tod_sts = DPLL1_TOD_STS;
    713		channel->dpll_mode_cnfg = DPLL1_OPERATING_MODE_CNFG;
    714		channel->dpll_freq_cnfg = DPLL1_HOLDOVER_FREQ_CNFG;
    715		channel->dpll_phase_cnfg = DPLL1_PHASE_OFFSET_CNFG;
    716		channel->dpll_sync_cnfg = DPLL1_SYNC_EDGE_CNFG;
    717		channel->dpll_input_mode_cnfg = DPLL1_INPUT_MODE_CNFG;
    718		break;
    719	case 1:
    720		channel->dpll_tod_cnfg = DPLL2_TOD_CNFG;
    721		channel->dpll_tod_trigger = DPLL2_TOD_TRIGGER;
    722		channel->dpll_tod_sts = DPLL2_TOD_STS;
    723		channel->dpll_mode_cnfg = DPLL2_OPERATING_MODE_CNFG;
    724		channel->dpll_freq_cnfg = DPLL2_HOLDOVER_FREQ_CNFG;
    725		channel->dpll_phase_cnfg = DPLL2_PHASE_OFFSET_CNFG;
    726		channel->dpll_sync_cnfg = DPLL2_SYNC_EDGE_CNFG;
    727		channel->dpll_input_mode_cnfg = DPLL2_INPUT_MODE_CNFG;
    728		break;
    729	default:
    730		return -EINVAL;
    731	}
    732
    733	channel->current_freq_ppb = 0;
    734
    735	return 0;
    736}
    737
    738static void idt82p33_caps_init(struct ptp_clock_info *caps)
    739{
    740	caps->owner = THIS_MODULE;
    741	caps->max_adj = DCO_MAX_PPB;
    742	caps->n_per_out = 11;
    743	caps->adjphase = idt82p33_adjwritephase;
    744	caps->adjfine = idt82p33_adjfine;
    745	caps->adjtime = idt82p33_adjtime;
    746	caps->gettime64 = idt82p33_gettime;
    747	caps->settime64 = idt82p33_settime;
    748	caps->enable = idt82p33_enable;
    749}
    750
    751static int idt82p33_enable_channel(struct idt82p33 *idt82p33, u32 index)
    752{
    753	struct idt82p33_channel *channel;
    754	int err;
    755
    756	if (!(index < MAX_PHC_PLL))
    757		return -EINVAL;
    758
    759	channel = &idt82p33->channel[index];
    760
    761	err = idt82p33_channel_init(channel, index);
    762	if (err) {
    763		dev_err(idt82p33->dev,
    764			"Channel_init failed in %s with err %d!\n",
    765			__func__, err);
    766		return err;
    767	}
    768
    769	channel->idt82p33 = idt82p33;
    770
    771	idt82p33_caps_init(&channel->caps);
    772	snprintf(channel->caps.name, sizeof(channel->caps.name),
    773		 "IDT 82P33 PLL%u", index);
    774
    775	channel->ptp_clock = ptp_clock_register(&channel->caps, NULL);
    776
    777	if (IS_ERR(channel->ptp_clock)) {
    778		err = PTR_ERR(channel->ptp_clock);
    779		channel->ptp_clock = NULL;
    780		return err;
    781	}
    782
    783	if (!channel->ptp_clock)
    784		return -ENOTSUPP;
    785
    786	err = idt82p33_dpll_set_mode(channel, PLL_MODE_DCO);
    787	if (err) {
    788		dev_err(idt82p33->dev,
    789			"Dpll_set_mode failed in %s with err %d!\n",
    790			__func__, err);
    791		return err;
    792	}
    793
    794	err = idt82p33_enable_tod(channel);
    795	if (err) {
    796		dev_err(idt82p33->dev,
    797			"Enable_tod failed in %s with err %d!\n",
    798			__func__, err);
    799		return err;
    800	}
    801
    802	dev_info(idt82p33->dev, "PLL%d registered as ptp%d\n",
    803		 index, channel->ptp_clock->index);
    804
    805	return 0;
    806}
    807
    808static int idt82p33_load_firmware(struct idt82p33 *idt82p33)
    809{
    810	const struct firmware *fw;
    811	struct idt82p33_fwrc *rec;
    812	u8 loaddr, page, val;
    813	int err;
    814	s32 len;
    815
    816	dev_dbg(idt82p33->dev, "requesting firmware '%s'\n", FW_FILENAME);
    817
    818	err = request_firmware(&fw, FW_FILENAME, idt82p33->dev);
    819
    820	if (err) {
    821		dev_err(idt82p33->dev,
    822			"Failed in %s with err %d!\n", __func__, err);
    823		return err;
    824	}
    825
    826	dev_dbg(idt82p33->dev, "firmware size %zu bytes\n", fw->size);
    827
    828	rec = (struct idt82p33_fwrc *) fw->data;
    829
    830	for (len = fw->size; len > 0; len -= sizeof(*rec)) {
    831
    832		if (rec->reserved) {
    833			dev_err(idt82p33->dev,
    834				"bad firmware, reserved field non-zero\n");
    835			err = -EINVAL;
    836		} else {
    837			val = rec->value;
    838			loaddr = rec->loaddr;
    839			page = rec->hiaddr;
    840
    841			rec++;
    842
    843			err = idt82p33_check_and_set_masks(idt82p33, page,
    844							   loaddr, val);
    845		}
    846
    847		if (err == 0) {
    848			/* Page size 128, last 4 bytes of page skipped */
    849			if (loaddr > 0x7b)
    850				continue;
    851
    852			err = idt82p33_write(idt82p33, REG_ADDR(page, loaddr),
    853					     &val, sizeof(val));
    854		}
    855
    856		if (err)
    857			goto out;
    858	}
    859
    860	idt82p33_display_masks(idt82p33);
    861out:
    862	release_firmware(fw);
    863	return err;
    864}
    865
    866
    867static int idt82p33_probe(struct platform_device *pdev)
    868{
    869	struct rsmu_ddata *ddata = dev_get_drvdata(pdev->dev.parent);
    870	struct idt82p33 *idt82p33;
    871	int err;
    872	u8 i;
    873
    874	idt82p33 = devm_kzalloc(&pdev->dev,
    875				sizeof(struct idt82p33), GFP_KERNEL);
    876	if (!idt82p33)
    877		return -ENOMEM;
    878
    879	idt82p33->dev = &pdev->dev;
    880	idt82p33->mfd = pdev->dev.parent;
    881	idt82p33->lock = &ddata->lock;
    882	idt82p33->regmap = ddata->regmap;
    883	idt82p33->tod_write_overhead_ns = 0;
    884	idt82p33->calculate_overhead_flag = 0;
    885	idt82p33->pll_mask = DEFAULT_PLL_MASK;
    886	idt82p33->channel[0].output_mask = DEFAULT_OUTPUT_MASK_PLL0;
    887	idt82p33->channel[1].output_mask = DEFAULT_OUTPUT_MASK_PLL1;
    888
    889	mutex_lock(idt82p33->lock);
    890
    891	err = idt82p33_load_firmware(idt82p33);
    892
    893	if (err)
    894		dev_warn(idt82p33->dev,
    895			 "loading firmware failed with %d\n", err);
    896
    897	if (idt82p33->pll_mask) {
    898		for (i = 0; i < MAX_PHC_PLL; i++) {
    899			if (idt82p33->pll_mask & (1 << i)) {
    900				err = idt82p33_enable_channel(idt82p33, i);
    901				if (err) {
    902					dev_err(idt82p33->dev,
    903						"Failed in %s with err %d!\n",
    904						__func__, err);
    905					break;
    906				}
    907			}
    908		}
    909	} else {
    910		dev_err(idt82p33->dev,
    911			"no PLLs flagged as PHCs, nothing to do\n");
    912		err = -ENODEV;
    913	}
    914
    915	mutex_unlock(idt82p33->lock);
    916
    917	if (err) {
    918		idt82p33_ptp_clock_unregister_all(idt82p33);
    919		return err;
    920	}
    921
    922	platform_set_drvdata(pdev, idt82p33);
    923
    924	return 0;
    925}
    926
    927static int idt82p33_remove(struct platform_device *pdev)
    928{
    929	struct idt82p33 *idt82p33 = platform_get_drvdata(pdev);
    930
    931	idt82p33_ptp_clock_unregister_all(idt82p33);
    932
    933	return 0;
    934}
    935
    936static struct platform_driver idt82p33_driver = {
    937	.driver = {
    938		.name = "82p33x1x-phc",
    939	},
    940	.probe = idt82p33_probe,
    941	.remove	= idt82p33_remove,
    942};
    943
    944module_platform_driver(idt82p33_driver);