diff --git a/arch/arm/mach-at91/include/mach/at91_pio.h b/arch/arm/mach-at91/include/mach/at91_pio.h
index 732b11c..28e8801 100644
--- a/arch/arm/mach-at91/include/mach/at91_pio.h
+++ b/arch/arm/mach-at91/include/mach/at91_pio.h
@@ -66,6 +66,15 @@
 #define PIO_FRLHSR	0xd8	/* Fall/Rise - Low/High Status Register */
 #define PIO_SCHMITT	0x100	/* Schmitt Trigger Register */
 
+/*
+ * SoC Specific PIO Address Offsets
+ */
+#define PIO_DRIVER1_V1		0x114
+#define PIO_DRIVER2_V1		0x118
+
+#define PIO_DRIVER1_V2		0x118
+#define PIO_DRIVER2_V2		0x11C
+
 #define ABCDSR_PERIPH_A	0x0
 #define ABCDSR_PERIPH_B	0x1
 #define ABCDSR_PERIPH_C	0x2
diff --git a/drivers/pinctrl/pinctrl-at91.c b/drivers/pinctrl/pinctrl-at91.c
index a7549c4..ccf456e 100644
--- a/drivers/pinctrl/pinctrl-at91.c
+++ b/drivers/pinctrl/pinctrl-at91.c
@@ -62,10 +62,37 @@ static int gpio_banks;
 #define DEGLITCH	(1 << 2)
 #define PULL_DOWN	(1 << 3)
 #define DIS_SCHMIT	(1 << 4)
+#define SET_DRIVE_STRENGTH  (1 << 5)
+#define DRIVE_STRENGTH_SHIFT    6
+#define DRIVE_STRENGTH   (0x3 << DRIVE_STRENGTH_SHIFT)
 #define DEBOUNCE	(1 << 16)
 #define DEBOUNCE_VAL_SHIFT	17
 #define DEBOUNCE_VAL	(0x3fff << DEBOUNCE_VAL_SHIFT)
 
+#define DRIVE_STRENGTH_MASK  0x3
+
+#define NUM_PINS_PER_DRIVE_STRENGTH_REG	16
+
+#define TWO_BIT_PIN_TO_SHIFT(pin)\
+	(2*((pin >= NUM_PINS_PER_DRIVE_STRENGTH_REG) ? \
+		pin - NUM_PINS_PER_DRIVE_STRENGTH_REG : pin))
+
+#define TWO_BIT_PIN_TO_MASK(pin)\
+	(DRIVE_STRENGTH_MASK  <<  TWO_BIT_PIN_TO_SHIFT(pin))
+
+#if defined(CONFIG_SOC_SAMA5D3)
+	#define PIO_DRIVER1  PIO_DRIVER1_V2  /* Drive Strength Register 1 */
+	#define PIO_DRIVER2  PIO_DRIVER2_V2  /* Drive Strength Register 2 */
+#elif defined(CONFIG_SOC_AT91SAM9X5) || defined(SOC_AT91SAM9G35) || defined(SOC_AT91SAM9G25)
+	#define PIO_DRIVER1  PIO_DRIVER1_V1  /* Drive Strength Register 1 */
+	#define PIO_DRIVER2  PIO_DRIVER2_V1  /* Drive Strength Register 2 */
+#endif
+
+#if defined(PIO_DRIVER1) && defined(PIO_DRIVER2)
+	#define PIO_DRIVER(pin)\
+		((pin > NUM_PINS_PER_DRIVE_STRENGTH_REG-1) ? PIO_DRIVER2 : PIO_DRIVER1)
+#endif
+
 /**
  * struct at91_pmx_func - describes AT91 pinmux functions
  * @name: the name of this specific function
@@ -148,6 +175,8 @@ struct at91_pinctrl_mux_ops {
 	void (*set_deglitch)(void __iomem *pio, unsigned mask, bool is_on);
 	bool (*get_debounce)(void __iomem *pio, unsigned pin, u32 *div);
 	void (*set_debounce)(void __iomem *pio, unsigned mask, bool is_on, u32 div);
+	int (*get_drivestrength)(void __iomem *pio, unsigned pin);
+	void (*set_drivestrength)(void __iomem *pio, unsigned pin, u32 strength);
 	bool (*get_pulldown)(void __iomem *pio, unsigned pin);
 	void (*set_pulldown)(void __iomem *pio, unsigned mask, bool is_on);
 	bool (*get_schmitt_trig)(void __iomem *pio, unsigned pin);
@@ -462,6 +491,20 @@ static void at91_mux_pio3_set_pulldown(void __iomem *pio, unsigned mask, bool is
 	__raw_writel(mask, pio + (is_on ? PIO_PPDER : PIO_PPDDR));
 }
 
+static int at91_mux_pio3_get_drivestrength(void __iomem *pio, unsigned pin)
+{
+    return (__raw_readl(pio + PIO_DRIVER(pin)) >> TWO_BIT_PIN_TO_SHIFT(pin)) & DRIVE_STRENGTH_MASK;
+}
+
+static void at91_mux_pio3_set_drivestrength(void __iomem *pio, unsigned pin, u32 strength)
+{
+    unsigned tmp = __raw_readl(pio + PIO_DRIVER(pin));
+    tmp &= ~TWO_BIT_PIN_TO_MASK(pin);
+    tmp |= strength << TWO_BIT_PIN_TO_SHIFT(pin);
+
+    __raw_writel(tmp, pio + PIO_DRIVER(pin));
+}
+
 static void at91_mux_pio3_disable_schmitt_trig(void __iomem *pio, unsigned mask)
 {
 	__raw_writel(__raw_readl(pio + PIO_SCHMITT) | mask, pio + PIO_SCHMITT);
@@ -491,6 +534,8 @@ static struct at91_pinctrl_mux_ops at91sam9x5_ops = {
 	.set_deglitch	= at91_mux_pio3_set_deglitch,
 	.get_debounce	= at91_mux_pio3_get_debounce,
 	.set_debounce	= at91_mux_pio3_set_debounce,
+	.get_drivestrength = at91_mux_pio3_get_drivestrength,
+	.set_drivestrength = at91_mux_pio3_set_drivestrength,
 	.get_pulldown	= at91_mux_pio3_get_pulldown,
 	.set_pulldown	= at91_mux_pio3_set_pulldown,
 	.get_schmitt_trig = at91_mux_pio3_get_schmitt_trig,
@@ -736,6 +781,8 @@ static int at91_pinconf_get(struct pinctrl_dev *pctldev,
 		*config |= DEGLITCH;
 	if (info->ops->get_debounce && info->ops->get_debounce(pio, pin, &div))
 		*config |= DEBOUNCE | (div << DEBOUNCE_VAL_SHIFT);
+	if (info->ops->get_drivestrength)
+		*config |= DRIVE_STRENGTH | (info->ops->get_drivestrength(pio, pin) << DRIVE_STRENGTH_SHIFT);
 	if (info->ops->get_pulldown && info->ops->get_pulldown(pio, pin))
 		*config |= PULL_DOWN;
 	if (info->ops->get_schmitt_trig && info->ops->get_schmitt_trig(pio, pin))
@@ -753,6 +800,7 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 	void __iomem *pio;
 	int i;
 	unsigned long config;
+	unsigned pin;
 
 	for (i = 0; i < num_configs; i++) {
 		config = configs[i];
@@ -761,7 +809,8 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 			"%s:%d, pin_id=%d, config=0x%lx",
 			__func__, __LINE__, pin_id, config);
 		pio = pin_to_controller(info, pin_to_bank(pin_id));
-		mask = pin_to_mask(pin_id % MAX_NB_GPIO_PER_BANK);
+		pin = pin_id % MAX_NB_GPIO_PER_BANK;
+		mask = pin_to_mask(pin);
 
 		if (config & PULL_UP && config & PULL_DOWN)
 			return -EINVAL;
@@ -773,6 +822,9 @@ static int at91_pinconf_set(struct pinctrl_dev *pctldev,
 		if (info->ops->set_debounce)
 			info->ops->set_debounce(pio, mask, config & DEBOUNCE,
 				(config & DEBOUNCE_VAL) >> DEBOUNCE_VAL_SHIFT);
+		if (info->ops->set_drivestrength && config & SET_DRIVE_STRENGTH)
+			info->ops->set_drivestrength(pio, pin,
+				(config & DRIVE_STRENGTH) >> DRIVE_STRENGTH_SHIFT);
 		if (info->ops->set_pulldown)
 			info->ops->set_pulldown(pio, mask, config & PULL_DOWN);
 		if (info->ops->disable_schmitt_trig && config & DIS_SCHMIT)
diff --git a/include/dt-bindings/pinctrl/at91.h b/include/dt-bindings/pinctrl/at91.h
index 0fee6ff..78621a4 100644
--- a/include/dt-bindings/pinctrl/at91.h
+++ b/include/dt-bindings/pinctrl/at91.h
@@ -20,6 +20,19 @@
 
 #define AT91_PINCTRL_PULL_UP_DEGLITCH	(AT91_PINCTRL_PULL_UP | AT91_PINCTRL_DEGLITCH)
 
+#define AT91_PINCTRL_SET_DRIVE_STRENGTH  (1 << 5)
+
+#if defined(CONFIG_SOC_SAMA5D3)
+	#define AT91_PINCTRL_DRIVE_STRENGTH_LOW	(0x2 << 6)
+	#define AT91_PINCTRL_DRIVE_STRENGTH_MED	(0x1 << 6)
+	#define AT91_PINCTRL_DRIVE_STRENGTH_HI	(0x0 << 6)
+#elif defined(CONFIG_SOC_AT91SAM9X5) || defined(SOC_AT91SAM9G35) || defined(SOC_AT91SAM9G25)
+	#define AT91_PINCTRL_DRIVE_STRENGTH_LOW	(0x1 << 6)
+	#define AT91_PINCTRL_DRIVE_STRENGTH_MED	(0x2 << 6)
+	#define AT91_PINCTRL_DRIVE_STRENGTH_HI	(0x3 << 6)
+#endif
+
+
 #define AT91_PIOA	0
 #define AT91_PIOB	1
 #define AT91_PIOC	2
