Did stuff

common-libaries/gobot_rpc/gobot_rpc_statemaschine.cpp

@@ -0,0 +1,74 @@
+#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;
+        }
+    }
+
+}