static int pl031_probe()

in rtc-pl031.c [294:385]

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

static int pl031_probe(struct amba_device *adev, const struct amba_id *id)
{
	int ret;
	struct pl031_local *ldata;
	struct pl031_vendor_data *vendor = id->data;
	struct rtc_class_ops *ops;
	unsigned long time, data;

	ret = amba_request_regions(adev, NULL);
	if (ret)
		goto err_req;

	ldata = devm_kzalloc(&adev->dev, sizeof(struct pl031_local),
			     GFP_KERNEL);
	ops = devm_kmemdup(&adev->dev, &vendor->ops, sizeof(vendor->ops),
			   GFP_KERNEL);
	if (!ldata || !ops) {
		ret = -ENOMEM;
		goto out;
	}

	ldata->vendor = vendor;
	ldata->base = devm_ioremap(&adev->dev, adev->res.start,
				   resource_size(&adev->res));
	if (!ldata->base) {
		ret = -ENOMEM;
		goto out;
	}

	amba_set_drvdata(adev, ldata);

	dev_dbg(&adev->dev, "designer ID = 0x%02x\n", amba_manf(adev));
	dev_dbg(&adev->dev, "revision = 0x%01x\n", amba_rev(adev));

	data = readl(ldata->base + RTC_CR);
	/* Enable the clockwatch on ST Variants */
	if (vendor->clockwatch)
		data |= RTC_CR_CWEN;
	else
		data |= RTC_CR_EN;
	writel(data, ldata->base + RTC_CR);

	/*
	 * On ST PL031 variants, the RTC reset value does not provide correct
	 * weekday for 2000-01-01. Correct the erroneous sunday to saturday.
	 */
	if (vendor->st_weekday) {
		if (readl(ldata->base + RTC_YDR) == 0x2000) {
			time = readl(ldata->base + RTC_DR);
			if ((time &
			     (RTC_MON_MASK | RTC_MDAY_MASK | RTC_WDAY_MASK))
			    == 0x02120000) {
				time = time | (0x7 << RTC_WDAY_SHIFT);
				writel(0x2000, ldata->base + RTC_YLR);
				writel(time, ldata->base + RTC_LR);
			}
		}
	}

	if (!adev->irq[0])
		clear_bit(RTC_FEATURE_ALARM, ldata->rtc->features);

	device_init_wakeup(&adev->dev, true);
	ldata->rtc = devm_rtc_allocate_device(&adev->dev);
	if (IS_ERR(ldata->rtc)) {
		ret = PTR_ERR(ldata->rtc);
		goto out;
	}

	ldata->rtc->ops = ops;
	ldata->rtc->range_min = vendor->range_min;
	ldata->rtc->range_max = vendor->range_max;

	ret = devm_rtc_register_device(ldata->rtc);
	if (ret)
		goto out;

	if (adev->irq[0]) {
		ret = request_irq(adev->irq[0], pl031_interrupt,
				  vendor->irqflags, "rtc-pl031", ldata);
		if (ret)
			goto out;
		dev_pm_set_wake_irq(&adev->dev, adev->irq[0]);
	}
	return 0;

out:
	amba_release_regions(adev);
err_req:

	return ret;
}