static int mxs_dcp_probe()

in mxs-dcp.c [971:1141]

• 124 lines of code
• 25 McCabe index (conditional complexity)

static int mxs_dcp_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct dcp *sdcp = NULL;
	int i, ret;
	int dcp_vmi_irq, dcp_irq;

	if (global_sdcp) {
		dev_err(dev, "Only one DCP instance allowed!\n");
		return -ENODEV;
	}

	dcp_vmi_irq = platform_get_irq(pdev, 0);
	if (dcp_vmi_irq < 0)
		return dcp_vmi_irq;

	dcp_irq = platform_get_irq(pdev, 1);
	if (dcp_irq < 0)
		return dcp_irq;

	sdcp = devm_kzalloc(dev, sizeof(*sdcp), GFP_KERNEL);
	if (!sdcp)
		return -ENOMEM;

	sdcp->dev = dev;
	sdcp->base = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(sdcp->base))
		return PTR_ERR(sdcp->base);


	ret = devm_request_irq(dev, dcp_vmi_irq, mxs_dcp_irq, 0,
			       "dcp-vmi-irq", sdcp);
	if (ret) {
		dev_err(dev, "Failed to claim DCP VMI IRQ!\n");
		return ret;
	}

	ret = devm_request_irq(dev, dcp_irq, mxs_dcp_irq, 0,
			       "dcp-irq", sdcp);
	if (ret) {
		dev_err(dev, "Failed to claim DCP IRQ!\n");
		return ret;
	}

	/* Allocate coherent helper block. */
	sdcp->coh = devm_kzalloc(dev, sizeof(*sdcp->coh) + DCP_ALIGNMENT,
				   GFP_KERNEL);
	if (!sdcp->coh)
		return -ENOMEM;

	/* Re-align the structure so it fits the DCP constraints. */
	sdcp->coh = PTR_ALIGN(sdcp->coh, DCP_ALIGNMENT);

	/* DCP clock is optional, only used on some SOCs */
	sdcp->dcp_clk = devm_clk_get(dev, "dcp");
	if (IS_ERR(sdcp->dcp_clk)) {
		if (sdcp->dcp_clk != ERR_PTR(-ENOENT))
			return PTR_ERR(sdcp->dcp_clk);
		sdcp->dcp_clk = NULL;
	}
	ret = clk_prepare_enable(sdcp->dcp_clk);
	if (ret)
		return ret;

	/* Restart the DCP block. */
	ret = stmp_reset_block(sdcp->base);
	if (ret) {
		dev_err(dev, "Failed reset\n");
		goto err_disable_unprepare_clk;
	}

	/* Initialize control register. */
	writel(MXS_DCP_CTRL_GATHER_RESIDUAL_WRITES |
	       MXS_DCP_CTRL_ENABLE_CONTEXT_CACHING | 0xf,
	       sdcp->base + MXS_DCP_CTRL);

	/* Enable all DCP DMA channels. */
	writel(MXS_DCP_CHANNELCTRL_ENABLE_CHANNEL_MASK,
	       sdcp->base + MXS_DCP_CHANNELCTRL);

	/*
	 * We do not enable context switching. Give the context buffer a
	 * pointer to an illegal address so if context switching is
	 * inadvertantly enabled, the DCP will return an error instead of
	 * trashing good memory. The DCP DMA cannot access ROM, so any ROM
	 * address will do.
	 */
	writel(0xffff0000, sdcp->base + MXS_DCP_CONTEXT);
	for (i = 0; i < DCP_MAX_CHANS; i++)
		writel(0xffffffff, sdcp->base + MXS_DCP_CH_N_STAT_CLR(i));
	writel(0xffffffff, sdcp->base + MXS_DCP_STAT_CLR);

	global_sdcp = sdcp;

	platform_set_drvdata(pdev, sdcp);

	for (i = 0; i < DCP_MAX_CHANS; i++) {
		spin_lock_init(&sdcp->lock[i]);
		init_completion(&sdcp->completion[i]);
		crypto_init_queue(&sdcp->queue[i], 50);
	}

	/* Create the SHA and AES handler threads. */
	sdcp->thread[DCP_CHAN_HASH_SHA] = kthread_run(dcp_chan_thread_sha,
						      NULL, "mxs_dcp_chan/sha");
	if (IS_ERR(sdcp->thread[DCP_CHAN_HASH_SHA])) {
		dev_err(dev, "Error starting SHA thread!\n");
		ret = PTR_ERR(sdcp->thread[DCP_CHAN_HASH_SHA]);
		goto err_disable_unprepare_clk;
	}

	sdcp->thread[DCP_CHAN_CRYPTO] = kthread_run(dcp_chan_thread_aes,
						    NULL, "mxs_dcp_chan/aes");
	if (IS_ERR(sdcp->thread[DCP_CHAN_CRYPTO])) {
		dev_err(dev, "Error starting SHA thread!\n");
		ret = PTR_ERR(sdcp->thread[DCP_CHAN_CRYPTO]);
		goto err_destroy_sha_thread;
	}

	/* Register the various crypto algorithms. */
	sdcp->caps = readl(sdcp->base + MXS_DCP_CAPABILITY1);

	if (sdcp->caps & MXS_DCP_CAPABILITY1_AES128) {
		ret = crypto_register_skciphers(dcp_aes_algs,
						ARRAY_SIZE(dcp_aes_algs));
		if (ret) {
			/* Failed to register algorithm. */
			dev_err(dev, "Failed to register AES crypto!\n");
			goto err_destroy_aes_thread;
		}
	}

	if (sdcp->caps & MXS_DCP_CAPABILITY1_SHA1) {
		ret = crypto_register_ahash(&dcp_sha1_alg);
		if (ret) {
			dev_err(dev, "Failed to register %s hash!\n",
				dcp_sha1_alg.halg.base.cra_name);
			goto err_unregister_aes;
		}
	}

	if (sdcp->caps & MXS_DCP_CAPABILITY1_SHA256) {
		ret = crypto_register_ahash(&dcp_sha256_alg);
		if (ret) {
			dev_err(dev, "Failed to register %s hash!\n",
				dcp_sha256_alg.halg.base.cra_name);
			goto err_unregister_sha1;
		}
	}

	return 0;

err_unregister_sha1:
	if (sdcp->caps & MXS_DCP_CAPABILITY1_SHA1)
		crypto_unregister_ahash(&dcp_sha1_alg);

err_unregister_aes:
	if (sdcp->caps & MXS_DCP_CAPABILITY1_AES128)
		crypto_unregister_skciphers(dcp_aes_algs, ARRAY_SIZE(dcp_aes_algs));

err_destroy_aes_thread:
	kthread_stop(sdcp->thread[DCP_CHAN_CRYPTO]);

err_destroy_sha_thread:
	kthread_stop(sdcp->thread[DCP_CHAN_HASH_SHA]);

err_disable_unprepare_clk:
	clk_disable_unprepare(sdcp->dcp_clk);

	return ret;
}