From 863981e96738983919de841ec669e157e6bdaeb0 Mon Sep 17 00:00:00 2001
From: André Fabian Silva Delgado <emulatorman@parabola.nu>
Date: Sun, 11 Sep 2016 04:34:46 -0300
Subject: Linux-libre 4.7.1-gnu

---
 drivers/pinctrl/sh-pfc/pinctrl.c | 124 +++++++++++++++++++++++++++++++++++----
 1 file changed, 112 insertions(+), 12 deletions(-)

(limited to 'drivers/pinctrl/sh-pfc/pinctrl.c')

diff --git a/drivers/pinctrl/sh-pfc/pinctrl.c b/drivers/pinctrl/sh-pfc/pinctrl.c
index 87b0a599a..fdb445d68 100644
--- a/drivers/pinctrl/sh-pfc/pinctrl.c
+++ b/drivers/pinctrl/sh-pfc/pinctrl.c
@@ -476,6 +476,91 @@ static const struct pinmux_ops sh_pfc_pinmux_ops = {
 	.gpio_set_direction	= sh_pfc_gpio_set_direction,
 };
 
+static u32 sh_pfc_pinconf_find_drive_strength_reg(struct sh_pfc *pfc,
+		unsigned int pin, unsigned int *offset, unsigned int *size)
+{
+	const struct pinmux_drive_reg_field *field;
+	const struct pinmux_drive_reg *reg;
+	unsigned int i;
+
+	for (reg = pfc->info->drive_regs; reg->reg; ++reg) {
+		for (i = 0; i < ARRAY_SIZE(reg->fields); ++i) {
+			field = &reg->fields[i];
+
+			if (field->size && field->pin == pin) {
+				*offset = field->offset;
+				*size = field->size;
+
+				return reg->reg;
+			}
+		}
+	}
+
+	return 0;
+}
+
+static int sh_pfc_pinconf_get_drive_strength(struct sh_pfc *pfc,
+					     unsigned int pin)
+{
+	unsigned long flags;
+	unsigned int offset;
+	unsigned int size;
+	u32 reg;
+	u32 val;
+
+	reg = sh_pfc_pinconf_find_drive_strength_reg(pfc, pin, &offset, &size);
+	if (!reg)
+		return -EINVAL;
+
+	spin_lock_irqsave(&pfc->lock, flags);
+	val = sh_pfc_read_reg(pfc, reg, 32);
+	spin_unlock_irqrestore(&pfc->lock, flags);
+
+	val = (val >> offset) & GENMASK(size - 1, 0);
+
+	/* Convert the value to mA based on a full drive strength value of 24mA.
+	 * We can make the full value configurable later if needed.
+	 */
+	return (val + 1) * (size == 2 ? 6 : 3);
+}
+
+static int sh_pfc_pinconf_set_drive_strength(struct sh_pfc *pfc,
+					     unsigned int pin, u16 strength)
+{
+	unsigned long flags;
+	unsigned int offset;
+	unsigned int size;
+	unsigned int step;
+	u32 reg;
+	u32 val;
+
+	reg = sh_pfc_pinconf_find_drive_strength_reg(pfc, pin, &offset, &size);
+	if (!reg)
+		return -EINVAL;
+
+	step = size == 2 ? 6 : 3;
+
+	if (strength < step || strength > 24)
+		return -EINVAL;
+
+	/* Convert the value from mA based on a full drive strength value of
+	 * 24mA. We can make the full value configurable later if needed.
+	 */
+	strength = strength / step - 1;
+
+	spin_lock_irqsave(&pfc->lock, flags);
+
+	val = sh_pfc_read_reg(pfc, reg, 32);
+	val &= ~GENMASK(offset + size - 1, offset);
+	val |= strength << offset;
+
+	sh_pfc_write_reg(pfc, reg, 32, val);
+
+	spin_unlock_irqrestore(&pfc->lock, flags);
+
+	return 0;
+}
+
 /* Check whether the requested parameter is supported for a pin. */
 static bool sh_pfc_pinconf_validate(struct sh_pfc *pfc, unsigned int _pin,
 				    enum pin_config_param param)
@@ -493,6 +578,9 @@ static bool sh_pfc_pinconf_validate(struct sh_pfc *pfc, unsigned int _pin,
 	case PIN_CONFIG_BIAS_PULL_DOWN:
 		return pin->configs & SH_PFC_PIN_CFG_PULL_DOWN;
 
+	case PIN_CONFIG_DRIVE_STRENGTH:
+		return pin->configs & SH_PFC_PIN_CFG_DRIVE_STRENGTH;
+
 	case PIN_CONFIG_POWER_SOURCE:
 		return pin->configs & SH_PFC_PIN_CFG_IO_VOLTAGE;
 
@@ -532,6 +620,17 @@ static int sh_pfc_pinconf_get(struct pinctrl_dev *pctldev, unsigned _pin,
 		break;
 	}
 
+	case PIN_CONFIG_DRIVE_STRENGTH: {
+		int ret;
+
+		ret = sh_pfc_pinconf_get_drive_strength(pfc, _pin);
+		if (ret < 0)
+			return ret;
+
+		*config = ret;
+		break;
+	}
+
 	case PIN_CONFIG_POWER_SOURCE: {
 		int ret;
 
@@ -584,6 +683,18 @@ static int sh_pfc_pinconf_set(struct pinctrl_dev *pctldev, unsigned _pin,
 
 			break;
 
+		case PIN_CONFIG_DRIVE_STRENGTH: {
+			unsigned int arg =
+				pinconf_to_config_argument(configs[i]);
+			int ret;
+
+			ret = sh_pfc_pinconf_set_drive_strength(pfc, _pin, arg);
+			if (ret < 0)
+				return ret;
+
+			break;
+		}
+
 		case PIN_CONFIG_POWER_SOURCE: {
 			unsigned int arg =
 				pinconf_to_config_argument(configs[i]);
@@ -678,7 +789,6 @@ int sh_pfc_register_pinctrl(struct sh_pfc *pfc)
 		return -ENOMEM;
 
 	pmx->pfc = pfc;
-	pfc->pinctrl = pmx;
 
 	ret = sh_pfc_map_pins(pfc, pmx);
 	if (ret < 0)
@@ -692,19 +802,9 @@ int sh_pfc_register_pinctrl(struct sh_pfc *pfc)
 	pmx->pctl_desc.pins = pmx->pins;
 	pmx->pctl_desc.npins = pfc->info->nr_pins;
 
-	pmx->pctl = pinctrl_register(&pmx->pctl_desc, pfc->dev, pmx);
+	pmx->pctl = devm_pinctrl_register(pfc->dev, &pmx->pctl_desc, pmx);
 	if (IS_ERR(pmx->pctl))
 		return PTR_ERR(pmx->pctl);
 
 	return 0;
 }
-
-int sh_pfc_unregister_pinctrl(struct sh_pfc *pfc)
-{
-	struct sh_pfc_pinctrl *pmx = pfc->pinctrl;
-
-	pinctrl_unregister(pmx->pctl);
-
-	pfc->pinctrl = NULL;
-	return 0;
-}
-- 
cgit v1.2.3-54-g00ecf