diff --git a/components/bootloader/src/main/bootloader_start.c b/components/bootloader/src/main/bootloader_start.c
index b708f894e..aa30ad4c7 100644
--- a/components/bootloader/src/main/bootloader_start.c
+++ b/components/bootloader/src/main/bootloader_start.c
@@ -240,6 +240,7 @@ void bootloader_main()
     /* Set CPU to 80MHz. Keep other clocks unmodified. */
     uart_tx_wait_idle(0);
     rtc_clk_config_t clk_cfg = RTC_CLK_CONFIG_DEFAULT();
+    clk_cfg.xtal_freq = CONFIG_ESP32_XTAL_FREQ;
     clk_cfg.cpu_freq = RTC_CPU_FREQ_80M;
     clk_cfg.slow_freq = rtc_clk_slow_freq_get();
     clk_cfg.fast_freq = rtc_clk_fast_freq_get();
diff --git a/components/esp32/Kconfig b/components/esp32/Kconfig
index f10457581..6a73463c5 100644
--- a/components/esp32/Kconfig
+++ b/components/esp32/Kconfig
@@ -560,6 +560,34 @@ config ESP32_DEEP_SLEEP_WAKEUP_DELAY
         If you are seeing "flash read err, 1000" message printed to the
         console after deep sleep reset, try increasing this value.
 
+choice ESP32_XTAL_FREQ_SEL
+    prompt "Main XTAL frequency"
+    default ESP32_XTAL_FREQ_AUTO
+    help
+        ESP32 currently supports the following XTAL frequencies:
+            - 26 MHz
+            - 40 MHz
+        Startup code can automatically estimate XTAL frequency. This feature
+        uses the internal 8MHz oscillator as a reference. Because the internal
+        oscillator frequency is temperature dependent, it is not recommended
+        to use automatic XTAL frequency detection in applications which need
+        to work at high ambient temperatures and use high-temperature
+        qualified chips and modules.
+config ESP32_XTAL_FREQ_40
+    bool "40 MHz"
+config ESP32_XTAL_FREQ_26
+    bool "26 MHz"
+config ESP32_XTAL_FREQ_AUTO
+    bool "Autodetect"
+endchoice
+
+# Keep these values in sync with rtc_xtal_freq_t enum in soc/rtc.h
+config ESP32_XTAL_FREQ
+    int
+    default 0 if ESP32_XTAL_FREQ_AUTO
+    default 40 if ESP32_XTAL_FREQ_40
+    default 26 if ESP32_XTAL_FREQ_26
+
 endmenu
 
 menuconfig WIFI_ENABLED
diff --git a/components/soc/esp32/include/soc/rtc.h b/components/soc/esp32/include/soc/rtc.h
index aecf76bec..a0813294a 100644
--- a/components/soc/esp32/include/soc/rtc.h
+++ b/components/soc/esp32/include/soc/rtc.h
@@ -92,6 +92,9 @@ typedef enum {
     RTC_FAST_FREQ_8M = 1,       //!< Internal 8 MHz RC oscillator
 } rtc_fast_freq_t;
 
+/* With the default value of CK8M_DFREQ, 8M clock frequency is 8.5 MHz +/- 7% */
+#define RTC_FAST_CLK_FREQ_APPROX 8500000
+
 /**
  * @brief Clock source to be calibrated using rtc_clk_cal function
  */
@@ -307,6 +310,15 @@ uint32_t rtc_clk_apb_freq_get();
  */
 uint32_t rtc_clk_cal(rtc_cal_sel_t cal_clk, uint32_t slow_clk_cycles);
 
+/**
+ * @brief Measure ratio between XTAL frequency and RTC slow clock frequency
+ * @param cal_clk slow clock to be measured
+ * @param slow_clk_cycles number of slow clock cycles to average
+ * @return average ratio between XTAL frequency and slow clock frequency,
+ *         Q13.19 fixed point format, or 0 if calibration has timed out.
+ */
+uint32_t rtc_clk_cal_ratio(rtc_cal_sel_t cal_clk, uint32_t slow_clk_cycles);
+
 /**
  * @brief Convert time interval from microseconds to RTC_SLOW_CLK cycles
  * @param time_in_us Time interval in microseconds
diff --git a/components/soc/esp32/include/soc/rtc_io_reg.h b/components/soc/esp32/include/soc/rtc_io_reg.h
index f6d0dac76..5a3ce10d4 100644
--- a/components/soc/esp32/include/soc/rtc_io_reg.h
+++ b/components/soc/esp32/include/soc/rtc_io_reg.h
@@ -511,6 +511,7 @@
 #define RTC_IO_DEBUG_SEL0_M  ((RTC_IO_DEBUG_SEL0_V)<<(RTC_IO_DEBUG_SEL0_S))
 #define RTC_IO_DEBUG_SEL0_V  0x1F
 #define RTC_IO_DEBUG_SEL0_S  0
+#define RTC_IO_DEBUG_SEL0_8M       1
 #define RTC_IO_DEBUG_SEL0_32K_XTAL 4
 #define RTC_IO_DEBUG_SEL0_150K_OSC 5
 
diff --git a/components/soc/esp32/rtc_clk.c b/components/soc/esp32/rtc_clk.c
index bc7eacc9d..1397705c2 100644
--- a/components/soc/esp32/rtc_clk.c
+++ b/components/soc/esp32/rtc_clk.c
@@ -67,6 +67,11 @@ static const char* TAG = "rtc_clk";
 #define DELAY_SLOW_CLK_SWITCH           300
 #define DELAY_8M_ENABLE                 50
 
+/* Number of 8M/256 clock cycles to use for XTAL frequency estimation.
+ * 10 cycles will take approximately 300 microseconds.
+ */
+#define XTAL_FREQ_EST_CYCLES            10
+
 
 void rtc_clk_32k_enable(bool enable)
 {
@@ -422,19 +427,12 @@ void rtc_clk_xtal_freq_update(rtc_xtal_freq_t xtal_freq)
 
 static rtc_xtal_freq_t rtc_clk_xtal_freq_estimate()
 {
-    /* ROM startup code estimates XTAL frequency using an 8MD256 clock and stores
-     * the value into RTC_APB_FREQ_REG. The value is in Hz, right shifted by 12.
-     * Use this value to guess the real XTAL frequency.
-     *
-     * TODO: make this more robust by calibrating again after setting
-     * RTC_CNTL_CK8M_DFREQ.
+    uint64_t cal_val = rtc_clk_cal_ratio(RTC_CAL_8MD256, XTAL_FREQ_EST_CYCLES);
+    /* cal_val contains period of 8M/256 clock in XTAL clock cycles
+     * (shifted by RTC_CLK_CAL_FRACT bits).
+     * Xtal frequency will be (cal_val * 8M / 256) / 2^19
      */
-    uint32_t apb_freq_reg = READ_PERI_REG(RTC_APB_FREQ_REG);
-    if (!clk_val_is_valid(apb_freq_reg)) {
-        SOC_LOGW(TAG, "invalid RTC_APB_FREQ_REG value: 0x%08x", apb_freq_reg);
-        return RTC_XTAL_FREQ_AUTO;
-    }
-    uint32_t freq_mhz = (reg_val_to_clk_val(apb_freq_reg) << 12) / MHZ;
+    uint32_t freq_mhz = (cal_val * (RTC_FAST_CLK_FREQ_APPROX / MHZ) / 256 ) >> RTC_CLK_CAL_FRACT;
     /* Guess the XTAL type. For now, only 40 and 26MHz are supported.
      */
     switch (freq_mhz) {
@@ -467,6 +465,19 @@ uint32_t rtc_clk_apb_freq_get()
 
 void rtc_clk_init(rtc_clk_config_t cfg)
 {
+    /* If we get a TG WDT system reset while running at 240MHz,
+     * DPORT_CPUPERIOD_SEL register will be reset to 0 resulting in 120MHz
+     * APB and CPU frequencies after reset. This will cause issues with XTAL
+     * frequency estimation, so we switch to XTAL frequency first.
+     *
+     * Ideally we would only do this if RTC_CNTL_SOC_CLK_SEL == PLL and
+     * PLL is configured for 480M, but it takes less time to switch to 40M and
+     * run the following code than querying the PLL does.
+     */
+    if (REG_GET_FIELD(RTC_CNTL_CLK_CONF_REG, RTC_CNTL_SOC_CLK_SEL) == RTC_CNTL_SOC_CLK_SEL_PLL) {
+        rtc_clk_cpu_freq_set(RTC_CPU_FREQ_XTAL);
+    }
+
     /* Set tuning parameters for 8M and 150k clocks.
      * Note: this doesn't attempt to set the clocks to precise frequencies.
      * Instead, we calibrate these clocks against XTAL frequency later, when necessary.
@@ -521,20 +532,3 @@ void rtc_clk_init(rtc_clk_config_t cfg)
  * TODO: update the library to use rtc_clk_xtal_freq_get
  */
 rtc_xtal_freq_t rtc_get_xtal() __attribute__((alias("rtc_clk_xtal_freq_get")));
-
-
-/* Referenced in librtc.a:rtc.o.
- * TODO: remove
- */
-void rtc_uart_div_modify(int latch)
-{
-
-}
-
-/* Referenced in librtc.a:rtc.o.
- * TODO: remove
- */
-void rtc_uart_tx_wait_idle(int uart)
-{
-
-}
diff --git a/components/soc/esp32/rtc_time.c b/components/soc/esp32/rtc_time.c
index 7b1fb45f3..9e974cc94 100644
--- a/components/soc/esp32/rtc_time.c
+++ b/components/soc/esp32/rtc_time.c
@@ -31,9 +31,8 @@
  * once, and TIMG_RTC_CALI_RDY bit is set when counting is done. One-off mode is
  * enabled using TIMG_RTC_CALI_START bit.
  */
-uint32_t rtc_clk_cal(rtc_cal_sel_t cal_clk, uint32_t slowclk_cycles)
+uint32_t rtc_clk_cal_ratio(rtc_cal_sel_t cal_clk, uint32_t slowclk_cycles)
 {
-    rtc_xtal_freq_t xtal_freq = rtc_clk_xtal_freq_get();
     /* Enable requested clock (150k clock is always on) */
     if (cal_clk == RTC_CAL_32K_XTAL) {
         SET_PERI_REG_MASK(RTC_CNTL_CLK_CONF_REG, RTC_CNTL_DIG_XTAL32K_EN);
@@ -53,7 +52,7 @@ uint32_t rtc_clk_cal(rtc_cal_sel_t cal_clk, uint32_t slowclk_cycles)
         expected_freq = 32768; /* standard 32k XTAL */
     } else if (cal_clk == RTC_CAL_8MD256 ||
             (cal_clk == RTC_CAL_RTC_MUX && slow_freq == RTC_SLOW_FREQ_8MD256)) {
-        expected_freq = 8 * MHZ / 256;
+        expected_freq = RTC_FAST_CLK_FREQ_APPROX / 256;
     } else {
         expected_freq = 150000; /* 150k internal oscillator */
     }
@@ -69,7 +68,8 @@ uint32_t rtc_clk_cal(rtc_cal_sel_t cal_clk, uint32_t slowclk_cycles)
     /* Wait for calibration to finish up to another us_time_estimate */
     int timeout_us = us_time_estimate;
     while (!GET_PERI_REG_MASK(TIMG_RTCCALICFG_REG(0), TIMG_RTC_CALI_RDY) &&
-            timeout_us-- > 0) {
+            timeout_us > 0) {
+        timeout_us--;
         ets_delay_us(1);
     }
     if (cal_clk == RTC_CAL_32K_XTAL) {
@@ -84,9 +84,16 @@ uint32_t rtc_clk_cal(rtc_cal_sel_t cal_clk, uint32_t slowclk_cycles)
     }
 
     uint64_t xtal_cycles = REG_GET_FIELD(TIMG_RTCCALICFG1_REG(0), TIMG_RTC_CALI_VALUE);
-    uint64_t divider = ((uint64_t)xtal_freq) * slowclk_cycles;
-    uint64_t period_64 = (xtal_cycles << RTC_CLK_CAL_FRACT) / divider;
-    uint32_t period = (uint32_t)(period_64 & UINT32_MAX);
+    uint64_t ratio_64 = (xtal_cycles << RTC_CLK_CAL_FRACT) / slowclk_cycles;
+    uint32_t ratio = (uint32_t)(ratio_64 & UINT32_MAX);
+    return ratio;
+}
+
+uint32_t rtc_clk_cal(rtc_cal_sel_t cal_clk, uint32_t slowclk_cycles)
+{
+    rtc_xtal_freq_t xtal_freq = rtc_clk_xtal_freq_get();
+    uint32_t ratio = rtc_clk_cal_ratio(cal_clk, slowclk_cycles);
+    uint32_t period = ratio / xtal_freq;
     return period;
 }
 
diff --git a/components/soc/esp32/test/test_rtc_clk.c b/components/soc/esp32/test/test_rtc_clk.c
index cad78297b..48905acd0 100644
--- a/components/soc/esp32/test/test_rtc_clk.c
+++ b/components/soc/esp32/test/test_rtc_clk.c
@@ -82,3 +82,10 @@ TEST_CASE("Output 32k XTAL clock to GPIO25", "[rtc_clk][ignore]")
     rtc_clk_32k_enable(true);
     pull_out_clk(RTC_IO_DEBUG_SEL0_32K_XTAL);
 }
+
+TEST_CASE("Output 8M XTAL clock to GPIO25", "[rtc_clk][ignore]")
+{
+    rtc_clk_8m_enable(true, true);
+    SET_PERI_REG_MASK(RTC_IO_RTC_DEBUG_SEL_REG, RTC_IO_DEBUG_12M_NO_GATING);
+    pull_out_clk(RTC_IO_DEBUG_SEL0_8M);
+}