|
@@
-79,7
+79,7
void updateLFRCurrentMode()
|
|
79
|
|
|
79
|
|
|
80
|
//*********************
|
|
80
|
//*********************
|
|
81
|
// ACCEPTANCE FUNCTIONS
|
|
81
|
// ACCEPTANCE FUNCTIONS
|
|
82
|
int TC_acceptance(ccsdsTelecommandPacket_t *TC, unsigned int tc_len_recv, rtems_id queue_id)
|
|
82
|
int TC_acceptance(ccsdsTelecommandPacket_t *TC, unsigned int tc_len_recv, rtems_id queue_queu_id, rtems_id queue_pkts_id)
|
|
83
|
{
|
|
83
|
{
|
|
84
|
int ret = 0;
|
|
84
|
int ret = 0;
|
|
85
|
rtems_status_code status;
|
|
85
|
rtems_status_code status;
|
|
@@
-87,13
+87,16
int TC_acceptance(ccsdsTelecommandPacket
|
|
87
|
unsigned int parserCode = 0;
|
|
87
|
unsigned int parserCode = 0;
|
|
88
|
unsigned char computed_CRC[2];
|
|
88
|
unsigned char computed_CRC[2];
|
|
89
|
unsigned int packetLength;
|
|
89
|
unsigned int packetLength;
|
|
|
|
|
90
|
unsigned char *packetDataField;
|
|
90
|
|
|
91
|
|
|
91
|
GetCRCAsTwoBytes( (unsigned char*) TC->packetID, computed_CRC, tc_len_recv + 5 );
|
|
92
|
GetCRCAsTwoBytes( (unsigned char*) TC->packetID, computed_CRC, tc_len_recv + 5 );
|
|
92
|
parserCode = TC_parser( TC, tc_len_recv ) ;
|
|
93
|
parserCode = TC_parser( TC, tc_len_recv ) ;
|
|
93
|
if ( (parserCode == ILLEGAL_APID) | (parserCode == WRONG_LEN_PACKET) | (parserCode == INCOR_CHECKSUM)
|
|
94
|
if ( (parserCode == ILLEGAL_APID) | (parserCode == WRONG_LEN_PACKET) | (parserCode == INCOR_CHECKSUM)
|
|
94
|
| (parserCode == ILL_TYPE) | (parserCode == ILL_SUBTYPE) | (parserCode == WRONG_APP_DATA) )
|
|
95
|
| (parserCode == ILL_TYPE) | (parserCode == ILL_SUBTYPE) | (parserCode == WRONG_APP_DATA) )
|
|
95
|
{ // generate TM_LFR_TC_EXE_CORRUPTED
|
|
96
|
{ // generate TM_LFR_TC_EXE_CORRUPTED
|
|
96
|
packetLength = (TC->packetLength[0] * 256) + TC->packetLength[1];
|
|
97
|
packetDataField = (unsigned char *) &TC->headerFlag_pusVersion_Ack; // get the beginning of the data field
|
|
|
|
|
98
|
packetLength = (TC->packetLength[0] * 256) + TC->packetLength[1]; // compute the packet length
|
|
|
|
|
99
|
//
|
|
97
|
packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
|
|
100
|
packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
|
|
98
|
packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
|
|
101
|
packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
|
|
99
|
packet.reserved = DEFAULT_RESERVED;
|
|
102
|
packet.reserved = DEFAULT_RESERVED;
|
|
@@
-129,21
+132,25
int TC_acceptance(ccsdsTelecommandPacket
|
|
129
|
packet.pkt_len_rcv_value[1] = TC->packetLength[1];
|
|
132
|
packet.pkt_len_rcv_value[1] = TC->packetLength[1];
|
|
130
|
packet.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
|
|
133
|
packet.pkt_datafieldsize_cnt[0] = currentTC_LEN_RCV[0];
|
|
131
|
packet.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
|
|
134
|
packet.pkt_datafieldsize_cnt[1] = currentTC_LEN_RCV[1];
|
|
132
|
packet.rcv_crc[0] = TC->dataAndCRC[packetLength];
|
|
135
|
packet.rcv_crc[0] = packetDataField[ packetLength - 1 ];
|
|
133
|
packet.rcv_crc[1] = TC->dataAndCRC[packetLength];
|
|
136
|
packet.rcv_crc[1] = packetDataField[ packetLength ];
|
|
134
|
packet.computed_crc[0] = computed_CRC[0];
|
|
137
|
packet.computed_crc[0] = computed_CRC[0];
|
|
135
|
packet.computed_crc[1] = computed_CRC[1];
|
|
138
|
packet.computed_crc[1] = computed_CRC[1];
|
|
136
|
// SEND PACKET
|
|
139
|
// SEND DATA
|
|
137
|
status = write( fdSPW, (char *) &packet, PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + 4);
|
|
140
|
status = rtems_message_queue_urgent( queue_pkts_id, &packet, PACKET_LENGTH_TC_EXE_CORRUPTED + CCSDS_TC_TM_PACKET_OFFSET + 4);
|
|
|
|
|
141
|
if (status != RTEMS_SUCCESSFUL) {
|
|
|
|
|
142
|
PRINTF("in TC_acceptance *** ERR rtems_message_queue_urgent\n")
|
|
|
|
|
143
|
ret = LFR_DEFAULT;
|
|
|
|
|
144
|
}
|
|
138
|
}
|
|
145
|
}
|
|
139
|
else { // send valid TC to the action launcher
|
|
146
|
else { // send valid TC to the action launcher
|
|
140
|
status = rtems_message_queue_send( queue_id, TC, tc_len_recv + CCSDS_TC_TM_PACKET_OFFSET + 3);
|
|
147
|
status = rtems_message_queue_send( queue_queu_id, TC, tc_len_recv + CCSDS_TC_TM_PACKET_OFFSET + 3);
|
|
141
|
ret = -1;
|
|
148
|
ret = LFR_SUCCESSFUL;
|
|
142
|
}
|
|
149
|
}
|
|
143
|
return ret;
|
|
150
|
return ret;
|
|
144
|
}
|
|
151
|
}
|
|
145
|
|
|
152
|
|
|
146
|
unsigned char TC_parser(ccsdsTelecommandPacket_t * TMPacket, unsigned int TC_LEN_RCV)
|
|
153
|
unsigned char TC_parser(ccsdsTelecommandPacket_t * TCPacket, unsigned int TC_LEN_RCV)
|
|
147
|
{
|
|
154
|
{
|
|
148
|
unsigned char ret = 0;
|
|
155
|
unsigned char ret = 0;
|
|
149
|
unsigned char pid = 0;
|
|
156
|
unsigned char pid = 0;
|
|
@@
-154,11
+161,11
unsigned char TC_parser(ccsdsTelecommand
|
|
154
|
unsigned char * CCSDSContent = NULL;
|
|
161
|
unsigned char * CCSDSContent = NULL;
|
|
155
|
|
|
162
|
|
|
156
|
// APID check *** APID on 2 bytes
|
|
163
|
// APID check *** APID on 2 bytes
|
|
157
|
pid = ((TMPacket->packetID[0] & 0x07)<<4) + ( (TMPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
|
|
164
|
pid = ((TCPacket->packetID[0] & 0x07)<<4) + ( (TCPacket->packetID[1]>>4) & 0x0f ); // PID = 11 *** 7 bits xxxxx210 7654xxxx
|
|
158
|
category = (TMPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
|
|
165
|
category = (TCPacket->packetID[1] & 0x0f); // PACKET_CATEGORY = 12 *** 4 bits xxxxxxxx xxxx3210
|
|
159
|
length = (TMPacket->packetLength[0] * 256) + TMPacket->packetLength[1];
|
|
166
|
length = (TCPacket->packetLength[0] * 256) + TCPacket->packetLength[1];
|
|
160
|
packetType = TMPacket->serviceType;
|
|
167
|
packetType = TCPacket->serviceType;
|
|
161
|
packetSubtype = TMPacket->serviceSubType;
|
|
168
|
packetSubtype = TCPacket->serviceSubType;
|
|
162
|
|
|
169
|
|
|
163
|
if ( pid != CCSDS_PROCESS_ID ) {
|
|
170
|
if ( pid != CCSDS_PROCESS_ID ) {
|
|
164
|
ret = ILLEGAL_APID;
|
|
171
|
ret = ILLEGAL_APID;
|
|
@@
-286,7
+293,7
unsigned char TC_parser(ccsdsTelecommand
|
|
286
|
|
|
293
|
|
|
287
|
// packet error control, CRC check
|
|
294
|
// packet error control, CRC check
|
|
288
|
if ( ret == CCSDS_TM_VALID ) {
|
|
295
|
if ( ret == CCSDS_TM_VALID ) {
|
|
289
|
CCSDSContent = (unsigned char*) TMPacket->packetID;
|
|
296
|
CCSDSContent = (unsigned char*) TCPacket->packetID;
|
|
290
|
GetCRCAsTwoBytes(CCSDSContent, currentTC_COMPUTED_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
|
|
297
|
GetCRCAsTwoBytes(CCSDSContent, currentTC_COMPUTED_CRC, length + CCSDS_TC_TM_PACKET_OFFSET - 2); // 2 CRC bytes removed from the calculation of the CRC
|
|
291
|
if (currentTC_COMPUTED_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) {
|
|
298
|
if (currentTC_COMPUTED_CRC[0] != CCSDSContent[length + CCSDS_TC_TM_PACKET_OFFSET -2]) {
|
|
292
|
ret = INCOR_CHECKSUM;
|
|
299
|
ret = INCOR_CHECKSUM;
|
|
@@
-308,18
+315,24
rtems_task recv_task( rtems_task_argumen
|
|
308
|
{
|
|
315
|
{
|
|
309
|
int len = 0;
|
|
316
|
int len = 0;
|
|
310
|
unsigned int i = 0;
|
|
317
|
unsigned int i = 0;
|
|
311
|
unsigned int data_length = 0;
|
|
|
|
|
312
|
ccsdsTelecommandPacket_t currentTC;
|
|
318
|
ccsdsTelecommandPacket_t currentTC;
|
|
313
|
char data[100];
|
|
319
|
char data[100];
|
|
314
|
rtems_status_code status;
|
|
320
|
rtems_status_code status;
|
|
315
|
rtems_id queue_id;
|
|
321
|
rtems_id queue_queu_id;
|
|
|
|
|
322
|
rtems_id queue_pkts_id;
|
|
316
|
|
|
323
|
|
|
317
|
for(i=0; i<100; i++) data[i] = 0;
|
|
324
|
for(i=0; i<100; i++) data[i] = 0;
|
|
318
|
|
|
325
|
|
|
319
|
status = rtems_message_queue_ident( misc_name[QUEUE_QUEU], 0, &queue_id );
|
|
326
|
status = rtems_message_queue_ident( misc_name[QUEUE_QUEU], 0, &queue_queu_id );
|
|
320
|
if (status != RTEMS_SUCCESSFUL)
|
|
327
|
if (status != RTEMS_SUCCESSFUL)
|
|
321
|
{
|
|
328
|
{
|
|
322
|
PRINTF1("in RECV *** ERR getting queue id, %d\n", status)
|
|
329
|
PRINTF1("in RECV *** ERR getting queue_queu id, %d\n", status)
|
|
|
|
|
330
|
}
|
|
|
|
|
331
|
|
|
|
|
|
332
|
status = rtems_message_queue_ident( misc_name[QUEUE_PKTS], 0, &queue_pkts_id );
|
|
|
|
|
333
|
if (status != RTEMS_SUCCESSFUL)
|
|
|
|
|
334
|
{
|
|
|
|
|
335
|
PRINTF1("in RECV *** ERR getting queue_pkts id, %d\n", status)
|
|
323
|
}
|
|
336
|
}
|
|
324
|
|
|
337
|
|
|
325
|
BOOT_PRINTF("in RECV *** \n")
|
|
338
|
BOOT_PRINTF("in RECV *** \n")
|
|
@@
-329,12
+342,6
rtems_task recv_task( rtems_task_argumen
|
|
329
|
len = read(fdSPW, (char*) ¤tTC, CCSDS_TC_PKT_MAX_SIZE); // the call to read is blocking
|
|
342
|
len = read(fdSPW, (char*) ¤tTC, CCSDS_TC_PKT_MAX_SIZE); // the call to read is blocking
|
|
330
|
if (len == -1){ // error during the read call
|
|
343
|
if (len == -1){ // error during the read call
|
|
331
|
PRINTF("In RECV *** last read call returned -1\n")
|
|
344
|
PRINTF("In RECV *** last read call returned -1\n")
|
|
332
|
//if (rtems_event_send( Task_id[TASKID_SPIQ], SPW_LINKERR_EVENT ) != RTEMS_SUCCESSFUL) {
|
|
|
|
|
333
|
// PRINTF("IN RECV *** Error: rtems_event_send SPW_LINKERR_EVENT\n")
|
|
|
|
|
334
|
//}
|
|
|
|
|
335
|
//if (rtems_task_suspend(RTEMS_SELF) != RTEMS_SUCCESSFUL) {
|
|
|
|
|
336
|
// PRINTF("In RECV *** Error: rtems_task_suspend(RTEMS_SELF)\n")
|
|
|
|
|
337
|
//}
|
|
|
|
|
338
|
}
|
|
345
|
}
|
|
339
|
else {
|
|
346
|
else {
|
|
340
|
if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
|
|
347
|
if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
|
|
@@
-344,11
+351,8
rtems_task recv_task( rtems_task_argumen
|
|
344
|
currentTC_LEN_RCV[0] = 0x00;
|
|
351
|
currentTC_LEN_RCV[0] = 0x00;
|
|
345
|
currentTC_LEN_RCV[1] = (unsigned char) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // build the corresponding packet size field
|
|
352
|
currentTC_LEN_RCV[1] = (unsigned char) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // build the corresponding packet size field
|
|
346
|
currentTC_LEN_RCV_AsUnsignedInt = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
|
|
353
|
currentTC_LEN_RCV_AsUnsignedInt = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
|
|
347
|
// CHECK THE TC AND BUILD THE APPROPRIATE TM
|
|
354
|
// CHECK THE TC
|
|
348
|
data_length = TC_acceptance(¤tTC, currentTC_LEN_RCV_AsUnsignedInt, queue_id);
|
|
355
|
TC_acceptance(¤tTC, currentTC_LEN_RCV_AsUnsignedInt, queue_queu_id, queue_pkts_id);
|
|
349
|
if (data_length!=-1)
|
|
|
|
|
350
|
{
|
|
|
|
|
351
|
}
|
|
|
|
|
352
|
}
|
|
356
|
}
|
|
353
|
}
|
|
357
|
}
|
|
354
|
}
|
|
358
|
}
|