espressif: Add support for hardware Secure Boot

Signed-off-by: Gustavo Henrique Nihei <gustavo.nihei@espressif.com>
diff --git a/boot/espressif/bootloader.conf b/boot/espressif/bootloader.conf
index 758d5ed..c4b833a 100644
--- a/boot/espressif/bootloader.conf
+++ b/boot/espressif/bootloader.conf
@@ -1,4 +1,4 @@
-# Copyright (c) 2021 Espressif Systems (Shanghai) Co., Ltd.
+# SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
 #
 # SPDX-License-Identifier: Apache-2.0
 
@@ -23,3 +23,14 @@
 # It is strongly recommended to generate a new signing key
 # using imgtool instead of use the existent sample
 # CONFIG_ESP_SIGN_KEY_FILE=root-ec-p256.pem
+
+# Hardware Secure Boot related options
+# CONFIG_SECURE_SIGNED_ON_BOOT=1
+# CONFIG_SECURE_SIGNED_APPS_RSA_SCHEME=1
+# CONFIG_SECURE_BOOT=1
+# CONFIG_SECURE_BOOT_V2_ENABLED=1
+# CONFIG_SECURE_BOOT_SUPPORTS_RSA=1
+
+# Options for enabling eFuse emulation in Flash
+# CONFIG_EFUSE_VIRTUAL=1
+# CONFIG_EFUSE_VIRTUAL_KEEP_IN_FLASH=1
diff --git a/boot/espressif/hal/CMakeLists.txt b/boot/espressif/hal/CMakeLists.txt
index 56056d2..9095e7e 100644
--- a/boot/espressif/hal/CMakeLists.txt
+++ b/boot/espressif/hal/CMakeLists.txt
@@ -36,6 +36,8 @@
     ${esp_idf_dir}/components/heap/include
     ${esp_idf_dir}/components/efuse/include
     ${esp_idf_dir}/components/efuse/${MCUBOOT_TARGET}/include
+    ${esp_idf_dir}/components/efuse/private_include
+    ${esp_idf_dir}/components/efuse/${MCUBOOT_TARGET}/private_include
     ${esp_idf_dir}/components/newlib/platform_include
     )
 
@@ -53,14 +55,23 @@
 
 set(hal_srcs
     ${src_dir}/bootloader_wdt.c
+    ${src_dir}/secure_boot.c
     ${src_dir}/${MCUBOOT_TARGET}/bootloader_init.c
     ${esp_idf_dir}/components/hal/mpu_hal.c
+    ${esp_idf_dir}/components/bootloader_support/src/bootloader_common_loader.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
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_clock_init.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_efuse_${MCUBOOT_TARGET}.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_panic.c
     ${esp_idf_dir}/components/bootloader_support/src/bootloader_mem.c
+    ${esp_idf_dir}/components/bootloader_support/src/bootloader_random.c
+    ${esp_idf_dir}/components/bootloader_support/src/bootloader_utility.c
+    ${esp_idf_dir}/components/bootloader_support/src/esp_image_format.c
+    ${esp_idf_dir}/components/bootloader_support/src/secure_boot_v2/secure_boot_signatures_bootloader.c
+    ${esp_idf_dir}/components/bootloader_support/src/${MCUBOOT_TARGET}/bootloader_sha.c
+    ${esp_idf_dir}/components/bootloader_support/src/${MCUBOOT_TARGET}/secure_boot_secure_features.c
     ${esp_idf_dir}/components/spi_flash/${MCUBOOT_TARGET}/spi_flash_rom_patch.c
     ${esp_idf_dir}/components/esp_hw_support/esp_clk.c
     ${esp_idf_dir}/components/esp_hw_support/port/${MCUBOOT_TARGET}/rtc_init.c
@@ -71,8 +82,24 @@
     ${esp_idf_dir}/components/esp_hw_support/cpu_util.c
     ${esp_idf_dir}/components/esp_rom/patches/esp_rom_uart.c
     ${esp_idf_dir}/components/esp_rom/patches/esp_rom_sys.c
+    ${esp_idf_dir}/components/efuse/${MCUBOOT_TARGET}/esp_efuse_table.c
+    ${esp_idf_dir}/components/efuse/src/esp_efuse_fields.c
+    ${esp_idf_dir}/components/efuse/${MCUBOOT_TARGET}/esp_efuse_fields.c
+    ${esp_idf_dir}/components/efuse/src/esp_efuse_api.c
+    ${esp_idf_dir}/components/efuse/src/esp_efuse_utility.c
+    ${esp_idf_dir}/components/efuse/${MCUBOOT_TARGET}/esp_efuse_utility.c
     )
 
+if("${MCUBOOT_TARGET}" STREQUAL "esp32")
+    list(APPEND hal_srcs
+        ${esp_idf_dir}/components/efuse/src/esp_efuse_api_key_esp32.c
+        )
+else()
+    list(APPEND hal_srcs
+        ${esp_idf_dir}/components/efuse/src/esp_efuse_api_key_esp32xx.c
+        )
+endif()
+
 if("${MCUBOOT_ARCH}" STREQUAL "xtensa")
     list(APPEND hal_srcs
         ${esp_idf_dir}/components/esp_rom/patches/esp_rom_longjmp.S
@@ -145,6 +172,12 @@
     -T${esp_idf_dir}/components/soc/${MCUBOOT_TARGET}/ld/${MCUBOOT_TARGET}.peripherals.ld
     )
 
+if("${MCUBOOT_TARGET}" STREQUAL "esp32" OR "${MCUBOOT_TARGET}" STREQUAL "esp32c3")
+    list(APPEND LINKER_SCRIPTS
+        -T${esp_idf_dir}/components/esp_rom/${MCUBOOT_TARGET}/ld/${MCUBOOT_TARGET}.rom.eco3.ld
+        )
+endif()
+
 include(${CMAKE_CURRENT_LIST_DIR}/include/${MCUBOOT_TARGET}/${MCUBOOT_TARGET}.cmake)
 
 add_library(hal STATIC ${hal_srcs} ${include_dirs})
diff --git a/boot/espressif/hal/include/esp32/sdkconfig.h b/boot/espressif/hal/include/esp32/sdkconfig.h
index 547d2ae..cb7d4e7 100644
--- a/boot/espressif/hal/include/esp32/sdkconfig.h
+++ b/boot/espressif/hal/include/esp32/sdkconfig.h
@@ -1,11 +1,14 @@
 /*
- * Copyright (c) 2021 Espressif Systems (Shanghai) Co., Ltd.
+ * SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
 
 #define BOOTLOADER_BUILD 1
+#define CONFIG_IDF_FIRMWARE_CHIP_ID 0x0000
 #define CONFIG_IDF_TARGET_ESP32 1
+#define CONFIG_ESP32_REV_MIN_3 1
+#define CONFIG_ESP32_REV_MIN 3
 #define CONFIG_SPI_FLASH_ROM_DRIVER_PATCH 1
 #define CONFIG_ESP32_XTAL_FREQ 40
 #define CONFIG_MCUBOOT 1
@@ -13,3 +16,7 @@
 #define CONFIG_BOOTLOADER_WDT_TIME_MS 9000
 #define CONFIG_ESP_CONSOLE_UART_BAUDRATE 115200
 #define CONFIG_BOOTLOADER_OFFSET_IN_FLASH 0x1000
+#define CONFIG_PARTITION_TABLE_OFFSET 0xE000
+#define CONFIG_EFUSE_VIRTUAL_OFFSET 0xE000
+#define CONFIG_EFUSE_VIRTUAL_SIZE 0x2000
+#define CONFIG_EFUSE_MAX_BLK_LEN 192
diff --git a/boot/espressif/hal/include/esp32c3/sdkconfig.h b/boot/espressif/hal/include/esp32c3/sdkconfig.h
index 06f60ae..f657725 100644
--- a/boot/espressif/hal/include/esp32c3/sdkconfig.h
+++ b/boot/espressif/hal/include/esp32c3/sdkconfig.h
@@ -1,3 +1,11 @@
+/*
+ * SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#define BOOTLOADER_BUILD 1
+#define CONFIG_IDF_FIRMWARE_CHIP_ID 0x0005
 #define CONFIG_IDF_TARGET_ESP32C3 1
 #define CONFIG_IDF_TARGET_ARCH_RISCV 1
 #define CONFIG_SPI_FLASH_ROM_DRIVER_PATCH 1
@@ -6,4 +14,7 @@
 #define CONFIG_BOOTLOADER_WDT_TIME_MS 9000
 #define CONFIG_ESP_CONSOLE_UART_BAUDRATE 115200
 #define CONFIG_BOOTLOADER_OFFSET_IN_FLASH 0x0000
-#define BOOTLOADER_BUILD 1
+#define CONFIG_PARTITION_TABLE_OFFSET 0xE000
+#define CONFIG_EFUSE_VIRTUAL_OFFSET 0xE000
+#define CONFIG_EFUSE_VIRTUAL_SIZE 0x2000
+#define CONFIG_EFUSE_MAX_BLK_LEN 256
diff --git a/boot/espressif/hal/include/esp32s2/sdkconfig.h b/boot/espressif/hal/include/esp32s2/sdkconfig.h
index b4114f3..84601c8 100644
--- a/boot/espressif/hal/include/esp32s2/sdkconfig.h
+++ b/boot/espressif/hal/include/esp32s2/sdkconfig.h
@@ -1,10 +1,11 @@
 /*
- * Copyright (c) 2021 Espressif Systems (Shanghai) Co., Ltd.
+ * SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
 
 #define BOOTLOADER_BUILD 1
+#define CONFIG_IDF_FIRMWARE_CHIP_ID 0x0002
 #define CONFIG_IDF_TARGET_ESP32S2 1
 #define CONFIG_SPI_FLASH_ROM_DRIVER_PATCH 1
 #define CONFIG_ESP32S2_XTAL_FREQ 40
@@ -13,3 +14,7 @@
 #define CONFIG_BOOTLOADER_WDT_TIME_MS 9000
 #define CONFIG_ESP_CONSOLE_UART_BAUDRATE 115200
 #define CONFIG_BOOTLOADER_OFFSET_IN_FLASH 0x1000
+#define CONFIG_PARTITION_TABLE_OFFSET 0xE000
+#define CONFIG_EFUSE_VIRTUAL_OFFSET 0xE000
+#define CONFIG_EFUSE_VIRTUAL_SIZE 0x2000
+#define CONFIG_EFUSE_MAX_BLK_LEN 256
diff --git a/boot/espressif/hal/include/esp_log.h b/boot/espressif/hal/include/esp_log.h
index abb9c79..6fcab74 100644
--- a/boot/espressif/hal/include/esp_log.h
+++ b/boot/espressif/hal/include/esp_log.h
@@ -7,6 +7,12 @@
 #pragma once
 #include <mcuboot_config/mcuboot_logging.h>
 
+/* Log levels from IDF are similar to MCUboot's */
+
+#ifndef CONFIG_BOOTLOADER_LOG_LEVEL
+#define CONFIG_BOOTLOADER_LOG_LEVEL MCUBOOT_LOG_LEVEL
+#endif
+
 #define ESP_LOGE(tag, fmt, ...) MCUBOOT_LOG_ERR("[%s] " fmt, tag, ##__VA_ARGS__)
 #define ESP_LOGW(tag, fmt, ...) MCUBOOT_LOG_WRN("[%s] " fmt, tag, ##__VA_ARGS__)
 #define ESP_LOGI(tag, fmt, ...) MCUBOOT_LOG_INF("[%s] " fmt, tag, ##__VA_ARGS__)
diff --git a/boot/espressif/hal/src/secure_boot.c b/boot/espressif/hal/src/secure_boot.c
new file mode 100644
index 0000000..9cb24be
--- /dev/null
+++ b/boot/espressif/hal/src/secure_boot.c
@@ -0,0 +1,248 @@
+/*
+ * SPDX-FileCopyrightText: 2015-2021 Espressif Systems (Shanghai) CO LTD
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <string.h>
+#include "sdkconfig.h"
+#include "esp_log.h"
+#include "esp_secure_boot.h"
+#include "bootloader_flash_priv.h"
+#include "bootloader_sha.h"
+#include "bootloader_utility.h"
+#include "esp_image_format.h"
+#include "esp_efuse.h"
+#include "esp_efuse_table.h"
+
+/* The following API implementations are used only when called
+ * from the bootloader code.
+ */
+
+#ifdef CONFIG_SECURE_BOOT_V2_ENABLED
+
+#define ALIGN_UP(num, align) (((num) + ((align) - 1)) & ~((align) - 1))
+static const char *TAG = "secure_boot_v2";
+
+/* A signature block is valid when it has correct magic byte, crc and image digest. */
+static esp_err_t validate_signature_block(const ets_secure_boot_sig_block_t *block, int block_num, const uint8_t *image_digest)
+{
+    if (block->magic_byte != ETS_SECURE_BOOT_V2_SIGNATURE_MAGIC) {
+        // All signature blocks have been parsed, no new signature block present.
+        ESP_LOGD(TAG, "Signature block(%d) invalid/absent.", block_num);
+        return ESP_FAIL;
+    }
+    if (block->block_crc != esp_rom_crc32_le(0, (uint8_t *)block, CRC_SIGN_BLOCK_LEN)) {
+        ESP_LOGE(TAG, "Magic byte correct but incorrect crc.");
+        return ESP_FAIL;
+    }
+    if (memcmp(image_digest, block->image_digest, ESP_SECURE_BOOT_DIGEST_LEN)) {
+        ESP_LOGE(TAG, "Magic byte & CRC correct but incorrect image digest.");
+        return ESP_FAIL;
+    } else {
+        ESP_LOGD(TAG, "valid signature block(%d) found", block_num);
+        return ESP_OK;
+    }
+    return ESP_FAIL;
+}
+
+/* Generates the public key digests of the valid public keys in an image's
+   signature block, verifies each signature, and stores the key digests in the
+   public_key_digests structure.
+
+   @param flash_offset Image offset in flash
+   @param flash_size Image size in flash (not including signature block)
+   @param[out] public_key_digests Pointer to structure to hold the key digests for valid sig blocks
+
+
+   Note that this function doesn't read any eFuses, so it doesn't know if the
+   keys are ultimately trusted by the hardware or not
+
+   @return - ESP_OK if no signatures failed to verify, or if no valid signature blocks are found at all.
+           - ESP_FAIL if there's a valid signature block that doesn't verify using the included public key (unexpected!)
+*/
+static esp_err_t s_calculate_image_public_key_digests(uint32_t flash_offset, uint32_t flash_size, esp_image_sig_public_key_digests_t *public_key_digests)
+{
+    esp_err_t ret;
+    uint8_t image_digest[ESP_SECURE_BOOT_DIGEST_LEN] = {0};
+    uint8_t __attribute__((aligned(4))) key_digest[ESP_SECURE_BOOT_DIGEST_LEN] = {0};
+    size_t sig_block_addr = flash_offset + ALIGN_UP(flash_size, FLASH_SECTOR_SIZE);
+
+    ESP_LOGD(TAG, "calculating public key digests for sig blocks of image offset 0x%x (sig block offset 0x%x)", flash_offset, sig_block_addr);
+
+    bzero(public_key_digests, sizeof(esp_image_sig_public_key_digests_t));
+
+    ret = bootloader_sha256_flash_contents(flash_offset, sig_block_addr - flash_offset, image_digest);
+    if (ret != ESP_OK) {
+        ESP_LOGE(TAG, "error generating image digest, %d", ret);
+        return ret;
+    }
+
+    ESP_LOGD(TAG, "reading signature(s)");
+    const ets_secure_boot_signature_t *signatures = bootloader_mmap(sig_block_addr, sizeof(ets_secure_boot_signature_t));
+    if (signatures == NULL) {
+        ESP_LOGE(TAG, "bootloader_mmap(0x%x, 0x%x) failed", sig_block_addr, sizeof(ets_secure_boot_signature_t));
+        return ESP_FAIL;
+    }
+
+    /* Validating Signature block */
+    for (unsigned i = 0; i < SECURE_BOOT_NUM_BLOCKS; i++) {
+        const ets_secure_boot_sig_block_t *block = &signatures->block[i];
+
+        ret = validate_signature_block(block, i, image_digest);
+        if (ret != ESP_OK) {
+            ret = ESP_OK;  // past the last valid signature block
+            break;
+        }
+
+        /* Generating the SHA of the public key components in the signature block */
+        bootloader_sha256_handle_t sig_block_sha;
+        sig_block_sha = bootloader_sha256_start();
+        bootloader_sha256_data(sig_block_sha, &block->key, sizeof(block->key));
+        bootloader_sha256_finish(sig_block_sha, key_digest);
+
+        // Check we can verify the image using this signature and this key
+        uint8_t temp_verified_digest[ESP_SECURE_BOOT_DIGEST_LEN];
+        bool verified = ets_rsa_pss_verify(&block->key, block->signature, image_digest, temp_verified_digest);
+
+        if (!verified) {
+            /* We don't expect this: the signature blocks before we enable secure boot should all be verifiable or invalid,
+               so this is a fatal error
+            */
+            ret = ESP_FAIL;
+            ESP_LOGE(TAG, "Secure boot key (%d) verification failed.", i);
+            break;
+        }
+        ESP_LOGD(TAG, "Signature block (%d) is verified", i);
+        /* Copy the key digest to the buffer provided by the caller */
+        memcpy((void *)public_key_digests->key_digests[i], key_digest, ESP_SECURE_BOOT_DIGEST_LEN);
+        public_key_digests->num_digests++;
+    }
+
+    if (ret == ESP_OK && public_key_digests->num_digests > 0) {
+        ESP_LOGI(TAG, "Digests successfully calculated, %d valid signatures (image offset 0x%x)",
+                 public_key_digests->num_digests, flash_offset);
+    }
+
+    bootloader_munmap(signatures);
+    return ret;
+}
+
+esp_err_t check_and_generate_secure_boot_keys(void)
+{
+    esp_err_t ret;
+#ifdef CONFIG_IDF_TARGET_ESP32
+    esp_efuse_purpose_t secure_boot_key_purpose[SECURE_BOOT_NUM_BLOCKS] = {
+        ESP_EFUSE_KEY_PURPOSE_SECURE_BOOT_V2,
+    };
+    esp_efuse_coding_scheme_t coding_scheme = esp_efuse_get_coding_scheme(EFUSE_BLK_SECURE_BOOT);
+    if (coding_scheme != EFUSE_CODING_SCHEME_NONE) {
+        ESP_LOGE(TAG, "No coding schemes are supported in secure boot v2.(Detected scheme: 0x%x)", coding_scheme);
+        return ESP_ERR_NOT_SUPPORTED;
+    }
+#else
+    esp_efuse_purpose_t secure_boot_key_purpose[SECURE_BOOT_NUM_BLOCKS] = {
+        ESP_EFUSE_KEY_PURPOSE_SECURE_BOOT_DIGEST0,
+        ESP_EFUSE_KEY_PURPOSE_SECURE_BOOT_DIGEST1,
+        ESP_EFUSE_KEY_PURPOSE_SECURE_BOOT_DIGEST2,
+    };
+#endif // CONFIG_IDF_TARGET_ESP32
+
+    /* Verify the bootloader */
+    esp_image_metadata_t bootloader_data = { 0 };
+    ret = esp_image_verify_bootloader_data(&bootloader_data);
+    if (ret != ESP_OK) {
+        ESP_LOGE(TAG, "bootloader image appears invalid! error %d", ret);
+        return ret;
+    }
+
+    /* Initialize all efuse block entries to invalid (max) value */
+    esp_efuse_block_t blocks[SECURE_BOOT_NUM_BLOCKS] = {[0 ... SECURE_BOOT_NUM_BLOCKS-1] = EFUSE_BLK_KEY_MAX};
+    /* Check if secure boot digests are present */
+    bool has_secure_boot_digest = false;
+    for (unsigned i = 0; i < SECURE_BOOT_NUM_BLOCKS; i++) {
+        bool tmp_has_key = esp_efuse_find_purpose(secure_boot_key_purpose[i], &blocks[i]);
+        if (tmp_has_key) { // For ESP32: esp_efuse_find_purpose() always returns True, need to check whether the key block is used or not.
+            tmp_has_key &= !esp_efuse_key_block_unused(blocks[i]);
+        }
+        has_secure_boot_digest |= tmp_has_key;
+    }
+
+    esp_image_sig_public_key_digests_t boot_key_digests = {0};
+    ESP_LOGI(TAG, "Secure boot digests %s", has_secure_boot_digest ? "already present":"absent, generating..");
+
+    if (!has_secure_boot_digest) {
+        /* Generate the bootloader public key digests */
+        ret = s_calculate_image_public_key_digests(bootloader_data.start_addr, bootloader_data.image_len - SIG_BLOCK_PADDING, &boot_key_digests);
+        if (ret != ESP_OK) {
+            ESP_LOGE(TAG, "Bootloader signature block is invalid");
+            return ret;
+        }
+
+        if (boot_key_digests.num_digests == 0) {
+            ESP_LOGE(TAG, "No valid bootloader signature blocks found.");
+            return ESP_FAIL;
+        }
+        ESP_LOGI(TAG, "%d signature block(s) found appended to the bootloader.", boot_key_digests.num_digests);
+
+        ESP_LOGI(TAG, "Burning public key hash to eFuse");
+        ret = esp_efuse_write_keys(secure_boot_key_purpose, boot_key_digests.key_digests, boot_key_digests.num_digests);
+        if (ret != ESP_OK) {
+            if (ret == ESP_ERR_NOT_ENOUGH_UNUSED_KEY_BLOCKS) {
+                ESP_LOGE(TAG, "Bootloader signatures(%d) more than available key slots.", boot_key_digests.num_digests);
+            } else {
+                ESP_LOGE(TAG, "Failed to write efuse block with purpose (err=0x%x). Can't continue.", ret);
+            }
+            return ret;
+        }
+    } else {
+        for (unsigned i = 0; i < SECURE_BOOT_NUM_BLOCKS; i++) {
+            /* Check if corresponding digest slot is used or not */
+            if (blocks[i] == EFUSE_BLK_KEY_MAX) {
+                ESP_LOGD(TAG, "SECURE_BOOT_DIGEST%d slot is not used", i);
+                continue;
+            }
+
+#if SOC_EFUSE_REVOKE_BOOT_KEY_DIGESTS
+            if (esp_efuse_get_digest_revoke(i)) {
+                continue;
+            }
+#endif
+            if (esp_efuse_get_key_dis_read(blocks[i])) {
+                ESP_LOGE(TAG, "Key digest (BLK%d) read protected, aborting...", blocks[i]);
+                return ESP_FAIL;
+            }
+            if (esp_efuse_block_is_empty(blocks[i])) {
+                ESP_LOGE(TAG, "%d eFuse block is empty, aborting...", blocks[i]);
+                return ESP_FAIL;
+            }
+            esp_efuse_set_key_dis_write(blocks[i]);
+            ret = esp_efuse_read_block(blocks[i], boot_key_digests.key_digests[boot_key_digests.num_digests], 0,
+                                            sizeof(boot_key_digests.key_digests[0]) * 8);
+            if (ret) {
+                ESP_LOGE(TAG, "Error during reading %d eFuse block (err=0x%x)", blocks[i], ret);
+                return ret;
+            }
+            boot_key_digests.num_digests++;
+        }
+        if (boot_key_digests.num_digests == 0) {
+            ESP_LOGE(TAG, "No valid pre-loaded public key digest in eFuse");
+            return ESP_FAIL;
+        }
+        ESP_LOGW(TAG, "Using pre-loaded public key digest in eFuse");
+    }
+
+#if SOC_EFUSE_REVOKE_BOOT_KEY_DIGESTS
+    /* Revoke the empty signature blocks */
+    if (boot_key_digests.num_digests < SECURE_BOOT_NUM_BLOCKS) {
+        /* The revocation index can be 0, 1, 2. Bootloader count can be 1,2,3. */
+        for (unsigned i = boot_key_digests.num_digests; i < SECURE_BOOT_NUM_BLOCKS; i++) {
+            ESP_LOGI(TAG, "Revoking empty key digest slot (%d)...", i);
+            esp_efuse_set_digest_revoke(i);
+        }
+    }
+#endif // SOC_EFUSE_REVOKE_BOOT_KEY_DIGESTS
+    return ESP_OK;
+}
+
+#endif // CONFIG_SECURE_BOOT_V2_ENABLED
diff --git a/boot/espressif/main.c b/boot/espressif/main.c
index 22391d1..bd1d04d 100644
--- a/boot/espressif/main.c
+++ b/boot/espressif/main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021 Espressif Systems (Shanghai) Co., Ltd.
+ * SPDX-FileCopyrightText: 2021 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -11,9 +11,20 @@
 
 #include "bootloader_init.h"
 
+#if defined(CONFIG_EFUSE_VIRTUAL_KEEP_IN_FLASH) || defined(CONFIG_SECURE_BOOT)
+#include "esp_efuse.h"
+#endif
+#ifdef CONFIG_SECURE_BOOT
+#include "esp_secure_boot.h"
+#endif
+
 #include "esp_loader.h"
 #include "os/os_malloc.h"
 
+#ifdef CONFIG_SECURE_BOOT
+extern esp_err_t check_and_generate_secure_boot_keys(void);
+#endif
+
 void do_boot(struct boot_rsp *rsp)
 {
     BOOT_LOG_INF("br_image_off = 0x%x", rsp->br_image_off);
@@ -25,19 +36,74 @@
 int main()
 {
     bootloader_init();
-    struct boot_rsp rsp;
-#ifdef MCUBOOT_VER
-    BOOT_LOG_INF("*** Booting MCUBoot build %s  ***", MCUBOOT_VER);
+
+#ifdef CONFIG_EFUSE_VIRTUAL_KEEP_IN_FLASH
+    BOOT_LOG_WRN("eFuse virtual mode is enabled. If Secure boot or Flash encryption is enabled then it does not provide any security. FOR TESTING ONLY!");
+    esp_efuse_init_virtual_mode_in_flash(CONFIG_EFUSE_VIRTUAL_OFFSET, CONFIG_EFUSE_VIRTUAL_SIZE);
 #endif
 
+#ifdef CONFIG_SECURE_BOOT
+    BOOT_LOG_INF("enabling secure boot v2...");
+
+    bool sb_hw_enabled = esp_secure_boot_enabled();
+
+    if (sb_hw_enabled) {
+        BOOT_LOG_INF("secure boot v2 is already enabled, continuing..");
+    } else {
+        esp_efuse_batch_write_begin(); /* Batch all efuse writes at the end of this function */
+
+        esp_err_t err;
+        err = check_and_generate_secure_boot_keys();
+        if (err != ESP_OK) {
+            esp_efuse_batch_write_cancel();
+            FIH_PANIC;
+        }
+    }
+#endif
+
+    BOOT_LOG_INF("*** Booting MCUboot build %s ***", MCUBOOT_VER);
+
     os_heap_init();
 
+    struct boot_rsp rsp;
     fih_int fih_rc = FIH_FAILURE;
+
     FIH_CALL(boot_go, fih_rc, &rsp);
+
     if (fih_not_eq(fih_rc, FIH_SUCCESS)) {
         BOOT_LOG_ERR("Unable to find bootable image");
+#ifdef CONFIG_SECURE_BOOT
+        esp_efuse_batch_write_cancel();
+#endif
         FIH_PANIC;
     }
+
+#ifdef CONFIG_SECURE_BOOT
+    if (!sb_hw_enabled) {
+        BOOT_LOG_INF("blowing secure boot efuse...");
+        esp_err_t err;
+        err = esp_secure_boot_enable_secure_features();
+        if (err != ESP_OK) {
+            esp_efuse_batch_write_cancel();
+            FIH_PANIC;
+        }
+
+        err = esp_efuse_batch_write_commit();
+        if (err != ESP_OK) {
+            BOOT_LOG_ERR("Error programming security eFuses (err=0x%x).", err);
+            FIH_PANIC;
+        }
+
+#ifdef CONFIG_SECURE_BOOT_ENABLE_AGGRESSIVE_KEY_REVOKE
+        assert(esp_efuse_read_field_bit(ESP_EFUSE_SECURE_BOOT_AGGRESSIVE_REVOKE));
+#endif
+
+        assert(esp_secure_boot_enabled());
+        BOOT_LOG_INF("Secure boot permanently enabled");
+    }
+#endif
+
     do_boot(&rsp);
+
     while(1);
 }