##// END OF EJS Templates
HG_REPOSITORIES » LPP » INSTRUMENTATION » SOLO_LFR » DEV_PLE
RSS

Clone from http://pc-instru.lpp.polytechnique.fr:5000/DEV_PLE

Bug 703 hk_lfr_le and hk_lfr_me handling corrected
paul -
r317:0d12752c6dcd R3_plus draft
parent child
Show More
Show file before | Show file after | Hide comments
@@ -1,87 +1,109
1 1 #ifndef FSW_MISC_H_INCLUDED
2 2 #define FSW_MISC_H_INCLUDED
3 3
4 4 #include <rtems.h>
5 5 #include <stdio.h>
6 6 #include <grspw.h>
7 7 #include <grlib_regs.h>
8 8
9 9 #include "fsw_params.h"
10 10 #include "fsw_spacewire.h"
11 11 #include "lfr_cpu_usage_report.h"
12 12
13 13
14 14 enum lfr_reset_cause_t{
15 15 UNKNOWN_CAUSE,
16 16 POWER_ON,
17 17 TC_RESET,
18 18 WATCHDOG,
19 19 ERROR_RESET,
20 20 UNEXP_RESET
21 21 };
22 22
23 typedef struct{
24 unsigned char dpu_spw_parity;
25 unsigned char dpu_spw_disconnect;
26 unsigned char dpu_spw_escape;
27 unsigned char dpu_spw_credit;
28 unsigned char dpu_spw_write_sync;
29 unsigned char timecode_erroneous;
30 unsigned char timecode_missing;
31 unsigned char timecode_invalid;
32 unsigned char time_timecode_it;
33 unsigned char time_not_synchro;
34 unsigned char time_timecode_ctr;
35 unsigned char ahb_correctable;
36 } hk_lfr_le_t;
37
38 typedef struct{
39 unsigned char dpu_spw_early_eop;
40 unsigned char dpu_spw_invalid_addr;
41 unsigned char dpu_spw_eep;
42 unsigned char dpu_spw_rx_too_big;
43 } hk_lfr_me_t;
44
23 45 extern gptimer_regs_t *gptimer_regs;
24 46 extern void ASR16_get_FPRF_IURF_ErrorCounters( unsigned int*, unsigned int* );
25 47 extern void CCR_getInstructionAndDataErrorCounters( unsigned int*, unsigned int* );
26 48
27 49 #define LFR_RESET_CAUSE_UNKNOWN_CAUSE 0
28 50
29 51 rtems_name name_hk_rate_monotonic; // name of the HK rate monotonic
30 52 rtems_id HK_id; // id of the HK rate monotonic period
31 53 rtems_name name_avgv_rate_monotonic; // name of the AVGV rate monotonic
32 54 rtems_id AVGV_id; // id of the AVGV rate monotonic period
33 55
34 56 void timer_configure( unsigned char timer, unsigned int clock_divider,
35 57 unsigned char interrupt_level, rtems_isr (*timer_isr)() );
36 58 void timer_start( unsigned char timer );
37 59 void timer_stop( unsigned char timer );
38 60 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider);
39 61
40 62 // WATCHDOG
41 63 rtems_isr watchdog_isr( rtems_vector_number vector );
42 64 void watchdog_configure(void);
43 65 void watchdog_stop(void);
44 66 void watchdog_reload(void);
45 67 void watchdog_start(void);
46 68
47 69 // SERIAL LINK
48 70 int send_console_outputs_on_apbuart_port( void );
49 71 int enable_apbuart_transmitter( void );
50 72 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value);
51 73
52 74 // RTEMS TASKS
53 75 rtems_task load_task( rtems_task_argument argument );
54 76 rtems_task hous_task( rtems_task_argument argument );
55 77 rtems_task avgv_task( rtems_task_argument argument );
56 78 rtems_task dumb_task( rtems_task_argument unused );
57 79
58 80 void init_housekeeping_parameters( void );
59 81 void increment_seq_counter(unsigned short *packetSequenceControl);
60 82 void getTime( unsigned char *time);
61 83 unsigned long long int getTimeAsUnsignedLongLongInt( );
62 84 void send_dumb_hk( void );
63 85 void get_temperatures( unsigned char *temperatures );
64 86 void get_v_e1_e2_f3( unsigned char *spacecraft_potential );
65 87 void get_cpu_load( unsigned char *resource_statistics );
66 88 void set_hk_lfr_sc_potential_flag( bool state );
67 89 void set_sy_lfr_pas_filter_enabled( bool state );
68 90 void set_sy_lfr_watchdog_enabled( bool state );
69 91 void set_hk_lfr_calib_enable( bool state );
70 92 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause );
71 93 void hk_lfr_le_me_he_update();
72 94 void set_hk_lfr_time_not_synchro();
73 95
74 96 extern int sched_yield( void );
75 97 extern void rtems_cpu_usage_reset();
76 98 extern ring_node *current_ring_node_f3;
77 99 extern ring_node *ring_node_to_send_cwf_f3;
78 100 extern ring_node waveform_ring_f3[];
79 101 extern unsigned short sequenceCounterHK;
80 102
81 103 extern unsigned char hk_lfr_q_sd_fifo_size_max;
82 104 extern unsigned char hk_lfr_q_rv_fifo_size_max;
83 105 extern unsigned char hk_lfr_q_p0_fifo_size_max;
84 106 extern unsigned char hk_lfr_q_p1_fifo_size_max;
85 107 extern unsigned char hk_lfr_q_p2_fifo_size_max;
86 108
87 109 #endif // FSW_MISC_H_INCLUDED
Show file before | Show file after | Hide comments
@@ -1,898 +1,983
1 1 /** General usage functions and RTEMS tasks.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 */
7 7
8 8 #include "fsw_misc.h"
9 9
10 10 void timer_configure(unsigned char timer, unsigned int clock_divider,
11 11 unsigned char interrupt_level, rtems_isr (*timer_isr)() )
12 12 {
13 13 /** This function configures a GPTIMER timer instantiated in the VHDL design.
14 14 *
15 15 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
16 16 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
17 17 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
18 18 * @param interrupt_level is the interrupt level that the timer drives.
19 19 * @param timer_isr is the interrupt subroutine that will be attached to the IRQ driven by the timer.
20 20 *
21 21 * Interrupt levels are described in the SPARC documentation sparcv8.pdf p.76
22 22 *
23 23 */
24 24
25 25 rtems_status_code status;
26 26 rtems_isr_entry old_isr_handler;
27 27
28 28 gptimer_regs->timer[timer].ctrl = 0x00; // reset the control register
29 29
30 30 status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels
31 31 if (status!=RTEMS_SUCCESSFUL)
32 32 {
33 33 PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n")
34 34 }
35 35
36 36 timer_set_clock_divider( timer, clock_divider);
37 37 }
38 38
39 39 void timer_start(unsigned char timer)
40 40 {
41 41 /** This function starts a GPTIMER timer.
42 42 *
43 43 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
44 44 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
45 45 *
46 46 */
47 47
48 48 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
49 49 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register
50 50 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer
51 51 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart
52 52 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable
53 53 }
54 54
55 55 void timer_stop(unsigned char timer)
56 56 {
57 57 /** This function stops a GPTIMER timer.
58 58 *
59 59 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
60 60 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
61 61 *
62 62 */
63 63
64 64 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer
65 65 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable
66 66 gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any
67 67 }
68 68
69 69 void timer_set_clock_divider(unsigned char timer, unsigned int clock_divider)
70 70 {
71 71 /** This function sets the clock divider of a GPTIMER timer.
72 72 *
73 73 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
74 74 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
75 75 * @param clock_divider is the divider of the 1 MHz clock that will be configured.
76 76 *
77 77 */
78 78
79 79 gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz
80 80 }
81 81
82 82 // WATCHDOG
83 83
84 84 rtems_isr watchdog_isr( rtems_vector_number vector )
85 85 {
86 86 rtems_status_code status_code;
87 87
88 88 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_12 );
89 89
90 90 PRINTF("watchdog_isr *** this is the end, exit(0)\n");
91 91
92 92 exit(0);
93 93 }
94 94
95 95 void watchdog_configure(void)
96 96 {
97 97 /** This function configure the watchdog.
98 98 *
99 99 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
100 100 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
101 101 *
102 102 * The watchdog is a timer provided by the GPTIMER IP core of the GRLIB.
103 103 *
104 104 */
105 105
106 106 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt during configuration
107 107
108 108 timer_configure( TIMER_WATCHDOG, CLKDIV_WATCHDOG, IRQ_SPARC_GPTIMER_WATCHDOG, watchdog_isr );
109 109
110 110 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
111 111 }
112 112
113 113 void watchdog_stop(void)
114 114 {
115 115 LEON_Mask_interrupt( IRQ_GPTIMER_WATCHDOG ); // mask gptimer/watchdog interrupt line
116 116 timer_stop( TIMER_WATCHDOG );
117 117 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG ); // clear gptimer/watchdog interrupt
118 118 }
119 119
120 120 void watchdog_reload(void)
121 121 {
122 122 /** This function reloads the watchdog timer counter with the timer reload value.
123 123 *
124 124 * @param void
125 125 *
126 126 * @return void
127 127 *
128 128 */
129 129
130 130 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000004; // LD load value from the reload register
131 131 }
132 132
133 133 void watchdog_start(void)
134 134 {
135 135 /** This function starts the watchdog timer.
136 136 *
137 137 * @param gptimer_regs points to the APB registers of the GPTIMER IP core.
138 138 * @param timer is the number of the timer in the IP core (several timers can be instantiated).
139 139 *
140 140 */
141 141
142 142 LEON_Clear_interrupt( IRQ_GPTIMER_WATCHDOG );
143 143
144 144 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000010; // clear pending IRQ if any
145 145 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000004; // LD load value from the reload register
146 146 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000001; // EN enable the timer
147 147 gptimer_regs->timer[TIMER_WATCHDOG].ctrl = gptimer_regs->timer[TIMER_WATCHDOG].ctrl | 0x00000008; // IE interrupt enable
148 148
149 149 LEON_Unmask_interrupt( IRQ_GPTIMER_WATCHDOG );
150 150
151 151 }
152 152
153 153 int enable_apbuart_transmitter( void ) // set the bit 1, TE Transmitter Enable to 1 in the APBUART control register
154 154 {
155 155 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART;
156 156
157 157 apbuart_regs->ctrl = APBUART_CTRL_REG_MASK_TE;
158 158
159 159 return 0;
160 160 }
161 161
162 162 void set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value)
163 163 {
164 164 /** This function sets the scaler reload register of the apbuart module
165 165 *
166 166 * @param regs is the address of the apbuart registers in memory
167 167 * @param value is the value that will be stored in the scaler register
168 168 *
169 169 * The value shall be set by the software to get data on the serial interface.
170 170 *
171 171 */
172 172
173 173 struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs;
174 174
175 175 apbuart_regs->scaler = value;
176 176
177 177 BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value)
178 178 }
179 179
180 180 //************
181 181 // RTEMS TASKS
182 182
183 183 rtems_task load_task(rtems_task_argument argument)
184 184 {
185 185 BOOT_PRINTF("in LOAD *** \n")
186 186
187 187 rtems_status_code status;
188 188 unsigned int i;
189 189 unsigned int j;
190 190 rtems_name name_watchdog_rate_monotonic; // name of the watchdog rate monotonic
191 191 rtems_id watchdog_period_id; // id of the watchdog rate monotonic period
192 192
193 193 name_watchdog_rate_monotonic = rtems_build_name( 'L', 'O', 'A', 'D' );
194 194
195 195 status = rtems_rate_monotonic_create( name_watchdog_rate_monotonic, &watchdog_period_id );
196 196 if( status != RTEMS_SUCCESSFUL ) {
197 197 PRINTF1( "in LOAD *** rtems_rate_monotonic_create failed with status of %d\n", status )
198 198 }
199 199
200 200 i = 0;
201 201 j = 0;
202 202
203 203 watchdog_configure();
204 204
205 205 watchdog_start();
206 206
207 207 set_sy_lfr_watchdog_enabled( true );
208 208
209 209 while(1){
210 210 status = rtems_rate_monotonic_period( watchdog_period_id, WATCHDOG_PERIOD );
211 211 watchdog_reload();
212 212 i = i + 1;
213 213 if ( i == 10 )
214 214 {
215 215 i = 0;
216 216 j = j + 1;
217 217 PRINTF1("%d\n", j)
218 218 }
219 219 #ifdef DEBUG_WATCHDOG
220 220 if (j == 3 )
221 221 {
222 222 status = rtems_task_delete(RTEMS_SELF);
223 223 }
224 224 #endif
225 225 }
226 226 }
227 227
228 228 rtems_task hous_task(rtems_task_argument argument)
229 229 {
230 230 rtems_status_code status;
231 231 rtems_status_code spare_status;
232 232 rtems_id queue_id;
233 233 rtems_rate_monotonic_period_status period_status;
234 234
235 235 status = get_message_queue_id_send( &queue_id );
236 236 if (status != RTEMS_SUCCESSFUL)
237 237 {
238 238 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
239 239 }
240 240
241 241 BOOT_PRINTF("in HOUS ***\n");
242 242
243 243 if (rtems_rate_monotonic_ident( name_hk_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
244 244 status = rtems_rate_monotonic_create( name_hk_rate_monotonic, &HK_id );
245 245 if( status != RTEMS_SUCCESSFUL ) {
246 246 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
247 247 }
248 248 }
249 249
250 250 status = rtems_rate_monotonic_cancel(HK_id);
251 251 if( status != RTEMS_SUCCESSFUL ) {
252 252 PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status );
253 253 }
254 254 else {
255 255 DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n");
256 256 }
257 257
258 258 // startup phase
259 259 status = rtems_rate_monotonic_period( HK_id, SY_LFR_TIME_SYN_TIMEOUT_in_ticks );
260 260 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
261 261 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
262 262 while(period_status.state != RATE_MONOTONIC_EXPIRED ) // after SY_LFR_TIME_SYN_TIMEOUT ms, starts HK anyway
263 263 {
264 264 if ((time_management_regs->coarse_time & 0x80000000) == 0x00000000) // check time synchronization
265 265 {
266 266 break; // break if LFR is synchronized
267 267 }
268 268 else
269 269 {
270 270 status = rtems_rate_monotonic_get_status( HK_id, &period_status );
271 271 // sched_yield();
272 272 status = rtems_task_wake_after( 10 ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 100 ms = 10 * 10 ms
273 273 }
274 274 }
275 275 status = rtems_rate_monotonic_cancel(HK_id);
276 276 DEBUG_PRINTF1("startup HK, HK_id status = %d\n", period_status.state)
277 277
278 278 set_hk_lfr_reset_cause( POWER_ON );
279 279
280 280 while(1){ // launch the rate monotonic task
281 281 status = rtems_rate_monotonic_period( HK_id, HK_PERIOD );
282 282 if ( status != RTEMS_SUCCESSFUL ) {
283 283 PRINTF1( "in HOUS *** ERR period: %d\n", status);
284 284 spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_6 );
285 285 }
286 286 else {
287 287 housekeeping_packet.packetSequenceControl[0] = (unsigned char) (sequenceCounterHK >> 8);
288 288 housekeeping_packet.packetSequenceControl[1] = (unsigned char) (sequenceCounterHK );
289 289 increment_seq_counter( &sequenceCounterHK );
290 290
291 291 housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
292 292 housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
293 293 housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
294 294 housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
295 295 housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
296 296 housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
297 297
298 298 spacewire_update_hk_lfr_link_state( &housekeeping_packet.lfr_status_word[0] );
299 299
300 300 spacewire_read_statistics();
301 301
302 302 update_hk_with_grspw_stats();
303 303
304 304 set_hk_lfr_time_not_synchro();
305 305
306 306 housekeeping_packet.hk_lfr_q_sd_fifo_size_max = hk_lfr_q_sd_fifo_size_max;
307 307 housekeeping_packet.hk_lfr_q_rv_fifo_size_max = hk_lfr_q_rv_fifo_size_max;
308 308 housekeeping_packet.hk_lfr_q_p0_fifo_size_max = hk_lfr_q_p0_fifo_size_max;
309 309 housekeeping_packet.hk_lfr_q_p1_fifo_size_max = hk_lfr_q_p1_fifo_size_max;
310 310 housekeeping_packet.hk_lfr_q_p2_fifo_size_max = hk_lfr_q_p2_fifo_size_max;
311 311
312 312 housekeeping_packet.sy_lfr_common_parameters_spare = parameter_dump_packet.sy_lfr_common_parameters_spare;
313 313 housekeeping_packet.sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
314 314 get_temperatures( housekeeping_packet.hk_lfr_temp_scm );
315 315 get_v_e1_e2_f3( housekeeping_packet.hk_lfr_sc_v_f3 );
316 316 get_cpu_load( (unsigned char *) &housekeeping_packet.hk_lfr_cpu_load );
317 317
318 318 hk_lfr_le_me_he_update();
319 319
320 320 housekeeping_packet.hk_lfr_sc_rw_f_flags = cp_rpw_sc_rw_f_flags;
321 321
322 322 // SEND PACKET
323 323 status = rtems_message_queue_send( queue_id, &housekeeping_packet,
324 324 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
325 325 if (status != RTEMS_SUCCESSFUL) {
326 326 PRINTF1("in HOUS *** ERR send: %d\n", status)
327 327 }
328 328 }
329 329 }
330 330
331 331 PRINTF("in HOUS *** deleting task\n")
332 332
333 333 status = rtems_task_delete( RTEMS_SELF ); // should not return
334 334
335 335 return;
336 336 }
337 337
338 338 rtems_task avgv_task(rtems_task_argument argument)
339 339 {
340 340 #define MOVING_AVERAGE 16
341 341 rtems_status_code status;
342 342 unsigned int v[MOVING_AVERAGE];
343 343 unsigned int e1[MOVING_AVERAGE];
344 344 unsigned int e2[MOVING_AVERAGE];
345 345 float average_v;
346 346 float average_e1;
347 347 float average_e2;
348 348 unsigned char k;
349 349 unsigned char indexOfOldValue;
350 350
351 351 BOOT_PRINTF("in AVGV ***\n");
352 352
353 353 if (rtems_rate_monotonic_ident( name_avgv_rate_monotonic, &HK_id) != RTEMS_SUCCESSFUL) {
354 354 status = rtems_rate_monotonic_create( name_avgv_rate_monotonic, &AVGV_id );
355 355 if( status != RTEMS_SUCCESSFUL ) {
356 356 PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status );
357 357 }
358 358 }
359 359
360 360 status = rtems_rate_monotonic_cancel(AVGV_id);
361 361 if( status != RTEMS_SUCCESSFUL ) {
362 362 PRINTF1( "ERR *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id) ***code: %d\n", status );
363 363 }
364 364 else {
365 365 DEBUG_PRINTF("OK *** in AVGV *** rtems_rate_monotonic_cancel(AVGV_id)\n");
366 366 }
367 367
368 368 // initialize values
369 369 k = 0;
370 370 indexOfOldValue = MOVING_AVERAGE - 1;
371 371 for (k = 0; k < MOVING_AVERAGE; k++)
372 372 {
373 373 v[k] = 0;
374 374 e1[k] = 0;
375 375 e2[k] = 0;
376 376 average_v = 0.;
377 377 average_e1 = 0.;
378 378 average_e2 = 0.;
379 379 }
380 380
381 381 k = 0;
382 382
383 383 while(1){ // launch the rate monotonic task
384 384 status = rtems_rate_monotonic_period( AVGV_id, AVGV_PERIOD );
385 385 if ( status != RTEMS_SUCCESSFUL ) {
386 386 PRINTF1( "in AVGV *** ERR period: %d\n", status);
387 387 }
388 388 else {
389 389 // store new value in buffer
390 390 v[k] = waveform_picker_regs->v;
391 391 e1[k] = waveform_picker_regs->e1;
392 392 e2[k] = waveform_picker_regs->e2;
393 393 if (k == (MOVING_AVERAGE - 1))
394 394 {
395 395 indexOfOldValue = 0;
396 396 }
397 397 else
398 398 {
399 399 indexOfOldValue = k + 1;
400 400 }
401 401 average_v = average_v + v[k] - v[indexOfOldValue];
402 402 average_e1 = average_e1 + e1[k] - e1[indexOfOldValue];
403 403 average_e2 = average_e2 + e2[k] - e2[indexOfOldValue];
404 404 }
405 405 if (k == (MOVING_AVERAGE-1))
406 406 {
407 407 k = 0;
408 408 printf("tick\n");
409 409 }
410 410 else
411 411 {
412 412 k++;
413 413 }
414 414 }
415 415
416 416 PRINTF("in AVGV *** deleting task\n")
417 417
418 418 status = rtems_task_delete( RTEMS_SELF ); // should not return
419 419
420 420 return;
421 421 }
422 422
423 423 rtems_task dumb_task( rtems_task_argument unused )
424 424 {
425 425 /** This RTEMS taks is used to print messages without affecting the general behaviour of the software.
426 426 *
427 427 * @param unused is the starting argument of the RTEMS task
428 428 *
429 429 * The DUMB taks waits for RTEMS events and print messages depending on the incoming events.
430 430 *
431 431 */
432 432
433 433 unsigned int i;
434 434 unsigned int intEventOut;
435 435 unsigned int coarse_time = 0;
436 436 unsigned int fine_time = 0;
437 437 rtems_event_set event_out;
438 438
439 439 char *DumbMessages[15] = {"in DUMB *** default", // RTEMS_EVENT_0
440 440 "in DUMB *** timecode_irq_handler", // RTEMS_EVENT_1
441 441 "in DUMB *** f3 buffer changed", // RTEMS_EVENT_2
442 442 "in DUMB *** in SMIQ *** Error sending event to AVF0", // RTEMS_EVENT_3
443 443 "in DUMB *** spectral_matrices_isr *** Error sending event to SMIQ", // RTEMS_EVENT_4
444 444 "in DUMB *** waveforms_simulator_isr", // RTEMS_EVENT_5
445 445 "VHDL SM *** two buffers f0 ready", // RTEMS_EVENT_6
446 446 "ready for dump", // RTEMS_EVENT_7
447 447 "VHDL ERR *** spectral matrix", // RTEMS_EVENT_8
448 448 "tick", // RTEMS_EVENT_9
449 449 "VHDL ERR *** waveform picker", // RTEMS_EVENT_10
450 450 "VHDL ERR *** unexpected ready matrix values", // RTEMS_EVENT_11
451 451 "WATCHDOG timer", // RTEMS_EVENT_12
452 452 "TIMECODE timer", // RTEMS_EVENT_13
453 453 "TIMECODE ISR" // RTEMS_EVENT_14
454 454 };
455 455
456 456 BOOT_PRINTF("in DUMB *** \n")
457 457
458 458 while(1){
459 459 rtems_event_receive(RTEMS_EVENT_0 | RTEMS_EVENT_1 | RTEMS_EVENT_2 | RTEMS_EVENT_3
460 460 | RTEMS_EVENT_4 | RTEMS_EVENT_5 | RTEMS_EVENT_6 | RTEMS_EVENT_7
461 461 | RTEMS_EVENT_8 | RTEMS_EVENT_9 | RTEMS_EVENT_12 | RTEMS_EVENT_13
462 462 | RTEMS_EVENT_14,
463 463 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); // wait for an RTEMS_EVENT
464 464 intEventOut = (unsigned int) event_out;
465 465 for ( i=0; i<32; i++)
466 466 {
467 467 if ( ((intEventOut >> i) & 0x0001) != 0)
468 468 {
469 469 coarse_time = time_management_regs->coarse_time;
470 470 fine_time = time_management_regs->fine_time;
471 471 if (i==12)
472 472 {
473 473 PRINTF1("%s\n", DumbMessages[12])
474 474 }
475 475 if (i==13)
476 476 {
477 477 PRINTF1("%s\n", DumbMessages[13])
478 478 }
479 479 if (i==14)
480 480 {
481 481 PRINTF1("%s\n", DumbMessages[1])
482 482 }
483 483 }
484 484 }
485 485 }
486 486 }
487 487
488 488 //*****************************
489 489 // init housekeeping parameters
490 490
491 491 void init_housekeeping_parameters( void )
492 492 {
493 493 /** This function initialize the housekeeping_packet global variable with default values.
494 494 *
495 495 */
496 496
497 497 unsigned int i = 0;
498 498 unsigned char *parameters;
499 499 unsigned char sizeOfHK;
500 500
501 501 sizeOfHK = sizeof( Packet_TM_LFR_HK_t );
502 502
503 503 parameters = (unsigned char*) &housekeeping_packet;
504 504
505 505 for(i = 0; i< sizeOfHK; i++)
506 506 {
507 507 parameters[i] = 0x00;
508 508 }
509 509
510 510 housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
511 511 housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
512 512 housekeeping_packet.reserved = DEFAULT_RESERVED;
513 513 housekeeping_packet.userApplication = CCSDS_USER_APP;
514 514 housekeeping_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
515 515 housekeeping_packet.packetID[1] = (unsigned char) (APID_TM_HK);
516 516 housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
517 517 housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
518 518 housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
519 519 housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
520 520 housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
521 521 housekeeping_packet.serviceType = TM_TYPE_HK;
522 522 housekeeping_packet.serviceSubType = TM_SUBTYPE_HK;
523 523 housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND;
524 524 housekeeping_packet.sid = SID_HK;
525 525
526 526 // init status word
527 527 housekeeping_packet.lfr_status_word[0] = DEFAULT_STATUS_WORD_BYTE0;
528 528 housekeeping_packet.lfr_status_word[1] = DEFAULT_STATUS_WORD_BYTE1;
529 529 // init software version
530 530 housekeeping_packet.lfr_sw_version[0] = SW_VERSION_N1;
531 531 housekeeping_packet.lfr_sw_version[1] = SW_VERSION_N2;
532 532 housekeeping_packet.lfr_sw_version[2] = SW_VERSION_N3;
533 533 housekeeping_packet.lfr_sw_version[3] = SW_VERSION_N4;
534 534 // init fpga version
535 535 parameters = (unsigned char *) (REGS_ADDR_VHDL_VERSION);
536 536 housekeeping_packet.lfr_fpga_version[0] = parameters[1]; // n1
537 537 housekeeping_packet.lfr_fpga_version[1] = parameters[2]; // n2
538 538 housekeeping_packet.lfr_fpga_version[2] = parameters[3]; // n3
539 539
540 540 housekeeping_packet.hk_lfr_q_sd_fifo_size = MSG_QUEUE_COUNT_SEND;
541 541 housekeeping_packet.hk_lfr_q_rv_fifo_size = MSG_QUEUE_COUNT_RECV;
542 542 housekeeping_packet.hk_lfr_q_p0_fifo_size = MSG_QUEUE_COUNT_PRC0;
543 543 housekeeping_packet.hk_lfr_q_p1_fifo_size = MSG_QUEUE_COUNT_PRC1;
544 544 housekeeping_packet.hk_lfr_q_p2_fifo_size = MSG_QUEUE_COUNT_PRC2;
545 545 }
546 546
547 547 void increment_seq_counter( unsigned short *packetSequenceControl )
548 548 {
549 549 /** This function increment the sequence counter passes in argument.
550 550 *
551 551 * The increment does not affect the grouping flag. In case of an overflow, the counter is reset to 0.
552 552 *
553 553 */
554 554
555 555 unsigned short segmentation_grouping_flag;
556 556 unsigned short sequence_cnt;
557 557
558 558 segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << 8; // keep bits 7 downto 6
559 559 sequence_cnt = (*packetSequenceControl) & 0x3fff; // [0011 1111 1111 1111]
560 560
561 561 if ( sequence_cnt < SEQ_CNT_MAX)
562 562 {
563 563 sequence_cnt = sequence_cnt + 1;
564 564 }
565 565 else
566 566 {
567 567 sequence_cnt = 0;
568 568 }
569 569
570 570 *packetSequenceControl = segmentation_grouping_flag | sequence_cnt ;
571 571 }
572 572
573 573 void getTime( unsigned char *time)
574 574 {
575 575 /** This function write the current local time in the time buffer passed in argument.
576 576 *
577 577 */
578 578
579 579 time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
580 580 time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
581 581 time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
582 582 time[3] = (unsigned char) (time_management_regs->coarse_time);
583 583 time[4] = (unsigned char) (time_management_regs->fine_time>>8);
584 584 time[5] = (unsigned char) (time_management_regs->fine_time);
585 585 }
586 586
587 587 unsigned long long int getTimeAsUnsignedLongLongInt( )
588 588 {
589 589 /** This function write the current local time in the time buffer passed in argument.
590 590 *
591 591 */
592 592 unsigned long long int time;
593 593
594 594 time = ( (unsigned long long int) (time_management_regs->coarse_time & 0x7fffffff) << 16 )
595 595 + time_management_regs->fine_time;
596 596
597 597 return time;
598 598 }
599 599
600 600 void send_dumb_hk( void )
601 601 {
602 602 Packet_TM_LFR_HK_t dummy_hk_packet;
603 603 unsigned char *parameters;
604 604 unsigned int i;
605 605 rtems_id queue_id;
606 606
607 607 dummy_hk_packet.targetLogicalAddress = CCSDS_DESTINATION_ID;
608 608 dummy_hk_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID;
609 609 dummy_hk_packet.reserved = DEFAULT_RESERVED;
610 610 dummy_hk_packet.userApplication = CCSDS_USER_APP;
611 611 dummy_hk_packet.packetID[0] = (unsigned char) (APID_TM_HK >> 8);
612 612 dummy_hk_packet.packetID[1] = (unsigned char) (APID_TM_HK);
613 613 dummy_hk_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
614 614 dummy_hk_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
615 615 dummy_hk_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8);
616 616 dummy_hk_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK );
617 617 dummy_hk_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
618 618 dummy_hk_packet.serviceType = TM_TYPE_HK;
619 619 dummy_hk_packet.serviceSubType = TM_SUBTYPE_HK;
620 620 dummy_hk_packet.destinationID = TM_DESTINATION_ID_GROUND;
621 621 dummy_hk_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24);
622 622 dummy_hk_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16);
623 623 dummy_hk_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8);
624 624 dummy_hk_packet.time[3] = (unsigned char) (time_management_regs->coarse_time);
625 625 dummy_hk_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8);
626 626 dummy_hk_packet.time[5] = (unsigned char) (time_management_regs->fine_time);
627 627 dummy_hk_packet.sid = SID_HK;
628 628
629 629 // init status word
630 630 dummy_hk_packet.lfr_status_word[0] = 0xff;
631 631 dummy_hk_packet.lfr_status_word[1] = 0xff;
632 632 // init software version
633 633 dummy_hk_packet.lfr_sw_version[0] = SW_VERSION_N1;
634 634 dummy_hk_packet.lfr_sw_version[1] = SW_VERSION_N2;
635 635 dummy_hk_packet.lfr_sw_version[2] = SW_VERSION_N3;
636 636 dummy_hk_packet.lfr_sw_version[3] = SW_VERSION_N4;
637 637 // init fpga version
638 638 parameters = (unsigned char *) (REGS_ADDR_WAVEFORM_PICKER + 0xb0);
639 639 dummy_hk_packet.lfr_fpga_version[0] = parameters[1]; // n1
640 640 dummy_hk_packet.lfr_fpga_version[1] = parameters[2]; // n2
641 641 dummy_hk_packet.lfr_fpga_version[2] = parameters[3]; // n3
642 642
643 643 parameters = (unsigned char *) &dummy_hk_packet.hk_lfr_cpu_load;
644 644
645 645 for (i=0; i<100; i++)
646 646 {
647 647 parameters[i] = 0xff;
648 648 }
649 649
650 650 get_message_queue_id_send( &queue_id );
651 651
652 652 rtems_message_queue_send( queue_id, &dummy_hk_packet,
653 653 PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES);
654 654 }
655 655
656 656 void get_temperatures( unsigned char *temperatures )
657 657 {
658 658 unsigned char* temp_scm_ptr;
659 659 unsigned char* temp_pcb_ptr;
660 660 unsigned char* temp_fpga_ptr;
661 661
662 662 // SEL1 SEL0
663 663 // 0 0 => PCB
664 664 // 0 1 => FPGA
665 665 // 1 0 => SCM
666 666
667 667 temp_scm_ptr = (unsigned char *) &time_management_regs->temp_scm;
668 668 temp_pcb_ptr = (unsigned char *) &time_management_regs->temp_pcb;
669 669 temp_fpga_ptr = (unsigned char *) &time_management_regs->temp_fpga;
670 670
671 671 temperatures[0] = temp_scm_ptr[2];
672 672 temperatures[1] = temp_scm_ptr[3];
673 673 temperatures[2] = temp_pcb_ptr[2];
674 674 temperatures[3] = temp_pcb_ptr[3];
675 675 temperatures[4] = temp_fpga_ptr[2];
676 676 temperatures[5] = temp_fpga_ptr[3];
677 677 }
678 678
679 679 void get_v_e1_e2_f3( unsigned char *spacecraft_potential )
680 680 {
681 681 unsigned char* v_ptr;
682 682 unsigned char* e1_ptr;
683 683 unsigned char* e2_ptr;
684 684
685 685 v_ptr = (unsigned char *) &waveform_picker_regs->v;
686 686 e1_ptr = (unsigned char *) &waveform_picker_regs->e1;
687 687 e2_ptr = (unsigned char *) &waveform_picker_regs->e2;
688 688
689 689 spacecraft_potential[0] = v_ptr[2];
690 690 spacecraft_potential[1] = v_ptr[3];
691 691 spacecraft_potential[2] = e1_ptr[2];
692 692 spacecraft_potential[3] = e1_ptr[3];
693 693 spacecraft_potential[4] = e2_ptr[2];
694 694 spacecraft_potential[5] = e2_ptr[3];
695 695 }
696 696
697 697 void get_cpu_load( unsigned char *resource_statistics )
698 698 {
699 699 unsigned char cpu_load;
700 700
701 701 cpu_load = lfr_rtems_cpu_usage_report();
702 702
703 703 // HK_LFR_CPU_LOAD
704 704 resource_statistics[0] = cpu_load;
705 705
706 706 // HK_LFR_CPU_LOAD_MAX
707 707 if (cpu_load > resource_statistics[1])
708 708 {
709 709 resource_statistics[1] = cpu_load;
710 710 }
711 711
712 712 // CPU_LOAD_AVE
713 713 resource_statistics[2] = 0;
714 714
715 715 #ifndef PRINT_TASK_STATISTICS
716 716 rtems_cpu_usage_reset();
717 717 #endif
718 718
719 719 }
720 720
721 721 void set_hk_lfr_sc_potential_flag( bool state )
722 722 {
723 723 if (state == true)
724 724 {
725 725 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x40; // [0100 0000]
726 726 }
727 727 else
728 728 {
729 729 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xbf; // [1011 1111]
730 730 }
731 731 }
732 732
733 733 void set_sy_lfr_pas_filter_enabled( bool state )
734 734 {
735 735 if (state == true)
736 736 {
737 737 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x20; // [0010 0000]
738 738 }
739 739 else
740 740 {
741 741 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xdf; // [1101 1111]
742 742 }
743 743 }
744 744
745 745 void set_sy_lfr_watchdog_enabled( bool state )
746 746 {
747 747 if (state == true)
748 748 {
749 749 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x10; // [0001 0000]
750 750 }
751 751 else
752 752 {
753 753 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xef; // [1110 1111]
754 754 }
755 755 }
756 756
757 757 void set_hk_lfr_calib_enable( bool state )
758 758 {
759 759 if (state == true)
760 760 {
761 761 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] | 0x08; // [0000 1000]
762 762 }
763 763 else
764 764 {
765 765 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf7; // [1111 0111]
766 766 }
767 767 }
768 768
769 769 void set_hk_lfr_reset_cause( enum lfr_reset_cause_t lfr_reset_cause )
770 770 {
771 771 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1] & 0xf8; // [1111 1000]
772 772
773 773 housekeeping_packet.lfr_status_word[1] = housekeeping_packet.lfr_status_word[1]
774 774 | (lfr_reset_cause & 0x07 ); // [0000 0111]
775 775
776 776 }
777 777
778 void hk_lfr_le_me_he_update()
778 void increment_hk_counter( unsigned char newValue, unsigned char oldValue, unsigned int *counter )
779 779 {
780 unsigned int hk_lfr_le_cnt;
781 unsigned int hk_lfr_me_cnt;
782 unsigned int hk_lfr_he_cnt;
783 unsigned int current_hk_lfr_le_cnt;
784 unsigned int current_hk_lfr_me_cnt;
785 unsigned int current_hk_lfr_he_cnt;
780 int delta;
781
782 delta = 0;
786 783
787 hk_lfr_le_cnt = 0;
788 hk_lfr_me_cnt = 0;
789 hk_lfr_he_cnt = 0;
790 current_hk_lfr_le_cnt = ((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * 256 + housekeeping_packet.hk_lfr_le_cnt[1];
791 current_hk_lfr_me_cnt = ((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * 256 + housekeeping_packet.hk_lfr_me_cnt[1];
792 current_hk_lfr_he_cnt = ((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256 + housekeeping_packet.hk_lfr_he_cnt[1];
784 if (newValue >= oldValue)
785 {
786 delta = newValue - oldValue;
787 }
788 else
789 {
790 delta = 255 - oldValue + newValue;
791 }
792
793 *counter = *counter + delta;
794 }
793 795
794 //update the low severity error counter
795 hk_lfr_le_cnt =
796 current_hk_lfr_le_cnt
797 + housekeeping_packet.hk_lfr_dpu_spw_parity
798 + housekeeping_packet.hk_lfr_dpu_spw_disconnect
799 + housekeeping_packet.hk_lfr_dpu_spw_escape
800 + housekeeping_packet.hk_lfr_dpu_spw_credit
801 + housekeeping_packet.hk_lfr_dpu_spw_write_sync
802 + housekeeping_packet.hk_lfr_timecode_erroneous
803 + housekeeping_packet.hk_lfr_timecode_missing
804 + housekeeping_packet.hk_lfr_timecode_invalid
805 + housekeeping_packet.hk_lfr_time_timecode_it
806 + housekeeping_packet.hk_lfr_time_not_synchro
807 + housekeeping_packet.hk_lfr_time_timecode_ctr
808 + housekeeping_packet.hk_lfr_ahb_correctable;
796 void hk_lfr_le_update( void )
797 {
798 static hk_lfr_le_t old_hk_lfr_le = {0};
799 hk_lfr_le_t new_hk_lfr_le;
800 unsigned int counter;
801
802 counter = ((unsigned int) housekeeping_packet.hk_lfr_le_cnt[0]) * 256 + housekeeping_packet.hk_lfr_le_cnt[1];
803
804 // DPU
805 new_hk_lfr_le.dpu_spw_parity = housekeeping_packet.hk_lfr_dpu_spw_parity;
806 new_hk_lfr_le.dpu_spw_disconnect= housekeeping_packet.hk_lfr_dpu_spw_disconnect;
807 new_hk_lfr_le.dpu_spw_escape = housekeeping_packet.hk_lfr_dpu_spw_escape;
808 new_hk_lfr_le.dpu_spw_credit = housekeeping_packet.hk_lfr_dpu_spw_credit;
809 new_hk_lfr_le.dpu_spw_write_sync= housekeeping_packet.hk_lfr_dpu_spw_write_sync;
810 // TIMECODE
811 new_hk_lfr_le.timecode_erroneous= housekeeping_packet.hk_lfr_timecode_erroneous;
812 new_hk_lfr_le.timecode_missing = housekeeping_packet.hk_lfr_timecode_missing;
813 new_hk_lfr_le.timecode_invalid = housekeeping_packet.hk_lfr_timecode_invalid;
814 // TIME
815 new_hk_lfr_le.time_timecode_it = housekeeping_packet.hk_lfr_time_timecode_it;
816 new_hk_lfr_le.time_not_synchro = housekeeping_packet.hk_lfr_time_not_synchro;
817 new_hk_lfr_le.time_timecode_ctr = housekeeping_packet.hk_lfr_time_timecode_ctr;
818 //AHB
819 new_hk_lfr_le.ahb_correctable = housekeeping_packet.hk_lfr_ahb_correctable;
809 820 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
810 821 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
811 822
823 // update the le counter
824 // DPU
825 increment_hk_counter( new_hk_lfr_le.dpu_spw_parity, old_hk_lfr_le.dpu_spw_parity, counter );
826 increment_hk_counter( new_hk_lfr_le.dpu_spw_disconnect,old_hk_lfr_le.dpu_spw_disconnect, counter );
827 increment_hk_counter( new_hk_lfr_le.dpu_spw_escape, old_hk_lfr_le.dpu_spw_escape, counter );
828 increment_hk_counter( new_hk_lfr_le.dpu_spw_credit, old_hk_lfr_le.dpu_spw_credit, counter );
829 increment_hk_counter( new_hk_lfr_le.dpu_spw_write_sync,old_hk_lfr_le.dpu_spw_write_sync, counter );
830 // TIMECODE
831 increment_hk_counter( new_hk_lfr_le.timecode_erroneous,old_hk_lfr_le.timecode_erroneous, counter );
832 increment_hk_counter( new_hk_lfr_le.timecode_missing, old_hk_lfr_le.timecode_missing, counter );
833 increment_hk_counter( new_hk_lfr_le.timecode_invalid, old_hk_lfr_le.timecode_invalid, counter );
834 // TIME
835 increment_hk_counter( new_hk_lfr_le.time_timecode_it, old_hk_lfr_le.time_timecode_it, counter );
836 increment_hk_counter( new_hk_lfr_le.time_not_synchro, old_hk_lfr_le.time_not_synchro, counter );
837 increment_hk_counter( new_hk_lfr_le.time_timecode_ctr, old_hk_lfr_le.time_timecode_ctr, counter );
838 // AHB
839 increment_hk_counter( new_hk_lfr_le.ahb_correctable, old_hk_lfr_le.ahb_correctable, counter );
840
841 // DPU
842 old_hk_lfr_le.dpu_spw_parity = new_hk_lfr_le.dpu_spw_parity;
843 old_hk_lfr_le.dpu_spw_disconnect= new_hk_lfr_le.dpu_spw_disconnect;
844 old_hk_lfr_le.dpu_spw_escape = new_hk_lfr_le.dpu_spw_escape;
845 old_hk_lfr_le.dpu_spw_credit = new_hk_lfr_le.dpu_spw_credit;
846 old_hk_lfr_le.dpu_spw_write_sync= new_hk_lfr_le.dpu_spw_write_sync;
847 // TIMECODE
848 old_hk_lfr_le.timecode_erroneous= new_hk_lfr_le.timecode_erroneous;
849 old_hk_lfr_le.timecode_missing = new_hk_lfr_le.timecode_missing;
850 old_hk_lfr_le.timecode_invalid = new_hk_lfr_le.timecode_invalid;
851 // TIME
852 old_hk_lfr_le.time_timecode_it = new_hk_lfr_le.time_timecode_it;
853 old_hk_lfr_le.time_not_synchro = new_hk_lfr_le.time_not_synchro;
854 old_hk_lfr_le.time_timecode_ctr = new_hk_lfr_le.time_timecode_ctr;
855 //AHB
856 old_hk_lfr_le.ahb_correctable = new_hk_lfr_le.ahb_correctable;
857 // housekeeping_packet.hk_lfr_dpu_spw_rx_ahb => not handled by the grspw driver
858 // housekeeping_packet.hk_lfr_dpu_spw_tx_ahb => not handled by the grspw driver
859
860 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
861 // LE
862 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((hk_lfr_le_cnt & 0xff00) >> 8);
863 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (hk_lfr_le_cnt & 0x00ff);
864 }
865
866 void hk_lfr_me_update( void )
867 {
868 static hk_lfr_me_t old_hk_lfr_me = {0};
869 hk_lfr_me_t new_hk_lfr_me;
870 unsigned int counter;
871
872 counter = ((unsigned int) housekeeping_packet.hk_lfr_me_cnt[0]) * 256 + housekeeping_packet.hk_lfr_me_cnt[1];
873
874 // get the current values
875 new_hk_lfr_me.dpu_spw_early_eop = housekeeping_packet.hk_lfr_dpu_spw_early_eop;
876 new_hk_lfr_me.dpu_spw_invalid_addr = housekeeping_packet.hk_lfr_dpu_spw_invalid_addr;
877 new_hk_lfr_me.dpu_spw_eep = housekeeping_packet.hk_lfr_dpu_spw_eep;
878 new_hk_lfr_me.dpu_spw_rx_too_big = housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
879
880 // update the me counter
881 increment_hk_counter( new_hk_lfr_me.dpu_spw_early_eop, old_hk_lfr_me.dpu_spw_early_eop, counter );
882 increment_hk_counter( new_hk_lfr_me.dpu_spw_invalid_addr, old_hk_lfr_me.dpu_spw_invalid_addr, counter );
883 increment_hk_counter( new_hk_lfr_me.dpu_spw_eep, old_hk_lfr_me.dpu_spw_eep, counter );
884 increment_hk_counter( new_hk_lfr_me.dpu_spw_rx_too_big, old_hk_lfr_me.dpu_spw_rx_too_big, counter );
885
886 // store the counters for the next time
887 old_hk_lfr_me.dpu_spw_early_eop = new_hk_lfr_me.dpu_spw_early_eop;
888 old_hk_lfr_me.dpu_spw_invalid_addr = new_hk_lfr_me.dpu_spw_invalid_addr;
889 old_hk_lfr_me.dpu_spw_eep = new_hk_lfr_me.dpu_spw_eep;
890 old_hk_lfr_me.dpu_spw_rx_too_big = new_hk_lfr_me.dpu_spw_rx_too_big;
891
892 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
893 // ME
894 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((hk_lfr_me_cnt & 0xff00) >> 8);
895 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (hk_lfr_me_cnt & 0x00ff);
896 }
897
898 void hk_lfr_le_me_he_update()
899 {
900
901 unsigned int hk_lfr_he_cnt;
902
903 hk_lfr_he_cnt = ((unsigned int) housekeeping_packet.hk_lfr_he_cnt[0]) * 256 + housekeeping_packet.hk_lfr_he_cnt[1];
904
905 //update the low severity error counter
906 hk_lfr_le_update( );
907
812 908 //update the medium severity error counter
813 hk_lfr_me_cnt =
814 current_hk_lfr_me_cnt
815 + housekeeping_packet.hk_lfr_dpu_spw_early_eop
816 + housekeeping_packet.hk_lfr_dpu_spw_invalid_addr
817 + housekeeping_packet.hk_lfr_dpu_spw_eep
818 + housekeeping_packet.hk_lfr_dpu_spw_rx_too_big;
909 hk_lfr_me_update();
819 910
820 911 //update the high severity error counter
821 912 hk_lfr_he_cnt = 0;
822 913
823 914 // update housekeeping packet counters, convert unsigned int numbers in 2 bytes numbers
824 // LE
825 housekeeping_packet.hk_lfr_le_cnt[0] = (unsigned char) ((hk_lfr_le_cnt & 0xff00) >> 8);
826 housekeeping_packet.hk_lfr_le_cnt[1] = (unsigned char) (hk_lfr_le_cnt & 0x00ff);
827 // ME
828 housekeeping_packet.hk_lfr_me_cnt[0] = (unsigned char) ((hk_lfr_me_cnt & 0xff00) >> 8);
829 housekeeping_packet.hk_lfr_me_cnt[1] = (unsigned char) (hk_lfr_me_cnt & 0x00ff);
830 915 // HE
831 916 housekeeping_packet.hk_lfr_he_cnt[0] = (unsigned char) ((hk_lfr_he_cnt & 0xff00) >> 8);
832 917 housekeeping_packet.hk_lfr_he_cnt[1] = (unsigned char) (hk_lfr_he_cnt & 0x00ff);
833 918
834 919 }
835 920
836 921 void set_hk_lfr_time_not_synchro()
837 922 {
838 923 static unsigned char synchroLost = 1;
839 924 int synchronizationBit;
840 925
841 926 // get the synchronization bit
842 927 synchronizationBit = (time_management_regs->coarse_time & 0x80000000) >> 31; // 1000 0000 0000 0000
843 928
844 929 switch (synchronizationBit)
845 930 {
846 931 case 0:
847 932 if (synchroLost == 1)
848 933 {
849 934 synchroLost = 0;
850 935 }
851 936 break;
852 937 case 1:
853 938 if (synchroLost == 0 )
854 939 {
855 940 synchroLost = 1;
856 941 increase_unsigned_char_counter(&housekeeping_packet.hk_lfr_time_not_synchro);
857 942 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_NOT_SYNCHRO );
858 943 }
859 944 break;
860 945 default:
861 946 PRINTF1("in hk_lfr_time_not_synchro *** unexpected value for synchronizationBit = %d\n", synchronizationBit);
862 947 break;
863 948 }
864 949
865 950 }
866 951
867 952 void set_hk_lfr_ahb_correctable() // CRITICITY L
868 953 {
869 954 /** This function builds the error counter hk_lfr_ahb_correctable using the statistics provided
870 955 * by the Cache Control Register (ASI 2, offset 0) and in the Register Protection Control Register (ASR16) on the
871 956 * detected errors in the cache, in the integer unit and in the floating point unit.
872 957 *
873 958 * @param void
874 959 *
875 960 * @return void
876 961 *
877 962 * All errors are summed to set the value of the hk_lfr_ahb_correctable counter.
878 963 *
879 964 */
880 965
881 966 unsigned int ahb_correctable;
882 967 unsigned int instructionErrorCounter;
883 968 unsigned int dataErrorCounter;
884 969 unsigned int fprfErrorCounter;
885 970 unsigned int iurfErrorCounter;
886 971
887 972 CCR_getInstructionAndDataErrorCounters( &instructionErrorCounter, &dataErrorCounter);
888 973 ASR16_get_FPRF_IURF_ErrorCounters( &fprfErrorCounter, &iurfErrorCounter);
889 974
890 975 ahb_correctable = instructionErrorCounter
891 976 + dataErrorCounter
892 977 + fprfErrorCounter
893 978 + iurfErrorCounter
894 979 + housekeeping_packet.hk_lfr_ahb_correctable;
895 980
896 981 housekeeping_packet.hk_lfr_ahb_correctable = (unsigned char) (ahb_correctable & 0xff); // [1111 1111]
897 982
898 983 }
Show file before | Show file after | Hide comments
@@ -1,1609 +1,1609
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 unsigned char previousTimecodeCtr = 0;
26 26 unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER);
27 27
28 28 //***********
29 29 // RTEMS TASK
30 30 rtems_task spiq_task(rtems_task_argument unused)
31 31 {
32 32 /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver.
33 33 *
34 34 * @param unused is the starting argument of the RTEMS task
35 35 *
36 36 */
37 37
38 38 rtems_event_set event_out;
39 39 rtems_status_code status;
40 40 int linkStatus;
41 41
42 42 BOOT_PRINTF("in SPIQ *** \n")
43 43
44 44 while(true){
45 45 rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT
46 46 PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n")
47 47
48 48 // [0] SUSPEND RECV AND SEND TASKS
49 49 status = rtems_task_suspend( Task_id[ TASKID_RECV ] );
50 50 if ( status != RTEMS_SUCCESSFUL ) {
51 51 PRINTF("in SPIQ *** ERR suspending RECV Task\n")
52 52 }
53 53 status = rtems_task_suspend( Task_id[ TASKID_SEND ] );
54 54 if ( status != RTEMS_SUCCESSFUL ) {
55 55 PRINTF("in SPIQ *** ERR suspending SEND Task\n")
56 56 }
57 57
58 58 // [1] CHECK THE LINK
59 59 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1)
60 60 if ( linkStatus != 5) {
61 61 PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus)
62 62 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
63 63 }
64 64
65 65 // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT
66 66 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2)
67 67 if ( linkStatus != 5 ) // [2.a] not in run state, reset the link
68 68 {
69 69 spacewire_read_statistics();
70 70 status = spacewire_several_connect_attemps( );
71 71 }
72 72 else // [2.b] in run state, start the link
73 73 {
74 74 status = spacewire_stop_and_start_link( fdSPW ); // start the link
75 75 if ( status != RTEMS_SUCCESSFUL)
76 76 {
77 77 PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status)
78 78 }
79 79 }
80 80
81 81 // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS
82 82 if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully
83 83 {
84 84 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
85 85 if ( status != RTEMS_SUCCESSFUL ) {
86 86 PRINTF("in SPIQ *** ERR resuming SEND Task\n")
87 87 }
88 88 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
89 89 if ( status != RTEMS_SUCCESSFUL ) {
90 90 PRINTF("in SPIQ *** ERR resuming RECV Task\n")
91 91 }
92 92 }
93 93 else // [3.b] the link is not in run state, go in STANDBY mode
94 94 {
95 95 status = enter_mode_standby();
96 96 if ( status != RTEMS_SUCCESSFUL )
97 97 {
98 98 PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status)
99 99 }
100 100 {
101 101 updateLFRCurrentMode( LFR_MODE_STANDBY );
102 102 }
103 103 // wake the LINK task up to wait for the link recovery
104 104 status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 );
105 105 status = rtems_task_suspend( RTEMS_SELF );
106 106 }
107 107 }
108 108 }
109 109
110 110 rtems_task recv_task( rtems_task_argument unused )
111 111 {
112 112 /** This RTEMS task is dedicated to the reception of incoming TeleCommands.
113 113 *
114 114 * @param unused is the starting argument of the RTEMS task
115 115 *
116 116 * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked:
117 117 * 1. It reads the incoming data.
118 118 * 2. Launches the acceptance procedure.
119 119 * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue.
120 120 *
121 121 */
122 122
123 123 int len;
124 124 ccsdsTelecommandPacket_t currentTC;
125 125 unsigned char computed_CRC[ 2 ];
126 126 unsigned char currentTC_LEN_RCV[ 2 ];
127 127 unsigned char destinationID;
128 128 unsigned int estimatedPacketLength;
129 129 unsigned int parserCode;
130 130 rtems_status_code status;
131 131 rtems_id queue_recv_id;
132 132 rtems_id queue_send_id;
133 133
134 134 initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes
135 135
136 136 status = get_message_queue_id_recv( &queue_recv_id );
137 137 if (status != RTEMS_SUCCESSFUL)
138 138 {
139 139 PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status)
140 140 }
141 141
142 142 status = get_message_queue_id_send( &queue_send_id );
143 143 if (status != RTEMS_SUCCESSFUL)
144 144 {
145 145 PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status)
146 146 }
147 147
148 148 BOOT_PRINTF("in RECV *** \n")
149 149
150 150 while(1)
151 151 {
152 152 len = read( fdSPW, (char*) &currentTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking
153 153 if (len == -1){ // error during the read call
154 154 PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno)
155 155 }
156 156 else {
157 157 if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) {
158 158 PRINTF("in RECV *** packet lenght too short\n")
159 159 }
160 160 else {
161 161 estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes
162 PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
162 //PRINTF1("incoming TC with Length (byte): %d\n", len - 3);
163 163 currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8);
164 164 currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength );
165 165 // CHECK THE TC
166 166 parserCode = tc_parser( &currentTC, estimatedPacketLength, computed_CRC ) ;
167 167 if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT)
168 168 || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE)
169 169 || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA)
170 170 || (parserCode == WRONG_SRC_ID) )
171 171 { // send TM_LFR_TC_EXE_CORRUPTED
172 172 PRINTF1("TC corrupted received, with code: %d\n", parserCode);
173 173 if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) )
174 174 &&
175 175 !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO))
176 176 )
177 177 {
178 178 if ( parserCode == WRONG_SRC_ID )
179 179 {
180 180 destinationID = SID_TC_GROUND;
181 181 }
182 182 else
183 183 {
184 184 destinationID = currentTC.sourceID;
185 185 }
186 186 send_tm_lfr_tc_exe_corrupted( &currentTC, queue_send_id,
187 187 computed_CRC, currentTC_LEN_RCV,
188 188 destinationID );
189 189 }
190 190 }
191 191 else
192 192 { // send valid TC to the action launcher
193 193 status = rtems_message_queue_send( queue_recv_id, &currentTC,
194 194 estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3);
195 195 }
196 196 }
197 197 }
198 198
199 199 update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max );
200 200
201 201 }
202 202 }
203 203
204 204 rtems_task send_task( rtems_task_argument argument)
205 205 {
206 206 /** This RTEMS task is dedicated to the transmission of TeleMetry packets.
207 207 *
208 208 * @param unused is the starting argument of the RTEMS task
209 209 *
210 210 * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives:
211 211 * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call.
212 212 * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After
213 213 * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the
214 214 * data it contains.
215 215 *
216 216 */
217 217
218 218 rtems_status_code status; // RTEMS status code
219 219 char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer
220 220 ring_node *incomingRingNodePtr;
221 221 int ring_node_address;
222 222 char *charPtr;
223 223 spw_ioctl_pkt_send *spw_ioctl_send;
224 224 size_t size; // size of the incoming TC packet
225 225 rtems_id queue_send_id;
226 226 unsigned int sid;
227 227 unsigned char sidAsUnsignedChar;
228 228 unsigned char type;
229 229
230 230 incomingRingNodePtr = NULL;
231 231 ring_node_address = 0;
232 232 charPtr = (char *) &ring_node_address;
233 233 sid = 0;
234 234 sidAsUnsignedChar = 0;
235 235
236 236 init_header_cwf( &headerCWF );
237 237 init_header_swf( &headerSWF );
238 238 init_header_asm( &headerASM );
239 239
240 240 status = get_message_queue_id_send( &queue_send_id );
241 241 if (status != RTEMS_SUCCESSFUL)
242 242 {
243 243 PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status)
244 244 }
245 245
246 246 BOOT_PRINTF("in SEND *** \n")
247 247
248 248 while(1)
249 249 {
250 250 status = rtems_message_queue_receive( queue_send_id, incomingData, &size,
251 251 RTEMS_WAIT, RTEMS_NO_TIMEOUT );
252 252
253 253 if (status!=RTEMS_SUCCESSFUL)
254 254 {
255 255 PRINTF1("in SEND *** (1) ERR = %d\n", status)
256 256 }
257 257 else
258 258 {
259 259 if ( size == sizeof(ring_node*) )
260 260 {
261 261 charPtr[0] = incomingData[0];
262 262 charPtr[1] = incomingData[1];
263 263 charPtr[2] = incomingData[2];
264 264 charPtr[3] = incomingData[3];
265 265 incomingRingNodePtr = (ring_node*) ring_node_address;
266 266 sid = incomingRingNodePtr->sid;
267 267 if ( (sid==SID_NORM_CWF_LONG_F3)
268 268 || (sid==SID_BURST_CWF_F2 )
269 269 || (sid==SID_SBM1_CWF_F1 )
270 270 || (sid==SID_SBM2_CWF_F2 ))
271 271 {
272 272 spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF );
273 273 }
274 274 else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) )
275 275 {
276 276 spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF );
277 277 }
278 278 else if ( (sid==SID_NORM_CWF_F3) )
279 279 {
280 280 spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF );
281 281 }
282 282 else if (sid==SID_NORM_ASM_F0)
283 283 {
284 284 spw_send_asm_f0( incomingRingNodePtr, &headerASM );
285 285 }
286 286 else if (sid==SID_NORM_ASM_F1)
287 287 {
288 288 spw_send_asm_f1( incomingRingNodePtr, &headerASM );
289 289 }
290 290 else if (sid==SID_NORM_ASM_F2)
291 291 {
292 292 spw_send_asm_f2( incomingRingNodePtr, &headerASM );
293 293 }
294 294 else if ( sid==TM_CODE_K_DUMP )
295 295 {
296 296 spw_send_k_dump( incomingRingNodePtr );
297 297 }
298 298 else
299 299 {
300 300 PRINTF1("unexpected sid = %d\n", sid);
301 301 }
302 302 }
303 303 else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet
304 304 {
305 305 sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ];
306 306 sid = sidAsUnsignedChar;
307 307 type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ];
308 308 if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently
309 309 // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS
310 310 {
311 311 increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid );
312 312 }
313 313
314 314 status = write( fdSPW, incomingData, size );
315 315 if (status == -1){
316 316 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
317 317 }
318 318 }
319 319 else // the incoming message is a spw_ioctl_pkt_send structure
320 320 {
321 321 spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData;
322 322 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send );
323 323 if (status == -1){
324 324 PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status)
325 325 }
326 326 }
327 327 }
328 328
329 329 update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max );
330 330
331 331 }
332 332 }
333 333
334 334 rtems_task link_task( rtems_task_argument argument )
335 335 {
336 336 rtems_event_set event_out;
337 337 rtems_status_code status;
338 338 int linkStatus;
339 339
340 340 BOOT_PRINTF("in LINK ***\n")
341 341
342 342 while(1)
343 343 {
344 344 // wait for an RTEMS_EVENT
345 345 rtems_event_receive( RTEMS_EVENT_0,
346 346 RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
347 347 PRINTF("in LINK *** wait for the link\n")
348 348 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
349 349 while( linkStatus != 5) // wait for the link
350 350 {
351 351 status = rtems_task_wake_after( 10 ); // monitor the link each 100ms
352 352 status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status
353 353 watchdog_reload();
354 354 }
355 355
356 356 spacewire_read_statistics();
357 357 status = spacewire_stop_and_start_link( fdSPW );
358 358
359 359 if (status != RTEMS_SUCCESSFUL)
360 360 {
361 361 PRINTF1("in LINK *** ERR link not started %d\n", status)
362 362 }
363 363 else
364 364 {
365 365 PRINTF("in LINK *** OK link started\n")
366 366 }
367 367
368 368 // restart the SPIQ task
369 369 status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 );
370 370 if ( status != RTEMS_SUCCESSFUL ) {
371 371 PRINTF("in SPIQ *** ERR restarting SPIQ Task\n")
372 372 }
373 373
374 374 // restart RECV and SEND
375 375 status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 );
376 376 if ( status != RTEMS_SUCCESSFUL ) {
377 377 PRINTF("in SPIQ *** ERR restarting SEND Task\n")
378 378 }
379 379 status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 );
380 380 if ( status != RTEMS_SUCCESSFUL ) {
381 381 PRINTF("in SPIQ *** ERR restarting RECV Task\n")
382 382 }
383 383 }
384 384 }
385 385
386 386 //****************
387 387 // OTHER FUNCTIONS
388 388 int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);]
389 389 {
390 390 /** This function opens the SpaceWire link.
391 391 *
392 392 * @return a valid file descriptor in case of success, -1 in case of a failure
393 393 *
394 394 */
395 395 rtems_status_code status;
396 396
397 397 fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware
398 398 if ( fdSPW < 0 ) {
399 399 PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno)
400 400 }
401 401 else
402 402 {
403 403 status = RTEMS_SUCCESSFUL;
404 404 }
405 405
406 406 return status;
407 407 }
408 408
409 409 int spacewire_start_link( int fd )
410 410 {
411 411 rtems_status_code status;
412 412
413 413 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
414 414 // -1 default hardcoded driver timeout
415 415
416 416 return status;
417 417 }
418 418
419 419 int spacewire_stop_and_start_link( int fd )
420 420 {
421 421 rtems_status_code status;
422 422
423 423 status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0
424 424 status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started
425 425 // -1 default hardcoded driver timeout
426 426
427 427 return status;
428 428 }
429 429
430 430 int spacewire_configure_link( int fd )
431 431 {
432 432 /** This function configures the SpaceWire link.
433 433 *
434 434 * @return GR-RTEMS-DRIVER directive status codes:
435 435 * - 22 EINVAL - Null pointer or an out of range value was given as the argument.
436 436 * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode.
437 437 * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used.
438 438 * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up.
439 439 * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers.
440 440 * - 5 EIO - Error when writing to grswp hardware registers.
441 441 * - 2 ENOENT - No such file or directory
442 442 */
443 443
444 444 rtems_status_code status;
445 445
446 446 spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force
447 447 spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration
448 448 spw_ioctl_packetsize packetsize;
449 449
450 450 packetsize.rxsize = 228;
451 451 packetsize.txdsize = 4096;
452 452 packetsize.txhsize = 34;
453 453
454 454 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception
455 455 if (status!=RTEMS_SUCCESSFUL) {
456 456 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n")
457 457 }
458 458 //
459 459 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a
460 460 if (status!=RTEMS_SUCCESSFUL) {
461 461 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs
462 462 }
463 463 //
464 464 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts
465 465 if (status!=RTEMS_SUCCESSFUL) {
466 466 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n")
467 467 }
468 468 //
469 469 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit
470 470 if (status!=RTEMS_SUCCESSFUL) {
471 471 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n")
472 472 }
473 473 //
474 474 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks
475 475 if (status!=RTEMS_SUCCESSFUL) {
476 476 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n")
477 477 }
478 478 //
479 479 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available
480 480 if (status!=RTEMS_SUCCESSFUL) {
481 481 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n")
482 482 }
483 483 //
484 484 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ]
485 485 if (status!=RTEMS_SUCCESSFUL) {
486 486 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n")
487 487 }
488 488 //
489 489 status = ioctl(fd, SPACEWIRE_IOCTRL_SET_PACKETSIZE, packetsize); // set rxsize, txdsize and txhsize
490 490 if (status!=RTEMS_SUCCESSFUL) {
491 491 PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_PACKETSIZE,\n")
492 492 }
493 493
494 494 return status;
495 495 }
496 496
497 497 int spacewire_several_connect_attemps( void )
498 498 {
499 499 /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver.
500 500 *
501 501 * @return RTEMS directive status code:
502 502 * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s.
503 503 * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout.
504 504 *
505 505 */
506 506
507 507 rtems_status_code status_spw;
508 508 rtems_status_code status;
509 509 int i;
510 510
511 511 for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ )
512 512 {
513 513 PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i);
514 514
515 515 // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM
516 516
517 517 status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms
518 518
519 519 status_spw = spacewire_stop_and_start_link( fdSPW );
520 520
521 521 if ( status_spw != RTEMS_SUCCESSFUL )
522 522 {
523 523 PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw)
524 524 }
525 525
526 526 if ( status_spw == RTEMS_SUCCESSFUL)
527 527 {
528 528 break;
529 529 }
530 530 }
531 531
532 532 return status_spw;
533 533 }
534 534
535 535 void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force
536 536 {
537 537 /** This function sets the [N]o [P]ort force bit of the GRSPW control register.
538 538 *
539 539 * @param val is the value, 0 or 1, used to set the value of the NP bit.
540 540 * @param regAddr is the address of the GRSPW control register.
541 541 *
542 542 * NP is the bit 20 of the GRSPW control register.
543 543 *
544 544 */
545 545
546 546 unsigned int *spwptr = (unsigned int*) regAddr;
547 547
548 548 if (val == 1) {
549 549 *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit
550 550 }
551 551 if (val== 0) {
552 552 *spwptr = *spwptr & 0xffdfffff;
553 553 }
554 554 }
555 555
556 556 void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable
557 557 {
558 558 /** This function sets the [R]MAP [E]nable bit of the GRSPW control register.
559 559 *
560 560 * @param val is the value, 0 or 1, used to set the value of the RE bit.
561 561 * @param regAddr is the address of the GRSPW control register.
562 562 *
563 563 * RE is the bit 16 of the GRSPW control register.
564 564 *
565 565 */
566 566
567 567 unsigned int *spwptr = (unsigned int*) regAddr;
568 568
569 569 if (val == 1)
570 570 {
571 571 *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit
572 572 }
573 573 if (val== 0)
574 574 {
575 575 *spwptr = *spwptr & 0xfffdffff;
576 576 }
577 577 }
578 578
579 579 void spacewire_read_statistics( void )
580 580 {
581 581 /** This function reads the SpaceWire statistics from the grspw RTEMS driver.
582 582 *
583 583 * @param void
584 584 *
585 585 * @return void
586 586 *
587 587 * Once they are read, the counters are stored in a global variable used during the building of the
588 588 * HK packets.
589 589 *
590 590 */
591 591
592 592 rtems_status_code status;
593 593 spw_stats current;
594 594
595 595 spacewire_get_last_error();
596 596
597 597 // read the current statistics
598 598 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
599 599
600 600 // clear the counters
601 601 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS );
602 602
603 603 // typedef struct {
604 604 // unsigned int tx_link_err; // NOT IN HK
605 605 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
606 606 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
607 607 // unsigned int rx_eep_err;
608 608 // unsigned int rx_truncated;
609 609 // unsigned int parity_err;
610 610 // unsigned int escape_err;
611 611 // unsigned int credit_err;
612 612 // unsigned int write_sync_err;
613 613 // unsigned int disconnect_err;
614 614 // unsigned int early_ep;
615 615 // unsigned int invalid_address;
616 616 // unsigned int packets_sent;
617 617 // unsigned int packets_received;
618 618 // } spw_stats;
619 619
620 620 // rx_eep_err
621 621 grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err;
622 622 // rx_truncated
623 623 grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated;
624 624 // parity_err
625 625 grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err;
626 626 // escape_err
627 627 grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err;
628 628 // credit_err
629 629 grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err;
630 630 // write_sync_err
631 631 grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err;
632 632 // disconnect_err
633 633 grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err;
634 634 // early_ep
635 635 grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep;
636 636 // invalid_address
637 637 grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address;
638 638 // packets_sent
639 639 grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent;
640 640 // packets_received
641 641 grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received;
642 642
643 643 }
644 644
645 645 void spacewire_get_last_error( void )
646 646 {
647 647 static spw_stats previous;
648 648 spw_stats current;
649 649 rtems_status_code status;
650 650
651 651 unsigned int hk_lfr_last_er_rid;
652 652 unsigned char hk_lfr_last_er_code;
653 653 int coarseTime;
654 654 int fineTime;
655 655 unsigned char update_hk_lfr_last_er;
656 656
657 657 update_hk_lfr_last_er = 0;
658 658
659 659 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &current );
660 660
661 661 // get current time
662 662 coarseTime = time_management_regs->coarse_time;
663 663 fineTime = time_management_regs->fine_time;
664 664
665 665 // typedef struct {
666 666 // unsigned int tx_link_err; // NOT IN HK
667 667 // unsigned int rx_rmap_header_crc_err; // NOT IN HK
668 668 // unsigned int rx_rmap_data_crc_err; // NOT IN HK
669 669 // unsigned int rx_eep_err;
670 670 // unsigned int rx_truncated;
671 671 // unsigned int parity_err;
672 672 // unsigned int escape_err;
673 673 // unsigned int credit_err;
674 674 // unsigned int write_sync_err;
675 675 // unsigned int disconnect_err;
676 676 // unsigned int early_ep;
677 677 // unsigned int invalid_address;
678 678 // unsigned int packets_sent;
679 679 // unsigned int packets_received;
680 680 // } spw_stats;
681 681
682 682 // tx_link_err *** no code associated to this field
683 683 // rx_rmap_header_crc_err *** LE *** in HK
684 684 if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err)
685 685 {
686 686 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
687 687 hk_lfr_last_er_code = CODE_HEADER_CRC;
688 688 update_hk_lfr_last_er = 1;
689 689 }
690 690 // rx_rmap_data_crc_err *** LE *** NOT IN HK
691 691 if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err)
692 692 {
693 693 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
694 694 hk_lfr_last_er_code = CODE_DATA_CRC;
695 695 update_hk_lfr_last_er = 1;
696 696 }
697 697 // rx_eep_err
698 698 if (previous.rx_eep_err != current.rx_eep_err)
699 699 {
700 700 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
701 701 hk_lfr_last_er_code = CODE_EEP;
702 702 update_hk_lfr_last_er = 1;
703 703 }
704 704 // rx_truncated
705 705 if (previous.rx_truncated != current.rx_truncated)
706 706 {
707 707 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
708 708 hk_lfr_last_er_code = CODE_RX_TOO_BIG;
709 709 update_hk_lfr_last_er = 1;
710 710 }
711 711 // parity_err
712 712 if (previous.parity_err != current.parity_err)
713 713 {
714 714 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
715 715 hk_lfr_last_er_code = CODE_PARITY;
716 716 update_hk_lfr_last_er = 1;
717 717 }
718 718 // escape_err
719 719 if (previous.parity_err != current.parity_err)
720 720 {
721 721 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
722 722 hk_lfr_last_er_code = CODE_ESCAPE;
723 723 update_hk_lfr_last_er = 1;
724 724 }
725 725 // credit_err
726 726 if (previous.credit_err != current.credit_err)
727 727 {
728 728 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
729 729 hk_lfr_last_er_code = CODE_CREDIT;
730 730 update_hk_lfr_last_er = 1;
731 731 }
732 732 // write_sync_err
733 733 if (previous.write_sync_err != current.write_sync_err)
734 734 {
735 735 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
736 736 hk_lfr_last_er_code = CODE_WRITE_SYNC;
737 737 update_hk_lfr_last_er = 1;
738 738 }
739 739 // disconnect_err
740 740 if (previous.disconnect_err != current.disconnect_err)
741 741 {
742 742 hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW;
743 743 hk_lfr_last_er_code = CODE_DISCONNECT;
744 744 update_hk_lfr_last_er = 1;
745 745 }
746 746 // early_ep
747 747 if (previous.early_ep != current.early_ep)
748 748 {
749 749 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
750 750 hk_lfr_last_er_code = CODE_EARLY_EOP_EEP;
751 751 update_hk_lfr_last_er = 1;
752 752 }
753 753 // invalid_address
754 754 if (previous.invalid_address != current.invalid_address)
755 755 {
756 756 hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW;
757 757 hk_lfr_last_er_code = CODE_INVALID_ADDRESS;
758 758 update_hk_lfr_last_er = 1;
759 759 }
760 760
761 761 // if a field has changed, update the hk_last_er fields
762 762 if (update_hk_lfr_last_er == 1)
763 763 {
764 764 update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code );
765 765 }
766 766
767 767 previous = current;
768 768 }
769 769
770 770 void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code)
771 771 {
772 772 unsigned char *coarseTimePtr;
773 773 unsigned char *fineTimePtr;
774 774
775 775 coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time;
776 776 fineTimePtr = (unsigned char*) &time_management_regs->fine_time;
777 777
778 778 housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & 0xff00) >> 8 );
779 779 housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & 0x00ff);
780 780 housekeeping_packet.hk_lfr_last_er_code = code;
781 781 housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0];
782 782 housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1];
783 783 housekeeping_packet.hk_lfr_last_er_time[2] = coarseTimePtr[2];
784 784 housekeeping_packet.hk_lfr_last_er_time[3] = coarseTimePtr[3];
785 785 housekeeping_packet.hk_lfr_last_er_time[4] = fineTimePtr[2];
786 786 housekeeping_packet.hk_lfr_last_er_time[5] = fineTimePtr[3];
787 787 }
788 788
789 789 void update_hk_with_grspw_stats( void )
790 790 {
791 791 //****************************
792 792 // DPU_SPACEWIRE_IF_STATISTICS
793 793 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> 8);
794 794 housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received);
795 795 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> 8);
796 796 housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent);
797 797
798 798 //******************************************
799 799 // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY
800 800 housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err;
801 801 housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err;
802 802 housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err;
803 803 housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err;
804 804 housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err;
805 805
806 806 //*********************************************
807 807 // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY
808 808 housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep;
809 809 housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address;
810 810 housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err;
811 811 housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated;
812 812 }
813 813
814 814 void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 )
815 815 {
816 816 unsigned int *statusRegisterPtr;
817 817 unsigned char linkState;
818 818
819 819 statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER);
820 820 linkState = (unsigned char) ( ( (*statusRegisterPtr) >> 21) & 0x07); // [0000 0111]
821 821
822 822 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & 0xf8; // [1111 1000] set link state to 0
823 823
824 824 *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state
825 825 }
826 826
827 827 void increase_unsigned_char_counter( unsigned char *counter )
828 828 {
829 829 // update the number of valid timecodes that have been received
830 830 if (*counter == 255)
831 831 {
832 832 *counter = 0;
833 833 }
834 834 else
835 835 {
836 836 *counter = *counter + 1;
837 837 }
838 838 }
839 839
840 840 unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr)
841 841 {
842 842 /** This function checks the coherency between the incoming timecode and the last valid timecode.
843 843 *
844 844 * @param currentTimecodeCtr is the incoming timecode
845 845 *
846 846 * @return returned codes::
847 847 * - LFR_DEFAULT
848 848 * - LFR_SUCCESSFUL
849 849 *
850 850 */
851 851
852 852 static unsigned char firstTickout = 1;
853 853 unsigned char ret;
854 854
855 855 ret = LFR_DEFAULT;
856 856
857 857 if (firstTickout == 0)
858 858 {
859 859 if (currentTimecodeCtr == 0)
860 860 {
861 861 if (previousTimecodeCtr == 63)
862 862 {
863 863 ret = LFR_SUCCESSFUL;
864 864 }
865 865 else
866 866 {
867 867 ret = LFR_DEFAULT;
868 868 }
869 869 }
870 870 else
871 871 {
872 872 if (currentTimecodeCtr == (previousTimecodeCtr +1))
873 873 {
874 874 ret = LFR_SUCCESSFUL;
875 875 }
876 876 else
877 877 {
878 878 ret = LFR_DEFAULT;
879 879 }
880 880 }
881 881 }
882 882 else
883 883 {
884 884 firstTickout = 0;
885 885 ret = LFR_SUCCESSFUL;
886 886 }
887 887
888 888 return ret;
889 889 }
890 890
891 891 unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime)
892 892 {
893 893 unsigned int ret;
894 894
895 895 ret = LFR_DEFAULT;
896 896
897 897 if (timecode == internalTime)
898 898 {
899 899 ret = LFR_SUCCESSFUL;
900 900 }
901 901 else
902 902 {
903 903 ret = LFR_DEFAULT;
904 904 }
905 905
906 906 return ret;
907 907 }
908 908
909 909 void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc )
910 910 {
911 911 // a tickout has been emitted, perform actions on the incoming timecode
912 912
913 913 unsigned char incomingTimecode;
914 914 unsigned char updateTime;
915 915 unsigned char internalTime;
916 916 rtems_status_code status;
917 917
918 918 incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
919 919 updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK;
920 920 internalTime = time_management_regs->coarse_time & TIMECODE_MASK;
921 921
922 922 housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode;
923 923
924 924 // update the number of tickout that have been generated
925 925 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt );
926 926
927 927 //**************************
928 928 // HK_LFR_TIMECODE_ERRONEOUS
929 929 // MISSING and INVALID are handled by the timecode_timer_routine service routine
930 930 if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT)
931 931 {
932 932 // this is unexpected but a tickout could have been raised despite of the timecode being erroneous
933 933 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous );
934 934 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS );
935 935 }
936 936
937 937 //************************
938 938 // HK_LFR_TIME_TIMECODE_IT
939 939 // check the coherency between the SpaceWire timecode and the Internal Time
940 940 if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT)
941 941 {
942 942 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it );
943 943 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT );
944 944 }
945 945
946 946 //********************
947 947 // HK_LFR_TIMECODE_CTR
948 948 // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370
949 949 if (oneTcLfrUpdateTimeReceived == 1)
950 950 {
951 951 if ( incomingTimecode != updateTime )
952 952 {
953 953 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr );
954 954 update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR );
955 955 }
956 956 }
957 957
958 958 // launch the timecode timer to detect missing or invalid timecodes
959 959 previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value
960 960 status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL );
961 961 if (status != RTEMS_SUCCESSFUL)
962 962 {
963 963 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 );
964 964 }
965 965 }
966 966
967 967 rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data )
968 968 {
969 969 static unsigned char initStep = 1;
970 970
971 971 unsigned char currentTimecodeCtr;
972 972
973 973 currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK);
974 974
975 975 if (initStep == 1)
976 976 {
977 977 if (currentTimecodeCtr == previousTimecodeCtr)
978 978 {
979 979 //************************
980 980 // HK_LFR_TIMECODE_MISSING
981 981 // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING
982 982 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
983 983 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
984 984 }
985 985 else if (currentTimecodeCtr == (previousTimecodeCtr+1))
986 986 {
987 987 // the timecode value has changed and the value is valid, this is unexpected because
988 988 // the timer should not have fired, the timecode_irq_handler should have been raised
989 989 }
990 990 else
991 991 {
992 992 //************************
993 993 // HK_LFR_TIMECODE_INVALID
994 994 // the timecode value has changed and the value is not valid, no tickout has been generated
995 995 // this is why the timer has fired
996 996 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid );
997 997 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID );
998 998 }
999 999 }
1000 1000 else
1001 1001 {
1002 1002 initStep = 1;
1003 1003 //************************
1004 1004 // HK_LFR_TIMECODE_MISSING
1005 1005 increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing );
1006 1006 update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING );
1007 1007 }
1008 1008
1009 1009 rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 );
1010 1010 }
1011 1011
1012 1012 void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header )
1013 1013 {
1014 1014 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1015 1015 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1016 1016 header->reserved = DEFAULT_RESERVED;
1017 1017 header->userApplication = CCSDS_USER_APP;
1018 1018 header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE;
1019 1019 header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT;
1020 1020 header->packetLength[0] = 0x00;
1021 1021 header->packetLength[1] = 0x00;
1022 1022 // DATA FIELD HEADER
1023 1023 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1024 1024 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1025 1025 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1026 1026 header->destinationID = TM_DESTINATION_ID_GROUND;
1027 1027 header->time[0] = 0x00;
1028 1028 header->time[0] = 0x00;
1029 1029 header->time[0] = 0x00;
1030 1030 header->time[0] = 0x00;
1031 1031 header->time[0] = 0x00;
1032 1032 header->time[0] = 0x00;
1033 1033 // AUXILIARY DATA HEADER
1034 1034 header->sid = 0x00;
1035 1035 header->pa_bia_status_info = DEFAULT_HKBIA;
1036 1036 header->blkNr[0] = 0x00;
1037 1037 header->blkNr[1] = 0x00;
1038 1038 }
1039 1039
1040 1040 void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header )
1041 1041 {
1042 1042 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1043 1043 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1044 1044 header->reserved = DEFAULT_RESERVED;
1045 1045 header->userApplication = CCSDS_USER_APP;
1046 1046 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1047 1047 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1048 1048 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1049 1049 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1050 1050 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1051 1051 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1052 1052 // DATA FIELD HEADER
1053 1053 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1054 1054 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1055 1055 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype
1056 1056 header->destinationID = TM_DESTINATION_ID_GROUND;
1057 1057 header->time[0] = 0x00;
1058 1058 header->time[0] = 0x00;
1059 1059 header->time[0] = 0x00;
1060 1060 header->time[0] = 0x00;
1061 1061 header->time[0] = 0x00;
1062 1062 header->time[0] = 0x00;
1063 1063 // AUXILIARY DATA HEADER
1064 1064 header->sid = 0x00;
1065 1065 header->pa_bia_status_info = DEFAULT_HKBIA;
1066 1066 header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT
1067 1067 header->pktNr = 0x00;
1068 1068 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1069 1069 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1070 1070 }
1071 1071
1072 1072 void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header )
1073 1073 {
1074 1074 header->targetLogicalAddress = CCSDS_DESTINATION_ID;
1075 1075 header->protocolIdentifier = CCSDS_PROTOCOLE_ID;
1076 1076 header->reserved = DEFAULT_RESERVED;
1077 1077 header->userApplication = CCSDS_USER_APP;
1078 1078 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1079 1079 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1080 1080 header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
1081 1081 header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT;
1082 1082 header->packetLength[0] = 0x00;
1083 1083 header->packetLength[1] = 0x00;
1084 1084 // DATA FIELD HEADER
1085 1085 header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2;
1086 1086 header->serviceType = TM_TYPE_LFR_SCIENCE; // service type
1087 1087 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
1088 1088 header->destinationID = TM_DESTINATION_ID_GROUND;
1089 1089 header->time[0] = 0x00;
1090 1090 header->time[0] = 0x00;
1091 1091 header->time[0] = 0x00;
1092 1092 header->time[0] = 0x00;
1093 1093 header->time[0] = 0x00;
1094 1094 header->time[0] = 0x00;
1095 1095 // AUXILIARY DATA HEADER
1096 1096 header->sid = 0x00;
1097 1097 header->pa_bia_status_info = 0x00;
1098 1098 header->pa_lfr_pkt_cnt_asm = 0x00;
1099 1099 header->pa_lfr_pkt_nr_asm = 0x00;
1100 1100 header->pa_lfr_asm_blk_nr[0] = 0x00;
1101 1101 header->pa_lfr_asm_blk_nr[1] = 0x00;
1102 1102 }
1103 1103
1104 1104 int spw_send_waveform_CWF( ring_node *ring_node_to_send,
1105 1105 Header_TM_LFR_SCIENCE_CWF_t *header )
1106 1106 {
1107 1107 /** This function sends CWF CCSDS packets (F2, F1 or F0).
1108 1108 *
1109 1109 * @param waveform points to the buffer containing the data that will be send.
1110 1110 * @param sid is the source identifier of the data that will be sent.
1111 1111 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1112 1112 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1113 1113 * contain information to setup the transmission of the data packets.
1114 1114 *
1115 1115 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1116 1116 *
1117 1117 */
1118 1118
1119 1119 unsigned int i;
1120 1120 int ret;
1121 1121 unsigned int coarseTime;
1122 1122 unsigned int fineTime;
1123 1123 rtems_status_code status;
1124 1124 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1125 1125 int *dataPtr;
1126 1126 unsigned char sid;
1127 1127
1128 1128 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1129 1129 spw_ioctl_send_CWF.options = 0;
1130 1130
1131 1131 ret = LFR_DEFAULT;
1132 1132 sid = (unsigned char) ring_node_to_send->sid;
1133 1133
1134 1134 coarseTime = ring_node_to_send->coarseTime;
1135 1135 fineTime = ring_node_to_send->fineTime;
1136 1136 dataPtr = (int*) ring_node_to_send->buffer_address;
1137 1137
1138 1138 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8);
1139 1139 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 );
1140 1140 header->pa_bia_status_info = pa_bia_status_info;
1141 1141 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1142 1142 header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8);
1143 1143 header->blkNr[1] = (unsigned char) (BLK_NR_CWF );
1144 1144
1145 1145 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform
1146 1146 {
1147 1147 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ];
1148 1148 spw_ioctl_send_CWF.hdr = (char*) header;
1149 1149 // BUILD THE DATA
1150 1150 spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK;
1151 1151
1152 1152 // SET PACKET SEQUENCE CONTROL
1153 1153 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1154 1154
1155 1155 // SET SID
1156 1156 header->sid = sid;
1157 1157
1158 1158 // SET PACKET TIME
1159 1159 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime);
1160 1160 //
1161 1161 header->time[0] = header->acquisitionTime[0];
1162 1162 header->time[1] = header->acquisitionTime[1];
1163 1163 header->time[2] = header->acquisitionTime[2];
1164 1164 header->time[3] = header->acquisitionTime[3];
1165 1165 header->time[4] = header->acquisitionTime[4];
1166 1166 header->time[5] = header->acquisitionTime[5];
1167 1167
1168 1168 // SET PACKET ID
1169 1169 if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) )
1170 1170 {
1171 1171 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8);
1172 1172 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2);
1173 1173 }
1174 1174 else
1175 1175 {
1176 1176 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1177 1177 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1178 1178 }
1179 1179
1180 1180 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1181 1181 if (status != RTEMS_SUCCESSFUL) {
1182 1182 ret = LFR_DEFAULT;
1183 1183 }
1184 1184 }
1185 1185
1186 1186 return ret;
1187 1187 }
1188 1188
1189 1189 int spw_send_waveform_SWF( ring_node *ring_node_to_send,
1190 1190 Header_TM_LFR_SCIENCE_SWF_t *header )
1191 1191 {
1192 1192 /** This function sends SWF CCSDS packets (F2, F1 or F0).
1193 1193 *
1194 1194 * @param waveform points to the buffer containing the data that will be send.
1195 1195 * @param sid is the source identifier of the data that will be sent.
1196 1196 * @param headerSWF points to a table of headers that have been prepared for the data transmission.
1197 1197 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1198 1198 * contain information to setup the transmission of the data packets.
1199 1199 *
1200 1200 * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks.
1201 1201 *
1202 1202 */
1203 1203
1204 1204 unsigned int i;
1205 1205 int ret;
1206 1206 unsigned int coarseTime;
1207 1207 unsigned int fineTime;
1208 1208 rtems_status_code status;
1209 1209 spw_ioctl_pkt_send spw_ioctl_send_SWF;
1210 1210 int *dataPtr;
1211 1211 unsigned char sid;
1212 1212
1213 1213 spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF;
1214 1214 spw_ioctl_send_SWF.options = 0;
1215 1215
1216 1216 ret = LFR_DEFAULT;
1217 1217
1218 1218 coarseTime = ring_node_to_send->coarseTime;
1219 1219 fineTime = ring_node_to_send->fineTime;
1220 1220 dataPtr = (int*) ring_node_to_send->buffer_address;
1221 1221 sid = ring_node_to_send->sid;
1222 1222
1223 1223 header->pa_bia_status_info = pa_bia_status_info;
1224 1224 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1225 1225
1226 1226 for (i=0; i<7; i++) // send waveform
1227 1227 {
1228 1228 spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ];
1229 1229 spw_ioctl_send_SWF.hdr = (char*) header;
1230 1230
1231 1231 // SET PACKET SEQUENCE CONTROL
1232 1232 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1233 1233
1234 1234 // SET PACKET LENGTH AND BLKNR
1235 1235 if (i == 6)
1236 1236 {
1237 1237 spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK;
1238 1238 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8);
1239 1239 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 );
1240 1240 header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8);
1241 1241 header->blkNr[1] = (unsigned char) (BLK_NR_224 );
1242 1242 }
1243 1243 else
1244 1244 {
1245 1245 spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK;
1246 1246 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8);
1247 1247 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 );
1248 1248 header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8);
1249 1249 header->blkNr[1] = (unsigned char) (BLK_NR_304 );
1250 1250 }
1251 1251
1252 1252 // SET PACKET TIME
1253 1253 compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime );
1254 1254 //
1255 1255 header->time[0] = header->acquisitionTime[0];
1256 1256 header->time[1] = header->acquisitionTime[1];
1257 1257 header->time[2] = header->acquisitionTime[2];
1258 1258 header->time[3] = header->acquisitionTime[3];
1259 1259 header->time[4] = header->acquisitionTime[4];
1260 1260 header->time[5] = header->acquisitionTime[5];
1261 1261
1262 1262 // SET SID
1263 1263 header->sid = sid;
1264 1264
1265 1265 // SET PKTNR
1266 1266 header->pktNr = i+1; // PKT_NR
1267 1267
1268 1268 // SEND PACKET
1269 1269 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF );
1270 1270 if (status != RTEMS_SUCCESSFUL) {
1271 1271 ret = LFR_DEFAULT;
1272 1272 }
1273 1273 }
1274 1274
1275 1275 return ret;
1276 1276 }
1277 1277
1278 1278 int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send,
1279 1279 Header_TM_LFR_SCIENCE_CWF_t *header )
1280 1280 {
1281 1281 /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
1282 1282 *
1283 1283 * @param waveform points to the buffer containing the data that will be send.
1284 1284 * @param headerCWF points to a table of headers that have been prepared for the data transmission.
1285 1285 * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures
1286 1286 * contain information to setup the transmission of the data packets.
1287 1287 *
1288 1288 * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer
1289 1289 * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks.
1290 1290 *
1291 1291 */
1292 1292
1293 1293 unsigned int i;
1294 1294 int ret;
1295 1295 unsigned int coarseTime;
1296 1296 unsigned int fineTime;
1297 1297 rtems_status_code status;
1298 1298 spw_ioctl_pkt_send spw_ioctl_send_CWF;
1299 1299 char *dataPtr;
1300 1300 unsigned char sid;
1301 1301
1302 1302 spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF;
1303 1303 spw_ioctl_send_CWF.options = 0;
1304 1304
1305 1305 ret = LFR_DEFAULT;
1306 1306 sid = ring_node_to_send->sid;
1307 1307
1308 1308 coarseTime = ring_node_to_send->coarseTime;
1309 1309 fineTime = ring_node_to_send->fineTime;
1310 1310 dataPtr = (char*) ring_node_to_send->buffer_address;
1311 1311
1312 1312 header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8);
1313 1313 header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 );
1314 1314 header->pa_bia_status_info = pa_bia_status_info;
1315 1315 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1316 1316 header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8);
1317 1317 header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 );
1318 1318
1319 1319 //*********************
1320 1320 // SEND CWF3_light DATA
1321 1321 for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform
1322 1322 {
1323 1323 spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ];
1324 1324 spw_ioctl_send_CWF.hdr = (char*) header;
1325 1325 // BUILD THE DATA
1326 1326 spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK;
1327 1327
1328 1328 // SET PACKET SEQUENCE COUNTER
1329 1329 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1330 1330
1331 1331 // SET SID
1332 1332 header->sid = sid;
1333 1333
1334 1334 // SET PACKET TIME
1335 1335 compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime );
1336 1336 //
1337 1337 header->time[0] = header->acquisitionTime[0];
1338 1338 header->time[1] = header->acquisitionTime[1];
1339 1339 header->time[2] = header->acquisitionTime[2];
1340 1340 header->time[3] = header->acquisitionTime[3];
1341 1341 header->time[4] = header->acquisitionTime[4];
1342 1342 header->time[5] = header->acquisitionTime[5];
1343 1343
1344 1344 // SET PACKET ID
1345 1345 header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8);
1346 1346 header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST);
1347 1347
1348 1348 // SEND PACKET
1349 1349 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF );
1350 1350 if (status != RTEMS_SUCCESSFUL) {
1351 1351 ret = LFR_DEFAULT;
1352 1352 }
1353 1353 }
1354 1354
1355 1355 return ret;
1356 1356 }
1357 1357
1358 1358 void spw_send_asm_f0( ring_node *ring_node_to_send,
1359 1359 Header_TM_LFR_SCIENCE_ASM_t *header )
1360 1360 {
1361 1361 unsigned int i;
1362 1362 unsigned int length = 0;
1363 1363 rtems_status_code status;
1364 1364 unsigned int sid;
1365 1365 float *spectral_matrix;
1366 1366 int coarseTime;
1367 1367 int fineTime;
1368 1368 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1369 1369
1370 1370 sid = ring_node_to_send->sid;
1371 1371 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1372 1372 coarseTime = ring_node_to_send->coarseTime;
1373 1373 fineTime = ring_node_to_send->fineTime;
1374 1374
1375 1375 header->pa_bia_status_info = pa_bia_status_info;
1376 1376 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1377 1377
1378 1378 for (i=0; i<3; i++)
1379 1379 {
1380 1380 if ((i==0) || (i==1))
1381 1381 {
1382 1382 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1;
1383 1383 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1384 1384 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1385 1385 ];
1386 1386 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1;
1387 1387 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1388 1388 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB
1389 1389 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB
1390 1390 }
1391 1391 else
1392 1392 {
1393 1393 spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2;
1394 1394 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1395 1395 ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM )
1396 1396 ];
1397 1397 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2;
1398 1398 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1399 1399 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB
1400 1400 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB
1401 1401 }
1402 1402
1403 1403 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1404 1404 spw_ioctl_send_ASM.hdr = (char *) header;
1405 1405 spw_ioctl_send_ASM.options = 0;
1406 1406
1407 1407 // (2) BUILD THE HEADER
1408 1408 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1409 1409 header->packetLength[0] = (unsigned char) (length>>8);
1410 1410 header->packetLength[1] = (unsigned char) (length);
1411 1411 header->sid = (unsigned char) sid; // SID
1412 1412 header->pa_lfr_pkt_cnt_asm = 3;
1413 1413 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1414 1414
1415 1415 // (3) SET PACKET TIME
1416 1416 header->time[0] = (unsigned char) (coarseTime>>24);
1417 1417 header->time[1] = (unsigned char) (coarseTime>>16);
1418 1418 header->time[2] = (unsigned char) (coarseTime>>8);
1419 1419 header->time[3] = (unsigned char) (coarseTime);
1420 1420 header->time[4] = (unsigned char) (fineTime>>8);
1421 1421 header->time[5] = (unsigned char) (fineTime);
1422 1422 //
1423 1423 header->acquisitionTime[0] = header->time[0];
1424 1424 header->acquisitionTime[1] = header->time[1];
1425 1425 header->acquisitionTime[2] = header->time[2];
1426 1426 header->acquisitionTime[3] = header->time[3];
1427 1427 header->acquisitionTime[4] = header->time[4];
1428 1428 header->acquisitionTime[5] = header->time[5];
1429 1429
1430 1430 // (4) SEND PACKET
1431 1431 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1432 1432 if (status != RTEMS_SUCCESSFUL) {
1433 1433 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1434 1434 }
1435 1435 }
1436 1436 }
1437 1437
1438 1438 void spw_send_asm_f1( ring_node *ring_node_to_send,
1439 1439 Header_TM_LFR_SCIENCE_ASM_t *header )
1440 1440 {
1441 1441 unsigned int i;
1442 1442 unsigned int length = 0;
1443 1443 rtems_status_code status;
1444 1444 unsigned int sid;
1445 1445 float *spectral_matrix;
1446 1446 int coarseTime;
1447 1447 int fineTime;
1448 1448 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1449 1449
1450 1450 sid = ring_node_to_send->sid;
1451 1451 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1452 1452 coarseTime = ring_node_to_send->coarseTime;
1453 1453 fineTime = ring_node_to_send->fineTime;
1454 1454
1455 1455 header->pa_bia_status_info = pa_bia_status_info;
1456 1456 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1457 1457
1458 1458 for (i=0; i<3; i++)
1459 1459 {
1460 1460 if ((i==0) || (i==1))
1461 1461 {
1462 1462 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1;
1463 1463 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1464 1464 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1465 1465 ];
1466 1466 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1;
1467 1467 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1468 1468 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB
1469 1469 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB
1470 1470 }
1471 1471 else
1472 1472 {
1473 1473 spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2;
1474 1474 spw_ioctl_send_ASM.data = (char*) &spectral_matrix[
1475 1475 ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM )
1476 1476 ];
1477 1477 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2;
1478 1478 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6;
1479 1479 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB
1480 1480 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB
1481 1481 }
1482 1482
1483 1483 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1484 1484 spw_ioctl_send_ASM.hdr = (char *) header;
1485 1485 spw_ioctl_send_ASM.options = 0;
1486 1486
1487 1487 // (2) BUILD THE HEADER
1488 1488 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1489 1489 header->packetLength[0] = (unsigned char) (length>>8);
1490 1490 header->packetLength[1] = (unsigned char) (length);
1491 1491 header->sid = (unsigned char) sid; // SID
1492 1492 header->pa_lfr_pkt_cnt_asm = 3;
1493 1493 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1494 1494
1495 1495 // (3) SET PACKET TIME
1496 1496 header->time[0] = (unsigned char) (coarseTime>>24);
1497 1497 header->time[1] = (unsigned char) (coarseTime>>16);
1498 1498 header->time[2] = (unsigned char) (coarseTime>>8);
1499 1499 header->time[3] = (unsigned char) (coarseTime);
1500 1500 header->time[4] = (unsigned char) (fineTime>>8);
1501 1501 header->time[5] = (unsigned char) (fineTime);
1502 1502 //
1503 1503 header->acquisitionTime[0] = header->time[0];
1504 1504 header->acquisitionTime[1] = header->time[1];
1505 1505 header->acquisitionTime[2] = header->time[2];
1506 1506 header->acquisitionTime[3] = header->time[3];
1507 1507 header->acquisitionTime[4] = header->time[4];
1508 1508 header->acquisitionTime[5] = header->time[5];
1509 1509
1510 1510 // (4) SEND PACKET
1511 1511 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1512 1512 if (status != RTEMS_SUCCESSFUL) {
1513 1513 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1514 1514 }
1515 1515 }
1516 1516 }
1517 1517
1518 1518 void spw_send_asm_f2( ring_node *ring_node_to_send,
1519 1519 Header_TM_LFR_SCIENCE_ASM_t *header )
1520 1520 {
1521 1521 unsigned int i;
1522 1522 unsigned int length = 0;
1523 1523 rtems_status_code status;
1524 1524 unsigned int sid;
1525 1525 float *spectral_matrix;
1526 1526 int coarseTime;
1527 1527 int fineTime;
1528 1528 spw_ioctl_pkt_send spw_ioctl_send_ASM;
1529 1529
1530 1530 sid = ring_node_to_send->sid;
1531 1531 spectral_matrix = (float*) ring_node_to_send->buffer_address;
1532 1532 coarseTime = ring_node_to_send->coarseTime;
1533 1533 fineTime = ring_node_to_send->fineTime;
1534 1534
1535 1535 header->pa_bia_status_info = pa_bia_status_info;
1536 1536 header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters;
1537 1537
1538 1538 for (i=0; i<3; i++)
1539 1539 {
1540 1540
1541 1541 spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT;
1542 1542 spw_ioctl_send_ASM.data = (char *) &spectral_matrix[
1543 1543 ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM )
1544 1544 ];
1545 1545 length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2;
1546 1546 header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3;
1547 1547 header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB
1548 1548 header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB
1549 1549
1550 1550 spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM;
1551 1551 spw_ioctl_send_ASM.hdr = (char *) header;
1552 1552 spw_ioctl_send_ASM.options = 0;
1553 1553
1554 1554 // (2) BUILD THE HEADER
1555 1555 increment_seq_counter_source_id( header->packetSequenceControl, sid );
1556 1556 header->packetLength[0] = (unsigned char) (length>>8);
1557 1557 header->packetLength[1] = (unsigned char) (length);
1558 1558 header->sid = (unsigned char) sid; // SID
1559 1559 header->pa_lfr_pkt_cnt_asm = 3;
1560 1560 header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1);
1561 1561
1562 1562 // (3) SET PACKET TIME
1563 1563 header->time[0] = (unsigned char) (coarseTime>>24);
1564 1564 header->time[1] = (unsigned char) (coarseTime>>16);
1565 1565 header->time[2] = (unsigned char) (coarseTime>>8);
1566 1566 header->time[3] = (unsigned char) (coarseTime);
1567 1567 header->time[4] = (unsigned char) (fineTime>>8);
1568 1568 header->time[5] = (unsigned char) (fineTime);
1569 1569 //
1570 1570 header->acquisitionTime[0] = header->time[0];
1571 1571 header->acquisitionTime[1] = header->time[1];
1572 1572 header->acquisitionTime[2] = header->time[2];
1573 1573 header->acquisitionTime[3] = header->time[3];
1574 1574 header->acquisitionTime[4] = header->time[4];
1575 1575 header->acquisitionTime[5] = header->time[5];
1576 1576
1577 1577 // (4) SEND PACKET
1578 1578 status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM );
1579 1579 if (status != RTEMS_SUCCESSFUL) {
1580 1580 PRINTF1("in ASM_send *** ERR %d\n", (int) status)
1581 1581 }
1582 1582 }
1583 1583 }
1584 1584
1585 1585 void spw_send_k_dump( ring_node *ring_node_to_send )
1586 1586 {
1587 1587 rtems_status_code status;
1588 1588 Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump;
1589 1589 unsigned int packetLength;
1590 1590 unsigned int size;
1591 1591
1592 1592 PRINTF("spw_send_k_dump\n")
1593 1593
1594 1594 kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address;
1595 1595
1596 1596 packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1];
1597 1597
1598 1598 size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES;
1599 1599
1600 1600 PRINTF2("packetLength %d, size %d\n", packetLength, size )
1601 1601
1602 1602 status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size );
1603 1603
1604 1604 if (status == -1){
1605 1605 PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size)
1606 1606 }
1607 1607
1608 1608 ring_node_to_send->status = 0x00;
1609 1609 }
Show file before | Show file after | Hide comments
@@ -1,115 +1,115
1 1 /*
2 2 * CPU Usage Reporter
3 3 *
4 4 * COPYRIGHT (c) 1989-2009
5 5 * On-Line Applications Research Corporation (OAR).
6 6 *
7 7 * The license and distribution terms for this file may be
8 8 * found in the file LICENSE in this distribution or at
9 9 * http://www.rtems.com/license/LICENSE.
10 10 *
11 11 * $Id$
12 12 */
13 13
14 14 #include "lfr_cpu_usage_report.h"
15 15
16 16 unsigned char lfr_rtems_cpu_usage_report( void )
17 17 {
18 18 uint32_t api_index;
19 19 Thread_Control *the_thread;
20 20 Objects_Information *information;
21 21 uint32_t ival, fval;
22 22 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
23 23 Timestamp_Control uptime;
24 24 Timestamp_Control total;
25 25 Timestamp_Control ran;
26 26 #else
27 27 uint32_t total_units = 0;
28 28 #endif
29 29
30 30 unsigned char cpu_load;
31 31 cpu_load = 0;
32 32
33 33 /*
34 34 * When not using nanosecond CPU usage resolution, we have to count
35 35 * the number of "ticks" we gave credit for to give the user a rough
36 36 * guideline as to what each number means proportionally.
37 37 */
38 38 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
39 39 _TOD_Get_uptime( &uptime );
40 40 _Timestamp_Subtract( &CPU_usage_Uptime_at_last_reset, &uptime, &total );
41 41 #else
42 42 for ( api_index = 1 ; api_index <= OBJECTS_APIS_LAST ; api_index++ ) {
43 43 if ( !_Objects_Information_table[ api_index ] ) { }
44 44 else
45 45 {
46 46 information = _Objects_Information_table[ api_index ][ 1 ];
47 47 if ( information != NULL )
48 48 {
49 49 for ( i=1 ; i <= information->maximum ; i++ ) {
50 50 the_thread = (Thread_Control *)information->local_table[ i ];
51 51
52 if ( the_thread != NULL )
53 total_units += the_thread->cpu_time_used;
52 if ( the_thread != NULL ) {
53 total_units += the_thread->cpu_time_used; }
54 54 }
55 55 }
56 56 }
57 57 }
58 58 #endif
59 59
60 60 for ( api_index = 1 ; api_index <= OBJECTS_APIS_LAST ; api_index++ )
61 61 {
62 62 if ( !_Objects_Information_table[ api_index ] ) { }
63 63 else
64 64 {
65 65 information = _Objects_Information_table[ api_index ][ 1 ];
66 66 if ( information != NULL )
67 67 {
68 68 the_thread = (Thread_Control *)information->local_table[ 1 ];
69 69
70 70 if ( the_thread == NULL ) { }
71 71 else
72 72 {
73 73 #ifndef __RTEMS_USE_TICKS_FOR_STATISTICS__
74 74 /*
75 75 * If this is the currently executing thread, account for time
76 76 * since the last context switch.
77 77 */
78 78 ran = the_thread->cpu_time_used;
79 79 if ( _Thread_Executing->Object.id == the_thread->Object.id )
80 80 {
81 81 Timestamp_Control used;
82 82 _Timestamp_Subtract(
83 83 &_Thread_Time_of_last_context_switch, &uptime, &used
84 84 );
85 85 _Timestamp_Add_to( &ran, &used );
86 86 }
87 87 _Timestamp_Divide( &ran, &total, &ival, &fval );
88 88
89 89 #else
90 90 if (total_units != 0)
91 91 {
92 92 uint64_t ival_64;
93 93
94 94 ival_64 = the_thread->cpu_time_used;
95 95 ival_64 *= 100000;
96 96 ival = ival_64 / total_units;
97 97 }
98 98 else
99 99 {
100 100 ival = 0;
101 101 }
102 102
103 103 fval = ival % 1000;
104 104 ival /= 1000;
105 105 #endif
106 106 }
107 107 }
108 108 }
109 109 }
110 110 cpu_load = (unsigned char) (100 - ival);
111 111
112 112 return cpu_load;
113 113 }
114 114
115 115
Show file before | Show file after | Hide comments
@@ -1,786 +1,792
1 1 /** Functions related to data processing.
2 2 *
3 3 * @file
4 4 * @author P. LEROY
5 5 *
6 6 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
7 7 *
8 8 */
9 9
10 10 #include "fsw_processing.h"
11 11 #include "fsw_processing_globals.c"
12 12 #include "fsw_init.h"
13 13
14 14 unsigned int nb_sm_f0;
15 15 unsigned int nb_sm_f0_aux_f1;
16 16 unsigned int nb_sm_f1;
17 17 unsigned int nb_sm_f0_aux_f2;
18 18
19 19 typedef enum restartState_t
20 20 {
21 21 WAIT_FOR_F2,
22 22 WAIT_FOR_F1,
23 23 WAIT_FOR_F0
24 24 } restartState;
25 25
26 26 //************************
27 27 // spectral matrices rings
28 28 ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ];
29 29 ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ];
30 30 ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ];
31 31 ring_node *current_ring_node_sm_f0;
32 32 ring_node *current_ring_node_sm_f1;
33 33 ring_node *current_ring_node_sm_f2;
34 34 ring_node *ring_node_for_averaging_sm_f0;
35 35 ring_node *ring_node_for_averaging_sm_f1;
36 36 ring_node *ring_node_for_averaging_sm_f2;
37 37
38 38 //
39 39 ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
40 40 {
41 41 ring_node *node;
42 42
43 43 node = NULL;
44 44 switch ( frequencyChannel ) {
45 45 case 0:
46 46 node = ring_node_for_averaging_sm_f0;
47 47 break;
48 48 case 1:
49 49 node = ring_node_for_averaging_sm_f1;
50 50 break;
51 51 case 2:
52 52 node = ring_node_for_averaging_sm_f2;
53 53 break;
54 54 default:
55 55 break;
56 56 }
57 57
58 58 return node;
59 59 }
60 60
61 61 //***********************************************************
62 62 // Interrupt Service Routine for spectral matrices processing
63 63
64 64 void spectral_matrices_isr_f0( int statusReg )
65 65 {
66 66 unsigned char status;
67 67 rtems_status_code status_code;
68 68 ring_node *full_ring_node;
69 69
70 70 status = (unsigned char) (statusReg & 0x03); // [0011] get the status_ready_matrix_f0_x bits
71 71
72 72 switch(status)
73 73 {
74 74 case 0:
75 75 break;
76 76 case 3:
77 77 // UNEXPECTED VALUE
78 78 spectral_matrix_regs->status = 0x03; // [0011]
79 79 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
80 80 break;
81 81 case 1:
82 82 full_ring_node = current_ring_node_sm_f0->previous;
83 83 full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
84 84 full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
85 85 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
86 86 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
87 87 // if there are enough ring nodes ready, wake up an AVFx task
88 88 nb_sm_f0 = nb_sm_f0 + 1;
89 89 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
90 90 {
91 91 ring_node_for_averaging_sm_f0 = full_ring_node;
92 92 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
93 93 {
94 94 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
95 95 }
96 96 nb_sm_f0 = 0;
97 97 }
98 98 spectral_matrix_regs->status = 0x01; // [0000 0001]
99 99 break;
100 100 case 2:
101 101 full_ring_node = current_ring_node_sm_f0->previous;
102 102 full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
103 103 full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
104 104 current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
105 105 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
106 106 // if there are enough ring nodes ready, wake up an AVFx task
107 107 nb_sm_f0 = nb_sm_f0 + 1;
108 108 if (nb_sm_f0 == NB_SM_BEFORE_AVF0)
109 109 {
110 110 ring_node_for_averaging_sm_f0 = full_ring_node;
111 111 if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
112 112 {
113 113 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
114 114 }
115 115 nb_sm_f0 = 0;
116 116 }
117 117 spectral_matrix_regs->status = 0x02; // [0000 0010]
118 118 break;
119 119 }
120 120 }
121 121
122 122 void spectral_matrices_isr_f1( int statusReg )
123 123 {
124 124 rtems_status_code status_code;
125 125 unsigned char status;
126 126 ring_node *full_ring_node;
127 127
128 128 status = (unsigned char) ((statusReg & 0x0c) >> 2); // [1100] get the status_ready_matrix_f1_x bits
129 129
130 130 switch(status)
131 131 {
132 132 case 0:
133 133 break;
134 134 case 3:
135 135 // UNEXPECTED VALUE
136 136 spectral_matrix_regs->status = 0xc0; // [1100]
137 137 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
138 138 break;
139 139 case 1:
140 140 full_ring_node = current_ring_node_sm_f1->previous;
141 141 full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
142 142 full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
143 143 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
144 144 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
145 145 // if there are enough ring nodes ready, wake up an AVFx task
146 146 nb_sm_f1 = nb_sm_f1 + 1;
147 147 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
148 148 {
149 149 ring_node_for_averaging_sm_f1 = full_ring_node;
150 150 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
151 151 {
152 152 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
153 153 }
154 154 nb_sm_f1 = 0;
155 155 }
156 156 spectral_matrix_regs->status = 0x04; // [0000 0100]
157 157 break;
158 158 case 2:
159 159 full_ring_node = current_ring_node_sm_f1->previous;
160 160 full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
161 161 full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
162 162 current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
163 163 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
164 164 // if there are enough ring nodes ready, wake up an AVFx task
165 165 nb_sm_f1 = nb_sm_f1 + 1;
166 166 if (nb_sm_f1 == NB_SM_BEFORE_AVF1)
167 167 {
168 168 ring_node_for_averaging_sm_f1 = full_ring_node;
169 169 if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
170 170 {
171 171 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
172 172 }
173 173 nb_sm_f1 = 0;
174 174 }
175 175 spectral_matrix_regs->status = 0x08; // [1000 0000]
176 176 break;
177 177 }
178 178 }
179 179
180 180 void spectral_matrices_isr_f2( int statusReg )
181 181 {
182 182 unsigned char status;
183 183 rtems_status_code status_code;
184 184
185 185 status = (unsigned char) ((statusReg & 0x30) >> 4); // [0011 0000] get the status_ready_matrix_f2_x bits
186 186
187 187 switch(status)
188 188 {
189 189 case 0:
190 190 break;
191 191 case 3:
192 192 // UNEXPECTED VALUE
193 193 spectral_matrix_regs->status = 0x30; // [0011 0000]
194 194 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
195 195 break;
196 196 case 1:
197 197 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
198 198 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
199 199 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
200 200 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
201 201 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
202 202 spectral_matrix_regs->status = 0x10; // [0001 0000]
203 203 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
204 204 {
205 205 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
206 206 }
207 207 break;
208 208 case 2:
209 209 ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
210 210 current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
211 211 ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
212 212 ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
213 213 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
214 214 spectral_matrix_regs->status = 0x20; // [0010 0000]
215 215 if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
216 216 {
217 217 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
218 218 }
219 219 break;
220 220 }
221 221 }
222 222
223 223 void spectral_matrix_isr_error_handler( int statusReg )
224 224 {
225 225 // STATUS REGISTER
226 226 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
227 227 // 10 9 8
228 228 // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
229 229 // 7 6 5 4 3 2 1 0
230 230 // [bad_component_err] not defined in the last version of the VHDL code
231 231
232 232 rtems_status_code status_code;
233 233
234 234 //***************************************************
235 235 // the ASM status register is copied in the HK packet
236 236 housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) (statusReg & 0x780 >> 7); // [0111 1000 0000]
237 237
238 238 if (statusReg & 0x7c0) // [0111 1100 0000]
239 239 {
240 240 status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
241 241 }
242 242
243 243 spectral_matrix_regs->status = spectral_matrix_regs->status & 0x7c0;
244 244
245 245 }
246 246
247 247 rtems_isr spectral_matrices_isr( rtems_vector_number vector )
248 248 {
249 249 // STATUS REGISTER
250 250 // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
251 251 // 10 9 8
252 252 // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
253 253 // 7 6 5 4 3 2 1 0
254 254
255 255 int statusReg;
256 256
257 257 static restartState state = WAIT_FOR_F2;
258 258
259 259 statusReg = spectral_matrix_regs->status;
260 260
261 261 if (thisIsAnASMRestart == 0)
262 262 { // this is not a restart sequence, process incoming matrices normally
263 263 spectral_matrices_isr_f0( statusReg );
264 264
265 265 spectral_matrices_isr_f1( statusReg );
266 266
267 267 spectral_matrices_isr_f2( statusReg );
268 268 }
269 269 else
270 270 { // a restart sequence has to be launched
271 271 switch (state) {
272 272 case WAIT_FOR_F2:
273 273 if ((statusReg & 0x30) != 0x00) // [0011 0000] check the status_ready_matrix_f2_x bits
274 274 {
275 275 state = WAIT_FOR_F1;
276 276 }
277 277 break;
278 278 case WAIT_FOR_F1:
279 279 if ((statusReg & 0x0c) != 0x00) // [0000 1100] check the status_ready_matrix_f1_x bits
280 280 {
281 281 state = WAIT_FOR_F0;
282 282 }
283 283 break;
284 284 case WAIT_FOR_F0:
285 285 if ((statusReg & 0x03) != 0x00) // [0000 0011] check the status_ready_matrix_f0_x bits
286 286 {
287 287 state = WAIT_FOR_F2;
288 288 thisIsAnASMRestart = 0;
289 289 }
290 290 break;
291 291 default:
292 292 break;
293 293 }
294 294 reset_sm_status();
295 295 }
296 296
297 297 spectral_matrix_isr_error_handler( statusReg );
298 298
299 299 }
300 300
301 301 //******************
302 302 // Spectral Matrices
303 303
304 304 void reset_nb_sm( void )
305 305 {
306 306 nb_sm_f0 = 0;
307 307 nb_sm_f0_aux_f1 = 0;
308 308 nb_sm_f0_aux_f2 = 0;
309 309
310 310 nb_sm_f1 = 0;
311 311 }
312 312
313 313 void SM_init_rings( void )
314 314 {
315 315 init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
316 316 init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
317 317 init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
318 318
319 319 DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
320 320 DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
321 321 DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
322 322 DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
323 323 DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
324 324 DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
325 325 }
326 326
327 327 void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
328 328 {
329 329 unsigned char i;
330 330
331 331 ring[ nbNodes - 1 ].next
332 332 = (ring_node_asm*) &ring[ 0 ];
333 333
334 334 for(i=0; i<nbNodes-1; i++)
335 335 {
336 336 ring[ i ].next = (ring_node_asm*) &ring[ i + 1 ];
337 337 }
338 338 }
339 339
340 340 void SM_reset_current_ring_nodes( void )
341 341 {
342 342 current_ring_node_sm_f0 = sm_ring_f0[0].next;
343 343 current_ring_node_sm_f1 = sm_ring_f1[0].next;
344 344 current_ring_node_sm_f2 = sm_ring_f2[0].next;
345 345
346 346 ring_node_for_averaging_sm_f0 = NULL;
347 347 ring_node_for_averaging_sm_f1 = NULL;
348 348 ring_node_for_averaging_sm_f2 = NULL;
349 349 }
350 350
351 351 //*****************
352 352 // Basic Parameters
353 353
354 354 void BP_init_header( bp_packet *packet,
355 355 unsigned int apid, unsigned char sid,
356 356 unsigned int packetLength, unsigned char blkNr )
357 357 {
358 358 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
359 359 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
360 360 packet->reserved = 0x00;
361 361 packet->userApplication = CCSDS_USER_APP;
362 362 packet->packetID[0] = (unsigned char) (apid >> 8);
363 363 packet->packetID[1] = (unsigned char) (apid);
364 364 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
365 365 packet->packetSequenceControl[1] = 0x00;
366 366 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
367 367 packet->packetLength[1] = (unsigned char) (packetLength);
368 368 // DATA FIELD HEADER
369 369 packet->spare1_pusVersion_spare2 = 0x10;
370 370 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
371 371 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
372 372 packet->destinationID = TM_DESTINATION_ID_GROUND;
373 373 packet->time[0] = 0x00;
374 374 packet->time[1] = 0x00;
375 375 packet->time[2] = 0x00;
376 376 packet->time[3] = 0x00;
377 377 packet->time[4] = 0x00;
378 378 packet->time[5] = 0x00;
379 379 // AUXILIARY DATA HEADER
380 380 packet->sid = sid;
381 381 packet->pa_bia_status_info = 0x00;
382 382 packet->sy_lfr_common_parameters_spare = 0x00;
383 383 packet->sy_lfr_common_parameters = 0x00;
384 384 packet->acquisitionTime[0] = 0x00;
385 385 packet->acquisitionTime[1] = 0x00;
386 386 packet->acquisitionTime[2] = 0x00;
387 387 packet->acquisitionTime[3] = 0x00;
388 388 packet->acquisitionTime[4] = 0x00;
389 389 packet->acquisitionTime[5] = 0x00;
390 390 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
391 391 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
392 392 }
393 393
394 394 void BP_init_header_with_spare( bp_packet_with_spare *packet,
395 395 unsigned int apid, unsigned char sid,
396 396 unsigned int packetLength , unsigned char blkNr)
397 397 {
398 398 packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
399 399 packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
400 400 packet->reserved = 0x00;
401 401 packet->userApplication = CCSDS_USER_APP;
402 402 packet->packetID[0] = (unsigned char) (apid >> 8);
403 403 packet->packetID[1] = (unsigned char) (apid);
404 404 packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
405 405 packet->packetSequenceControl[1] = 0x00;
406 406 packet->packetLength[0] = (unsigned char) (packetLength >> 8);
407 407 packet->packetLength[1] = (unsigned char) (packetLength);
408 408 // DATA FIELD HEADER
409 409 packet->spare1_pusVersion_spare2 = 0x10;
410 410 packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
411 411 packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
412 412 packet->destinationID = TM_DESTINATION_ID_GROUND;
413 413 // AUXILIARY DATA HEADER
414 414 packet->sid = sid;
415 415 packet->pa_bia_status_info = 0x00;
416 416 packet->sy_lfr_common_parameters_spare = 0x00;
417 417 packet->sy_lfr_common_parameters = 0x00;
418 418 packet->time[0] = 0x00;
419 419 packet->time[0] = 0x00;
420 420 packet->time[0] = 0x00;
421 421 packet->time[0] = 0x00;
422 422 packet->time[0] = 0x00;
423 423 packet->time[0] = 0x00;
424 424 packet->source_data_spare = 0x00;
425 425 packet->pa_lfr_bp_blk_nr[0] = 0x00; // BLK_NR MSB
426 426 packet->pa_lfr_bp_blk_nr[1] = blkNr; // BLK_NR LSB
427 427 }
428 428
429 429 void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
430 430 {
431 431 rtems_status_code status;
432 432
433 433 // SEND PACKET
434 434 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
435 435 if (status != RTEMS_SUCCESSFUL)
436 436 {
437 437 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
438 438 }
439 439 }
440 440
441 441 void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
442 442 {
443 443 /** This function is used to send the BP paquets when needed.
444 444 *
445 445 * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
446 446 *
447 447 * @return void
448 448 *
449 449 * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
450 450 * BURST paquets are sent everytime.
451 451 *
452 452 */
453 453
454 454 rtems_status_code status;
455 455
456 456 // SEND PACKET
457 457 // before lastValidTransitionDate, the data are drops even if they are ready
458 458 // this guarantees that no SBM packets will be received before the requested enter mode time
459 459 if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
460 460 {
461 461 status = rtems_message_queue_send( queue_id, data, nbBytesToSend);
462 462 if (status != RTEMS_SUCCESSFUL)
463 463 {
464 464 PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
465 465 }
466 466 }
467 467 }
468 468
469 469 //******************
470 470 // general functions
471 471
472 472 void reset_sm_status( void )
473 473 {
474 474 // error
475 475 // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
476 476 // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
477 477 // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
478 478 // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
479 479
480 480 spectral_matrix_regs->status = 0x7ff; // [0111 1111 1111]
481 481 }
482 482
483 483 void reset_spectral_matrix_regs( void )
484 484 {
485 485 /** This function resets the spectral matrices module registers.
486 486 *
487 487 * The registers affected by this function are located at the following offset addresses:
488 488 *
489 489 * - 0x00 config
490 490 * - 0x04 status
491 491 * - 0x08 matrixF0_Address0
492 492 * - 0x10 matrixFO_Address1
493 493 * - 0x14 matrixF1_Address
494 494 * - 0x18 matrixF2_Address
495 495 *
496 496 */
497 497
498 498 set_sm_irq_onError( 0 );
499 499
500 500 set_sm_irq_onNewMatrix( 0 );
501 501
502 502 reset_sm_status();
503 503
504 504 // F1
505 505 spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
506 506 spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
507 507 // F2
508 508 spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
509 509 spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
510 510 // F3
511 511 spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
512 512 spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
513 513
514 514 spectral_matrix_regs->matrix_length = 0xc8; // 25 * 128 / 16 = 200 = 0xc8
515 515 }
516 516
517 517 void set_time( unsigned char *time, unsigned char * timeInBuffer )
518 518 {
519 519 time[0] = timeInBuffer[0];
520 520 time[1] = timeInBuffer[1];
521 521 time[2] = timeInBuffer[2];
522 522 time[3] = timeInBuffer[3];
523 523 time[4] = timeInBuffer[6];
524 524 time[5] = timeInBuffer[7];
525 525 }
526 526
527 527 unsigned long long int get_acquisition_time( unsigned char *timePtr )
528 528 {
529 529 unsigned long long int acquisitionTimeAslong;
530 530 acquisitionTimeAslong = 0x00;
531 531 acquisitionTimeAslong = ( (unsigned long long int) (timePtr[0] & 0x7f) << 40 ) // [0111 1111] mask the synchronization bit
532 532 + ( (unsigned long long int) timePtr[1] << 32 )
533 533 + ( (unsigned long long int) timePtr[2] << 24 )
534 534 + ( (unsigned long long int) timePtr[3] << 16 )
535 535 + ( (unsigned long long int) timePtr[6] << 8 )
536 536 + ( (unsigned long long int) timePtr[7] );
537 537 return acquisitionTimeAslong;
538 538 }
539 539
540 540 unsigned char getSID( rtems_event_set event )
541 541 {
542 542 unsigned char sid;
543 543
544 544 rtems_event_set eventSetBURST;
545 545 rtems_event_set eventSetSBM;
546 546
547 547 //******
548 548 // BURST
549 549 eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
550 550 | RTEMS_EVENT_BURST_BP1_F1
551 551 | RTEMS_EVENT_BURST_BP2_F0
552 552 | RTEMS_EVENT_BURST_BP2_F1;
553 553
554 554 //****
555 555 // SBM
556 556 eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
557 557 | RTEMS_EVENT_SBM_BP1_F1
558 558 | RTEMS_EVENT_SBM_BP2_F0
559 559 | RTEMS_EVENT_SBM_BP2_F1;
560 560
561 561 if (event & eventSetBURST)
562 562 {
563 563 sid = SID_BURST_BP1_F0;
564 564 }
565 565 else if (event & eventSetSBM)
566 566 {
567 567 sid = SID_SBM1_BP1_F0;
568 568 }
569 569 else
570 570 {
571 571 sid = 0;
572 572 }
573 573
574 574 return sid;
575 575 }
576 576
577 577 void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
578 578 {
579 579 unsigned int i;
580 580 float re;
581 581 float im;
582 582
583 583 for (i=0; i<NB_BINS_PER_SM; i++){
584 584 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 ];
585 585 im = inputASM[ (asmComponent*NB_BINS_PER_SM) + i * 2 + 1];
586 586 outputASM[ (asmComponent *NB_BINS_PER_SM) + i] = re;
587 587 outputASM[ (asmComponent+1)*NB_BINS_PER_SM + i] = im;
588 588 }
589 589 }
590 590
591 591 void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
592 592 {
593 593 unsigned int i;
594 594 float re;
595 595
596 596 for (i=0; i<NB_BINS_PER_SM; i++){
597 597 re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
598 598 outputASM[ (asmComponent*NB_BINS_PER_SM) + i] = re;
599 599 }
600 600 }
601 601
602 602 void ASM_patch( float *inputASM, float *outputASM )
603 603 {
604 604 extractReImVectors( inputASM, outputASM, 1); // b1b2
605 605 extractReImVectors( inputASM, outputASM, 3 ); // b1b3
606 606 extractReImVectors( inputASM, outputASM, 5 ); // b1e1
607 607 extractReImVectors( inputASM, outputASM, 7 ); // b1e2
608 608 extractReImVectors( inputASM, outputASM, 10 ); // b2b3
609 609 extractReImVectors( inputASM, outputASM, 12 ); // b2e1
610 610 extractReImVectors( inputASM, outputASM, 14 ); // b2e2
611 611 extractReImVectors( inputASM, outputASM, 17 ); // b3e1
612 612 extractReImVectors( inputASM, outputASM, 19 ); // b3e2
613 613 extractReImVectors( inputASM, outputASM, 22 ); // e1e2
614 614
615 615 copyReVectors(inputASM, outputASM, 0 ); // b1b1
616 616 copyReVectors(inputASM, outputASM, 9 ); // b2b2
617 617 copyReVectors(inputASM, outputASM, 16); // b3b3
618 618 copyReVectors(inputASM, outputASM, 21); // e1e1
619 619 copyReVectors(inputASM, outputASM, 24); // e2e2
620 620 }
621 621
622 622 void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
623 623 unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
624 624 unsigned char ASMIndexStart,
625 625 unsigned char channel )
626 626 {
627 627 //*************
628 628 // input format
629 629 // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
630 630 //**************
631 631 // output format
632 632 // matr0[0 .. 24] matr1[0 .. 24] .. matr127[0 .. 24]
633 633 //************
634 634 // compression
635 635 // matr0[0 .. 24] matr1[0 .. 24] .. matr11[0 .. 24] => f0 NORM
636 636 // matr0[0 .. 24] matr1[0 .. 24] .. matr22[0 .. 24] => f0 BURST, SBM
637 637
638 638 int frequencyBin;
639 639 int asmComponent;
640 640 int offsetASM;
641 641 int offsetCompressed;
642 642 int offsetFBin;
643 643 int fBinMask;
644 644 int k;
645 645
646 646 // BUILD DATA
647 647 for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
648 648 {
649 649 for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
650 650 {
651 651 offsetCompressed = // NO TIME OFFSET
652 652 frequencyBin * NB_VALUES_PER_SM
653 653 + asmComponent;
654 654 offsetASM = // NO TIME OFFSET
655 655 asmComponent * NB_BINS_PER_SM
656 656 + ASMIndexStart
657 657 + frequencyBin * nbBinsToAverage;
658 658 offsetFBin = ASMIndexStart
659 659 + frequencyBin * nbBinsToAverage;
660 660 compressed_spec_mat[ offsetCompressed ] = 0;
661 661 for ( k = 0; k < nbBinsToAverage; k++ )
662 662 {
663 663 fBinMask = getFBinMask( offsetFBin + k, channel );
664 664 compressed_spec_mat[offsetCompressed ] =
665 665 ( compressed_spec_mat[ offsetCompressed ]
666 666 + averaged_spec_mat[ offsetASM + k ] * fBinMask );
667 667 }
668 compressed_spec_mat[ offsetCompressed ] =
669 (divider != 0.) ? compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage) : 0.0;
668 if (divider != 0)
669 {
670 compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
671 }
672 else
673 {
674 compressed_spec_mat[ offsetCompressed ] = 0.0;
675 }
670 676 }
671 677 }
672 678
673 679 }
674 680
675 681 int getFBinMask( int index, unsigned char channel )
676 682 {
677 683 unsigned int indexInChar;
678 684 unsigned int indexInTheChar;
679 685 int fbin;
680 686 unsigned char *sy_lfr_fbins_fx_word1;
681 687
682 688 sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins.fx.f0_word1;
683 689
684 690 switch(channel)
685 691 {
686 692 case 0:
687 693 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
688 694 break;
689 695 case 1:
690 696 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
691 697 break;
692 698 case 2:
693 699 sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
694 700 break;
695 701 default:
696 702 PRINTF("ERR *** in getFBinMask, wrong frequency channel")
697 703 }
698 704
699 705 indexInChar = index >> 3;
700 706 indexInTheChar = index - indexInChar * 8;
701 707
702 708 fbin = (int) ((sy_lfr_fbins_fx_word1[ NB_BYTES_PER_FREQ_MASK - 1 - indexInChar] >> indexInTheChar) & 0x1);
703 709
704 710 return fbin;
705 711 }
706 712
707 713 unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
708 714 {
709 715 u_int64_t acquisitionTime;
710 716 u_int64_t timecodeReference;
711 717 u_int64_t offsetInFineTime;
712 718 u_int64_t shiftInFineTime;
713 719 u_int64_t tBadInFineTime;
714 720 u_int64_t acquisitionTimeRangeMin;
715 721 u_int64_t acquisitionTimeRangeMax;
716 722 unsigned char pasFilteringIsEnabled;
717 723 unsigned char ret;
718 724
719 725 pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 0x01); // [0000 0001]
720 726 ret = 1;
721 727
722 728 // compute acquisition time from caoarseTime and fineTime
723 729 acquisitionTime = ( ((u_int64_t)coarseTime) << 16 )
724 730 + (u_int64_t) fineTime;
725 731
726 732 // compute the timecode reference
727 733 timecodeReference = (u_int64_t) (floor( ((double) coarseTime) / ((double) filterPar.sy_lfr_pas_filter_modulus) )
728 734 * ((double) filterPar.sy_lfr_pas_filter_modulus)) * 65536;
729 735
730 736 // compute the acquitionTime range
731 737 offsetInFineTime = ((double) filterPar.sy_lfr_pas_filter_offset) * 65536;
732 738 shiftInFineTime = ((double) filterPar.sy_lfr_pas_filter_shift) * 65536;
733 739 tBadInFineTime = ((double) filterPar.sy_lfr_pas_filter_tbad) * 65536;
734 740
735 741 acquisitionTimeRangeMin =
736 742 timecodeReference
737 743 + offsetInFineTime
738 744 + shiftInFineTime
739 745 - acquisitionDurations[channel];
740 746 acquisitionTimeRangeMax =
741 747 timecodeReference
742 748 + offsetInFineTime
743 749 + shiftInFineTime
744 750 + tBadInFineTime;
745 751
746 752 if ( (acquisitionTime >= acquisitionTimeRangeMin)
747 753 && (acquisitionTime <= acquisitionTimeRangeMax)
748 754 && (pasFilteringIsEnabled == 1) )
749 755 {
750 756 ret = 0; // the acquisition time is INSIDE the range, the matrix shall be ignored
751 757 }
752 758 else
753 759 {
754 760 ret = 1; // the acquisition time is OUTSIDE the range, the matrix can be used for the averaging
755 761 }
756 762
757 763 // printf("coarseTime = %x, fineTime = %x\n",
758 764 // coarseTime,
759 765 // fineTime);
760 766
761 767 // printf("[ret = %d] *** acquisitionTime = %f, Reference = %f",
762 768 // ret,
763 769 // acquisitionTime / 65536.,
764 770 // timecodeReference / 65536.);
765 771
766 772 // printf(", Min = %f, Max = %f\n",
767 773 // acquisitionTimeRangeMin / 65536.,
768 774 // acquisitionTimeRangeMax / 65536.);
769 775
770 776 return ret;
771 777 }
772 778
773 779 void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
774 780 {
775 781 unsigned char bin;
776 782 unsigned char kcoeff;
777 783
778 784 for (bin=0; bin<nb_bins_norm; bin++)
779 785 {
780 786 for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
781 787 {
782 788 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
783 789 output_kcoeff[ (bin*NB_K_COEFF_PER_BIN + kcoeff)*2 + 1 ] = input_kcoeff[ bin*NB_K_COEFF_PER_BIN + kcoeff ];
784 790 }
785 791 }
786 792 }
General Comments 0
You need to be logged in to leave comments. Login now