1 |
|
|
/*------------------------------------------------------------------------------ |
2 |
|
|
-- Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW), |
3 |
|
|
-- This file is a part of the LFR FSW |
4 |
|
|
-- Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS |
5 |
|
|
-- |
6 |
|
|
-- This program is free software; you can redistribute it and/or modify |
7 |
|
|
-- it under the terms of the GNU General Public License as published by |
8 |
|
|
-- the Free Software Foundation; either version 2 of the License, or |
9 |
|
|
-- (at your option) any later version. |
10 |
|
|
-- |
11 |
|
|
-- This program is distributed in the hope that it will be useful, |
12 |
|
|
-- but WITHOUT ANY WARRANTY; without even the implied warranty of |
13 |
|
|
-- MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the |
14 |
|
|
-- GNU General Public License for more details. |
15 |
|
|
-- |
16 |
|
|
-- You should have received a copy of the GNU General Public License |
17 |
|
|
-- along with this program; if not, write to the Free Software |
18 |
|
|
-- Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA |
19 |
|
|
-------------------------------------------------------------------------------*/ |
20 |
|
|
/*-- Author : Paul Leroy |
21 |
|
|
-- Contact : Alexis Jeandet |
22 |
|
|
-- Mail : alexis.jeandet@lpp.polytechnique.fr |
23 |
|
|
----------------------------------------------------------------------------*/ |
24 |
|
|
|
25 |
|
|
/** Functions related to the SpaceWire interface. |
26 |
|
|
* |
27 |
|
|
* @file |
28 |
|
|
* @author P. LEROY |
29 |
|
|
* |
30 |
|
|
* A group of functions to handle SpaceWire transmissions: |
31 |
|
|
* - configuration of the SpaceWire link |
32 |
|
|
* - SpaceWire related interruption requests processing |
33 |
|
|
* - transmission of TeleMetry packets by a dedicated RTEMS task |
34 |
|
|
* - reception of TeleCommands by a dedicated RTEMS task |
35 |
|
|
* |
36 |
|
|
*/ |
37 |
|
|
|
38 |
|
|
#include "fsw_spacewire.h" |
39 |
|
|
|
40 |
|
|
rtems_name semq_name = 0; |
41 |
|
|
rtems_id semq_id = RTEMS_ID_NONE; |
42 |
|
|
|
43 |
|
|
//***************** |
44 |
|
|
// waveform headers |
45 |
|
|
Header_TM_LFR_SCIENCE_CWF_t headerCWF = {0}; |
46 |
|
|
Header_TM_LFR_SCIENCE_SWF_t headerSWF = {0}; |
47 |
|
|
Header_TM_LFR_SCIENCE_ASM_t headerASM = {0}; |
48 |
|
|
|
49 |
|
|
unsigned char previousTimecodeCtr = 0; |
50 |
|
|
unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER); |
51 |
|
|
|
52 |
|
|
//*********** |
53 |
|
|
// RTEMS TASK |
54 |
|
1 |
rtems_task spiq_task(rtems_task_argument unused) |
55 |
|
|
{ |
56 |
|
|
/** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver. |
57 |
|
|
* |
58 |
|
|
* @param unused is the starting argument of the RTEMS task |
59 |
|
|
* |
60 |
|
|
*/ |
61 |
|
|
|
62 |
|
|
rtems_event_set event_out; |
63 |
|
|
rtems_status_code status; |
64 |
|
|
int linkStatus; |
65 |
|
|
|
66 |
|
1 |
event_out = EVENT_SETS_NONE_PENDING; |
67 |
|
1 |
linkStatus = 0; |
68 |
|
|
|
69 |
|
|
BOOT_PRINTF("in SPIQ *** \n") |
70 |
|
|
|
71 |
|
|
while(true){ |
72 |
|
1 |
rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT |
73 |
|
|
PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n") |
74 |
|
|
|
75 |
|
|
// [0] SUSPEND RECV AND SEND TASKS |
76 |
|
|
status = rtems_task_suspend( Task_id[ TASKID_RECV ] ); |
77 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
78 |
|
|
PRINTF("in SPIQ *** ERR suspending RECV Task\n") |
79 |
|
|
} |
80 |
|
|
status = rtems_task_suspend( Task_id[ TASKID_SEND ] ); |
81 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
82 |
|
|
PRINTF("in SPIQ *** ERR suspending SEND Task\n") |
83 |
|
|
} |
84 |
|
|
|
85 |
|
|
// [1] CHECK THE LINK |
86 |
|
|
status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1) |
87 |
|
|
if ( linkStatus != SPW_LINK_OK) { |
88 |
|
|
PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus) |
89 |
|
|
status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms |
90 |
|
|
} |
91 |
|
|
|
92 |
|
|
// [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT |
93 |
|
|
status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2) |
94 |
|
|
if ( linkStatus != SPW_LINK_OK ) // [2.a] not in run state, reset the link |
95 |
|
|
{ |
96 |
|
|
spacewire_read_statistics(); |
97 |
|
|
status = spacewire_several_connect_attemps( ); |
98 |
|
|
} |
99 |
|
|
else // [2.b] in run state, start the link |
100 |
|
|
{ |
101 |
|
|
status = spacewire_stop_and_start_link( fdSPW ); // start the link |
102 |
|
|
if ( status != RTEMS_SUCCESSFUL) |
103 |
|
|
{ |
104 |
|
|
PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status) |
105 |
|
|
} |
106 |
|
|
} |
107 |
|
|
|
108 |
|
|
// [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS |
109 |
|
|
if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully |
110 |
|
|
{ |
111 |
|
|
status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 ); |
112 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
113 |
|
|
PRINTF("in SPIQ *** ERR resuming SEND Task\n") |
114 |
|
|
} |
115 |
|
|
status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 ); |
116 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
117 |
|
|
PRINTF("in SPIQ *** ERR resuming RECV Task\n") |
118 |
|
|
} |
119 |
|
|
} |
120 |
|
|
else // [3.b] the link is not in run state, go in STANDBY mode |
121 |
|
|
{ |
122 |
|
|
status = enter_mode_standby(); |
123 |
|
|
if ( status != RTEMS_SUCCESSFUL ) |
124 |
|
|
{ |
125 |
|
|
PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status) |
126 |
|
|
} |
127 |
|
|
{ |
128 |
|
|
updateLFRCurrentMode( LFR_MODE_STANDBY ); |
129 |
|
|
} |
130 |
|
|
// wake the LINK task up to wait for the link recovery |
131 |
|
|
status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 ); |
132 |
|
|
status = rtems_task_suspend( RTEMS_SELF ); |
133 |
|
|
} |
134 |
|
|
} |
135 |
|
|
} |
136 |
|
|
|
137 |
|
1 |
rtems_task recv_task( rtems_task_argument unused ) |
138 |
|
|
{ |
139 |
|
|
/** This RTEMS task is dedicated to the reception of incoming TeleCommands. |
140 |
|
|
* |
141 |
|
|
* @param unused is the starting argument of the RTEMS task |
142 |
|
|
* |
143 |
|
|
* The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked: |
144 |
|
|
* 1. It reads the incoming data. |
145 |
|
|
* 2. Launches the acceptance procedure. |
146 |
|
|
* 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue. |
147 |
|
|
* |
148 |
|
|
*/ |
149 |
|
|
|
150 |
|
|
int len; |
151 |
|
|
ccsdsTelecommandPacket_t __attribute__((aligned(4))) currentTC; |
152 |
|
|
unsigned char computed_CRC[ BYTES_PER_CRC ]; |
153 |
|
|
unsigned char currentTC_LEN_RCV[ BYTES_PER_PKT_LEN ]; |
154 |
|
|
unsigned char destinationID; |
155 |
|
|
unsigned int estimatedPacketLength; |
156 |
|
|
unsigned int parserCode; |
157 |
|
|
rtems_status_code status; |
158 |
|
|
rtems_id queue_recv_id; |
159 |
|
|
rtems_id queue_send_id; |
160 |
|
|
|
161 |
|
1 |
memset( ¤tTC, 0, sizeof(ccsdsTelecommandPacket_t) ); |
162 |
|
1 |
destinationID = 0; |
163 |
|
1 |
queue_recv_id = RTEMS_ID_NONE; |
164 |
|
1 |
queue_send_id = RTEMS_ID_NONE; |
165 |
|
|
|
166 |
|
1 |
initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes |
167 |
|
|
|
168 |
|
1 |
status = get_message_queue_id_recv( &queue_recv_id ); |
169 |
|
|
if (status != RTEMS_SUCCESSFUL) |
170 |
|
|
{ |
171 |
|
|
PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status) |
172 |
|
|
} |
173 |
|
|
|
174 |
|
1 |
status = get_message_queue_id_send( &queue_send_id ); |
175 |
|
|
if (status != RTEMS_SUCCESSFUL) |
176 |
|
|
{ |
177 |
|
|
PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status) |
178 |
|
|
} |
179 |
|
|
|
180 |
|
|
BOOT_PRINTF("in RECV *** \n") |
181 |
|
|
|
182 |
|
|
while(1) |
183 |
|
|
{ |
184 |
|
2 |
len = read( fdSPW, (char*) ¤tTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking |
185 |
✓✗ |
1 |
if (len == -1){ // error during the read call |
186 |
|
|
PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno) |
187 |
|
|
} |
188 |
|
|
else { |
189 |
✓✗ |
1 |
if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) { |
190 |
|
|
PRINTF("in RECV *** packet lenght too short\n") |
191 |
|
|
} |
192 |
|
|
else { |
193 |
|
1 |
estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - PROTID_RES_APP); // => -3 is for Prot ID, Reserved and User App bytes |
194 |
|
|
PRINTF1("incoming TC with Length (byte): %d\n", len - 3); |
195 |
|
1 |
currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> SHIFT_1_BYTE); |
196 |
|
1 |
currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength ); |
197 |
|
|
// CHECK THE TC |
198 |
|
1 |
parserCode = tc_parser( ¤tTC, estimatedPacketLength, computed_CRC ) ; |
199 |
✗✓ |
1 |
if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT) |
200 |
|
|
|| (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE) |
201 |
|
|
|| (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA) |
202 |
|
1 |
|| (parserCode == WRONG_SRC_ID) ) |
203 |
|
|
{ // send TM_LFR_TC_EXE_CORRUPTED |
204 |
|
|
PRINTF1("TC corrupted received, with code: %d\n", parserCode); |
205 |
|
|
if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) ) |
206 |
|
|
&& |
207 |
|
|
!( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO)) |
208 |
|
|
) |
209 |
|
|
{ |
210 |
|
|
if ( parserCode == WRONG_SRC_ID ) |
211 |
|
|
{ |
212 |
|
|
destinationID = SID_TC_GROUND; |
213 |
|
|
} |
214 |
|
|
else |
215 |
|
|
{ |
216 |
|
|
destinationID = currentTC.sourceID; |
217 |
|
|
} |
218 |
|
|
send_tm_lfr_tc_exe_corrupted( ¤tTC, queue_send_id, |
219 |
|
|
computed_CRC, currentTC_LEN_RCV, |
220 |
|
|
destinationID ); |
221 |
|
|
} |
222 |
|
|
} |
223 |
|
|
else |
224 |
|
|
{ // send valid TC to the action launcher |
225 |
|
1 |
status = rtems_message_queue_send( queue_recv_id, ¤tTC, |
226 |
|
|
estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + PROTID_RES_APP); |
227 |
|
|
} |
228 |
|
|
} |
229 |
|
|
} |
230 |
|
|
|
231 |
|
1 |
update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max ); |
232 |
|
|
|
233 |
|
1 |
} |
234 |
|
|
} |
235 |
|
|
|
236 |
|
1 |
rtems_task send_task( rtems_task_argument argument) |
237 |
|
|
{ |
238 |
|
|
/** This RTEMS task is dedicated to the transmission of TeleMetry packets. |
239 |
|
|
* |
240 |
|
|
* @param unused is the starting argument of the RTEMS task |
241 |
|
|
* |
242 |
|
|
* The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives: |
243 |
|
|
* - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call. |
244 |
|
|
* - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After |
245 |
|
|
* analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the |
246 |
|
|
* data it contains. |
247 |
|
|
* |
248 |
|
|
*/ |
249 |
|
|
|
250 |
|
|
rtems_status_code status; // RTEMS status code |
251 |
|
|
char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer |
252 |
|
|
ring_node *incomingRingNodePtr; |
253 |
|
|
int ring_node_address; |
254 |
|
|
char *charPtr; |
255 |
|
|
spw_ioctl_pkt_send *spw_ioctl_send; |
256 |
|
|
size_t size; // size of the incoming TC packet |
257 |
|
|
rtems_id queue_send_id; |
258 |
|
|
unsigned int sid; |
259 |
|
|
unsigned char sidAsUnsignedChar; |
260 |
|
|
unsigned char type; |
261 |
|
|
|
262 |
|
1 |
incomingRingNodePtr = NULL; |
263 |
|
1 |
ring_node_address = 0; |
264 |
|
1 |
charPtr = (char *) &ring_node_address; |
265 |
|
1 |
size = 0; |
266 |
|
1 |
queue_send_id = RTEMS_ID_NONE; |
267 |
|
1 |
sid = 0; |
268 |
|
1 |
sidAsUnsignedChar = 0; |
269 |
|
|
|
270 |
|
1 |
init_header_cwf( &headerCWF ); |
271 |
|
1 |
init_header_swf( &headerSWF ); |
272 |
|
1 |
init_header_asm( &headerASM ); |
273 |
|
|
|
274 |
|
1 |
status = get_message_queue_id_send( &queue_send_id ); |
275 |
|
|
if (status != RTEMS_SUCCESSFUL) |
276 |
|
|
{ |
277 |
|
|
PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status) |
278 |
|
|
} |
279 |
|
|
|
280 |
|
|
BOOT_PRINTF("in SEND *** \n") |
281 |
|
|
|
282 |
|
|
while(1) |
283 |
|
|
{ |
284 |
|
152 |
status = rtems_message_queue_receive( queue_send_id, incomingData, &size, |
285 |
|
|
RTEMS_WAIT, RTEMS_NO_TIMEOUT ); |
286 |
|
|
|
287 |
✓✗ |
151 |
if (status!=RTEMS_SUCCESSFUL) |
288 |
|
|
{ |
289 |
|
|
PRINTF1("in SEND *** (1) ERR = %d\n", status) |
290 |
|
|
} |
291 |
|
|
else |
292 |
|
|
{ |
293 |
✗✓ |
151 |
if ( size == sizeof(ring_node*) ) |
294 |
|
|
{ |
295 |
|
|
charPtr[0] = incomingData[0]; |
296 |
|
|
charPtr[1] = incomingData[1]; |
297 |
|
|
charPtr[BYTE_2] = incomingData[BYTE_2]; |
298 |
|
|
charPtr[BYTE_3] = incomingData[BYTE_3]; |
299 |
|
|
incomingRingNodePtr = (ring_node*) ring_node_address; |
300 |
|
|
sid = incomingRingNodePtr->sid; |
301 |
|
|
if ( (sid==SID_NORM_CWF_LONG_F3) |
302 |
|
|
|| (sid==SID_BURST_CWF_F2 ) |
303 |
|
|
|| (sid==SID_SBM1_CWF_F1 ) |
304 |
|
|
|| (sid==SID_SBM2_CWF_F2 )) |
305 |
|
|
{ |
306 |
|
|
spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF ); |
307 |
|
|
} |
308 |
|
|
else if ( (sid==SID_NORM_SWF_F0) || (sid==SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) ) |
309 |
|
|
{ |
310 |
|
|
spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF ); |
311 |
|
|
} |
312 |
|
|
else if (sid==SID_NORM_CWF_F3) |
313 |
|
|
{ |
314 |
|
|
spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF ); |
315 |
|
|
} |
316 |
|
|
else if (sid==SID_NORM_ASM_F0) |
317 |
|
|
{ |
318 |
|
|
spw_send_asm_f0( incomingRingNodePtr, &headerASM ); |
319 |
|
|
} |
320 |
|
|
else if (sid==SID_NORM_ASM_F1) |
321 |
|
|
{ |
322 |
|
|
spw_send_asm_f1( incomingRingNodePtr, &headerASM ); |
323 |
|
|
} |
324 |
|
|
else if (sid==SID_NORM_ASM_F2) |
325 |
|
|
{ |
326 |
|
|
spw_send_asm_f2( incomingRingNodePtr, &headerASM ); |
327 |
|
|
} |
328 |
|
|
else if (sid==TM_CODE_K_DUMP) |
329 |
|
|
{ |
330 |
|
|
spw_send_k_dump( incomingRingNodePtr ); |
331 |
|
|
} |
332 |
|
|
else |
333 |
|
|
{ |
334 |
|
|
PRINTF1("unexpected sid = %d\n", sid); |
335 |
|
|
} |
336 |
|
|
} |
337 |
✓✗ |
151 |
else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet |
338 |
|
|
{ |
339 |
|
151 |
sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ]; |
340 |
|
151 |
sid = sidAsUnsignedChar; |
341 |
|
151 |
type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ]; |
342 |
✗✓ |
151 |
if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently |
343 |
|
|
// SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS |
344 |
|
|
{ |
345 |
|
|
increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid ); |
346 |
|
|
} |
347 |
|
|
|
348 |
|
151 |
status = write( fdSPW, incomingData, size ); |
349 |
|
|
if (status == -1){ |
350 |
|
|
PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size) |
351 |
|
|
} |
352 |
|
|
} |
353 |
|
|
else // the incoming message is a spw_ioctl_pkt_send structure |
354 |
|
|
{ |
355 |
|
|
spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData; |
356 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send ); |
357 |
|
|
if (status == -1){ |
358 |
|
|
PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status) |
359 |
|
|
} |
360 |
|
|
} |
361 |
|
|
} |
362 |
|
|
|
363 |
|
151 |
update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max ); |
364 |
|
|
|
365 |
|
151 |
} |
366 |
|
|
} |
367 |
|
|
|
368 |
|
1 |
rtems_task link_task( rtems_task_argument argument ) |
369 |
|
|
{ |
370 |
|
|
rtems_event_set event_out; |
371 |
|
|
rtems_status_code status; |
372 |
|
|
int linkStatus; |
373 |
|
|
|
374 |
|
1 |
event_out = EVENT_SETS_NONE_PENDING; |
375 |
|
1 |
linkStatus = 0; |
376 |
|
|
|
377 |
|
|
BOOT_PRINTF("in LINK ***\n") |
378 |
|
|
|
379 |
|
|
while(1) |
380 |
|
|
{ |
381 |
|
|
// wait for an RTEMS_EVENT |
382 |
|
1 |
rtems_event_receive( RTEMS_EVENT_0, |
383 |
|
|
RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); |
384 |
|
|
PRINTF("in LINK *** wait for the link\n") |
385 |
|
|
status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status |
386 |
|
|
while( linkStatus != SPW_LINK_OK) // wait for the link |
387 |
|
|
{ |
388 |
|
|
status = rtems_task_wake_after( SPW_LINK_WAIT ); // monitor the link each 100ms |
389 |
|
|
status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status |
390 |
|
|
watchdog_reload(); |
391 |
|
|
} |
392 |
|
|
|
393 |
|
|
spacewire_read_statistics(); |
394 |
|
|
status = spacewire_stop_and_start_link( fdSPW ); |
395 |
|
|
|
396 |
|
|
if (status != RTEMS_SUCCESSFUL) |
397 |
|
|
{ |
398 |
|
|
PRINTF1("in LINK *** ERR link not started %d\n", status) |
399 |
|
|
} |
400 |
|
|
else |
401 |
|
|
{ |
402 |
|
|
PRINTF("in LINK *** OK link started\n") |
403 |
|
|
} |
404 |
|
|
|
405 |
|
|
// restart the SPIQ task |
406 |
|
|
status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 ); |
407 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
408 |
|
|
PRINTF("in SPIQ *** ERR restarting SPIQ Task\n") |
409 |
|
|
} |
410 |
|
|
|
411 |
|
|
// restart RECV and SEND |
412 |
|
|
status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 ); |
413 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
414 |
|
|
PRINTF("in SPIQ *** ERR restarting SEND Task\n") |
415 |
|
|
} |
416 |
|
|
status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 ); |
417 |
|
|
if ( status != RTEMS_SUCCESSFUL ) { |
418 |
|
|
PRINTF("in SPIQ *** ERR restarting RECV Task\n") |
419 |
|
|
} |
420 |
|
|
} |
421 |
|
|
} |
422 |
|
|
|
423 |
|
|
//**************** |
424 |
|
|
// OTHER FUNCTIONS |
425 |
|
1 |
int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);] |
426 |
|
|
{ |
427 |
|
|
/** This function opens the SpaceWire link. |
428 |
|
|
* |
429 |
|
|
* @return a valid file descriptor in case of success, -1 in case of a failure |
430 |
|
|
* |
431 |
|
|
*/ |
432 |
|
|
rtems_status_code status; |
433 |
|
|
|
434 |
|
1 |
status = RTEMS_SUCCESSFUL; |
435 |
|
|
|
436 |
|
1 |
fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware |
437 |
✓✗ |
1 |
if ( fdSPW < 0 ) { |
438 |
|
|
PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno) |
439 |
|
|
} |
440 |
|
|
else |
441 |
|
|
{ |
442 |
|
1 |
status = RTEMS_SUCCESSFUL; |
443 |
|
|
} |
444 |
|
|
|
445 |
|
1 |
return status; |
446 |
|
|
} |
447 |
|
|
|
448 |
|
1 |
int spacewire_start_link( int fd ) |
449 |
|
|
{ |
450 |
|
|
rtems_status_code status; |
451 |
|
|
|
452 |
|
1 |
status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started |
453 |
|
|
// -1 default hardcoded driver timeout |
454 |
|
|
|
455 |
|
1 |
return status; |
456 |
|
|
} |
457 |
|
|
|
458 |
|
|
int spacewire_stop_and_start_link( int fd ) |
459 |
|
|
{ |
460 |
|
|
rtems_status_code status; |
461 |
|
|
|
462 |
|
|
status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0 |
463 |
|
|
status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started |
464 |
|
|
// -1 default hardcoded driver timeout |
465 |
|
|
|
466 |
|
|
return status; |
467 |
|
|
} |
468 |
|
|
|
469 |
|
1 |
int spacewire_configure_link( int fd ) |
470 |
|
|
{ |
471 |
|
|
/** This function configures the SpaceWire link. |
472 |
|
|
* |
473 |
|
|
* @return GR-RTEMS-DRIVER directive status codes: |
474 |
|
|
* - 22 EINVAL - Null pointer or an out of range value was given as the argument. |
475 |
|
|
* - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode. |
476 |
|
|
* - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used. |
477 |
|
|
* - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up. |
478 |
|
|
* - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers. |
479 |
|
|
* - 5 EIO - Error when writing to grswp hardware registers. |
480 |
|
|
* - 2 ENOENT - No such file or directory |
481 |
|
|
*/ |
482 |
|
|
|
483 |
|
|
rtems_status_code status; |
484 |
|
|
|
485 |
|
1 |
spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force |
486 |
|
1 |
spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration |
487 |
|
|
spw_ioctl_packetsize packetsize; |
488 |
|
|
|
489 |
|
1 |
packetsize.rxsize = SPW_RXSIZE; |
490 |
|
1 |
packetsize.txdsize = SPW_TXDSIZE; |
491 |
|
1 |
packetsize.txhsize = SPW_TXHSIZE; |
492 |
|
|
|
493 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception |
494 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
495 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n") |
496 |
|
|
} |
497 |
|
|
// |
498 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a |
499 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
500 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs |
501 |
|
|
} |
502 |
|
|
// |
503 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts |
504 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
505 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n") |
506 |
|
|
} |
507 |
|
|
// |
508 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit |
509 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
510 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") |
511 |
|
|
} |
512 |
|
|
// |
513 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks |
514 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
515 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n") |
516 |
|
|
} |
517 |
|
|
// |
518 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available |
519 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
520 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n") |
521 |
|
|
} |
522 |
|
|
// |
523 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, CONF_TCODE_CTRL); // [Time Rx : Time Tx : Link error : Tick-out IRQ] |
524 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
525 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n") |
526 |
|
|
} |
527 |
|
|
// |
528 |
|
1 |
status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize |
529 |
|
|
if (status!=RTEMS_SUCCESSFUL) { |
530 |
|
|
PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n") |
531 |
|
|
} |
532 |
|
|
|
533 |
|
1 |
return status; |
534 |
|
|
} |
535 |
|
|
|
536 |
|
|
int spacewire_several_connect_attemps( void ) |
537 |
|
|
{ |
538 |
|
|
/** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver. |
539 |
|
|
* |
540 |
|
|
* @return RTEMS directive status code: |
541 |
|
|
* - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s. |
542 |
|
|
* - RTEMS_SUCCESSFUL is returned if the link is up before the timeout. |
543 |
|
|
* |
544 |
|
|
*/ |
545 |
|
|
|
546 |
|
|
rtems_status_code status_spw; |
547 |
|
|
rtems_status_code status; |
548 |
|
|
int i; |
549 |
|
|
|
550 |
|
|
status_spw = RTEMS_SUCCESSFUL; |
551 |
|
|
|
552 |
|
|
i = 0; |
553 |
|
|
while (i < SY_LFR_DPU_CONNECT_ATTEMPT) |
554 |
|
|
{ |
555 |
|
|
PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i); |
556 |
|
|
|
557 |
|
|
// CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM |
558 |
|
|
|
559 |
|
|
status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms |
560 |
|
|
|
561 |
|
|
status_spw = spacewire_stop_and_start_link( fdSPW ); |
562 |
|
|
|
563 |
|
|
if ( status_spw != RTEMS_SUCCESSFUL ) |
564 |
|
|
{ |
565 |
|
|
i = i + 1; |
566 |
|
|
PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw); |
567 |
|
|
} |
568 |
|
|
else |
569 |
|
|
{ |
570 |
|
|
i = SY_LFR_DPU_CONNECT_ATTEMPT; |
571 |
|
|
} |
572 |
|
|
} |
573 |
|
|
|
574 |
|
|
return status_spw; |
575 |
|
|
} |
576 |
|
|
|
577 |
|
1 |
void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force |
578 |
|
|
{ |
579 |
|
|
/** This function sets the [N]o [P]ort force bit of the GRSPW control register. |
580 |
|
|
* |
581 |
|
|
* @param val is the value, 0 or 1, used to set the value of the NP bit. |
582 |
|
|
* @param regAddr is the address of the GRSPW control register. |
583 |
|
|
* |
584 |
|
|
* NP is the bit 20 of the GRSPW control register. |
585 |
|
|
* |
586 |
|
|
*/ |
587 |
|
|
|
588 |
|
1 |
unsigned int *spwptr = (unsigned int*) regAddr; |
589 |
|
|
|
590 |
✓✗ |
1 |
if (val == 1) { |
591 |
|
1 |
*spwptr = *spwptr | SPW_BIT_NP; // [NP] set the No port force bit |
592 |
|
|
} |
593 |
✗✓ |
1 |
if (val== 0) { |
594 |
|
|
*spwptr = *spwptr & SPW_BIT_NP_MASK; |
595 |
|
|
} |
596 |
|
1 |
} |
597 |
|
|
|
598 |
|
1 |
void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable |
599 |
|
|
{ |
600 |
|
|
/** This function sets the [R]MAP [E]nable bit of the GRSPW control register. |
601 |
|
|
* |
602 |
|
|
* @param val is the value, 0 or 1, used to set the value of the RE bit. |
603 |
|
|
* @param regAddr is the address of the GRSPW control register. |
604 |
|
|
* |
605 |
|
|
* RE is the bit 16 of the GRSPW control register. |
606 |
|
|
* |
607 |
|
|
*/ |
608 |
|
|
|
609 |
|
1 |
unsigned int *spwptr = (unsigned int*) regAddr; |
610 |
|
|
|
611 |
✓✗ |
1 |
if (val == 1) |
612 |
|
|
{ |
613 |
|
1 |
*spwptr = *spwptr | SPW_BIT_RE; // [RE] set the RMAP Enable bit |
614 |
|
|
} |
615 |
✗✓ |
1 |
if (val== 0) |
616 |
|
|
{ |
617 |
|
|
*spwptr = *spwptr & SPW_BIT_RE_MASK; |
618 |
|
|
} |
619 |
|
1 |
} |
620 |
|
|
|
621 |
|
151 |
void spacewire_read_statistics( void ) |
622 |
|
|
{ |
623 |
|
|
/** This function reads the SpaceWire statistics from the grspw RTEMS driver. |
624 |
|
|
* |
625 |
|
|
* @param void |
626 |
|
|
* |
627 |
|
|
* @return void |
628 |
|
|
* |
629 |
|
|
* Once they are read, the counters are stored in a global variable used during the building of the |
630 |
|
|
* HK packets. |
631 |
|
|
* |
632 |
|
|
*/ |
633 |
|
|
|
634 |
|
|
rtems_status_code status; |
635 |
|
|
spw_stats current; |
636 |
|
|
|
637 |
|
151 |
memset(¤t, 0, sizeof(spw_stats)); |
638 |
|
|
|
639 |
|
151 |
spacewire_get_last_error(); |
640 |
|
|
|
641 |
|
|
// read the current statistics |
642 |
|
151 |
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, ¤t ); |
643 |
|
|
|
644 |
|
|
// clear the counters |
645 |
|
151 |
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS ); |
646 |
|
|
|
647 |
|
|
// rx_eep_err |
648 |
|
151 |
grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err; |
649 |
|
|
// rx_truncated |
650 |
|
151 |
grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated; |
651 |
|
|
// parity_err |
652 |
|
151 |
grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err; |
653 |
|
|
// escape_err |
654 |
|
151 |
grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err; |
655 |
|
|
// credit_err |
656 |
|
151 |
grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err; |
657 |
|
|
// write_sync_err |
658 |
|
151 |
grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err; |
659 |
|
|
// disconnect_err |
660 |
|
151 |
grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err; |
661 |
|
|
// early_ep |
662 |
|
151 |
grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep; |
663 |
|
|
// invalid_address |
664 |
|
151 |
grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address; |
665 |
|
|
// packets_sent |
666 |
|
151 |
grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent; |
667 |
|
|
// packets_received |
668 |
|
151 |
grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received; |
669 |
|
|
|
670 |
|
151 |
} |
671 |
|
|
|
672 |
|
151 |
void spacewire_get_last_error( void ) |
673 |
|
|
{ |
674 |
|
|
static spw_stats previous = {0}; |
675 |
|
|
spw_stats current; |
676 |
|
|
rtems_status_code status; |
677 |
|
|
|
678 |
|
|
unsigned int hk_lfr_last_er_rid; |
679 |
|
|
unsigned char hk_lfr_last_er_code; |
680 |
|
|
int coarseTime; |
681 |
|
|
int fineTime; |
682 |
|
|
unsigned char update_hk_lfr_last_er; |
683 |
|
|
|
684 |
|
151 |
memset(¤t, 0, sizeof(spw_stats)); |
685 |
|
151 |
hk_lfr_last_er_rid = INIT_CHAR; |
686 |
|
151 |
hk_lfr_last_er_code = INIT_CHAR; |
687 |
|
151 |
update_hk_lfr_last_er = INIT_CHAR; |
688 |
|
|
|
689 |
|
151 |
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, ¤t ); |
690 |
|
|
|
691 |
|
|
// get current time |
692 |
|
151 |
coarseTime = time_management_regs->coarse_time; |
693 |
|
151 |
fineTime = time_management_regs->fine_time; |
694 |
|
|
|
695 |
|
|
// tx_link_err *** no code associated to this field |
696 |
|
|
// rx_rmap_header_crc_err *** LE *** in HK |
697 |
✗✓ |
151 |
if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err) |
698 |
|
|
{ |
699 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
700 |
|
|
hk_lfr_last_er_code = CODE_HEADER_CRC; |
701 |
|
|
update_hk_lfr_last_er = 1; |
702 |
|
|
} |
703 |
|
|
// rx_rmap_data_crc_err *** LE *** NOT IN HK |
704 |
✗✓ |
151 |
if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err) |
705 |
|
|
{ |
706 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
707 |
|
|
hk_lfr_last_er_code = CODE_DATA_CRC; |
708 |
|
|
update_hk_lfr_last_er = 1; |
709 |
|
|
} |
710 |
|
|
// rx_eep_err |
711 |
✗✓ |
151 |
if (previous.rx_eep_err != current.rx_eep_err) |
712 |
|
|
{ |
713 |
|
|
hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; |
714 |
|
|
hk_lfr_last_er_code = CODE_EEP; |
715 |
|
|
update_hk_lfr_last_er = 1; |
716 |
|
|
} |
717 |
|
|
// rx_truncated |
718 |
✗✓ |
151 |
if (previous.rx_truncated != current.rx_truncated) |
719 |
|
|
{ |
720 |
|
|
hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; |
721 |
|
|
hk_lfr_last_er_code = CODE_RX_TOO_BIG; |
722 |
|
|
update_hk_lfr_last_er = 1; |
723 |
|
|
} |
724 |
|
|
// parity_err |
725 |
✗✓ |
151 |
if (previous.parity_err != current.parity_err) |
726 |
|
|
{ |
727 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
728 |
|
|
hk_lfr_last_er_code = CODE_PARITY; |
729 |
|
|
update_hk_lfr_last_er = 1; |
730 |
|
|
} |
731 |
|
|
// escape_err |
732 |
✗✓ |
151 |
if (previous.parity_err != current.parity_err) |
733 |
|
|
{ |
734 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
735 |
|
|
hk_lfr_last_er_code = CODE_ESCAPE; |
736 |
|
|
update_hk_lfr_last_er = 1; |
737 |
|
|
} |
738 |
|
|
// credit_err |
739 |
✗✓ |
151 |
if (previous.credit_err != current.credit_err) |
740 |
|
|
{ |
741 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
742 |
|
|
hk_lfr_last_er_code = CODE_CREDIT; |
743 |
|
|
update_hk_lfr_last_er = 1; |
744 |
|
|
} |
745 |
|
|
// write_sync_err |
746 |
✗✓ |
151 |
if (previous.write_sync_err != current.write_sync_err) |
747 |
|
|
{ |
748 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
749 |
|
|
hk_lfr_last_er_code = CODE_WRITE_SYNC; |
750 |
|
|
update_hk_lfr_last_er = 1; |
751 |
|
|
} |
752 |
|
|
// disconnect_err |
753 |
✗✓ |
151 |
if (previous.disconnect_err != current.disconnect_err) |
754 |
|
|
{ |
755 |
|
|
hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; |
756 |
|
|
hk_lfr_last_er_code = CODE_DISCONNECT; |
757 |
|
|
update_hk_lfr_last_er = 1; |
758 |
|
|
} |
759 |
|
|
// early_ep |
760 |
✗✓ |
151 |
if (previous.early_ep != current.early_ep) |
761 |
|
|
{ |
762 |
|
|
hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; |
763 |
|
|
hk_lfr_last_er_code = CODE_EARLY_EOP_EEP; |
764 |
|
|
update_hk_lfr_last_er = 1; |
765 |
|
|
} |
766 |
|
|
// invalid_address |
767 |
✗✓ |
151 |
if (previous.invalid_address != current.invalid_address) |
768 |
|
|
{ |
769 |
|
|
hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; |
770 |
|
|
hk_lfr_last_er_code = CODE_INVALID_ADDRESS; |
771 |
|
|
update_hk_lfr_last_er = 1; |
772 |
|
|
} |
773 |
|
|
|
774 |
|
|
// if a field has changed, update the hk_last_er fields |
775 |
✗✓ |
151 |
if (update_hk_lfr_last_er == 1) |
776 |
|
|
{ |
777 |
|
|
update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code ); |
778 |
|
|
} |
779 |
|
|
|
780 |
|
151 |
previous = current; |
781 |
|
151 |
} |
782 |
|
|
|
783 |
|
154 |
void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code) |
784 |
|
|
{ |
785 |
|
|
unsigned char *coarseTimePtr; |
786 |
|
|
unsigned char *fineTimePtr; |
787 |
|
|
|
788 |
|
154 |
coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time; |
789 |
|
154 |
fineTimePtr = (unsigned char*) &time_management_regs->fine_time; |
790 |
|
|
|
791 |
|
154 |
housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & BYTE0_MASK) >> SHIFT_1_BYTE ); |
792 |
|
154 |
housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & BYTE1_MASK); |
793 |
|
154 |
housekeeping_packet.hk_lfr_last_er_code = code; |
794 |
|
154 |
housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0]; |
795 |
|
154 |
housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1]; |
796 |
|
154 |
housekeeping_packet.hk_lfr_last_er_time[BYTE_2] = coarseTimePtr[BYTE_2]; |
797 |
|
154 |
housekeeping_packet.hk_lfr_last_er_time[BYTE_3] = coarseTimePtr[BYTE_3]; |
798 |
|
154 |
housekeeping_packet.hk_lfr_last_er_time[BYTE_4] = fineTimePtr[BYTE_2]; |
799 |
|
154 |
housekeeping_packet.hk_lfr_last_er_time[BYTE_5] = fineTimePtr[BYTE_3]; |
800 |
|
154 |
} |
801 |
|
|
|
802 |
|
151 |
void update_hk_with_grspw_stats( void ) |
803 |
|
|
{ |
804 |
|
|
//**************************** |
805 |
|
|
// DPU_SPACEWIRE_IF_STATISTICS |
806 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> SHIFT_1_BYTE); |
807 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received); |
808 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> SHIFT_1_BYTE); |
809 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent); |
810 |
|
|
|
811 |
|
|
//****************************************** |
812 |
|
|
// ERROR COUNTERS / SPACEWIRE / LOW SEVERITY |
813 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err; |
814 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err; |
815 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err; |
816 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err; |
817 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err; |
818 |
|
|
|
819 |
|
|
//********************************************* |
820 |
|
|
// ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY |
821 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep; |
822 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address; |
823 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err; |
824 |
|
151 |
housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated; |
825 |
|
151 |
} |
826 |
|
|
|
827 |
|
151 |
void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 ) |
828 |
|
|
{ |
829 |
|
|
unsigned int *statusRegisterPtr; |
830 |
|
|
unsigned char linkState; |
831 |
|
|
|
832 |
|
151 |
statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER); |
833 |
|
151 |
linkState = |
834 |
|
151 |
(unsigned char) ( ( (*statusRegisterPtr) >> SPW_LINK_STAT_POS) & STATUS_WORD_LINK_STATE_BITS); // [0000 0111] |
835 |
|
|
|
836 |
|
151 |
*hk_lfr_status_word_0 = *hk_lfr_status_word_0 & STATUS_WORD_LINK_STATE_MASK; // [1111 1000] set link state to 0 |
837 |
|
|
|
838 |
|
151 |
*hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state |
839 |
|
151 |
} |
840 |
|
|
|
841 |
|
308 |
void increase_unsigned_char_counter( unsigned char *counter ) |
842 |
|
|
{ |
843 |
|
|
// update the number of valid timecodes that have been received |
844 |
✗✓ |
308 |
if (*counter == UINT8_MAX) |
845 |
|
|
{ |
846 |
|
|
*counter = 0; |
847 |
|
|
} |
848 |
|
|
else |
849 |
|
|
{ |
850 |
|
308 |
*counter = *counter + 1; |
851 |
|
|
} |
852 |
|
308 |
} |
853 |
|
|
|
854 |
|
154 |
unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr) |
855 |
|
|
{ |
856 |
|
|
/** This function checks the coherency between the incoming timecode and the last valid timecode. |
857 |
|
|
* |
858 |
|
|
* @param currentTimecodeCtr is the incoming timecode |
859 |
|
|
* |
860 |
|
|
* @return returned codes:: |
861 |
|
|
* - LFR_DEFAULT |
862 |
|
|
* - LFR_SUCCESSFUL |
863 |
|
|
* |
864 |
|
|
*/ |
865 |
|
|
|
866 |
|
|
static unsigned char firstTickout = 1; |
867 |
|
|
unsigned char ret; |
868 |
|
|
|
869 |
|
154 |
ret = LFR_DEFAULT; |
870 |
|
|
|
871 |
✓✓ |
154 |
if (firstTickout == 0) |
872 |
|
|
{ |
873 |
✓✓ |
153 |
if (currentTimecodeCtr == 0) |
874 |
|
|
{ |
875 |
✓✗ |
2 |
if (previousTimecodeCtr == SPW_TIMECODE_MAX) |
876 |
|
|
{ |
877 |
|
2 |
ret = LFR_SUCCESSFUL; |
878 |
|
|
} |
879 |
|
|
else |
880 |
|
|
{ |
881 |
|
|
ret = LFR_DEFAULT; |
882 |
|
|
} |
883 |
|
|
} |
884 |
|
|
else |
885 |
|
|
{ |
886 |
✓✗ |
151 |
if (currentTimecodeCtr == (previousTimecodeCtr +1)) |
887 |
|
|
{ |
888 |
|
151 |
ret = LFR_SUCCESSFUL; |
889 |
|
|
} |
890 |
|
|
else |
891 |
|
|
{ |
892 |
|
|
ret = LFR_DEFAULT; |
893 |
|
|
} |
894 |
|
|
} |
895 |
|
|
} |
896 |
|
|
else |
897 |
|
|
{ |
898 |
|
1 |
firstTickout = 0; |
899 |
|
1 |
ret = LFR_SUCCESSFUL; |
900 |
|
|
} |
901 |
|
|
|
902 |
|
154 |
return ret; |
903 |
|
|
} |
904 |
|
|
|
905 |
|
154 |
unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime) |
906 |
|
|
{ |
907 |
|
|
unsigned int ret; |
908 |
|
|
|
909 |
|
154 |
ret = LFR_DEFAULT; |
910 |
|
|
|
911 |
✗✓ |
154 |
if (timecode == internalTime) |
912 |
|
|
{ |
913 |
|
|
ret = LFR_SUCCESSFUL; |
914 |
|
|
} |
915 |
|
|
else |
916 |
|
|
{ |
917 |
|
154 |
ret = LFR_DEFAULT; |
918 |
|
|
} |
919 |
|
|
|
920 |
|
154 |
return ret; |
921 |
|
|
} |
922 |
|
|
|
923 |
|
154 |
void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc ) |
924 |
|
|
{ |
925 |
|
|
// a tickout has been emitted, perform actions on the incoming timecode |
926 |
|
|
|
927 |
|
|
unsigned char incomingTimecode; |
928 |
|
|
unsigned char updateTime; |
929 |
|
|
unsigned char internalTime; |
930 |
|
|
rtems_status_code status; |
931 |
|
|
|
932 |
|
154 |
incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK); |
933 |
|
154 |
updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK; |
934 |
|
154 |
internalTime = time_management_regs->coarse_time & TIMECODE_MASK; |
935 |
|
|
|
936 |
|
154 |
housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode; |
937 |
|
|
|
938 |
|
|
// update the number of tickout that have been generated |
939 |
|
154 |
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt ); |
940 |
|
|
|
941 |
|
|
//************************** |
942 |
|
|
// HK_LFR_TIMECODE_ERRONEOUS |
943 |
|
|
// MISSING and INVALID are handled by the timecode_timer_routine service routine |
944 |
✗✓ |
154 |
if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT) |
945 |
|
|
{ |
946 |
|
|
// this is unexpected but a tickout could have been raised despite of the timecode being erroneous |
947 |
|
|
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous ); |
948 |
|
|
update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS ); |
949 |
|
|
} |
950 |
|
|
|
951 |
|
|
//************************ |
952 |
|
|
// HK_LFR_TIME_TIMECODE_IT |
953 |
|
|
// check the coherency between the SpaceWire timecode and the Internal Time |
954 |
✓✗ |
154 |
if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT) |
955 |
|
|
{ |
956 |
|
154 |
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it ); |
957 |
|
154 |
update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT ); |
958 |
|
|
} |
959 |
|
|
|
960 |
|
|
//******************** |
961 |
|
|
// HK_LFR_TIMECODE_CTR |
962 |
|
|
// check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370 |
963 |
✗✓ |
154 |
if (oneTcLfrUpdateTimeReceived == 1) |
964 |
|
|
{ |
965 |
|
|
if ( incomingTimecode != updateTime ) |
966 |
|
|
{ |
967 |
|
|
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr ); |
968 |
|
|
update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR ); |
969 |
|
|
} |
970 |
|
|
} |
971 |
|
|
|
972 |
|
|
// launch the timecode timer to detect missing or invalid timecodes |
973 |
|
154 |
previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value |
974 |
|
154 |
status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL ); |
975 |
✗✓ |
154 |
if (status != RTEMS_SUCCESSFUL) |
976 |
|
|
{ |
977 |
|
|
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 ); |
978 |
|
|
} |
979 |
|
154 |
} |
980 |
|
|
|
981 |
|
|
rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data ) |
982 |
|
|
{ |
983 |
|
|
static unsigned char initStep = 1; |
984 |
|
|
|
985 |
|
|
unsigned char currentTimecodeCtr; |
986 |
|
|
|
987 |
|
|
currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK); |
988 |
|
|
|
989 |
|
|
if (initStep == 1) |
990 |
|
|
{ |
991 |
|
|
if (currentTimecodeCtr == previousTimecodeCtr) |
992 |
|
|
{ |
993 |
|
|
//************************ |
994 |
|
|
// HK_LFR_TIMECODE_MISSING |
995 |
|
|
// the timecode value has not changed, no valid timecode has been received, the timecode is MISSING |
996 |
|
|
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing ); |
997 |
|
|
update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING ); |
998 |
|
|
} |
999 |
|
|
else if (currentTimecodeCtr == (previousTimecodeCtr+1)) |
1000 |
|
|
{ |
1001 |
|
|
// the timecode value has changed and the value is valid, this is unexpected because |
1002 |
|
|
// the timer should not have fired, the timecode_irq_handler should have been raised |
1003 |
|
|
} |
1004 |
|
|
else |
1005 |
|
|
{ |
1006 |
|
|
//************************ |
1007 |
|
|
// HK_LFR_TIMECODE_INVALID |
1008 |
|
|
// the timecode value has changed and the value is not valid, no tickout has been generated |
1009 |
|
|
// this is why the timer has fired |
1010 |
|
|
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid ); |
1011 |
|
|
update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID ); |
1012 |
|
|
} |
1013 |
|
|
} |
1014 |
|
|
else |
1015 |
|
|
{ |
1016 |
|
|
initStep = 1; |
1017 |
|
|
//************************ |
1018 |
|
|
// HK_LFR_TIMECODE_MISSING |
1019 |
|
|
increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing ); |
1020 |
|
|
update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING ); |
1021 |
|
|
} |
1022 |
|
|
|
1023 |
|
|
rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 ); |
1024 |
|
|
} |
1025 |
|
|
|
1026 |
|
1 |
void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header ) |
1027 |
|
|
{ |
1028 |
|
1 |
header->targetLogicalAddress = CCSDS_DESTINATION_ID; |
1029 |
|
1 |
header->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
1030 |
|
1 |
header->reserved = DEFAULT_RESERVED; |
1031 |
|
1 |
header->userApplication = CCSDS_USER_APP; |
1032 |
|
1 |
header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE; |
1033 |
|
1 |
header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT; |
1034 |
|
1 |
header->packetLength[0] = INIT_CHAR; |
1035 |
|
1 |
header->packetLength[1] = INIT_CHAR; |
1036 |
|
|
// DATA FIELD HEADER |
1037 |
|
1 |
header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; |
1038 |
|
1 |
header->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
1039 |
|
1 |
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype |
1040 |
|
1 |
header->destinationID = TM_DESTINATION_ID_GROUND; |
1041 |
|
1 |
header->time[BYTE_0] = INIT_CHAR; |
1042 |
|
1 |
header->time[BYTE_1] = INIT_CHAR; |
1043 |
|
1 |
header->time[BYTE_2] = INIT_CHAR; |
1044 |
|
1 |
header->time[BYTE_3] = INIT_CHAR; |
1045 |
|
1 |
header->time[BYTE_4] = INIT_CHAR; |
1046 |
|
1 |
header->time[BYTE_5] = INIT_CHAR; |
1047 |
|
|
// AUXILIARY DATA HEADER |
1048 |
|
1 |
header->sid = INIT_CHAR; |
1049 |
|
1 |
header->pa_bia_status_info = DEFAULT_HKBIA; |
1050 |
|
1 |
header->blkNr[0] = INIT_CHAR; |
1051 |
|
1 |
header->blkNr[1] = INIT_CHAR; |
1052 |
|
1 |
} |
1053 |
|
|
|
1054 |
|
1 |
void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header ) |
1055 |
|
|
{ |
1056 |
|
1 |
header->targetLogicalAddress = CCSDS_DESTINATION_ID; |
1057 |
|
1 |
header->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
1058 |
|
1 |
header->reserved = DEFAULT_RESERVED; |
1059 |
|
1 |
header->userApplication = CCSDS_USER_APP; |
1060 |
|
1 |
header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE); |
1061 |
|
1 |
header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); |
1062 |
|
1 |
header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; |
1063 |
|
1 |
header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; |
1064 |
|
1 |
header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE); |
1065 |
|
1 |
header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); |
1066 |
|
|
// DATA FIELD HEADER |
1067 |
|
1 |
header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; |
1068 |
|
1 |
header->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
1069 |
|
1 |
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype |
1070 |
|
1 |
header->destinationID = TM_DESTINATION_ID_GROUND; |
1071 |
|
1 |
header->time[BYTE_0] = INIT_CHAR; |
1072 |
|
1 |
header->time[BYTE_1] = INIT_CHAR; |
1073 |
|
1 |
header->time[BYTE_2] = INIT_CHAR; |
1074 |
|
1 |
header->time[BYTE_3] = INIT_CHAR; |
1075 |
|
1 |
header->time[BYTE_4] = INIT_CHAR; |
1076 |
|
1 |
header->time[BYTE_5] = INIT_CHAR; |
1077 |
|
|
// AUXILIARY DATA HEADER |
1078 |
|
1 |
header->sid = INIT_CHAR; |
1079 |
|
1 |
header->pa_bia_status_info = DEFAULT_HKBIA; |
1080 |
|
1 |
header->pktCnt = PKTCNT_SWF; // PKT_CNT |
1081 |
|
1 |
header->pktNr = INIT_CHAR; |
1082 |
|
1 |
header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE); |
1083 |
|
1 |
header->blkNr[1] = (unsigned char) (BLK_NR_CWF ); |
1084 |
|
1 |
} |
1085 |
|
|
|
1086 |
|
1 |
void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header ) |
1087 |
|
|
{ |
1088 |
|
1 |
header->targetLogicalAddress = CCSDS_DESTINATION_ID; |
1089 |
|
1 |
header->protocolIdentifier = CCSDS_PROTOCOLE_ID; |
1090 |
|
1 |
header->reserved = DEFAULT_RESERVED; |
1091 |
|
1 |
header->userApplication = CCSDS_USER_APP; |
1092 |
|
1 |
header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE); |
1093 |
|
1 |
header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); |
1094 |
|
1 |
header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; |
1095 |
|
1 |
header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; |
1096 |
|
1 |
header->packetLength[0] = INIT_CHAR; |
1097 |
|
1 |
header->packetLength[1] = INIT_CHAR; |
1098 |
|
|
// DATA FIELD HEADER |
1099 |
|
1 |
header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; |
1100 |
|
1 |
header->serviceType = TM_TYPE_LFR_SCIENCE; // service type |
1101 |
|
1 |
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype |
1102 |
|
1 |
header->destinationID = TM_DESTINATION_ID_GROUND; |
1103 |
|
1 |
header->time[BYTE_0] = INIT_CHAR; |
1104 |
|
1 |
header->time[BYTE_1] = INIT_CHAR; |
1105 |
|
1 |
header->time[BYTE_2] = INIT_CHAR; |
1106 |
|
1 |
header->time[BYTE_3] = INIT_CHAR; |
1107 |
|
1 |
header->time[BYTE_4] = INIT_CHAR; |
1108 |
|
1 |
header->time[BYTE_5] = INIT_CHAR; |
1109 |
|
|
// AUXILIARY DATA HEADER |
1110 |
|
1 |
header->sid = INIT_CHAR; |
1111 |
|
1 |
header->pa_bia_status_info = INIT_CHAR; |
1112 |
|
1 |
header->pa_lfr_pkt_cnt_asm = INIT_CHAR; |
1113 |
|
1 |
header->pa_lfr_pkt_nr_asm = INIT_CHAR; |
1114 |
|
1 |
header->pa_lfr_asm_blk_nr[0] = INIT_CHAR; |
1115 |
|
1 |
header->pa_lfr_asm_blk_nr[1] = INIT_CHAR; |
1116 |
|
1 |
} |
1117 |
|
|
|
1118 |
|
|
int spw_send_waveform_CWF( ring_node *ring_node_to_send, |
1119 |
|
|
Header_TM_LFR_SCIENCE_CWF_t *header ) |
1120 |
|
|
{ |
1121 |
|
|
/** This function sends CWF CCSDS packets (F2, F1 or F0). |
1122 |
|
|
* |
1123 |
|
|
* @param waveform points to the buffer containing the data that will be send. |
1124 |
|
|
* @param sid is the source identifier of the data that will be sent. |
1125 |
|
|
* @param headerCWF points to a table of headers that have been prepared for the data transmission. |
1126 |
|
|
* @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures |
1127 |
|
|
* contain information to setup the transmission of the data packets. |
1128 |
|
|
* |
1129 |
|
|
* One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. |
1130 |
|
|
* |
1131 |
|
|
*/ |
1132 |
|
|
|
1133 |
|
|
unsigned int i; |
1134 |
|
|
int ret; |
1135 |
|
|
unsigned int coarseTime; |
1136 |
|
|
unsigned int fineTime; |
1137 |
|
|
rtems_status_code status; |
1138 |
|
|
spw_ioctl_pkt_send spw_ioctl_send_CWF; |
1139 |
|
|
int *dataPtr; |
1140 |
|
|
unsigned char sid; |
1141 |
|
|
|
1142 |
|
|
spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF; |
1143 |
|
|
spw_ioctl_send_CWF.options = 0; |
1144 |
|
|
|
1145 |
|
|
ret = LFR_DEFAULT; |
1146 |
|
|
sid = (unsigned char) ring_node_to_send->sid; |
1147 |
|
|
|
1148 |
|
|
coarseTime = ring_node_to_send->coarseTime; |
1149 |
|
|
fineTime = ring_node_to_send->fineTime; |
1150 |
|
|
dataPtr = (int*) ring_node_to_send->buffer_address; |
1151 |
|
|
|
1152 |
|
|
header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> SHIFT_1_BYTE); |
1153 |
|
|
header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); |
1154 |
|
|
header->pa_bia_status_info = pa_bia_status_info; |
1155 |
|
|
header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
1156 |
|
|
header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> SHIFT_1_BYTE); |
1157 |
|
|
header->blkNr[1] = (unsigned char) (BLK_NR_CWF ); |
1158 |
|
|
|
1159 |
|
|
for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform |
1160 |
|
|
{ |
1161 |
|
|
spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ]; |
1162 |
|
|
spw_ioctl_send_CWF.hdr = (char*) header; |
1163 |
|
|
// BUILD THE DATA |
1164 |
|
|
spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK; |
1165 |
|
|
|
1166 |
|
|
// SET PACKET SEQUENCE CONTROL |
1167 |
|
|
increment_seq_counter_source_id( header->packetSequenceControl, sid ); |
1168 |
|
|
|
1169 |
|
|
// SET SID |
1170 |
|
|
header->sid = sid; |
1171 |
|
|
|
1172 |
|
|
// SET PACKET TIME |
1173 |
|
|
compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime); |
1174 |
|
|
// |
1175 |
|
|
header->time[0] = header->acquisitionTime[0]; |
1176 |
|
|
header->time[1] = header->acquisitionTime[1]; |
1177 |
|
|
header->time[BYTE_2] = header->acquisitionTime[BYTE_2]; |
1178 |
|
|
header->time[BYTE_3] = header->acquisitionTime[BYTE_3]; |
1179 |
|
|
header->time[BYTE_4] = header->acquisitionTime[BYTE_4]; |
1180 |
|
|
header->time[BYTE_5] = header->acquisitionTime[BYTE_5]; |
1181 |
|
|
|
1182 |
|
|
// SET PACKET ID |
1183 |
|
|
if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) ) |
1184 |
|
|
{ |
1185 |
|
|
header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> SHIFT_1_BYTE); |
1186 |
|
|
header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2); |
1187 |
|
|
} |
1188 |
|
|
else |
1189 |
|
|
{ |
1190 |
|
|
header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE); |
1191 |
|
|
header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); |
1192 |
|
|
} |
1193 |
|
|
|
1194 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF ); |
1195 |
|
|
if (status != RTEMS_SUCCESSFUL) { |
1196 |
|
|
ret = LFR_DEFAULT; |
1197 |
|
|
} |
1198 |
|
|
} |
1199 |
|
|
|
1200 |
|
|
return ret; |
1201 |
|
|
} |
1202 |
|
|
|
1203 |
|
|
int spw_send_waveform_SWF( ring_node *ring_node_to_send, |
1204 |
|
|
Header_TM_LFR_SCIENCE_SWF_t *header ) |
1205 |
|
|
{ |
1206 |
|
|
/** This function sends SWF CCSDS packets (F2, F1 or F0). |
1207 |
|
|
* |
1208 |
|
|
* @param waveform points to the buffer containing the data that will be send. |
1209 |
|
|
* @param sid is the source identifier of the data that will be sent. |
1210 |
|
|
* @param headerSWF points to a table of headers that have been prepared for the data transmission. |
1211 |
|
|
* @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures |
1212 |
|
|
* contain information to setup the transmission of the data packets. |
1213 |
|
|
* |
1214 |
|
|
* One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. |
1215 |
|
|
* |
1216 |
|
|
*/ |
1217 |
|
|
|
1218 |
|
|
unsigned int i; |
1219 |
|
|
int ret; |
1220 |
|
|
unsigned int coarseTime; |
1221 |
|
|
unsigned int fineTime; |
1222 |
|
|
rtems_status_code status; |
1223 |
|
|
spw_ioctl_pkt_send spw_ioctl_send_SWF; |
1224 |
|
|
int *dataPtr; |
1225 |
|
|
unsigned char sid; |
1226 |
|
|
|
1227 |
|
|
spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF; |
1228 |
|
|
spw_ioctl_send_SWF.options = 0; |
1229 |
|
|
|
1230 |
|
|
ret = LFR_DEFAULT; |
1231 |
|
|
|
1232 |
|
|
coarseTime = ring_node_to_send->coarseTime; |
1233 |
|
|
fineTime = ring_node_to_send->fineTime; |
1234 |
|
|
dataPtr = (int*) ring_node_to_send->buffer_address; |
1235 |
|
|
sid = ring_node_to_send->sid; |
1236 |
|
|
|
1237 |
|
|
header->pa_bia_status_info = pa_bia_status_info; |
1238 |
|
|
header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
1239 |
|
|
|
1240 |
|
|
for (i=0; i<PKTCNT_SWF; i++) // send waveform |
1241 |
|
|
{ |
1242 |
|
|
spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ]; |
1243 |
|
|
spw_ioctl_send_SWF.hdr = (char*) header; |
1244 |
|
|
|
1245 |
|
|
// SET PACKET SEQUENCE CONTROL |
1246 |
|
|
increment_seq_counter_source_id( header->packetSequenceControl, sid ); |
1247 |
|
|
|
1248 |
|
|
// SET PACKET LENGTH AND BLKNR |
1249 |
|
|
if (i == (PKTCNT_SWF-1)) |
1250 |
|
|
{ |
1251 |
|
|
spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK; |
1252 |
|
|
header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> SHIFT_1_BYTE); |
1253 |
|
|
header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 ); |
1254 |
|
|
header->blkNr[0] = (unsigned char) (BLK_NR_224 >> SHIFT_1_BYTE); |
1255 |
|
|
header->blkNr[1] = (unsigned char) (BLK_NR_224 ); |
1256 |
|
|
} |
1257 |
|
|
else |
1258 |
|
|
{ |
1259 |
|
|
spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK; |
1260 |
|
|
header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> SHIFT_1_BYTE); |
1261 |
|
|
header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 ); |
1262 |
|
|
header->blkNr[0] = (unsigned char) (BLK_NR_304 >> SHIFT_1_BYTE); |
1263 |
|
|
header->blkNr[1] = (unsigned char) (BLK_NR_304 ); |
1264 |
|
|
} |
1265 |
|
|
|
1266 |
|
|
// SET PACKET TIME |
1267 |
|
|
compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime ); |
1268 |
|
|
// |
1269 |
|
|
header->time[BYTE_0] = header->acquisitionTime[BYTE_0]; |
1270 |
|
|
header->time[BYTE_1] = header->acquisitionTime[BYTE_1]; |
1271 |
|
|
header->time[BYTE_2] = header->acquisitionTime[BYTE_2]; |
1272 |
|
|
header->time[BYTE_3] = header->acquisitionTime[BYTE_3]; |
1273 |
|
|
header->time[BYTE_4] = header->acquisitionTime[BYTE_4]; |
1274 |
|
|
header->time[BYTE_5] = header->acquisitionTime[BYTE_5]; |
1275 |
|
|
|
1276 |
|
|
// SET SID |
1277 |
|
|
header->sid = sid; |
1278 |
|
|
|
1279 |
|
|
// SET PKTNR |
1280 |
|
|
header->pktNr = i+1; // PKT_NR |
1281 |
|
|
|
1282 |
|
|
// SEND PACKET |
1283 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF ); |
1284 |
|
|
if (status != RTEMS_SUCCESSFUL) { |
1285 |
|
|
ret = LFR_DEFAULT; |
1286 |
|
|
} |
1287 |
|
|
} |
1288 |
|
|
|
1289 |
|
|
return ret; |
1290 |
|
|
} |
1291 |
|
|
|
1292 |
|
|
int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, |
1293 |
|
|
Header_TM_LFR_SCIENCE_CWF_t *header ) |
1294 |
|
|
{ |
1295 |
|
|
/** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data. |
1296 |
|
|
* |
1297 |
|
|
* @param waveform points to the buffer containing the data that will be send. |
1298 |
|
|
* @param headerCWF points to a table of headers that have been prepared for the data transmission. |
1299 |
|
|
* @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures |
1300 |
|
|
* contain information to setup the transmission of the data packets. |
1301 |
|
|
* |
1302 |
|
|
* By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer |
1303 |
|
|
* from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks. |
1304 |
|
|
* |
1305 |
|
|
*/ |
1306 |
|
|
|
1307 |
|
|
unsigned int i; |
1308 |
|
|
int ret; |
1309 |
|
|
unsigned int coarseTime; |
1310 |
|
|
unsigned int fineTime; |
1311 |
|
|
rtems_status_code status; |
1312 |
|
|
spw_ioctl_pkt_send spw_ioctl_send_CWF; |
1313 |
|
|
char *dataPtr; |
1314 |
|
|
unsigned char sid; |
1315 |
|
|
|
1316 |
|
|
spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF; |
1317 |
|
|
spw_ioctl_send_CWF.options = 0; |
1318 |
|
|
|
1319 |
|
|
ret = LFR_DEFAULT; |
1320 |
|
|
sid = ring_node_to_send->sid; |
1321 |
|
|
|
1322 |
|
|
coarseTime = ring_node_to_send->coarseTime; |
1323 |
|
|
fineTime = ring_node_to_send->fineTime; |
1324 |
|
|
dataPtr = (char*) ring_node_to_send->buffer_address; |
1325 |
|
|
|
1326 |
|
|
header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> SHIFT_1_BYTE); |
1327 |
|
|
header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 ); |
1328 |
|
|
header->pa_bia_status_info = pa_bia_status_info; |
1329 |
|
|
header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
1330 |
|
|
header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> SHIFT_1_BYTE); |
1331 |
|
|
header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 ); |
1332 |
|
|
|
1333 |
|
|
//********************* |
1334 |
|
|
// SEND CWF3_light DATA |
1335 |
|
|
for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform |
1336 |
|
|
{ |
1337 |
|
|
spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ]; |
1338 |
|
|
spw_ioctl_send_CWF.hdr = (char*) header; |
1339 |
|
|
// BUILD THE DATA |
1340 |
|
|
spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK; |
1341 |
|
|
|
1342 |
|
|
// SET PACKET SEQUENCE COUNTER |
1343 |
|
|
increment_seq_counter_source_id( header->packetSequenceControl, sid ); |
1344 |
|
|
|
1345 |
|
|
// SET SID |
1346 |
|
|
header->sid = sid; |
1347 |
|
|
|
1348 |
|
|
// SET PACKET TIME |
1349 |
|
|
compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime ); |
1350 |
|
|
// |
1351 |
|
|
header->time[BYTE_0] = header->acquisitionTime[BYTE_0]; |
1352 |
|
|
header->time[BYTE_1] = header->acquisitionTime[BYTE_1]; |
1353 |
|
|
header->time[BYTE_2] = header->acquisitionTime[BYTE_2]; |
1354 |
|
|
header->time[BYTE_3] = header->acquisitionTime[BYTE_3]; |
1355 |
|
|
header->time[BYTE_4] = header->acquisitionTime[BYTE_4]; |
1356 |
|
|
header->time[BYTE_5] = header->acquisitionTime[BYTE_5]; |
1357 |
|
|
|
1358 |
|
|
// SET PACKET ID |
1359 |
|
|
header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> SHIFT_1_BYTE); |
1360 |
|
|
header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); |
1361 |
|
|
|
1362 |
|
|
// SEND PACKET |
1363 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF ); |
1364 |
|
|
if (status != RTEMS_SUCCESSFUL) { |
1365 |
|
|
ret = LFR_DEFAULT; |
1366 |
|
|
} |
1367 |
|
|
} |
1368 |
|
|
|
1369 |
|
|
return ret; |
1370 |
|
|
} |
1371 |
|
|
|
1372 |
|
|
void spw_send_asm_f0( ring_node *ring_node_to_send, |
1373 |
|
|
Header_TM_LFR_SCIENCE_ASM_t *header ) |
1374 |
|
|
{ |
1375 |
|
|
unsigned int i; |
1376 |
|
|
unsigned int length = 0; |
1377 |
|
|
rtems_status_code status; |
1378 |
|
|
unsigned int sid; |
1379 |
|
|
float *spectral_matrix; |
1380 |
|
|
int coarseTime; |
1381 |
|
|
int fineTime; |
1382 |
|
|
spw_ioctl_pkt_send spw_ioctl_send_ASM; |
1383 |
|
|
|
1384 |
|
|
sid = ring_node_to_send->sid; |
1385 |
|
|
spectral_matrix = (float*) ring_node_to_send->buffer_address; |
1386 |
|
|
coarseTime = ring_node_to_send->coarseTime; |
1387 |
|
|
fineTime = ring_node_to_send->fineTime; |
1388 |
|
|
|
1389 |
|
|
header->pa_bia_status_info = pa_bia_status_info; |
1390 |
|
|
header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
1391 |
|
|
|
1392 |
|
|
for (i=0; i<PKTCNT_ASM; i++) |
1393 |
|
|
{ |
1394 |
|
|
if ((i==0) || (i==1)) |
1395 |
|
|
{ |
1396 |
|
|
spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1; |
1397 |
|
|
spw_ioctl_send_ASM.data = (char *) &spectral_matrix[ |
1398 |
|
|
( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM ) |
1399 |
|
|
]; |
1400 |
|
|
length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1; |
1401 |
|
|
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; |
1402 |
|
|
header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> SHIFT_1_BYTE ); // BLK_NR MSB |
1403 |
|
|
header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB |
1404 |
|
|
} |
1405 |
|
|
else |
1406 |
|
|
{ |
1407 |
|
|
spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2; |
1408 |
|
|
spw_ioctl_send_ASM.data = (char*) &spectral_matrix[ |
1409 |
|
|
( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM ) |
1410 |
|
|
]; |
1411 |
|
|
length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2; |
1412 |
|
|
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; |
1413 |
|
|
header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> SHIFT_1_BYTE ); // BLK_NR MSB |
1414 |
|
|
header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB |
1415 |
|
|
} |
1416 |
|
|
|
1417 |
|
|
spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM; |
1418 |
|
|
spw_ioctl_send_ASM.hdr = (char *) header; |
1419 |
|
|
spw_ioctl_send_ASM.options = 0; |
1420 |
|
|
|
1421 |
|
|
// (2) BUILD THE HEADER |
1422 |
|
|
increment_seq_counter_source_id( header->packetSequenceControl, sid ); |
1423 |
|
|
header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE); |
1424 |
|
|
header->packetLength[1] = (unsigned char) (length); |
1425 |
|
|
header->sid = (unsigned char) sid; // SID |
1426 |
|
|
header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM; |
1427 |
|
|
header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); |
1428 |
|
|
|
1429 |
|
|
// (3) SET PACKET TIME |
1430 |
|
|
header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES); |
1431 |
|
|
header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES); |
1432 |
|
|
header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE); |
1433 |
|
|
header->time[BYTE_3] = (unsigned char) (coarseTime); |
1434 |
|
|
header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE); |
1435 |
|
|
header->time[BYTE_5] = (unsigned char) (fineTime); |
1436 |
|
|
// |
1437 |
|
|
header->acquisitionTime[BYTE_0] = header->time[BYTE_0]; |
1438 |
|
|
header->acquisitionTime[BYTE_1] = header->time[BYTE_1]; |
1439 |
|
|
header->acquisitionTime[BYTE_2] = header->time[BYTE_2]; |
1440 |
|
|
header->acquisitionTime[BYTE_3] = header->time[BYTE_3]; |
1441 |
|
|
header->acquisitionTime[BYTE_4] = header->time[BYTE_4]; |
1442 |
|
|
header->acquisitionTime[BYTE_5] = header->time[BYTE_5]; |
1443 |
|
|
|
1444 |
|
|
// (4) SEND PACKET |
1445 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); |
1446 |
|
|
if (status != RTEMS_SUCCESSFUL) { |
1447 |
|
|
PRINTF1("in ASM_send *** ERR %d\n", (int) status) |
1448 |
|
|
} |
1449 |
|
|
} |
1450 |
|
|
} |
1451 |
|
|
|
1452 |
|
|
void spw_send_asm_f1( ring_node *ring_node_to_send, |
1453 |
|
|
Header_TM_LFR_SCIENCE_ASM_t *header ) |
1454 |
|
|
{ |
1455 |
|
|
unsigned int i; |
1456 |
|
|
unsigned int length = 0; |
1457 |
|
|
rtems_status_code status; |
1458 |
|
|
unsigned int sid; |
1459 |
|
|
float *spectral_matrix; |
1460 |
|
|
int coarseTime; |
1461 |
|
|
int fineTime; |
1462 |
|
|
spw_ioctl_pkt_send spw_ioctl_send_ASM; |
1463 |
|
|
|
1464 |
|
|
sid = ring_node_to_send->sid; |
1465 |
|
|
spectral_matrix = (float*) ring_node_to_send->buffer_address; |
1466 |
|
|
coarseTime = ring_node_to_send->coarseTime; |
1467 |
|
|
fineTime = ring_node_to_send->fineTime; |
1468 |
|
|
|
1469 |
|
|
header->pa_bia_status_info = pa_bia_status_info; |
1470 |
|
|
header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
1471 |
|
|
|
1472 |
|
|
for (i=0; i<PKTCNT_ASM; i++) |
1473 |
|
|
{ |
1474 |
|
|
if ((i==0) || (i==1)) |
1475 |
|
|
{ |
1476 |
|
|
spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1; |
1477 |
|
|
spw_ioctl_send_ASM.data = (char *) &spectral_matrix[ |
1478 |
|
|
( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM ) |
1479 |
|
|
]; |
1480 |
|
|
length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1; |
1481 |
|
|
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; |
1482 |
|
|
header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> SHIFT_1_BYTE ); // BLK_NR MSB |
1483 |
|
|
header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB |
1484 |
|
|
} |
1485 |
|
|
else |
1486 |
|
|
{ |
1487 |
|
|
spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2; |
1488 |
|
|
spw_ioctl_send_ASM.data = (char*) &spectral_matrix[ |
1489 |
|
|
( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM ) |
1490 |
|
|
]; |
1491 |
|
|
length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2; |
1492 |
|
|
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; |
1493 |
|
|
header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> SHIFT_1_BYTE ); // BLK_NR MSB |
1494 |
|
|
header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB |
1495 |
|
|
} |
1496 |
|
|
|
1497 |
|
|
spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM; |
1498 |
|
|
spw_ioctl_send_ASM.hdr = (char *) header; |
1499 |
|
|
spw_ioctl_send_ASM.options = 0; |
1500 |
|
|
|
1501 |
|
|
// (2) BUILD THE HEADER |
1502 |
|
|
increment_seq_counter_source_id( header->packetSequenceControl, sid ); |
1503 |
|
|
header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE); |
1504 |
|
|
header->packetLength[1] = (unsigned char) (length); |
1505 |
|
|
header->sid = (unsigned char) sid; // SID |
1506 |
|
|
header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM; |
1507 |
|
|
header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); |
1508 |
|
|
|
1509 |
|
|
// (3) SET PACKET TIME |
1510 |
|
|
header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES); |
1511 |
|
|
header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES); |
1512 |
|
|
header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE); |
1513 |
|
|
header->time[BYTE_3] = (unsigned char) (coarseTime); |
1514 |
|
|
header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE); |
1515 |
|
|
header->time[BYTE_5] = (unsigned char) (fineTime); |
1516 |
|
|
// |
1517 |
|
|
header->acquisitionTime[BYTE_0] = header->time[BYTE_0]; |
1518 |
|
|
header->acquisitionTime[BYTE_1] = header->time[BYTE_1]; |
1519 |
|
|
header->acquisitionTime[BYTE_2] = header->time[BYTE_2]; |
1520 |
|
|
header->acquisitionTime[BYTE_3] = header->time[BYTE_3]; |
1521 |
|
|
header->acquisitionTime[BYTE_4] = header->time[BYTE_4]; |
1522 |
|
|
header->acquisitionTime[BYTE_5] = header->time[BYTE_5]; |
1523 |
|
|
|
1524 |
|
|
// (4) SEND PACKET |
1525 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); |
1526 |
|
|
if (status != RTEMS_SUCCESSFUL) { |
1527 |
|
|
PRINTF1("in ASM_send *** ERR %d\n", (int) status) |
1528 |
|
|
} |
1529 |
|
|
} |
1530 |
|
|
} |
1531 |
|
|
|
1532 |
|
|
/** |
1533 |
|
|
* @brief spw_send_asm_f2 Sends an ASM packet at F2 over spacewire |
1534 |
|
|
* @param ring_node_to_send node pointing to the actual buffer to send |
1535 |
|
|
* @param header |
1536 |
|
|
*/ |
1537 |
|
|
void spw_send_asm_f2( ring_node *ring_node_to_send, |
1538 |
|
|
Header_TM_LFR_SCIENCE_ASM_t *header ) |
1539 |
|
|
{ |
1540 |
|
|
unsigned int i; |
1541 |
|
|
unsigned int length = 0; |
1542 |
|
|
rtems_status_code status; |
1543 |
|
|
unsigned int sid; |
1544 |
|
|
float *spectral_matrix; |
1545 |
|
|
int coarseTime; |
1546 |
|
|
int fineTime; |
1547 |
|
|
spw_ioctl_pkt_send spw_ioctl_send_ASM; |
1548 |
|
|
|
1549 |
|
|
sid = ring_node_to_send->sid; |
1550 |
|
|
spectral_matrix = (float*) ring_node_to_send->buffer_address; |
1551 |
|
|
coarseTime = ring_node_to_send->coarseTime; |
1552 |
|
|
fineTime = ring_node_to_send->fineTime; |
1553 |
|
|
|
1554 |
|
|
header->pa_bia_status_info = pa_bia_status_info; |
1555 |
|
|
header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; |
1556 |
|
|
|
1557 |
|
|
for (i=0; i<PKTCNT_ASM; i++) |
1558 |
|
|
{ |
1559 |
|
|
|
1560 |
|
|
spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT; |
1561 |
|
|
spw_ioctl_send_ASM.data = (char *) &spectral_matrix[ |
1562 |
|
|
( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) |
1563 |
|
|
]; |
1564 |
|
|
length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2; |
1565 |
|
|
header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; |
1566 |
|
|
header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> SHIFT_1_BYTE ); // BLK_NR MSB |
1567 |
|
|
header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB |
1568 |
|
|
|
1569 |
|
|
spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM; |
1570 |
|
|
spw_ioctl_send_ASM.hdr = (char *) header; |
1571 |
|
|
spw_ioctl_send_ASM.options = 0; |
1572 |
|
|
|
1573 |
|
|
// (2) BUILD THE HEADER |
1574 |
|
|
increment_seq_counter_source_id( header->packetSequenceControl, sid ); |
1575 |
|
|
header->packetLength[0] = (unsigned char) (length >> SHIFT_1_BYTE); |
1576 |
|
|
header->packetLength[1] = (unsigned char) (length); |
1577 |
|
|
header->sid = (unsigned char) sid; // SID |
1578 |
|
|
header->pa_lfr_pkt_cnt_asm = PKTCNT_ASM; |
1579 |
|
|
header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); |
1580 |
|
|
|
1581 |
|
|
// (3) SET PACKET TIME |
1582 |
|
|
header->time[BYTE_0] = (unsigned char) (coarseTime >> SHIFT_3_BYTES); |
1583 |
|
|
header->time[BYTE_1] = (unsigned char) (coarseTime >> SHIFT_2_BYTES); |
1584 |
|
|
header->time[BYTE_2] = (unsigned char) (coarseTime >> SHIFT_1_BYTE); |
1585 |
|
|
header->time[BYTE_3] = (unsigned char) (coarseTime); |
1586 |
|
|
header->time[BYTE_4] = (unsigned char) (fineTime >> SHIFT_1_BYTE); |
1587 |
|
|
header->time[BYTE_5] = (unsigned char) (fineTime); |
1588 |
|
|
// |
1589 |
|
|
header->acquisitionTime[BYTE_0] = header->time[BYTE_0]; |
1590 |
|
|
header->acquisitionTime[BYTE_1] = header->time[BYTE_1]; |
1591 |
|
|
header->acquisitionTime[BYTE_2] = header->time[BYTE_2]; |
1592 |
|
|
header->acquisitionTime[BYTE_3] = header->time[BYTE_3]; |
1593 |
|
|
header->acquisitionTime[BYTE_4] = header->time[BYTE_4]; |
1594 |
|
|
header->acquisitionTime[BYTE_5] = header->time[BYTE_5]; |
1595 |
|
|
|
1596 |
|
|
// (4) SEND PACKET |
1597 |
|
|
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); |
1598 |
|
|
if (status != RTEMS_SUCCESSFUL) { |
1599 |
|
|
PRINTF1("in ASM_send *** ERR %d\n", (int) status) |
1600 |
|
|
} |
1601 |
|
|
} |
1602 |
|
|
} |
1603 |
|
|
|
1604 |
|
|
/** |
1605 |
|
|
* @brief spw_send_k_dump Sends k coefficients dump packet over spacewire |
1606 |
|
|
* @param ring_node_to_send node pointing to the actual buffer to send |
1607 |
|
|
*/ |
1608 |
|
|
void spw_send_k_dump( ring_node *ring_node_to_send ) |
1609 |
|
|
{ |
1610 |
|
|
rtems_status_code status; |
1611 |
|
|
Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump; |
1612 |
|
|
unsigned int packetLength; |
1613 |
|
|
unsigned int size; |
1614 |
|
|
|
1615 |
|
|
PRINTF("spw_send_k_dump\n") |
1616 |
|
|
|
1617 |
|
|
kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address; |
1618 |
|
|
|
1619 |
|
|
packetLength = (kcoefficients_dump->packetLength[0] * CONST_256) + kcoefficients_dump->packetLength[1]; |
1620 |
|
|
|
1621 |
|
|
size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES; |
1622 |
|
|
|
1623 |
|
|
PRINTF2("packetLength %d, size %d\n", packetLength, size ) |
1624 |
|
|
|
1625 |
|
|
status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size ); |
1626 |
|
|
|
1627 |
|
|
if (status == -1){ |
1628 |
|
|
PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size) |
1629 |
|
|
} |
1630 |
|
|
|
1631 |
|
|
ring_node_to_send->status = INIT_CHAR; |
1632 |
|
|
} |