diff --git a/blah.patch b/blah.patch
new file mode 100644
index 0000000..a5f7d6c
--- /dev/null
+++ b/blah.patch
@@ -0,0 +1,23 @@
+diff --git a/include/DAP_config.h b/include/DAP_config.h
+index fb02fb1..88c11d9 100755
+--- a/include/DAP_config.h
++++ b/include/DAP_config.h
+@@ -44,6 +44,7 @@ This information includes:
+  - Optional information about a connected Target Device (for Evaluation Boards).
+ */
+ #include <pico/stdlib.h>
++#include <hardware/clocks.h>
+ #include <hardware/gpio.h>
+
+ #include "cmsis_compiler.h"
+@@ -52,8 +53,8 @@ This information includes:
+
+ /// Processor Clock of the Cortex-M MCU used in the Debug Unit.
+ /// This value is used to calculate the SWD/JTAG clock speed.
+-/* Debugprobe actually uses kHz rather than Hz, so just lie about it here */
+-#define CPU_CLOCK               125000000U      ///< Specifies the CPU Clock in Hz.
++/* Debugprobe uses PIO for clock generation, so return the current system clock. */
++#define CPU_CLOCK               clock_get_hz(clk_sys)
+
+ /// Number of processor cycles for I/O Port write operations.
+ /// This value is used to calculate the SWD/JTAG clock speed that is generated with I/O
diff --git a/include/DAP_config.h b/include/DAP_config.h
index fb02fb1..88c11d9 100755
--- a/include/DAP_config.h
+++ b/include/DAP_config.h
@@ -44,6 +44,7 @@ This information includes:
  - Optional information about a connected Target Device (for Evaluation Boards).
 */
 #include <pico/stdlib.h>
+#include <hardware/clocks.h>
 #include <hardware/gpio.h>
 
 #include "cmsis_compiler.h"
@@ -52,8 +53,8 @@ This information includes:
 
 /// Processor Clock of the Cortex-M MCU used in the Debug Unit.
 /// This value is used to calculate the SWD/JTAG clock speed.
-/* Debugprobe actually uses kHz rather than Hz, so just lie about it here */
-#define CPU_CLOCK               125000000U      ///< Specifies the CPU Clock in Hz.
+/* Debugprobe uses PIO for clock generation, so return the current system clock. */
+#define CPU_CLOCK               clock_get_hz(clk_sys)
 
 /// Number of processor cycles for I/O Port write operations.
 /// This value is used to calculate the SWD/JTAG clock speed that is generated with I/O
diff --git a/src/probe.c b/src/probe.c
index 17c3f7c..dbe0ea8 100644
--- a/src/probe.c
+++ b/src/probe.c
@@ -61,9 +61,13 @@ static struct _probe probe;
 void probe_set_swclk_freq(uint freq_khz) {
         uint clk_sys_freq_khz = clock_get_hz(clk_sys) / 1000;
         probe_info("Set swclk freq %dKHz sysclk %dkHz\n", freq_khz, clk_sys_freq_khz);
-        uint32_t divider = clk_sys_freq_khz / freq_khz / 4;
+        // Round up (otherwise fast swclks get faster)
+        uint32_t divider = (((clk_sys_freq_khz + freq_khz - 1)/ freq_khz) + 3) / 4;
         if (divider == 0)
             divider = 1;
+        if (divider > 65535)
+            divider = 65535;
+
         pio_sm_set_clkdiv_int_frac(pio0, PROBE_SM, divider, 0);
 }