4 files changed, 219 insertions, 54 deletions

diff --git a/drivers/phy/marvell/comphy_a3700.c b/drivers/phy/marvell/comphy_a3700.c
index 047c8bb045..4104353555 100644
--- a/drivers/phy/marvell/comphy_a3700.c
+++ b/drivers/phy/marvell/comphy_a3700.c
@@ -11,6 +11,7 @@
 #include <asm/arch/cpu.h>
 #include <asm/arch/soc.h>
 #include <linux/delay.h>
+#include <phy.h>
 
 #include "comphy_a3700.h"
 
@@ -982,6 +983,138 @@ void comphy_dedicated_phys_init(void)
 	debug_exit();
 }
 
+static int find_available_node_by_compatible(int offset, const char *compatible)
+{
+	do {
+		offset = fdt_node_offset_by_compatible(gd->fdt_blob, offset,
+						       compatible);
+	} while (offset > 0 && !fdtdec_get_is_enabled(gd->fdt_blob, offset));
+
+	return offset;
+}
+
+static bool comphy_a3700_find_lane(const int nodes[3], int node,
+				   int port, int *lane, int *invert)
+{
+	int res, i, j;
+
+	for (i = 0; ; i++) {
+		struct fdtdec_phandle_args args;
+
+		res = fdtdec_parse_phandle_with_args(gd->fdt_blob, node, "phys",
+						     "#phy-cells", 0, i, &args);
+		if (res)
+			return false;
+
+		for (j = 0; j < 3; j++) {
+			if (nodes[j] >= 0 && args.node == nodes[j] &&
+			    (args.args_count >= 1 ? args.args[0] : 0) == port) {
+				*lane = j;
+				*invert = args.args_count >= 2 ? args.args[1]
+							       : 0;
+				return true;
+			}
+		}
+	}
+
+	return false;
+}
+
+static void comphy_a3700_fill_cfg(struct chip_serdes_phy_config *cfg,
+				  const int nodes[3], const char *compatible,
+				  int type)
+{
+	int node, lane, port, speed, invert;
+
+	port = (type == COMPHY_TYPE_SGMII1) ? 1 : 0;
+
+	node = -1;
+	while (1) {
+		node = find_available_node_by_compatible(node, compatible);
+		if (node < 0)
+			return;
+
+		if (comphy_a3700_find_lane(nodes, node, port, &lane, &invert))
+			break;
+	}
+
+	if (cfg->comphy_map_data[lane].type != COMPHY_TYPE_UNCONNECTED) {
+		printf("Error: More PHYs defined for lane %d, skipping\n",
+		       lane);
+		return;
+	}
+
+	if (type == COMPHY_TYPE_SGMII0 || type == COMPHY_TYPE_SGMII1) {
+		const char *phy_mode;
+
+		phy_mode = fdt_getprop(gd->fdt_blob, node, "phy-mode", NULL);
+		if (phy_mode &&
+		    !strcmp(phy_mode,
+			    phy_string_for_interface(PHY_INTERFACE_MODE_2500BASEX)))
+			speed = COMPHY_SPEED_3_125G;
+		else
+			speed = COMPHY_SPEED_1_25G;
+	} else if (type == COMPHY_TYPE_SATA0) {
+		speed = COMPHY_SPEED_6G;
+	} else {
+		speed = COMPHY_SPEED_5G;
+	}
+
+	cfg->comphy_map_data[lane].type = type;
+	cfg->comphy_map_data[lane].speed = speed;
+	cfg->comphy_map_data[lane].invert = invert;
+}
+
+static const fdt32_t comphy_a3700_mux_lane_order[3] = {
+	__constant_cpu_to_be32(1),
+	__constant_cpu_to_be32(0),
+	__constant_cpu_to_be32(2),
+};
+
+int comphy_a3700_init_serdes_map(int node, struct chip_serdes_phy_config *cfg)
+{
+	int comphy_nodes[3];
+	int child, i;
+
+	for (i = 0; i < ARRAY_SIZE(comphy_nodes); i++)
+		comphy_nodes[i] = -FDT_ERR_NOTFOUND;
+
+	fdt_for_each_subnode(child, gd->fdt_blob, node) {
+		if (!fdtdec_get_is_enabled(gd->fdt_blob, child))
+			continue;
+
+		i = fdtdec_get_int(gd->fdt_blob, child, "reg", -1);
+		if (i < 0 || i >= ARRAY_SIZE(comphy_nodes))
+			continue;
+
+		comphy_nodes[i] = child;
+	}
+
+	for (i = 0; i < ARRAY_SIZE(comphy_nodes); i++) {
+		cfg->comphy_map_data[i].type = COMPHY_TYPE_UNCONNECTED;
+		cfg->comphy_map_data[i].speed = COMPHY_SPEED_INVALID;
+	}
+
+	comphy_a3700_fill_cfg(cfg, comphy_nodes, "marvell,armada3700-u3d",
+			      COMPHY_TYPE_USB3_DEVICE);
+	comphy_a3700_fill_cfg(cfg, comphy_nodes, "marvell,armada3700-xhci",
+			      COMPHY_TYPE_USB3_HOST0);
+	comphy_a3700_fill_cfg(cfg, comphy_nodes, "marvell,armada-3700-pcie",
+			      COMPHY_TYPE_PEX0);
+	comphy_a3700_fill_cfg(cfg, comphy_nodes, "marvell,armada-3700-ahci",
+			      COMPHY_TYPE_SATA0);
+	comphy_a3700_fill_cfg(cfg, comphy_nodes, "marvell,armada-3700-neta",
+			      COMPHY_TYPE_SGMII0);
+	comphy_a3700_fill_cfg(cfg, comphy_nodes, "marvell,armada-3700-neta",
+			      COMPHY_TYPE_SGMII1);
+
+	cfg->comphy_lanes_count = 3;
+	cfg->comphy_mux_bitcount = 4;
+	cfg->comphy_mux_lane_order = comphy_a3700_mux_lane_order;
+
+	return 0;
+}
+
 int comphy_a3700_init(struct chip_serdes_phy_config *chip_cfg,
 		      struct comphy_map *serdes_map)
 {
diff --git a/drivers/phy/marvell/comphy_core.c b/drivers/phy/marvell/comphy_core.c
index 2c9d7b2288..233a973035 100644
--- a/drivers/phy/marvell/comphy_core.c
+++ b/drivers/phy/marvell/comphy_core.c
@@ -86,11 +86,8 @@ __weak int comphy_update_map(struct comphy_map *serdes_map, int count)
 
 static int comphy_probe(struct udevice *dev)
 {
-	const void *blob = gd->fdt_blob;
 	int node = dev_of_offset(dev);
 	struct chip_serdes_phy_config *chip_cfg = dev_get_priv(dev);
-	int subnode;
-	int lane;
 	int last_idx = 0;
 	static int current_idx;
 	int res;
@@ -104,30 +101,14 @@ static int comphy_probe(struct udevice *dev)
 	if (IS_ERR(chip_cfg->hpipe3_base_addr))
 		return PTR_ERR(chip_cfg->hpipe3_base_addr);
 
-	chip_cfg->comphy_lanes_count = fdtdec_get_int(blob, node,
-						      "max-lanes", 0);
-	if (chip_cfg->comphy_lanes_count <= 0) {
-		dev_err(dev, "comphy max lanes is wrong\n");
-		return -EINVAL;
-	}
-
-	chip_cfg->comphy_mux_bitcount = fdtdec_get_int(blob, node,
-						       "mux-bitcount", 0);
-	if (chip_cfg->comphy_mux_bitcount <= 0) {
-		dev_err(dev, "comphy mux bit count is wrong\n");
-		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->comphy_init_map = comphy_a3700_init_serdes_map;
 		chip_cfg->ptr_comphy_chip_init = comphy_a3700_init;
 		chip_cfg->rx_training = NULL;
 	}
 
 	if (device_is_compatible(dev, "marvell,comphy-cp110")) {
+		chip_cfg->comphy_init_map = comphy_cp110_init_serdes_map;
 		chip_cfg->ptr_comphy_chip_init = comphy_cp110_init;
 		chip_cfg->rx_training = comphy_cp110_sfi_rx_training;
 	}
@@ -141,39 +122,9 @@ static int comphy_probe(struct udevice *dev)
 		return -ENODEV;
 	}
 
-	lane = 0;
-	fdt_for_each_subnode(subnode, blob, node) {
-		/* Skip disabled ports */
-		if (!fdtdec_get_is_enabled(blob, subnode))
-			continue;
-
-		chip_cfg->comphy_map_data[lane].type =
-			fdtdec_get_int(blob, subnode, "phy-type",
-				       COMPHY_TYPE_INVALID);
-
-		if (chip_cfg->comphy_map_data[lane].type ==
-		    COMPHY_TYPE_INVALID) {
-			printf("no phy type for lane %d, setting lane as unconnected\n",
-			       lane + 1);
-			continue;
-		}
-
-		chip_cfg->comphy_map_data[lane].speed =
-			fdtdec_get_int(blob, subnode, "phy-speed",
-				       COMPHY_SPEED_INVALID);
-
-		chip_cfg->comphy_map_data[lane].invert =
-			fdtdec_get_int(blob, subnode, "phy-invert",
-				       COMPHY_POLARITY_NO_INVERT);
-
-		chip_cfg->comphy_map_data[lane].clk_src =
-			fdtdec_get_bool(blob, subnode, "clk-src");
-
-		chip_cfg->comphy_map_data[lane].end_point =
-			fdtdec_get_bool(blob, subnode, "end_point");
-
-		lane++;
-	}
+	res = chip_cfg->comphy_init_map(node, chip_cfg);
+	if (res < 0)
+		return res;
 
 	res = comphy_update_map(chip_cfg->comphy_map_data, chip_cfg->comphy_lanes_count);
 	if (res < 0)
diff --git a/drivers/phy/marvell/comphy_core.h b/drivers/phy/marvell/comphy_core.h
index 9bbd7f8f35..d573776c05 100644
--- a/drivers/phy/marvell/comphy_core.h
+++ b/drivers/phy/marvell/comphy_core.h
@@ -32,6 +32,7 @@ struct comphy_mux_data {
 
 struct chip_serdes_phy_config {
 	struct comphy_mux_data *mux_data;
+	int (*comphy_init_map)(int, struct chip_serdes_phy_config *);
 	int (*ptr_comphy_chip_init)(struct chip_serdes_phy_config *,
 				    struct comphy_map *);
 	int (*rx_training)(struct chip_serdes_phy_config *, u32);
@@ -85,9 +86,20 @@ static inline void reg_set16(void __iomem *addr, u16 data, u16 mask)
 
 /* SoC specific init functions */
 #ifdef CONFIG_ARMADA_3700
+int comphy_a3700_init_serdes_map(int node, struct chip_serdes_phy_config *cfg);
 int comphy_a3700_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 		      struct comphy_map *serdes_map);
 #else
+static inline int
+comphy_a3700_init_serdes_map(int node, struct chip_serdes_phy_config *cfg)
+{
+	/*
+	 * This function should never be called in this configuration, so
+	 * lets return an error here.
+	 */
+	return -1;
+}
+
 static inline int comphy_a3700_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 				    struct comphy_map *serdes_map)
 {
@@ -100,11 +112,22 @@ static inline int comphy_a3700_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 #endif
 
 #ifdef CONFIG_ARMADA_8K
+int comphy_cp110_init_serdes_map(int node, struct chip_serdes_phy_config *cfg);
 int comphy_cp110_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 		      struct comphy_map *serdes_map);
 int comphy_cp110_sfi_rx_training(struct chip_serdes_phy_config *ptr_chip_cfg,
 				 u32 lane);
 #else
+static inline int
+comphy_cp110_init_serdes_map(int node, struct chip_serdes_phy_config *cfg)
+{
+	/*
+	 * This function should never be called in this configuration, so
+	 * lets return an error here.
+	 */
+	return -1;
+}
+
 static inline int comphy_cp110_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 		      struct comphy_map *serdes_map)
 {
diff --git a/drivers/phy/marvell/comphy_cp110.c b/drivers/phy/marvell/comphy_cp110.c
index 4fe2dfcdd1..e063b51c6d 100644
--- a/drivers/phy/marvell/comphy_cp110.c
+++ b/drivers/phy/marvell/comphy_cp110.c
@@ -554,6 +554,64 @@ void comphy_dedicated_phys_init(void)
 	debug_exit();
 }
 
+int comphy_cp110_init_serdes_map(int node, struct chip_serdes_phy_config *cfg)
+{
+	int lane, subnode;
+
+	cfg->comphy_lanes_count = fdtdec_get_int(gd->fdt_blob, node,
+						 "max-lanes", 0);
+	if (cfg->comphy_lanes_count <= 0) {
+		printf("comphy max lanes is wrong\n");
+		return -EINVAL;
+	}
+
+	cfg->comphy_mux_bitcount = fdtdec_get_int(gd->fdt_blob, node,
+						  "mux-bitcount", 0);
+	if (cfg->comphy_mux_bitcount <= 0) {
+		printf("comphy mux bit count is wrong\n");
+		return -EINVAL;
+	}
+
+	cfg->comphy_mux_lane_order = fdtdec_locate_array(gd->fdt_blob, node,
+							 "mux-lane-order",
+							 cfg->comphy_lanes_count);
+
+	lane = 0;
+	fdt_for_each_subnode(subnode, gd->fdt_blob, node) {
+		/* Skip disabled ports */
+		if (!fdtdec_get_is_enabled(gd->fdt_blob, subnode))
+			continue;
+
+		cfg->comphy_map_data[lane].type =
+			fdtdec_get_int(gd->fdt_blob, subnode, "phy-type",
+				       COMPHY_TYPE_INVALID);
+
+		if (cfg->comphy_map_data[lane].type == COMPHY_TYPE_INVALID) {
+			printf("no phy type for lane %d, setting lane as unconnected\n",
+			       lane + 1);
+			continue;
+		}
+
+		cfg->comphy_map_data[lane].speed =
+			fdtdec_get_int(gd->fdt_blob, subnode, "phy-speed",
+				       COMPHY_SPEED_INVALID);
+
+		cfg->comphy_map_data[lane].invert =
+			fdtdec_get_int(gd->fdt_blob, subnode, "phy-invert",
+				       COMPHY_POLARITY_NO_INVERT);
+
+		cfg->comphy_map_data[lane].clk_src =
+			fdtdec_get_bool(gd->fdt_blob, subnode, "clk-src");
+
+		cfg->comphy_map_data[lane].end_point =
+			fdtdec_get_bool(gd->fdt_blob, subnode, "end_point");
+
+		lane++;
+	}
+
+	return 0;
+}
+
 int comphy_cp110_init(struct chip_serdes_phy_config *ptr_chip_cfg,
 		      struct comphy_map *serdes_map)
 {