diff --git a/cores/esp32/HWCDC.cpp b/cores/esp32/HWCDC.cpp
index cd6ce46db55..b0f653e889e 100644
--- a/cores/esp32/HWCDC.cpp
+++ b/cores/esp32/HWCDC.cpp
@@ -39,8 +39,9 @@ static intr_handle_t intr_handle = NULL;
 static SemaphoreHandle_t tx_lock = NULL;
 static volatile bool connected = false;
 
-static volatile unsigned long lastSOF_ms;
-static volatile uint8_t SOF_TIMEOUT;
+// SOF in ISR causes problems for uploading firmware
+//static volatile unsigned long lastSOF_ms;
+//static volatile uint8_t SOF_TIMEOUT;
 
 // timeout has no effect when USB CDC is unplugged
 static uint32_t tx_timeout_ms = 100;
@@ -90,7 +91,7 @@ static void hw_cdc_isr_handler(void *arg) {
     if (tx_ring_buf != NULL && usb_serial_jtag_ll_txfifo_writable() == 1) {
       // We disable the interrupt here so that the interrupt won't be triggered if there is no data to send.
       usb_serial_jtag_ll_disable_intr_mask(USB_SERIAL_JTAG_INTR_SERIAL_IN_EMPTY);
-      size_t queued_size;
+      size_t queued_size = 0;
       uint8_t *queued_buff = (uint8_t *)xRingbufferReceiveUpToFromISR(tx_ring_buf, &queued_size, 64);
       // If the hardware fifo is available, write in it. Otherwise, do nothing.
       if (queued_buff != NULL) {  //Although tx_queued_bytes may be larger than 0. We may have interrupt before xRingbufferSend() was called.
@@ -134,19 +135,23 @@ static void hw_cdc_isr_handler(void *arg) {
     connected = false;
   }
 
-  if (usbjtag_intr_status & USB_SERIAL_JTAG_INTR_SOF) {
-    usb_serial_jtag_ll_clr_intsts_mask(USB_SERIAL_JTAG_INTR_SOF);
-    lastSOF_ms = millis();
-  }
+  // SOF ISR is causing esptool to be unable to upload firmware to the board
+  // if (usbjtag_intr_status & USB_SERIAL_JTAG_INTR_SOF) {
+  //   usb_serial_jtag_ll_clr_intsts_mask(USB_SERIAL_JTAG_INTR_SOF);
+  //   lastSOF_ms = millis();
+  // }
 
   if (xTaskWoken == pdTRUE) {
     portYIELD_FROM_ISR();
   }
 }
 
-inline bool HWCDC::isPlugged(void) {
-  return (lastSOF_ms + SOF_TIMEOUT) >= millis();
-}
+// Moved to header file as inline function. Kept just as future reference.
+//inline bool HWCDC::isPlugged(void) {
+// SOF ISR is causing esptool to be unable to upload firmware to the board
+// Timer test for SOF seems to work when uploading firmware
+//  return usb_serial_jtag_is_connected();//(lastSOF_ms + SOF_TIMEOUT) >= millis();
+//}
 
 bool HWCDC::isCDC_Connected() {
   static bool running = false;
@@ -155,11 +160,13 @@ bool HWCDC::isCDC_Connected() {
   if (!isPlugged()) {
     connected = false;
     running = false;
-    SOF_TIMEOUT = 5;  // SOF timeout when unplugged
+    // SOF in ISR causes problems for uploading firmware
+    //SOF_TIMEOUT = 5;  // SOF timeout when unplugged
     return false;
-  } else {
-    SOF_TIMEOUT = 50;  // SOF timeout when plugged
   }
+  //else {
+  //  SOF_TIMEOUT = 50;  // SOF timeout when plugged
+  //}
 
   if (connected) {
     running = false;
@@ -242,13 +249,15 @@ static void ARDUINO_ISR_ATTR cdc0_write_char(char c) {
     xRingbufferSend(tx_ring_buf, (void *)(&c), 1, tx_timeout_ms / portTICK_PERIOD_MS);
   }
   usb_serial_jtag_ll_txfifo_flush();
+  usb_serial_jtag_ll_ena_intr_mask(USB_SERIAL_JTAG_INTR_SERIAL_IN_EMPTY);
 }
 
 HWCDC::HWCDC() {
   perimanSetBusDeinit(ESP32_BUS_TYPE_USB_DM, HWCDC::deinit);
   perimanSetBusDeinit(ESP32_BUS_TYPE_USB_DP, HWCDC::deinit);
-  lastSOF_ms = 0;
-  SOF_TIMEOUT = 5;
+  // SOF in ISR causes problems for uploading firmware
+  //  lastSOF_ms = 0;
+  //  SOF_TIMEOUT = 5;
 }
 
 HWCDC::~HWCDC() {
@@ -309,8 +318,9 @@ void HWCDC::begin(unsigned long baud) {
   }
 
   // the HW Serial pins needs to be first deinited in order to allow `if(Serial)` to work :-(
-  deinit(NULL);
-  delay(10);  // USB Host has to enumerate it again
+  // But this is also causing terminal to hang, so they are disabled
+  // deinit(NULL);
+  // delay(10);  // USB Host has to enumerate it again
 
   // Peripheral Manager setting for USB D+ D- pins
   uint8_t pin = USB_DM_GPIO_NUM;
@@ -332,9 +342,11 @@ void HWCDC::begin(unsigned long baud) {
   // Enable USB pad function
   USB_SERIAL_JTAG.conf0.usb_pad_enable = 1;
   usb_serial_jtag_ll_disable_intr_mask(USB_SERIAL_JTAG_LL_INTR_MASK);
-  usb_serial_jtag_ll_ena_intr_mask(
-    USB_SERIAL_JTAG_INTR_SERIAL_IN_EMPTY | USB_SERIAL_JTAG_INTR_SERIAL_OUT_RECV_PKT | USB_SERIAL_JTAG_INTR_BUS_RESET | USB_SERIAL_JTAG_INTR_SOF
-  );
+  usb_serial_jtag_ll_ena_intr_mask(USB_SERIAL_JTAG_INTR_SERIAL_IN_EMPTY | USB_SERIAL_JTAG_INTR_SERIAL_OUT_RECV_PKT | USB_SERIAL_JTAG_INTR_BUS_RESET);
+  // SOF ISR is causing esptool to be unable to upload firmware to the board
+  // usb_serial_jtag_ll_ena_intr_mask(
+  //   USB_SERIAL_JTAG_INTR_SERIAL_IN_EMPTY | USB_SERIAL_JTAG_INTR_SERIAL_OUT_RECV_PKT | USB_SERIAL_JTAG_INTR_BUS_RESET | USB_SERIAL_JTAG_INTR_SOF
+  // );
   if (!intr_handle && esp_intr_alloc(ETS_USB_SERIAL_JTAG_INTR_SOURCE, 0, hw_cdc_isr_handler, NULL, &intr_handle) != ESP_OK) {
     isr_log_e("HW USB CDC failed to init interrupts");
     end();
@@ -587,9 +599,9 @@ size_t HWCDC::read(uint8_t *buffer, size_t size) {
 void HWCDC::setDebugOutput(bool en) {
   if (en) {
     uartSetDebug(NULL);
-    ets_install_putc1((void (*)(char)) & cdc0_write_char);
+    ets_install_putc2((void (*)(char)) & cdc0_write_char);
   } else {
-    ets_install_putc1(NULL);
+    ets_install_putc2(NULL);
   }
 }
 
diff --git a/cores/esp32/HWCDC.h b/cores/esp32/HWCDC.h
index ebfbf7c4d60..29caae34062 100644
--- a/cores/esp32/HWCDC.h
+++ b/cores/esp32/HWCDC.h
@@ -21,6 +21,7 @@
 #include <inttypes.h>
 #include "esp_event.h"
 #include "Stream.h"
+#include "driver/usb_serial_jtag.h"
 
 ESP_EVENT_DECLARE_BASE(ARDUINO_HW_CDC_EVENTS);
 
@@ -69,7 +70,12 @@ class HWCDC : public Stream {
   size_t write(const uint8_t *buffer, size_t size);
   void flush(void);
 
-  static bool isPlugged(void);
+  inline static bool isPlugged(void) {
+    // SOF ISR is causing esptool to be unable to upload firmware to the board
+    // Using IDF 5.1 helper function because it is based on Timer check instead of ISR
+    return usb_serial_jtag_is_connected();
+  }
+
   inline static bool isConnected(void) {
     return isCDC_Connected();
   }
diff --git a/cores/esp32/USBCDC.cpp b/cores/esp32/USBCDC.cpp
index 6252b8c1d7b..2689086013a 100644
--- a/cores/esp32/USBCDC.cpp
+++ b/cores/esp32/USBCDC.cpp
@@ -417,9 +417,9 @@ uint32_t USBCDC::baudRate() {
 void USBCDC::setDebugOutput(bool en) {
   if (en) {
     uartSetDebug(NULL);
-    ets_install_putc1((void (*)(char)) & cdc0_write_char);
+    ets_install_putc2((void (*)(char)) & cdc0_write_char);
   } else {
-    ets_install_putc1(NULL);
+    ets_install_putc2(NULL);
   }
 }
 
diff --git a/cores/esp32/esp32-hal-uart.c b/cores/esp32/esp32-hal-uart.c
index 0aa5090b6bd..0386c30a7a3 100644
--- a/cores/esp32/esp32-hal-uart.c
+++ b/cores/esp32/esp32-hal-uart.c
@@ -828,6 +828,7 @@ void uart_install_putc() {
 #endif
     default: ets_install_putc1(NULL); break;
   }
+  ets_install_putc2(NULL);
 }
 
 // Routines that take care of UART mode in the HardwareSerial Class code
@@ -878,7 +879,7 @@ int log_printfv(const char *format, va_list arg) {
     }
 #endif
 */
-#if CONFIG_IDF_TARGET_ESP32C3
+#if CONFIG_IDF_TARGET_ESP32C3 || ((CONFIG_IDF_TARGET_ESP32H2 || CONFIG_IDF_TARGET_ESP32C6) && ARDUINO_USB_CDC_ON_BOOT)
   vsnprintf(temp, len + 1, format, arg);
   ets_printf("%s", temp);
 #else