gpio: stmfx: add function stmfx_read_reg and stmfx_write_reg

[platform/kernel/u-boot.git] / drivers / pinctrl / pinctrl-stmfx.c

diff --git a/drivers/pinctrl/pinctrl-stmfx.c b/drivers/pinctrl/pinctrl-stmfx.c
index 5431df9..5d15424 100644 (file)
--- a/drivers/pinctrl/pinctrl-stmfx.c
+++ b/drivers/pinctrl/pinctrl-stmfx.c
@@ -11,9 +11,12 @@
 #include <asm/gpio.h>
 #include <dm/device.h>
 #include <dm/device-internal.h>
+#include <dm/device_compat.h>
 #include <dm/lists.h>
 #include <dm/pinctrl.h>
 #include <linux/bitfield.h>
+#include <linux/bitops.h>
+#include <linux/delay.h>
 #include <power/regulator.h>
 
 /* STMFX pins = GPIO[15:0] + aGPIO[7:0] */
@@ -71,17 +74,51 @@ static int stmfx_write(struct udevice *dev, uint offset, unsigned int val)
        return dm_i2c_reg_write(dev_get_parent(dev), offset, val);
 }
 
-static int stmfx_gpio_get(struct udevice *dev, unsigned int offset)
+static int stmfx_read_reg(struct udevice *dev, u8 reg_base, uint offset)
 {
-       u32 reg = STMFX_REG_GPIO_STATE + get_reg(offset);
+       u8 reg = reg_base + get_reg(offset);
        u32 mask = get_mask(offset);
        int ret;
 
        ret = stmfx_read(dev, reg);
+       if (ret < 0)
+               return ret;
 
        return ret < 0 ? ret : !!(ret & mask);
 }
 
+static int stmfx_write_reg(struct udevice *dev, u8 reg_base, uint offset,
+                          uint val)
+{
+       u8 reg = reg_base + get_reg(offset);
+       u32 mask = get_mask(offset);
+       int ret;
+
+       ret = stmfx_read(dev, reg);
+       if (ret < 0)
+               return ret;
+       ret = (ret & ~mask) | (val ? mask : 0);
+
+       return stmfx_write(dev, reg, ret);
+}
+
+static int stmfx_conf_set_pupd(struct udevice *dev, unsigned int offset,
+                              uint pupd)
+{
+       return stmfx_write_reg(dev, STMFX_REG_GPIO_PUPD, offset, pupd);
+}
+
+static int stmfx_conf_set_type(struct udevice *dev, unsigned int offset,
+                              uint type)
+{
+       return stmfx_write_reg(dev, STMFX_REG_GPIO_TYPE, offset, type);
+}
+
+static int stmfx_gpio_get(struct udevice *dev, unsigned int offset)
+{
+       return stmfx_read_reg(dev, STMFX_REG_GPIO_STATE, offset);
+}
+
 static int stmfx_gpio_set(struct udevice *dev, unsigned int offset, int value)
 {
        u32 reg = value ? STMFX_REG_GPO_SET : STMFX_REG_GPO_CLR;
@@ -92,50 +129,28 @@ static int stmfx_gpio_set(struct udevice *dev, unsigned int offset, int value)
 
 static int stmfx_gpio_get_function(struct udevice *dev, unsigned int offset)
 {
-       u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
-       u32 mask = get_mask(offset);
-       int ret;
-
-       ret = stmfx_read(dev, reg);
+       int ret = stmfx_read_reg(dev, STMFX_REG_GPIO_DIR, offset);
 
        if (ret < 0)
                return ret;
        /* On stmfx, gpio pins direction is (0)input, (1)output. */
 
-       return ret & mask ? GPIOF_OUTPUT : GPIOF_INPUT;
+       return ret ? GPIOF_OUTPUT : GPIOF_INPUT;
 }
 
 static int stmfx_gpio_direction_input(struct udevice *dev, unsigned int offset)
 {
-       u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
-       u32 mask = get_mask(offset);
-       int ret;
-
-       ret = stmfx_read(dev, reg);
-       if (ret < 0)
-               return ret;
-
-       ret &= ~mask;
-
-       return stmfx_write(dev, reg, ret & ~mask);
+       return stmfx_write_reg(dev, STMFX_REG_GPIO_DIR, offset, 0);
 }
 
 static int stmfx_gpio_direction_output(struct udevice *dev,
                                       unsigned int offset, int value)
 {
-       u32 reg = STMFX_REG_GPIO_DIR + get_reg(offset);
-       u32 mask = get_mask(offset);
-       int ret;
-
-       ret = stmfx_gpio_set(dev, offset, value);
+       int ret = stmfx_gpio_set(dev, offset, value);
        if (ret < 0)
                return ret;
 
-       ret = stmfx_read(dev, reg);
-       if (ret < 0)
-               return ret;
-
-       return stmfx_write(dev, reg, ret | mask);
+       return stmfx_write_reg(dev, STMFX_REG_GPIO_DIR, offset, 1);
 }
 
 static int stmfx_gpio_probe(struct udevice *dev)
@@ -187,36 +202,6 @@ static const struct pinconf_param stmfx_pinctrl_conf_params[] = {
        { "output-low", PIN_CONFIG_OUTPUT, 0 },
 };
 
-static int stmfx_pinctrl_set_pupd(struct udevice *dev,
-                                 unsigned int pin, u32 pupd)
-{
-       u8 reg = STMFX_REG_GPIO_PUPD + get_reg(pin);
-       u32 mask = get_mask(pin);
-       int ret;
-
-       ret = stmfx_read(dev, reg);
-       if (ret < 0)
-               return ret;
-       ret = (ret & ~mask) | (pupd ? mask : 0);
-
-       return stmfx_write(dev, reg, ret);
-}
-
-static int stmfx_pinctrl_set_type(struct udevice *dev,
-                                 unsigned int pin, u32 type)
-{
-       u8 reg = STMFX_REG_GPIO_TYPE + get_reg(pin);
-       u32 mask = get_mask(pin);
-       int ret;
-
-       ret = stmfx_read(dev, reg);
-       if (ret < 0)
-               return ret;
-       ret = (ret & ~mask) | (type ? mask : 0);
-
-       return stmfx_write(dev, reg, ret);
-}
-
 static int stmfx_pinctrl_conf_set(struct udevice *dev, unsigned int pin,
                                  unsigned int param, unsigned int arg)
 {
@@ -231,23 +216,23 @@ static int stmfx_pinctrl_conf_set(struct udevice *dev, unsigned int pin,
        switch (param) {
        case PIN_CONFIG_BIAS_PULL_PIN_DEFAULT:
        case PIN_CONFIG_BIAS_DISABLE:
+       case PIN_CONFIG_DRIVE_PUSH_PULL:
+               ret = stmfx_conf_set_type(dev, pin, 0);
+               break;
        case PIN_CONFIG_BIAS_PULL_DOWN:
-               ret = stmfx_pinctrl_set_pupd(dev, pin, 0);
+               ret = stmfx_conf_set_type(dev, pin, 1);
+               if (ret)
+                       return ret;
+               ret = stmfx_conf_set_pupd(dev, pin, 0);
                break;
        case PIN_CONFIG_BIAS_PULL_UP:
-               ret = stmfx_pinctrl_set_pupd(dev, pin, 1);
+               ret = stmfx_conf_set_type(dev, pin, 1);
+               if (ret)
+                       return ret;
+               ret = stmfx_conf_set_pupd(dev, pin, 1);
                break;
        case PIN_CONFIG_DRIVE_OPEN_DRAIN:
-               if (dir == GPIOF_OUTPUT)
-                       ret = stmfx_pinctrl_set_type(dev, pin, 1);
-               else
-                       ret = stmfx_pinctrl_set_type(dev, pin, 0);
-               break;
-       case PIN_CONFIG_DRIVE_PUSH_PULL:
-               if (dir == GPIOF_OUTPUT)
-                       ret = stmfx_pinctrl_set_type(dev, pin, 0);
-               else
-                       ret = stmfx_pinctrl_set_type(dev, pin, 1);
+               ret = stmfx_conf_set_type(dev, pin, 1);
                break;
        case PIN_CONFIG_OUTPUT:
                ret = stmfx_gpio_direction_output(plat->gpio, pin, arg);
@@ -351,11 +336,12 @@ static int stmfx_chip_init(struct udevice *dev)
        int ret;
        struct dm_i2c_chip *chip = dev_get_parent_platdata(dev);
 
-       id = dm_i2c_reg_read(dev, STMFX_REG_CHIP_ID);
-       if (id < 0) {
-               dev_err(dev, "error reading chip id: %d\n", id);
+       ret = dm_i2c_reg_read(dev, STMFX_REG_CHIP_ID);
+       if (ret < 0) {
+               dev_err(dev, "error reading chip id: %d\n", ret);
                return ret;
        }
+       id = (u8)ret;
        /*
         * Check that ID is the complement of the I2C address:
         * STMFX I2C address follows the 7-bit format (MSB), that's why