74 lines
2.3 KiB
C++
74 lines
2.3 KiB
C++
#include "sm.hpp"
|
|
#include "protocol_spec.hpp"
|
|
#include <string.h>
|
|
|
|
GobotRPCStateMashine::GobotRPCStateMashine() {
|
|
memset(requestDataBuffer, 0, REQUEST_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
|
|
memset(responseDataBuffer, 0, RESPONSE_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
|
|
memset(errorDataBuffer, 0, RESPONSE_DATA_BUFFER_SIZE*sizeof(uint8_t)*SLOTS);
|
|
|
|
memset(requestSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
|
|
memset(responseSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
|
|
memset(errorSegmentArrivedFlags, 0, sizeof(uint8_t)*SLOTS);
|
|
|
|
slotCounter = 0;
|
|
rpc_number_map = GobotRPCNumberMap();
|
|
}
|
|
|
|
void GobotRPCStateMashine::registerRPC(RpcNum rpc_num) {
|
|
rpc_number_map.set(rpc_num, slotCounter);
|
|
slotCounter = (slotCounter + 1) % SLOTS;
|
|
}
|
|
|
|
void GobotRPCStateMashine::submitFrame(RPCFrame * frame, SMResult * result) {
|
|
RpcNum rpcNum = (RpcNum)(frame->header.rpc_num);
|
|
|
|
result->done = false;
|
|
result->rpc_num = rpcNum;
|
|
|
|
if(rpcNum == RpcNum::INVALID)
|
|
return;
|
|
|
|
size_t slot = rpc_number_map.get(rpcNum);
|
|
if (slot == 0xffffffff)
|
|
return;
|
|
|
|
uint8_t segNum = frame->header.rpc_segement;
|
|
|
|
if(frame->header.error) {
|
|
memcpy(result->data, frame->data, 7);
|
|
errorSegmentArrivedFlags[slot] |= (1 << segNum);
|
|
|
|
result->done = true;
|
|
result->type = RPCPackageType::ERROR;
|
|
|
|
} else if(frame->header.response) {
|
|
memcpy(responseDataBuffer[slot] + segNum, frame->data, 7);
|
|
responseSegmentArrivedFlags[slot] |= (1 << segNum);
|
|
|
|
uint8_t mask = SEGMENT_MASK_RESPONSE[(int)rpcNum];
|
|
|
|
if(((responseSegmentArrivedFlags[slot] & mask) ^ mask) == 0) {
|
|
responseSegmentArrivedFlags[slot] = 0;
|
|
|
|
result->done = true;
|
|
memcpy(result->data, responseDataBuffer[slot], 7*4);
|
|
result->type = RPCPackageType::RESPONSE;
|
|
}
|
|
|
|
} else {
|
|
memcpy(requestDataBuffer[slot] + segNum, frame->data, 7);
|
|
requestSegmentArrivedFlags[slot] |= (1 << segNum);
|
|
|
|
uint8_t mask = SEGMENT_MASK_RESPONSE[(int)rpcNum];
|
|
|
|
if(((requestSegmentArrivedFlags[slot] & mask) ^ mask) == 0) {
|
|
requestSegmentArrivedFlags[slot] = 0;
|
|
|
|
result->done = true;
|
|
memcpy(result->data, requestDataBuffer[slot], 7*4);
|
|
result->type = RPCPackageType::REQUEST;
|
|
}
|
|
}
|
|
|
|
} |