mvebu: cp110: add support for XFI training

It will be exposed as run time service, so the OS or Bootloader can trigger
it.

Change-Id: I08143856e3b8f3e51d472acd2182942b6ef358b8
Signed-off-by: Grzegorz Jaszczyk <jaz@semihalf.com>
Reviewed-on: http://vgitil04.il.marvell.com:8080/52901
Reviewed-by: Kostya Porotchkin <kostap@marvell.com>
Tested-by: Kostya Porotchkin <kostap@marvell.com>

3 files changed, 194 insertions, 1 deletions

diff --git a/drivers/marvell/comphy/phy-comphy-cp110.c b/drivers/marvell/comphy/phy-comphy-cp110.c
index c7984174..1dfc165c 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.c
+++ b/drivers/marvell/comphy/phy-comphy-cp110.c
@@ -1871,6 +1871,193 @@ static int mvebu_cp110_comphy_usb3_power_on(uint64_t comphy_base,
 	return ret;
 }
 
+/* This function performs RX training for one Feed Forward Equalization (FFE)
+ * value.
+ * The RX traiing result is stored in 'Saved DFE values Register' (SAV_F0D).
+ *
+ * Return '0' on success, error code in  a case of failure.
+ */
+static int mvebu_cp110_comphy_test_single_ffe(uint64_t comphy_base,
+					      uint8_t comphy_index,
+					      uint32_t ffe, uint32_t *result)
+{
+	uint32_t mask, data, timeout;
+	uintptr_t hpipe_addr, sd_ip_addr;
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+
+	sd_ip_addr = SD_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base), comphy_index);
+
+	/* Configure PRBS counters */
+	mask = HPIPE_PHY_TEST_PATTERN_SEL_MASK;
+	data = 0xe << HPIPE_PHY_TEST_PATTERN_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+
+	mask = HPIPE_PHY_TEST_DATA_MASK;
+	data = 0x64 << HPIPE_PHY_TEST_DATA_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_DATA_REG, data, mask);
+
+	mask = HPIPE_PHY_TEST_EN_MASK;
+	data = 0x1 << HPIPE_PHY_TEST_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+
+	mdelay(50);
+
+	/* Set the FFE value */
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_MASK;
+	data = ffe << HPIPE_G1_SETTINGS_3_G1_FFE_RES_SEL_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* Start RX training */
+	mask = SD_EXTERNAL_STATUS_START_RX_TRAINING_MASK;
+	data = 1 << SD_EXTERNAL_STATUS_START_RX_TRAINING_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_STATUS_REG, data, mask);
+
+	/* Check the result of RX training */
+	timeout = RX_TRAINING_TIMEOUT;
+	while (timeout) {
+		data = mmio_read_32(sd_ip_addr + SD_EXTERNAL_STATAUS1_REG);
+		if (data & SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_COMP_MASK)
+			break;
+		mdelay(1);
+		timeout--;
+	}
+
+	if (timeout == 0)
+		return -ETIMEDOUT;
+
+	if (data & SD_EXTERNAL_STATAUS1_REG_RX_TRAIN_FAILED_MASK)
+		return -EINVAL;
+
+	/* Stop RX training */
+	mask = SD_EXTERNAL_STATUS_START_RX_TRAINING_MASK;
+	data = 0 << SD_EXTERNAL_STATUS_START_RX_TRAINING_OFFSET;
+	reg_set(sd_ip_addr + SD_EXTERNAL_STATUS_REG, data, mask);
+
+	/* Read the result */
+	data = mmio_read_32(hpipe_addr + HPIPE_SAVED_DFE_VALUES_REG);
+	data &= HPIPE_SAVED_DFE_VALUES_SAV_F0D_MASK;
+	data >>= HPIPE_SAVED_DFE_VALUES_SAV_F0D_OFFSET;
+	*result = data;
+
+	mask = HPIPE_PHY_TEST_RESET_MASK;
+	data = 0x1 << HPIPE_PHY_TEST_RESET_OFFSET;
+	mask |= HPIPE_PHY_TEST_EN_MASK;
+	data |= 0x0 << HPIPE_PHY_TEST_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+
+	mask = HPIPE_PHY_TEST_RESET_MASK;
+	data = 0x0 << HPIPE_PHY_TEST_RESET_OFFSET;
+	reg_set(hpipe_addr + HPIPE_PHY_TEST_CONTROL_REG, data, mask);
+
+	return 0;
+}
+
+/* This function runs complete RX training sequence:
+ *	- Run RX training for all possible Feed Forward Equalization values
+ *	- Choose the FFE which gives the best result.
+ *	- Run RX training again with the best result.
+ *
+ * Return '0' on success, error code in  a case of failure.
+ */
+int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base,
+					      uint8_t comphy_index)
+{
+	uint32_t mask, data, max_rx_train = 0, max_rx_train_index = 0;
+	uintptr_t hpipe_addr;
+	uint32_t rx_train_result;
+	int ret, i;
+
+	hpipe_addr = HPIPE_ADDR(COMPHY_PIPE_FROM_COMPHY_ADDR(comphy_base),
+				comphy_index);
+
+	debug_enter();
+
+	/* Configure SQ threshold and CDR lock */
+	mask = HPIPE_SQUELCH_THRESH_IN_MASK;
+	data = 0xc << HPIPE_SQUELCH_THRESH_IN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SQUELCH_FFE_SETTING_REG, data, mask);
+
+	mask = HPIPE_SQ_DEGLITCH_WIDTH_P_MASK;
+	data = 0xf << HPIPE_SQ_DEGLITCH_WIDTH_P_OFFSET;
+	mask |= HPIPE_SQ_DEGLITCH_WIDTH_N_MASK;
+	data |= 0xf << HPIPE_SQ_DEGLITCH_WIDTH_N_OFFSET;
+	mask |= HPIPE_SQ_DEGLITCH_EN_MASK;
+	data |= 0x1 << HPIPE_SQ_DEGLITCH_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_SQ_GLITCH_FILTER_CTRL, data, mask);
+
+	mask = HPIPE_CDR_LOCK_DET_EN_MASK;
+	data = 0x1 << HPIPE_CDR_LOCK_DET_EN_OFFSET;
+	reg_set(hpipe_addr + HPIPE_LOOPBACK_REG, data, mask);
+
+	udelay(100);
+
+	/* Determine if we have a cable attached to this comphy, if not,
+	 * we can't perform RX training.
+	 */
+	data = mmio_read_32(hpipe_addr + HPIPE_SQUELCH_FFE_SETTING_REG);
+	if (data & HPIPE_SQUELCH_DETECTED_MASK) {
+		ERROR("Squelsh is not detected, can't perform RX training\n");
+		return -EINVAL;
+	}
+
+	data = mmio_read_32(hpipe_addr + HPIPE_LOOPBACK_REG);
+	if (!(data & HPIPE_CDR_LOCK_MASK)) {
+		ERROR("CDR is not locked, can't perform RX training\n");
+		return -EINVAL;
+	}
+
+	/* Do preparations for RX training */
+	mask = HPIPE_DFE_RES_FORCE_MASK;
+	data = 0x0 << HPIPE_DFE_RES_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_DFE_REG0, data, mask);
+
+	mask = HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_MASK;
+	data = 0xf << HPIPE_G1_SETTINGS_3_G1_FFE_CAP_SEL_OFFSET;
+	mask |= HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_MASK;
+	data |= 1 << HPIPE_G1_SETTINGS_3_G1_FFE_SETTING_FORCE_OFFSET;
+	reg_set(hpipe_addr + HPIPE_G1_SETTINGS_3_REG, data, mask);
+
+	/* Perform RX training for all possible FFE (Feed Forward
+	 * Equalization, possible values are 0-7).
+	 * We update the best value reached and the FFE which gave this value.
+	 */
+	for (i = 0; i < MAX_NUM_OF_FFE; i++) {
+		rx_train_result = 0;
+		ret = mvebu_cp110_comphy_test_single_ffe(comphy_base,
+							 comphy_index, i,
+							 &rx_train_result);
+
+		if ((!ret) && (rx_train_result > max_rx_train)) {
+			max_rx_train = rx_train_result;
+			max_rx_train_index = i;
+		}
+	}
+
+	/* If we were able to determine which FFE gives the best value,
+	 * now we need to set it and run RX training again (only for this
+	 * FFE).
+	 */
+	if (max_rx_train) {
+		ret = mvebu_cp110_comphy_test_single_ffe(comphy_base,
+							 comphy_index,
+							 max_rx_train_index,
+							 &rx_train_result);
+
+		if (ret == 0)
+			debug("RX Training passed (FFE = %d, result = 0x%x)\n",
+			       max_rx_train_index, rx_train_result);
+	} else {
+		ERROR("RX Training failed for comphy%d\n", comphy_index);
+		ret = -EINVAL;
+	}
+
+	debug_exit();
+
+	return ret;
+}
+
 int mvebu_cp110_comphy_power_on(uint64_t comphy_base, uint64_t comphy_index, uint64_t comphy_mode)
 {
 	int mode = COMPHY_GET_MODE(comphy_mode);
diff --git a/drivers/marvell/comphy/phy-comphy-cp110.h b/drivers/marvell/comphy/phy-comphy-cp110.h
index 9315e3e4..6476f8ce 100644
--- a/drivers/marvell/comphy/phy-comphy-cp110.h
+++ b/drivers/marvell/comphy/phy-comphy-cp110.h
@@ -8,3 +8,4 @@
 int mvebu_cp110_comphy_is_pll_locked(uint64_t comphy_base, uint64_t comphy_index);
 int mvebu_cp110_comphy_power_off(uint64_t comphy_base, uint64_t comphy_index);
 int mvebu_cp110_comphy_power_on(uint64_t comphy_base, uint64_t comphy_index, uint64_t comphy_mode);
+int mvebu_cp110_comphy_xfi_rx_training(uint64_t comphy_base, uint8_t comphy_index);
diff --git a/plat/marvell/common/mrvl_sip_svc.c b/plat/marvell/common/mrvl_sip_svc.c
index a2e02ddc..066ff87d 100644
--- a/plat/marvell/common/mrvl_sip_svc.c
+++ b/plat/marvell/common/mrvl_sip_svc.c
@@ -20,13 +20,14 @@
 #define MV_SIP_COMPHY_POWER_ON	0x82000001
 #define MV_SIP_COMPHY_POWER_OFF	0x82000002
 #define MV_SIP_COMPHY_PLL_LOCK	0x82000003
+#define MV_SIP_COMPHY_XFI_TRAIN	0x82000004
 
 #define MAX_LANE_NR		6
 #define MVEBU_COMPHY_OFFSET	0x441000
 
 /* This macro is used to identify COMPHY related calls from SMC function ID */
 #define is_comphy_fid(fid)	\
-	((fid) >= MV_SIP_COMPHY_POWER_ON && (fid) <= MV_SIP_COMPHY_PLL_LOCK)
+	((fid) >= MV_SIP_COMPHY_POWER_ON && (fid) <= MV_SIP_COMPHY_XFI_TRAIN)
 
 
 uint64_t mrvl_sip_smc_handler(uint32_t smc_fid,
@@ -68,6 +69,10 @@ uint64_t mrvl_sip_smc_handler(uint32_t smc_fid,
 		/* x1:  comphy_base, x2: comphy_index */
 		ret = mvebu_cp110_comphy_is_pll_locked(x1, x2);
 		SMC_RET1(handle, ret);
+	case MV_SIP_COMPHY_XFI_TRAIN:
+		/* x1:  comphy_base, x2: comphy_index */
+		ret = mvebu_cp110_comphy_xfi_rx_training(x1, x2);
+		SMC_RET1(handle, ret);
 	default:
 		ERROR("%s: unhandled SMC (0x%x)\n", __func__, smc_fid);
 		SMC_RET1(handle, SMC_UNK);