##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » USERS » JEANDET » LFR_FSW
RSS

Fork of HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Clone from https://hephaistos.lpp.polytechnique.fr/rhodecode/HG_REPOSITORIES/LPP/INSTRUMENTATION/SOLO_LFR/DEV_PLE

Bug 408, service_subtype changed to 6 instead of 3 in...
paul -
r201:35056c087191 R3
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,1283 +1,1283
1 1 /** Functions related to the SpaceWire interface.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * A group of functions to handle SpaceWire transmissions:
7 7 * - configuration of the SpaceWire link
8 8 * - SpaceWire related interruption requests processing
9 9 * - transmission of TeleMetry packets by a dedicated RTEMS task
10 10 * - reception of TeleCommands by a dedicated RTEMS task
11 11 *
12 12 */
13 13
14 14 #include "fsw_spacewire.h"
15 15
16 16 rtems_name semq_name;
17 17 rtems_id semq_id;
18 18
19 19 //*****************
20 20 // waveform headers
21 21 Header_TM_LFR_SCIENCE_CWF_t headerCWF;
22 22 Header_TM_LFR_SCIENCE_SWF_t headerSWF;
23 23 Header_TM_LFR_SCIENCE_ASM_t headerASM;
24 24
25 25 //***********
26 26 // RTEMS TASK
27 27 rtems_task spiq_task(rtems_task_argument unused)
28 28 {
29 29 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
30 30 *
31 31 * @param unused is the starting argument of the RTEMS task
32 32 *
33 33 */
34 34
35 35 rtems_event_set event_out;
36 36 rtems_status_code status;
37 37 int linkStatus;
38 38
39 39 BOOT_PRINTF("in SPIQ *** \n")
40 40
41 41 while(true){
42 42 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
43 43 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
44 44
45 45 // [0] SUSPEND RECV AND SEND TASKS
46 46 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
47 47 if ( status != RTEMS_SUCCESSFUL ) {
48 48 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
49 49 }
50 50 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
51 51 if ( status != RTEMS_SUCCESSFUL ) {
52 52 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
53 53 }
54 54
55 55 // [1] CHECK THE LINK
56 56 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
57 57 if ( linkStatus != 5) {
58 58 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
59 59 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
60 60 }
61 61
62 62 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
63 63 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
64 64 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
65 65 {
66 66 spacewire_compute_stats_offsets();
67 67 status = spacewire_reset_link( );
68 68 }
69 69 else // [2.b] in run state, start the link
70 70 {
71 71 status = spacewire_stop_and_start_link( fdSPW ); // start the link
72 72 if ( status != RTEMS_SUCCESSFUL)
73 73 {
74 74 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
75 75 }
76 76 }
77 77
78 78 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
79 79 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
80 80 {
81 81 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
82 82 if ( status != RTEMS_SUCCESSFUL ) {
83 83 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
84 84 }
85 85 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
86 86 if ( status != RTEMS_SUCCESSFUL ) {
87 87 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
88 88 }
89 89 }
90 90 else // [3.b] the link is not in run state, go in STANDBY mode
91 91 {
92 92 status = enter_mode( LFR_MODE_STANDBY, 0 );
93 93 if ( status != RTEMS_SUCCESSFUL ) {
94 94 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
95 95 }
96 96 // wake the WTDG task up to wait for the link recovery
97 97 status = rtems_event_send ( Task_id[TASKID_WTDG], RTEMS_EVENT_0 );
98 98 status = rtems_task_suspend( RTEMS_SELF );
99 99 }
100 100 }
101 101 }
102 102
103 103 rtems_task recv_task( rtems_task_argument unused )
104 104 {
105 105 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
106 106 *
107 107 * @param unused is the starting argument of the RTEMS task
108 108 *
109 109 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
110 110 * 1. It reads the incoming data.
111 111 * 2. Launches the acceptance procedure.
112 112 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
113 113 *
114 114 */
115 115
116 116 int len;
117 117 ccsdsTelecommandPacket_t currentTC;
118 118 unsigned char computed_CRC[ 2 ];
119 119 unsigned char currentTC_LEN_RCV[ 2 ];
120 120 unsigned char destinationID;
121 121 unsigned int estimatedPacketLength;
122 122 unsigned int parserCode;
123 123 rtems_status_code status;
124 124 rtems_id queue_recv_id;
125 125 rtems_id queue_send_id;
126 126
127 127 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
128 128
129 129 status = get_message_queue_id_recv( &queue_recv_id );
130 130 if (status != RTEMS_SUCCESSFUL)
131 131 {
132 132 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
133 133 }
134 134
135 135 status = get_message_queue_id_send( &queue_send_id );
136 136 if (status != RTEMS_SUCCESSFUL)
137 137 {
138 138 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
139 139 }
140 140
141 141 BOOT_PRINTF("in RECV *** \n")
142 142
143 143 while(1)
144 144 {
145 145 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
146 146 if (len == -1){ // error during the read call
147 147 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
148 148 }
149 149 else {
150 150 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
151 151 PRINTF("in RECV *** packet lenght too short\n")
152 152 }
153 153 else {
154 154 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
155 155 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
156 156 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
157 157 // CHECK THE TC
158 158 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
159 159 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
160 160 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
161 161 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
162 162 || (parserCode == WRONG_SRC_ID) )
163 163 { // send TM_LFR_TC_EXE_CORRUPTED
164 164 PRINTF1("TC corrupted received, with code: %d\n", parserCode)
165 165 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
166 166 &&
167 167 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
168 168 )
169 169 {
170 170 if ( parserCode == WRONG_SRC_ID )
171 171 {
172 172 destinationID = SID_TC_GROUND;
173 173 }
174 174 else
175 175 {
176 176 destinationID = currentTC.sourceID;
177 177 }
178 178 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
179 179 computed_CRC, currentTC_LEN_RCV,
180 180 destinationID );
181 181 }
182 182 }
183 183 else
184 184 { // send valid TC to the action launcher
185 185 status = rtems_message_queue_send( queue_recv_id, &currentTC,
186 186 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
187 187 }
188 188 }
189 189 }
190 190
191 191 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
192 192
193 193 }
194 194 }
195 195
196 196 rtems_task send_task( rtems_task_argument argument)
197 197 {
198 198 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
199 199 *
200 200 * @param unused is the starting argument of the RTEMS task
201 201 *
202 202 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
203 203 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
204 204 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
205 205 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
206 206 * data it contains.
207 207 *
208 208 */
209 209
210 210 rtems_status_code status; // RTEMS status code
211 211 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
212 212 ring_node *incomingRingNodePtr;
213 213 int ring_node_address;
214 214 char *charPtr;
215 215 spw_ioctl_pkt_send *spw_ioctl_send;
216 216 size_t size; // size of the incoming TC packet
217 217 rtems_id queue_send_id;
218 218 unsigned int sid;
219 219
220 220 incomingRingNodePtr = NULL;
221 221 ring_node_address = 0;
222 222 charPtr = (char *) &ring_node_address;
223 223 sid = 0;
224 224
225 225 init_header_cwf( &headerCWF );
226 226 init_header_swf( &headerSWF );
227 227 init_header_asm( &headerASM );
228 228
229 229 status = get_message_queue_id_send( &queue_send_id );
230 230 if (status != RTEMS_SUCCESSFUL)
231 231 {
232 232 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
233 233 }
234 234
235 235 BOOT_PRINTF("in SEND *** \n")
236 236
237 237 while(1)
238 238 {
239 239 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
240 240 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
241 241
242 242 if (status!=RTEMS_SUCCESSFUL)
243 243 {
244 244 PRINTF1("in SEND *** (1) ERR = %d\n", status)
245 245 }
246 246 else
247 247 {
248 248 if ( size == sizeof(ring_node*) )
249 249 {
250 250 charPtr[0] = incomingData[0];
251 251 charPtr[1] = incomingData[1];
252 252 charPtr[2] = incomingData[2];
253 253 charPtr[3] = incomingData[3];
254 254 incomingRingNodePtr = (ring_node*) ring_node_address;
255 255 sid = incomingRingNodePtr->sid;
256 256 if ( (sid==SID_NORM_CWF_LONG_F3)
257 257 || (sid==SID_BURST_CWF_F2 )
258 258 || (sid==SID_SBM1_CWF_F1 )
259 259 || (sid==SID_SBM2_CWF_F2 ))
260 260 {
261 261 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
262 262 }
263 263 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
264 264 {
265 265 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
266 266 }
267 267 else if ( (sid==SID_NORM_CWF_F3) )
268 268 {
269 269 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
270 270 }
271 271 else if (sid==SID_NORM_ASM_F0)
272 272 {
273 273 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
274 274 }
275 275 else if (sid==SID_NORM_ASM_F1)
276 276 {
277 277 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
278 278 }
279 279 else if (sid==SID_NORM_ASM_F2)
280 280 {
281 281 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
282 282 }
283 283 else if ( sid==TM_CODE_K_DUMP )
284 284 {
285 285 spw_send_k_dump( incomingRingNodePtr );
286 286 }
287 287 else
288 288 {
289 289 printf("unexpected sid = %d\n", sid);
290 290 }
291 291 }
292 292 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
293 293 {
294 294 status = write( fdSPW, incomingData, size );
295 295 if (status == -1){
296 296 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
297 297 }
298 298 }
299 299 else // the incoming message is a spw_ioctl_pkt_send structure
300 300 {
301 301 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
302 302 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
303 303 if (status == -1){
304 304 printf("size = %d, %x, %x, %x, %x, %x\n",
305 305 size,
306 306 incomingData[0],
307 307 incomingData[1],
308 308 incomingData[2],
309 309 incomingData[3],
310 310 incomingData[4]);
311 311 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
312 312 }
313 313 }
314 314 }
315 315
316 316 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
317 317
318 318 }
319 319 }
320 320
321 321 rtems_task wtdg_task( rtems_task_argument argument )
322 322 {
323 323 rtems_event_set event_out;
324 324 rtems_status_code status;
325 325 int linkStatus;
326 326
327 327 BOOT_PRINTF("in WTDG ***\n")
328 328
329 329 while(1)
330 330 {
331 331 // wait for an RTEMS_EVENT
332 332 rtems_event_receive( RTEMS_EVENT_0,
333 333 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
334 334 PRINTF("in WTDG *** wait for the link\n")
335 335 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
336 336 while( linkStatus != 5) // wait for the link
337 337 {
338 338 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
339 339 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
340 340 }
341 341
342 342 status = spacewire_stop_and_start_link( fdSPW );
343 343
344 344 if (status != RTEMS_SUCCESSFUL)
345 345 {
346 346 PRINTF1("in WTDG *** ERR link not started %d\n", status)
347 347 }
348 348 else
349 349 {
350 350 PRINTF("in WTDG *** OK link started\n")
351 351 }
352 352
353 353 // restart the SPIQ task
354 354 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
355 355 if ( status != RTEMS_SUCCESSFUL ) {
356 356 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
357 357 }
358 358
359 359 // restart RECV and SEND
360 360 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
361 361 if ( status != RTEMS_SUCCESSFUL ) {
362 362 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
363 363 }
364 364 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
365 365 if ( status != RTEMS_SUCCESSFUL ) {
366 366 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
367 367 }
368 368 }
369 369 }
370 370
371 371 //****************
372 372 // OTHER FUNCTIONS
373 373 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
374 374 {
375 375 /** This function opens the SpaceWire link.
376 376 *
377 377 * @return a valid file descriptor in case of success, -1 in case of a failure
378 378 *
379 379 */
380 380 rtems_status_code status;
381 381
382 382 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
383 383 if ( fdSPW < 0 ) {
384 384 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
385 385 }
386 386 else
387 387 {
388 388 status = RTEMS_SUCCESSFUL;
389 389 }
390 390
391 391 return status;
392 392 }
393 393
394 394 int spacewire_start_link( int fd )
395 395 {
396 396 rtems_status_code status;
397 397
398 398 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
399 399 // -1 default hardcoded driver timeout
400 400
401 401 return status;
402 402 }
403 403
404 404 int spacewire_stop_and_start_link( int fd )
405 405 {
406 406 rtems_status_code status;
407 407
408 408 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
409 409 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
410 410 // -1 default hardcoded driver timeout
411 411
412 412 return status;
413 413 }
414 414
415 415 int spacewire_configure_link( int fd )
416 416 {
417 417 /** This function configures the SpaceWire link.
418 418 *
419 419 * @return GR-RTEMS-DRIVER directive status codes:
420 420 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
421 421 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
422 422 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
423 423 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
424 424 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
425 425 * - 5 EIO - Error when writing to grswp hardware registers.
426 426 * - 2 ENOENT - No such file or directory
427 427 */
428 428
429 429 rtems_status_code status;
430 430
431 431 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
432 432 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
433 433
434 434 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
435 435 if (status!=RTEMS_SUCCESSFUL) {
436 436 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
437 437 }
438 438 //
439 439 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
440 440 if (status!=RTEMS_SUCCESSFUL) {
441 441 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
442 442 }
443 443 //
444 444 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
445 445 if (status!=RTEMS_SUCCESSFUL) {
446 446 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
447 447 }
448 448 //
449 449 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
450 450 if (status!=RTEMS_SUCCESSFUL) {
451 451 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
452 452 }
453 453 //
454 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
455 455 if (status!=RTEMS_SUCCESSFUL) {
456 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
457 457 }
458 458 //
459 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
460 460 if (status!=RTEMS_SUCCESSFUL) {
461 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
462 462 }
463 463 //
464 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
465 465 if (status!=RTEMS_SUCCESSFUL) {
466 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
467 467 }
468 468
469 469 return status;
470 470 }
471 471
472 472 int spacewire_reset_link( void )
473 473 {
474 474 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
475 475 *
476 476 * @return RTEMS directive status code:
477 477 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
478 478 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
479 479 *
480 480 */
481 481
482 482 rtems_status_code status_spw;
483 483 rtems_status_code status;
484 484 int i;
485 485
486 486 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
487 487 {
488 488 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
489 489
490 490 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
491 491
492 492 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
493 493
494 494 status_spw = spacewire_stop_and_start_link( fdSPW );
495 495 if ( status_spw != RTEMS_SUCCESSFUL )
496 496 {
497 497 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
498 498 }
499 499
500 500 if ( status_spw == RTEMS_SUCCESSFUL)
501 501 {
502 502 break;
503 503 }
504 504 }
505 505
506 506 return status_spw;
507 507 }
508 508
509 509 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
510 510 {
511 511 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
512 512 *
513 513 * @param val is the value, 0 or 1, used to set the value of the NP bit.
514 514 * @param regAddr is the address of the GRSPW control register.
515 515 *
516 516 * NP is the bit 20 of the GRSPW control register.
517 517 *
518 518 */
519 519
520 520 unsigned int *spwptr = (unsigned int*) regAddr;
521 521
522 522 if (val == 1) {
523 523 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
524 524 }
525 525 if (val== 0) {
526 526 *spwptr = *spwptr & 0xffdfffff;
527 527 }
528 528 }
529 529
530 530 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
531 531 {
532 532 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
533 533 *
534 534 * @param val is the value, 0 or 1, used to set the value of the RE bit.
535 535 * @param regAddr is the address of the GRSPW control register.
536 536 *
537 537 * RE is the bit 16 of the GRSPW control register.
538 538 *
539 539 */
540 540
541 541 unsigned int *spwptr = (unsigned int*) regAddr;
542 542
543 543 if (val == 1)
544 544 {
545 545 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
546 546 }
547 547 if (val== 0)
548 548 {
549 549 *spwptr = *spwptr & 0xfffdffff;
550 550 }
551 551 }
552 552
553 553 void spacewire_compute_stats_offsets( void )
554 554 {
555 555 /** This function computes the SpaceWire statistics offsets in case of a SpaceWire related interruption raising.
556 556 *
557 557 * The offsets keep a record of the statistics in case of a reset of the statistics. They are added to the current statistics
558 558 * to keep the counters consistent even after a reset of the SpaceWire driver (the counter are set to zero by the driver when it
559 559 * during the open systel call).
560 560 *
561 561 */
562 562
563 563 spw_stats spacewire_stats_grspw;
564 564 rtems_status_code status;
565 565
566 566 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
567 567
568 568 spacewire_stats_backup.packets_received = spacewire_stats_grspw.packets_received
569 569 + spacewire_stats.packets_received;
570 570 spacewire_stats_backup.packets_sent = spacewire_stats_grspw.packets_sent
571 571 + spacewire_stats.packets_sent;
572 572 spacewire_stats_backup.parity_err = spacewire_stats_grspw.parity_err
573 573 + spacewire_stats.parity_err;
574 574 spacewire_stats_backup.disconnect_err = spacewire_stats_grspw.disconnect_err
575 575 + spacewire_stats.disconnect_err;
576 576 spacewire_stats_backup.escape_err = spacewire_stats_grspw.escape_err
577 577 + spacewire_stats.escape_err;
578 578 spacewire_stats_backup.credit_err = spacewire_stats_grspw.credit_err
579 579 + spacewire_stats.credit_err;
580 580 spacewire_stats_backup.write_sync_err = spacewire_stats_grspw.write_sync_err
581 581 + spacewire_stats.write_sync_err;
582 582 spacewire_stats_backup.rx_rmap_header_crc_err = spacewire_stats_grspw.rx_rmap_header_crc_err
583 583 + spacewire_stats.rx_rmap_header_crc_err;
584 584 spacewire_stats_backup.rx_rmap_data_crc_err = spacewire_stats_grspw.rx_rmap_data_crc_err
585 585 + spacewire_stats.rx_rmap_data_crc_err;
586 586 spacewire_stats_backup.early_ep = spacewire_stats_grspw.early_ep
587 587 + spacewire_stats.early_ep;
588 588 spacewire_stats_backup.invalid_address = spacewire_stats_grspw.invalid_address
589 589 + spacewire_stats.invalid_address;
590 590 spacewire_stats_backup.rx_eep_err = spacewire_stats_grspw.rx_eep_err
591 591 + spacewire_stats.rx_eep_err;
592 592 spacewire_stats_backup.rx_truncated = spacewire_stats_grspw.rx_truncated
593 593 + spacewire_stats.rx_truncated;
594 594 }
595 595
596 596 void spacewire_update_statistics( void )
597 597 {
598 598 rtems_status_code status;
599 599 spw_stats spacewire_stats_grspw;
600 600
601 601 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw );
602 602
603 603 spacewire_stats.packets_received = spacewire_stats_backup.packets_received
604 604 + spacewire_stats_grspw.packets_received;
605 605 spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent
606 606 + spacewire_stats_grspw.packets_sent;
607 607 spacewire_stats.parity_err = spacewire_stats_backup.parity_err
608 608 + spacewire_stats_grspw.parity_err;
609 609 spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err
610 610 + spacewire_stats_grspw.disconnect_err;
611 611 spacewire_stats.escape_err = spacewire_stats_backup.escape_err
612 612 + spacewire_stats_grspw.escape_err;
613 613 spacewire_stats.credit_err = spacewire_stats_backup.credit_err
614 614 + spacewire_stats_grspw.credit_err;
615 615 spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err
616 616 + spacewire_stats_grspw.write_sync_err;
617 617 spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err
618 618 + spacewire_stats_grspw.rx_rmap_header_crc_err;
619 619 spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err
620 620 + spacewire_stats_grspw.rx_rmap_data_crc_err;
621 621 spacewire_stats.early_ep = spacewire_stats_backup.early_ep
622 622 + spacewire_stats_grspw.early_ep;
623 623 spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address
624 624 + spacewire_stats_grspw.invalid_address;
625 625 spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err
626 626 + spacewire_stats_grspw.rx_eep_err;
627 627 spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated
628 628 + spacewire_stats_grspw.rx_truncated;
629 629 //spacewire_stats.tx_link_err;
630 630
631 631 //****************************
632 632 // DPU_SPACEWIRE_IF_STATISTICS
633 633 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8);
634 634 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received);
635 635 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8);
636 636 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent);
637 637 //housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt;
638 638 //housekeeping_packet.hk_lfr_dpu_spw_last_timc;
639 639
640 640 //******************************************
641 641 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
642 642 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err;
643 643 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err;
644 644 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err;
645 645 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err;
646 646 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err;
647 647
648 648 //*********************************************
649 649 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
650 650 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep;
651 651 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address;
652 652 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err;
653 653 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated;
654 654 }
655 655
656 656 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
657 657 {
658 658 // a valid timecode has been received, write it in the HK report
659 659 unsigned int * grspwPtr;
660 660
661 661 grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
662 662
663 663 housekeeping_packet.hk_lfr_dpu_spw_last_timc = (unsigned char) (grspwPtr[0] & 0xff); // [11 1111]
664 664
665 665 // update the number of valid timecodes that have been received
666 666 if (housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt == 255)
667 667 {
668 668 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = 0;
669 669 }
670 670 else
671 671 {
672 672 housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt = housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt + 1;
673 673 }
674 674 }
675 675
676 676 rtems_timer_service_routine user_routine( rtems_id timer_id, void *user_data )
677 677 {
678 678 int linkStatus;
679 679 rtems_status_code status;
680 680
681 681 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
682 682
683 683 if ( linkStatus == 5) {
684 684 PRINTF("in spacewire_reset_link *** link is running\n")
685 685 status = RTEMS_SUCCESSFUL;
686 686 }
687 687 }
688 688
689 689 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
690 690 {
691 691 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
692 692 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
693 693 header->reserved = DEFAULT_RESERVED;
694 694 header->userApplication = CCSDS_USER_APP;
695 695 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
696 696 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
697 697 header->packetLength[0] = 0x00;
698 698 header->packetLength[1] = 0x00;
699 699 // DATA FIELD HEADER
700 700 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
701 701 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
702 702 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
703 703 header->destinationID = TM_DESTINATION_ID_GROUND;
704 704 header->time[0] = 0x00;
705 705 header->time[0] = 0x00;
706 706 header->time[0] = 0x00;
707 707 header->time[0] = 0x00;
708 708 header->time[0] = 0x00;
709 709 header->time[0] = 0x00;
710 710 // AUXILIARY DATA HEADER
711 711 header->sid = 0x00;
712 712 header->hkBIA = DEFAULT_HKBIA;
713 713 header->blkNr[0] = 0x00;
714 714 header->blkNr[1] = 0x00;
715 715 }
716 716
717 717 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
718 718 {
719 719 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
720 720 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
721 721 header->reserved = DEFAULT_RESERVED;
722 722 header->userApplication = CCSDS_USER_APP;
723 723 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
724 724 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
725 725 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
726 726 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
727 727 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
728 728 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
729 729 // DATA FIELD HEADER
730 730 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
731 731 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
732 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
732 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
733 733 header->destinationID = TM_DESTINATION_ID_GROUND;
734 734 header->time[0] = 0x00;
735 735 header->time[0] = 0x00;
736 736 header->time[0] = 0x00;
737 737 header->time[0] = 0x00;
738 738 header->time[0] = 0x00;
739 739 header->time[0] = 0x00;
740 740 // AUXILIARY DATA HEADER
741 741 header->sid = 0x00;
742 742 header->hkBIA = DEFAULT_HKBIA;
743 743 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
744 744 header->pktNr = 0x00;
745 745 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
746 746 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
747 747 }
748 748
749 749 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
750 750 {
751 751 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
752 752 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
753 753 header->reserved = DEFAULT_RESERVED;
754 754 header->userApplication = CCSDS_USER_APP;
755 755 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
756 756 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
757 757 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
758 758 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
759 759 header->packetLength[0] = 0x00;
760 760 header->packetLength[1] = 0x00;
761 761 // DATA FIELD HEADER
762 762 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
763 763 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
764 764 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
765 765 header->destinationID = TM_DESTINATION_ID_GROUND;
766 766 header->time[0] = 0x00;
767 767 header->time[0] = 0x00;
768 768 header->time[0] = 0x00;
769 769 header->time[0] = 0x00;
770 770 header->time[0] = 0x00;
771 771 header->time[0] = 0x00;
772 772 // AUXILIARY DATA HEADER
773 773 header->sid = 0x00;
774 774 header->biaStatusInfo = 0x00;
775 775 header->pa_lfr_pkt_cnt_asm = 0x00;
776 776 header->pa_lfr_pkt_nr_asm = 0x00;
777 777 header->pa_lfr_asm_blk_nr[0] = 0x00;
778 778 header->pa_lfr_asm_blk_nr[1] = 0x00;
779 779 }
780 780
781 781 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
782 782 Header_TM_LFR_SCIENCE_CWF_t *header )
783 783 {
784 784 /** This function sends CWF CCSDS packets (F2, F1 or F0).
785 785 *
786 786 * @param waveform points to the buffer containing the data that will be send.
787 787 * @param sid is the source identifier of the data that will be sent.
788 788 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
789 789 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
790 790 * contain information to setup the transmission of the data packets.
791 791 *
792 792 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
793 793 *
794 794 */
795 795
796 796 unsigned int i;
797 797 int ret;
798 798 unsigned int coarseTime;
799 799 unsigned int fineTime;
800 800 rtems_status_code status;
801 801 spw_ioctl_pkt_send spw_ioctl_send_CWF;
802 802 int *dataPtr;
803 803 unsigned char sid;
804 804
805 805 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
806 806 spw_ioctl_send_CWF.options = 0;
807 807
808 808 ret = LFR_DEFAULT;
809 809 sid = (unsigned char) ring_node_to_send->sid;
810 810
811 811 coarseTime = ring_node_to_send->coarseTime;
812 812 fineTime = ring_node_to_send->fineTime;
813 813 dataPtr = (int*) ring_node_to_send->buffer_address;
814 814
815 815 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
816 816 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
817 817 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
818 818 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
819 819 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
820 820
821 821 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
822 822 {
823 823 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
824 824 spw_ioctl_send_CWF.hdr = (char*) header;
825 825 // BUILD THE DATA
826 826 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
827 827
828 828 // SET PACKET SEQUENCE CONTROL
829 829 increment_seq_counter_source_id( header->packetSequenceControl, sid );
830 830
831 831 // SET SID
832 832 header->sid = sid;
833 833
834 834 // SET PACKET TIME
835 835 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
836 836 //
837 837 header->time[0] = header->acquisitionTime[0];
838 838 header->time[1] = header->acquisitionTime[1];
839 839 header->time[2] = header->acquisitionTime[2];
840 840 header->time[3] = header->acquisitionTime[3];
841 841 header->time[4] = header->acquisitionTime[4];
842 842 header->time[5] = header->acquisitionTime[5];
843 843
844 844 // SET PACKET ID
845 845 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
846 846 {
847 847 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
848 848 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
849 849 }
850 850 else
851 851 {
852 852 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
853 853 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
854 854 }
855 855
856 856 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
857 857 if (status != RTEMS_SUCCESSFUL) {
858 858 printf("%d-%d, ERR %d\n", sid, i, (int) status);
859 859 ret = LFR_DEFAULT;
860 860 }
861 861 }
862 862
863 863 return ret;
864 864 }
865 865
866 866 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
867 867 Header_TM_LFR_SCIENCE_SWF_t *header )
868 868 {
869 869 /** This function sends SWF CCSDS packets (F2, F1 or F0).
870 870 *
871 871 * @param waveform points to the buffer containing the data that will be send.
872 872 * @param sid is the source identifier of the data that will be sent.
873 873 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
874 874 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
875 875 * contain information to setup the transmission of the data packets.
876 876 *
877 877 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
878 878 *
879 879 */
880 880
881 881 unsigned int i;
882 882 int ret;
883 883 unsigned int coarseTime;
884 884 unsigned int fineTime;
885 885 rtems_status_code status;
886 886 spw_ioctl_pkt_send spw_ioctl_send_SWF;
887 887 int *dataPtr;
888 888 unsigned char sid;
889 889
890 890 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
891 891 spw_ioctl_send_SWF.options = 0;
892 892
893 893 ret = LFR_DEFAULT;
894 894
895 895 coarseTime = ring_node_to_send->coarseTime;
896 896 fineTime = ring_node_to_send->fineTime;
897 897 dataPtr = (int*) ring_node_to_send->buffer_address;
898 898 sid = ring_node_to_send->sid;
899 899
900 900 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
901 901
902 902 for (i=0; i<7; i++) // send waveform
903 903 {
904 904 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
905 905 spw_ioctl_send_SWF.hdr = (char*) header;
906 906
907 907 // SET PACKET SEQUENCE CONTROL
908 908 increment_seq_counter_source_id( header->packetSequenceControl, sid );
909 909
910 910 // SET PACKET LENGTH AND BLKNR
911 911 if (i == 6)
912 912 {
913 913 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
914 914 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
915 915 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
916 916 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
917 917 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
918 918 }
919 919 else
920 920 {
921 921 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
922 922 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
923 923 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
924 924 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
925 925 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
926 926 }
927 927
928 928 // SET PACKET TIME
929 929 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
930 930 //
931 931 header->time[0] = header->acquisitionTime[0];
932 932 header->time[1] = header->acquisitionTime[1];
933 933 header->time[2] = header->acquisitionTime[2];
934 934 header->time[3] = header->acquisitionTime[3];
935 935 header->time[4] = header->acquisitionTime[4];
936 936 header->time[5] = header->acquisitionTime[5];
937 937
938 938 // SET SID
939 939 header->sid = sid;
940 940
941 941 // SET PKTNR
942 942 header->pktNr = i+1; // PKT_NR
943 943
944 944 // SEND PACKET
945 945 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
946 946 if (status != RTEMS_SUCCESSFUL) {
947 947 printf("%d-%d, ERR %d\n", sid, i, (int) status);
948 948 ret = LFR_DEFAULT;
949 949 }
950 950 }
951 951
952 952 return ret;
953 953 }
954 954
955 955 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
956 956 Header_TM_LFR_SCIENCE_CWF_t *header )
957 957 {
958 958 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
959 959 *
960 960 * @param waveform points to the buffer containing the data that will be send.
961 961 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
962 962 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
963 963 * contain information to setup the transmission of the data packets.
964 964 *
965 965 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
966 966 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
967 967 *
968 968 */
969 969
970 970 unsigned int i;
971 971 int ret;
972 972 unsigned int coarseTime;
973 973 unsigned int fineTime;
974 974 rtems_status_code status;
975 975 spw_ioctl_pkt_send spw_ioctl_send_CWF;
976 976 char *dataPtr;
977 977 unsigned char sid;
978 978
979 979 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
980 980 spw_ioctl_send_CWF.options = 0;
981 981
982 982 ret = LFR_DEFAULT;
983 983 sid = ring_node_to_send->sid;
984 984
985 985 coarseTime = ring_node_to_send->coarseTime;
986 986 fineTime = ring_node_to_send->fineTime;
987 987 dataPtr = (char*) ring_node_to_send->buffer_address;
988 988
989 989 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
990 990 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
991 991 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
992 992 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
993 993 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
994 994
995 995 //*********************
996 996 // SEND CWF3_light DATA
997 997 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
998 998 {
999 999 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1000 1000 spw_ioctl_send_CWF.hdr = (char*) header;
1001 1001 // BUILD THE DATA
1002 1002 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1003 1003
1004 1004 // SET PACKET SEQUENCE COUNTER
1005 1005 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1006 1006
1007 1007 // SET SID
1008 1008 header->sid = sid;
1009 1009
1010 1010 // SET PACKET TIME
1011 1011 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1012 1012 //
1013 1013 header->time[0] = header->acquisitionTime[0];
1014 1014 header->time[1] = header->acquisitionTime[1];
1015 1015 header->time[2] = header->acquisitionTime[2];
1016 1016 header->time[3] = header->acquisitionTime[3];
1017 1017 header->time[4] = header->acquisitionTime[4];
1018 1018 header->time[5] = header->acquisitionTime[5];
1019 1019
1020 1020 // SET PACKET ID
1021 1021 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1022 1022 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1023 1023
1024 1024 // SEND PACKET
1025 1025 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1026 1026 if (status != RTEMS_SUCCESSFUL) {
1027 1027 printf("%d-%d, ERR %d\n", sid, i, (int) status);
1028 1028 ret = LFR_DEFAULT;
1029 1029 }
1030 1030 }
1031 1031
1032 1032 return ret;
1033 1033 }
1034 1034
1035 1035 void spw_send_asm_f0( ring_node *ring_node_to_send,
1036 1036 Header_TM_LFR_SCIENCE_ASM_t *header )
1037 1037 {
1038 1038 unsigned int i;
1039 1039 unsigned int length = 0;
1040 1040 rtems_status_code status;
1041 1041 unsigned int sid;
1042 1042 float *spectral_matrix;
1043 1043 int coarseTime;
1044 1044 int fineTime;
1045 1045 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1046 1046
1047 1047 sid = ring_node_to_send->sid;
1048 1048 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1049 1049 coarseTime = ring_node_to_send->coarseTime;
1050 1050 fineTime = ring_node_to_send->fineTime;
1051 1051
1052 1052 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1053 1053
1054 1054 for (i=0; i<3; i++)
1055 1055 {
1056 1056 if ((i==0) || (i==1))
1057 1057 {
1058 1058 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1059 1059 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1060 1060 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1061 1061 ];
1062 1062 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1063 1063 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1064 1064 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1065 1065 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1066 1066 }
1067 1067 else
1068 1068 {
1069 1069 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1070 1070 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1071 1071 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1072 1072 ];
1073 1073 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1074 1074 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1075 1075 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1076 1076 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1077 1077 }
1078 1078
1079 1079 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1080 1080 spw_ioctl_send_ASM.hdr = (char *) header;
1081 1081 spw_ioctl_send_ASM.options = 0;
1082 1082
1083 1083 // (2) BUILD THE HEADER
1084 1084 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1085 1085 header->packetLength[0] = (unsigned char) (length>>8);
1086 1086 header->packetLength[1] = (unsigned char) (length);
1087 1087 header->sid = (unsigned char) sid; // SID
1088 1088 header->pa_lfr_pkt_cnt_asm = 3;
1089 1089 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1090 1090
1091 1091 // (3) SET PACKET TIME
1092 1092 header->time[0] = (unsigned char) (coarseTime>>24);
1093 1093 header->time[1] = (unsigned char) (coarseTime>>16);
1094 1094 header->time[2] = (unsigned char) (coarseTime>>8);
1095 1095 header->time[3] = (unsigned char) (coarseTime);
1096 1096 header->time[4] = (unsigned char) (fineTime>>8);
1097 1097 header->time[5] = (unsigned char) (fineTime);
1098 1098 //
1099 1099 header->acquisitionTime[0] = header->time[0];
1100 1100 header->acquisitionTime[1] = header->time[1];
1101 1101 header->acquisitionTime[2] = header->time[2];
1102 1102 header->acquisitionTime[3] = header->time[3];
1103 1103 header->acquisitionTime[4] = header->time[4];
1104 1104 header->acquisitionTime[5] = header->time[5];
1105 1105
1106 1106 // (4) SEND PACKET
1107 1107 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1108 1108 if (status != RTEMS_SUCCESSFUL) {
1109 1109 printf("in ASM_send *** ERR %d\n", (int) status);
1110 1110 }
1111 1111 }
1112 1112 }
1113 1113
1114 1114 void spw_send_asm_f1( ring_node *ring_node_to_send,
1115 1115 Header_TM_LFR_SCIENCE_ASM_t *header )
1116 1116 {
1117 1117 unsigned int i;
1118 1118 unsigned int length = 0;
1119 1119 rtems_status_code status;
1120 1120 unsigned int sid;
1121 1121 float *spectral_matrix;
1122 1122 int coarseTime;
1123 1123 int fineTime;
1124 1124 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1125 1125
1126 1126 sid = ring_node_to_send->sid;
1127 1127 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1128 1128 coarseTime = ring_node_to_send->coarseTime;
1129 1129 fineTime = ring_node_to_send->fineTime;
1130 1130
1131 1131 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1132 1132
1133 1133 for (i=0; i<3; i++)
1134 1134 {
1135 1135 if ((i==0) || (i==1))
1136 1136 {
1137 1137 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1138 1138 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1139 1139 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1140 1140 ];
1141 1141 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1142 1142 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1143 1143 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1144 1144 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1145 1145 }
1146 1146 else
1147 1147 {
1148 1148 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1149 1149 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1150 1150 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1151 1151 ];
1152 1152 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1153 1153 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1154 1154 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1155 1155 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1156 1156 }
1157 1157
1158 1158 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1159 1159 spw_ioctl_send_ASM.hdr = (char *) header;
1160 1160 spw_ioctl_send_ASM.options = 0;
1161 1161
1162 1162 // (2) BUILD THE HEADER
1163 1163 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1164 1164 header->packetLength[0] = (unsigned char) (length>>8);
1165 1165 header->packetLength[1] = (unsigned char) (length);
1166 1166 header->sid = (unsigned char) sid; // SID
1167 1167 header->pa_lfr_pkt_cnt_asm = 3;
1168 1168 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1169 1169
1170 1170 // (3) SET PACKET TIME
1171 1171 header->time[0] = (unsigned char) (coarseTime>>24);
1172 1172 header->time[1] = (unsigned char) (coarseTime>>16);
1173 1173 header->time[2] = (unsigned char) (coarseTime>>8);
1174 1174 header->time[3] = (unsigned char) (coarseTime);
1175 1175 header->time[4] = (unsigned char) (fineTime>>8);
1176 1176 header->time[5] = (unsigned char) (fineTime);
1177 1177 //
1178 1178 header->acquisitionTime[0] = header->time[0];
1179 1179 header->acquisitionTime[1] = header->time[1];
1180 1180 header->acquisitionTime[2] = header->time[2];
1181 1181 header->acquisitionTime[3] = header->time[3];
1182 1182 header->acquisitionTime[4] = header->time[4];
1183 1183 header->acquisitionTime[5] = header->time[5];
1184 1184
1185 1185 // (4) SEND PACKET
1186 1186 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1187 1187 if (status != RTEMS_SUCCESSFUL) {
1188 1188 printf("in ASM_send *** ERR %d\n", (int) status);
1189 1189 }
1190 1190 }
1191 1191 }
1192 1192
1193 1193 void spw_send_asm_f2( ring_node *ring_node_to_send,
1194 1194 Header_TM_LFR_SCIENCE_ASM_t *header )
1195 1195 {
1196 1196 unsigned int i;
1197 1197 unsigned int length = 0;
1198 1198 rtems_status_code status;
1199 1199 unsigned int sid;
1200 1200 float *spectral_matrix;
1201 1201 int coarseTime;
1202 1202 int fineTime;
1203 1203 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1204 1204
1205 1205 sid = ring_node_to_send->sid;
1206 1206 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1207 1207 coarseTime = ring_node_to_send->coarseTime;
1208 1208 fineTime = ring_node_to_send->fineTime;
1209 1209
1210 1210 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1211 1211
1212 1212 for (i=0; i<3; i++)
1213 1213 {
1214 1214
1215 1215 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1216 1216 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1217 1217 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1218 1218 ];
1219 1219 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1220 1220 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1221 1221 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1222 1222 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1223 1223
1224 1224 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1225 1225 spw_ioctl_send_ASM.hdr = (char *) header;
1226 1226 spw_ioctl_send_ASM.options = 0;
1227 1227
1228 1228 // (2) BUILD THE HEADER
1229 1229 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1230 1230 header->packetLength[0] = (unsigned char) (length>>8);
1231 1231 header->packetLength[1] = (unsigned char) (length);
1232 1232 header->sid = (unsigned char) sid; // SID
1233 1233 header->pa_lfr_pkt_cnt_asm = 3;
1234 1234 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1235 1235
1236 1236 // (3) SET PACKET TIME
1237 1237 header->time[0] = (unsigned char) (coarseTime>>24);
1238 1238 header->time[1] = (unsigned char) (coarseTime>>16);
1239 1239 header->time[2] = (unsigned char) (coarseTime>>8);
1240 1240 header->time[3] = (unsigned char) (coarseTime);
1241 1241 header->time[4] = (unsigned char) (fineTime>>8);
1242 1242 header->time[5] = (unsigned char) (fineTime);
1243 1243 //
1244 1244 header->acquisitionTime[0] = header->time[0];
1245 1245 header->acquisitionTime[1] = header->time[1];
1246 1246 header->acquisitionTime[2] = header->time[2];
1247 1247 header->acquisitionTime[3] = header->time[3];
1248 1248 header->acquisitionTime[4] = header->time[4];
1249 1249 header->acquisitionTime[5] = header->time[5];
1250 1250
1251 1251 // (4) SEND PACKET
1252 1252 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1253 1253 if (status != RTEMS_SUCCESSFUL) {
1254 1254 printf("in ASM_send *** ERR %d\n", (int) status);
1255 1255 }
1256 1256 }
1257 1257 }
1258 1258
1259 1259 void spw_send_k_dump( ring_node *ring_node_to_send )
1260 1260 {
1261 1261 rtems_status_code status;
1262 1262 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1263 1263 unsigned int packetLength;
1264 1264 unsigned int size;
1265 1265
1266 1266 printf("spw_send_k_dump\n");
1267 1267
1268 1268 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1269 1269
1270 1270 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1271 1271
1272 1272 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1273 1273
1274 1274 printf("packetLength %d, size %d\n", packetLength, size );
1275 1275
1276 1276 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1277 1277
1278 1278 if (status == -1){
1279 1279 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1280 1280 }
1281 1281
1282 1282 ring_node_to_send->status = 0x00;
1283 1283 }
General Comments 0
You need to be logged in to leave comments. Login now