diff --git a/.github/workflows/cmake_arm.yml b/.github/workflows/cmake_arm.yml
index e57c297d7..d2a551699 100644
--- a/.github/workflows/cmake_arm.yml
+++ b/.github/workflows/cmake_arm.yml
@@ -105,15 +105,14 @@ jobs:
 
   # ---------------------------------------
   # Hardware in the loop (HIL)
-  # Current self-hosted instance is running on an RPI4 with
-  # - pico + pico-probe connected via USB
-  # - pico-probe is /dev/ttyACM0
+  # Current self-hosted instance is running on an RPI4.
+  # For attached hardware checkout hil_pi4.json
   # ---------------------------------------
   hw-rp2040-test:
     # run only with hathach's commit due to limited resource on RPI4
     if: github.repository_owner == 'hathach'
     needs: build-arm
-    runs-on: [self-hosted, Linux, ARM64, rp2040]
+    runs-on: [self-hosted, rp2040, hardware-in-the-loop]
 
     steps:
       - name: Clean workspace
@@ -127,38 +126,6 @@ jobs:
         with:
           name: rp2040
 
-      - name: Create flash.sh
+      - name: Test on actual hardware (hardware in the loop)
         run: |
-          echo > flash.sh 'cmdout=$(openocd -f "interface/cmsis-dap.cfg" -f "target/rp2040.cfg" -c "adapter speed 5000" -c "program $1 reset exit")'
-          echo >> flash.sh 'if (( $? )) ; then echo $cmdout ; fi'
-          chmod +x flash.sh
-
-      - name: Test cdc_dual_ports
-        run: |
-          ./flash.sh cdc_dual_ports.elf
-          while (! ([ -e /dev/ttyACM1 ] && [ -e /dev/ttyACM2 ])) && [ $SECONDS -le 10 ]; do :; done
-          test -e /dev/ttyACM1 && echo "ttyACM1 exists"
-          test -e /dev/ttyACM2 && echo "ttyACM2 exists"
-
-      - name: Test cdc_msc
-        run: |
-          ./flash.sh cdc_msc.elf
-          readme='/media/pi/TinyUSB MSC/README.TXT'
-          while (! ([ -e /dev/ttyACM1 ] && [ -f "$readme" ])) && [ $SECONDS -le 10 ]; do :; done
-          test -e /dev/ttyACM1 && echo "ttyACM1 exists"
-          test -f "$readme" && echo "$readme exists"
-          cat "$readme"
-
-      - name: Test dfu
-        run: |
-          ./flash.sh dfu.elf
-          while (! (dfu-util -l | grep "Found DFU")) && [ $SECONDS -le 10 ]; do :; done
-          dfu-util -d cafe -a 0 -U dfu0
-          dfu-util -d cafe -a 1 -U dfu1
-          grep "TinyUSB DFU! - Partition 0" dfu0
-          grep "TinyUSB DFU! - Partition 1" dfu1
-
-      - name: Test dfu_runtime
-        run: |
-          ./flash.sh dfu_runtime.elf
-          while (! (dfu-util -l | grep "Found Runtime")) && [ $SECONDS -le 10 ]; do :; done
+          python3 test/hil/hil_test.py hil_pi4.json
diff --git a/hw/bsp/family_support.cmake b/hw/bsp/family_support.cmake
index 930237fea..bd072e89d 100644
--- a/hw/bsp/family_support.cmake
+++ b/hw/bsp/family_support.cmake
@@ -395,6 +395,22 @@ function(family_flash_stlink TARGET)
 endfunction()
 
 
+# Add flash openocd target
+function(family_flash_openocd TARGET CLI_OPTIONS)
+  if (NOT DEFINED OPENOCD)
+    set(OPENOCD openocd)
+  endif ()
+
+  separate_arguments(CLI_OPTIONS_LIST UNIX_COMMAND ${CLI_OPTIONS})
+
+  # note skip verify since it has issue with rp2040
+  add_custom_target(${TARGET}-openocd
+    DEPENDS ${TARGET}
+    COMMAND ${OPENOCD} ${CLI_OPTIONS_LIST} -c "program $<TARGET_FILE:${TARGET}> reset exit"
+    VERBATIM
+    )
+endfunction()
+
 # Add flash pycod target
 function(family_flash_pyocd TARGET)
   if (NOT DEFINED PYOC)
diff --git a/hw/bsp/rp2040/family.cmake b/hw/bsp/rp2040/family.cmake
index 53e2add56..77d8029d9 100644
--- a/hw/bsp/rp2040/family.cmake
+++ b/hw/bsp/rp2040/family.cmake
@@ -12,6 +12,7 @@ include(${CMAKE_CURRENT_LIST_DIR}/pico_sdk_import.cmake)
 # include basic family CMake functionality
 set(FAMILY_MCUS RP2040)
 set(JLINK_DEVICE rp2040_m0_0)
+set(OPENOCD_OPTION "-f interface/cmsis-dap.cfg -f target/rp2040.cfg -c \"adapter speed 5000\"")
 
 include(${CMAKE_CURRENT_LIST_DIR}/boards/${BOARD}/board.cmake)
 
@@ -158,6 +159,7 @@ function(family_configure_target TARGET RTOS)
 	pico_enable_stdio_uart(${TARGET} 1)
 	target_link_libraries(${TARGET} PUBLIC pico_stdlib pico_bootsel_via_double_reset tinyusb_board${RTOS_SUFFIX} tinyusb_additions)
 
+	family_flash_openocd(${TARGET} ${OPENOCD_OPTION})
 	family_flash_jlink(${TARGET})
 endfunction()
 
diff --git a/test/hil/hil_pi4.json b/test/hil/hil_pi4.json
new file mode 100644
index 000000000..86bae3154
--- /dev/null
+++ b/test/hil/hil_pi4.json
@@ -0,0 +1,11 @@
+{
+    "boards": [
+        {
+            "name": "raspberry_pi_pico",
+            "uid": "E6614864D35DAE36",
+            "debugger": "openocd",
+            "debugger_sn": "E6614103E78E8324",
+            "debugger_args": "-f interface/cmsis-dap.cfg -f target/rp2040.cfg -c \"adapter speed 5000\""
+        }
+    ]
+}
diff --git a/test/hil/hil_test.py b/test/hil/hil_test.py
index 6bcde36c6..06640a470 100644
--- a/test/hil/hil_test.py
+++ b/test/hil/hil_test.py
@@ -38,6 +38,7 @@ def get_serial_dev(id, product, ifnum):
     return f'/dev/serial/by-id/usb-TinyUSB_{product}_{id}-if{ifnum:02d}'
 
 
+# Currently not used, left as reference
 def get_disk_dev(id, lun):
     # get usb disk by id
     return f'/dev/disk/by-id/usb-TinyUSB_Mass_Storage_{id}-0:{lun}'
@@ -59,10 +60,22 @@ def flash_jlink(sn, dev, firmware):
     assert ret.returncode == 0, 'Flash failed\n' + stdout
 
 
+def flash_openocd(sn, args, firmware):
+
+    ret = subprocess.run(f'openocd {args} -c "program {firmware} reset exit"',
+                         shell=True, stdout=subprocess.PIPE, stderr=subprocess.STDOUT)
+    stdout = ret.stdout.decode()
+    assert ret.returncode == 0, 'Flash failed\n' + stdout
+
+
+# -------------------------------------------------------------
+# Tests
+# -------------------------------------------------------------
 def test_board_test(id):
     # Dummy test
     pass
 
+
 def test_cdc_dual_ports(id):
     port1 = get_serial_dev(id, "TinyUSB_Device", 0)
     port2 = get_serial_dev(id, "TinyUSB_Device", 2)
@@ -99,12 +112,22 @@ def test_cdc_dual_ports(id):
 
 def test_cdc_msc(id):
     port = get_serial_dev(id, "TinyUSB_Device", 0)
-    file = f'/media/blkUSB_{id[-8:]}.02/README.TXT'
+    file_list = [f'/media/blkUSB_{id[-8:]}.02/README.TXT', f'/media/{os.getenv("USER")}/TinyUSB MSC/README.TXT']
     # Wait device enum
     timeout = 10
     while timeout:
-        if os.path.exists(port) and os.path.isfile(file):
+        f_found = False
+        if os.path.exists(port):
+            for file in file_list:
+                if os.path.isfile(file):
+                    f_found = True
+                    f = open(file, 'rb')
+                    data = f.read()
+                    break
+
+        if f_found:
             break
+
         time.sleep(1)
         timeout = timeout - 1
 
@@ -112,7 +135,6 @@ def test_cdc_msc(id):
 
     # Echo test
     ser1 = serial.Serial(port)
-
     ser1.timeout = 1
 
     str = b"test_str"
@@ -121,9 +143,6 @@ def test_cdc_msc(id):
     assert ser1.read(100) == str, 'CDC wrong data'
 
     # Block test
-    f = open(file, 'rb')
-    data = f.read()
-
     readme = \
     b"This is tinyusb's MassStorage Class demo.\r\n\r\n\
 If you find any bugs or get any questions, feel free to file an\r\n\
@@ -131,6 +150,7 @@ issue at github.com/hathach/tinyusb"
 
     assert data == readme, 'MSC wrong data'
 
+
 def test_dfu(id):
     # Wait device enum
     timeout = 10
@@ -211,11 +231,13 @@ if __name__ == '__main__':
 
     # all possible tests, board_test is last to disable board's usb
     all_tests = [
-        'cdc_dual_ports', 'cdc_msc', 'dfu', 'dfu_runtime', 'hid_boot_interface', 'board_test'
+        'cdc_dual_ports', 'cdc_msc', 'dfu', 'dfu_runtime', 'hid_boot_interface',
+        'board_test'
     ]
 
     for board in config['boards']:
         print(f'Testing board:{board["name"]}')
+        debugger = board['debugger'].lower()
 
         # default to all tests
         if 'tests' in board:
@@ -240,8 +262,10 @@ if __name__ == '__main__':
                 print(f'Cannot find firmware file for {test}')
                 sys.exit(-1)
 
-            if board['debugger'].lower() == 'jlink':
+            if debugger == 'jlink':
                 flash_jlink(board['debugger_sn'], board['cpu'], elf)
+            elif debugger == 'openocd':
+                flash_openocd(board['debugger_sn'], board['debugger_args'], elf)
             else:
                 # ToDo
                 pass