Add sysctls for changing GPIO pins function

Submitted by:	Luiz Otavio O Souza

1 files changed, 163 insertions, 26 deletions

diff --git a/sys/arm/broadcom/bcm2835/bcm2835_gpio.c b/sys/arm/broadcom/bcm2835/bcm2835_gpio.c
index a1d97b4..8029d80 100644
--- a/sys/arm/broadcom/bcm2835/bcm2835_gpio.c
+++ b/sys/arm/broadcom/bcm2835/bcm2835_gpio.c
@@ -38,6 +38,7 @@ __FBSDID("$FreeBSD$");
 #include <sys/lock.h>
 #include <sys/mutex.h>
 #include <sys/gpio.h>
+#include <sys/sysctl.h>
 
 #include <machine/bus.h>
 #include <machine/cpu.h>
@@ -66,6 +67,11 @@ __FBSDID("$FreeBSD$");
 #define	BCM_GPIO_DEFAULT_CAPS	(GPIO_PIN_INPUT | GPIO_PIN_OUTPUT |	\
     GPIO_PIN_PULLUP | GPIO_PIN_PULLDOWN)
 
+struct bcm_gpio_sysctl {
+	struct bcm_gpio_softc	*sc;
+	uint32_t		pin;
+};
+
 struct bcm_gpio_softc {
 	device_t		sc_dev;
 	struct mtx		sc_mtx;
@@ -78,6 +84,7 @@ struct bcm_gpio_softc {
 	int			sc_ro_npins;
 	int			sc_ro_pins[BCM_GPIO_PINS];
 	struct gpio_pin		sc_gpio_pins[BCM_GPIO_PINS];
+	struct bcm_gpio_sysctl	sc_sysctl[BCM_GPIO_PINS];
 };
 
 enum bcm_gpio_fsel {
@@ -99,6 +106,7 @@ enum bcm_gpio_pud {
 
 #define	BCM_GPIO_LOCK(_sc)	mtx_lock(&_sc->sc_mtx)
 #define	BCM_GPIO_UNLOCK(_sc)	mtx_unlock(&_sc->sc_mtx)
+#define	BCM_GPIO_LOCK_ASSERT(_sc)	mtx_assert(&_sc->sc_mtx, MA_OWNED)
 
 #define	BCM_GPIO_GPFSEL(_bank)	0x00 + _bank * 4
 #define	BCM_GPIO_GPSET(_bank)	0x1c + _bank * 4
@@ -126,53 +134,89 @@ bcm_gpio_pin_is_ro(struct bcm_gpio_softc *sc, int pin)
 static uint32_t
 bcm_gpio_get_function(struct bcm_gpio_softc *sc, uint32_t pin)
 {
-	uint32_t bank, data, offset;
+	uint32_t bank, func, offset;
 
 	/* Five banks, 10 pins per bank, 3 bits per pin. */
 	bank = pin / 10;
 	offset = (pin - bank * 10) * 3;
 
 	BCM_GPIO_LOCK(sc);
-	data = (BCM_GPIO_READ(sc, BCM_GPIO_GPFSEL(bank)) >> offset) & 7;
+	func = (BCM_GPIO_READ(sc, BCM_GPIO_GPFSEL(bank)) >> offset) & 7;
 	BCM_GPIO_UNLOCK(sc);
 
-#ifdef	DEBUG
-	device_printf(sc->sc_dev, "pin %d function: ", pin);
-	switch (data) {
+	return (func);
+}
+
+static void
+bcm_gpio_func_str(uint32_t nfunc, char *buf, int bufsize)
+{
+
+	switch (nfunc) {
 	case BCM_GPIO_INPUT:
-		printf("input\n");
+		strncpy(buf, "input", bufsize);
 		break;
 	case BCM_GPIO_OUTPUT:
-		printf("output\n");
+		strncpy(buf, "output", bufsize);
 		break;
 	case BCM_GPIO_ALT0:
-		printf("alt0\n");
+		strncpy(buf, "alt0", bufsize);
 		break;
 	case BCM_GPIO_ALT1:
-		printf("alt1\n");
+		strncpy(buf, "alt1", bufsize);
 		break;
 	case BCM_GPIO_ALT2:
-		printf("alt2\n");
+		strncpy(buf, "alt2", bufsize);
 		break;
 	case BCM_GPIO_ALT3:
-		printf("alt3\n");
+		strncpy(buf, "alt3", bufsize);
 		break;
 	case BCM_GPIO_ALT4:
-		printf("alt4\n");
+		strncpy(buf, "alt4", bufsize);
 		break;
 	case BCM_GPIO_ALT5:
-		printf("alt5\n");
+		strncpy(buf, "alt5", bufsize);
 		break;
+	default:
+		strncpy(buf, "invalid", bufsize);
 	}
-#endif
+}
+
+static int
+bcm_gpio_str_func(char *func, uint32_t *nfunc)
+{
+
+	if (strcasecmp(func, "input") == 0)
+		*nfunc = BCM_GPIO_INPUT;
+	else if (strcasecmp(func, "output") == 0)
+		*nfunc = BCM_GPIO_OUTPUT;
+	else if (strcasecmp(func, "alt0") == 0)
+		*nfunc = BCM_GPIO_ALT0;
+	else if (strcasecmp(func, "alt1") == 0)
+		*nfunc = BCM_GPIO_ALT1;
+	else if (strcasecmp(func, "alt2") == 0)
+		*nfunc = BCM_GPIO_ALT2;
+	else if (strcasecmp(func, "alt3") == 0)
+		*nfunc = BCM_GPIO_ALT3;
+	else if (strcasecmp(func, "alt4") == 0)
+		*nfunc = BCM_GPIO_ALT4;
+	else if (strcasecmp(func, "alt5") == 0)
+		*nfunc = BCM_GPIO_ALT5;
+	else
+		return (-1);
+
+	return (0);
+}
+
+static uint32_t
+bcm_gpio_func_flag(uint32_t nfunc)
+{
 
-	switch (data) {
+	switch (nfunc) {
 	case BCM_GPIO_INPUT:
 		return (GPIO_PIN_INPUT);
 	case BCM_GPIO_OUTPUT:
 		return (GPIO_PIN_OUTPUT);
 	}
-
 	return (0);
 }
 
@@ -181,16 +225,17 @@ bcm_gpio_set_function(struct bcm_gpio_softc *sc, uint32_t pin, uint32_t f)
 {
 	uint32_t bank, data, offset;
 
+	/* Must be called with lock held. */
+	BCM_GPIO_LOCK_ASSERT(sc);
+
 	/* Five banks, 10 pins per bank, 3 bits per pin. */
 	bank = pin / 10;
 	offset = (pin - bank * 10) * 3;
 
-	BCM_GPIO_LOCK(sc);
 	data = BCM_GPIO_READ(sc, BCM_GPIO_GPFSEL(bank));
 	data &= ~(7 << offset);
 	data |= (f << offset);
 	BCM_GPIO_WRITE(sc, BCM_GPIO_GPFSEL(bank), data);
-	BCM_GPIO_UNLOCK(sc);
 }
 
 static void
@@ -198,17 +243,18 @@ bcm_gpio_set_pud(struct bcm_gpio_softc *sc, uint32_t pin, uint32_t state)
 {
 	uint32_t bank, offset;
 
+	/* Must be called with lock held. */
+	BCM_GPIO_LOCK_ASSERT(sc);
+
 	bank = pin / 32;
 	offset = pin - 32 * bank;
 
-	BCM_GPIO_LOCK(sc);
 	BCM_GPIO_WRITE(sc, BCM_GPIO_GPPUD(0), state);
 	DELAY(10);
 	BCM_GPIO_WRITE(sc, BCM_GPIO_GPPUDCLK(bank), (1 << offset));
 	DELAY(10);
 	BCM_GPIO_WRITE(sc, BCM_GPIO_GPPUD(0), 0);
 	BCM_GPIO_WRITE(sc, BCM_GPIO_GPPUDCLK(bank), 0);
-	BCM_GPIO_UNLOCK(sc);
 }
 
 static void
@@ -216,6 +262,8 @@ bcm_gpio_pin_configure(struct bcm_gpio_softc *sc, struct gpio_pin *pin,
     unsigned int flags)
 {
 
+	BCM_GPIO_LOCK(sc);
+
 	/*
 	 * Manage input/output.
 	 */
@@ -244,6 +292,8 @@ bcm_gpio_pin_configure(struct bcm_gpio_softc *sc, struct gpio_pin *pin,
 		}
 	} else 
 		bcm_gpio_set_pud(sc, pin->gp_pin, BCM_GPIO_NONE);
+
+	BCM_GPIO_UNLOCK(sc);
 }
 
 static int
@@ -471,7 +521,7 @@ bcm_gpio_get_ro_pins(struct bcm_gpio_softc *sc)
 			printf(",");
 		printf("%d", sc->sc_ro_pins[i]);
 	}
-        if (i > 0)
+	if (i > 0)
 		printf(".");
 	printf("\n");
 
@@ -479,6 +529,89 @@ bcm_gpio_get_ro_pins(struct bcm_gpio_softc *sc)
 }
 
 static int
+bcm_gpio_func_proc(SYSCTL_HANDLER_ARGS)
+{
+	char buf[16];
+	struct bcm_gpio_softc *sc;
+	struct bcm_gpio_sysctl *sc_sysctl;
+	uint32_t nfunc;
+	int i, error;
+
+	sc_sysctl = arg1;
+	sc = sc_sysctl->sc;
+
+	/* Get the current pin function. */
+	nfunc = bcm_gpio_get_function(sc, sc_sysctl->pin);
+	bcm_gpio_func_str(nfunc, buf, sizeof(buf));
+
+	error = sysctl_handle_string(oidp, buf, sizeof(buf), req);
+	if (error != 0 || req->newptr == NULL)
+		return (error);
+
+	/* Parse the user supplied string and check for a valid pin function. */
+	if (bcm_gpio_str_func(buf, &nfunc) != 0)
+		return (EINVAL);
+
+	BCM_GPIO_LOCK(sc);
+
+	/* Disable pull-up or pull-down on pin. */
+	bcm_gpio_set_pud(sc, sc_sysctl->pin, BCM_GPIO_NONE);
+
+	/* And now set the pin function. */
+	bcm_gpio_set_function(sc, sc_sysctl->pin, nfunc);
+
+	/* Update the pin flags. */
+	for (i = 0; i < sc->sc_gpio_npins; i++) {
+		if (sc->sc_gpio_pins[i].gp_pin == sc_sysctl->pin)
+			break;
+	}
+	if (i < sc->sc_gpio_npins)
+		sc->sc_gpio_pins[i].gp_flags = bcm_gpio_func_flag(nfunc);
+
+	BCM_GPIO_UNLOCK(sc);
+
+	return (0);
+}
+
+static void
+bcm_gpio_sysctl_init(struct bcm_gpio_softc *sc)
+{
+	char pinbuf[3];
+	struct bcm_gpio_sysctl *sc_sysctl;
+	struct sysctl_ctx_list *ctx;
+	struct sysctl_oid *tree_node, *pin_node, *pinN_node;
+	struct sysctl_oid_list *tree, *pin_tree, *pinN_tree;
+	int i;
+
+	/*
+	 * Add per-pin sysctl tree/handlers.
+	 */
+	ctx = device_get_sysctl_ctx(sc->sc_dev);
+ 	tree_node = device_get_sysctl_tree(sc->sc_dev);
+ 	tree = SYSCTL_CHILDREN(tree_node);
+	pin_node = SYSCTL_ADD_NODE(ctx, tree, OID_AUTO, "pin",
+	    CTLFLAG_RW, NULL, "GPIO Pins");
+	pin_tree = SYSCTL_CHILDREN(pin_node);
+
+	for (i = 0; i < sc->sc_gpio_npins; i++) {
+
+		snprintf(pinbuf, sizeof(pinbuf), "%d", i);
+		pinN_node = SYSCTL_ADD_NODE(ctx, pin_tree, OID_AUTO, pinbuf,
+		    CTLFLAG_RD, NULL, "GPIO Pin");
+		pinN_tree = SYSCTL_CHILDREN(pinN_node);
+
+		sc->sc_sysctl[i].sc = sc;
+		sc_sysctl = &sc->sc_sysctl[i];
+		sc_sysctl->sc = sc;
+		sc_sysctl->pin = sc->sc_gpio_pins[i].gp_pin;
+		SYSCTL_ADD_PROC(ctx, pinN_tree, OID_AUTO, "function",
+		    CTLFLAG_RW | CTLTYPE_STRING, sc_sysctl,
+		    sizeof(struct bcm_gpio_sysctl), bcm_gpio_func_proc,
+		    "A", "Pin Function");
+	}
+}
+
+static int
 bcm_gpio_get_reserved_pins(struct bcm_gpio_softc *sc)
 {
 	int i, j, len, npins;
@@ -499,7 +632,7 @@ bcm_gpio_get_reserved_pins(struct bcm_gpio_softc *sc)
 	 */
 	reserved = 0;
 	while ((node != 0) && (reserved == 0)) {
-        	len = OF_getprop(node, "name", name,
+		len = OF_getprop(node, "name", name,
 		    sizeof(name) - 1);
 		name[len] = 0;
 		if (strcmp(name, "reserved") == 0)
@@ -532,7 +665,7 @@ bcm_gpio_get_reserved_pins(struct bcm_gpio_softc *sc)
 		sc->sc_ro_pins[j++ + sc->sc_ro_npins] = fdt32_to_cpu(pins[i]);
 	}
 	sc->sc_ro_npins += j;
-        if (i > 0)
+	if (i > 0)
 		printf(".");
 	printf("\n");
 
@@ -553,6 +686,7 @@ static int
 bcm_gpio_attach(device_t dev)
 {
 	struct bcm_gpio_softc *sc = device_get_softc(dev);
+	uint32_t func;
 	int i, j, rid;
 	phandle_t gpio;
 
@@ -600,15 +734,18 @@ bcm_gpio_attach(device_t dev)
 			continue;
 		snprintf(sc->sc_gpio_pins[i].gp_name, GPIOMAXNAME,
 		    "pin %d", j);
+		func = bcm_gpio_get_function(sc, j);
 		sc->sc_gpio_pins[i].gp_pin = j;
 		sc->sc_gpio_pins[i].gp_caps = BCM_GPIO_DEFAULT_CAPS;
-		sc->sc_gpio_pins[i].gp_flags = bcm_gpio_get_function(sc, j);
+		sc->sc_gpio_pins[i].gp_flags = bcm_gpio_func_flag(func);
 		i++;
 	}
 	sc->sc_gpio_npins = i;
 
-        device_add_child(dev, "gpioc", device_get_unit(dev));
-        device_add_child(dev, "gpiobus", device_get_unit(dev));
+	bcm_gpio_sysctl_init(sc);
+
+	device_add_child(dev, "gpioc", device_get_unit(dev));
+	device_add_child(dev, "gpiobus", device_get_unit(dev));
 	return (bus_generic_attach(dev));
 
 fail: