DO NOT MERGE [AUR-101] Enforce custom APID values

src/Broker/Broker.c

@@ -54,7 +54,7 @@ Broker_deliver_request(const enum RemoteInterface interface, const uint8_t* cons
 #elif defined BROKER_SEND_TELEMETRY
     const Packetizer_PacketType packet_type = Packetizer_PacketType_Telemetry;
 #else
-    const Packetizer_PacketType packet_type = Packetizer_PacketType_Telemetry;
+    const Packetizer_PacketType packet_type = Packetizer_PacketType_Telecommand;
 #endif
 
     const size_t packet_size = Packetizer_packetize(&packetizer_data,
@@ -63,7 +63,7 @@ Broker_deliver_request(const enum RemoteInterface interface, const uint8_t* cons
                                                     (uint16_t)interface,
                                                     packetizer_buffer,
                                                     SPACE_PACKET_PRIMARY_HEADER_SIZE,
-                                                    length);
+                                                    length + SPACE_PACKET_ERROR_CONTROL_SIZE);
 
     const enum SystemBus bus_id = port_to_bus_map[interface];
     void* driver_private_data = bus_to_driver_private_data[bus_id];
@@ -85,7 +85,7 @@ Broker_receive_packet(uint8_t* const data, const size_t length)
 
 #if defined BROKER_EXPECT_TELECOMMAND
     const Packetizer_PacketType packet_type = Packetizer_PacketType_Telecommand;
-#elif defined BROKER_EXPECT_TELEMETRT
+#elif defined BROKER_EXPECT_TELEMETRY
     const Packetizer_PacketType packet_type = Packetizer_PacketType_Telemetry;
 #else
     const Packetizer_PacketType packet_type = Packetizer_PacketType_Telemetry;

src/Escaper/Escaper.c

@@ -34,7 +34,6 @@ Escaper_init(Escaper* const self,
 {
     self->m_parse_state = Escaper_State_Wait;
     self->m_encode_started = false;
-    self->m_escape = false;
     self->m_encode_finished = false;
     self->m_encoded_packet_buffer = m_encoded_packet_buffer;
     self->m_encoded_packet_max_size = m_encoded_packet_buffer_size;
@@ -90,7 +89,15 @@ Escaper_decode_packet(Escaper* const self, uint8_t* buffer, const size_t length,
                     self->m_decoded_packet_buffer_index = 0;
                     self->m_parse_state = Escaper_State_Wait;
                 } else {
-                    self->m_decoded_packet_buffer[self->m_decoded_packet_buffer_index] = buffer[i];
+                    if(buffer[i] == ESC_START_BYTE) {
+                        self->m_decoded_packet_buffer[self->m_decoded_packet_buffer_index] = START_BYTE;
+                    } else if(buffer[i] == ESC_ESCAPE_BYTE) {
+                        self->m_decoded_packet_buffer[self->m_decoded_packet_buffer_index] = ESCAPE_BYTE;
+                    } else if(buffer[i] == ESC_STOP_BYTE) {
+                        self->m_decoded_packet_buffer[self->m_decoded_packet_buffer_index] = STOP_BYTE;
+                    } else {
+                        self->m_decoded_packet_buffer[self->m_decoded_packet_buffer_index] = buffer[i];
+                    }
                     ++self->m_decoded_packet_buffer_index;
                     self->m_parse_state = Escaper_State_Data_Byte;
                 }
@@ -106,7 +113,6 @@ void
 Escaper_start_encoder(Escaper* const self)
 {
     self->m_encode_finished = false;
-    self->m_escape = false;
     self->m_encode_started = false;
 }
 
@@ -122,23 +128,24 @@ Escaper_encode_packet(Escaper* const self, const uint8_t* const data, const size
     }
 
     while(*index < length) {
-        if(self->m_escape) {
-            self->m_encoded_packet_buffer[encoded_packet_buffer_index] = data[*index];
-            ++encoded_packet_buffer_index;
-            ++*index;
-            self->m_escape = false;
-        } else if(data[*index] == START_BYTE) {
+        if(data[*index] == START_BYTE) {
             self->m_encoded_packet_buffer[encoded_packet_buffer_index] = ESCAPE_BYTE;
             ++encoded_packet_buffer_index;
-            self->m_escape = true;
+            self->m_encoded_packet_buffer[encoded_packet_buffer_index] = ESC_START_BYTE;
+            ++encoded_packet_buffer_index;
+            ++*index;
         } else if(data[*index] == ESCAPE_BYTE) {
             self->m_encoded_packet_buffer[encoded_packet_buffer_index] = ESCAPE_BYTE;
             ++encoded_packet_buffer_index;
-            self->m_escape = true;
+            self->m_encoded_packet_buffer[encoded_packet_buffer_index] = ESC_ESCAPE_BYTE;
+            ++encoded_packet_buffer_index;
+            ++*index;
         } else if(data[*index] == STOP_BYTE) {
             self->m_encoded_packet_buffer[encoded_packet_buffer_index] = ESCAPE_BYTE;
             ++encoded_packet_buffer_index;
-            self->m_escape = true;
+            self->m_encoded_packet_buffer[encoded_packet_buffer_index] = ESC_STOP_BYTE;
+            ++encoded_packet_buffer_index;
+            ++*index;
         } else {
             self->m_encoded_packet_buffer[encoded_packet_buffer_index] = data[*index];
             ++encoded_packet_buffer_index;

src/Escaper/Escaper.h

@@ -46,7 +46,6 @@ typedef struct
 {
     Escaper_PacketParseState m_parse_state;
     bool m_encode_started;
-    bool m_escape;
     bool m_encode_finished;
     uint8_t* m_encoded_packet_buffer;
     size_t m_encoded_packet_max_size;

src/Escaper/EscaperInternal.h

@@ -23,8 +23,12 @@
 #ifndef ESCAPER_INTERNAL_H
 #define ESCAPER_INTERNAL_H
 
-#define START_BYTE (uint8_t)0x00
-#define STOP_BYTE (uint8_t)0xFF
-#define ESCAPE_BYTE (uint8_t)0xFE
+#define START_BYTE (uint8_t)0xB0
+#define STOP_BYTE (uint8_t)0xC0
+#define ESCAPE_BYTE (uint8_t)0xDA
+
+#define ESC_START_BYTE (uint8_t)0xDB
+#define ESC_STOP_BYTE (uint8_t)0xDC
+#define ESC_ESCAPE_BYTE (uint8_t)0xDD
 
 #endif

src/Packetizer/CMakeLists.txt

@@ -3,8 +3,10 @@ project(Packetizer VERSION 1.0.0 LANGUAGES C)
 add_library(Packetizer STATIC)
 target_sources(Packetizer
     PRIVATE     Crc16.c
+                IsoChecksum.c
                 Packetizer.c
                 Crc16.h
+                IsoChecksum.h
                 PacketizerInternal.h
                 SpacePacketInternal.h
     PUBLIC      Packetizer.h

src/Packetizer/IsoChecksum.c

@@ -0,0 +1,62 @@
+/**@file
+ * This file is part of the TASTE Runtime Common.
+ *
+ * @copyright 2021 N7 Space Sp. z o.o.
+ *
+ * TASTE Runtime Common was developed under a programme of,
+ * and funded by, the European Space Agency (the "ESA").
+ *
+ * Licensed under the ESA Public License (ESA-PL) Permissive,
+ * Version 2.3 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://essr.esa.int/license/list
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "IsoChecksum.h"
+
+#define MAKE_UINT16(hi, lo) (uint16_t)(((hi) << 8u) | (lo))
+#define REDUCE(x) (((x)&0xFFu) + ((x) >> 8u))
+
+static uint16_t
+fletcher16(const uint8_t* const data, const size_t bytes)
+{
+    uint32_t sum1 = 0xffu;
+    uint32_t sum2 = 0xffu;
+    size_t bytesToProcess = bytes;
+    const uint8_t* it = data;
+
+    while(bytesToProcess > 0u) {
+        size_t tlen = (bytesToProcess > 20u) ? 20u : bytesToProcess;
+        bytesToProcess -= tlen;
+        do {
+            sum1 += *it++;
+            sum2 += sum1;
+        } while(--tlen > 0);
+        sum1 = REDUCE(sum1);
+        sum2 = REDUCE(sum2);
+    }
+
+    return MAKE_UINT16(REDUCE(sum2), REDUCE(sum1));
+}
+
+uint16_t
+IsoChecksum_calculate(const uint8_t* const data, const size_t length)
+{
+    const uint16_t csum = fletcher16(data, length);
+
+    const uint32_t f0 = csum & 0xFFu;
+    const uint32_t f1 = (csum >> 8u) & 0xFFu;
+
+    const uint32_t c0 = 0xFFu - ((f0 + f1) % 0xFFu);
+    const uint32_t c1 = 0xFFu - ((f0 + c0) % 0xFFu);
+
+    return MAKE_UINT16(c0, c1);
+}

src/Packetizer/IsoChecksum.h

@@ -0,0 +1,40 @@
+/**@file
+ * This file is part of the TASTE Runtime Common.
+ *
+ * @copyright 2021 N7 Space Sp. z o.o.
+ *
+ * TASTE Runtime Common was developed under a programme of,
+ * and funded by, the European Space Agency (the "ESA").
+ *
+ * Licensed under the ESA Public License (ESA-PL) Permissive,
+ * Version 2.3 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     https://essr.esa.int/license/list
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef PACKETIZER_ISOCHECKSUM_H
+#define PACKETIZER_ISOCHECKSUM_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+/**
+ * @brief   Calculates ISO checksum for given data block.
+ *          Checksum is calculated according to as ECSS-E-70-41A Annex A.
+ *
+ * @param[in] data     Pointer to start of data block. Can't be NULL.
+ * @param[in] length   Data block length in bytes.
+ *
+ * @return  Calculated checksum.
+ */
+uint16_t IsoChecksum_calculate(const uint8_t* const data, const size_t length);
+
+#endif // PACKETIZER_ISOCHECKSUM_H

src/Packetizer/Packetizer.c

@@ -26,7 +26,11 @@
 #include <stdio.h>
 #include <string.h>
 
-#include "Crc16.h"
+#ifdef FORCE_APIDS
+#include <apids_translator.h>
+#endif
+
+#include "IsoChecksum.h"
 #include "PacketizerInternal.h"
 #include "SpacePacketInternal.h"
 
@@ -49,8 +53,13 @@ Packetizer_packetize(Packetizer* const self,
     (void)source;
     (void)dataOffset;
 
+    uint16_t rawDestination = destination;
+#ifdef FORCE_APIDS
+    rawDestination = translate_apid_to_send(rawDestination);
+#endif
+    assert(rawDestination <= SPACE_PACKET_MAX_APID);
+
     assert(self->packetSequenceCount <= SPACE_PACKET_MAX_PACKET_SEQUENCE_COUNT);
-    assert(destination <= SPACE_PACKET_MAX_APID);
     assert(packetPointer != NULL);
     assert(dataOffset == SPACE_PACKET_PRIMARY_HEADER_SIZE);
 #ifdef STANDARD_SPACE_PACKET
@@ -60,7 +69,7 @@ Packetizer_packetize(Packetizer* const self,
 
     memset(packetPointer, 0, SPACE_PACKET_PRIMARY_HEADER_SIZE);
 
-    writePacketId(packetPointer, packetType, destination);
+    writePacketId(packetPointer, packetType, rawDestination);
     writePacketSequenceControl(packetPointer, self);
     writePacketDataLength(packetPointer, dataSize);
     writeCrc(packetPointer, dataSize);
@@ -71,7 +80,7 @@ Packetizer_packetize(Packetizer* const self,
         self->packetSequenceCount = 0; // Counter should wrap to zero
     }
 
-    return SPACE_PACKET_PRIMARY_HEADER_SIZE + dataSize + SPACE_PACKET_ERROR_CONTROL_SIZE;
+    return SPACE_PACKET_PRIMARY_HEADER_SIZE + dataSize;
 }
 
 bool
@@ -108,21 +117,26 @@ Packetizer_depacketize(const Packetizer* const self,
 
     // Get and check data size
     const size_t receivedDataSize = readPacketDataLength(packetPointer);
-    if(packetSize != SPACE_PACKET_PRIMARY_HEADER_SIZE + receivedDataSize + SPACE_PACKET_ERROR_CONTROL_SIZE) {
+    // Do not add SPACE_PACKET_ERROR_CONTROL_SIZE here, becouse checksum is a part of payload
+    if(packetSize != SPACE_PACKET_PRIMARY_HEADER_SIZE + receivedDataSize) {
         if(errorCode != NULL) {
             *errorCode = Packetizer_ErrorCode_IncorrectPacketSize;
         }
         return false;
     }
 
     // Check if CRC matches
-    uint16_t receivedCrc = packetPointer[SPACE_PACKET_PRIMARY_HEADER_SIZE + receivedDataSize + 1]
-                           | (packetPointer[SPACE_PACKET_PRIMARY_HEADER_SIZE + receivedDataSize] << 8);
-    uint16_t expectedCrc = calculateCrc16(packetPointer, packetSize - SPACE_PACKET_ERROR_CONTROL_SIZE);
+    uint16_t receivedChecksum = packetPointer[SPACE_PACKET_PRIMARY_HEADER_SIZE + receivedDataSize - 1]
+                                | (packetPointer[SPACE_PACKET_PRIMARY_HEADER_SIZE + receivedDataSize - 2] << 8);
+#ifdef PACKETIZER_USE_CRC16_CHECKSUM
+    uint16_t expectedChecksum = calculateCrc16(packetPointer, packetSize - SPACE_PACKET_ERROR_CONTROL_SIZE);
+#else    
+    uint16_t expectedChecksum = IsoChecksum_calculate(packetPointer, packetSize - SPACE_PACKET_ERROR_CONTROL_SIZE);
+#endif
 
-    if(receivedCrc != expectedCrc) {
+    if(receivedChecksum != expectedChecksum) {
         if(errorCode != NULL) {
-            *errorCode = Packetizer_ErrorCode_IncorrectCrc16;
+            *errorCode = Packetizer_ErrorCode_IncorrectChecksum;
         }
         return false;
     }
@@ -138,8 +152,12 @@ Packetizer_depacketize(const Packetizer* const self,
     }
 
     // Save the results
-    *destination = packetPointer[1]
-                   | (packetPointer[0] & SPACE_PACKET_APID_HIGH_BITS_MASK) << (8u - SPACE_PACKET_APID_HIGH_BITS_OFFSET);
+    uint16_t rawDestination = packetPointer[1] | (packetPointer[0] & SPACE_PACKET_APID_HIGH_BITS_MASK) << 8u;
+#ifdef FORCE_APIDS
+    rawDestination = translate_received_apid(rawDestination);
+#endif
+    *destination = rawDestination;
+
     *dataOffset = SPACE_PACKET_PRIMARY_HEADER_SIZE;
     *dataSize = receivedDataSize;
 
@@ -156,8 +174,7 @@ writePacketId(uint8_t* const packetPointer, const Packetizer_PacketType packetTy
     packetPointer[0] |= 1 << SPACE_PACKET_SECONDARY_HEADER_FLAG_OFFSET;
 
     // 6th bit - Application process ID (11 bits) - BIG ENDIAN
-    packetPointer[0] |=
-            (destination & PACKETIZER_DESTINATION_HIGH_BITS_MASK) >> (8u - SPACE_PACKET_APID_HIGH_BITS_OFFSET);
+    packetPointer[0] |= (destination & PACKETIZER_DESTINATION_HIGH_BITS_MASK) >> 8u;
     packetPointer[1] |= destination & 0xFF;
 }
 
@@ -169,8 +186,7 @@ writePacketSequenceControl(uint8_t* const packetPointer, const Packetizer* const
     packetPointer[2] |= 1 << SPACE_PACKET_SEQUENCE_FLAGS_SECOND_OFFSET;
 
     // 3rd bit - Packet sequence count (14 bits)
-    packetPointer[2] |= (packetizer->packetSequenceCount & PACKETIZER_PACKET_SEQUENCE_CONTROL_HIGH_BITS_MASK)
-                        >> (8u - SPACE_PACKET_SEQUENCE_CONTROL_HIGH_BITS_OFFSET);
+    packetPointer[2] |= (packetizer->packetSequenceCount & PACKETIZER_PACKET_SEQUENCE_CONTROL_HIGH_BITS_MASK) >> 8u;
     packetPointer[3] |= packetizer->packetSequenceCount & 0xFF;
 }
 
@@ -198,10 +214,16 @@ writePacketDataLength(uint8_t* const packetPointer, const size_t dataSize)
 void
 writeCrc(uint8_t* const packetPointer, const size_t dataSize)
 {
-    uint16_t crc = calculateCrc16(packetPointer, SPACE_PACKET_PRIMARY_HEADER_SIZE + dataSize);
+    const size_t checksumInputSize = SPACE_PACKET_PRIMARY_HEADER_SIZE + dataSize - SPACE_PACKET_ERROR_CONTROL_SIZE;
+
+#ifdef PACKETIZER_USE_CRC16_CHECKSUM
+    uint16_t checksum = calculateCrc16(packetPointer, checksumInputSize);
+#else
+    uint16_t checksum = IsoChecksum_calculate(packetPointer, checksumInputSize);
+#endif
 
-    packetPointer[SPACE_PACKET_PRIMARY_HEADER_SIZE + dataSize] = (crc >> 8) & 0xFF;
-    packetPointer[SPACE_PACKET_PRIMARY_HEADER_SIZE + dataSize + 1] = crc & 0xFF;
+    packetPointer[checksumInputSize] = (checksum >> 8) & 0xFF;
+    packetPointer[checksumInputSize + 1] = checksum & 0xFF;
 }
 
 size_t

src/Packetizer/Packetizer.h

@@ -46,7 +46,7 @@ typedef enum
 /// @brief  Enumeration listing possible error codes.
 typedef enum
 {
-    Packetizer_ErrorCode_IncorrectCrc16 = 1,      ///< Mismatching CRC16 during depacketization
+    Packetizer_ErrorCode_IncorrectChecksum = 1,   ///< Mismatching checksum during depacketization
     Packetizer_ErrorCode_IncorrectPacketType = 2, ///< Mismatching packet type during depacketization
     Packetizer_ErrorCode_IncorrectPacketSize = 3  ///< Mismatching packet size during depacketization
 } Packetizer_ErrorCode;