+static const unsigned int uniphier_pinconf_drv_strengths_1bit[] = {
+ 4, 8,
+};
+
+static const unsigned int uniphier_pinconf_drv_strengths_2bit[] = {
+ 8, 12, 16, 20,
+};
+
+static const unsigned int uniphier_pinconf_drv_strengths_3bit[] = {
+ 4, 5, 7, 9, 11, 12, 14, 16,
+};
+
+static int uniphier_pinconf_drive_set(struct udevice *dev, unsigned int pin,
+ unsigned int strength)
+{
+ struct uniphier_pinctrl_priv *priv = dev_get_priv(dev);
+ const struct uniphier_pinctrl_pin *desc;
+ const unsigned int *strengths;
+ unsigned int base, stride, width, drvctrl, reg, shift;
+ u32 val, mask, tmp;
+
+ desc = uniphier_pinctrl_pin_get(priv, pin);
+ if (WARN_ON(!desc))
+ return -EINVAL;
+
+ switch (uniphier_pin_get_drv_type(desc->data)) {
+ case UNIPHIER_PIN_DRV_1BIT:
+ strengths = uniphier_pinconf_drv_strengths_1bit;
+ base = UNIPHIER_PINCTRL_DRVCTRL_BASE;
+ stride = 1;
+ width = 1;
+ break;
+ case UNIPHIER_PIN_DRV_2BIT:
+ strengths = uniphier_pinconf_drv_strengths_2bit;
+ base = UNIPHIER_PINCTRL_DRV2CTRL_BASE;
+ stride = 2;
+ width = 2;
+ break;
+ case UNIPHIER_PIN_DRV_3BIT:
+ strengths = uniphier_pinconf_drv_strengths_3bit;
+ base = UNIPHIER_PINCTRL_DRV3CTRL_BASE;
+ stride = 4;
+ width = 3;
+ break;
+ default:
+ /* drive strength control is not supported for this pin */
+ return -EINVAL;
+ }
+
+ drvctrl = uniphier_pin_get_drvctrl(desc->data);
+ drvctrl *= stride;
+
+ reg = base + drvctrl / 32 * 4;
+ shift = drvctrl % 32;
+ mask = (1U << width) - 1;
+
+ for (val = 0; val <= mask; val++) {
+ if (strengths[val] > strength)
+ break;
+ }
+
+ if (val == 0) {
+ dev_err(dev, "unsupported drive strength %u mA for pin %s\n",
+ strength, desc->name);
+ return -EINVAL;
+ }
+
+ if (!mask)
+ return 0;
+
+ val--;
+
+ tmp = readl(priv->base + reg);
+ tmp &= ~(mask << shift);
+ tmp |= (mask & val) << shift;
+ writel(tmp, priv->base + reg);
+
+ return 0;
+}
+