void usb_rx_helper(const uint8_t *buf, size_t length, MessageMapType type)
{
    static bool firstFrame = true;

    static uint16_t msgId;
    static uint32_t msgSize;
    static uint8_t msg[MAX_FRAME_SIZE];
    static size_t cursor; //< Index into msg where the current frame is to be written.
    static const MessagesMap_t *entry;

    if (firstFrame) {
        msgId = 0xffff;
        msgSize = 0;
        memset(msg, 0, sizeof(msg));
        cursor = 0;
        entry = NULL;
    }

    assert(buf != NULL);

    if (length < 1 + 2 + 2 + 4) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Buffer too small");
        goto reset;
    }

    if (buf[0] != '?') {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Malformed packet");
        goto reset;
    }

    if (firstFrame && (buf[1] != '#' || buf[2] != '#')) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Malformed packet");
        goto reset;
    }

    // Details of the chunk being copied out of the current frame.
    const uint8_t *frame;
    size_t frameSize;

    if (firstFrame) {
        // Reset the buffer that we're writing fragments into.
        memset(msg, 0, sizeof(msg));

        // Then fish out the id / size, which are big-endian uint16 /
        // uint32's respectively.
        msgId = buf[4] | ((uint16_t)buf[3]) << 8;
        msgSize =    buf[8]        |
          ((uint32_t)buf[7]) <<  8 |
          ((uint32_t)buf[6]) << 16 |
          ((uint32_t)buf[5]) << 24;

        // Determine callback handler and message map type.
        entry = message_map_entry(type, msgId, IN_MSG);

        // And reset the cursor.
        cursor = 0;

        // Then take note of the fragment boundaries.
        frame = &buf[9];
        frameSize = MIN(length - 9, msgSize);
    } else {
        // Otherwise it's a continuation/fragment.
        frame = &buf[1];
        frameSize = length - 1;
    }

    // If the msgId wasn't in our map, bail.
    if (!entry) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Unknown message");
        goto reset;
    }

    if (entry->dispatch == RAW) {
        /* Call dispatch for every segment since we are not buffering and parsing, and
         * assume the raw dispatched callbacks will handle their own state and
         * buffering internally
         */
        raw_dispatch(entry, frame, frameSize, msgSize);
        firstFrame = false;
        return;
    }

    size_t end;
    if (check_uadd_overflow(cursor, frameSize, &end) || sizeof(msg) < end) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Malformed message");
        goto reset;
    }

    // Copy content to frame buffer.
    memcpy(&msg[cursor], frame, frameSize);

    // Advance the cursor.
    cursor = end;

    // Only parse and message map if all segments have been buffered.
    bool last_segment = cursor >= msgSize;
    if (!last_segment) {
        firstFrame = false;
        return;
    }

    dispatch(entry, msg, msgSize);

reset:
    msgId = 0xffff;
    msgSize = 0;
    memset(msg, 0, sizeof(msg));
    cursor = 0;
    firstFrame = true;
    entry = NULL;
}
void usb_rx_helper(const uint8_t *buf, size_t length, MessageMapType type)
{
    static bool firstFrame = true;

    static uint16_t msgId;
    static uint32_t msgSize;
    static uint8_t msg[MAX_FRAME_SIZE];
    static size_t cursor;  
    static const MessagesMap_t *entry;

    if (firstFrame) {
        msgId = 0xffff;
        msgSize = 0;
        memset(msg, 0, sizeof(msg));
        cursor = 0;
        entry = NULL;
    }

    assert(buf != NULL);

    if (length < 1 + 2 + 2 + 4) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Buffer too small");
        goto reset;
    }

    if (buf[0] != '?') {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Malformed packet");
        goto reset;
    }

    if (firstFrame && (buf[1] != '#' || buf[2] != '#')) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Malformed packet");
        goto reset;
    }

     
    const uint8_t *frame;
    size_t frameSize;

    if (firstFrame) {
         
        memset(msg, 0, sizeof(msg));

         
         
        msgId = buf[4] | ((uint16_t)buf[3]) << 8;
        msgSize =    buf[8]        |
          ((uint32_t)buf[7]) <<  8 |
          ((uint32_t)buf[6]) << 16 |
          ((uint32_t)buf[5]) << 24;

         
        entry = message_map_entry(type, msgId, IN_MSG);

         
        cursor = 0;

         
        frame = &buf[9];
        frameSize = MIN(length - 9, msgSize);
    } else {
         
        frame = &buf[1];
        frameSize = length - 1;
    }

     
    if (!entry) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Unknown message");
        goto reset;
    }

    if (entry->dispatch == RAW) {
         
        raw_dispatch(entry, frame, frameSize, msgSize);
        firstFrame = false;
        return;
    }

    size_t end;
    if (check_uadd_overflow(cursor, frameSize, &end) || sizeof(msg) < end) {
        (*msg_failure)(FailureType_Failure_UnexpectedMessage, "Malformed message");
        goto reset;
    }

     
    memcpy(&msg[cursor], frame, frameSize);

     
    cursor = end;

     
    bool last_segment = cursor >= msgSize;
    if (!last_segment) {
        firstFrame = false;
        return;
    }

    dispatch(entry, msg, msgSize);

reset:
    msgId = 0xffff;
    msgSize = 0;
    memset(msg, 0, sizeof(msg));
    cursor = 0;
    firstFrame = true;
    entry = NULL;
}