int mvebu_pinctrl_probe()

in mvebu/pinctrl-mvebu.c [568:749]

• 132 lines of code
• 28 McCabe index (conditional complexity)

int mvebu_pinctrl_probe(struct platform_device *pdev)
{
	struct mvebu_pinctrl_soc_info *soc = dev_get_platdata(&pdev->dev);
	struct mvebu_pinctrl *pctl;
	struct pinctrl_pin_desc *pdesc;
	unsigned gid, n, k;
	unsigned size, noname = 0;
	char *noname_buf;
	void *p;
	int ret;

	if (!soc || !soc->controls || !soc->modes) {
		dev_err(&pdev->dev, "wrong pinctrl soc info\n");
		return -EINVAL;
	}

	pctl = devm_kzalloc(&pdev->dev, sizeof(struct mvebu_pinctrl),
			GFP_KERNEL);
	if (!pctl)
		return -ENOMEM;

	pctl->desc.name = dev_name(&pdev->dev);
	pctl->desc.owner = THIS_MODULE;
	pctl->desc.pctlops = &mvebu_pinctrl_ops;
	pctl->desc.pmxops = &mvebu_pinmux_ops;
	pctl->desc.confops = &mvebu_pinconf_ops;
	pctl->variant = soc->variant;
	pctl->dev = &pdev->dev;
	platform_set_drvdata(pdev, pctl);

	/* count controls and create names for mvebu generic
	   register controls; also does sanity checks */
	pctl->num_groups = 0;
	pctl->desc.npins = 0;
	for (n = 0; n < soc->ncontrols; n++) {
		const struct mvebu_mpp_ctrl *ctrl = &soc->controls[n];

		pctl->desc.npins += ctrl->npins;
		/* initialize control's pins[] array */
		for (k = 0; k < ctrl->npins; k++)
			ctrl->pins[k] = ctrl->pid + k;

		/*
		 * We allow to pass controls with NULL name that we treat
		 * as a range of one-pin groups with generic mvebu register
		 * controls.
		 */
		if (!ctrl->name) {
			pctl->num_groups += ctrl->npins;
			noname += ctrl->npins;
		} else {
			pctl->num_groups += 1;
		}
	}

	pdesc = devm_kcalloc(&pdev->dev,
			     pctl->desc.npins,
			     sizeof(struct pinctrl_pin_desc),
			     GFP_KERNEL);
	if (!pdesc)
		return -ENOMEM;

	for (n = 0; n < pctl->desc.npins; n++)
		pdesc[n].number = n;
	pctl->desc.pins = pdesc;

	/*
	 * allocate groups and name buffers for unnamed groups.
	 */
	size = pctl->num_groups * sizeof(*pctl->groups) + noname * 8;
	p = devm_kzalloc(&pdev->dev, size, GFP_KERNEL);
	if (!p)
		return -ENOMEM;

	pctl->groups = p;
	noname_buf = p + pctl->num_groups * sizeof(*pctl->groups);

	/* assign mpp controls to groups */
	gid = 0;
	for (n = 0; n < soc->ncontrols; n++) {
		const struct mvebu_mpp_ctrl *ctrl = &soc->controls[n];
		struct mvebu_mpp_ctrl_data *data = soc->control_data ?
						   &soc->control_data[n] : NULL;

		pctl->groups[gid].gid = gid;
		pctl->groups[gid].ctrl = ctrl;
		pctl->groups[gid].data = data;
		pctl->groups[gid].name = ctrl->name;
		pctl->groups[gid].pins = ctrl->pins;
		pctl->groups[gid].npins = ctrl->npins;

		/*
		 * We treat unnamed controls as a range of one-pin groups
		 * with generic mvebu register controls. Use one group for
		 * each in this range and assign a default group name.
		 */
		if (!ctrl->name) {
			pctl->groups[gid].name = noname_buf;
			pctl->groups[gid].npins = 1;
			sprintf(noname_buf, "mpp%d", ctrl->pid+0);
			noname_buf += 8;

			for (k = 1; k < ctrl->npins; k++) {
				gid++;
				pctl->groups[gid].gid = gid;
				pctl->groups[gid].ctrl = ctrl;
				pctl->groups[gid].data = data;
				pctl->groups[gid].name = noname_buf;
				pctl->groups[gid].pins = &ctrl->pins[k];
				pctl->groups[gid].npins = 1;
				sprintf(noname_buf, "mpp%d", ctrl->pid+k);
				noname_buf += 8;
			}
		}
		gid++;
	}

	/* assign mpp modes to groups */
	for (n = 0; n < soc->nmodes; n++) {
		struct mvebu_mpp_mode *mode = &soc->modes[n];
		struct mvebu_mpp_ctrl_setting *set = &mode->settings[0];
		struct mvebu_pinctrl_group *grp;
		unsigned num_settings;
		unsigned supp_settings;

		for (num_settings = 0, supp_settings = 0; ; set++) {
			if (!set->name)
				break;

			num_settings++;

			/* skip unsupported settings for this variant */
			if (pctl->variant && !(pctl->variant & set->variant))
				continue;

			supp_settings++;

			/* find gpio/gpo/gpi settings */
			if (strcmp(set->name, "gpio") == 0)
				set->flags = MVEBU_SETTING_GPI |
					MVEBU_SETTING_GPO;
			else if (strcmp(set->name, "gpo") == 0)
				set->flags = MVEBU_SETTING_GPO;
			else if (strcmp(set->name, "gpi") == 0)
				set->flags = MVEBU_SETTING_GPI;
		}

		/* skip modes with no settings for this variant */
		if (!supp_settings)
			continue;

		grp = mvebu_pinctrl_find_group_by_pid(pctl, mode->pid);
		if (!grp) {
			dev_warn(&pdev->dev, "unknown pinctrl group %d\n",
				mode->pid);
			continue;
		}

		grp->settings = mode->settings;
		grp->num_settings = num_settings;
	}

	ret = mvebu_pinctrl_build_functions(pdev, pctl);
	if (ret) {
		dev_err(&pdev->dev, "unable to build functions\n");
		return ret;
	}

	pctl->pctldev = devm_pinctrl_register(&pdev->dev, &pctl->desc, pctl);
	if (IS_ERR(pctl->pctldev)) {
		dev_err(&pdev->dev, "unable to register pinctrl driver\n");
		return PTR_ERR(pctl->pctldev);
	}

	dev_info(&pdev->dev, "registered pinctrl driver\n");

	/* register gpio ranges */
	for (n = 0; n < soc->ngpioranges; n++)
		pinctrl_add_gpio_range(pctl->pctldev, &soc->gpioranges[n]);

	return 0;
}