diff --git a/src/command.c b/src/command.c
index b0c7f882..e3167baf 100644
--- a/src/command.c
+++ b/src/command.c
@@ -15,6 +15,18 @@
 
 static uint8_t next_sequence = MESSAGE_DEST;
 
+static uint32_t
+command_encode_ptr(void *p)
+{
+    return (size_t)p;
+}
+
+void *
+command_decode_ptr(uint32_t v)
+{
+    return (void*)(size_t)v;
+}
+
 
 /****************************************************************
  * Binary message parsing
@@ -78,7 +90,7 @@ command_parsef(uint8_t *p, uint8_t *maxend
             if (p + len > maxend)
                 goto error;
             *args++ = len;
-            *args++ = (size_t)p;
+            *args++ = command_encode_ptr(p);
             p += len;
             break;
         }
diff --git a/src/command.h b/src/command.h
index e7551ed7..67e595bc 100644
--- a/src/command.h
+++ b/src/command.h
@@ -71,6 +71,7 @@ enum {
 };
 
 // command.c
+void *command_decode_ptr(uint32_t v);
 uint8_t *command_parsef(uint8_t *p, uint8_t *maxend
                         , const struct command_parser *cp, uint32_t *args);
 uint_fast8_t command_encodef(uint8_t *buf, const struct command_encoder *ce
diff --git a/src/debugcmds.c b/src/debugcmds.c
index bdbd0d48..22350c18 100644
--- a/src/debugcmds.c
+++ b/src/debugcmds.c
@@ -48,7 +48,7 @@ void
 command_debug_read(uint32_t *args)
 {
     uint8_t order = args[0];
-    void *ptr = (void*)(size_t)args[1];
+    void *ptr = command_decode_ptr(args[1]);
     uint32_t v;
     irqstatus_t flag = irq_save();
     switch (order) {
@@ -66,7 +66,7 @@ void
 command_debug_write(uint32_t *args)
 {
     uint8_t order = args[0];
-    void *ptr = (void*)(size_t)args[1];
+    void *ptr = command_decode_ptr(args[1]);
     uint32_t v = args[2];
     irqstatus_t flag = irq_save();
     switch (order) {
@@ -83,7 +83,7 @@ void
 command_debug_ping(uint32_t *args)
 {
     uint8_t len = args[0];
-    char *data = (void*)(size_t)args[1];
+    char *data = command_decode_ptr(args[1]);
     sendf("pong data=%*s", len, data);
 }
 DECL_COMMAND_FLAGS(command_debug_ping, HF_IN_SHUTDOWN, "debug_ping data=%*s");
diff --git a/src/generic/misc.h b/src/generic/misc.h
index eebd16d8..5e50633e 100644
--- a/src/generic/misc.h
+++ b/src/generic/misc.h
@@ -2,7 +2,6 @@
 #define __GENERIC_MISC_H
 
 #include <stdarg.h> // va_list
-#include <stddef.h> // size_t
 #include <stdint.h> // uint8_t
 
 struct command_encoder;
diff --git a/src/i2ccmds.c b/src/i2ccmds.c
index 388af6ed..cde0f6fa 100644
--- a/src/i2ccmds.c
+++ b/src/i2ccmds.c
@@ -4,6 +4,7 @@
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
+#include <string.h> // memcpy
 #include "basecmd.h" //oid_alloc
 #include "command.h"  //sendf
 #include "sched.h" //DECL_COMMAND
@@ -30,7 +31,7 @@ command_i2c_write(uint32_t *args)
     uint8_t oid = args[0];
     struct i2cdev_s *i2c = oid_lookup(oid, command_config_i2c);
     uint8_t data_len = args[1];
-    uint8_t *data = (void*)(size_t)args[2];
+    uint8_t *data = command_decode_ptr(args[2]);
     i2c_write(i2c->i2c_config, data_len, data);
 }
 DECL_COMMAND(command_i2c_write, "i2c_write oid=%c data=%*s");
@@ -41,10 +42,9 @@ command_i2c_read(uint32_t * args)
     uint8_t oid = args[0];
     struct i2cdev_s *i2c = oid_lookup(oid, command_config_i2c);
     uint8_t reg_len = args[1];
-    uint8_t *reg = (void*)(size_t)args[2];
+    uint8_t *reg = command_decode_ptr(args[2]);
     uint8_t data_len = args[3];
-    uint8_t receive_array[data_len];
-    uint8_t *data = (void*)(size_t)receive_array;
+    uint8_t data[data_len];
     i2c_read(i2c->i2c_config, reg_len, reg, data_len, data);
     sendf("i2c_read_response oid=%c response=%*s", oid, data_len, data);
 }
@@ -56,24 +56,20 @@ command_i2c_modify_bits(uint32_t *args)
     uint8_t oid = args[0];
     struct i2cdev_s *i2c = oid_lookup(oid, command_config_i2c);
     uint8_t reg_len = args[1];
-    uint8_t *reg = (void*)(size_t)args[2];
+    uint8_t *reg = command_decode_ptr(args[2]);
     uint32_t clear_set_len = args[3];
-    if (clear_set_len % 2 != 0) {
+    if (clear_set_len % 2 != 0)
         shutdown("i2c_modify_bits: Odd number of bits!");
-    }
     uint8_t data_len = clear_set_len/2;
-    uint8_t *clear_set = (void*)(size_t)args[4];
-    uint8_t receive_array[reg_len + data_len];
-    uint8_t *receive_data = (void*)(size_t)receive_array;
-    for (int i = 0; i < reg_len; i++) {
-        *(receive_data + i) = *(reg + i);
-    }
+    uint8_t *clear_set = command_decode_ptr(args[4]);
+    uint8_t receive_data[reg_len + data_len];
+    memcpy(receive_data, reg, reg_len);
     i2c_read(i2c->i2c_config, reg_len, reg, data_len, receive_data + reg_len);
     for (int i = 0; i < data_len; i++) {
-        *(receive_data + reg_len + i) &= ~(*(clear_set + i));
-        *(receive_data + reg_len + i) |= *(clear_set + clear_set_len/2 + i);
+        receive_data[reg_len + i] &= ~clear_set[i];
+        receive_data[reg_len + i] |= clear_set[data_len + i];
     }
-    i2c_write(i2c->i2c_config, reg_len + data_len, receive_array);
+    i2c_write(i2c->i2c_config, reg_len + data_len, receive_data);
 }
 DECL_COMMAND(command_i2c_modify_bits,
              "i2c_modify_bits oid=%c reg=%*s clear_set_bits=%*s");
diff --git a/src/lcd_hd44780.c b/src/lcd_hd44780.c
index 9137cc71..fdba70c9 100644
--- a/src/lcd_hd44780.c
+++ b/src/lcd_hd44780.c
@@ -124,7 +124,7 @@ command_hd44780_send_cmds(uint32_t *args)
 {
     struct hd44780 *h = oid_lookup(args[0], command_config_hd44780);
     gpio_out_write(h->rs, 0);
-    uint8_t len = args[1], *cmds = (void*)(size_t)args[2];
+    uint8_t len = args[1], *cmds = command_decode_ptr(args[2]);
     hd44780_xmit(h, len, cmds);
 }
 DECL_COMMAND(command_hd44780_send_cmds, "hd44780_send_cmds oid=%c cmds=%*s");
@@ -134,7 +134,7 @@ command_hd44780_send_data(uint32_t *args)
 {
     struct hd44780 *h = oid_lookup(args[0], command_config_hd44780);
     gpio_out_write(h->rs, 1);
-    uint8_t len = args[1], *data = (void*)(size_t)args[2];
+    uint8_t len = args[1], *data = command_decode_ptr(args[2]);
     hd44780_xmit(h, len, data);
 }
 DECL_COMMAND(command_hd44780_send_data, "hd44780_send_data oid=%c data=%*s");
diff --git a/src/lcd_st7920.c b/src/lcd_st7920.c
index e4728e74..095870c5 100644
--- a/src/lcd_st7920.c
+++ b/src/lcd_st7920.c
@@ -137,7 +137,7 @@ command_st7920_send_cmds(uint32_t *args)
 {
     struct st7920 *s = oid_lookup(args[0], command_config_st7920);
     st7920_xmit_byte(s, SYNC_CMD);
-    uint8_t len = args[1], *cmds = (void*)(size_t)args[2];
+    uint8_t len = args[1], *cmds = command_decode_ptr(args[2]);
     st7920_xmit(s, len, cmds);
 }
 DECL_COMMAND(command_st7920_send_cmds, "st7920_send_cmds oid=%c cmds=%*s");
@@ -147,7 +147,7 @@ command_st7920_send_data(uint32_t *args)
 {
     struct st7920 *s = oid_lookup(args[0], command_config_st7920);
     st7920_xmit_byte(s, SYNC_DATA);
-    uint8_t len = args[1], *data = (void*)(size_t)args[2];
+    uint8_t len = args[1], *data = command_decode_ptr(args[2]);
     st7920_xmit(s, len, data);
 }
 DECL_COMMAND(command_st7920_send_data, "st7920_send_data oid=%c data=%*s");
diff --git a/src/neopixel.c b/src/neopixel.c
index 5bf17cad..1a38d4da 100644
--- a/src/neopixel.c
+++ b/src/neopixel.c
@@ -181,7 +181,7 @@ command_neopixel_update(uint32_t *args)
     struct neopixel_s *n = oid_lookup(oid, command_config_neopixel);
     uint_fast16_t pos = args[1];
     uint_fast8_t data_len = args[2];
-    uint8_t *data = (void*)(size_t)args[3];
+    uint8_t *data = command_decode_ptr(args[3]);
     if (pos & 0x8000 || pos + data_len > n->data_size)
         shutdown("Invalid neopixel update command");
     memcpy(&n->data[pos], data, data_len);
diff --git a/src/spicmds.c b/src/spicmds.c
index 654e9436..f24bfca3 100644
--- a/src/spicmds.c
+++ b/src/spicmds.c
@@ -101,7 +101,7 @@ command_spi_transfer(uint32_t *args)
     uint8_t oid = args[0];
     struct spidev_s *spi = spidev_oid_lookup(oid);
     uint8_t data_len = args[1];
-    uint8_t *data = (void*)(size_t)args[2];
+    uint8_t *data = command_decode_ptr(args[2]);
     spidev_transfer(spi, 1, data_len, data);
     sendf("spi_transfer_response oid=%c response=%*s", oid, data_len, data);
 }
@@ -112,7 +112,7 @@ command_spi_send(uint32_t *args)
 {
     struct spidev_s *spi = spidev_oid_lookup(args[0]);
     uint8_t data_len = args[1];
-    uint8_t *data = (void*)(size_t)args[2];
+    uint8_t *data = command_decode_ptr(args[2]);
     spidev_transfer(spi, 0, data_len, data);
 }
 DECL_COMMAND(command_spi_send, "spi_send oid=%c data=%*s");
@@ -137,7 +137,7 @@ command_config_spi_shutdown(uint32_t *args)
         args[0], command_config_spi_shutdown, sizeof(*sd) + shutdown_msg_len);
     sd->spi = spi;
     sd->shutdown_msg_len = shutdown_msg_len;
-    uint8_t *shutdown_msg = (void*)(size_t)args[3];
+    uint8_t *shutdown_msg = command_decode_ptr(args[3]);
     memcpy(sd->shutdown_msg, shutdown_msg, shutdown_msg_len);
 }
 DECL_COMMAND(command_config_spi_shutdown,
diff --git a/src/tmcuart.c b/src/tmcuart.c
index 50f05122..a2f5f8fc 100644
--- a/src/tmcuart.c
+++ b/src/tmcuart.c
@@ -195,7 +195,7 @@ command_tmcuart_send(uint32_t *args)
         // Uart is busy - silently drop this request (host should retransmit)
         return;
     uint8_t write_len = args[1];
-    uint8_t *write = (void*)(size_t)args[2];
+    uint8_t *write = command_decode_ptr(args[2]);
     uint8_t read_len = args[3];
     if (write_len > sizeof(t->data) || read_len > sizeof(t->data))
         shutdown("tmcuart data too large");