Move duplicate code to a new public function.

This new function can be used by other drivers to reserve the use of GPIO
pins.

Anyway, the use of ofw_gpiobus_parse_gpios() is preferred when possible.

Requested by:	Michal Meloun

3 files changed, 29 insertions, 29 deletions

diff --git a/sys/dev/gpio/gpiobus.c b/sys/dev/gpio/gpiobus.c
index c2cc793..f3ad49c 100644
--- a/sys/dev/gpio/gpiobus.c
+++ b/sys/dev/gpio/gpiobus.c
@@ -228,6 +228,29 @@ gpiobus_free_ivars(struct gpiobus_ivar *devi)
 	}
 }
 
+int
+gpiobus_map_pin(device_t bus, device_t child, uint32_t pin)
+{
+	struct gpiobus_softc *sc;
+
+	sc = device_get_softc(bus);
+	/* Consistency check. */
+	if (pin >= sc->sc_npins) {
+		device_printf(child,
+		    "invalid pin %d, max: %d\n", pin, sc->sc_npins - 1);
+		return (-1);
+	}
+	/* Mark pin as mapped and give warning if it's already mapped. */
+	if (sc->sc_pins_mapped[pin]) {
+		device_printf(child,
+		    "warning: pin %d is already mapped\n", pin);
+		return (-1);
+	}
+	sc->sc_pins_mapped[pin] = 1;
+
+	return (0);
+}
+
 static int
 gpiobus_parse_pins(struct gpiobus_softc *sc, device_t child, int mask)
 {
@@ -252,24 +275,12 @@ gpiobus_parse_pins(struct gpiobus_softc *sc, device_t child, int mask)
 	for (i = 0; i < 32; i++) {
 		if ((mask & (1 << i)) == 0)
 			continue;
-		if (i >= sc->sc_npins) {
-			device_printf(child, 
-			    "invalid pin %d, max: %d\n", i, sc->sc_npins - 1);
+		/* Reserve the GPIO pin. */
+		if (gpiobus_map_pin(sc->sc_busdev, child, i) != 0) {
 			gpiobus_free_ivars(devi);
 			return (EINVAL);
 		}
-
 		devi->pins[npins++] = i;
-		/*
-		 * Mark pin as mapped and give warning if it's already mapped
-		 */
-		if (sc->sc_pins_mapped[i]) {
-			device_printf(child, 
-			    "warning: pin %d is already mapped\n", i);
-			gpiobus_free_ivars(devi);
-			return (EINVAL);
-		}
-		sc->sc_pins_mapped[i] = 1;
 	}
 
 	return (0);
diff --git a/sys/dev/gpio/gpiobusvar.h b/sys/dev/gpio/gpiobusvar.h
index c735a3c..beffb7f 100644
--- a/sys/dev/gpio/gpiobusvar.h
+++ b/sys/dev/gpio/gpiobusvar.h
@@ -110,6 +110,7 @@ int gpiobus_detach_bus(device_t);
 int gpiobus_init_softc(device_t);
 int gpiobus_alloc_ivars(struct gpiobus_ivar *);
 void gpiobus_free_ivars(struct gpiobus_ivar *);
+int gpiobus_map_pin(device_t, device_t, uint32_t);
 
 extern driver_t gpiobus_driver;
 
diff --git a/sys/dev/gpio/ofw_gpiobus.c b/sys/dev/gpio/ofw_gpiobus.c
index c6f6e8e..be961ea 100644
--- a/sys/dev/gpio/ofw_gpiobus.c
+++ b/sys/dev/gpio/ofw_gpiobus.c
@@ -272,22 +272,10 @@ ofw_gpiobus_parse_gpios_impl(device_t consumer, phandle_t cnode, char *pname,
 			    "cannot map the gpios specifier.\n");
 			goto fail;
 		}
-		/* Consistency check. */
-		if ((*pins)[j].pin >= bussc->sc_npins) {
-			device_printf(consumer, "invalid pin %d, max: %d\n",
-			    (*pins)[j].pin, bussc->sc_npins - 1);
+		/* Reserve the GPIO pin. */
+		if (gpiobus_map_pin(bussc->sc_busdev, consumer,
+		    (*pins)[j].pin) != 0)
 			goto fail;
-		}
-		/*
-		 * Mark pin as mapped and give warning if it's already mapped.
-		 */
-		if (bussc->sc_pins_mapped[(*pins)[j].pin]) {
-			device_printf(consumer,
-			    "warning: pin %d is already mapped\n",
-			    pins[j]->pin);
-			goto fail;
-		}
-		bussc->sc_pins_mapped[(*pins)[j].pin] = 1;
 		j++;
 		i += gpiocells + 1;
 	}