Line data Source code
1 : /**
2 : * @ingroup packet
3 : *
4 : * @file packet.c
5 : *
6 : * @{
7 : */
8 :
9 : #include "../firmware.h"
10 :
11 5 : void Packet_sign_shake128(const uint8_t *input, uint32_t input_length, const uint8_t *key, uint32_t key_length, uint8_t *output)
12 : {
13 : util_shake_ctx_t shake_ctx;
14 5 : Util_shake_init(&shake_ctx, 16);
15 :
16 5 : Util_shake_update(&shake_ctx, input, input_length);
17 5 : Util_shake_update(&shake_ctx, key, key_length);
18 :
19 5 : Util_shake_out(&shake_ctx, output);
20 5 : }
21 :
22 1 : void Packet_telecommand_512_encode(packet_telecommand_512 *input_packet, uint8_t *output_buffer, const uint16_t origin, const uint8_t *key, uint32_t key_length)
23 : {
24 1 : input_packet->spacecraft = origin;
25 :
26 : /* Authentication hash */
27 1 : Packet_sign_shake128(((packet_telecommand_512_hash *)input_packet)->data, 46, key, key_length, input_packet->hash);
28 :
29 : /* Integrity CRC */
30 1 : input_packet->crc = Util_crc16(((packet_telecommand_512_crc *)input_packet)->data, 62);
31 :
32 : /* Whiten input data to improve FEC performance */
33 1 : Util_pn9((uint8_t *)input_packet, 0, 64);
34 :
35 : /* There's no CCSDS k=512, r=1/2 LDPC, so we're appending 2x k=256 r=1/2 blocks */
36 1 : labrador_ldpc_copy_encode(LABRADOR_LDPC_CODE_TC512, (uint8_t *)input_packet, output_buffer);
37 1 : labrador_ldpc_copy_encode(LABRADOR_LDPC_CODE_TC512, &((uint8_t *)input_packet)[32], &output_buffer[64]);
38 :
39 : /* Interleave to improve FEC performance */
40 1 : Packet_interleave_32x32(output_buffer);
41 1 : }
42 :
43 1 : bool Packet_telecommand_512_decode(uint8_t *input_buffer, packet_telecommand_512 *output_packet, const uint16_t destination, const uint8_t *key, uint32_t key_length)
44 : {
45 : uint8_t ldpc_wa[LABRADOR_LDPC_BF_WORKING_LEN_TC512];
46 : uint8_t ldpc_out[LABRADOR_LDPC_OUTPUT_LEN_TC512];
47 :
48 1 : Packet_uninterleave_32x32(input_buffer);
49 :
50 : /* Attempt decode of first half of packet */
51 1 : if(!labrador_ldpc_decode_bf(LABRADOR_LDPC_CODE_TC512,
52 : input_buffer,
53 : (uint8_t *)output_packet,
54 : ldpc_wa,
55 : 50, /* Iterations to use. TODO: Vary this for CPU budget */
56 : NULL))
57 : {
58 0 : return false;
59 : }
60 :
61 : /* Un-whiten first half of packet */
62 1 : Util_pn9((uint8_t *)output_packet, 0, 32);
63 :
64 : /* Check of the spacecraft id matching our destination, easy and cheap initial sanity check */
65 1 : if(output_packet->spacecraft != destination)
66 : {
67 0 : return false;
68 : }
69 :
70 : /* Attempt decode of second half of packet */
71 1 : if(!labrador_ldpc_decode_bf(LABRADOR_LDPC_CODE_TC512,
72 1 : &input_buffer[64],
73 : ldpc_out,
74 : ldpc_wa,
75 : 50, /* Iterations to use. TODO: Vary this for CPU budget */
76 : NULL))
77 : {
78 0 : return false;
79 : }
80 :
81 1 : memcpy(&((uint8_t *)output_packet)[32], ldpc_out, 32);
82 :
83 : /* Un-whiten second half of packet */
84 1 : Util_pn9(&((uint8_t *)output_packet)[32], 32, 32);
85 :
86 : /* Check packet CRC */
87 1 : if(output_packet->crc != Util_crc16(((packet_telecommand_512_crc *)output_packet)->data, 62))
88 : {
89 0 : return false;
90 : }
91 :
92 : /* Check packet authentication hash */
93 : uint8_t packet_hash[16];
94 1 : Packet_sign_shake128(((packet_telecommand_512_hash *)output_packet)->data, 46, key, key_length, packet_hash);
95 1 : if(memcmp(packet_hash, output_packet->hash, 16) != 0)
96 : {
97 0 : return false;
98 : }
99 :
100 1 : return true;
101 : }
102 :
103 :
104 1 : void Packet_telemetry_1024_encode(packet_telemetry_1024 *input_packet, uint8_t *output_buffer, const uint16_t origin, const uint8_t *key, uint32_t key_length)
105 : {
106 1 : input_packet->spacecraft = origin;
107 :
108 1 : Packet_sign_shake128(((packet_telemetry_1024_hash *)input_packet)->data, 110, key, key_length, input_packet->hash);
109 :
110 1 : input_packet->crc = Util_crc16(((packet_telemetry_1024_crc *)input_packet)->data, 126);
111 :
112 1 : Util_pn9((uint8_t *)input_packet, 0, 128);
113 :
114 : /* TODO: Add encoder for Matt's r=1/3 Turbo FEC */
115 : /* packet[1024b] -> output_buffer[3072b] */
116 1 : memcpy(output_buffer, input_packet, sizeof(packet_telemetry_1024));
117 :
118 : /* TODO: Need a rectangular interleaver */
119 : //Packet_interleave_64x64((uint8_t *)output_buffer);
120 1 : }
121 :
122 1 : bool Packet_telemetry_1024_decode(uint8_t *input_buffer, packet_telemetry_1024 *output_packet, const uint16_t destination, const uint8_t *key, uint32_t key_length)
123 : {
124 : /* TODO: Need a rectangular interleaver */
125 : //Packet_uninterleave_64x64(input_buffer);
126 :
127 : /* TODO: Add hard-decoder for Matt's r=1/3 Turbo FEC (not used on the spacecraft) */
128 : /* input_buffer[3072b] -> output_packet [1024b] */
129 1 : memcpy(output_packet, input_buffer, sizeof(packet_telemetry_1024));
130 :
131 1 : Util_pn9((uint8_t *)output_packet, 0, 128);
132 :
133 : /* Check of the spacecraft id matching our destination, easy and cheap initial sanity check */
134 1 : if(output_packet->spacecraft != destination)
135 : {
136 0 : return false;
137 : }
138 :
139 1 : if(output_packet->crc != Util_crc16(((packet_telemetry_1024_crc *)output_packet)->data, 126))
140 : {
141 0 : return false;
142 : }
143 :
144 : uint8_t packet_hash[16];
145 1 : Packet_sign_shake128(((packet_telemetry_1024_hash *)output_packet)->data, 110, key, key_length, packet_hash);
146 1 : if(memcmp(packet_hash, output_packet->hash, 16) != 0)
147 : {
148 0 : return false;
149 : }
150 :
151 1 : return true;
152 : }
153 :
154 : /**
155 : * @}
156 : */
|
