static int bcm2835_pinctrl_probe()

in bcm/pinctrl-bcm2835.c [1190:1334]

• 110 lines of code
• 16 McCabe index (conditional complexity)

static int bcm2835_pinctrl_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	struct device_node *np = dev->of_node;
	const struct bcm_plat_data *pdata;
	struct bcm2835_pinctrl *pc;
	struct gpio_irq_chip *girq;
	struct resource iomem;
	int err, i;
	const struct of_device_id *match;
	int is_7211 = 0;

	BUILD_BUG_ON(ARRAY_SIZE(bcm2835_gpio_pins) != BCM2711_NUM_GPIOS);
	BUILD_BUG_ON(ARRAY_SIZE(bcm2835_gpio_groups) != BCM2711_NUM_GPIOS);

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

	platform_set_drvdata(pdev, pc);
	pc->dev = dev;

	err = of_address_to_resource(np, 0, &iomem);
	if (err) {
		dev_err(dev, "could not get IO memory\n");
		return err;
	}

	pc->base = devm_ioremap_resource(dev, &iomem);
	if (IS_ERR(pc->base))
		return PTR_ERR(pc->base);

	match = of_match_node(bcm2835_pinctrl_match, pdev->dev.of_node);
	if (!match)
		return -EINVAL;

	pdata = match->data;
	is_7211 = of_device_is_compatible(np, "brcm,bcm7211-gpio");

	pc->gpio_chip = *pdata->gpio_chip;
	pc->gpio_chip.parent = dev;

	for (i = 0; i < BCM2835_NUM_BANKS; i++) {
		unsigned long events;
		unsigned offset;

		/* clear event detection flags */
		bcm2835_gpio_wr(pc, GPREN0 + i * 4, 0);
		bcm2835_gpio_wr(pc, GPFEN0 + i * 4, 0);
		bcm2835_gpio_wr(pc, GPHEN0 + i * 4, 0);
		bcm2835_gpio_wr(pc, GPLEN0 + i * 4, 0);
		bcm2835_gpio_wr(pc, GPAREN0 + i * 4, 0);
		bcm2835_gpio_wr(pc, GPAFEN0 + i * 4, 0);

		/* clear all the events */
		events = bcm2835_gpio_rd(pc, GPEDS0 + i * 4);
		for_each_set_bit(offset, &events, 32)
			bcm2835_gpio_wr(pc, GPEDS0 + i * 4, BIT(offset));

		raw_spin_lock_init(&pc->irq_lock[i]);
	}

	pc->pctl_desc = *pdata->pctl_desc;
	pc->pctl_dev = devm_pinctrl_register(dev, &pc->pctl_desc, pc);
	if (IS_ERR(pc->pctl_dev)) {
		gpiochip_remove(&pc->gpio_chip);
		return PTR_ERR(pc->pctl_dev);
	}

	pc->gpio_range = *pdata->gpio_range;
	pc->gpio_range.base = pc->gpio_chip.base;
	pc->gpio_range.gc = &pc->gpio_chip;
	pinctrl_add_gpio_range(pc->pctl_dev, &pc->gpio_range);

	girq = &pc->gpio_chip.irq;
	girq->chip = &bcm2835_gpio_irq_chip;
	girq->parent_handler = bcm2835_gpio_irq_handler;
	girq->num_parents = BCM2835_NUM_IRQS;
	girq->parents = devm_kcalloc(dev, BCM2835_NUM_IRQS,
				     sizeof(*girq->parents),
				     GFP_KERNEL);
	if (!girq->parents) {
		pinctrl_remove_gpio_range(pc->pctl_dev, &pc->gpio_range);
		return -ENOMEM;
	}

	if (is_7211) {
		pc->wake_irq = devm_kcalloc(dev, BCM2835_NUM_IRQS,
					    sizeof(*pc->wake_irq),
					    GFP_KERNEL);
		if (!pc->wake_irq)
			return -ENOMEM;
	}

	/*
	 * Use the same handler for all groups: this is necessary
	 * since we use one gpiochip to cover all lines - the
	 * irq handler then needs to figure out which group and
	 * bank that was firing the IRQ and look up the per-group
	 * and bank data.
	 */
	for (i = 0; i < BCM2835_NUM_IRQS; i++) {
		int len;
		char *name;

		girq->parents[i] = irq_of_parse_and_map(np, i);
		if (!is_7211) {
			if (!girq->parents[i]) {
				girq->num_parents = i;
				break;
			}
			continue;
		}
		/* Skip over the all banks interrupts */
		pc->wake_irq[i] = irq_of_parse_and_map(np, i +
						       BCM2835_NUM_IRQS + 1);

		len = strlen(dev_name(pc->dev)) + 16;
		name = devm_kzalloc(pc->dev, len, GFP_KERNEL);
		if (!name)
			return -ENOMEM;

		snprintf(name, len, "%s:bank%d", dev_name(pc->dev), i);

		/* These are optional interrupts */
		err = devm_request_irq(dev, pc->wake_irq[i],
				       bcm2835_gpio_wake_irq_handler,
				       IRQF_SHARED, name, pc);
		if (err)
			dev_warn(dev, "unable to request wake IRQ %d\n",
				 pc->wake_irq[i]);
	}

	girq->default_type = IRQ_TYPE_NONE;
	girq->handler = handle_level_irq;

	err = gpiochip_add_data(&pc->gpio_chip, pc);
	if (err) {
		dev_err(dev, "could not add GPIO chip\n");
		pinctrl_remove_gpio_range(pc->pctl_dev, &pc->gpio_range);
		return err;
	}

	return 0;
}