diff --git a/firmware/rust1/src/bin/heizung.rs b/firmware/rust1/src/bin/heizung.rs
index 2480bcf740df148f88d501451fbbdee52c8639e0..6934c33ea978b9f04c454b71656af0b5937d8cb2 100644
--- a/firmware/rust1/src/bin/heizung.rs
+++ b/firmware/rust1/src/bin/heizung.rs
@@ -303,11 +303,15 @@ async fn autobaud_task(pio0: embassy_rp::peripherals::PIO0, rx_pin: embassy_rp::
 
     sm.set_enable(false);
 
-    let prg = pio_proc::pio_asm!(
+    let prg_set_osr = pio_proc::pio_asm!(
         ".origin 0",
-
-        // First, pull waiting time for "long 1" from input FIFO.
         "pull",
+        "hang:",
+        "jmp hang",
+    );
+
+    let prg = pio_proc::pio_asm!(
+        ".origin 0",
 
         // Send zero to RX FIFO to signal start of next measurement.
         ".wrap_target",
@@ -367,9 +371,10 @@ async fn autobaud_task(pio0: embassy_rp::peripherals::PIO0, rx_pin: embassy_rp::
         ".wrap",
     );
 
-    let relocated = RelocatedProgram::new(&prg.program);
+    let relocated = RelocatedProgram::new(&prg_set_osr.program);
     let mut cfg = Config::default();
-    cfg.use_program(&common.load_program(&relocated), &[]);
+    let loaded_program = common.load_program(&relocated);
+    cfg.use_program(&loaded_program, &[]);
     const SM_FREQ: u32 = 125_000_000;
     cfg.clock_divider = (U56F8!(125_000_000) / U56F8!(125_000_000 /* SM_FREQ */)).to_fixed();
     cfg.shift_in = ShiftConfig {
@@ -389,25 +394,27 @@ async fn autobaud_task(pio0: embassy_rp::peripherals::PIO0, rx_pin: embassy_rp::
     sm.set_config(&cfg);
     sm.set_enable(true);
 
-    // set rx pin function back to UART
-    unsafe {
-        pin_io(&rx_pin).ctrl().write(|w| w.set_funcsel(embassy_rp::pac::io::vals::Gpio17ctrlFuncsel::UART0_RX.0));
-    }
-
-    if false {
-        cfg.fifo_join = FifoJoin::RxOnly;
-        sm.set_config(&cfg);
-        // set_config has also set the origin so let's go back to after the pull
-        // -> still doesn't work for some reason
-        unsafe { embassy_rp::pio_instr_util::exec_jmp(&mut sm, 1); };
-    }
-
     // Timeout: Modbus wants 1.5 ms between frames so we make this a bit smaller. SM runs at 25 MHz
     // but we need two clocks per loop.
     let timeout_start_value = (SM_FREQ as f32 * 1.2e-3 / 2.) as u32;
     sm.tx().push(timeout_start_value);
     info!("timeout_start_value: {} = 0x{:08x}", timeout_start_value, timeout_start_value);
 
+    // switch to the real program and join FIFOs
+    unsafe { common.free_instr(loaded_program.used_memory); };
+    sm.set_enable(false);
+    let relocated = RelocatedProgram::new(&prg.program);
+    cfg.use_program(&common.load_program(&relocated), &[]);
+    cfg.fifo_join = FifoJoin::RxOnly;
+    sm.set_config(&cfg);
+    sm.set_enable(true);
+
+    // set rx pin function back to UART
+    // (PIO can always read and we don't want to write but embassy_rp cannot know that so it "helpfully" configured the pin for PIO function.)
+    unsafe {
+        pin_io(&rx_pin).ctrl().write(|w| w.set_funcsel(embassy_rp::pac::io::vals::Gpio17ctrlFuncsel::UART0_RX.0));
+    }
+
     let mut dma_in_ref = dma_channel.into_ref();
     let mut din = [42u32; 9];
     let mut bit_index = 0;
@@ -421,7 +428,7 @@ async fn autobaud_task(pio0: embassy_rp::peripherals::PIO0, rx_pin: embassy_rp::
             Either3::First(_) => {
                 let mut sum = 0;
                 let mut first = 0;
-                let mut ok = true;
+                let mut ok = false;
                 let mut got_to_eight = false;
                 for i in 0..din.len() {
                     if din[i] == 0 {
@@ -488,6 +495,18 @@ async fn autobaud_task(pio0: embassy_rp::peripherals::PIO0, rx_pin: embassy_rp::
                     info!("Guessed baud rate: {}", baud);
                 }
             },
+            Either3::Second(_) => {
+                if false {
+                    for i in 0..din.len() {
+                        if din[i] == 0 {
+                            bit_index = 0
+                        } else {
+                            bit_index += 1
+                        }
+                        info!("SM in {}: {:08x}", i, din[i]);
+                    }
+                }
+            },
             _ => {
             },
         }