diff mbox series

[v2,12/70] net/ice/base: move code block

Message ID 20220815073206.2917968-13-qi.z.zhang@intel.com (mailing list archive)
State Awaiting Upstream
Delegated to: Qi Zhang
Headers show
Series ice base code update | expand

Checks

Context Check Description
ci/checkpatch success coding style OK

Commit Message

Zhang, Qi Z Aug. 15, 2022, 7:31 a.m. UTC
Move some code block to the beginning of ice_ptp_hw.c to align
withkernel driver.

Signed-off-by: Sergey Temerkhanov <sergey.temerkhanov@intel.com>
Signed-off-by: Qi Zhang <qi.z.zhang@intel.com>
---
 drivers/net/ice/base/ice_ptp_hw.c | 997 +++++++++++++++---------------
 1 file changed, 498 insertions(+), 499 deletions(-)

Comments

Qiming Yang Aug. 15, 2022, 6:30 a.m. UTC | #1
> -----Original Message-----
> From: Zhang, Qi Z <qi.z.zhang@intel.com>
> Sent: Monday, August 15, 2022 3:31 PM
> To: Yang, Qiming <qiming.yang@intel.com>
> Cc: dev@dpdk.org; Zhang, Qi Z <qi.z.zhang@intel.com>; Temerkhanov,
> Sergey <sergey.temerkhanov@intel.com>
> Subject: [PATCH v2 12/70] net/ice/base: move code block
> 
> Move some code block to the beginning of ice_ptp_hw.c to align withkernel
> driver.
> 
> Signed-off-by: Sergey Temerkhanov <sergey.temerkhanov@intel.com>
> Signed-off-by: Qi Zhang <qi.z.zhang@intel.com>
> ---
>  drivers/net/ice/base/ice_ptp_hw.c | 997 +++++++++++++++---------------
>  1 file changed, 498 insertions(+), 499 deletions(-)
> 
> diff --git a/drivers/net/ice/base/ice_ptp_hw.c
> b/drivers/net/ice/base/ice_ptp_hw.c
> index 23d90b127d..22d0774dd7 100644
> --- a/drivers/net/ice/base/ice_ptp_hw.c
> +++ b/drivers/net/ice/base/ice_ptp_hw.c
> @@ -101,6 +101,286 @@ u64 ice_ptp_read_src_incval(struct ice_hw *hw)
>  	return ((u64)(hi & INCVAL_HIGH_M) << 32) | lo;  }
> 
> +/**
> + * ice_read_cgu_reg_e822 - Read a CGU register
> + * @hw: pointer to the HW struct
> + * @addr: Register address to read
> + * @val: storage for register value read
> + *
> + * Read the contents of a register of the Clock Generation Unit. Only
> + * applicable to E822 devices.
> + */
> +static enum ice_status
> +ice_read_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 *val) {
> +	struct ice_sbq_msg_input cgu_msg;
> +	enum ice_status status;
> +
> +	cgu_msg.opcode = ice_sbq_msg_rd;
> +	cgu_msg.dest_dev = cgu;
> +	cgu_msg.msg_addr_low = addr;
> +	cgu_msg.msg_addr_high = 0x0;
> +
> +	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to read CGU register
> 0x%04x, status %d\n",
> +			  addr, status);
> +		return status;
> +	}
> +
> +	*val = cgu_msg.data;
> +
> +	return ICE_SUCCESS;
> +}
> +
> +/**
> + * ice_write_cgu_reg_e822 - Write a CGU register
> + * @hw: pointer to the HW struct
> + * @addr: Register address to write
> + * @val: value to write into the register
> + *
> + * Write the specified value to a register of the Clock Generation
> +Unit. Only
> + * applicable to E822 devices.
> + */
> +static enum ice_status
> +ice_write_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 val) {
> +	struct ice_sbq_msg_input cgu_msg;
> +	enum ice_status status;
> +
> +	cgu_msg.opcode = ice_sbq_msg_wr;
> +	cgu_msg.dest_dev = cgu;
> +	cgu_msg.msg_addr_low = addr;
> +	cgu_msg.msg_addr_high = 0x0;
> +	cgu_msg.data = val;
> +
> +	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to write CGU register
> 0x%04x, status %d\n",
> +			  addr, status);
> +		return status;
> +	}
> +
> +	return ICE_SUCCESS;
> +}
> +
> +/**
> + * ice_clk_freq_str - Convert time_ref_freq to string
> + * @clk_freq: Clock frequency
> + *
> + * Convert the specified TIME_REF clock frequency to a string.
> + */
> +static const char *ice_clk_freq_str(u8 clk_freq) {
> +	switch ((enum ice_time_ref_freq)clk_freq) {
> +	case ICE_TIME_REF_FREQ_25_000:
> +		return "25 MHz";
> +	case ICE_TIME_REF_FREQ_122_880:
> +		return "122.88 MHz";
> +	case ICE_TIME_REF_FREQ_125_000:
> +		return "125 MHz";
> +	case ICE_TIME_REF_FREQ_153_600:
> +		return "153.6 MHz";
> +	case ICE_TIME_REF_FREQ_156_250:
> +		return "156.25 MHz";
> +	case ICE_TIME_REF_FREQ_245_760:
> +		return "245.76 MHz";
> +	default:
> +		return "Unknown";
> +	}
> +}
> +
> +/**
> + * ice_clk_src_str - Convert time_ref_src to string
> + * @clk_src: Clock source
> + *
> + * Convert the specified clock source to its string name.
> + */
> +static const char *ice_clk_src_str(u8 clk_src) {
> +	switch ((enum ice_clk_src)clk_src) {
> +	case ICE_CLK_SRC_TCX0:
> +		return "TCX0";
> +	case ICE_CLK_SRC_TIME_REF:
> +		return "TIME_REF";
> +	default:
> +		return "Unknown";
> +	}
> +}
> +
> +/**
> + * ice_cfg_cgu_pll_e822 - Configure the Clock Generation Unit
> + * @hw: pointer to the HW struct
> + * @clk_freq: Clock frequency to program
> + * @clk_src: Clock source to select (TIME_REF, or TCX0)
> + *
> + * Configure the Clock Generation Unit with the desired clock frequency
> +and
> + * time reference, enabling the PLL which drives the PTP hardware clock.
> + */
> +enum ice_status
> +ice_cfg_cgu_pll_e822(struct ice_hw *hw, enum ice_time_ref_freq clk_freq,
> +		     enum ice_clk_src clk_src)
> +{
> +	union tspll_ro_bwm_lf bwm_lf;
> +	union nac_cgu_dword19 dw19;
> +	union nac_cgu_dword22 dw22;
> +	union nac_cgu_dword24 dw24;
> +	union nac_cgu_dword9 dw9;
> +	enum ice_status status;
> +
> +	if (clk_freq >= NUM_ICE_TIME_REF_FREQ) {
> +		ice_warn(hw, "Invalid TIME_REF frequency %u\n", clk_freq);
> +		return ICE_ERR_PARAM;
> +	}
> +
> +	if (clk_src >= NUM_ICE_CLK_SRC) {
> +		ice_warn(hw, "Invalid clock source %u\n", clk_src);
> +		return ICE_ERR_PARAM;
> +	}
> +
> +	if (clk_src == ICE_CLK_SRC_TCX0 &&
> +	    clk_freq != ICE_TIME_REF_FREQ_25_000) {
> +		ice_warn(hw, "TCX0 only supports 25 MHz frequency\n");
> +		return ICE_ERR_PARAM;
> +	}
> +
> +	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD9, &dw9.val);
> +	if (status)
> +		return status;
> +
> +	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> &dw24.val);
> +	if (status)
> +		return status;
> +
> +	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF,
> &bwm_lf.val);
> +	if (status)
> +		return status;
> +
> +	/* Log the current clock configuration */
> +	ice_debug(hw, ICE_DBG_PTP, "Current CGU configuration -- %s,
> clk_src %s, clk_freq %s, PLL %s\n",
> +		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
> +		  ice_clk_src_str(dw24.field.time_ref_sel),
> +		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
> +		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
> +
> +	/* Disable the PLL before changing the clock source or frequency */
> +	if (dw24.field.ts_pll_enable) {
> +		dw24.field.ts_pll_enable = 0;
> +
> +		status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> dw24.val);
> +		if (status)
> +			return status;
> +	}
> +
> +	/* Set the frequency */
> +	dw9.field.time_ref_freq_sel = clk_freq;
> +	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD9, dw9.val);
> +	if (status)
> +		return status;
> +
> +	/* Configure the TS PLL feedback divisor */
> +	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD19,
> &dw19.val);
> +	if (status)
> +		return status;
> +
> +	dw19.field.tspll_fbdiv_intgr =
> e822_cgu_params[clk_freq].feedback_div;
> +	dw19.field.tspll_ndivratio = 1;
> +
> +	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD19,
> dw19.val);
> +	if (status)
> +		return status;
> +
> +	/* Configure the TS PLL post divisor */
> +	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD22,
> &dw22.val);
> +	if (status)
> +		return status;
> +
> +	dw22.field.time1588clk_div =
> e822_cgu_params[clk_freq].post_pll_div;
> +	dw22.field.time1588clk_sel_div2 = 0;
> +
> +	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD22,
> dw22.val);
> +	if (status)
> +		return status;
> +
> +	/* Configure the TS PLL pre divisor and clock source */
> +	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> &dw24.val);
> +	if (status)
> +		return status;
> +
> +	dw24.field.ref1588_ck_div =
> e822_cgu_params[clk_freq].refclk_pre_div;
> +	dw24.field.tspll_fbdiv_frac = e822_cgu_params[clk_freq].frac_n_div;
> +	dw24.field.time_ref_sel = clk_src;
> +
> +	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> dw24.val);
> +	if (status)
> +		return status;
> +
> +	/* Finally, enable the PLL */
> +	dw24.field.ts_pll_enable = 1;
> +
> +	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> dw24.val);
> +	if (status)
> +		return status;
> +
> +	/* Wait to verify if the PLL locks */
> +	ice_msec_delay(1, true);
> +
> +	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF,
> &bwm_lf.val);
> +	if (status)
> +		return status;
> +
> +	if (!bwm_lf.field.plllock_true_lock_cri) {
> +		ice_warn(hw, "CGU PLL failed to lock\n");
> +		return ICE_ERR_NOT_READY;
> +	}
> +
> +	/* Log the current clock configuration */
> +	ice_debug(hw, ICE_DBG_PTP, "New CGU configuration -- %s,
> clk_src %s, clk_freq %s, PLL %s\n",
> +		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
> +		  ice_clk_src_str(dw24.field.time_ref_sel),
> +		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
> +		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
> +
> +	return ICE_SUCCESS;
> +}
> +
> +/**
> + * ice_init_cgu_e822 - Initialize CGU with settings from firmware
> + * @hw: pointer to the HW structure
> + *
> + * Initialize the Clock Generation Unit of the E822 device.
> + */
> +static enum ice_status ice_init_cgu_e822(struct ice_hw *hw) {
> +	struct ice_ts_func_info *ts_info = &hw->func_caps.ts_func_info;
> +	union tspll_cntr_bist_settings cntr_bist;
> +	enum ice_status status;
> +
> +	status = ice_read_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
> +				       &cntr_bist.val);
> +	if (status)
> +		return status;
> +
> +	/* Disable sticky lock detection so lock status reported is accurate */
> +	cntr_bist.field.i_plllock_sel_0 = 0;
> +	cntr_bist.field.i_plllock_sel_1 = 0;
> +
> +	status = ice_write_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
> +					cntr_bist.val);
> +	if (status)
> +		return status;
> +
> +	/* Configure the CGU PLL using the parameters from the function
> +	 * capabilities.
> +	 */
> +	status = ice_cfg_cgu_pll_e822(hw, ts_info->time_ref,
> +				      (enum ice_clk_src)ts_info->clk_src);
> +	if (status)
> +		return status;
> +
> +	return ICE_SUCCESS;
> +}
> +
>  /**
>   * ice_ptp_exec_tmr_cmd - Execute all prepared timer commands
>   * @hw: pointer to HW struct
> @@ -346,261 +626,59 @@ ice_read_40b_phy_reg_e822(struct ice_hw *hw,
> u8 port, u16 low_addr, u64 *val)
>   * The high offset is looked up. This function only operates on registers
>   * known to be two parts of a 64bit value.
>   */
> -static enum ice_status
> -ice_read_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr,
> u64 *val) -{
> -	enum ice_status status;
> -	u32 low, high;
> -	u16 high_addr;
> -
> -	/* Only operate on registers known to be split into two 32bit
> -	 * registers.
> -	 */
> -	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
> -		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr
> 0x%08x\n",
> -			  low_addr);
> -		return ICE_ERR_PARAM;
> -	}
> -
> -	status = ice_read_phy_reg_e822(hw, port, low_addr, &low);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to read from low
> register 0x%08x\n, status %d",
> -			  low_addr, status);
> -		return status;
> -	}
> -
> -	status = ice_read_phy_reg_e822(hw, port, high_addr, &high);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to read from high
> register 0x%08x\n, status %d",
> -			  high_addr, status);
> -		return status;
> -	}
> -
> -	*val = (u64)high << 32 | low;
> -
> -	return ICE_SUCCESS;
> -}
> -
> -/**
> - * ice_write_phy_reg_e822_lp - Write a PHY register
> - * @hw: pointer to the HW struct
> - * @port: PHY port to write to
> - * @offset: PHY register offset to write
> - * @val: The value to write to the register
> - * @lock_sbq: true if the sideband queue lock must be acquired
> - *
> - * Write a PHY register for the given port over the device sideband queue.
> - */
> -static enum ice_status
> -ice_write_phy_reg_e822_lp(struct ice_hw *hw, u8 port, u16 offset, u32 val,
> -			  bool lock_sbq)
> -{
> -	struct ice_sbq_msg_input msg = {0};
> -	enum ice_status status;
> -
> -	ice_fill_phy_msg_e822(&msg, port, offset);
> -	msg.opcode = ice_sbq_msg_wr;
> -	msg.data = val;
> -
> -	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to
> phy, status %d\n",
> -			  status);
> -		return status;
> -	}
> -
> -	return ICE_SUCCESS;
> -}
> -
> -enum ice_status
> -ice_write_phy_reg_e822(struct ice_hw *hw, u8 port, u16 offset, u32 val) -{
> -	return ice_write_phy_reg_e822_lp(hw, port, offset, val, true);
> -}
> -
> -/**
> - * ice_write_40b_phy_reg_e822 - Write a 40b value to the PHY
> - * @hw: pointer to the HW struct
> - * @port: port to write to
> - * @low_addr: offset of the low register
> - * @val: 40b value to write
> - *
> - * Write the provided 40b value to the two associated registers by splitting
> - * it up into two chunks, the lower 8 bits and the upper 32 bits.
> - */
> -static enum ice_status
> -ice_write_40b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr,
> u64 val) -{
> -	enum ice_status status;
> -	u32 low, high;
> -	u16 high_addr;
> -
> -	/* Only operate on registers known to be split into a lower 8 bit
> -	 * register and an upper 32 bit register.
> -	 */
> -	if (!ice_is_40b_phy_reg_e822(low_addr, &high_addr)) {
> -		ice_debug(hw, ICE_DBG_PTP, "Invalid 40b register addr
> 0x%08x\n",
> -			  low_addr);
> -		return ICE_ERR_PARAM;
> -	}
> -
> -	low = (u32)(val & P_REG_40B_LOW_M);
> -	high = (u32)(val >> P_REG_40B_HIGH_S);
> -
> -	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register
> 0x%08x\n, status %d",
> -			  low_addr, status);
> -		return status;
> -	}
> -
> -	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high
> register 0x%08x\n, status %d",
> -			  high_addr, status);
> -		return status;
> -	}
> -
> -	return ICE_SUCCESS;
> -}
> -
> -/**
> - * ice_write_64b_phy_reg_e822 - Write a 64bit value to PHY registers
> - * @hw: pointer to the HW struct
> - * @port: PHY port to read from
> - * @low_addr: offset of the lower register to read from
> - * @val: the contents of the 64bit value to write to PHY
> - *
> - * Write the 64bit value to the two associated 32bit PHY registers. The
> offset
> - * is always specified as the lower register, and the high address is looked
> - * up. This function only operates on registers known to be two parts of
> - * a 64bit value.
> - */
> -static enum ice_status
> -ice_write_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr,
> u64 val) -{
> -	enum ice_status status;
> -	u32 low, high;
> -	u16 high_addr;
> -
> -	/* Only operate on registers known to be split into two 32bit
> -	 * registers.
> -	 */
> -	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
> -		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr
> 0x%08x\n",
> -			  low_addr);
> -		return ICE_ERR_PARAM;
> -	}
> -
> -	low = ICE_LO_DWORD(val);
> -	high = ICE_HI_DWORD(val);
> -
> -	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register
> 0x%08x\n, status %d",
> -			  low_addr, status);
> -		return status;
> -	}
> -
> -	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high
> register 0x%08x\n, status %d",
> -			  high_addr, status);
> -		return status;
> -	}
> -
> -	return ICE_SUCCESS;
> -}
> -
> -/**
> - * ice_fill_quad_msg_e822 - Fill message data for quad register access
> - * @msg: the PHY message buffer to fill in
> - * @quad: the quad to access
> - * @offset: the register offset
> - *
> - * Fill a message buffer for accessing a register in a quad shared between
> - * multiple PHYs.
> - */
> -static void
> -ice_fill_quad_msg_e822(struct ice_sbq_msg_input *msg, u8 quad, u16
> offset) -{
> -	u32 addr;
> -
> -	msg->dest_dev = rmn_0;
> -
> -	if ((quad % ICE_NUM_QUAD_TYPE) == 0)
> -		addr = Q_0_BASE + offset;
> -	else
> -		addr = Q_1_BASE + offset;
> -
> -	msg->msg_addr_low = ICE_LO_WORD(addr);
> -	msg->msg_addr_high = ICE_HI_WORD(addr);
> -}
> -
> -/**
> - * ice_read_quad_reg_e822_lp - Read a PHY quad register
> - * @hw: pointer to the HW struct
> - * @quad: quad to read from
> - * @offset: quad register offset to read
> - * @val: on return, the contents read from the quad
> - * @lock_sbq: true if the sideband queue lock must be acquired
> - *
> - * Read a quad register over the device sideband queue. Quad registers are
> - * shared between multiple PHYs.
> - */
> -static enum ice_status
> -ice_read_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32
> *val,
> -			  bool lock_sbq)
> +static enum ice_status
> +ice_read_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr,
> u64
> +*val)
>  {
> -	struct ice_sbq_msg_input msg = {0};
>  	enum ice_status status;
> +	u32 low, high;
> +	u16 high_addr;
> 
> -	if (quad >= ICE_MAX_QUAD)
> +	/* Only operate on registers known to be split into two 32bit
> +	 * registers.
> +	 */
> +	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
> +		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr
> 0x%08x\n",
> +			  low_addr);
>  		return ICE_ERR_PARAM;
> +	}
> 
> -	ice_fill_quad_msg_e822(&msg, quad, offset);
> -	msg.opcode = ice_sbq_msg_rd;
> +	status = ice_read_phy_reg_e822(hw, port, low_addr, &low);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to read from low
> register 0x%08x\n, status %d",
> +			  low_addr, status);
> +		return status;
> +	}
> 
> -	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
> +	status = ice_read_phy_reg_e822(hw, port, high_addr, &high);
>  	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to
> phy, status %d\n",
> -			  status);
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to read from high
> register 0x%08x\n, status %d",
> +			  high_addr, status);
>  		return status;
>  	}
> 
> -	*val = msg.data;
> +	*val = (u64)high << 32 | low;
> 
>  	return ICE_SUCCESS;
>  }
> 
> -enum ice_status
> -ice_read_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32 *val)
> -{
> -	return ice_read_quad_reg_e822_lp(hw, quad, offset, val, true);
> -}
> -
>  /**
> - * ice_write_quad_reg_e822_lp - Write a PHY quad register
> + * ice_write_phy_reg_e822_lp - Write a PHY register
>   * @hw: pointer to the HW struct
> - * @quad: quad to write to
> - * @offset: quad register offset to write
> + * @port: PHY port to write to
> + * @offset: PHY register offset to write
>   * @val: The value to write to the register
>   * @lock_sbq: true if the sideband queue lock must be acquired
>   *
> - * Write a quad register over the device sideband queue. Quad registers
> are
> - * shared between multiple PHYs.
> + * Write a PHY register for the given port over the device sideband queue.
>   */
>  static enum ice_status
> -ice_write_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32
> val,
> -			   bool lock_sbq)
> +ice_write_phy_reg_e822_lp(struct ice_hw *hw, u8 port, u16 offset, u32 val,
> +			  bool lock_sbq)
>  {
>  	struct ice_sbq_msg_input msg = {0};
>  	enum ice_status status;
> 
> -	if (quad >= ICE_MAX_QUAD)
> -		return ICE_ERR_PARAM;
> -
> -	ice_fill_quad_msg_e822(&msg, quad, offset);
> +	ice_fill_phy_msg_e822(&msg, port, offset);
>  	msg.opcode = ice_sbq_msg_wr;
>  	msg.data = val;
> 
> @@ -615,84 +693,51 @@ ice_write_quad_reg_e822_lp(struct ice_hw *hw,
> u8 quad, u16 offset, u32 val,  }
> 
>  enum ice_status
> -ice_write_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32 val)
> +ice_write_phy_reg_e822(struct ice_hw *hw, u8 port, u16 offset, u32 val)
>  {
> -	return ice_write_quad_reg_e822_lp(hw, quad, offset, val, true);
> +	return ice_write_phy_reg_e822_lp(hw, port, offset, val, true);
>  }
> 
>  /**
> - * ice_read_phy_tstamp_e822 - Read a PHY timestamp out of the quad
> block
> + * ice_write_40b_phy_reg_e822 - Write a 40b value to the PHY
>   * @hw: pointer to the HW struct
> - * @quad: the quad to read from
> - * @idx: the timestamp index to read
> - * @tstamp: on return, the 40bit timestamp value
> + * @port: port to write to
> + * @low_addr: offset of the low register
> + * @val: 40b value to write
>   *
> - * Read a 40bit timestamp value out of the two associated registers in the
> - * quad memory block that is shared between the internal PHYs of the
> E822
> - * family of devices.
> + * Write the provided 40b value to the two associated registers by
> + splitting
> + * it up into two chunks, the lower 8 bits and the upper 32 bits.
>   */
>  static enum ice_status
> -ice_read_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx, u64
> *tstamp)
> +ice_write_40b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr,
> +u64 val)
>  {
>  	enum ice_status status;
> -	u16 lo_addr, hi_addr;
> -	u32 lo, hi;
> -
> -	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
> -	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
> -
> -	status = ice_read_quad_reg_e822(hw, quad, lo_addr, &lo);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to read low PTP
> timestamp register, status %d\n",
> -			  status);
> -		return status;
> -	}
> -
> -	status = ice_read_quad_reg_e822(hw, quad, hi_addr, &hi);
> -	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to read high PTP
> timestamp register, status %d\n",
> -			  status);
> -		return status;
> -	}
> +	u32 low, high;
> +	u16 high_addr;
> 
> -	/* For E822 based internal PHYs, the timestamp is reported with the
> -	 * lower 8 bits in the low register, and the upper 32 bits in the high
> -	 * register.
> +	/* Only operate on registers known to be split into a lower 8 bit
> +	 * register and an upper 32 bit register.
>  	 */
> -	*tstamp = ((u64)hi) << TS_PHY_HIGH_S | ((u64)lo &
> TS_PHY_LOW_M);
> -
> -	return ICE_SUCCESS;
> -}
> -
> -/**
> - * ice_clear_phy_tstamp_e822 - Clear a timestamp from the quad block
> - * @hw: pointer to the HW struct
> - * @quad: the quad to read from
> - * @idx: the timestamp index to reset
> - *
> - * Clear a timestamp, resetting its valid bit, from the PHY quad block that is
> - * shared between the internal PHYs on the E822 devices.
> - */
> -static enum ice_status
> -ice_clear_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx) -{
> -	enum ice_status status;
> -	u16 lo_addr, hi_addr;
> +	if (!ice_is_40b_phy_reg_e822(low_addr, &high_addr)) {
> +		ice_debug(hw, ICE_DBG_PTP, "Invalid 40b register addr
> 0x%08x\n",
> +			  low_addr);
> +		return ICE_ERR_PARAM;
> +	}
> 
> -	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
> -	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
> +	low = (u32)(val & P_REG_40B_LOW_M);
> +	high = (u32)(val >> P_REG_40B_HIGH_S);
> 
> -	status = ice_write_quad_reg_e822(hw, quad, lo_addr, 0);
> +	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
>  	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to clear low PTP
> timestamp register, status %d\n",
> -			  status);
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register
> 0x%08x\n, status %d",
> +			  low_addr, status);
>  		return status;
>  	}
> 
> -	status = ice_write_quad_reg_e822(hw, quad, hi_addr, 0);
> +	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
>  	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to clear high PTP
> timestamp register, status %d\n",
> -			  status);
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high
> register 0x%08x\n, status %d",
> +			  high_addr, status);
>  		return status;
>  	}
> 
> @@ -700,282 +745,236 @@ ice_clear_phy_tstamp_e822(struct ice_hw *hw,
> u8 quad, u8 idx)  }
> 
>  /**
> - * ice_read_cgu_reg_e822 - Read a CGU register
> + * ice_write_64b_phy_reg_e822 - Write a 64bit value to PHY registers
>   * @hw: pointer to the HW struct
> - * @addr: Register address to read
> - * @val: storage for register value read
> + * @port: PHY port to read from
> + * @low_addr: offset of the lower register to read from
> + * @val: the contents of the 64bit value to write to PHY
>   *
> - * Read the contents of a register of the Clock Generation Unit. Only
> - * applicable to E822 devices.
> + * Write the 64bit value to the two associated 32bit PHY registers. The
> + offset
> + * is always specified as the lower register, and the high address is
> + looked
> + * up. This function only operates on registers known to be two parts
> + of
> + * a 64bit value.
>   */
>  static enum ice_status
> -ice_read_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 *val)
> +ice_write_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr,
> +u64 val)
>  {
> -	struct ice_sbq_msg_input cgu_msg;
>  	enum ice_status status;
> +	u32 low, high;
> +	u16 high_addr;
> 
> -	cgu_msg.opcode = ice_sbq_msg_rd;
> -	cgu_msg.dest_dev = cgu;
> -	cgu_msg.msg_addr_low = addr;
> -	cgu_msg.msg_addr_high = 0x0;
> +	/* Only operate on registers known to be split into two 32bit
> +	 * registers.
> +	 */
> +	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
> +		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr
> 0x%08x\n",
> +			  low_addr);
> +		return ICE_ERR_PARAM;
> +	}
> 
> -	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
> +	low = ICE_LO_DWORD(val);
> +	high = ICE_HI_DWORD(val);
> +
> +	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
>  	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to read CGU register
> 0x%04x, status %d\n",
> -			  addr, status);
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register
> 0x%08x\n, status %d",
> +			  low_addr, status);
>  		return status;
>  	}
> 
> -	*val = cgu_msg.data;
> -
> -	return status;
> -}
> -
> -/**
> - * ice_write_cgu_reg_e822 - Write a CGU register
> - * @hw: pointer to the HW struct
> - * @addr: Register address to write
> - * @val: value to write into the register
> - *
> - * Write the specified value to a register of the Clock Generation Unit. Only
> - * applicable to E822 devices.
> - */
> -static enum ice_status
> -ice_write_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 val) -{
> -	struct ice_sbq_msg_input cgu_msg;
> -	enum ice_status status;
> -
> -	cgu_msg.opcode = ice_sbq_msg_wr;
> -	cgu_msg.dest_dev = cgu;
> -	cgu_msg.msg_addr_low = addr;
> -	cgu_msg.msg_addr_high = 0x0;
> -	cgu_msg.data = val;
> -
> -	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
> +	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
>  	if (status) {
> -		ice_debug(hw, ICE_DBG_PTP, "Failed to write CGU register
> 0x%04x, status %d\n",
> -			  addr, status);
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high
> register 0x%08x\n, status %d",
> +			  high_addr, status);
>  		return status;
>  	}
> 
> -	return status;
> -}
> -
> -/**
> - * ice_clk_freq_str - Convert time_ref_freq to string
> - * @clk_freq: Clock frequency
> - *
> - * Convert the specified TIME_REF clock frequency to a string.
> - */
> -static const char *ice_clk_freq_str(u8 clk_freq) -{
> -	switch ((enum ice_time_ref_freq)clk_freq) {
> -	case ICE_TIME_REF_FREQ_25_000:
> -		return "25 MHz";
> -	case ICE_TIME_REF_FREQ_122_880:
> -		return "122.88 MHz";
> -	case ICE_TIME_REF_FREQ_125_000:
> -		return "125 MHz";
> -	case ICE_TIME_REF_FREQ_153_600:
> -		return "153.6 MHz";
> -	case ICE_TIME_REF_FREQ_156_250:
> -		return "156.25 MHz";
> -	case ICE_TIME_REF_FREQ_245_760:
> -		return "245.76 MHz";
> -	default:
> -		return "Unknown";
> -	}
> +	return ICE_SUCCESS;
>  }
> 
>  /**
> - * ice_clk_src_str - Convert time_ref_src to string
> - * @clk_src: Clock source
> + * ice_fill_quad_msg_e822 - Fill message data for quad register access
> + * @msg: the PHY message buffer to fill in
> + * @quad: the quad to access
> + * @offset: the register offset
>   *
> - * Convert the specified clock source to its string name.
> - */
> -static const char *ice_clk_src_str(u8 clk_src) -{
> -	switch ((enum ice_clk_src)clk_src) {
> -	case ICE_CLK_SRC_TCX0:
> -		return "TCX0";
> -	case ICE_CLK_SRC_TIME_REF:
> -		return "TIME_REF";
> -	default:
> -		return "Unknown";
> -	}
> + * Fill a message buffer for accessing a register in a quad shared
> +between
> + * multiple PHYs.
> + */
> +static void
> +ice_fill_quad_msg_e822(struct ice_sbq_msg_input *msg, u8 quad, u16
> +offset) {
> +	u32 addr;
> +
> +	msg->dest_dev = rmn_0;
> +
> +	if ((quad % ICE_NUM_QUAD_TYPE) == 0)
> +		addr = Q_0_BASE + offset;
> +	else
> +		addr = Q_1_BASE + offset;
> +
> +	msg->msg_addr_low = ICE_LO_WORD(addr);
> +	msg->msg_addr_high = ICE_HI_WORD(addr);
>  }
> 
>  /**
> - * ice_cfg_cgu_pll_e822 - Configure the Clock Generation Unit
> + * ice_read_quad_reg_e822_lp - Read a PHY quad register
>   * @hw: pointer to the HW struct
> - * @clk_freq: Clock frequency to program
> - * @clk_src: Clock source to select (TIME_REF, or TCX0)
> + * @quad: quad to read from
> + * @offset: quad register offset to read
> + * @val: on return, the contents read from the quad
> + * @lock_sbq: true if the sideband queue lock must be acquired
>   *
> - * Configure the Clock Generation Unit with the desired clock frequency
> and
> - * time reference, enabling the PLL which drives the PTP hardware clock.
> + * Read a quad register over the device sideband queue. Quad registers
> + are
> + * shared between multiple PHYs.
>   */
> -enum ice_status
> -ice_cfg_cgu_pll_e822(struct ice_hw *hw, enum ice_time_ref_freq clk_freq,
> -		     enum ice_clk_src clk_src)
> +static enum ice_status
> +ice_read_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32
> *val,
> +			  bool lock_sbq)
>  {
> -	union tspll_ro_bwm_lf bwm_lf;
> -	union nac_cgu_dword19 dw19;
> -	union nac_cgu_dword22 dw22;
> -	union nac_cgu_dword24 dw24;
> -	union nac_cgu_dword9 dw9;
> +	struct ice_sbq_msg_input msg = {0};
>  	enum ice_status status;
> 
> -	if (clk_freq >= NUM_ICE_TIME_REF_FREQ) {
> -		ice_warn(hw, "Invalid TIME_REF frequency %u\n", clk_freq);
> -		return ICE_ERR_PARAM;
> -	}
> -
> -	if (clk_src >= NUM_ICE_CLK_SRC) {
> -		ice_warn(hw, "Invalid clock source %u\n", clk_src);
> -		return ICE_ERR_PARAM;
> -	}
> -
> -	if (clk_src == ICE_CLK_SRC_TCX0 &&
> -	    clk_freq != ICE_TIME_REF_FREQ_25_000) {
> -		ice_warn(hw, "TCX0 only supports 25 MHz frequency\n");
> +	if (quad >= ICE_MAX_QUAD)
>  		return ICE_ERR_PARAM;
> -	}
> -
> -	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD9, &dw9.val);
> -	if (status)
> -		return status;
> 
> -	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> &dw24.val);
> -	if (status)
> -		return status;
> +	ice_fill_quad_msg_e822(&msg, quad, offset);
> +	msg.opcode = ice_sbq_msg_rd;
> 
> -	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF,
> &bwm_lf.val);
> -	if (status)
> +	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to
> phy, status %d\n",
> +			  status);
>  		return status;
> -
> -	/* Log the current clock configuration */
> -	ice_debug(hw, ICE_DBG_PTP, "Current CGU configuration -- %s,
> clk_src %s, clk_freq %s, PLL %s\n",
> -		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
> -		  ice_clk_src_str(dw24.field.time_ref_sel),
> -		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
> -		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
> -
> -	/* Disable the PLL before changing the clock source or frequency */
> -	if (dw24.field.ts_pll_enable) {
> -		dw24.field.ts_pll_enable = 0;
> -
> -		status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> dw24.val);
> -		if (status)
> -			return status;
>  	}
> 
> -	/* Set the frequency */
> -	dw9.field.time_ref_freq_sel = clk_freq;
> -	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD9, dw9.val);
> -	if (status)
> -		return status;
> +	*val = msg.data;
> 
> -	/* Configure the TS PLL feedback divisor */
> -	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD19,
> &dw19.val);
> -	if (status)
> -		return status;
> +	return ICE_SUCCESS;
> +}
> 
> -	dw19.field.tspll_fbdiv_intgr =
> e822_cgu_params[clk_freq].feedback_div;
> -	dw19.field.tspll_ndivratio = 1;
> +enum ice_status
> +ice_read_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32
> +*val) {
> +	return ice_read_quad_reg_e822_lp(hw, quad, offset, val, true); }
> 
> -	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD19,
> dw19.val);
> -	if (status)
> -		return status;
> +/**
> + * ice_write_quad_reg_e822_lp - Write a PHY quad register
> + * @hw: pointer to the HW struct
> + * @quad: quad to write to
> + * @offset: quad register offset to write
> + * @val: The value to write to the register
> + * @lock_sbq: true if the sideband queue lock must be acquired
> + *
> + * Write a quad register over the device sideband queue. Quad registers
> +are
> + * shared between multiple PHYs.
> + */
> +static enum ice_status
> +ice_write_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32
> val,
> +			   bool lock_sbq)
> +{
> +	struct ice_sbq_msg_input msg = {0};
> +	enum ice_status status;
> 
> -	/* Configure the TS PLL post divisor */
> -	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD22,
> &dw22.val);
> -	if (status)
> -		return status;
> +	if (quad >= ICE_MAX_QUAD)
> +		return ICE_ERR_PARAM;
> 
> -	dw22.field.time1588clk_div =
> e822_cgu_params[clk_freq].post_pll_div;
> -	dw22.field.time1588clk_sel_div2 = 0;
> +	ice_fill_quad_msg_e822(&msg, quad, offset);
> +	msg.opcode = ice_sbq_msg_wr;
> +	msg.data = val;
> 
> -	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD22,
> dw22.val);
> -	if (status)
> +	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to
> phy, status %d\n",
> +			  status);
>  		return status;
> +	}
> 
> -	/* Configure the TS PLL pre divisor and clock source */
> -	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> &dw24.val);
> -	if (status)
> -		return status;
> +	return ICE_SUCCESS;
> +}
> 
> -	dw24.field.ref1588_ck_div =
> e822_cgu_params[clk_freq].refclk_pre_div;
> -	dw24.field.tspll_fbdiv_frac = e822_cgu_params[clk_freq].frac_n_div;
> -	dw24.field.time_ref_sel = clk_src;
> +enum ice_status
> +ice_write_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32
> +val) {
> +	return ice_write_quad_reg_e822_lp(hw, quad, offset, val, true); }
> 
> -	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> dw24.val);
> -	if (status)
> -		return status;
> +/**
> + * ice_read_phy_tstamp_e822 - Read a PHY timestamp out of the quad
> +block
> + * @hw: pointer to the HW struct
> + * @quad: the quad to read from
> + * @idx: the timestamp index to read
> + * @tstamp: on return, the 40bit timestamp value
> + *
> + * Read a 40bit timestamp value out of the two associated registers in
> +the
> + * quad memory block that is shared between the internal PHYs of the
> +E822
> + * family of devices.
> + */
> +static enum ice_status
> +ice_read_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx, u64
> +*tstamp) {
> +	enum ice_status status;
> +	u16 lo_addr, hi_addr;
> +	u32 lo, hi;
> 
> -	/* Finally, enable the PLL */
> -	dw24.field.ts_pll_enable = 1;
> +	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
> +	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
> 
> -	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24,
> dw24.val);
> -	if (status)
> +	status = ice_read_quad_reg_e822(hw, quad, lo_addr, &lo);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to read low PTP
> timestamp register, status %d\n",
> +			  status);
>  		return status;
> +	}
> 
> -	/* Wait to verify if the PLL locks */
> -	ice_msec_delay(1, true);
> -
> -	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF,
> &bwm_lf.val);
> -	if (status)
> +	status = ice_read_quad_reg_e822(hw, quad, hi_addr, &hi);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to read high PTP
> timestamp register, status %d\n",
> +			  status);
>  		return status;
> -
> -	if (!bwm_lf.field.plllock_true_lock_cri) {
> -		ice_warn(hw, "CGU PLL failed to lock\n");
> -		return ICE_ERR_NOT_READY;
>  	}
> 
> -	/* Log the current clock configuration */
> -	ice_debug(hw, ICE_DBG_PTP, "New CGU configuration -- %s,
> clk_src %s, clk_freq %s, PLL %s\n",
> -		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
> -		  ice_clk_src_str(dw24.field.time_ref_sel),
> -		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
> -		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
> -
> +	/* For E822 based internal PHYs, the timestamp is reported with the
> +	 * lower 8 bits in the low register, and the upper 32 bits in the high
> +	 * register.
> +	 */
> +	*tstamp = ((u64)hi) << TS_PHY_HIGH_S | ((u64)lo &
> TS_PHY_LOW_M);
> 
>  	return ICE_SUCCESS;
>  }
> 
>  /**
> - * ice_init_cgu_e822 - Initialize CGU with settings from firmware
> - * @hw: pointer to the HW structure
> + * ice_clear_phy_tstamp_e822 - Clear a timestamp from the quad block
> + * @hw: pointer to the HW struct
> + * @quad: the quad to read from
> + * @idx: the timestamp index to reset
>   *
> - * Initialize the Clock Generation Unit of the E822 device.
> + * Clear a timestamp, resetting its valid bit, from the PHY quad block
> + that is
> + * shared between the internal PHYs on the E822 devices.
>   */
> -static enum ice_status ice_init_cgu_e822(struct ice_hw *hw)
> +static enum ice_status
> +ice_clear_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx)
>  {
> -	struct ice_ts_func_info *ts_info = &hw->func_caps.ts_func_info;
> -	union tspll_cntr_bist_settings cntr_bist;
>  	enum ice_status status;
> +	u16 lo_addr, hi_addr;
> 
> -	status = ice_read_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
> -				       &cntr_bist.val);
> -	if (status)
> -		return status;
> -
> -	/* Disable sticky lock detection so lock status reported is accurate */
> -	cntr_bist.field.i_plllock_sel_0 = 0;
> -	cntr_bist.field.i_plllock_sel_1 = 0;
> +	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
> +	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
> 
> -	status = ice_write_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
> -					cntr_bist.val);
> -	if (status)
> +	status = ice_write_quad_reg_e822(hw, quad, lo_addr, 0);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to clear low PTP
> timestamp register, status %d\n",
> +			  status);
>  		return status;
> +	}
> 
> -	/* Configure the CGU PLL using the parameters from the function
> -	 * capabilities.
> -	 */
> -	status = ice_cfg_cgu_pll_e822(hw, ts_info->time_ref,
> -				      (enum ice_clk_src)ts_info->clk_src);
> -	if (status)
> +	status = ice_write_quad_reg_e822(hw, quad, hi_addr, 0);
> +	if (status) {
> +		ice_debug(hw, ICE_DBG_PTP, "Failed to clear high PTP
> timestamp register, status %d\n",
> +			  status);
>  		return status;
> +	}
> 
>  	return ICE_SUCCESS;
>  }
> --
> 2.31.1

12/70-19/70
Acked-by: Qiming Yang <qiming.yang@intel.com>
diff mbox series

Patch

diff --git a/drivers/net/ice/base/ice_ptp_hw.c b/drivers/net/ice/base/ice_ptp_hw.c
index 23d90b127d..22d0774dd7 100644
--- a/drivers/net/ice/base/ice_ptp_hw.c
+++ b/drivers/net/ice/base/ice_ptp_hw.c
@@ -101,6 +101,286 @@  u64 ice_ptp_read_src_incval(struct ice_hw *hw)
 	return ((u64)(hi & INCVAL_HIGH_M) << 32) | lo;
 }
 
+/**
+ * ice_read_cgu_reg_e822 - Read a CGU register
+ * @hw: pointer to the HW struct
+ * @addr: Register address to read
+ * @val: storage for register value read
+ *
+ * Read the contents of a register of the Clock Generation Unit. Only
+ * applicable to E822 devices.
+ */
+static enum ice_status
+ice_read_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 *val)
+{
+	struct ice_sbq_msg_input cgu_msg;
+	enum ice_status status;
+
+	cgu_msg.opcode = ice_sbq_msg_rd;
+	cgu_msg.dest_dev = cgu;
+	cgu_msg.msg_addr_low = addr;
+	cgu_msg.msg_addr_high = 0x0;
+
+	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to read CGU register 0x%04x, status %d\n",
+			  addr, status);
+		return status;
+	}
+
+	*val = cgu_msg.data;
+
+	return ICE_SUCCESS;
+}
+
+/**
+ * ice_write_cgu_reg_e822 - Write a CGU register
+ * @hw: pointer to the HW struct
+ * @addr: Register address to write
+ * @val: value to write into the register
+ *
+ * Write the specified value to a register of the Clock Generation Unit. Only
+ * applicable to E822 devices.
+ */
+static enum ice_status
+ice_write_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 val)
+{
+	struct ice_sbq_msg_input cgu_msg;
+	enum ice_status status;
+
+	cgu_msg.opcode = ice_sbq_msg_wr;
+	cgu_msg.dest_dev = cgu;
+	cgu_msg.msg_addr_low = addr;
+	cgu_msg.msg_addr_high = 0x0;
+	cgu_msg.data = val;
+
+	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to write CGU register 0x%04x, status %d\n",
+			  addr, status);
+		return status;
+	}
+
+	return ICE_SUCCESS;
+}
+
+/**
+ * ice_clk_freq_str - Convert time_ref_freq to string
+ * @clk_freq: Clock frequency
+ *
+ * Convert the specified TIME_REF clock frequency to a string.
+ */
+static const char *ice_clk_freq_str(u8 clk_freq)
+{
+	switch ((enum ice_time_ref_freq)clk_freq) {
+	case ICE_TIME_REF_FREQ_25_000:
+		return "25 MHz";
+	case ICE_TIME_REF_FREQ_122_880:
+		return "122.88 MHz";
+	case ICE_TIME_REF_FREQ_125_000:
+		return "125 MHz";
+	case ICE_TIME_REF_FREQ_153_600:
+		return "153.6 MHz";
+	case ICE_TIME_REF_FREQ_156_250:
+		return "156.25 MHz";
+	case ICE_TIME_REF_FREQ_245_760:
+		return "245.76 MHz";
+	default:
+		return "Unknown";
+	}
+}
+
+/**
+ * ice_clk_src_str - Convert time_ref_src to string
+ * @clk_src: Clock source
+ *
+ * Convert the specified clock source to its string name.
+ */
+static const char *ice_clk_src_str(u8 clk_src)
+{
+	switch ((enum ice_clk_src)clk_src) {
+	case ICE_CLK_SRC_TCX0:
+		return "TCX0";
+	case ICE_CLK_SRC_TIME_REF:
+		return "TIME_REF";
+	default:
+		return "Unknown";
+	}
+}
+
+/**
+ * ice_cfg_cgu_pll_e822 - Configure the Clock Generation Unit
+ * @hw: pointer to the HW struct
+ * @clk_freq: Clock frequency to program
+ * @clk_src: Clock source to select (TIME_REF, or TCX0)
+ *
+ * Configure the Clock Generation Unit with the desired clock frequency and
+ * time reference, enabling the PLL which drives the PTP hardware clock.
+ */
+enum ice_status
+ice_cfg_cgu_pll_e822(struct ice_hw *hw, enum ice_time_ref_freq clk_freq,
+		     enum ice_clk_src clk_src)
+{
+	union tspll_ro_bwm_lf bwm_lf;
+	union nac_cgu_dword19 dw19;
+	union nac_cgu_dword22 dw22;
+	union nac_cgu_dword24 dw24;
+	union nac_cgu_dword9 dw9;
+	enum ice_status status;
+
+	if (clk_freq >= NUM_ICE_TIME_REF_FREQ) {
+		ice_warn(hw, "Invalid TIME_REF frequency %u\n", clk_freq);
+		return ICE_ERR_PARAM;
+	}
+
+	if (clk_src >= NUM_ICE_CLK_SRC) {
+		ice_warn(hw, "Invalid clock source %u\n", clk_src);
+		return ICE_ERR_PARAM;
+	}
+
+	if (clk_src == ICE_CLK_SRC_TCX0 &&
+	    clk_freq != ICE_TIME_REF_FREQ_25_000) {
+		ice_warn(hw, "TCX0 only supports 25 MHz frequency\n");
+		return ICE_ERR_PARAM;
+	}
+
+	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD9, &dw9.val);
+	if (status)
+		return status;
+
+	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24, &dw24.val);
+	if (status)
+		return status;
+
+	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF, &bwm_lf.val);
+	if (status)
+		return status;
+
+	/* Log the current clock configuration */
+	ice_debug(hw, ICE_DBG_PTP, "Current CGU configuration -- %s, clk_src %s, clk_freq %s, PLL %s\n",
+		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
+		  ice_clk_src_str(dw24.field.time_ref_sel),
+		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
+		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
+
+	/* Disable the PLL before changing the clock source or frequency */
+	if (dw24.field.ts_pll_enable) {
+		dw24.field.ts_pll_enable = 0;
+
+		status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24, dw24.val);
+		if (status)
+			return status;
+	}
+
+	/* Set the frequency */
+	dw9.field.time_ref_freq_sel = clk_freq;
+	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD9, dw9.val);
+	if (status)
+		return status;
+
+	/* Configure the TS PLL feedback divisor */
+	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD19, &dw19.val);
+	if (status)
+		return status;
+
+	dw19.field.tspll_fbdiv_intgr = e822_cgu_params[clk_freq].feedback_div;
+	dw19.field.tspll_ndivratio = 1;
+
+	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD19, dw19.val);
+	if (status)
+		return status;
+
+	/* Configure the TS PLL post divisor */
+	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD22, &dw22.val);
+	if (status)
+		return status;
+
+	dw22.field.time1588clk_div = e822_cgu_params[clk_freq].post_pll_div;
+	dw22.field.time1588clk_sel_div2 = 0;
+
+	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD22, dw22.val);
+	if (status)
+		return status;
+
+	/* Configure the TS PLL pre divisor and clock source */
+	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24, &dw24.val);
+	if (status)
+		return status;
+
+	dw24.field.ref1588_ck_div = e822_cgu_params[clk_freq].refclk_pre_div;
+	dw24.field.tspll_fbdiv_frac = e822_cgu_params[clk_freq].frac_n_div;
+	dw24.field.time_ref_sel = clk_src;
+
+	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24, dw24.val);
+	if (status)
+		return status;
+
+	/* Finally, enable the PLL */
+	dw24.field.ts_pll_enable = 1;
+
+	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24, dw24.val);
+	if (status)
+		return status;
+
+	/* Wait to verify if the PLL locks */
+	ice_msec_delay(1, true);
+
+	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF, &bwm_lf.val);
+	if (status)
+		return status;
+
+	if (!bwm_lf.field.plllock_true_lock_cri) {
+		ice_warn(hw, "CGU PLL failed to lock\n");
+		return ICE_ERR_NOT_READY;
+	}
+
+	/* Log the current clock configuration */
+	ice_debug(hw, ICE_DBG_PTP, "New CGU configuration -- %s, clk_src %s, clk_freq %s, PLL %s\n",
+		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
+		  ice_clk_src_str(dw24.field.time_ref_sel),
+		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
+		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
+
+	return ICE_SUCCESS;
+}
+
+/**
+ * ice_init_cgu_e822 - Initialize CGU with settings from firmware
+ * @hw: pointer to the HW structure
+ *
+ * Initialize the Clock Generation Unit of the E822 device.
+ */
+static enum ice_status ice_init_cgu_e822(struct ice_hw *hw)
+{
+	struct ice_ts_func_info *ts_info = &hw->func_caps.ts_func_info;
+	union tspll_cntr_bist_settings cntr_bist;
+	enum ice_status status;
+
+	status = ice_read_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
+				       &cntr_bist.val);
+	if (status)
+		return status;
+
+	/* Disable sticky lock detection so lock status reported is accurate */
+	cntr_bist.field.i_plllock_sel_0 = 0;
+	cntr_bist.field.i_plllock_sel_1 = 0;
+
+	status = ice_write_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
+					cntr_bist.val);
+	if (status)
+		return status;
+
+	/* Configure the CGU PLL using the parameters from the function
+	 * capabilities.
+	 */
+	status = ice_cfg_cgu_pll_e822(hw, ts_info->time_ref,
+				      (enum ice_clk_src)ts_info->clk_src);
+	if (status)
+		return status;
+
+	return ICE_SUCCESS;
+}
+
 /**
  * ice_ptp_exec_tmr_cmd - Execute all prepared timer commands
  * @hw: pointer to HW struct
@@ -346,261 +626,59 @@  ice_read_40b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 *val)
  * The high offset is looked up. This function only operates on registers
  * known to be two parts of a 64bit value.
  */
-static enum ice_status
-ice_read_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 *val)
-{
-	enum ice_status status;
-	u32 low, high;
-	u16 high_addr;
-
-	/* Only operate on registers known to be split into two 32bit
-	 * registers.
-	 */
-	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
-		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr 0x%08x\n",
-			  low_addr);
-		return ICE_ERR_PARAM;
-	}
-
-	status = ice_read_phy_reg_e822(hw, port, low_addr, &low);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to read from low register 0x%08x\n, status %d",
-			  low_addr, status);
-		return status;
-	}
-
-	status = ice_read_phy_reg_e822(hw, port, high_addr, &high);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to read from high register 0x%08x\n, status %d",
-			  high_addr, status);
-		return status;
-	}
-
-	*val = (u64)high << 32 | low;
-
-	return ICE_SUCCESS;
-}
-
-/**
- * ice_write_phy_reg_e822_lp - Write a PHY register
- * @hw: pointer to the HW struct
- * @port: PHY port to write to
- * @offset: PHY register offset to write
- * @val: The value to write to the register
- * @lock_sbq: true if the sideband queue lock must be acquired
- *
- * Write a PHY register for the given port over the device sideband queue.
- */
-static enum ice_status
-ice_write_phy_reg_e822_lp(struct ice_hw *hw, u8 port, u16 offset, u32 val,
-			  bool lock_sbq)
-{
-	struct ice_sbq_msg_input msg = {0};
-	enum ice_status status;
-
-	ice_fill_phy_msg_e822(&msg, port, offset);
-	msg.opcode = ice_sbq_msg_wr;
-	msg.data = val;
-
-	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to phy, status %d\n",
-			  status);
-		return status;
-	}
-
-	return ICE_SUCCESS;
-}
-
-enum ice_status
-ice_write_phy_reg_e822(struct ice_hw *hw, u8 port, u16 offset, u32 val)
-{
-	return ice_write_phy_reg_e822_lp(hw, port, offset, val, true);
-}
-
-/**
- * ice_write_40b_phy_reg_e822 - Write a 40b value to the PHY
- * @hw: pointer to the HW struct
- * @port: port to write to
- * @low_addr: offset of the low register
- * @val: 40b value to write
- *
- * Write the provided 40b value to the two associated registers by splitting
- * it up into two chunks, the lower 8 bits and the upper 32 bits.
- */
-static enum ice_status
-ice_write_40b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 val)
-{
-	enum ice_status status;
-	u32 low, high;
-	u16 high_addr;
-
-	/* Only operate on registers known to be split into a lower 8 bit
-	 * register and an upper 32 bit register.
-	 */
-	if (!ice_is_40b_phy_reg_e822(low_addr, &high_addr)) {
-		ice_debug(hw, ICE_DBG_PTP, "Invalid 40b register addr 0x%08x\n",
-			  low_addr);
-		return ICE_ERR_PARAM;
-	}
-
-	low = (u32)(val & P_REG_40B_LOW_M);
-	high = (u32)(val >> P_REG_40B_HIGH_S);
-
-	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register 0x%08x\n, status %d",
-			  low_addr, status);
-		return status;
-	}
-
-	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high register 0x%08x\n, status %d",
-			  high_addr, status);
-		return status;
-	}
-
-	return ICE_SUCCESS;
-}
-
-/**
- * ice_write_64b_phy_reg_e822 - Write a 64bit value to PHY registers
- * @hw: pointer to the HW struct
- * @port: PHY port to read from
- * @low_addr: offset of the lower register to read from
- * @val: the contents of the 64bit value to write to PHY
- *
- * Write the 64bit value to the two associated 32bit PHY registers. The offset
- * is always specified as the lower register, and the high address is looked
- * up. This function only operates on registers known to be two parts of
- * a 64bit value.
- */
-static enum ice_status
-ice_write_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 val)
-{
-	enum ice_status status;
-	u32 low, high;
-	u16 high_addr;
-
-	/* Only operate on registers known to be split into two 32bit
-	 * registers.
-	 */
-	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
-		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr 0x%08x\n",
-			  low_addr);
-		return ICE_ERR_PARAM;
-	}
-
-	low = ICE_LO_DWORD(val);
-	high = ICE_HI_DWORD(val);
-
-	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register 0x%08x\n, status %d",
-			  low_addr, status);
-		return status;
-	}
-
-	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high register 0x%08x\n, status %d",
-			  high_addr, status);
-		return status;
-	}
-
-	return ICE_SUCCESS;
-}
-
-/**
- * ice_fill_quad_msg_e822 - Fill message data for quad register access
- * @msg: the PHY message buffer to fill in
- * @quad: the quad to access
- * @offset: the register offset
- *
- * Fill a message buffer for accessing a register in a quad shared between
- * multiple PHYs.
- */
-static void
-ice_fill_quad_msg_e822(struct ice_sbq_msg_input *msg, u8 quad, u16 offset)
-{
-	u32 addr;
-
-	msg->dest_dev = rmn_0;
-
-	if ((quad % ICE_NUM_QUAD_TYPE) == 0)
-		addr = Q_0_BASE + offset;
-	else
-		addr = Q_1_BASE + offset;
-
-	msg->msg_addr_low = ICE_LO_WORD(addr);
-	msg->msg_addr_high = ICE_HI_WORD(addr);
-}
-
-/**
- * ice_read_quad_reg_e822_lp - Read a PHY quad register
- * @hw: pointer to the HW struct
- * @quad: quad to read from
- * @offset: quad register offset to read
- * @val: on return, the contents read from the quad
- * @lock_sbq: true if the sideband queue lock must be acquired
- *
- * Read a quad register over the device sideband queue. Quad registers are
- * shared between multiple PHYs.
- */
-static enum ice_status
-ice_read_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32 *val,
-			  bool lock_sbq)
+static enum ice_status
+ice_read_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 *val)
 {
-	struct ice_sbq_msg_input msg = {0};
 	enum ice_status status;
+	u32 low, high;
+	u16 high_addr;
 
-	if (quad >= ICE_MAX_QUAD)
+	/* Only operate on registers known to be split into two 32bit
+	 * registers.
+	 */
+	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
+		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr 0x%08x\n",
+			  low_addr);
 		return ICE_ERR_PARAM;
+	}
 
-	ice_fill_quad_msg_e822(&msg, quad, offset);
-	msg.opcode = ice_sbq_msg_rd;
+	status = ice_read_phy_reg_e822(hw, port, low_addr, &low);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to read from low register 0x%08x\n, status %d",
+			  low_addr, status);
+		return status;
+	}
 
-	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
+	status = ice_read_phy_reg_e822(hw, port, high_addr, &high);
 	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to phy, status %d\n",
-			  status);
+		ice_debug(hw, ICE_DBG_PTP, "Failed to read from high register 0x%08x\n, status %d",
+			  high_addr, status);
 		return status;
 	}
 
-	*val = msg.data;
+	*val = (u64)high << 32 | low;
 
 	return ICE_SUCCESS;
 }
 
-enum ice_status
-ice_read_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32 *val)
-{
-	return ice_read_quad_reg_e822_lp(hw, quad, offset, val, true);
-}
-
 /**
- * ice_write_quad_reg_e822_lp - Write a PHY quad register
+ * ice_write_phy_reg_e822_lp - Write a PHY register
  * @hw: pointer to the HW struct
- * @quad: quad to write to
- * @offset: quad register offset to write
+ * @port: PHY port to write to
+ * @offset: PHY register offset to write
  * @val: The value to write to the register
  * @lock_sbq: true if the sideband queue lock must be acquired
  *
- * Write a quad register over the device sideband queue. Quad registers are
- * shared between multiple PHYs.
+ * Write a PHY register for the given port over the device sideband queue.
  */
 static enum ice_status
-ice_write_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32 val,
-			   bool lock_sbq)
+ice_write_phy_reg_e822_lp(struct ice_hw *hw, u8 port, u16 offset, u32 val,
+			  bool lock_sbq)
 {
 	struct ice_sbq_msg_input msg = {0};
 	enum ice_status status;
 
-	if (quad >= ICE_MAX_QUAD)
-		return ICE_ERR_PARAM;
-
-	ice_fill_quad_msg_e822(&msg, quad, offset);
+	ice_fill_phy_msg_e822(&msg, port, offset);
 	msg.opcode = ice_sbq_msg_wr;
 	msg.data = val;
 
@@ -615,84 +693,51 @@  ice_write_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32 val,
 }
 
 enum ice_status
-ice_write_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32 val)
+ice_write_phy_reg_e822(struct ice_hw *hw, u8 port, u16 offset, u32 val)
 {
-	return ice_write_quad_reg_e822_lp(hw, quad, offset, val, true);
+	return ice_write_phy_reg_e822_lp(hw, port, offset, val, true);
 }
 
 /**
- * ice_read_phy_tstamp_e822 - Read a PHY timestamp out of the quad block
+ * ice_write_40b_phy_reg_e822 - Write a 40b value to the PHY
  * @hw: pointer to the HW struct
- * @quad: the quad to read from
- * @idx: the timestamp index to read
- * @tstamp: on return, the 40bit timestamp value
+ * @port: port to write to
+ * @low_addr: offset of the low register
+ * @val: 40b value to write
  *
- * Read a 40bit timestamp value out of the two associated registers in the
- * quad memory block that is shared between the internal PHYs of the E822
- * family of devices.
+ * Write the provided 40b value to the two associated registers by splitting
+ * it up into two chunks, the lower 8 bits and the upper 32 bits.
  */
 static enum ice_status
-ice_read_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx, u64 *tstamp)
+ice_write_40b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 val)
 {
 	enum ice_status status;
-	u16 lo_addr, hi_addr;
-	u32 lo, hi;
-
-	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
-	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
-
-	status = ice_read_quad_reg_e822(hw, quad, lo_addr, &lo);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to read low PTP timestamp register, status %d\n",
-			  status);
-		return status;
-	}
-
-	status = ice_read_quad_reg_e822(hw, quad, hi_addr, &hi);
-	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to read high PTP timestamp register, status %d\n",
-			  status);
-		return status;
-	}
+	u32 low, high;
+	u16 high_addr;
 
-	/* For E822 based internal PHYs, the timestamp is reported with the
-	 * lower 8 bits in the low register, and the upper 32 bits in the high
-	 * register.
+	/* Only operate on registers known to be split into a lower 8 bit
+	 * register and an upper 32 bit register.
 	 */
-	*tstamp = ((u64)hi) << TS_PHY_HIGH_S | ((u64)lo & TS_PHY_LOW_M);
-
-	return ICE_SUCCESS;
-}
-
-/**
- * ice_clear_phy_tstamp_e822 - Clear a timestamp from the quad block
- * @hw: pointer to the HW struct
- * @quad: the quad to read from
- * @idx: the timestamp index to reset
- *
- * Clear a timestamp, resetting its valid bit, from the PHY quad block that is
- * shared between the internal PHYs on the E822 devices.
- */
-static enum ice_status
-ice_clear_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx)
-{
-	enum ice_status status;
-	u16 lo_addr, hi_addr;
+	if (!ice_is_40b_phy_reg_e822(low_addr, &high_addr)) {
+		ice_debug(hw, ICE_DBG_PTP, "Invalid 40b register addr 0x%08x\n",
+			  low_addr);
+		return ICE_ERR_PARAM;
+	}
 
-	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
-	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
+	low = (u32)(val & P_REG_40B_LOW_M);
+	high = (u32)(val >> P_REG_40B_HIGH_S);
 
-	status = ice_write_quad_reg_e822(hw, quad, lo_addr, 0);
+	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
 	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to clear low PTP timestamp register, status %d\n",
-			  status);
+		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register 0x%08x\n, status %d",
+			  low_addr, status);
 		return status;
 	}
 
-	status = ice_write_quad_reg_e822(hw, quad, hi_addr, 0);
+	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
 	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to clear high PTP timestamp register, status %d\n",
-			  status);
+		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high register 0x%08x\n, status %d",
+			  high_addr, status);
 		return status;
 	}
 
@@ -700,282 +745,236 @@  ice_clear_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx)
 }
 
 /**
- * ice_read_cgu_reg_e822 - Read a CGU register
+ * ice_write_64b_phy_reg_e822 - Write a 64bit value to PHY registers
  * @hw: pointer to the HW struct
- * @addr: Register address to read
- * @val: storage for register value read
+ * @port: PHY port to read from
+ * @low_addr: offset of the lower register to read from
+ * @val: the contents of the 64bit value to write to PHY
  *
- * Read the contents of a register of the Clock Generation Unit. Only
- * applicable to E822 devices.
+ * Write the 64bit value to the two associated 32bit PHY registers. The offset
+ * is always specified as the lower register, and the high address is looked
+ * up. This function only operates on registers known to be two parts of
+ * a 64bit value.
  */
 static enum ice_status
-ice_read_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 *val)
+ice_write_64b_phy_reg_e822(struct ice_hw *hw, u8 port, u16 low_addr, u64 val)
 {
-	struct ice_sbq_msg_input cgu_msg;
 	enum ice_status status;
+	u32 low, high;
+	u16 high_addr;
 
-	cgu_msg.opcode = ice_sbq_msg_rd;
-	cgu_msg.dest_dev = cgu;
-	cgu_msg.msg_addr_low = addr;
-	cgu_msg.msg_addr_high = 0x0;
+	/* Only operate on registers known to be split into two 32bit
+	 * registers.
+	 */
+	if (!ice_is_64b_phy_reg_e822(low_addr, &high_addr)) {
+		ice_debug(hw, ICE_DBG_PTP, "Invalid 64b register addr 0x%08x\n",
+			  low_addr);
+		return ICE_ERR_PARAM;
+	}
 
-	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
+	low = ICE_LO_DWORD(val);
+	high = ICE_HI_DWORD(val);
+
+	status = ice_write_phy_reg_e822(hw, port, low_addr, low);
 	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to read CGU register 0x%04x, status %d\n",
-			  addr, status);
+		ice_debug(hw, ICE_DBG_PTP, "Failed to write to low register 0x%08x\n, status %d",
+			  low_addr, status);
 		return status;
 	}
 
-	*val = cgu_msg.data;
-
-	return status;
-}
-
-/**
- * ice_write_cgu_reg_e822 - Write a CGU register
- * @hw: pointer to the HW struct
- * @addr: Register address to write
- * @val: value to write into the register
- *
- * Write the specified value to a register of the Clock Generation Unit. Only
- * applicable to E822 devices.
- */
-static enum ice_status
-ice_write_cgu_reg_e822(struct ice_hw *hw, u16 addr, u32 val)
-{
-	struct ice_sbq_msg_input cgu_msg;
-	enum ice_status status;
-
-	cgu_msg.opcode = ice_sbq_msg_wr;
-	cgu_msg.dest_dev = cgu;
-	cgu_msg.msg_addr_low = addr;
-	cgu_msg.msg_addr_high = 0x0;
-	cgu_msg.data = val;
-
-	status = ice_sbq_rw_reg_lp(hw, &cgu_msg, true);
+	status = ice_write_phy_reg_e822(hw, port, high_addr, high);
 	if (status) {
-		ice_debug(hw, ICE_DBG_PTP, "Failed to write CGU register 0x%04x, status %d\n",
-			  addr, status);
+		ice_debug(hw, ICE_DBG_PTP, "Failed to write to high register 0x%08x\n, status %d",
+			  high_addr, status);
 		return status;
 	}
 
-	return status;
-}
-
-/**
- * ice_clk_freq_str - Convert time_ref_freq to string
- * @clk_freq: Clock frequency
- *
- * Convert the specified TIME_REF clock frequency to a string.
- */
-static const char *ice_clk_freq_str(u8 clk_freq)
-{
-	switch ((enum ice_time_ref_freq)clk_freq) {
-	case ICE_TIME_REF_FREQ_25_000:
-		return "25 MHz";
-	case ICE_TIME_REF_FREQ_122_880:
-		return "122.88 MHz";
-	case ICE_TIME_REF_FREQ_125_000:
-		return "125 MHz";
-	case ICE_TIME_REF_FREQ_153_600:
-		return "153.6 MHz";
-	case ICE_TIME_REF_FREQ_156_250:
-		return "156.25 MHz";
-	case ICE_TIME_REF_FREQ_245_760:
-		return "245.76 MHz";
-	default:
-		return "Unknown";
-	}
+	return ICE_SUCCESS;
 }
 
 /**
- * ice_clk_src_str - Convert time_ref_src to string
- * @clk_src: Clock source
+ * ice_fill_quad_msg_e822 - Fill message data for quad register access
+ * @msg: the PHY message buffer to fill in
+ * @quad: the quad to access
+ * @offset: the register offset
  *
- * Convert the specified clock source to its string name.
- */
-static const char *ice_clk_src_str(u8 clk_src)
-{
-	switch ((enum ice_clk_src)clk_src) {
-	case ICE_CLK_SRC_TCX0:
-		return "TCX0";
-	case ICE_CLK_SRC_TIME_REF:
-		return "TIME_REF";
-	default:
-		return "Unknown";
-	}
+ * Fill a message buffer for accessing a register in a quad shared between
+ * multiple PHYs.
+ */
+static void
+ice_fill_quad_msg_e822(struct ice_sbq_msg_input *msg, u8 quad, u16 offset)
+{
+	u32 addr;
+
+	msg->dest_dev = rmn_0;
+
+	if ((quad % ICE_NUM_QUAD_TYPE) == 0)
+		addr = Q_0_BASE + offset;
+	else
+		addr = Q_1_BASE + offset;
+
+	msg->msg_addr_low = ICE_LO_WORD(addr);
+	msg->msg_addr_high = ICE_HI_WORD(addr);
 }
 
 /**
- * ice_cfg_cgu_pll_e822 - Configure the Clock Generation Unit
+ * ice_read_quad_reg_e822_lp - Read a PHY quad register
  * @hw: pointer to the HW struct
- * @clk_freq: Clock frequency to program
- * @clk_src: Clock source to select (TIME_REF, or TCX0)
+ * @quad: quad to read from
+ * @offset: quad register offset to read
+ * @val: on return, the contents read from the quad
+ * @lock_sbq: true if the sideband queue lock must be acquired
  *
- * Configure the Clock Generation Unit with the desired clock frequency and
- * time reference, enabling the PLL which drives the PTP hardware clock.
+ * Read a quad register over the device sideband queue. Quad registers are
+ * shared between multiple PHYs.
  */
-enum ice_status
-ice_cfg_cgu_pll_e822(struct ice_hw *hw, enum ice_time_ref_freq clk_freq,
-		     enum ice_clk_src clk_src)
+static enum ice_status
+ice_read_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32 *val,
+			  bool lock_sbq)
 {
-	union tspll_ro_bwm_lf bwm_lf;
-	union nac_cgu_dword19 dw19;
-	union nac_cgu_dword22 dw22;
-	union nac_cgu_dword24 dw24;
-	union nac_cgu_dword9 dw9;
+	struct ice_sbq_msg_input msg = {0};
 	enum ice_status status;
 
-	if (clk_freq >= NUM_ICE_TIME_REF_FREQ) {
-		ice_warn(hw, "Invalid TIME_REF frequency %u\n", clk_freq);
-		return ICE_ERR_PARAM;
-	}
-
-	if (clk_src >= NUM_ICE_CLK_SRC) {
-		ice_warn(hw, "Invalid clock source %u\n", clk_src);
-		return ICE_ERR_PARAM;
-	}
-
-	if (clk_src == ICE_CLK_SRC_TCX0 &&
-	    clk_freq != ICE_TIME_REF_FREQ_25_000) {
-		ice_warn(hw, "TCX0 only supports 25 MHz frequency\n");
+	if (quad >= ICE_MAX_QUAD)
 		return ICE_ERR_PARAM;
-	}
-
-	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD9, &dw9.val);
-	if (status)
-		return status;
 
-	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24, &dw24.val);
-	if (status)
-		return status;
+	ice_fill_quad_msg_e822(&msg, quad, offset);
+	msg.opcode = ice_sbq_msg_rd;
 
-	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF, &bwm_lf.val);
-	if (status)
+	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to phy, status %d\n",
+			  status);
 		return status;
-
-	/* Log the current clock configuration */
-	ice_debug(hw, ICE_DBG_PTP, "Current CGU configuration -- %s, clk_src %s, clk_freq %s, PLL %s\n",
-		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
-		  ice_clk_src_str(dw24.field.time_ref_sel),
-		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
-		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
-
-	/* Disable the PLL before changing the clock source or frequency */
-	if (dw24.field.ts_pll_enable) {
-		dw24.field.ts_pll_enable = 0;
-
-		status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24, dw24.val);
-		if (status)
-			return status;
 	}
 
-	/* Set the frequency */
-	dw9.field.time_ref_freq_sel = clk_freq;
-	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD9, dw9.val);
-	if (status)
-		return status;
+	*val = msg.data;
 
-	/* Configure the TS PLL feedback divisor */
-	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD19, &dw19.val);
-	if (status)
-		return status;
+	return ICE_SUCCESS;
+}
 
-	dw19.field.tspll_fbdiv_intgr = e822_cgu_params[clk_freq].feedback_div;
-	dw19.field.tspll_ndivratio = 1;
+enum ice_status
+ice_read_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32 *val)
+{
+	return ice_read_quad_reg_e822_lp(hw, quad, offset, val, true);
+}
 
-	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD19, dw19.val);
-	if (status)
-		return status;
+/**
+ * ice_write_quad_reg_e822_lp - Write a PHY quad register
+ * @hw: pointer to the HW struct
+ * @quad: quad to write to
+ * @offset: quad register offset to write
+ * @val: The value to write to the register
+ * @lock_sbq: true if the sideband queue lock must be acquired
+ *
+ * Write a quad register over the device sideband queue. Quad registers are
+ * shared between multiple PHYs.
+ */
+static enum ice_status
+ice_write_quad_reg_e822_lp(struct ice_hw *hw, u8 quad, u16 offset, u32 val,
+			   bool lock_sbq)
+{
+	struct ice_sbq_msg_input msg = {0};
+	enum ice_status status;
 
-	/* Configure the TS PLL post divisor */
-	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD22, &dw22.val);
-	if (status)
-		return status;
+	if (quad >= ICE_MAX_QUAD)
+		return ICE_ERR_PARAM;
 
-	dw22.field.time1588clk_div = e822_cgu_params[clk_freq].post_pll_div;
-	dw22.field.time1588clk_sel_div2 = 0;
+	ice_fill_quad_msg_e822(&msg, quad, offset);
+	msg.opcode = ice_sbq_msg_wr;
+	msg.data = val;
 
-	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD22, dw22.val);
-	if (status)
+	status = ice_sbq_rw_reg_lp(hw, &msg, lock_sbq);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to send message to phy, status %d\n",
+			  status);
 		return status;
+	}
 
-	/* Configure the TS PLL pre divisor and clock source */
-	status = ice_read_cgu_reg_e822(hw, NAC_CGU_DWORD24, &dw24.val);
-	if (status)
-		return status;
+	return ICE_SUCCESS;
+}
 
-	dw24.field.ref1588_ck_div = e822_cgu_params[clk_freq].refclk_pre_div;
-	dw24.field.tspll_fbdiv_frac = e822_cgu_params[clk_freq].frac_n_div;
-	dw24.field.time_ref_sel = clk_src;
+enum ice_status
+ice_write_quad_reg_e822(struct ice_hw *hw, u8 quad, u16 offset, u32 val)
+{
+	return ice_write_quad_reg_e822_lp(hw, quad, offset, val, true);
+}
 
-	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24, dw24.val);
-	if (status)
-		return status;
+/**
+ * ice_read_phy_tstamp_e822 - Read a PHY timestamp out of the quad block
+ * @hw: pointer to the HW struct
+ * @quad: the quad to read from
+ * @idx: the timestamp index to read
+ * @tstamp: on return, the 40bit timestamp value
+ *
+ * Read a 40bit timestamp value out of the two associated registers in the
+ * quad memory block that is shared between the internal PHYs of the E822
+ * family of devices.
+ */
+static enum ice_status
+ice_read_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx, u64 *tstamp)
+{
+	enum ice_status status;
+	u16 lo_addr, hi_addr;
+	u32 lo, hi;
 
-	/* Finally, enable the PLL */
-	dw24.field.ts_pll_enable = 1;
+	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
+	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
 
-	status = ice_write_cgu_reg_e822(hw, NAC_CGU_DWORD24, dw24.val);
-	if (status)
+	status = ice_read_quad_reg_e822(hw, quad, lo_addr, &lo);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to read low PTP timestamp register, status %d\n",
+			  status);
 		return status;
+	}
 
-	/* Wait to verify if the PLL locks */
-	ice_msec_delay(1, true);
-
-	status = ice_read_cgu_reg_e822(hw, TSPLL_RO_BWM_LF, &bwm_lf.val);
-	if (status)
+	status = ice_read_quad_reg_e822(hw, quad, hi_addr, &hi);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to read high PTP timestamp register, status %d\n",
+			  status);
 		return status;
-
-	if (!bwm_lf.field.plllock_true_lock_cri) {
-		ice_warn(hw, "CGU PLL failed to lock\n");
-		return ICE_ERR_NOT_READY;
 	}
 
-	/* Log the current clock configuration */
-	ice_debug(hw, ICE_DBG_PTP, "New CGU configuration -- %s, clk_src %s, clk_freq %s, PLL %s\n",
-		  dw24.field.ts_pll_enable ? "enabled" : "disabled",
-		  ice_clk_src_str(dw24.field.time_ref_sel),
-		  ice_clk_freq_str(dw9.field.time_ref_freq_sel),
-		  bwm_lf.field.plllock_true_lock_cri ? "locked" : "unlocked");
-
+	/* For E822 based internal PHYs, the timestamp is reported with the
+	 * lower 8 bits in the low register, and the upper 32 bits in the high
+	 * register.
+	 */
+	*tstamp = ((u64)hi) << TS_PHY_HIGH_S | ((u64)lo & TS_PHY_LOW_M);
 
 	return ICE_SUCCESS;
 }
 
 /**
- * ice_init_cgu_e822 - Initialize CGU with settings from firmware
- * @hw: pointer to the HW structure
+ * ice_clear_phy_tstamp_e822 - Clear a timestamp from the quad block
+ * @hw: pointer to the HW struct
+ * @quad: the quad to read from
+ * @idx: the timestamp index to reset
  *
- * Initialize the Clock Generation Unit of the E822 device.
+ * Clear a timestamp, resetting its valid bit, from the PHY quad block that is
+ * shared between the internal PHYs on the E822 devices.
  */
-static enum ice_status ice_init_cgu_e822(struct ice_hw *hw)
+static enum ice_status
+ice_clear_phy_tstamp_e822(struct ice_hw *hw, u8 quad, u8 idx)
 {
-	struct ice_ts_func_info *ts_info = &hw->func_caps.ts_func_info;
-	union tspll_cntr_bist_settings cntr_bist;
 	enum ice_status status;
+	u16 lo_addr, hi_addr;
 
-	status = ice_read_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
-				       &cntr_bist.val);
-	if (status)
-		return status;
-
-	/* Disable sticky lock detection so lock status reported is accurate */
-	cntr_bist.field.i_plllock_sel_0 = 0;
-	cntr_bist.field.i_plllock_sel_1 = 0;
+	lo_addr = (u16)TS_L(Q_REG_TX_MEMORY_BANK_START, idx);
+	hi_addr = (u16)TS_H(Q_REG_TX_MEMORY_BANK_START, idx);
 
-	status = ice_write_cgu_reg_e822(hw, TSPLL_CNTR_BIST_SETTINGS,
-					cntr_bist.val);
-	if (status)
+	status = ice_write_quad_reg_e822(hw, quad, lo_addr, 0);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to clear low PTP timestamp register, status %d\n",
+			  status);
 		return status;
+	}
 
-	/* Configure the CGU PLL using the parameters from the function
-	 * capabilities.
-	 */
-	status = ice_cfg_cgu_pll_e822(hw, ts_info->time_ref,
-				      (enum ice_clk_src)ts_info->clk_src);
-	if (status)
+	status = ice_write_quad_reg_e822(hw, quad, hi_addr, 0);
+	if (status) {
+		ice_debug(hw, ICE_DBG_PTP, "Failed to clear high PTP timestamp register, status %d\n",
+			  status);
 		return status;
+	}
 
 	return ICE_SUCCESS;
 }