phy: marvell: mux: Support nontrivial node order in selector register

Currently comphy_mux supports only trivial order of nodes in pin
selector register, that is lane N on position N*bitcount.

Add support for nontrivial order, with map stored in device tree
property mux-lane-order.

This is needed for Armada 37xx.

As far as I know, there is no driver for Armada 37xx comphy in the
kernel. When such a driver comes, this will need to be rewritten to
support the device tree bindings from the kernel.

Signed-off-by: Marek Behun <marek.behun@nic.cz>
Signed-off-by: Stefan Roese <sr@denx.de>

3 files changed, 19 insertions, 3 deletions

diff --git a/drivers/phy/marvell/comphy.h b/drivers/phy/marvell/comphy.h
index 30ab528..b402777 100644
--- a/drivers/phy/marvell/comphy.h
+++ b/drivers/phy/marvell/comphy.h
@@ -96,6 +96,7 @@ struct chip_serdes_phy_config {
 	void __iomem *hpipe3_base_addr;
 	u32 comphy_lanes_count;
 	u32 comphy_mux_bitcount;
+	const fdt32_t *comphy_mux_lane_order;
 	u32 cp_index;
 };
 
diff --git a/drivers/phy/marvell/comphy_core.c b/drivers/phy/marvell/comphy_core.c
index 17ab39c..2622751 100644
--- a/drivers/phy/marvell/comphy_core.c
+++ b/drivers/phy/marvell/comphy_core.c
@@ -134,6 +134,10 @@ static int comphy_probe(struct udevice *dev)
 		return -EINVAL;
 	}
 
+	chip_cfg->comphy_mux_lane_order =
+		fdtdec_locate_array(blob, node, "mux-lane-order",
+				    chip_cfg->comphy_lanes_count);
+
 	if (device_is_compatible(dev, "marvell,comphy-armada-3700"))
 		chip_cfg->ptr_comphy_chip_init = comphy_a3700_init;
 
diff --git a/drivers/phy/marvell/comphy_mux.c b/drivers/phy/marvell/comphy_mux.c
index 0538702..1f757d8 100644
--- a/drivers/phy/marvell/comphy_mux.c
+++ b/drivers/phy/marvell/comphy_mux.c
@@ -78,7 +78,8 @@ static u32 comphy_mux_get_mux_value(struct comphy_mux_data *mux_data,
 static void comphy_mux_reg_write(struct comphy_mux_data *mux_data,
 				 struct comphy_map *comphy_map_data,
 				 int comphy_max_lanes,
-				 void __iomem *selector_base, u32 bitcount)
+				 void __iomem *selector_base,
+				 const fdt32_t *mux_lane_order, u32 bitcount)
 {
 	u32 lane, value, offset, mask;
 
@@ -89,7 +90,15 @@ static void comphy_mux_reg_write(struct comphy_mux_data *mux_data,
 		if (comphy_map_data->type == PHY_TYPE_IGNORE)
 			continue;
 
-		offset = lane * bitcount;
+		/*
+		 * if the order of nodes in selector base register is
+		 * nontrivial, use mapping from mux_lane_order
+		 */
+		if (mux_lane_order)
+			offset = fdt32_to_cpu(mux_lane_order[lane]) * bitcount;
+		else
+			offset = lane * bitcount;
+
 		mask = (((1 << bitcount) - 1) << offset);
 		value = (comphy_mux_get_mux_value(mux_data,
 						  comphy_map_data->type,
@@ -105,6 +114,7 @@ void comphy_mux_init(struct chip_serdes_phy_config *chip_cfg,
 		     void __iomem *selector_base)
 {
 	struct comphy_mux_data *mux_data;
+	const fdt32_t *mux_lane_order;
 	u32 mux_bitcount;
 	u32 comphy_max_lanes;
 
@@ -112,13 +122,14 @@ void comphy_mux_init(struct chip_serdes_phy_config *chip_cfg,
 
 	comphy_max_lanes = chip_cfg->comphy_lanes_count;
 	mux_data = chip_cfg->mux_data;
+	mux_lane_order = chip_cfg->comphy_mux_lane_order;
 	mux_bitcount = chip_cfg->comphy_mux_bitcount;
 
 	/* check if the configuration is valid */
 	comphy_mux_check_config(mux_data, comphy_map_data, comphy_max_lanes);
 	/* Init COMPHY selectors */
 	comphy_mux_reg_write(mux_data, comphy_map_data, comphy_max_lanes,
-			     selector_base, mux_bitcount);
+			     selector_base, mux_lane_order, mux_bitcount);
 
 	debug_exit();
 }