diff --git a/core/esp_spi.c b/core/esp_spi.c
index f7d8cf3..ab33ea4 100644
--- a/core/esp_spi.c
+++ b/core/esp_spi.c
@@ -30,32 +30,27 @@
 
 static bool _minimal_pins[2] = {false, false};
 
-inline static void _set_pin_function(uint8_t pin, uint32_t function)
-{
-    iomux_set_function(gpio_to_iomux(pin), function);
-}
-
 bool spi_init(uint8_t bus, spi_mode_t mode, uint32_t freq_divider, bool msb, spi_endianness_t endianness, bool minimal_pins)
 {
     switch (bus)
     {
         case 0:
-            _set_pin_function(_SPI0_MISO_GPIO, _SPI0_FUNC);
-            _set_pin_function(_SPI0_MOSI_GPIO, _SPI0_FUNC);
-            _set_pin_function(_SPI0_SCK_GPIO, _SPI0_FUNC);
+            gpio_set_iomux_function(_SPI0_MISO_GPIO, _SPI0_FUNC);
+            gpio_set_iomux_function(_SPI0_MOSI_GPIO, _SPI0_FUNC);
+            gpio_set_iomux_function(_SPI0_SCK_GPIO, _SPI0_FUNC);
             if (!minimal_pins)
             {
-                _set_pin_function(_SPI0_HD_GPIO, _SPI0_FUNC);
-                _set_pin_function(_SPI0_WP_GPIO, _SPI0_FUNC);
-                _set_pin_function(_SPI0_CS0_GPIO, _SPI0_FUNC);
+                gpio_set_iomux_function(_SPI0_HD_GPIO, _SPI0_FUNC);
+                gpio_set_iomux_function(_SPI0_WP_GPIO, _SPI0_FUNC);
+                gpio_set_iomux_function(_SPI0_CS0_GPIO, _SPI0_FUNC);
             }
             break;
         case 1:
-            _set_pin_function(_SPI1_MISO_GPIO, _SPI1_FUNC);
-            _set_pin_function(_SPI1_MOSI_GPIO, _SPI1_FUNC);
-            _set_pin_function(_SPI1_SCK_GPIO, _SPI1_FUNC);
+            gpio_set_iomux_function(_SPI1_MISO_GPIO, _SPI1_FUNC);
+            gpio_set_iomux_function(_SPI1_MOSI_GPIO, _SPI1_FUNC);
+            gpio_set_iomux_function(_SPI1_SCK_GPIO, _SPI1_FUNC);
             if (!minimal_pins)
-                _set_pin_function(_SPI1_CS0_GPIO, _SPI1_FUNC);
+                gpio_set_iomux_function(_SPI1_CS0_GPIO, _SPI1_FUNC);
             break;
         default:
             return false;
diff --git a/core/include/esp/gpio.h b/core/include/esp/gpio.h
index 426d1ea..80fd43f 100644
--- a/core/include/esp/gpio.h
+++ b/core/include/esp/gpio.h
@@ -143,6 +143,16 @@ static inline gpio_inttype_t gpio_get_interrupt(const uint8_t gpio_num)
     return (gpio_inttype_t)FIELD2VAL(GPIO_CONF_INTTYPE, GPIO.CONF[gpio_num]);
 }
 
+/* Set GPIO I/O Mux function.
+ * The 'func' is an IOMUX_GPIO<n>_FUNC_<function> value, see iomux_regs.h
+ */
+inline static void gpio_set_iomux_function(const uint8_t gpio_num, uint32_t func)
+{
+    uint8_t iomux_num = gpio_to_iomux(gpio_num);
+    uint32_t prev = IOMUX.PIN[iomux_num] & ~IOMUX_PIN_FUNC_MASK;
+    IOMUX.PIN[iomux_num] = func | prev;
+}
+
 #ifdef	__cplusplus
 }
 #endif