u-boot-brain/arch/arm/cpu/armv8/fsl-layerscape/icid.c

// SPDX-License-Identifier: GPL-2.0+
/*
 * Copyright 2018 NXP
 */

#include <common.h>
#include <linux/libfdt.h>
#include <fdt_support.h>

#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)
{
	int i;

	for (i = 0; i < size; i++)
		if (tbl[i].le)
			out_le32((u32 *)(tbl[i].reg_addr), tbl[i].reg);
		else
			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)
{
	int i, ret;
	u32 prop[8];

	/*
	 * Note: The "iommus" property definition mentions Stream IDs while
	 * this code handles ICIDs. The current implementation assumes that
	 * ICIDs and Stream IDs are equal.
	 */
	for (i = 0; i < num_ids; i++) {
		prop[i * 2] = cpu_to_fdt32(smmu_ph);
		prop[i * 2 + 1] = cpu_to_fdt32(ids[i]);
	}
	ret = fdt_setprop(blob, off, "iommus",
			  prop, sizeof(u32) * num_ids * 2);
	if (ret) {
		printf("WARNING unable to set iommus: %s\n", fdt_strerror(ret));
		return ret;
	}

	return 0;
}

int fdt_fixup_icid_tbl(void *blob, int smmu_ph,
		       struct icid_id_table *tbl, int size)
{
	int i, err, off;

	for (i = 0; i < size; i++) {
		if (!tbl[i].compat)
			continue;

		off = fdt_node_offset_by_compat_reg(blob,
						    tbl[i].compat,
						    tbl[i].compat_addr);
		if (off > 0) {
			err = fdt_set_iommu_prop(blob, off, smmu_ph,
						 &tbl[i].id, 1);
			if (err)
				return err;
		} else {
			printf("WARNING could not find node %s: %s.\n",
			       tbl[i].compat, fdt_strerror(off));
		}
	}

	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;

	noff = fdt_node_offset_by_compatible(blob, -1, "arm,mmu-500");
	if (noff < 0) {
		printf("WARNING failed to get smmu node: %s\n",
		       fdt_strerror(noff));
		return noff;
	}

	smmu_ph = fdt_get_phandle(blob, noff);
	if (!smmu_ph) {
		smmu_ph = fdt_create_phandle(blob, noff);
		if (!smmu_ph) {
			printf("WARNING failed to get smmu phandle\n");
			return -1;
		}
	}

	return smmu_ph;
}

void fdt_fixup_icid(void *blob)
{
	int smmu_ph;

	smmu_ph = fdt_get_smmu_phandle(blob);
	if (smmu_ph < 0)
		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
}