static int __init egpio_probe()

in gpio-htc-egpio.c [263:365]

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

static int __init egpio_probe(struct platform_device *pdev)
{
	struct htc_egpio_platform_data *pdata = dev_get_platdata(&pdev->dev);
	struct resource   *res;
	struct egpio_info *ei;
	struct gpio_chip  *chip;
	unsigned int      irq, irq_end;
	int               i;

	/* Initialize ei data structure. */
	ei = devm_kzalloc(&pdev->dev, sizeof(*ei), GFP_KERNEL);
	if (!ei)
		return -ENOMEM;

	spin_lock_init(&ei->lock);

	/* Find chained irq */
	res = platform_get_resource(pdev, IORESOURCE_IRQ, 0);
	if (res)
		ei->chained_irq = res->start;

	/* Map egpio chip into virtual address space. */
	ei->base_addr = devm_platform_ioremap_resource(pdev, 0);
	if (IS_ERR(ei->base_addr))
		return PTR_ERR(ei->base_addr);

	if ((pdata->bus_width != 16) && (pdata->bus_width != 32))
		return -EINVAL;

	ei->bus_shift = fls(pdata->bus_width - 1) - 3;
	pr_debug("bus_shift = %d\n", ei->bus_shift);

	if ((pdata->reg_width != 8) && (pdata->reg_width != 16))
		return -EINVAL;

	ei->reg_shift = fls(pdata->reg_width - 1);
	pr_debug("reg_shift = %d\n", ei->reg_shift);

	ei->reg_mask = (1 << pdata->reg_width) - 1;

	platform_set_drvdata(pdev, ei);

	ei->nchips = pdata->num_chips;
	ei->chip = devm_kcalloc(&pdev->dev,
				ei->nchips, sizeof(struct egpio_chip),
				GFP_KERNEL);
	if (!ei->chip)
		return -ENOMEM;

	for (i = 0; i < ei->nchips; i++) {
		ei->chip[i].reg_start = pdata->chip[i].reg_start;
		ei->chip[i].cached_values = pdata->chip[i].initial_values;
		ei->chip[i].is_out = pdata->chip[i].direction;
		ei->chip[i].dev = &(pdev->dev);
		chip = &(ei->chip[i].chip);
		chip->label = devm_kasprintf(&pdev->dev, GFP_KERNEL,
					     "htc-egpio-%d",
					     i);
		if (!chip->label)
			return -ENOMEM;

		chip->parent          = &pdev->dev;
		chip->owner           = THIS_MODULE;
		chip->get             = egpio_get;
		chip->set             = egpio_set;
		chip->direction_input = egpio_direction_input;
		chip->direction_output = egpio_direction_output;
		chip->get_direction   = egpio_get_direction;
		chip->base            = pdata->chip[i].gpio_base;
		chip->ngpio           = pdata->chip[i].num_gpios;

		gpiochip_add_data(chip, &ei->chip[i]);
	}

	/* Set initial pin values */
	egpio_write_cache(ei);

	ei->irq_start = pdata->irq_base;
	ei->nirqs = pdata->num_irqs;
	ei->ack_register = pdata->ack_register;

	if (ei->chained_irq) {
		/* Setup irq handlers */
		ei->ack_write = 0xFFFF;
		if (pdata->invert_acks)
			ei->ack_write = 0;
		irq_end = ei->irq_start + ei->nirqs;
		for (irq = ei->irq_start; irq < irq_end; irq++) {
			irq_set_chip_and_handler(irq, &egpio_muxed_chip,
						 handle_simple_irq);
			irq_set_chip_data(irq, ei);
			irq_clear_status_flags(irq, IRQ_NOREQUEST | IRQ_NOPROBE);
		}
		irq_set_irq_type(ei->chained_irq, IRQ_TYPE_EDGE_RISING);
		irq_set_chained_handler_and_data(ei->chained_irq,
						 egpio_handler, ei);
		ack_irqs(ei);

		device_init_wakeup(&pdev->dev, 1);
	}

	return 0;
}