134 lines
3.9 KiB
C++
134 lines
3.9 KiB
C++
#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;
|
|
this->buffer[slotNum].length = MAX(this->buffer[slotNum].length, header.segment*7 + (length - 1));
|
|
volatile FrameSubmitionResult res = getPackageStatus(&(this->buffer[slotNum]));
|
|
|
|
// Push the update to the control interface
|
|
ctrlInterface->pushPackageSlotUpdate(res == COMPLEATE, this->buffer[slotNum].is_in_use, slotNum, addr, timestamp, this->buffer[slotNum].used_bit_masked);
|
|
|
|
y 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;
|
|
dest->length = buffer[index].length;
|
|
|
|
this->ctrlInterface->pushPacket(*dest);
|
|
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;
|
|
this->buffer[index].length = 0;
|
|
} |