1 files changed, 13 insertions, 64 deletions

diff --git a/keyboards/system76/launch_1/usb_mux.c b/keyboards/system76/launch_1/usb_mux.c
index 1c6dd2376e..dd51b7c4b2 100644
--- a/keyboards/system76/launch_1/usb_mux.c
+++ b/keyboards/system76/launch_1/usb_mux.c
@@ -77,51 +77,14 @@ i2c_status_t usb7206_read_reg(struct USB7206* self, uint32_t addr, uint8_t* data
         return status;
     }
 
-    uint8_t read[2] = {
-        0x00,  // Buffer address MSB: always 0
-        0x06,  // Buffer address LSB: 6 to skip header
-    };
-
-    status = i2c_start((self->addr << 1) | I2C_WRITE, I2C_TIMEOUT);
-    if (status >= 0) {
-        for (uint16_t i = 0; i < sizeof(read); i++) {
-            status = i2c_write(read[i], I2C_TIMEOUT);
-            if (status < 0) {
-                goto error;
-            }
-        }
-    } else {
-        goto error;
-    }
-
-    status = i2c_start((self->addr << 1) | I2C_READ, I2C_TIMEOUT);
-    if (status < 0) {
-        goto error;
-    }
-
-    // Read and ignore buffer length
-    status = i2c_read_ack(I2C_TIMEOUT);
-    if (status < 0) {
-        goto error;
-    }
+    uint16_t read = 0x0006; // Buffer address 6 to skip header
+    uint8_t data_with_buffer_length[length];
+    status = i2c_readReg16((self->addr << 1), read, data_with_buffer_length, length, I2C_TIMEOUT);
 
     for (uint16_t i = 0; i < (length - 1) && status >= 0; i++) {
-        status = i2c_read_ack(I2C_TIMEOUT);
-        if (status >= 0) {
-            data[i] = (uint8_t)status;
-        }
+        data[i] = data_with_buffer_length[i+1];
     }
 
-    if (status >= 0) {
-        status = i2c_read_nack(I2C_TIMEOUT);
-        if (status >= 0) {
-            data[(length - 1)] = (uint8_t)status;
-        }
-    }
-
-error:
-    i2c_stop();
-
     return (status < 0) ? status : length;
 }
 
@@ -160,35 +123,21 @@ i2c_status_t usb7206_write_reg(struct USB7206* self, uint32_t addr, uint8_t* dat
         (uint8_t)(addr >> 8),   // Register address byte 1
         (uint8_t)(addr >> 0),   // Register address byte 0
     };
+    uint8_t send_buffer_length = sizeof(register_write) + length;
+    uint8_t send_buffer[send_buffer_length];
+    uint8_t j = 0;
 
-    status = i2c_start((self->addr << 1) | I2C_WRITE, I2C_TIMEOUT);
-    if (status >= 0) {
-        for (uint16_t i = 0; i < sizeof(register_write); i++) {
-            status = i2c_write(register_write[i], I2C_TIMEOUT);
-            if (status < 0) {
-                goto error;
-            }
-        }
+    for (uint16_t i = 0; i < sizeof(register_write); i++) {
+        send_buffer[j++] = register_write[i];
+    }
 
-        for (uint16_t i = 0; i < length; i++) {
-            status = i2c_write(data[i], I2C_TIMEOUT);
-            if (status < 0) {
-                goto error;
-            }
-        }
-    } else {
-        goto error;
+    for (uint16_t i = 0; i < length; i++) {
+        send_buffer[j++] = data[i];
     }
 
-    i2c_stop();
+    status = i2c_transmit((self->addr << 1), send_buffer, send_buffer_length, I2C_TIMEOUT);
 
     status = usb7206_register_access(self);
-    if (status < 0) {
-        goto error;
-    }
-
-error:
-    i2c_stop();
 
     return (status < 0) ? status : length;
 }