Some cleanups

firmware/src/CMakeLists.txt

@@ -13,7 +13,7 @@ function(make_firmware board board_def)
 
     pico_generate_pio_header(${board} ${CMAKE_CURRENT_LIST_DIR}/ws2812.pio)
     
-    target_compile_options(${board} PRIVATE -Wfatal-errors -O3)
+    target_compile_options(${board} PRIVATE -Wall -Werror -Wfatal-errors -O3)
     target_include_directories(${board} PRIVATE ${CMAKE_CURRENT_LIST_DIR})
     target_include_directories(${board} PRIVATE 
                                ${BTSTACK_ROOT}/src

firmware/src/aime.c

@@ -76,7 +76,7 @@ const char *hw_version[] = { "TN32MSEC003S H/W Ver3.0", "837-15396" };
 const char *led_info[] = { "15084\xFF\x10\x00\x12", "000-00000\xFF\x11\x40" };
 static int baudrate_mode = 0;
 
-static void aime_set_baudrate(int mode)
+void aime_set_baudrate(int mode)
 {
     baudrate_mode = (mode == 0) ? 0 : 1;
 }
@@ -91,9 +91,7 @@ void aime_init(int interface)
     pn532_config_sam();
 }
 
-typedef struct __attribute__((packed)) {
+static uint8_t mifare_keys[2][6]; // 'KeyA' and 'KeyB'
-
-} felica_req_t;
 
 static union __attribute__((packed)) {
     struct {
@@ -116,6 +114,10 @@ static union __attribute__((packed)) {
         uint8_t cmd;
         uint8_t payload_len;
         union {
+            struct {
+                uint8_t uid[4];
+                uint8_t block_id;
+            } mifare;
             struct {
                 uint8_t idm[8];
                 uint8_t len;
@@ -136,8 +138,6 @@ struct {
     uint64_t time;
 } req_ctx;
 
-static uint8_t key_sets[2][6]; // 'KeyA' and 'KeyB'
-
 static void build_response(int payload_len)
 {
     response.len = payload_len + 6;
@@ -170,7 +170,7 @@ static void send_response()
     tud_cdc_n_write_flush(aime_interface);
 }
 
-static void simple_response(uint8_t status)
+static void send_simple_response(uint8_t status)
 {
     build_response(0);
     response.status = status;
@@ -179,8 +179,8 @@ static void simple_response(uint8_t status)
 
 static void cmd_to_normal_mode()
 {
-    simple_response(pn532_firmware_ver() ? STATUS_NFCRW_FIRMWARE_UP_TO_DATE
+    send_simple_response(pn532_firmware_ver() ? STATUS_NFCRW_FIRMWARE_UP_TO_DATE
-                                         : STATUS_NFCRW_INIT_ERROR);
+                                              : STATUS_NFCRW_INIT_ERROR);
 }
 
 static void cmd_fake_version(const char *version[])
@@ -191,25 +191,16 @@ static void cmd_fake_version(const char *version[])
     send_response();
 }
 
-static void cmd_get_hw_version()
+static void cmd_key_set(uint8_t key[6])
 {
-    build_response(2);
+    memcpy(key, request.payload, 6);
-    response.payload[0] = 0x01;
+    send_simple_response(STATUS_OK);
-    response.payload[1] = 0x00;
-    send_response();
-}
-
-static void cmd_key_set(int type)
-{
-    memcpy(key_sets[type], request.payload, 6);
-    build_response(0);
-    send_response();
 }
 
 static void cmd_set_polling(bool enabled)
 {
     pn532_set_rf_field(0, enabled ? 1 : 0);
-    simple_response(STATUS_OK);
+    send_simple_response(STATUS_OK);
 }
 
 static void cmd_detect_card()
@@ -218,26 +209,31 @@ static void cmd_detect_card()
         uint8_t count;
         uint8_t type;
         uint8_t id_len;
-        uint8_t idm[8];
+        union {
-        uint8_t pmm[8];
+            struct {
-        uint8_t syscode[2];
+                uint8_t idm[8];
+                uint8_t pmm[8];
+                uint8_t syscode[2];
+            };
+            uint8_t uid[6];
+        };
     } card_info_t;
     
     card_info_t *card = (card_info_t *) response.payload;
 
     int len = sizeof(card->idm);
-    if (pn532_poll_mifare(card->idm, &len)) {
+    if (pn532_poll_mifare(card->uid, &len)) {
         build_response(len > 4 ? 10 : 7);
         card->count = 1;
         card->type = 0x10;
         card->id_len = len;
-        printf("Card Mifare %d\n", card->id_len);
+        printf("Detected Mifare %d\n", card->id_len);
     } else if (pn532_poll_felica(card->idm, card->pmm, card->syscode)) {
         build_response(19);
         card->count = 1;
         card->type = 0x20;
         card->id_len = 16;
-        printf("Card Felica - ");
+        printf("Detected Felica -");
         for (int i = 0; i < 8; i++) {
             printf(" %02x", card->idm[i]);
         }
@@ -255,6 +251,36 @@ static void cmd_detect_card()
     send_response();
 }
 
+static void cmd_card_select()
+{
+    printf("CARD SELECT %d\n", request.payload_len);
+    send_simple_response(STATUS_OK);
+}
+
+static void cmd_mifare_auth(int type)
+{
+    printf("MIFARE AUTH\n");
+    pn532_mifare_auth(request.mifare.uid, request.mifare.block_id, type, mifare_keys[type]);
+    send_simple_response(STATUS_OK);
+}
+
+static void cmd_mifare_read()
+{
+    printf("MIFARE READ %02x %02x %02x %02x %02x\n", request.mifare.block_id,
+            request.mifare.uid[0], request.mifare.uid[1], request.mifare.uid[2],
+           request.mifare.uid[3]);
+    build_response(16);
+    memset(response.payload, 0, 16);
+    pn532_mifare_read(request.mifare.block_id, response.payload);
+    send_response();
+}
+
+static void cmd_mifare_halt()
+{
+    printf("MIFARE HALT\n");
+    send_simple_response(STATUS_OK);
+}
+
 typedef struct __attribute__((packed)) {
     uint8_t len;
     uint8_t code;
@@ -327,7 +353,7 @@ static int cmd_felica_read()
         if (block_id == 0x8082) {
             memcpy(read_resp->block_data[i], felica_resp->idm, 8);
         }
-//        pn532_felica_read_wo_encrypt(svc_code, block_id, read_resp->block_data[i]);
+        pn532_felica_read_wo_encrypt(svc_code, block_id, read_resp->block_data[i]);
     }
 
     read_resp->rw_status[0] = 0x00;
@@ -355,13 +381,23 @@ static int cmd_felica_write()
     felica_resp_t *felica_resp = (felica_resp_t *) response.payload;
     uint8_t *rw_status = felica_resp->data;
 
+    printf("svc code: %04x\n", svc_code);
+    printf("blocks:");
+    for (int i = 0; i < block_num; i++) {
+        uint16_t block_id = (block[i * 2 + 1] << 8) | block[i * 2 + 2];
+        printf(" %04x", block_id);
+    }
+    printf("\n");
+
+    uint8_t *block_data = block + 1 + block_num * 2;
+
     rw_status[0] = 0x00;
     rw_status[1] = 0x00;
 
     for (int i = 0; i < block_num; i++) {
+        printf("writing %02x %02x\n", block_data[i * 16], block_data[i * 16 + 15]);
         uint16_t block_id = (block[i * 2 + 1] << 8) | block[i * 2 + 2];
-        int result = 0;
+        int result = pn532_felica_write_wo_encrypt(svc_code, block_id, block_data + i * 16);
-        //pn532_felica_write_wo_encrypt(svc_code, block_id, read_resp->block_data[i]);
         if (result < 0) {
             rw_status[0] = 0x01;
             rw_status[1] = 0x01;
@@ -389,7 +425,7 @@ static void cmd_felica()
     card_id_t card;
 
     if (!pn532_poll_felica(card.idm, card.pmm, card.syscode)) {
-        simple_response(STATUS_FELICA_ERROR);        
+        send_simple_response(STATUS_FELICA_ERROR);        
     }
 
     uint8_t felica_code = request.felica.code;
@@ -420,7 +456,7 @@ static void cmd_felica()
     }
 
     if (datalen < 0) {
-        simple_response(STATUS_FELICA_ERROR);
+        send_simple_response(STATUS_FELICA_ERROR);
         return;
     }
 
@@ -466,11 +502,11 @@ static void aime_handle_frame()
             break;
         case CMD_MIFARE_KEY_SET_A:
             printf("CMD_MIFARE_KEY_SET_A\n");
-            cmd_key_set(0);
+            cmd_key_set(mifare_keys[0]);
             break;
         case CMD_MIFARE_KEY_SET_B:
             printf("CMD_MIFARE_KEY_SET_B\n");
-            cmd_key_set(1);
+            cmd_key_set(mifare_keys[1]);
             break;
 
         case CMD_START_POLLING:
@@ -487,9 +523,23 @@ static void aime_handle_frame()
             break;
 
         case CMD_CARD_SELECT:
+            cmd_card_select();
+            break;
+        
+        case CMD_MIFARE_AUTHORIZE_A:
+            cmd_mifare_auth(0);
+            break;
+
         case CMD_MIFARE_AUTHORIZE_B:
+            cmd_mifare_auth(1);
+            break;
+        
         case CMD_MIFARE_READ:
+            cmd_mifare_read();
+            break;
+
         case CMD_CARD_HALT:
+            cmd_mifare_halt();
             break;
 
         case CMD_EXT_BOARD_INFO:
@@ -501,7 +551,7 @@ static void aime_handle_frame()
 
         case CMD_SEND_HEX_DATA:
         case CMD_EXT_TO_NORMAL_MODE:
-            simple_response(STATUS_OK);
+            send_simple_response(STATUS_OK);
             break;
 
         default:
@@ -510,7 +560,7 @@ static void aime_handle_frame()
                 printf(" %02x", request.raw[i]);
             }
             printf("]\n");
-            simple_response(STATUS_OK);
+            send_simple_response(STATUS_OK);
             break;
     }
 }

firmware/src/aime.h

@@ -10,4 +10,7 @@ void aime_init(int interface);
 void aime_update();
 uint32_t aime_led_color();
 
+// mode 0 or 1
+void aime_set_baudrate(int mode);
+
 #endif

firmware/src/cli.c

@@ -94,7 +94,7 @@ int cli_extract_non_neg_int(const char *param, int len)
     }
     int result = 0;
     for (int i = 0; i < len; i++) {
-        if (!isdigit(param[i])) {
+        if (!isdigit((uint8_t)param[i])) {
             return -1;
         }
         result = result * 10 + param[i] - '0';

firmware/src/commands.c

@@ -23,10 +23,10 @@
 static void disp_colors()
 {
     printf("[Colors]\n");
-    printf("  Key upper: %06x, lower: %06x, both: %06x, off: %06x\n", 
+    printf("  Key upper: %06lx, lower: %06lx, both: %06lx, off: %06lx\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: %06x\n", chu_cfg->colors.gap);
+    printf("  Gap: %06lx\n", chu_cfg->colors.gap);
 }
 
 static void disp_style()
@@ -263,7 +263,7 @@ static void handle_filter(int argc, char *argv[])
     disp_sense();
 }
 
-static uint8_t *extract_key(const char *param)
+static int8_t *extract_key(const char *param)
 {
     int len = strlen(param);
 
@@ -306,7 +306,7 @@ static void handle_sense(int argc, char *argv[])
                         "  >sense +\n"
                         "  >sense -\n"
                         "  >sense 1A +\n"
-                        "  >sense 13B -\n";
+                        "  >sense 13B -\n"
                         "  >sense * 0\n";
     if ((argc < 1) || (argc > 2)) {
         printf(usage);
@@ -327,7 +327,7 @@ static void handle_sense(int argc, char *argv[])
                 sense_do_op(&chu_cfg->sense.keys[i], op[0]);
             }
         } else {
-            uint8_t *key = extract_key(argv[0]);
+            int8_t *key = extract_key(argv[0]);
             if (!key) {
                 printf(usage);
                 return;

firmware/src/gp2y0e.h

@@ -11,7 +11,7 @@
 
 #define GP2Y0E_DEF_ADDR 0x40
 
-static inline uint16_t gp2y0e_write(i2c_inst_t *port, uint8_t addr, uint8_t val)
+static inline void gp2y0e_write(i2c_inst_t *port, uint8_t addr, uint8_t val)
 {
     uint8_t cmd[] = {addr, val};
     i2c_write_blocking_until(port, GP2Y0E_DEF_ADDR, cmd, 2, false, time_us_64() + 1000);

firmware/src/main.c

@@ -79,7 +79,7 @@ static void gen_joy_report()
 }
 
 const uint8_t keycode_table[128][2] = { HID_ASCII_TO_KEYCODE };
-const char keymap[38 + 1] = NKRO_KEYMAP; // 32 keys, 6 air keys, 1 terminator
+const uint8_t keymap[38 + 1] = NKRO_KEYMAP; // 32 keys, 6 air keys, 1 terminator
 static void gen_nkro_report()
 {
     for (int i = 0; i < 32; i++) {
@@ -137,7 +137,7 @@ static void run_lights()
         uint8_t b = aime_color;
 
         uint32_t blink = now / 100000;
-        aime_color = (now & 1) ? rgb32(r, g, b, false) : 0;
+        aime_color = (blink & 1) ? rgb32(r, g, b, false) : 0;
 
         rgb_set_color(34, aime_color);
         rgb_set_color(35, aime_color);

firmware/src/mpr121.c

@@ -133,19 +133,19 @@ void mpr121_init(uint8_t i2c_addr)
 
 #define ABS(x) ((x) < 0 ? -(x) : (x))
 
-static void mpr121_read_many(uint8_t addr, uint8_t reg, uint8_t *buf, size_t n)
+static void mpr121_read_many(uint8_t addr, uint8_t reg, uint8_t *buf, int num)
 {
     i2c_write_blocking_until(I2C_PORT, addr, &reg, 1, true,
                              time_us_64() + IO_TIMEOUT_US);
-    i2c_read_blocking_until(I2C_PORT, addr, buf, n, false,
+    i2c_read_blocking_until(I2C_PORT, addr, buf, num, false,
-                             time_us_64() + IO_TIMEOUT_US * n / 2);
+                             time_us_64() + IO_TIMEOUT_US * num / 2);
 }
 
-static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, size_t n)
+static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, int num)
 {
-    uint8_t vals[n * 2];
+    uint8_t vals[num * 2];
-    mpr121_read_many(addr, reg, vals, n * 2);
+    mpr121_read_many(addr, reg, vals, num * 2);
-    for (int i = 0; i < n; i++) {
+    for (int i = 0; i < num; i++) {
         buf[i] = (vals[i * 2 + 1] << 8) | vals[i * 2];
     }
 }
@@ -153,7 +153,7 @@ static void mpr121_read_many16(uint8_t addr, uint8_t reg, uint16_t *buf, size_t
 uint16_t mpr121_touched(uint8_t addr)
 {
     uint16_t touched;
-    mpr121_read_many16(addr, MPR121_TOUCH_STATUS_REG, &touched, 2);
+    mpr121_read_many16(addr, MPR121_TOUCH_STATUS_REG, &touched, 1);
     return touched;
 }
 
@@ -169,7 +169,7 @@ static uint8_t mpr121_stop(uint8_t addr)
     return ecr;
 }
 
-static uint8_t mpr121_resume(uint8_t addr, uint8_t ecr)
+static void mpr121_resume(uint8_t addr, uint8_t ecr)
 {
     write_reg(addr, MPR121_ELECTRODE_CONFIG_REG, ecr);
 }

firmware/src/pn532.c

@@ -103,7 +103,7 @@ static bool read_ack()
         return false;
     }
 
-    int result = read_frame(resp, 6);
+    read_frame(resp, 6);
 
     const uint8_t expect_ack[] = {0, 0, 0xff, 0, 0xff, 0};
     if (memcmp(resp, expect_ack, 6) != 0) {
@@ -131,7 +131,7 @@ int pn532_write_data(const uint8_t *data, uint8_t len)
     frame[5 + len] = ~checksum;
     frame[6 + len] = PN532_POSTAMBLE;
 
-    int result = write_frame(frame, 7 + len);
+    write_frame(frame, 7 + len);
 
     return read_ack();
 }
@@ -140,7 +140,7 @@ int pn532_read_data(uint8_t *data, uint8_t len)
 {
     uint8_t resp[len + 7];
 
-    int result = read_frame(resp, len + 7);
+    read_frame(resp, len + 7);
 
     if (resp[0] != PN532_PREAMBLE ||
         resp[1] != PN532_STARTCODE1 ||
@@ -264,7 +264,7 @@ uint32_t pn532_firmware_ver()
 bool pn532_config_sam()
 {
     uint8_t param[] = {0x01, 0x14, 0x01};
-    int result = pn532_write_command(0x14, param, 3);
+    pn532_write_command(0x14, param, 3);
 
     return pn532_read_response(0x14, NULL, 0) == 0;
 }
@@ -273,7 +273,7 @@ bool pn532_config_sam()
 bool pn532_set_rf_field(uint8_t auto_rf, uint8_t on_off)
 {
     uint8_t param[] = { 1, auto_rf | on_off };
-    int result = pn532_write_command(0x32, param, 2);
+    pn532_write_command(0x32, param, 2);
 
     return pn532_read_response(0x32, NULL, 0) >= 0;
 }
@@ -337,6 +337,47 @@ bool pn532_poll_felica(uint8_t uid[8], uint8_t pmm[8], uint8_t syscode[2])
     return true;
 }
 
+bool pn532_mifare_auth(const uint8_t uid[4], uint8_t block_id, uint8_t key_id, const uint8_t *key)
+{
+    uint8_t param[] = { 1, key_id ? 1 : 0, block_id,
+                       key[0], key[1], key[2], key[3], key[4], key[5],
+                       uid[0], uid[1], uid[2], uid[3] };
+    int ret = pn532_write_command(0x40, param, sizeof(param));
+    if (ret < 0) {
+        printf("Failed mifare auth command\n");
+        return false;
+    }
+    int result = pn532_read_response(0x40, readbuf, sizeof(readbuf));
+    if (readbuf[0] != 0) {
+        printf("PN532 Mifare AUTH failed %d %02x\n", result, readbuf[0]);
+        return false;
+    }
+
+    return true;
+}
+
+bool pn532_mifare_read(uint8_t block_id, uint8_t block_data[16])
+{
+    uint8_t param[] = { 1, 0x30, block_id };
+
+    int ret = pn532_write_command(0x40, param, sizeof(param));
+    if (ret < 0) {
+        printf("Failed mifare read command\n");
+        return false;
+    }
+
+    int result = pn532_read_response(0x40, readbuf, sizeof(readbuf));
+
+    if (readbuf[0] != 0 || result != 17) {
+        printf("PN532 Mifare READ failed %d %02x\n", result, readbuf[0]);
+        return false;
+    }
+
+    memmove(block_data, readbuf + 1, 16);
+
+    return true;
+}
+
 int pn532_felica_command(uint8_t cmd, const uint8_t *param, uint8_t param_len, uint8_t *outbuf)
 {
     int cmd_len = param_len + 11;
@@ -357,7 +398,7 @@ int pn532_felica_command(uint8_t cmd, const uint8_t *param, uint8_t param_len, u
     int result = pn532_read_response(0x40, readbuf, sizeof(readbuf));
 
     int outlen = readbuf[1] - 1;
-    if (readbuf[0] & 0x3f != 0 || result - 2 != outlen) {
+    if ((readbuf[0] & 0x3f) != 0 || result - 2 != outlen) {
         return -1;
     }
 
@@ -375,7 +416,7 @@ bool pn532_felica_read_wo_encrypt(uint16_t svc_code, uint16_t block_id, uint8_t
     int result = pn532_felica_command(0x06, param, sizeof(param), readbuf);
 
     if (result != 12 + 16 || readbuf[9] != 0 || readbuf[10] != 0) {
-        printf("PN532 Felica READ read response failed %d %02x %02x\n",
+        printf("PN532 Felica READ read failed %d %02x %02x\n",
                result, readbuf[9], readbuf[10]);
         for (int i = 0; i < result; i++) {
             printf(" %02x", readbuf[i]);
@@ -397,8 +438,8 @@ bool pn532_felica_write_wo_encrypt(uint16_t svc_code, uint16_t block_id, const u
 
     int result = pn532_felica_command(0x08, param, sizeof(param), readbuf);
 
-    printf("PN532 Felica Write response %d\n", result);
     if (result < 0) {
+        printf("PN532 Felica WRITE failed %d\n", result);
         return false;
     }
 

firmware/src/pn532.h

@@ -18,6 +18,11 @@ bool pn532_set_rf_field(uint8_t auto_rf, uint8_t on_off);
 
 bool pn532_poll_mifare(uint8_t *uid, int *len);
 bool pn532_poll_felica(uint8_t uid[8], uint8_t pmm[8], uint8_t syscode[2]);
+
+bool pn532_mifare_auth(const uint8_t uid[4], uint8_t block_id, uint8_t key_id, const uint8_t *key);
+bool pn532_mifare_read(uint8_t block_id, uint8_t block_data[16]);
+
 bool pn532_felica_read_wo_encrypt(uint16_t svc_code, uint16_t block_id, uint8_t block_data[16]);
+bool pn532_felica_write_wo_encrypt(uint16_t svc_code, uint16_t block_id, const uint8_t block_data[16]);
 
 #endif

firmware/src/vl53l0x.c

@@ -331,6 +331,59 @@ float getSignalRateLimit()
     return (float)read_reg16(FINAL_RANGE_CONFIG_MIN_COUNT_RATE_RTN_LIMIT) / (1 << 7);
 }
 
+// Decode sequence step timeout in MCLKs from register value
+// based on VL53L0X_decode_timeout()
+// Note: the original function returned a uint32_t, but the return value is
+// always stored in a uint16_t.
+uint16_t decodeTimeout(uint16_t reg_val)
+{
+    // format: "(LSByte * 2^MSByte) + 1"
+    return (uint16_t)((reg_val & 0x00FF) <<
+           (uint16_t)((reg_val & 0xFF00) >> 8)) + 1;
+}
+
+// Encode sequence step timeout register value from timeout in MCLKs
+// based on VL53L0X_encode_timeout()
+uint16_t encodeTimeout(uint32_t timeout_mclks)
+{
+    // format: "(LSByte * 2^MSByte) + 1"
+
+    uint32_t ls_byte = 0;
+    uint16_t ms_byte = 0;
+
+    if (timeout_mclks > 0) {
+        ls_byte = timeout_mclks - 1;
+
+        while ((ls_byte & 0xFFFFFF00) > 0) {
+            ls_byte >>= 1;
+            ms_byte++;
+        }
+
+        return (ms_byte << 8) | (ls_byte & 0xFF);
+    }
+    else {
+        return 0;
+    }
+}
+
+// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
+// based on VL53L0X_calc_timeout_us()
+uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
+{
+    uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
+
+    return ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
+}
+
+// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
+// based on VL53L0X_calc_timeout_mclks()
+uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
+{
+    uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
+
+    return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
+}
+
 // Set the measurement timing budget in microseconds, which is the time allowed
 // for one measurement; the ST API and this library take care of splitting the
 // timing budget among the sub-steps in the ranging sequence. A longer timing
@@ -403,7 +456,7 @@ bool setMeasurementTimingBudget(uint32_t budget_us)
 
         uint32_t final_range_timeout_mclks =
             timeoutMicrosecondsToMclks(final_range_timeout_us,
-                                                                 timeouts.final_range_vcsel_period_pclks);
+                                       timeouts.final_range_vcsel_period_pclks);
 
         if (enables.pre_range)
         {
@@ -720,7 +773,7 @@ void vl53l0x_stop_continuous()
 // single-shot range measurement)
 uint16_t readRangeContinuousMillimeters()
 {
-    if (read_reg(RESULT_INTERRUPT_STATUS) & 0x07 == 0) {
+    if ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
         return 65535;
     }
 
@@ -854,66 +907,13 @@ void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTi
                                    timeouts->final_range_vcsel_period_pclks);
 }
 
-// Decode sequence step timeout in MCLKs from register value
-// based on VL53L0X_decode_timeout()
-// Note: the original function returned a uint32_t, but the return value is
-// always stored in a uint16_t.
-uint16_t decodeTimeout(uint16_t reg_val)
-{
-    // format: "(LSByte * 2^MSByte) + 1"
-    return (uint16_t)((reg_val & 0x00FF) <<
-           (uint16_t)((reg_val & 0xFF00) >> 8)) + 1;
-}
-
-// Encode sequence step timeout register value from timeout in MCLKs
-// based on VL53L0X_encode_timeout()
-uint16_t encodeTimeout(uint32_t timeout_mclks)
-{
-    // format: "(LSByte * 2^MSByte) + 1"
-
-    uint32_t ls_byte = 0;
-    uint16_t ms_byte = 0;
-
-    if (timeout_mclks > 0) {
-        ls_byte = timeout_mclks - 1;
-
-        while ((ls_byte & 0xFFFFFF00) > 0) {
-            ls_byte >>= 1;
-            ms_byte++;
-        }
-
-        return (ms_byte << 8) | (ls_byte & 0xFF);
-    }
-    else {
-        return 0;
-    }
-}
-
-// Convert sequence step timeout from MCLKs to microseconds with given VCSEL period in PCLKs
-// based on VL53L0X_calc_timeout_us()
-uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks)
-{
-    uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
-
-    return ((timeout_period_mclks * macro_period_ns) + 500) / 1000;
-}
-
-// Convert sequence step timeout from microseconds to MCLKs with given VCSEL period in PCLKs
-// based on VL53L0X_calc_timeout_mclks()
-uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks)
-{
-    uint32_t macro_period_ns = calcMacroPeriod(vcsel_period_pclks);
-
-    return (((timeout_period_us * 1000) + (macro_period_ns / 2)) / macro_period_ns);
-}
-
 // based on VL53L0X_perform_single_ref_calibration()
 bool performSingleRefCalibration(uint8_t vhv_init_byte)
 {
     write_reg(SYSRANGE_START, 0x01 | vhv_init_byte); // VL53L0X_REG_SYSRANGE_MODE_START_STOP
 
     uint32_t start = time_us_32();
-    while (read_reg(RESULT_INTERRUPT_STATUS) & 0x07 == 0) {
+    while ((read_reg(RESULT_INTERRUPT_STATUS) & 0x07) == 0) {
         if (time_us_32() - start > IO_TIMEOUT_US) {
             return false;
         }

firmware/src/vl53l0x.h

@@ -154,9 +154,4 @@ void getSequenceStepTimeouts(SequenceStepEnables const * enables, SequenceStepTi
 
 bool performSingleRefCalibration(uint8_t vhv_init_byte);
 
-static uint16_t decodeTimeout(uint16_t value);
-static uint16_t encodeTimeout(uint32_t timeout_mclks);
-static uint32_t timeoutMclksToMicroseconds(uint16_t timeout_period_mclks, uint8_t vcsel_period_pclks);
-static uint32_t timeoutMicrosecondsToMclks(uint32_t timeout_period_us, uint8_t vcsel_period_pclks);
-
 #endif