static int fsl_mc_bus_probe()

in fsl-mc/fsl-mc-bus.c [1107:1230]

• 92 lines of code
• 17 McCabe index (conditional complexity)

static int fsl_mc_bus_probe(struct platform_device *pdev)
{
	struct fsl_mc_obj_desc obj_desc;
	int error;
	struct fsl_mc *mc;
	struct fsl_mc_device *mc_bus_dev = NULL;
	struct fsl_mc_io *mc_io = NULL;
	int container_id;
	phys_addr_t mc_portal_phys_addr;
	u32 mc_portal_size, mc_stream_id;
	struct resource *plat_res;

	mc = devm_kzalloc(&pdev->dev, sizeof(*mc), GFP_KERNEL);
	if (!mc)
		return -ENOMEM;

	platform_set_drvdata(pdev, mc);

	plat_res = platform_get_resource(pdev, IORESOURCE_MEM, 1);
	if (plat_res) {
		mc->fsl_mc_regs = devm_ioremap_resource(&pdev->dev, plat_res);
		if (IS_ERR(mc->fsl_mc_regs))
			return PTR_ERR(mc->fsl_mc_regs);
	}

	if (mc->fsl_mc_regs) {
		if (IS_ENABLED(CONFIG_ACPI) && !dev_of_node(&pdev->dev)) {
			mc_stream_id = readl(mc->fsl_mc_regs + FSL_MC_FAPR);
			/*
			 * HW ORs the PL and BMT bit, places the result in bit
			 * 14 of the StreamID and ORs in the ICID. Calculate it
			 * accordingly.
			 */
			mc_stream_id = (mc_stream_id & 0xffff) |
				((mc_stream_id & (MC_FAPR_PL | MC_FAPR_BMT)) ?
					BIT(14) : 0);
			error = acpi_dma_configure_id(&pdev->dev,
						      DEV_DMA_COHERENT,
						      &mc_stream_id);
			if (error == -EPROBE_DEFER)
				return error;
			if (error)
				dev_warn(&pdev->dev,
					 "failed to configure dma: %d.\n",
					 error);
		}

		/*
		 * Some bootloaders pause the MC firmware before booting the
		 * kernel so that MC will not cause faults as soon as the
		 * SMMU probes due to the fact that there's no configuration
		 * in place for MC.
		 * At this point MC should have all its SMMU setup done so make
		 * sure it is resumed.
		 */
		writel(readl(mc->fsl_mc_regs + FSL_MC_GCR1) &
			     (~(GCR1_P1_STOP | GCR1_P2_STOP)),
		       mc->fsl_mc_regs + FSL_MC_GCR1);
	}

	/*
	 * Get physical address of MC portal for the root DPRC:
	 */
	plat_res = platform_get_resource(pdev, IORESOURCE_MEM, 0);
	mc_portal_phys_addr = plat_res->start;
	mc_portal_size = resource_size(plat_res);
	mc_portal_base_phys_addr = mc_portal_phys_addr & ~0x3ffffff;

	error = fsl_create_mc_io(&pdev->dev, mc_portal_phys_addr,
				 mc_portal_size, NULL,
				 FSL_MC_IO_ATOMIC_CONTEXT_PORTAL, &mc_io);
	if (error < 0)
		return error;

	error = mc_get_version(mc_io, 0, &mc_version);
	if (error != 0) {
		dev_err(&pdev->dev,
			"mc_get_version() failed with error %d\n", error);
		goto error_cleanup_mc_io;
	}

	dev_info(&pdev->dev, "MC firmware version: %u.%u.%u\n",
		 mc_version.major, mc_version.minor, mc_version.revision);

	if (dev_of_node(&pdev->dev)) {
		error = get_mc_addr_translation_ranges(&pdev->dev,
						&mc->translation_ranges,
						&mc->num_translation_ranges);
		if (error < 0)
			goto error_cleanup_mc_io;
	}

	error = dprc_get_container_id(mc_io, 0, &container_id);
	if (error < 0) {
		dev_err(&pdev->dev,
			"dprc_get_container_id() failed: %d\n", error);
		goto error_cleanup_mc_io;
	}

	memset(&obj_desc, 0, sizeof(struct fsl_mc_obj_desc));
	error = dprc_get_api_version(mc_io, 0,
				     &obj_desc.ver_major,
				     &obj_desc.ver_minor);
	if (error < 0)
		goto error_cleanup_mc_io;

	obj_desc.vendor = FSL_MC_VENDOR_FREESCALE;
	strcpy(obj_desc.type, "dprc");
	obj_desc.id = container_id;
	obj_desc.irq_count = 1;
	obj_desc.region_count = 0;

	error = fsl_mc_device_add(&obj_desc, mc_io, &pdev->dev, &mc_bus_dev);
	if (error < 0)
		goto error_cleanup_mc_io;

	mc->root_mc_bus_dev = mc_bus_dev;
	mc_bus_dev->dev.fwnode = pdev->dev.fwnode;
	return 0;

error_cleanup_mc_io:
	fsl_destroy_mc_io(mc_io);
	return error;
}