diff --git a/Production/Firmware/chu_pico.uf2 b/Production/Firmware/chu_pico.uf2
index 4fa6347..ec2531e 100644
Binary files a/Production/Firmware/chu_pico.uf2 and b/Production/Firmware/chu_pico.uf2 differ
diff --git a/firmware/src/cmd.c b/firmware/src/cmd.c
index 5c64f35..1bc0b5b 100644
--- a/firmware/src/cmd.c
+++ b/firmware/src/cmd.c
@@ -51,10 +51,10 @@ static void handle_help(int argc, char *argv[])
 static void list_colors()
 {
     printf("[Colors]\n");
-    printf("  Key upper: %6x, lower: %6x, both: %6x, off: %6x\n", 
+    printf("  Key upper: %06x, lower: %06x, both: %06x, off: %06x\n", 
            chu_cfg->colors.key_on_upper, chu_cfg->colors.key_on_lower,
            chu_cfg->colors.key_on_both, chu_cfg->colors.key_off);
-    printf("  Gap: %6x\n", chu_cfg->colors.gap);
+    printf("  Gap: %06x\n", chu_cfg->colors.gap);
 }
 
 static void list_style()
@@ -234,8 +234,8 @@ static uint8_t *extract_key(const char *param)
         return NULL;
     }
 
-    int id = extract_non_neg_int(param, len - 1);
-    if ((id < 1) || (id > 16)) {
+    int id = extract_non_neg_int(param, len - 1) - 1;
+    if ((id < 0) || (id > 15)) {
         return NULL;
     }
 
diff --git a/firmware/src/vl53l0x.c b/firmware/src/vl53l0x.c
index 8c769f2..4647f8c 100644
--- a/firmware/src/vl53l0x.c
+++ b/firmware/src/vl53l0x.c
@@ -247,14 +247,14 @@ bool vl53l0x_init_tof(bool io_2v8)
 void write_reg(uint8_t reg, uint8_t value)
 {
     uint8_t data[2] = { reg, value };
-    i2c_write_blocking(I2C_PORT, addr, data, 2, false);
+    i2c_write_blocking_until(I2C_PORT, addr, data, 2, false, time_us_64() + IO_TIMEOUT_US);
 }
 
 // Write a 16-bit register
 void write_reg16(uint8_t reg, uint16_t value)
 {
     uint8_t data[3] = { reg, value >> 8, value & 0xff };
-    i2c_write_blocking(I2C_PORT, addr, data, 3, false);
+    i2c_write_blocking_until(I2C_PORT, addr, data, 3, false, time_us_64() + IO_TIMEOUT_US);
 }
 
 // Write a 32-bit register
@@ -262,15 +262,15 @@ void write_reg32(uint8_t reg, uint32_t value)
 {
     uint8_t data[5] = { reg, value >> 24, (value >> 16) & 0xff,
                              (value >> 8) & 0xff, value & 0xff };
-    i2c_write_blocking(I2C_PORT, addr, data, 5, false);
+    i2c_write_blocking_until(I2C_PORT, addr, data, 5, false, time_us_64() + IO_TIMEOUT_US);
 }
 
 // Read an 8-bit register
 uint8_t read_reg(uint8_t reg)
 {
     uint8_t value;
-    i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
-    i2c_read_blocking(I2C_PORT, addr, &value, 1, false);
+    i2c_write_blocking_until(I2C_PORT, addr, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
+    i2c_read_blocking_until(I2C_PORT, addr, &value, 1, false, time_us_64() + IO_TIMEOUT_US);
     return value;
 }
 
@@ -278,8 +278,8 @@ uint8_t read_reg(uint8_t reg)
 uint16_t read_reg16(uint8_t reg)
 {
     uint8_t value[2];
-    i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
-    i2c_read_blocking(I2C_PORT, addr, value, 2, false);
+    i2c_write_blocking_until(I2C_PORT, addr, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
+    i2c_read_blocking_until(I2C_PORT, addr, value, 2, false, time_us_64() + IO_TIMEOUT_US);
     return value[0] << 8 | value[1];
 }
 
@@ -287,8 +287,8 @@ uint16_t read_reg16(uint8_t reg)
 uint32_t read_reg32(uint8_t reg)
 {
     uint8_t value[4];
-    i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
-    i2c_read_blocking(I2C_PORT, addr, value, 4, false);
+    i2c_write_blocking_until(I2C_PORT, addr, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
+    i2c_read_blocking_until(I2C_PORT, addr, value, 4, false, time_us_64() + IO_TIMEOUT_US);
     return (uint32_t)((value[0] << 24) | (value[1] << 16) | (value[2] << 8) | value[3]);
 }
 
@@ -296,16 +296,16 @@ uint32_t read_reg32(uint8_t reg)
 // starting at the given register
 void write_many(uint8_t reg, const uint8_t *src, uint8_t len)
 {
-    i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
-    i2c_write_blocking(I2C_PORT, addr, src, len, false);
+    i2c_write_blocking_until(I2C_PORT, addr, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
+    i2c_write_blocking_until(I2C_PORT, addr, src, len, false, time_us_64() + IO_TIMEOUT_US);
 }
 
 // Read an arbitrary number of bytes from the sensor, starting at the given
 // register, into the given array
 void read_many(uint8_t reg, uint8_t *dst, uint8_t len)
 {
-    i2c_write_blocking(I2C_PORT, addr, &reg, 1, true);
-    i2c_read_blocking(I2C_PORT, addr, dst, len, false);
+    i2c_write_blocking_until(I2C_PORT, addr, &reg, 1, true, time_us_64() + IO_TIMEOUT_US);
+    i2c_read_blocking_until(I2C_PORT, addr, dst, len, false, time_us_64() + IO_TIMEOUT_US);
 }
 
 // Set the return signal rate limit check value in units of MCPS (mega counts