static int wm831x_power_probe()

in supply/wm831x_power.c [532:695]

• 145 lines of code
• 20 McCabe index (conditional complexity)

static int wm831x_power_probe(struct platform_device *pdev)
{
	struct wm831x *wm831x = dev_get_drvdata(pdev->dev.parent);
	struct wm831x_pdata *wm831x_pdata = wm831x->dev->platform_data;
	struct wm831x_power *power;
	int ret, irq, i;

	power = devm_kzalloc(&pdev->dev, sizeof(struct wm831x_power),
			     GFP_KERNEL);
	if (power == NULL)
		return -ENOMEM;

	power->wm831x = wm831x;
	platform_set_drvdata(pdev, power);

	if (wm831x_pdata && wm831x_pdata->wm831x_num) {
		snprintf(power->wall_name, sizeof(power->wall_name),
			 "wm831x-wall.%d", wm831x_pdata->wm831x_num);
		snprintf(power->battery_name, sizeof(power->wall_name),
			 "wm831x-battery.%d", wm831x_pdata->wm831x_num);
		snprintf(power->usb_name, sizeof(power->wall_name),
			 "wm831x-usb.%d", wm831x_pdata->wm831x_num);
	} else {
		snprintf(power->wall_name, sizeof(power->wall_name),
			 "wm831x-wall");
		snprintf(power->battery_name, sizeof(power->wall_name),
			 "wm831x-battery");
		snprintf(power->usb_name, sizeof(power->wall_name),
			 "wm831x-usb");
	}

	/* We ignore configuration failures since we can still read back
	 * the status without enabling the charger.
	 */
	wm831x_config_battery(wm831x);

	power->wall_desc.name = power->wall_name;
	power->wall_desc.type = POWER_SUPPLY_TYPE_MAINS;
	power->wall_desc.properties = wm831x_wall_props;
	power->wall_desc.num_properties = ARRAY_SIZE(wm831x_wall_props);
	power->wall_desc.get_property = wm831x_wall_get_prop;
	power->wall = power_supply_register(&pdev->dev, &power->wall_desc,
					    NULL);
	if (IS_ERR(power->wall)) {
		ret = PTR_ERR(power->wall);
		goto err;
	}

	power->usb_desc.name = power->usb_name,
	power->usb_desc.type = POWER_SUPPLY_TYPE_USB;
	power->usb_desc.properties = wm831x_usb_props;
	power->usb_desc.num_properties = ARRAY_SIZE(wm831x_usb_props);
	power->usb_desc.get_property = wm831x_usb_get_prop;
	power->usb = power_supply_register(&pdev->dev, &power->usb_desc, NULL);
	if (IS_ERR(power->usb)) {
		ret = PTR_ERR(power->usb);
		goto err_wall;
	}

	ret = wm831x_reg_read(wm831x, WM831X_CHARGER_CONTROL_1);
	if (ret < 0)
		goto err_wall;
	power->have_battery = ret & WM831X_CHG_ENA;

	if (power->have_battery) {
		power->battery_desc.name = power->battery_name;
		power->battery_desc.properties = wm831x_bat_props;
		power->battery_desc.num_properties = ARRAY_SIZE(wm831x_bat_props);
		power->battery_desc.get_property = wm831x_bat_get_prop;
		power->battery_desc.use_for_apm = 1;
		power->battery = power_supply_register(&pdev->dev,
						       &power->battery_desc,
						       NULL);
		if (IS_ERR(power->battery)) {
			ret = PTR_ERR(power->battery);
			goto err_usb;
		}
	}

	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "SYSLO"));
	ret = request_threaded_irq(irq, NULL, wm831x_syslo_irq,
				   IRQF_TRIGGER_RISING | IRQF_ONESHOT, "System power low",
				   power);
	if (ret != 0) {
		dev_err(&pdev->dev, "Failed to request SYSLO IRQ %d: %d\n",
			irq, ret);
		goto err_battery;
	}

	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "PWR SRC"));
	ret = request_threaded_irq(irq, NULL, wm831x_pwr_src_irq,
				   IRQF_TRIGGER_RISING | IRQF_ONESHOT, "Power source",
				   power);
	if (ret != 0) {
		dev_err(&pdev->dev, "Failed to request PWR SRC IRQ %d: %d\n",
			irq, ret);
		goto err_syslo;
	}

	for (i = 0; i < ARRAY_SIZE(wm831x_bat_irqs); i++) {
		irq = wm831x_irq(wm831x,
				 platform_get_irq_byname(pdev,
							 wm831x_bat_irqs[i]));
		ret = request_threaded_irq(irq, NULL, wm831x_bat_irq,
					   IRQF_TRIGGER_RISING | IRQF_ONESHOT,
					   wm831x_bat_irqs[i],
					   power);
		if (ret != 0) {
			dev_err(&pdev->dev,
				"Failed to request %s IRQ %d: %d\n",
				wm831x_bat_irqs[i], irq, ret);
			goto err_bat_irq;
		}
	}

	power->usb_phy = devm_usb_get_phy_by_phandle(&pdev->dev, "phys", 0);
	ret = PTR_ERR_OR_ZERO(power->usb_phy);

	switch (ret) {
	case 0:
		power->usb_notify.notifier_call = wm831x_usb_limit_change;
		ret = usb_register_notifier(power->usb_phy, &power->usb_notify);
		if (ret) {
			dev_err(&pdev->dev, "Failed to register notifier: %d\n",
				ret);
			goto err_bat_irq;
		}
		break;
	case -EINVAL:
	case -ENODEV:
		/* ignore missing usb-phy, it's optional */
		power->usb_phy = NULL;
		ret = 0;
		break;
	default:
		dev_err(&pdev->dev, "Failed to find USB phy: %d\n", ret);
		fallthrough;
	case -EPROBE_DEFER:
		goto err_bat_irq;
	}

	return ret;

err_bat_irq:
	--i;
	for (; i >= 0; i--) {
		irq = platform_get_irq_byname(pdev, wm831x_bat_irqs[i]);
		free_irq(irq, power);
	}
	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "PWR SRC"));
	free_irq(irq, power);
err_syslo:
	irq = wm831x_irq(wm831x, platform_get_irq_byname(pdev, "SYSLO"));
	free_irq(irq, power);
err_battery:
	if (power->have_battery)
		power_supply_unregister(power->battery);
err_usb:
	power_supply_unregister(power->usb);
err_wall:
	power_supply_unregister(power->wall);
err:
	return ret;
}