static int max310x_probe()

in serial/max310x.c [1253:1427]

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

static int max310x_probe(struct device *dev, const struct max310x_devtype *devtype,
			 struct regmap *regmap, int irq)
{
	int i, ret, fmin, fmax, freq;
	struct max310x_port *s;
	u32 uartclk = 0;
	bool xtal;

	if (IS_ERR(regmap))
		return PTR_ERR(regmap);

	/* Alloc port structure */
	s = devm_kzalloc(dev, struct_size(s, p, devtype->nr), GFP_KERNEL);
	if (!s) {
		dev_err(dev, "Error allocating port structure\n");
		return -ENOMEM;
	}

	/* Always ask for fixed clock rate from a property. */
	device_property_read_u32(dev, "clock-frequency", &uartclk);

	xtal = device_property_match_string(dev, "clock-names", "osc") < 0;
	if (xtal)
		s->clk = devm_clk_get_optional(dev, "xtal");
	else
		s->clk = devm_clk_get_optional(dev, "osc");
	if (IS_ERR(s->clk))
		return PTR_ERR(s->clk);

	ret = clk_prepare_enable(s->clk);
	if (ret)
		return ret;

	freq = clk_get_rate(s->clk);
	if (freq == 0)
		freq = uartclk;
	if (freq == 0) {
		dev_err(dev, "Cannot get clock rate\n");
		ret = -EINVAL;
		goto out_clk;
	}

	if (xtal) {
		fmin = 1000000;
		fmax = 4000000;
	} else {
		fmin = 500000;
		fmax = 35000000;
	}

	/* Check frequency limits */
	if (freq < fmin || freq > fmax) {
		ret = -ERANGE;
		goto out_clk;
	}

	s->regmap = regmap;
	s->devtype = devtype;
	dev_set_drvdata(dev, s);

	/* Check device to ensure we are talking to what we expect */
	ret = devtype->detect(dev);
	if (ret)
		goto out_clk;

	for (i = 0; i < devtype->nr; i++) {
		unsigned int offs = i << 5;

		/* Reset port */
		regmap_write(s->regmap, MAX310X_MODE2_REG + offs,
			     MAX310X_MODE2_RST_BIT);
		/* Clear port reset */
		regmap_write(s->regmap, MAX310X_MODE2_REG + offs, 0);

		/* Wait for port startup */
		do {
			regmap_read(s->regmap,
				    MAX310X_BRGDIVLSB_REG + offs, &ret);
		} while (ret != 0x01);

		regmap_write(s->regmap, MAX310X_MODE1_REG + offs,
			     devtype->mode1);
	}

	uartclk = max310x_set_ref_clk(dev, s, freq, xtal);
	dev_dbg(dev, "Reference clock set to %i Hz\n", uartclk);

	for (i = 0; i < devtype->nr; i++) {
		unsigned int line;

		line = find_first_zero_bit(max310x_lines, MAX310X_UART_NRMAX);
		if (line == MAX310X_UART_NRMAX) {
			ret = -ERANGE;
			goto out_uart;
		}

		/* Initialize port data */
		s->p[i].port.line	= line;
		s->p[i].port.dev	= dev;
		s->p[i].port.irq	= irq;
		s->p[i].port.type	= PORT_MAX310X;
		s->p[i].port.fifosize	= MAX310X_FIFO_SIZE;
		s->p[i].port.flags	= UPF_FIXED_TYPE | UPF_LOW_LATENCY;
		s->p[i].port.iotype	= UPIO_PORT;
		s->p[i].port.iobase	= i * 0x20;
		s->p[i].port.membase	= (void __iomem *)~0;
		s->p[i].port.uartclk	= uartclk;
		s->p[i].port.rs485_config = max310x_rs485_config;
		s->p[i].port.ops	= &max310x_ops;
		/* Disable all interrupts */
		max310x_port_write(&s->p[i].port, MAX310X_IRQEN_REG, 0);
		/* Clear IRQ status register */
		max310x_port_read(&s->p[i].port, MAX310X_IRQSTS_REG);
		/* Initialize queue for start TX */
		INIT_WORK(&s->p[i].tx_work, max310x_tx_proc);
		/* Initialize queue for changing LOOPBACK mode */
		INIT_WORK(&s->p[i].md_work, max310x_md_proc);
		/* Initialize queue for changing RS485 mode */
		INIT_WORK(&s->p[i].rs_work, max310x_rs_proc);
		/* Initialize SPI-transfer buffers */
		s->p[i].wr_header = (s->p[i].port.iobase + MAX310X_THR_REG) |
				    MAX310X_WRITE_BIT;
		s->p[i].rd_header = (s->p[i].port.iobase + MAX310X_RHR_REG);

		/* Register port */
		ret = uart_add_one_port(&max310x_uart, &s->p[i].port);
		if (ret) {
			s->p[i].port.dev = NULL;
			goto out_uart;
		}
		set_bit(line, max310x_lines);

		/* Go to suspend mode */
		devtype->power(&s->p[i].port, 0);
	}

#ifdef CONFIG_GPIOLIB
	/* Setup GPIO cotroller */
	s->gpio.owner		= THIS_MODULE;
	s->gpio.parent		= dev;
	s->gpio.label		= devtype->name;
	s->gpio.direction_input	= max310x_gpio_direction_input;
	s->gpio.get		= max310x_gpio_get;
	s->gpio.direction_output= max310x_gpio_direction_output;
	s->gpio.set		= max310x_gpio_set;
	s->gpio.set_config	= max310x_gpio_set_config;
	s->gpio.base		= -1;
	s->gpio.ngpio		= devtype->nr * 4;
	s->gpio.can_sleep	= 1;
	ret = devm_gpiochip_add_data(dev, &s->gpio, s);
	if (ret)
		goto out_uart;
#endif

	/* Setup interrupt */
	ret = devm_request_threaded_irq(dev, irq, NULL, max310x_ist,
					IRQF_ONESHOT | IRQF_SHARED, dev_name(dev), s);
	if (!ret)
		return 0;

	dev_err(dev, "Unable to reguest IRQ %i\n", irq);

out_uart:
	for (i = 0; i < devtype->nr; i++) {
		if (s->p[i].port.dev) {
			uart_remove_one_port(&max310x_uart, &s->p[i].port);
			clear_bit(s->p[i].port.line, max310x_lines);
		}
	}

out_clk:
	clk_disable_unprepare(s->clk);

	return ret;
}