diff --git a/components/ulp/README.rst b/components/ulp/README.rst
index 14c67d7109..875a01ef88 100644
--- a/components/ulp/README.rst
+++ b/components/ulp/README.rst
@@ -83,8 +83,11 @@ ULP coprocessor instruction defines
 
 .. doxygendefine:: I_DELAY
 .. doxygendefine:: I_HALT
+.. doxygendefine:: I_END
 .. doxygendefine:: I_ST
 .. doxygendefine:: I_LD
+.. doxygendefine:: I_WR_REG
+.. doxygendefine:: I_RD_REG
 .. doxygendefine:: I_BL
 .. doxygendefine:: I_BGE
 .. doxygendefine:: I_BXR
diff --git a/components/ulp/include/esp32/ulp.h b/components/ulp/include/esp32/ulp.h
index c23d9cf9d1..f4e37e924a 100644
--- a/components/ulp/include/esp32/ulp.h
+++ b/components/ulp/include/esp32/ulp.h
@@ -47,6 +47,10 @@ extern "C" {
 
 #define OPCODE_RD_REG 2         /*!< Instruction: read peripheral register (RTC_CNTL/RTC_IO/SARADC) (not implemented yet) */
 
+#define RD_REG_PERIPH_RTC_CNTL 0    /*!< Identifier of RTC_CNTL peripheral for RD_REG and WR_REG instructions */
+#define RD_REG_PERIPH_RTC_IO   1    /*!< Identifier of RTC_IO peripheral for RD_REG and WR_REG instructions */
+#define RD_REG_PERIPH_SENS     2    /*!< Identifier of SARADC peripheral for RD_REG and WR_REG instructions */
+
 #define OPCODE_I2C 3            /*!< Instruction: read/write I2C (not implemented yet) */
 
 #define OPCODE_DELAY 4          /*!< Instruction: delay (nop) for a given number of cycles */
@@ -191,8 +195,8 @@ typedef union {
         uint32_t addr : 8;          /*!< Address within either RTC_CNTL, RTC_IO, or SARADC */
         uint32_t periph_sel : 2;    /*!< Select peripheral: RTC_CNTL (0), RTC_IO(1), SARADC(2) */
         uint32_t data : 8;          /*!< 8 bits of data to write */
-        uint32_t high : 5;          /*!< High bit */
         uint32_t low : 5;           /*!< Low bit */
+        uint32_t high : 5;          /*!< High bit */
         uint32_t opcode : 4;        /*!< Opcode (OPCODE_WR_REG) */
     } wr_reg;                       /*!< Format of WR_REG instruction */
 
@@ -200,10 +204,10 @@ typedef union {
         uint32_t addr : 8;          /*!< Address within either RTC_CNTL, RTC_IO, or SARADC */
         uint32_t periph_sel : 2;    /*!< Select peripheral: RTC_CNTL (0), RTC_IO(1), SARADC(2) */
         uint32_t unused : 8;        /*!< Unused */
-        uint32_t high : 5;          /*!< High bit */
         uint32_t low : 5;           /*!< Low bit */
+        uint32_t high : 5;          /*!< High bit */
         uint32_t opcode : 4;        /*!< Opcode (OPCODE_WR_REG) */
-    } rd_reg;                       /*!< Format of WR_REG instruction */
+    } rd_reg;                       /*!< Format of RD_REG instruction */
 
     struct {
         uint32_t dreg : 2;          /*!< Register where to store ADC result */
@@ -256,6 +260,8 @@ typedef union {
 
 } ulp_insn_t;
 
+_Static_assert(sizeof(ulp_insn_t) == 4, "ULP coprocessor instruction size should be 4 bytes");
+
 /**
  * Delay (nop) for a given number of cycles
  */
@@ -271,6 +277,67 @@ typedef union {
     .unused = 0, \
     .opcode = OPCODE_HALT } }
 
+/**
+ * Map SoC peripheral register to periph_sel field of RD_REG and WR_REG
+ * instructions.
+ *
+ * @param reg peripheral register in RTC_CNTL_, RTC_IO_, SENS_ peripherals.
+ * @return periph_sel value for the peripheral to which this register belongs.
+ */
+static inline uint32_t SOC_REG_TO_ULP_PERIPH_SEL(uint32_t reg) {
+    uint32_t ret = 3;
+    if (reg < DR_REG_RTCCNTL_BASE) {
+        assert(0 && "invalid register base");
+    } else if (reg < DR_REG_RTCIO_BASE) {
+        ret = RD_REG_PERIPH_RTC_CNTL;
+    } else if (reg < DR_REG_SENS_BASE) {
+        ret = RD_REG_PERIPH_RTC_IO;
+    } else if (reg < DR_REG_RTCMEM0_BASE){
+        ret = RD_REG_PERIPH_SENS;
+    } else {
+        assert(0 && "invalid register base");
+    }
+    return ret;
+}
+
+/**
+ * Write literal value to a peripheral register
+ *
+ * reg[high_bit : low_bit] = val
+ * This instruction can access RTC_CNTL_, RTC_IO_, and SENS_ peripheral registers.
+ */
+#define I_WR_REG(reg, low_bit, high_bit, val) {.wr_reg = {\
+    .addr = reg & 0xff, \
+    .periph_sel = SOC_REG_TO_ULP_PERIPH_SEL(reg), \
+    .data = val, \
+    .low = low_bit, \
+    .high = high_bit, \
+    .opcode = OPCODE_WR_REG } }
+
+/**
+ * Read from peripheral register into R0
+ *
+ * R0 = reg[high_bit : low_bit]
+ * This instruction can access RTC_CNTL_, RTC_IO_, and SENS_ peripheral registers.
+ */
+#define I_RD_REG(reg, low_bit, high_bit, val) {.wr_reg = {\
+    .addr = reg & 0xff, \
+    .periph_sel = SOC_REG_TO_ULP_PERIPH_SEL(reg), \
+    .unused = 0, \
+    .low = low_bit, \
+    .high = high_bit, \
+    .opcode = OPCODE_RD_REG } }
+
+/**
+ * End program.
+ *
+ * If wake == 1, wake up main CPU.
+ */
+#define I_END(wake) { .end = { \
+        .wakeup = wake, \
+        .unused = 0, \
+        .sub_opcode = SUB_OPCODE_END, \
+        .opcode = OPCODE_END } }
 
 /**
  * Store value from register reg_val into RTC memory.
@@ -541,7 +608,7 @@ typedef union {
 #define I_ANDI(reg_dest, reg_src, imm_) { .alu_imm = { \
     .dreg = reg_dest, \
     .sreg = reg_src, \
-    .imm = reg_imm_, \
+    .imm = imm_, \
     .unused = 0, \
     .sel = ALU_SEL_AND, \
     .sub_opcode = SUB_OPCODE_ALU_IMM, \
diff --git a/components/ulp/test/test_ulp.c b/components/ulp/test/test_ulp.c
index f0000e1aa2..854eb3ee20 100644
--- a/components/ulp/test/test_ulp.c
+++ b/components/ulp/test/test_ulp.c
@@ -22,12 +22,14 @@
 #include "esp_attr.h"
 #include "esp_err.h"
 #include "esp_log.h"
+#include "esp_deep_sleep.h"
 
 #include "esp32/ulp.h"
 
 #include "soc/soc.h"
 #include "soc/rtc_cntl_reg.h"
 #include "soc/sens_reg.h"
+#include "driver/rtc_io.h"
 
 #include "sdkconfig.h"
 
@@ -92,3 +94,77 @@ TEST_CASE("ulp branch test", "[ulp]")
     }
     TEST_ASSERT_EQUAL(0, RTC_SLOW_MEM[64]);
 }
+
+TEST_CASE("ulp wakeup test", "[ulp]")
+{
+    assert(CONFIG_ULP_COPROC_RESERVE_MEM >= 260 && "this test needs ULP_COPROC_RESERVE_MEM option set in menuconfig");
+    memset(RTC_SLOW_MEM, 0, CONFIG_ULP_COPROC_RESERVE_MEM);
+    const ulp_insn_t program[] = {
+        I_MOVI(R1, 1024),
+        M_LABEL(1),
+        I_DELAY(32000),
+        I_SUBI(R1, R1, 1),
+        M_BXZ(3),
+        I_RSHI(R3, R1, 5),  // R3 = R1 / 32
+        I_ST(R1, R3, 16),
+        M_BX(1),
+        M_LABEL(3),
+        I_MOVI(R2, 42),
+        I_MOVI(R3, 15),
+        I_ST(R2, R3, 0),
+        I_END(1)
+    };
+    size_t size = sizeof(program)/sizeof(ulp_insn_t);
+    ulp_process_macros_and_load(0, program, &size);
+    ulp_run(0);
+    esp_deep_sleep_enable_ulp_wakeup();
+    esp_deep_sleep_start();
+}
+
+TEST_CASE("ulp controls RTC_IO", "[ulp]")
+{
+    assert(CONFIG_ULP_COPROC_RESERVE_MEM >= 260 && "this test needs ULP_COPROC_RESERVE_MEM option set in menuconfig");
+    memset(RTC_SLOW_MEM, 0, CONFIG_ULP_COPROC_RESERVE_MEM);
+    const ulp_insn_t program[] = {
+        I_MOVI(R0, 0),                  // R0 is LED state
+        I_MOVI(R2, 16),                 // loop R2 from 16 down to 0
+        M_LABEL(4),
+            I_SUBI(R2, R2, 1),
+            M_BXZ(6),
+            I_ADDI(R0, R0, 1),          // R0 = (R0 + 1) % 2
+            I_ANDI(R0, R0, 0x1),
+            M_BL(0, 1),                 // if R0 < 1 goto 0
+            M_LABEL(1),
+                I_WR_REG(RTC_GPIO_OUT_REG, 26, 27, 1), // RTC_GPIO12 = 1
+                M_BX(2),                // goto 2
+            M_LABEL(0),                 // 0:
+                I_WR_REG(RTC_GPIO_OUT_REG, 26, 27, 0), // RTC_GPIO12 = 0
+            M_LABEL(2),                 // 2:
+            I_MOVI(R1, 100),            // loop R1 from 100 down to 0
+            M_LABEL(3),
+                I_SUBI(R1, R1, 1),
+                M_BXZ(5),
+                I_DELAY(32000),         // delay for a while
+                M_BX(3),
+            M_LABEL(5),
+            M_BX(4),
+        M_LABEL(6),
+        I_END(1)                        // wake up the SoC
+    };
+    const gpio_num_t led_gpios[] = {
+        GPIO_NUM_2,
+        GPIO_NUM_0,
+        GPIO_NUM_4
+    };
+    for (size_t i = 0; i < sizeof(led_gpios)/sizeof(led_gpios[0]); ++i) {
+        rtc_gpio_init(led_gpios[i]);
+        rtc_gpio_set_direction(led_gpios[i], RTC_GPIO_MODE_OUTPUT_ONLY);
+        rtc_gpio_set_level(led_gpios[i], 0);
+    }
+    size_t size = sizeof(program)/sizeof(ulp_insn_t);
+    ulp_process_macros_and_load(0, program, &size);
+    ulp_run(0);
+    esp_deep_sleep_enable_ulp_wakeup();
+    esp_deep_sleep_start();
+}
+
diff --git a/components/ulp/ulp.c b/components/ulp/ulp.c
index 228e3ff16d..60fa292f7c 100644
--- a/components/ulp/ulp.c
+++ b/components/ulp/ulp.c
@@ -263,8 +263,15 @@ esp_err_t ulp_process_macros_and_load(uint32_t load_addr, const ulp_insn_t* prog
 
 esp_err_t ulp_run(uint32_t entry_point)
 {
-    SET_PERI_REG_MASK(SENS_SAR_START_FORCE_REG, SENS_ULP_CP_FORCE_START_TOP_M);
+    // disable ULP timer
+    CLEAR_PERI_REG_MASK(RTC_CNTL_STATE0_REG, RTC_CNTL_ULP_CP_SLP_TIMER_EN);
+    // set entry point
     SET_PERI_REG_BITS(SENS_SAR_START_FORCE_REG, SENS_PC_INIT_V, entry_point, SENS_PC_INIT_S);
-    SET_PERI_REG_MASK(SENS_SAR_START_FORCE_REG, SENS_ULP_CP_START_TOP_M);
+    // disable force start
+    CLEAR_PERI_REG_MASK(SENS_SAR_START_FORCE_REG, SENS_ULP_CP_FORCE_START_TOP_M);
+    // make sure voltage is raised when RTC 8MCLK is enabled
+    SET_PERI_REG_MASK(RTC_CNTL_OPTIONS0_REG, RTC_CNTL_BIAS_I2C_FOLW_8M);
+    // enable ULP timer
+    SET_PERI_REG_MASK(RTC_CNTL_STATE0_REG, RTC_CNTL_ULP_CP_SLP_TIMER_EN);
     return ESP_OK;
 }