diff --git a/firmware/rust1/Cargo.lock b/firmware/rust1/Cargo.lock
index 1826c0d37baec39b756e34c094e36fb9b5770356..cad3501ee83190e952d33627698bd789b97b22b3 100644
--- a/firmware/rust1/Cargo.lock
+++ b/firmware/rust1/Cargo.lock
@@ -1027,6 +1027,7 @@ name = "heizung"
 version = "0.1.0"
 dependencies = [
  "byte-slice-cast 1.2.2",
+ "byteorder",
  "cortex-m",
  "cortex-m-rt",
  "crc",
diff --git a/firmware/rust1/Cargo.toml b/firmware/rust1/Cargo.toml
index d2db0ff56b5059dba886271b197a64c6bfae76be..3ea67b9473f3ed25c70a997f5696eca58746186a 100644
--- a/firmware/rust1/Cargo.toml
+++ b/firmware/rust1/Cargo.toml
@@ -56,6 +56,7 @@ pio = "0.2.1"
 
 heapless = "0.7.16"
 crc = "3.0.1"
+byteorder = "1.4.3"
 
 [profile.release]
 debug = true
diff --git a/firmware/rust1/src/modbus_server.rs b/firmware/rust1/src/modbus_server.rs
index a119290a1535fac064b132ac297827157d34aef5..14511a68f1da8c2999634181fc80c52256f642f4 100644
--- a/firmware/rust1/src/modbus_server.rs
+++ b/firmware/rust1/src/modbus_server.rs
@@ -4,6 +4,8 @@ use futures::Future;
 use heapless::Vec;
 use crc::{Crc, CRC_16_MODBUS, Digest};
 use embassy_rp::uart;
+use core::fmt::Write;
+use byteorder::{BigEndian, ReadBytesExt, WriteBytesExt};
 
 use crate::rs485::{RS485Handler};
 
@@ -99,7 +101,20 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
 
         txbuf.clear();
         let capacity = txbuf.capacity();
-        let mut push = |x| -> _ { txbuf.push(x).or(Err(ServerDeviceFailure)) };
+
+        let map_err = |x: Result<(), ()> | x.or(Err(ServerDeviceFailure));
+        fn push(txbuf: &mut Vec<u8, BUF_LENGTH>, x: u8) -> Result<(), ModbusErrorCode> {
+            map_err(txbuf.push(x));
+        }
+        fn push_many(txbuf: &mut Vec<u8, BUF_LENGTH>, xs: &[u8]) -> Result<(), ModbusErrorCode> {
+            for x in xs {
+                push(txbuf, x)?;
+            }
+            Ok(())
+        }
+        fn push_u16be(txbuf: &mut Vec<u8, BUF_LENGTH>, x: u16) -> Result<(), ModbusErrorCode> {
+            map_err(txbuf.write_u16::<BigEndian>(x));
+        }
     
         // see https://modbus.org/docs/Modbus_Application_Protocol_V1_1b3.pdf
         match rxbuf[1] {
@@ -111,9 +126,8 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                 if quantity < 1 || quantity > 2000 || byte_count as usize > capacity - 5 {
                     return Err(IllegalDataValue);
                 };
-                push(rxbuf[0])?;
-                push(rxbuf[1])?;
-                push(byte_count as u8)?;
+                push_many(txbuf, &rxbuf[0..2])?;
+                push(txbuf, byte_count as u8)?;
                 for i in 0..byte_count {
                     let mut x = 0;
                     for j in 0..7 {
@@ -121,7 +135,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                             x |= (if self.regs.read_discrete_input(rxbuf[0], start + i*8 + j)? {1} else {0}) << j;
                         }
                     }
-                    push(x)?;
+                    push(txbuf, x)?;
                 }
                 Ok(())
             },
@@ -137,13 +151,11 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                 if byte_count as usize > (capacity - 5) || quantity >= 0x7d {
                     return Err(IllegalDataValue);
                 }
-                push(rxbuf[0])?;
-                push(rxbuf[1])?;
-                push(byte_count as u8)?;
+                push_many(txbuf, &rxbuf[0..2])?;
+                push(txbuf, byte_count as u8)?;
                 for i in 0..quantity {
                     let value = self.regs.read_holding_register(rxbuf[0], start + i)?;
-                    push((value >> 8) as u8)?;
-                    push((value & 0xff) as u8)?;
+                    push_u16be(txbuf, value)?;
                 }
                 Ok(())
             },
@@ -158,13 +170,11 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                 if quantity as usize > (capacity - 5) / 2 || quantity >= 0x7d {
                     return Err(IllegalDataValue); // is that right?
                 }
-                push(rxbuf[0])?;
-                push(rxbuf[1])?;
-                push((quantity*2) as u8)?;
+                push_many(txbuf, &rxbuf[0..2])?;
+                push(txbuf, (quantity*2) as u8)?;
                 for i in 0..quantity {
                     let value = self.regs.read_input_register(rxbuf[0], start + i)?;
-                    push((value >> 8) as u8)?;
-                    push((value & 0xff) as u8)?;
+                    push_u16be(txbuf, value)?;
                 }
                 Ok(())
             },
@@ -185,9 +195,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                 };
                 self.regs.write_coil(rxbuf[0], start, value)?;
                 if should_reply {
-                    for i in 0..6 {
-                        push(rxbuf[i])?;
-                    }
+                    push_many(txbuf, &rxbuf[0..6])?;
                 }
                 Ok(())
             },
@@ -201,11 +209,8 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                 let value = ((rxbuf[4] as u16) << 8) | rxbuf[5] as u16;
                 let actual_new_value = self.regs.write_register(rxbuf[0], start, value)?;
                 if should_reply {
-                    for i in 0..4 {
-                        push(rxbuf[i])?;
-                    }
-                    push((actual_new_value >> 8) as u8)?;
-                    push((actual_new_value & 0xff) as u8)?;
+                    push_many(txbuf, &rxbuf[0..4])?;
+                    push_u16be(txbuf, value)?;
                 }
                 Ok(())
             },
@@ -235,9 +240,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                     }
                 }
                 if should_reply {
-                    for i in 0..6 {
-                        push(rxbuf[i])?;
-                    }
+                    push_many(txbuf, &rxbuf[0..6])?;
                 }
                 Ok(())
             },
@@ -263,9 +266,7 @@ impl<REGS: ModbusRegisters> ModbusServer<REGS> {
                     self.regs.write_register(rxbuf[0], start + i, value)?;
                 }
                 if should_reply {
-                    for i in 0..6 {
-                        push(rxbuf[i])?;
-                    }
+                    push_many(txbuf, &rxbuf[0..6])?;
                 }
                 Ok(())
             },