4 files changed, 49 insertions, 16 deletions

diff --git a/quantum/api/api_sysex.c b/quantum/api/api_sysex.c
index a4a554e764..868f854b92 100644
--- a/quantum/api/api_sysex.c
+++ b/quantum/api/api_sysex.c
@@ -1,4 +1,6 @@
 #include "api_sysex.h"
+#include "sysex_tools.h"
+#include "print.h"
 
 void send_bytes_sysex(uint8_t message_type, uint8_t data_type, uint8_t * bytes, uint16_t length) {
     // SEND_STRING("\nTX: ");
@@ -6,24 +8,50 @@ void send_bytes_sysex(uint8_t message_type, uint8_t data_type, uint8_t * bytes,
     //     send_byte(bytes[i]);
     //     SEND_STRING(" ");
     // }
-    uint8_t * precode = malloc(sizeof(uint8_t) * (length + 2));
-    precode[0] = message_type;
-    precode[1] = data_type;
-    memcpy(precode + 2, bytes, length);
-    uint8_t * encoded = malloc(sizeof(uint8_t) * (sysex_encoded_length(length + 2)));
-    uint16_t encoded_length = sysex_encode(encoded, precode, length + 2);
-    uint8_t * array = malloc(sizeof(uint8_t) * (encoded_length + 5));
-    array[0] = 0xF0;
-    array[1] = 0x00;
-    array[2] = 0x00;
-    array[3] = 0x00;
-    array[encoded_length + 4] = 0xF7;
-    memcpy(array + 4, encoded, encoded_length);
-    midi_send_array(&midi_device, encoded_length + 5, array);
+    if (length > API_SYSEX_MAX_SIZE) {
+        xprintf("Sysex msg too big %d %d %d", message_type, data_type, length);
+        return;
+    }
+
+
+    // The buffer size required is calculated as the following
+    // API_SYSEX_MAX_SIZE is the maximum length
+    // In addition to that we have a two byte message header consisting of the message_type and data_type
+    // This has to be encoded with an additional overhead of one byte for every starting 7 bytes
+    // We just add one extra byte in case it's not divisible by 7
+    // Then we have an unencoded header consisting of 4 bytes
+    // Plus a one byte terminator
+    const unsigned message_header = 2;
+    const unsigned unencoded_message = API_SYSEX_MAX_SIZE + message_header;
+    const unsigned encoding_overhead = unencoded_message / 7 + 1;
+    const unsigned encoded_size = unencoded_message + encoding_overhead;
+    const unsigned unencoded_header = 4;
+    const unsigned terminator = 1;
+    const unsigned buffer_size = encoded_size + unencoded_header + terminator;
+    uint8_t buffer[encoded_size + unencoded_header + terminator];
+    // The unencoded header
+    buffer[0] = 0xF0;
+    buffer[1] = 0x00;
+    buffer[2] = 0x00;
+    buffer[3] = 0x00;
+
+    // We copy the message to the end of the array, this way we can do an inplace encoding, using the same
+    // buffer for both input and output
+    const unsigned message_size = length + message_header;
+    uint8_t* unencoded_start = buffer + buffer_size - message_size;
+    uint8_t* ptr = unencoded_start;
+    *(ptr++) = message_type;
+    *(ptr++) = data_type;
+    memcpy(ptr, bytes, length);
+
+    unsigned encoded_length = sysex_encode(buffer + unencoded_header, unencoded_start, message_size);
+    unsigned final_size = unencoded_header + encoded_length + terminator;
+    buffer[final_size - 1] = 0xF7;
+    midi_send_array(&midi_device, final_size, buffer);
 
     // SEND_STRING("\nTD: ");
     // for (uint8_t i = 0; i < encoded_length + 5; i++) {
-    //     send_byte(array[i]);
+    //     send_byte(buffer[i]);
     //     SEND_STRING(" ");
     // }
-}
\ No newline at end of file
+}
diff --git a/quantum/config_common.h b/quantum/config_common.h
index 17c11faeb6..4bdb2065d9 100644
--- a/quantum/config_common.h
+++ b/quantum/config_common.h
@@ -80,4 +80,6 @@
 #   endif
 #endif
 
+#define API_SYSEX_MAX_SIZE 32
+
 #endif
diff --git a/quantum/rgblight.c b/quantum/rgblight.c
index 6e335a4831..52a09817a1 100644
--- a/quantum/rgblight.c
+++ b/quantum/rgblight.c
@@ -370,6 +370,7 @@ void rgblight_setrgb(uint8_t r, uint8_t g, uint8_t b) {
   rgblight_set();
 }
 
+__attribute__ ((weak))
 void rgblight_set(void) {
   if (rgblight_config.enable) {
     #ifdef RGBW
diff --git a/quantum/rgblight.h b/quantum/rgblight.h
index ceb624692a..726b8de72e 100644
--- a/quantum/rgblight.h
+++ b/quantum/rgblight.h
@@ -40,6 +40,8 @@
 #include "eeconfig.h"
 #include "light_ws2812.h"
 
+extern LED_TYPE led[RGBLED_NUM];
+
 extern const uint8_t RGBLED_BREATHING_INTERVALS[4] PROGMEM;
 extern const uint8_t RGBLED_RAINBOW_MOOD_INTERVALS[3] PROGMEM;
 extern const uint8_t RGBLED_RAINBOW_SWIRL_INTERVALS[3] PROGMEM;