static int wdat_wdt_probe()

in wdat_wdt.c [313:466]

• 121 lines of code
• 25 McCabe index (conditional complexity)

static int wdat_wdt_probe(struct platform_device *pdev)
{
	struct device *dev = &pdev->dev;
	const struct acpi_wdat_entry *entries;
	const struct acpi_table_wdat *tbl;
	struct wdat_wdt *wdat;
	struct resource *res;
	void __iomem **regs;
	acpi_status status;
	int i, ret;

	status = acpi_get_table(ACPI_SIG_WDAT, 0,
				(struct acpi_table_header **)&tbl);
	if (ACPI_FAILURE(status))
		return -ENODEV;

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

	regs = devm_kcalloc(dev, pdev->num_resources, sizeof(*regs),
			    GFP_KERNEL);
	if (!regs)
		return -ENOMEM;

	/* WDAT specification wants to have >= 1ms period */
	if (tbl->timer_period < 1)
		return -EINVAL;
	if (tbl->min_count > tbl->max_count)
		return -EINVAL;

	wdat->period = tbl->timer_period;
	wdat->wdd.min_hw_heartbeat_ms = wdat->period * tbl->min_count;
	wdat->wdd.max_hw_heartbeat_ms = wdat->period * tbl->max_count;
	wdat->stopped_in_sleep = tbl->flags & ACPI_WDAT_STOPPED;
	wdat->wdd.info = &wdat_wdt_info;
	wdat->wdd.ops = &wdat_wdt_ops;
	wdat->pdev = pdev;

	/* Request and map all resources */
	for (i = 0; i < pdev->num_resources; i++) {
		void __iomem *reg;

		res = &pdev->resource[i];
		if (resource_type(res) == IORESOURCE_MEM) {
			reg = devm_ioremap_resource(dev, res);
			if (IS_ERR(reg))
				return PTR_ERR(reg);
		} else if (resource_type(res) == IORESOURCE_IO) {
			reg = devm_ioport_map(dev, res->start, 1);
			if (!reg)
				return -ENOMEM;
		} else {
			dev_err(dev, "Unsupported resource\n");
			return -EINVAL;
		}

		regs[i] = reg;
	}

	entries = (struct acpi_wdat_entry *)(tbl + 1);
	for (i = 0; i < tbl->entries; i++) {
		const struct acpi_generic_address *gas;
		struct wdat_instruction *instr;
		struct list_head *instructions;
		unsigned int action;
		struct resource r;
		int j;

		action = entries[i].action;
		if (action >= MAX_WDAT_ACTIONS) {
			dev_dbg(dev, "Skipping unknown action: %u\n", action);
			continue;
		}

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

		INIT_LIST_HEAD(&instr->node);
		instr->entry = entries[i];

		gas = &entries[i].register_region;

		memset(&r, 0, sizeof(r));
		r.start = gas->address;
		r.end = r.start + ACPI_ACCESS_BYTE_WIDTH(gas->access_width) - 1;
		if (gas->space_id == ACPI_ADR_SPACE_SYSTEM_MEMORY) {
			r.flags = IORESOURCE_MEM;
		} else if (gas->space_id == ACPI_ADR_SPACE_SYSTEM_IO) {
			r.flags = IORESOURCE_IO;
		} else {
			dev_dbg(dev, "Unsupported address space: %d\n",
				gas->space_id);
			continue;
		}

		/* Find the matching resource */
		for (j = 0; j < pdev->num_resources; j++) {
			res = &pdev->resource[j];
			if (resource_contains(res, &r)) {
				instr->reg = regs[j] + r.start - res->start;
				break;
			}
		}

		if (!instr->reg) {
			dev_err(dev, "I/O resource not found\n");
			return -EINVAL;
		}

		instructions = wdat->instructions[action];
		if (!instructions) {
			instructions = devm_kzalloc(dev,
						    sizeof(*instructions),
						    GFP_KERNEL);
			if (!instructions)
				return -ENOMEM;

			INIT_LIST_HEAD(instructions);
			wdat->instructions[action] = instructions;
		}

		list_add_tail(&instr->node, instructions);
	}

	wdat_wdt_boot_status(wdat);
	wdat_wdt_set_running(wdat);

	ret = wdat_wdt_enable_reboot(wdat);
	if (ret)
		return ret;

	platform_set_drvdata(pdev, wdat);

	/*
	 * Set initial timeout so that userspace has time to configure the
	 * watchdog properly after it has opened the device. In some cases
	 * the BIOS default is too short and causes immediate reboot.
	 */
	if (timeout * 1000 < wdat->wdd.min_hw_heartbeat_ms ||
	    timeout * 1000 > wdat->wdd.max_hw_heartbeat_ms) {
		dev_warn(dev, "Invalid timeout %d given, using %d\n",
			 timeout, WDAT_DEFAULT_TIMEOUT);
		timeout = WDAT_DEFAULT_TIMEOUT;
	}

	ret = wdat_wdt_set_timeout(&wdat->wdd, timeout);
	if (ret)
		return ret;

	watchdog_set_nowayout(&wdat->wdd, nowayout);
	return devm_watchdog_register_device(dev, &wdat->wdd);
}