2 files changed, 112 insertions, 0 deletions

diff --git a/arch/arm/cpu/armv8/fsl-layerscape/icid.c b/arch/arm/cpu/armv8/fsl-layerscape/icid.c
index ae3b8daa95..b1a950e7f9 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/icid.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/icid.c
@@ -10,6 +10,7 @@
 #include <asm/io.h>
 #include <asm/processor.h>
 #include <asm/arch-fsl-layerscape/fsl_icid.h>
+#include <fsl_fman.h>
 
 static void set_icid(struct icid_id_table *tbl, int size)
 {
@@ -19,10 +20,27 @@ static void set_icid(struct icid_id_table *tbl, int size)
 		out_be32((u32 *)(tbl[i].reg_addr), tbl[i].reg);
 }
 
+#ifdef CONFIG_SYS_DPAA_FMAN
+void set_fman_icids(struct fman_icid_id_table *tbl, int size)
+{
+	int i;
+	ccsr_fman_t *fm = (void *)CONFIG_SYS_FSL_FM1_ADDR;
+
+	for (i = 0; i < size; i++) {
+		out_be32(&fm->fm_bmi_common.fmbm_ppid[tbl[i].port_id - 1],
+			 tbl[i].icid);
+	}
+}
+#endif
+
 void set_icids(void)
 {
 	/* setup general icid offsets */
 	set_icid(icid_tbl, icid_tbl_sz);
+
+#ifdef CONFIG_SYS_DPAA_FMAN
+	set_fman_icids(fman_icid_tbl, fman_icid_tbl_sz);
+#endif
 }
 
 int fdt_set_iommu_prop(void *blob, int off, int smmu_ph, u32 *ids, int num_ids)
@@ -75,6 +93,66 @@ int fdt_fixup_icid_tbl(void *blob, int smmu_ph,
 	return 0;
 }
 
+#ifdef CONFIG_SYS_DPAA_FMAN
+int get_fman_port_icid(int port_id, struct fman_icid_id_table *tbl,
+		       const int size)
+{
+	int i;
+
+	for (i = 0; i < size; i++) {
+		if (tbl[i].port_id == port_id)
+			return tbl[i].icid;
+	}
+
+	return -1;
+}
+
+void fdt_fixup_fman_port_icid_by_compat(void *blob, int smmu_ph,
+					const char *compat)
+{
+	int noff, len, icid;
+	const u32 *prop;
+
+	noff = fdt_node_offset_by_compatible(blob, -1, compat);
+	while (noff > 0) {
+		prop = fdt_getprop(blob, noff, "cell-index", &len);
+		if (!prop) {
+			printf("WARNING missing cell-index for fman port\n");
+			continue;
+		}
+		if (len != 4) {
+			printf("WARNING bad cell-index size for fman port\n");
+			continue;
+		}
+
+		icid = get_fman_port_icid(fdt32_to_cpu(*prop),
+					  fman_icid_tbl, fman_icid_tbl_sz);
+		if (icid < 0) {
+			printf("WARNING unknown ICID for fman port %d\n",
+			       *prop);
+			continue;
+		}
+
+		fdt_set_iommu_prop(blob, noff, smmu_ph, (u32 *)&icid, 1);
+
+		noff = fdt_node_offset_by_compatible(blob, noff, compat);
+	}
+}
+
+void fdt_fixup_fman_icids(void *blob, int smmu_ph)
+{
+	static const char * const compats[] = {
+		"fsl,fman-v3-port-oh",
+		"fsl,fman-v3-port-rx",
+		"fsl,fman-v3-port-tx",
+	};
+	int i;
+
+	for (i = 0; i < ARRAY_SIZE(compats); i++)
+		fdt_fixup_fman_port_icid_by_compat(blob, smmu_ph, compats[i]);
+}
+#endif
+
 int fdt_get_smmu_phandle(void *blob)
 {
 	int noff, smmu_ph;
@@ -107,4 +185,8 @@ void fdt_fixup_icid(void *blob)
 		return;
 
 	fdt_fixup_icid_tbl(blob, smmu_ph, icid_tbl, icid_tbl_sz);
+
+#ifdef CONFIG_SYS_DPAA_FMAN
+	fdt_fixup_fman_icids(blob, smmu_ph);
+#endif
 }
diff --git a/arch/arm/cpu/armv8/fsl-layerscape/ls1046_ids.c b/arch/arm/cpu/armv8/fsl-layerscape/ls1046_ids.c
index 80e1ceadc0..30c7d8d28a 100644
--- a/arch/arm/cpu/armv8/fsl-layerscape/ls1046_ids.c
+++ b/arch/arm/cpu/armv8/fsl-layerscape/ls1046_ids.c
@@ -43,3 +43,33 @@ struct icid_id_table icid_tbl[] = {
 };
 
 int icid_tbl_sz = ARRAY_SIZE(icid_tbl);
+
+#ifdef CONFIG_SYS_DPAA_FMAN
+struct fman_icid_id_table fman_icid_tbl[] = {
+	/* port id, icid */
+	SET_FMAN_ICID_ENTRY(0x02, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x03, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x04, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x05, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x06, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x07, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x08, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x09, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x0a, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x0b, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x0c, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x0d, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x28, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x29, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x2a, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x2b, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x2c, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x2d, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x10, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x11, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x30, FSL_DPAA1_STREAM_ID_END),
+	SET_FMAN_ICID_ENTRY(0x31, FSL_DPAA1_STREAM_ID_END),
+};
+
+int fman_icid_tbl_sz = ARRAY_SIZE(fman_icid_tbl);
+#endif