boot_serial: espressif: ESP32 serial recovery mode interface

Add the serial adapter for ESP32 for boot recovery and MCUMGR
communication.

Signed-off-by: Almir Okato <almir.okato@espressif.com>

espressif: Configure console via bootloader_support functions

Signed-off-by: Gustavo Henrique Nihei <gustavo.nihei@espressif.com>
diff --git a/boot/espressif/CMakeLists.txt b/boot/espressif/CMakeLists.txt
index 790b60d..c708da5 100644
--- a/boot/espressif/CMakeLists.txt
+++ b/boot/espressif/CMakeLists.txt
@@ -13,6 +13,7 @@
 project(mcuboot_${MCUBOOT_TARGET})
 
 add_definitions(-DMCUBOOT_TARGET=${MCUBOOT_TARGET})
+add_definitions(-D__ESPRESSIF__=1)
 
 if ("${MCUBOOT_TARGET}" STREQUAL "esp32" OR
     "${MCUBOOT_TARGET}" STREQUAL "esp32s2" OR
@@ -70,6 +71,7 @@
 
 set(MCUBOOT_ROOT_DIR ${CMAKE_CURRENT_LIST_DIR}/../..)
 set(BOOTUTIL_DIR ${MCUBOOT_ROOT_DIR}/boot/bootutil)
+set(BOOT_SERIAL_DIR ${MCUBOOT_ROOT_DIR}/boot/boot_serial)
 set(ESPRESSIF_PORT_DIR ${CMAKE_CURRENT_LIST_DIR})
 
 # Find imgtool.
@@ -207,20 +209,45 @@
     ${CFLAGS}
     )
 
+set(port_srcs
+    ${CMAKE_CURRENT_LIST_DIR}/port/esp_mcuboot.c
+    ${CMAKE_CURRENT_LIST_DIR}/port/esp_loader.c
+    ${CMAKE_CURRENT_LIST_DIR}/os.c
+    ${CMAKE_CURRENT_LIST_DIR}/serial_adapter.c
+    )
+
+if(CONFIG_ESP_MCUBOOT_SERIAL)
+    set(MBEDTLS_DIR "${MCUBOOT_ROOT_DIR}/ext/mbedtls")
+
+    list(APPEND bootutil_srcs
+        ${BOOT_SERIAL_DIR}/src/boot_serial.c
+        ${BOOT_SERIAL_DIR}/src/serial_recovery_cbor.c
+        ${BOOT_SERIAL_DIR}/src/zcbor_decode.c
+        ${BOOT_SERIAL_DIR}/src/zcbor_encode.c
+        ${BOOT_SERIAL_DIR}/src/zcbor_common.c
+        )
+    list(APPEND port_srcs
+        ${CMAKE_CURRENT_LIST_DIR}/serial_adapter.c
+        ${MBEDTLS_DIR}/library/base64.c
+        )
+    list(APPEND CRYPTO_INC
+        ${MBEDTLS_DIR}/include
+        )
+endif()
+
 target_sources(
     ${APP_EXECUTABLE}
     PUBLIC
     ${bootutil_srcs}
     ${crypto_srcs}
-    ${CMAKE_CURRENT_LIST_DIR}/port/esp_mcuboot.c
-    ${CMAKE_CURRENT_LIST_DIR}/port/esp_loader.c
-    ${CMAKE_CURRENT_LIST_DIR}/os.c
+    ${port_srcs}
     )
 
 target_include_directories(
     ${APP_EXECUTABLE}
     PUBLIC
     ${BOOTUTIL_DIR}/include
+    ${BOOT_SERIAL_DIR}/include
     ${CRYPTO_INC}
     ${CMAKE_CURRENT_LIST_DIR}/include
     )
diff --git a/boot/espressif/hal/CMakeLists.txt b/boot/espressif/hal/CMakeLists.txt
index da2f737..3e9fa6c 100644
--- a/boot/espressif/hal/CMakeLists.txt
+++ b/boot/espressif/hal/CMakeLists.txt
@@ -58,7 +58,10 @@
     ${src_dir}/${MCUBOOT_TARGET}/bootloader_init.c
     ${esp_idf_dir}/components/hal/mpu_hal.c
     ${esp_idf_dir}/components/hal/soc_hal.c
+    ${esp_idf_dir}/components/soc/${MCUBOOT_TARGET}/uart_periph.c
+    ${esp_idf_dir}/components/soc/${MCUBOOT_TARGET}/gpio_periph.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_common_loader.c
+    ${esp_idf_dir}/components/bootloader_support/src/bootloader_console.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_console_loader.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_flash.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_flash_config_${MCUBOOT_TARGET}.c
diff --git a/boot/espressif/hal/include/mcuboot_config/mcuboot_config.h b/boot/espressif/hal/include/mcuboot_config/mcuboot_config.h
index 8f309de..5cfe786 100644
--- a/boot/espressif/hal/include/mcuboot_config/mcuboot_config.h
+++ b/boot/espressif/hal/include/mcuboot_config/mcuboot_config.h
@@ -139,6 +139,14 @@
  * "assert" is used. */
 #define MCUBOOT_HAVE_ASSERT_H 1
 
+#ifdef CONFIG_ESP_MCUBOOT_SERIAL
+#define CONFIG_MCUBOOT_SERIAL
+#endif
+
+/* Serial extensions are not implemented
+ */
+#define MCUBOOT_PERUSER_MGMT_GROUP_ENABLED 0
+
 /*
  * Watchdog feeding
  */
@@ -154,4 +162,8 @@
           bootloader_wdt_feed(); \
       } while (0)
 
+#define MCUBOOT_CPU_IDLE() \
+    do {                   \
+    } while (0)
+
 #endif /* __MCUBOOT_CONFIG_H__ */
diff --git a/boot/espressif/hal/src/esp32/bootloader_init.c b/boot/espressif/hal/src/esp32/bootloader_init.c
index 028cf08..244648d 100644
--- a/boot/espressif/hal/src/esp32/bootloader_init.c
+++ b/boot/espressif/hal/src/esp32/bootloader_init.c
@@ -12,6 +12,7 @@
 
 #include "bootloader_init.h"
 #include "bootloader_mem.h"
+#include "bootloader_console.h"
 #include "bootloader_clock.h"
 #include "bootloader_flash_config.h"
 #include "bootloader_flash.h"
@@ -28,8 +29,26 @@
 #include "esp32/rom/spi_flash.h"
 #include "esp32/rom/uart.h"
 
+#include <esp_rom_uart.h>
+#include <esp_rom_gpio.h>
+#include <esp_rom_sys.h>
+#include <soc/uart_periph.h>
+#include <soc/gpio_struct.h>
+#include <hal/gpio_types.h>
+#include <hal/gpio_ll.h>
+#include <hal/uart_ll.h>
+
 extern esp_image_header_t WORD_ALIGNED_ATTR bootloader_image_hdr;
 
+#if CONFIG_ESP_CONSOLE_UART_CUSTOM
+static uart_dev_t *alt_console_uart_dev = (CONFIG_ESP_CONSOLE_UART_NUM == 0) ?
+                                          &UART0 :
+                                          (CONFIG_ESP_CONSOLE_UART_NUM == 1) ?
+                                          &UART1 :
+                                          &UART2;
+#endif
+
+
 static void bootloader_common_vddsdio_configure(void)
 {
     rtc_vddsdio_config_t cfg = rtc_vddsdio_get_config();
@@ -39,7 +58,7 @@
         cfg.drefl = 3;
         cfg.force = 1;
         rtc_vddsdio_set_config(cfg);
-        ets_delay_us(10); /* wait for regulator to become stable */
+        esp_rom_delay_us(10); /* wait for regulator to become stable */
     }
 }
 
@@ -127,17 +146,13 @@
     return ESP_OK;
 }
 
-static void bootloader_init_uart_console(void)
+#if CONFIG_ESP_CONSOLE_UART_CUSTOM
+void IRAM_ATTR esp_rom_uart_putc(char c)
 {
-    const int uart_num = 0;
-
-    uartAttach();
-    ets_install_uart_printf();
-    uart_tx_wait_idle(0);
-
-    const int uart_baud = CONFIG_ESP_CONSOLE_UART_BAUDRATE;
-    uart_div_modify(uart_num, (rtc_clk_apb_freq_get() << 4) / uart_baud);
+    while (uart_ll_get_txfifo_len(alt_console_uart_dev) == 0);
+    uart_ll_write_txfifo(alt_console_uart_dev, (const uint8_t *) &c, 1);
 }
+#endif
 
 esp_err_t bootloader_init(void)
 {
@@ -167,7 +182,7 @@
     /* config clock */
     bootloader_clock_configure();
     /* initialize uart console, from now on, we can use ets_printf */
-    bootloader_init_uart_console();
+    bootloader_console_init();
     /* read bootloader header */
     if ((ret = bootloader_read_bootloader_header()) != ESP_OK) {
         goto err;
diff --git a/boot/espressif/include/os/os.h b/boot/espressif/include/os/os.h
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/boot/espressif/include/os/os.h
diff --git a/boot/espressif/include/serial_adapter/serial_adapter.h b/boot/espressif/include/serial_adapter/serial_adapter.h
new file mode 100644
index 0000000..1502ec3
--- /dev/null
+++ b/boot/espressif/include/serial_adapter/serial_adapter.h
@@ -0,0 +1,29 @@
+/*
+ * SPDX-FileCopyrightText: 2022 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#pragma once
+
+/**
+ * Serial write implementation used by MCUboot boot serial structure
+ * in boot_serial.h
+ */
+void console_write(const char *str, int cnt);
+
+/**
+ * Serial read implementation used by MCUboot boot serial structure
+ * in boot_serial.h
+ */
+int console_read(char *str, int str_cnt, int *newline);
+
+/**
+ * Initialize GPIOs used by console serial read/write
+ */
+void boot_console_init(void);
+
+/**
+ * Check if serial recovery detection pin is active
+ */
+bool boot_serial_detect_pin(void);
diff --git a/boot/espressif/main.c b/boot/espressif/main.c
index b3d2b76..2cfe114 100644
--- a/boot/espressif/main.c
+++ b/boot/espressif/main.c
@@ -13,6 +13,16 @@
 #include "bootloader_utility.h"
 #include "bootloader_random.h"
 
+#ifdef CONFIG_MCUBOOT_SERIAL
+#include "boot_serial/boot_serial.h"
+#include "serial_adapter/serial_adapter.h"
+
+const struct boot_uart_funcs boot_funcs = {
+    .read = console_read,
+    .write = console_write
+};
+#endif
+
 #if defined(CONFIG_EFUSE_VIRTUAL_KEEP_IN_FLASH) || defined(CONFIG_SECURE_BOOT)
 #include "esp_efuse.h"
 #endif
@@ -140,6 +150,14 @@
 
     fih_int fih_rc = FIH_FAILURE;
 
+#ifdef CONFIG_MCUBOOT_SERIAL
+    boot_console_init();
+    if (boot_serial_detect_pin()) {
+        BOOT_LOG_INF("Enter the serial recovery mode");
+        boot_serial_start(&boot_funcs);
+    }
+#endif
+
     /* Step 2 (see above for full description):
      *   2) MCUboot validates the application images and prepares the booting process.
      */
diff --git a/boot/espressif/port/esp32/bootloader.conf b/boot/espressif/port/esp32/bootloader.conf
index 67efcfe..c374e13 100644
--- a/boot/espressif/port/esp32/bootloader.conf
+++ b/boot/espressif/port/esp32/bootloader.conf
@@ -12,6 +12,31 @@
 CONFIG_ESP_SCRATCH_OFFSET=0x210000
 CONFIG_ESP_SCRATCH_SIZE=0x40000
 
+# Enables the MCUboot Serial Recovery, that allows the use of
+# MCUMGR to upload a firmware through the serial port
+# CONFIG_ESP_MCUBOOT_SERIAL=y
+# GPIO used to boot on Serial Recovery
+# CONFIG_ESP_SERIAL_BOOT_GPIO_DETECT=32
+# GPIO input type (0 for Pull-down, 1 for Pull-up)
+# CONFIG_ESP_SERIAL_BOOT_GPIO_INPUT_TYPE=0
+# GPIO signal value
+# CONFIG_ESP_SERIAL_BOOT_GPIO_DETECT_VAL=1
+# Delay time for identify the GPIO signal
+# CONFIG_ESP_SERIAL_BOOT_DETECT_DELAY_S=5
+# UART port used for serial communication
+# CONFIG_ESP_SERIAL_BOOT_UART_NUM=1
+# GPIO for Serial RX signal
+# CONFIG_ESP_SERIAL_BOOT_GPIO_RX=25
+# GPIO for Serial TX signal
+# CONFIG_ESP_SERIAL_BOOT_GPIO_TX=26
+
+CONFIG_ESP_CONSOLE_UART=y
+CONFIG_ESP_CONSOLE_UART_NUM=0
+# Configures alternative UART port for console printing
+# CONFIG_ESP_CONSOLE_UART_CUSTOM=y
+# CONFIG_ESP_CONSOLE_UART_TX_GPIO=26
+# CONFIG_ESP_CONSOLE_UART_RX_GPIO=25
+
 # Enables multi image, if it is not defined, it is assumed
 # only one updatable image
 # CONFIG_ESP_IMAGE_NUMBER=2
diff --git a/boot/espressif/serial_adapter.c b/boot/espressif/serial_adapter.c
new file mode 100644
index 0000000..f605665
--- /dev/null
+++ b/boot/espressif/serial_adapter.c
@@ -0,0 +1,170 @@
+/*
+ * SPDX-FileCopyrightText: 2022 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <bootutil/bootutil_log.h>
+
+#include <esp_rom_uart.h>
+#include <esp_rom_gpio.h>
+#include <esp_rom_sys.h>
+#include <soc/uart_periph.h>
+#include <soc/gpio_struct.h>
+#include <hal/gpio_types.h>
+#include <hal/gpio_ll.h>
+#include <hal/uart_ll.h>
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_GPIO_DETECT
+#define SERIAL_BOOT_GPIO_DETECT     CONFIG_ESP_SERIAL_BOOT_GPIO_DETECT
+#else
+#define SERIAL_BOOT_GPIO_DETECT     GPIO_NUM_5
+#endif
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_GPIO_DETECT_VAL
+#define SERIAL_BOOT_GPIO_DETECT_VAL     CONFIG_ESP_SERIAL_BOOT_GPIO_DETECT_VAL
+#else
+#define SERIAL_BOOT_GPIO_DETECT_VAL     1
+#endif
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_DETECT_DELAY_S
+#define SERIAL_BOOT_DETECT_DELAY_S     CONFIG_ESP_SERIAL_BOOT_DETECT_DELAY_S
+#else
+#define SERIAL_BOOT_DETECT_DELAY_S     5
+#endif
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_GPIO_INPUT_TYPE
+#define SERIAL_BOOT_GPIO_INPUT_TYPE    CONFIG_ESP_SERIAL_BOOT_GPIO_INPUT_TYPE
+#else
+// pull-down
+#define SERIAL_BOOT_GPIO_INPUT_TYPE    0
+#endif
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_UART_NUM
+#define SERIAL_BOOT_UART_NUM    CONFIG_ESP_SERIAL_BOOT_UART_NUM
+#else
+#define SERIAL_BOOT_UART_NUM    ESP_ROM_UART_1
+#endif
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_GPIO_RX
+#define SERIAL_BOOT_GPIO_RX     CONFIG_ESP_SERIAL_BOOT_GPIO_RX
+#else
+#define SERIAL_BOOT_GPIO_RX     GPIO_NUM_8
+#endif
+
+#ifdef CONFIG_ESP_SERIAL_BOOT_GPIO_TX
+#define SERIAL_BOOT_GPIO_TX     CONFIG_ESP_SERIAL_BOOT_GPIO_TX
+#else
+#define SERIAL_BOOT_GPIO_TX     GPIO_NUM_9
+#endif
+
+static uart_dev_t *serial_boot_uart_dev = (SERIAL_BOOT_UART_NUM == 0) ?
+                                          &UART0 :
+                                          &UART1;
+
+void console_write(const char *str, int cnt)
+{
+    uint32_t tx_len;
+
+    do {
+        tx_len = uart_ll_get_txfifo_len(serial_boot_uart_dev);
+    } while (tx_len < cnt);
+
+    uart_ll_write_txfifo(serial_boot_uart_dev, (const uint8_t *)str, cnt);
+}
+
+int console_read(char *str, int cnt, int *newline)
+{
+    volatile uint32_t len = 0;
+    volatile uint32_t read_len = 0;
+    volatile bool stop = false;
+    do {
+        len = uart_ll_get_rxfifo_len(serial_boot_uart_dev);
+
+        if (len) {
+            for (uint32_t i = 0; i < len; i++) {
+                /* Read the character from the RX FIFO */
+                uart_ll_read_rxfifo(serial_boot_uart_dev, (uint8_t *)&str[read_len], 1);
+                read_len++;
+                if (read_len == cnt || str[read_len - 1] == '\n') {
+                    stop = true;
+                }
+            }
+        }
+        MCUBOOT_WATCHDOG_FEED();
+        esp_rom_delay_us(1000);
+    } while (!stop);
+
+    *newline = (str[read_len - 1] == '\n') ? 1 : 0;
+    return read_len;
+}
+
+int boot_console_init(void)
+{
+    BOOT_LOG_INF("Initializing serial boot pins");
+
+    /* Enable GPIO for UART RX */
+    esp_rom_gpio_pad_select_gpio(SERIAL_BOOT_GPIO_RX);
+    esp_rom_gpio_connect_in_signal(SERIAL_BOOT_GPIO_RX,
+                                   UART_PERIPH_SIGNAL(SERIAL_BOOT_UART_NUM, SOC_UART_RX_PIN_IDX),
+                                   0);
+    gpio_ll_input_enable(&GPIO, SERIAL_BOOT_GPIO_RX);
+
+    /* Enable GPIO for UART TX */
+    esp_rom_gpio_pad_select_gpio(SERIAL_BOOT_GPIO_TX);
+    esp_rom_gpio_connect_out_signal(SERIAL_BOOT_GPIO_TX,
+                                    UART_PERIPH_SIGNAL(SERIAL_BOOT_UART_NUM, SOC_UART_TX_PIN_IDX),
+                                    0, 0);
+    gpio_ll_output_enable(&GPIO, SERIAL_BOOT_GPIO_TX);
+
+    uart_ll_set_mode_normal(serial_boot_uart_dev);
+    uart_ll_set_baudrate(serial_boot_uart_dev, 115200  );
+    uart_ll_set_stop_bits(serial_boot_uart_dev, 1u );
+    uart_ll_set_parity(serial_boot_uart_dev, UART_PARITY_DISABLE );
+    uart_ll_set_rx_tout(serial_boot_uart_dev, 16 );
+
+    uart_ll_txfifo_rst(serial_boot_uart_dev);
+    uart_ll_rxfifo_rst(serial_boot_uart_dev);
+    esp_rom_delay_us(50000);
+
+    return 0;
+}
+
+bool boot_serial_detect_pin(void)
+{
+    bool detected = false;
+    int pin_value = 0;
+
+    esp_rom_gpio_pad_select_gpio(SERIAL_BOOT_GPIO_DETECT);
+    gpio_ll_input_enable(&GPIO, SERIAL_BOOT_GPIO_DETECT);
+    switch (SERIAL_BOOT_GPIO_INPUT_TYPE) {
+        // Pull-down
+        case 0:
+            gpio_ll_pulldown_en(&GPIO, SERIAL_BOOT_GPIO_DETECT);
+            break;
+        // Pull-up
+        case 1:
+            gpio_ll_pullup_en(&GPIO, SERIAL_BOOT_GPIO_DETECT);
+            break;
+    }
+    esp_rom_delay_us(50000);
+
+    pin_value = gpio_ll_get_level(&GPIO, SERIAL_BOOT_GPIO_DETECT);
+    detected = (pin_value == SERIAL_BOOT_GPIO_DETECT_VAL);
+    esp_rom_delay_us(50000);
+
+    if(detected) {
+        if(SERIAL_BOOT_DETECT_DELAY_S > 0) {
+            /* The delay time is an approximation */
+            for(int i = 0; i < (SERIAL_BOOT_DETECT_DELAY_S * 100); i++) {
+                esp_rom_delay_us(10000);
+                pin_value = gpio_ll_get_level(&GPIO, SERIAL_BOOT_GPIO_DETECT);
+                detected =  (pin_value == SERIAL_BOOT_GPIO_DETECT_VAL);
+                if(!detected) {
+                    break;
+                }
+            }
+        }
+    }
+    return detected;
+}