+ /*
+ * If we've backed up the SLPM registers (glitch workaround), modify
+ * the backups since they will be restored.
+ */
+ if (slpmregs) {
+ if (slpm == NMK_GPIO_SLPM_NOCHANGE)
+ slpmregs[nmk_chip->bank] |= BIT(offset);
+ else
+ slpmregs[nmk_chip->bank] &= ~BIT(offset);
+ } else
+ __nmk_gpio_set_slpm(nmk_chip, offset, slpm);
+
+ __nmk_gpio_set_mode_safe(nmk_chip, offset, af, glitch);
+}
+
+/*
+ * Safe sequence used to switch IOs between GPIO and Alternate-C mode:
+ * - Save SLPM registers
+ * - Set SLPM=0 for the IOs you want to switch and others to 1
+ * - Configure the GPIO registers for the IOs that are being switched
+ * - Set IOFORCE=1
+ * - Modify the AFLSA/B registers for the IOs that are being switched
+ * - Set IOFORCE=0
+ * - Restore SLPM registers
+ * - Any spurious wake up event during switch sequence to be ignored and
+ * cleared
+ */
+static void nmk_gpio_glitch_slpm_init(unsigned int *slpm)
+{
+ int i;
+
+ for (i = 0; i < NUM_BANKS; i++) {
+ struct nmk_gpio_chip *chip = nmk_gpio_chips[i];
+ unsigned int temp = slpm[i];
+
+ if (!chip)
+ break;
+
+ slpm[i] = readl(chip->addr + NMK_GPIO_SLPC);
+ writel(temp, chip->addr + NMK_GPIO_SLPC);
+ }
+}
+
+static void nmk_gpio_glitch_slpm_restore(unsigned int *slpm)
+{
+ int i;
+
+ for (i = 0; i < NUM_BANKS; i++) {
+ struct nmk_gpio_chip *chip = nmk_gpio_chips[i];
+
+ if (!chip)
+ break;
+
+ writel(slpm[i], chip->addr + NMK_GPIO_SLPC);
+ }
+}
+
+static int __nmk_config_pins(pin_cfg_t *cfgs, int num, bool sleep)
+{
+ static unsigned int slpm[NUM_BANKS];
+ unsigned long flags;
+ bool glitch = false;
+ int ret = 0;
+ int i;
+
+ for (i = 0; i < num; i++) {
+ if (PIN_ALT(cfgs[i]) == NMK_GPIO_ALT_C) {
+ glitch = true;
+ break;
+ }
+ }
+
+ spin_lock_irqsave(&nmk_gpio_slpm_lock, flags);
+
+ if (glitch) {
+ memset(slpm, 0xff, sizeof(slpm));
+
+ for (i = 0; i < num; i++) {
+ int pin = PIN_NUM(cfgs[i]);
+ int offset = pin % NMK_GPIO_PER_CHIP;
+
+ if (PIN_ALT(cfgs[i]) == NMK_GPIO_ALT_C)
+ slpm[pin / NMK_GPIO_PER_CHIP] &= ~BIT(offset);
+ }
+
+ nmk_gpio_glitch_slpm_init(slpm);
+ }
+
+ for (i = 0; i < num; i++) {
+ struct nmk_gpio_chip *nmk_chip;
+ int pin = PIN_NUM(cfgs[i]);
+
+ nmk_chip = get_irq_chip_data(NOMADIK_GPIO_TO_IRQ(pin));
+ if (!nmk_chip) {
+ ret = -EINVAL;
+ break;
+ }
+
+ spin_lock(&nmk_chip->lock);
+ __nmk_config_pin(nmk_chip, pin - nmk_chip->chip.base,
+ cfgs[i], sleep, glitch ? slpm : NULL);
+ spin_unlock(&nmk_chip->lock);
+ }
+
+ if (glitch)
+ nmk_gpio_glitch_slpm_restore(slpm);
+
+ spin_unlock_irqrestore(&nmk_gpio_slpm_lock, flags);
+
+ return ret;