diff --git a/cores/rp2040/Tone.cpp b/cores/rp2040/Tone.cpp
index 114eca4..34f3964 100644
--- a/cores/rp2040/Tone.cpp
+++ b/cores/rp2040/Tone.cpp
@@ -71,10 +71,7 @@ void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
         return;    // Weird deadlock case
     }
 
-    int us = 1'000'000 / frequency / 2;
-    if (us < 5) {
-        us = 5;
-    }
+    unsigned int delay = (RP2040::f_cpu() + frequency) / (frequency * 2) - 3; // rounded
     auto entry = _toneMap.find(pin);
     Tone *newTone;
     if (entry == _toneMap.end()) {
@@ -99,7 +96,7 @@ void tone(uint8_t pin, unsigned int frequency, unsigned long duration) {
         tone2_program_init(newTone->pio, newTone->sm, newTone->off, pin);
     }
     pio_sm_clear_fifos(newTone->pio, newTone->sm); // Remove any old updates that haven't yet taken effect
-    pio_sm_put_blocking(newTone->pio, newTone->sm, RP2040::usToPIOCycles(us));
+    pio_sm_put_blocking(newTone->pio, newTone->sm, delay);
     pio_sm_exec(newTone->pio, newTone->sm, pio_encode_pull(false, false));
     pio_sm_exec(newTone->pio, newTone->sm, pio_encode_mov(pio_x, pio_osr));
     pio_sm_set_enabled(newTone->pio, newTone->sm, true);
diff --git a/cores/rp2040/tone2.pio b/cores/rp2040/tone2.pio
index 0df248c..1af4701 100644
--- a/cores/rp2040/tone2.pio
+++ b/cores/rp2040/tone2.pio
@@ -18,7 +18,7 @@
 
 ; Side-set pin 0 is used for Tone output
 
-; OSR == Halfcycle count
+; OSR == Halfcycle count - 3
 
 .program tone2
 .side_set 1 opt
@@ -26,10 +26,11 @@
 ;    pull                     ; TXFIFO -> OSR, or X -> OSR if no new period
 ;    mov x, osr               ; OSR -> X
 
+.wrap_target
 high:
     pull noblock             ; Potentially grab new HALFCYCLECOUNT, OTW copy from backup in X
-    mov x, osr               ; OSR -> X
-    mov y, osr      side 1   ; HALFCYCLECOUNT -> Y
+    mov x, osr      side 1   ; OSR -> X
+    mov y, osr               ; HALFCYCLECOUNT -> Y
 highloop:
     jmp y-- highloop         ; while (y--) { /* noop delay */ }
 
@@ -38,7 +39,7 @@ low:
 lowloop:
     jmp y-- lowloop          ; while (y--) { /* noop delay */ }
 
-    jmp high                 ; GOTO high
+    .wrap                    ; GOTO high
 
 % c-sdk {
 static inline void tone2_program_init(PIO pio, uint sm, uint offset, uint pin) {
diff --git a/cores/rp2040/tone2.pio.h b/cores/rp2040/tone2.pio.h
index d0e7be3..48bffde 100644
--- a/cores/rp2040/tone2.pio.h
+++ b/cores/rp2040/tone2.pio.h
@@ -13,27 +13,26 @@
 // ----- //
 
 #define tone2_wrap_target 0
-#define tone2_wrap 6
+#define tone2_wrap 5
 #define tone2_pio_version 0
 
 static const uint16_t tone2_program_instructions[] = {
-    //     .wrap_target
+    //         .wrap_target
     0x8080, //  0: pull   noblock
-    0xa027, //  1: mov    x, osr
-    0xb847, //  2: mov    y, osr          side 1
+    0xb827, //  1: mov    x, osr          side 1
+    0xa047, //  2: mov    y, osr
     0x0083, //  3: jmp    y--, 3
     0xb047, //  4: mov    y, osr          side 0
     0x0085, //  5: jmp    y--, 5
-    0x0000, //  6: jmp    0
-    //     .wrap
+    //         .wrap
 };
 
 #if !PICO_NO_HARDWARE
 static const struct pio_program tone2_program = {
     .instructions = tone2_program_instructions,
-    .length = 7,
+    .length = 6,
     .origin = -1,
-    .pio_version = 0,
+    .pio_version = tone2_pio_version,
 #if PICO_PIO_VERSION > 0
     .used_gpio_ranges = 0x0
 #endif