Build RPC Parsers

i2c-hub/firmware/i2c-hub-firmware/src/gobotrpc/rx.cpp

@@ -0,0 +1,127 @@
+#include "gobotrpc.hpp"
+#include "protocol.hpp"
+#include <string.h>
+
+RPCHeader parseRPCHeader(char data)
+{
+    RPCHeader header;
+    header.rpcNum = (RPCNumber)(data & 0b00001111);
+    header.type = (RPCType)((data & 0b00110000) >> 4);
+    header.segment = (data & 0b11000000) >> 6;
+
+    return header;
+}
+
+FrameSubmitionResult GobotRPCParser::submit_frame(char *data, size_t length, uint32_t addr, uint32_t timestamp)
+{
+    RPCHeader header = parseRPCHeader(data[0]);
+
+    if (header.type == INVALID) {
+        return ERROR_INVALID_RPC_TYPE;
+    }
+
+    if (header.rpcNum > RPC_NUMBER_MAX) {
+        return ERROR_INVALID_RPC_NUMBER;
+    }
+
+    int fitting_slot;
+    int empty_slot;
+    find_fitting_slot(header, addr, &fitting_slot, &empty_slot);
+
+    int slotNum;
+
+    // Check if packageSlot is complete
+
+    if (fitting_slot != -1) {
+        slotNum = fitting_slot;
+    }
+    else if (empty_slot != -1) {
+        slotNum = empty_slot;
+
+        this->buffer[slotNum].is_in_use = true;
+        this->buffer[slotNum].is_complete = false;
+        this->buffer[slotNum].sender_address = addr;
+        this->buffer[slotNum].rpcNum = header.rpcNum;
+        this->buffer[slotNum].type = header.type;
+    }
+    else {
+        return ERROR_NO_EMPTY_OR_FITTING_SLOT;
+    }
+
+    insertFrameInPackageSlot(&this->buffer[slotNum], data, header.segment, length);
+    this->buffer[slotNum].timestamp = timestamp;
+    volatile FrameSubmitionResult res = getPackageStatus(&(this->buffer[slotNum]));
+
+    if (res == COMPLEATE) {
+        this->buffer[slotNum].is_complete = true;
+    }
+
+    return res;
+}
+
+void GobotRPCParser::insertFrameInPackageSlot(RPC_RX_PackageSlot *packageSlot, char *data, size_t segment, size_t length)
+{
+    size_t offset = segment * 7;
+
+    for (int i = 0; i < length - 1; i++) {
+        packageSlot->buffer[offset + i] = data[i + 1];
+        packageSlot->used_bit_masked |= 1 << (offset + i);
+    }
+}
+
+void GobotRPCParser::find_fitting_slot(RPCHeader header, uint32_t addr, int *fitting_slot, int *empty_slot)
+{
+    uint32_t masked_addr = addr & this->address_mask;
+
+    *fitting_slot = -1;
+    *empty_slot = -1;
+
+    for (int i = 0; i < NUM_SLOTS; i++) {
+        if (this->buffer[i].is_complete) {
+            continue;
+        }
+
+        uint32_t masked_slot_addr = this->buffer[i].sender_address & this->address_mask;
+
+        if (masked_slot_addr == masked_addr && header.rpcNum == this->buffer[i].rpcNum) {
+            *fitting_slot = i;
+            return;
+        }
+
+        if (*empty_slot == -1 && !buffer[i].is_in_use) {
+            *empty_slot = i;
+        }
+    }
+}
+
+int GobotRPCParser::getFinishedIndexPackages() {
+    
+    for (int i = 0; i < NUM_SLOTS; i++) {
+        size_t index = (i + this->finishedScanIndex) % NUM_SLOTS;
+        if (this->buffer[i].is_complete) {
+            this->finishedScanIndex = (index + 1) % NUM_SLOTS;
+            return index;
+        }
+    }
+
+    return -1;
+}
+
+void GobotRPCParser::retrivePackage(RPCPackage *dest, int index) {
+    memcpy(dest->buffer, buffer[index].buffer, MAX_PAGE_SIZES);
+    dest->addr = buffer[index].sender_address;
+    dest->rpcNum = buffer[index].rpcNum;
+    dest->type = buffer[index].type;
+
+    freePackageSlot(index);
+}
+
+void GobotRPCParser::freePackageSlot(size_t index) {
+    this->buffer[index].timestamp = 0;
+    this->buffer[index].is_complete = false;
+    this->buffer[index].is_in_use = false;
+    this->buffer[index].used_bit_masked = 0;
+    this->buffer[index].sender_address = 0;
+    this->buffer[index].rpcNum = Invalid;
+    this->buffer[index].type = INVALID;
+}