static int __init aspeed_gpio_probe()

in gpio-aspeed.c [1136:1239]

• 84 lines of code
• 12 McCabe index (conditional complexity)

static int __init aspeed_gpio_probe(struct platform_device *pdev)
{
	const struct of_device_id *gpio_id;
	struct aspeed_gpio *gpio;
	int rc, i, banks, err;
	u32 ngpio;

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

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

	raw_spin_lock_init(&gpio->lock);

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

	gpio->clk = of_clk_get(pdev->dev.of_node, 0);
	if (IS_ERR(gpio->clk)) {
		dev_warn(&pdev->dev,
				"Failed to get clock from devicetree, debouncing disabled\n");
		gpio->clk = NULL;
	}

	gpio->config = gpio_id->data;

	gpio->chip.parent = &pdev->dev;
	err = of_property_read_u32(pdev->dev.of_node, "ngpios", &ngpio);
	gpio->chip.ngpio = (u16) ngpio;
	if (err)
		gpio->chip.ngpio = gpio->config->nr_gpios;
	gpio->chip.direction_input = aspeed_gpio_dir_in;
	gpio->chip.direction_output = aspeed_gpio_dir_out;
	gpio->chip.get_direction = aspeed_gpio_get_direction;
	gpio->chip.request = aspeed_gpio_request;
	gpio->chip.free = aspeed_gpio_free;
	gpio->chip.get = aspeed_gpio_get;
	gpio->chip.set = aspeed_gpio_set;
	gpio->chip.set_config = aspeed_gpio_set_config;
	gpio->chip.label = dev_name(&pdev->dev);
	gpio->chip.base = -1;

	/* Allocate a cache of the output registers */
	banks = DIV_ROUND_UP(gpio->chip.ngpio, 32);
	gpio->dcache = devm_kcalloc(&pdev->dev,
				    banks, sizeof(u32), GFP_KERNEL);
	if (!gpio->dcache)
		return -ENOMEM;

	/*
	 * Populate it with initial values read from the HW and switch
	 * all command sources to the ARM by default
	 */
	for (i = 0; i < banks; i++) {
		const struct aspeed_gpio_bank *bank = &aspeed_gpio_banks[i];
		void __iomem *addr = bank_reg(gpio, bank, reg_rdata);
		gpio->dcache[i] = ioread32(addr);
		aspeed_gpio_change_cmd_source(gpio, bank, 0, GPIO_CMDSRC_ARM);
		aspeed_gpio_change_cmd_source(gpio, bank, 1, GPIO_CMDSRC_ARM);
		aspeed_gpio_change_cmd_source(gpio, bank, 2, GPIO_CMDSRC_ARM);
		aspeed_gpio_change_cmd_source(gpio, bank, 3, GPIO_CMDSRC_ARM);
	}

	/* Optionally set up an irqchip if there is an IRQ */
	rc = platform_get_irq(pdev, 0);
	if (rc > 0) {
		struct gpio_irq_chip *girq;

		gpio->irq = rc;
		girq = &gpio->chip.irq;
		girq->chip = &gpio->irqc;
		girq->chip->name = dev_name(&pdev->dev);
		girq->chip->irq_ack = aspeed_gpio_irq_ack;
		girq->chip->irq_mask = aspeed_gpio_irq_mask;
		girq->chip->irq_unmask = aspeed_gpio_irq_unmask;
		girq->chip->irq_set_type = aspeed_gpio_set_type;
		girq->parent_handler = aspeed_gpio_irq_handler;
		girq->num_parents = 1;
		girq->parents = devm_kcalloc(&pdev->dev, 1,
					     sizeof(*girq->parents),
					     GFP_KERNEL);
		if (!girq->parents)
			return -ENOMEM;
		girq->parents[0] = gpio->irq;
		girq->default_type = IRQ_TYPE_NONE;
		girq->handler = handle_bad_irq;
		girq->init_valid_mask = aspeed_init_irq_valid_mask;
	}

	gpio->offset_timer =
		devm_kzalloc(&pdev->dev, gpio->chip.ngpio, GFP_KERNEL);
	if (!gpio->offset_timer)
		return -ENOMEM;

	rc = devm_gpiochip_add_data(&pdev->dev, &gpio->chip, gpio);
	if (rc < 0)
		return rc;

	return 0;
}