static int stratix10_svc_drv_probe()

in stratix10-svc.c [964:1064]

• 79 lines of code
• 11 McCabe index (conditional complexity)

static int stratix10_svc_drv_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct stratix10_svc_controller *controller;
	struct stratix10_svc_chan *chans;
	struct gen_pool *genpool;
	struct stratix10_svc_sh_memory *sh_memory;
	struct stratix10_svc *svc;

	svc_invoke_fn *invoke_fn;
	size_t fifo_size;
	int ret;

	/* get SMC or HVC function */
	invoke_fn = get_invoke_func(dev);
	if (IS_ERR(invoke_fn))
		return -EINVAL;

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

	sh_memory->invoke_fn = invoke_fn;
	ret = svc_get_sh_memory(pdev, sh_memory);
	if (ret)
		return ret;

	genpool = svc_create_memory_pool(pdev, sh_memory);
	if (!genpool)
		return -ENOMEM;

	/* allocate service controller and supporting channel */
	controller = devm_kzalloc(dev, sizeof(*controller), GFP_KERNEL);
	if (!controller)
		return -ENOMEM;

	chans = devm_kmalloc_array(dev, SVC_NUM_CHANNEL,
				   sizeof(*chans), GFP_KERNEL | __GFP_ZERO);
	if (!chans)
		return -ENOMEM;

	controller->dev = dev;
	controller->num_chans = SVC_NUM_CHANNEL;
	controller->num_active_client = 0;
	controller->chans = chans;
	controller->genpool = genpool;
	controller->task = NULL;
	controller->invoke_fn = invoke_fn;
	init_completion(&controller->complete_status);

	fifo_size = sizeof(struct stratix10_svc_data) * SVC_NUM_DATA_IN_FIFO;
	ret = kfifo_alloc(&controller->svc_fifo, fifo_size, GFP_KERNEL);
	if (ret) {
		dev_err(dev, "failed to allocate FIFO\n");
		return ret;
	}
	spin_lock_init(&controller->svc_fifo_lock);

	chans[0].scl = NULL;
	chans[0].ctrl = controller;
	chans[0].name = SVC_CLIENT_FPGA;
	spin_lock_init(&chans[0].lock);

	chans[1].scl = NULL;
	chans[1].ctrl = controller;
	chans[1].name = SVC_CLIENT_RSU;
	spin_lock_init(&chans[1].lock);

	list_add_tail(&controller->node, &svc_ctrl);
	platform_set_drvdata(pdev, controller);

	/* add svc client device(s) */
	svc = devm_kzalloc(dev, sizeof(*svc), GFP_KERNEL);
	if (!svc) {
		ret = -ENOMEM;
		goto err_free_kfifo;
	}

	svc->stratix10_svc_rsu = platform_device_alloc(STRATIX10_RSU, 0);
	if (!svc->stratix10_svc_rsu) {
		dev_err(dev, "failed to allocate %s device\n", STRATIX10_RSU);
		ret = -ENOMEM;
		goto err_free_kfifo;
	}

	ret = platform_device_add(svc->stratix10_svc_rsu);
	if (ret)
		goto err_put_device;

	dev_set_drvdata(dev, svc);

	pr_info("Intel Service Layer Driver Initialized\n");

	return 0;

err_put_device:
	platform_device_put(svc->stratix10_svc_rsu);
err_free_kfifo:
	kfifo_free(&controller->svc_fifo);
	return ret;
}