GCC Code Coverage Report
Directory: ./ Exec Total Coverage
File: src/processing/fsw_processing.c Lines: 354 371 95.4 %
Date: 2018-10-05 11:31:10 Branches: 78 96 81.2 %

Line Branch Exec Source
1
/*------------------------------------------------------------------------------
2
--  Solar Orbiter's Low Frequency Receiver Flight Software (LFR FSW),
3
--  This file is a part of the LFR FSW
4
--  Copyright (C) 2012-2018, Plasma Physics Laboratory - CNRS
5
--
6
--  This program is free software; you can redistribute it and/or modify
7
--  it under the terms of the GNU General Public License as published by
8
--  the Free Software Foundation; either version 2 of the License, or
9
--  (at your option) any later version.
10
--
11
--  This program is distributed in the hope that it will be useful,
12
--  but WITHOUT ANY WARRANTY; without even the implied warranty of
13
--  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14
--  GNU General Public License for more details.
15
--
16
--  You should have received a copy of the GNU General Public License
17
--  along with this program; if not, write to the Free Software
18
--  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
19
-------------------------------------------------------------------------------*/
20
/*--                  Author : Paul Leroy
21
--                   Contact : Alexis Jeandet
22
--                      Mail : alexis.jeandet@lpp.polytechnique.fr
23
----------------------------------------------------------------------------*/
24
25
/** Functions related to data processing.
26
 *
27
 * @file
28
 * @author P. LEROY
29
 *
30
 * These function are related to data processing, i.e. spectral matrices averaging and basic parameters computation.
31
 *
32
 */
33
34
#include "fsw_processing.h"
35
#include "fsw_processing_globals.c"
36
#include "fsw_init.h"
37
38
unsigned int nb_sm_f0       = 0;
39
unsigned int nb_sm_f0_aux_f1= 0;
40
unsigned int nb_sm_f1       = 0;
41
unsigned int nb_sm_f0_aux_f2= 0;
42
43
typedef enum restartState_t
44
{
45
    WAIT_FOR_F2,
46
    WAIT_FOR_F1,
47
    WAIT_FOR_F0
48
} restartState;
49
50
//************************
51
// spectral matrices rings
52
ring_node sm_ring_f0[ NB_RING_NODES_SM_F0 ] = {0};
53
ring_node sm_ring_f1[ NB_RING_NODES_SM_F1 ] = {0};
54
ring_node sm_ring_f2[ NB_RING_NODES_SM_F2 ] = {0};
55
ring_node *current_ring_node_sm_f0      = NULL;
56
ring_node *current_ring_node_sm_f1      = NULL;
57
ring_node *current_ring_node_sm_f2      = NULL;
58
ring_node *ring_node_for_averaging_sm_f0= NULL;
59
ring_node *ring_node_for_averaging_sm_f1= NULL;
60
ring_node *ring_node_for_averaging_sm_f2= NULL;
61
62
//
63
48009
ring_node * getRingNodeForAveraging( unsigned char frequencyChannel)
64
{
65
    ring_node *node;
66
67
48009
    node = NULL;
68

48009
    switch ( frequencyChannel ) {
69
    case CHANNELF0:
70
38525
        node = ring_node_for_averaging_sm_f0;
71
38525
        break;
72
    case CHANNELF1:
73
6350
        node = ring_node_for_averaging_sm_f1;
74
6350
        break;
75
    case CHANNELF2:
76
3135
        node = ring_node_for_averaging_sm_f2;
77
        break;
78
    default:
79
        break;
80
    }
81
82
48009
    return node;
83
}
84
85
//***********************************************************
86
// Interrupt Service Routine for spectral matrices processing
87
88
363087
void spectral_matrices_isr_f0( int statusReg )
89
{
90
    unsigned char status;
91
    rtems_status_code status_code;
92
    ring_node *full_ring_node;
93
94
363087
    status = (unsigned char) (statusReg & BITS_STATUS_F0);   // [0011] get the status_ready_matrix_f0_x bits
95
96

363087
    switch(status)
97
    {
98
    case 0:
99
        break;
100
    case BIT_READY_0_1:
101
        // UNEXPECTED VALUE
102
        spectral_matrix_regs->status = BIT_READY_0_1;   // [0011]
103
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
104
        break;
105
    case BIT_READY_0:
106
154469
        full_ring_node = current_ring_node_sm_f0->previous;
107
154469
        full_ring_node->coarseTime = spectral_matrix_regs->f0_0_coarse_time;
108
154469
        full_ring_node->fineTime = spectral_matrix_regs->f0_0_fine_time;
109
154469
        current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
110
154469
        spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->buffer_address;
111
        // if there are enough ring nodes ready, wake up an AVFx task
112
154469
        nb_sm_f0 = nb_sm_f0 + 1;
113
154469
        if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
114
        {
115
10791
            ring_node_for_averaging_sm_f0 = full_ring_node;
116
10791
            if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
117
            {
118
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
119
            }
120
10791
            nb_sm_f0 = 0;
121
        }
122
154469
        spectral_matrix_regs->status = BIT_READY_0;   // [0000 0001]
123
154469
        break;
124
    case BIT_READY_1:
125
154386
        full_ring_node = current_ring_node_sm_f0->previous;
126
154386
        full_ring_node->coarseTime = spectral_matrix_regs->f0_1_coarse_time;
127
154386
        full_ring_node->fineTime = spectral_matrix_regs->f0_1_fine_time;
128
154386
        current_ring_node_sm_f0 = current_ring_node_sm_f0->next;
129
154386
        spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
130
        // if there are enough ring nodes ready, wake up an AVFx task
131
154386
        nb_sm_f0 = nb_sm_f0 + 1;
132
154386
        if (nb_sm_f0 == NB_SM_BEFORE_AVF0_F1)
133
        {
134
27747
            ring_node_for_averaging_sm_f0 = full_ring_node;
135
27747
            if (rtems_event_send( Task_id[TASKID_AVF0], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
136
            {
137
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
138
            }
139
27747
            nb_sm_f0 = 0;
140
        }
141
154386
        spectral_matrix_regs->status = BIT_READY_1;   // [0000 0010]
142
        break;
143
    default:
144
        break;
145
    }
146
363087
}
147
148
363087
void spectral_matrices_isr_f1( int statusReg )
149
{
150
    rtems_status_code status_code;
151
    unsigned char status;
152
    ring_node *full_ring_node;
153
154
363087
    status = (unsigned char) ((statusReg & BITS_STATUS_F1) >> SHIFT_2_BITS);   // [1100] get the status_ready_matrix_f1_x bits
155
156

363087
    switch(status)
157
    {
158
    case 0:
159
        break;
160
    case BIT_READY_0_1:
161
        // UNEXPECTED VALUE
162
        spectral_matrix_regs->status = BITS_STATUS_F1;   // [1100]
163
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
164
        break;
165
    case BIT_READY_0:
166
25740
        full_ring_node = current_ring_node_sm_f1->previous;
167
25740
        full_ring_node->coarseTime = spectral_matrix_regs->f1_0_coarse_time;
168
25740
        full_ring_node->fineTime = spectral_matrix_regs->f1_0_fine_time;
169
25740
        current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
170
25740
        spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->buffer_address;
171
        // if there are enough ring nodes ready, wake up an AVFx task
172
25740
        nb_sm_f1 = nb_sm_f1 + 1;
173
25740
        if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
174
        {
175
659
            ring_node_for_averaging_sm_f1 = full_ring_node;
176
659
            if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
177
            {
178
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
179
            }
180
659
            nb_sm_f1 = 0;
181
        }
182
25740
        spectral_matrix_regs->status = BIT_STATUS_F1_0;   // [0000 0100]
183
25740
        break;
184
    case BIT_READY_1:
185
25670
        full_ring_node = current_ring_node_sm_f1->previous;
186
25670
        full_ring_node->coarseTime = spectral_matrix_regs->f1_1_coarse_time;
187
25670
        full_ring_node->fineTime = spectral_matrix_regs->f1_1_fine_time;
188
25670
        current_ring_node_sm_f1 = current_ring_node_sm_f1->next;
189
25670
        spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
190
        // if there are enough ring nodes ready, wake up an AVFx task
191
25670
        nb_sm_f1 = nb_sm_f1 + 1;
192
25670
        if (nb_sm_f1 == NB_SM_BEFORE_AVF0_F1)
193
        {
194
5702
            ring_node_for_averaging_sm_f1 = full_ring_node;
195
5702
            if (rtems_event_send( Task_id[TASKID_AVF1], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
196
            {
197
                status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
198
            }
199
5702
            nb_sm_f1 = 0;
200
        }
201
25670
        spectral_matrix_regs->status = BIT_STATUS_F1_1;   // [1000 0000]
202
        break;
203
    default:
204
        break;
205
    }
206
363087
}
207
208
363087
void spectral_matrices_isr_f2( int statusReg )
209
{
210
    unsigned char status;
211
    rtems_status_code status_code;
212
213
363087
    status = (unsigned char) ((statusReg & BITS_STATUS_F2) >> SHIFT_4_BITS);   // [0011 0000] get the status_ready_matrix_f2_x bits
214
215

363087
    switch(status)
216
    {
217
    case 0:
218
        break;
219
    case BIT_READY_0_1:
220
        // UNEXPECTED VALUE
221
        spectral_matrix_regs->status = BITS_STATUS_F2;   // [0011 0000]
222
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_11 );
223
        break;
224
    case BIT_READY_0:
225
1578
        ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
226
1578
        current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
227
1578
        ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_0_coarse_time;
228
1578
        ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_0_fine_time;
229
1578
        spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->buffer_address;
230
1578
        spectral_matrix_regs->status = BIT_STATUS_F2_0;   // [0001 0000]
231
1578
        if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
232
        {
233
            status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
234
        }
235
        break;
236
    case BIT_READY_1:
237
1565
        ring_node_for_averaging_sm_f2 = current_ring_node_sm_f2->previous;
238
1565
        current_ring_node_sm_f2 = current_ring_node_sm_f2->next;
239
1565
        ring_node_for_averaging_sm_f2->coarseTime = spectral_matrix_regs->f2_1_coarse_time;
240
1565
        ring_node_for_averaging_sm_f2->fineTime = spectral_matrix_regs->f2_1_fine_time;
241
1565
        spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
242
1565
        spectral_matrix_regs->status = BIT_STATUS_F2_1;   // [0010 0000]
243
1565
        if (rtems_event_send( Task_id[TASKID_AVF2], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL)
244
        {
245
            status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_3 );
246
        }
247
        break;
248
    default:
249
        break;
250
    }
251
363087
}
252
253
366412
void spectral_matrix_isr_error_handler( int statusReg )
254
{
255
    // STATUS REGISTER
256
    // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
257
    //           10                    9                       8
258
    // buffer_full ** [bad_component_err] ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
259
    //      7                  6             5       4       3       2       1       0
260
    // [bad_component_err] not defined in the last version of the VHDL code
261
262
    rtems_status_code status_code;
263
264
    //***************************************************
265
    // the ASM status register is copied in the HK packet
266
366412
    housekeeping_packet.hk_lfr_vhdl_aa_sm = (unsigned char) ((statusReg & BITS_HK_AA_SM) >> SHIFT_7_BITS);    // [0111 1000 0000]
267
268
366412
    if (statusReg & BITS_SM_ERR)    // [0111 1100 0000]
269
    {
270
        status_code = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_8 );
271
    }
272
273
366412
    spectral_matrix_regs->status = spectral_matrix_regs->status & BITS_SM_ERR;
274
275
366412
}
276
277
366412
rtems_isr spectral_matrices_isr( rtems_vector_number vector )
278
{
279
    // STATUS REGISTER
280
    // input_fifo_write(2) *** input_fifo_write(1) *** input_fifo_write(0)
281
    //           10                    9                       8
282
    // buffer_full ** bad_component_err ** f2_1 ** f2_0 ** f1_1 ** f1_0 ** f0_1 ** f0_0
283
    //      7                  6             5       4       3       2       1       0
284
285
    int statusReg;
286
287
    static restartState state = WAIT_FOR_F2;
288
289
366412
    statusReg = spectral_matrix_regs->status;
290
291
366412
    if (thisIsAnASMRestart == 0)
292
    {   // this is not a restart sequence, process incoming matrices normally
293
363087
        spectral_matrices_isr_f0( statusReg );
294
295
363087
        spectral_matrices_isr_f1( statusReg );
296
297
363087
        spectral_matrices_isr_f2( statusReg );
298
    }
299
    else
300
    {   // a restart sequence has to be launched
301

3325
        switch (state) {
302
        case WAIT_FOR_F2:
303
3204
            if ((statusReg & BITS_STATUS_F2) != INIT_CHAR)   // [0011 0000] check the status_ready_matrix_f2_x bits
304
            {
305
48
                state = WAIT_FOR_F1;
306
            }
307
            break;
308
        case WAIT_FOR_F1:
309
73
            if ((statusReg & BITS_STATUS_F1) != INIT_CHAR)   // [0000 1100] check the status_ready_matrix_f1_x bits
310
            {
311
48
                state = WAIT_FOR_F0;
312
            }
313
            break;
314
        case WAIT_FOR_F0:
315
48
            if ((statusReg & BITS_STATUS_F0) != INIT_CHAR)   // [0000 0011] check the status_ready_matrix_f0_x bits
316
            {
317
48
                state = WAIT_FOR_F2;
318
48
                thisIsAnASMRestart = 0;
319
            }
320
            break;
321
        default:
322
            break;
323
        }
324
3325
        reset_sm_status();
325
    }
326
327
366412
    spectral_matrix_isr_error_handler( statusReg );
328
329
366412
}
330
331
//******************
332
// Spectral Matrices
333
334
179
void reset_nb_sm( void )
335
{
336
179
    nb_sm_f0 = 0;
337
179
    nb_sm_f0_aux_f1 = 0;
338
179
    nb_sm_f0_aux_f2 = 0;
339
340
179
    nb_sm_f1 = 0;
341
179
}
342
343
1
void SM_init_rings( void )
344
{
345
1
    init_ring( sm_ring_f0, NB_RING_NODES_SM_F0, sm_f0, TOTAL_SIZE_SM );
346
1
    init_ring( sm_ring_f1, NB_RING_NODES_SM_F1, sm_f1, TOTAL_SIZE_SM );
347
1
    init_ring( sm_ring_f2, NB_RING_NODES_SM_F2, sm_f2, TOTAL_SIZE_SM );
348
349
    DEBUG_PRINTF1("sm_ring_f0 @%x\n", (unsigned int) sm_ring_f0)
350
    DEBUG_PRINTF1("sm_ring_f1 @%x\n", (unsigned int) sm_ring_f1)
351
    DEBUG_PRINTF1("sm_ring_f2 @%x\n", (unsigned int) sm_ring_f2)
352
    DEBUG_PRINTF1("sm_f0 @%x\n", (unsigned int) sm_f0)
353
    DEBUG_PRINTF1("sm_f1 @%x\n", (unsigned int) sm_f1)
354
    DEBUG_PRINTF1("sm_f2 @%x\n", (unsigned int) sm_f2)
355
1
}
356
357
895
void ASM_generic_init_ring( ring_node_asm *ring, unsigned char nbNodes )
358
{
359
    unsigned char i;
360
361
895
    ring[ nbNodes - 1 ].next
362
            = (ring_node_asm*) &ring[ 0 ];
363
364
5907
    for(i=0; i<nbNodes-1; i++)
365
    {
366
5012
        ring[ i ].next            = (ring_node_asm*) &ring[ i + 1 ];
367
    }
368
895
}
369
370
180
void SM_reset_current_ring_nodes( void )
371
{
372
180
    current_ring_node_sm_f0 = sm_ring_f0[0].next;
373
180
    current_ring_node_sm_f1 = sm_ring_f1[0].next;
374
180
    current_ring_node_sm_f2 = sm_ring_f2[0].next;
375
376
180
    ring_node_for_averaging_sm_f0 = NULL;
377
180
    ring_node_for_averaging_sm_f1 = NULL;
378
180
    ring_node_for_averaging_sm_f2 = NULL;
379
180
}
380
381
//*****************
382
// Basic Parameters
383
384
1162
void BP_init_header( bp_packet *packet,
385
                     unsigned int apid, unsigned char sid,
386
                     unsigned int packetLength, unsigned char blkNr )
387
{
388
1162
    packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
389
1162
    packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
390
1162
    packet->reserved = INIT_CHAR;
391
1162
    packet->userApplication = CCSDS_USER_APP;
392
1162
    packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
393
1162
    packet->packetID[1] = (unsigned char) (apid);
394
1162
    packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
395
1162
    packet->packetSequenceControl[1] = INIT_CHAR;
396
1162
    packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
397
1162
    packet->packetLength[1] = (unsigned char) (packetLength);
398
    // DATA FIELD HEADER
399
1162
    packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
400
1162
    packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
401
1162
    packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
402
1162
    packet->destinationID = TM_DESTINATION_ID_GROUND;
403
1162
    packet->time[BYTE_0] = INIT_CHAR;
404
1162
    packet->time[BYTE_1] = INIT_CHAR;
405
1162
    packet->time[BYTE_2] = INIT_CHAR;
406
1162
    packet->time[BYTE_3] = INIT_CHAR;
407
1162
    packet->time[BYTE_4] = INIT_CHAR;
408
1162
    packet->time[BYTE_5] = INIT_CHAR;
409
    // AUXILIARY DATA HEADER
410
1162
    packet->sid = sid;
411
1162
    packet->pa_bia_status_info = INIT_CHAR;
412
1162
    packet->sy_lfr_common_parameters_spare = INIT_CHAR;
413
1162
    packet->sy_lfr_common_parameters = INIT_CHAR;
414
1162
    packet->acquisitionTime[BYTE_0] = INIT_CHAR;
415
1162
    packet->acquisitionTime[BYTE_1] = INIT_CHAR;
416
1162
    packet->acquisitionTime[BYTE_2] = INIT_CHAR;
417
1162
    packet->acquisitionTime[BYTE_3] = INIT_CHAR;
418
1162
    packet->acquisitionTime[BYTE_4] = INIT_CHAR;
419
1162
    packet->acquisitionTime[BYTE_5] = INIT_CHAR;
420
1162
    packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR;  // BLK_NR MSB
421
1162
    packet->pa_lfr_bp_blk_nr[1] = blkNr;  // BLK_NR LSB
422
1162
}
423
424
358
void BP_init_header_with_spare( bp_packet_with_spare *packet,
425
                                unsigned int apid, unsigned char sid,
426
                                unsigned int packetLength , unsigned char blkNr)
427
{
428
358
    packet->targetLogicalAddress = CCSDS_DESTINATION_ID;
429
358
    packet->protocolIdentifier = CCSDS_PROTOCOLE_ID;
430
358
    packet->reserved = INIT_CHAR;
431
358
    packet->userApplication = CCSDS_USER_APP;
432
358
    packet->packetID[0] = (unsigned char) (apid >> SHIFT_1_BYTE);
433
358
    packet->packetID[1] = (unsigned char) (apid);
434
358
    packet->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE;
435
358
    packet->packetSequenceControl[1] = INIT_CHAR;
436
358
    packet->packetLength[0] = (unsigned char) (packetLength >> SHIFT_1_BYTE);
437
358
    packet->packetLength[1] = (unsigned char) (packetLength);
438
    // DATA FIELD HEADER
439
358
    packet->spare1_pusVersion_spare2 = SPARE1_PUSVERSION_SPARE2;
440
358
    packet->serviceType = TM_TYPE_LFR_SCIENCE; // service type
441
358
    packet->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype
442
358
    packet->destinationID = TM_DESTINATION_ID_GROUND;
443
    // AUXILIARY DATA HEADER
444
358
    packet->sid = sid;
445
358
    packet->pa_bia_status_info = INIT_CHAR;
446
358
    packet->sy_lfr_common_parameters_spare = INIT_CHAR;
447
358
    packet->sy_lfr_common_parameters = INIT_CHAR;
448
358
    packet->time[BYTE_0] = INIT_CHAR;
449
358
    packet->time[BYTE_1] = INIT_CHAR;
450
358
    packet->time[BYTE_2] = INIT_CHAR;
451
358
    packet->time[BYTE_3] = INIT_CHAR;
452
358
    packet->time[BYTE_4] = INIT_CHAR;
453
358
    packet->time[BYTE_5] = INIT_CHAR;
454
358
    packet->source_data_spare = INIT_CHAR;
455
358
    packet->pa_lfr_bp_blk_nr[0] = INIT_CHAR;  // BLK_NR MSB
456
358
    packet->pa_lfr_bp_blk_nr[1] = blkNr;  // BLK_NR LSB
457
358
}
458
459
1948
void BP_send(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
460
{
461
    rtems_status_code status;
462
463
    // SEND PACKET
464
1948
    status =  rtems_message_queue_send( queue_id, data, nbBytesToSend);
465
    if (status != RTEMS_SUCCESSFUL)
466
    {
467
        PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
468
    }
469
1948
}
470
471
8077
void BP_send_s1_s2(char *data, rtems_id queue_id, unsigned int nbBytesToSend, unsigned int sid )
472
{
473
    /** This function is used to send the BP paquets when needed.
474
     *
475
     * @param transitionCoarseTime is the requested transition time contained in the TC_LFR_ENTER_MODE
476
     *
477
     * @return void
478
     *
479
     * SBM1 and SBM2 paquets are sent depending on the type of the LFR mode transition.
480
     * BURST paquets are sent everytime.
481
     *
482
     */
483
484
    rtems_status_code status;
485
486
    // SEND PACKET
487
    // before lastValidTransitionDate, the data are drops even if they are ready
488
    // this guarantees that no SBM packets will be received before the requested enter mode time
489
8077
    if ( time_management_regs->coarse_time >= lastValidEnterModeTime)
490
    {
491
8077
        status =  rtems_message_queue_send( queue_id, data, nbBytesToSend);
492
        if (status != RTEMS_SUCCESSFUL)
493
        {
494
            PRINTF1("ERR *** in BP_send *** ERR %d\n", (int) status)
495
        }
496
    }
497
8077
}
498
499
//******************
500
// general functions
501
502
3689
void reset_sm_status( void )
503
{
504
    // error
505
    // 10 --------------- 9 ---------------- 8 ---------------- 7 ---------
506
    // input_fif0_write_2 input_fifo_write_1 input_fifo_write_0 buffer_full
507
    // ---------- 5 -- 4 -- 3 -- 2 -- 1 -- 0 --
508
    // ready bits f2_1 f2_0 f1_1 f1_1 f0_1 f0_0
509
510
3689
    spectral_matrix_regs->status = BITS_STATUS_REG;   // [0111 1111 1111]
511
3689
}
512
513
180
void reset_spectral_matrix_regs( void )
514
{
515
    /** This function resets the spectral matrices module registers.
516
     *
517
     * The registers affected by this function are located at the following offset addresses:
518
     *
519
     * - 0x00 config
520
     * - 0x04 status
521
     * - 0x08 matrixF0_Address0
522
     * - 0x10 matrixFO_Address1
523
     * - 0x14 matrixF1_Address
524
     * - 0x18 matrixF2_Address
525
     *
526
     */
527
528
180
    set_sm_irq_onError( 0 );
529
530
180
    set_sm_irq_onNewMatrix( 0 );
531
532
180
    reset_sm_status();
533
534
    // F1
535
180
    spectral_matrix_regs->f0_0_address = current_ring_node_sm_f0->previous->buffer_address;
536
180
    spectral_matrix_regs->f0_1_address = current_ring_node_sm_f0->buffer_address;
537
    // F2
538
180
    spectral_matrix_regs->f1_0_address = current_ring_node_sm_f1->previous->buffer_address;
539
180
    spectral_matrix_regs->f1_1_address = current_ring_node_sm_f1->buffer_address;
540
    // F3
541
180
    spectral_matrix_regs->f2_0_address = current_ring_node_sm_f2->previous->buffer_address;
542
180
    spectral_matrix_regs->f2_1_address = current_ring_node_sm_f2->buffer_address;
543
544
180
    spectral_matrix_regs->matrix_length = DEFAULT_MATRIX_LENGTH; // 25 * 128 / 16 = 200 = 0xc8
545
180
}
546
547
20050
void set_time( unsigned char *time, unsigned char * timeInBuffer )
548
{
549
20050
    time[BYTE_0] = timeInBuffer[BYTE_0];
550
20050
    time[BYTE_1] = timeInBuffer[BYTE_1];
551
20050
    time[BYTE_2] = timeInBuffer[BYTE_2];
552
20050
    time[BYTE_3] = timeInBuffer[BYTE_3];
553
20050
    time[BYTE_4] = timeInBuffer[BYTE_6];
554
20050
    time[BYTE_5] = timeInBuffer[BYTE_7];
555
20050
}
556
557
188
unsigned long long int get_acquisition_time( unsigned char *timePtr )
558
{
559
    unsigned long long int acquisitionTimeAslong;
560
188
    acquisitionTimeAslong = INIT_CHAR;
561
188
    acquisitionTimeAslong =
562
188
            ( (unsigned long long int) (timePtr[BYTE_0] & SYNC_BIT_MASK) << SHIFT_5_BYTES ) // [0111 1111] mask the synchronization bit
563
188
            + ( (unsigned long long int) timePtr[BYTE_1] << SHIFT_4_BYTES )
564
188
            + ( (unsigned long long int) timePtr[BYTE_2] << SHIFT_3_BYTES )
565
188
            + ( (unsigned long long int) timePtr[BYTE_3] << SHIFT_2_BYTES )
566
188
            + ( (unsigned long long int) timePtr[BYTE_6] << SHIFT_1_BYTE  )
567
188
            + ( (unsigned long long int) timePtr[BYTE_7]       );
568
188
    return acquisitionTimeAslong;
569
}
570
571
6593
unsigned char getSID( rtems_event_set event )
572
{
573
    unsigned char sid;
574
575
    rtems_event_set eventSetBURST;
576
    rtems_event_set eventSetSBM;
577
578
6593
    sid = 0;
579
580
    //******
581
    // BURST
582
6593
    eventSetBURST = RTEMS_EVENT_BURST_BP1_F0
583
            | RTEMS_EVENT_BURST_BP1_F1
584
            | RTEMS_EVENT_BURST_BP2_F0
585
            | RTEMS_EVENT_BURST_BP2_F1;
586
587
    //****
588
    // SBM
589
6593
    eventSetSBM = RTEMS_EVENT_SBM_BP1_F0
590
            | RTEMS_EVENT_SBM_BP1_F1
591
            | RTEMS_EVENT_SBM_BP2_F0
592
            | RTEMS_EVENT_SBM_BP2_F1;
593
594
6593
    if (event & eventSetBURST)
595
    {
596
1501
        sid = SID_BURST_BP1_F0;
597
    }
598
5092
    else if (event & eventSetSBM)
599
    {
600
5092
        sid = SID_SBM1_BP1_F0;
601
    }
602
    else
603
    {
604
        sid = 0;
605
    }
606
607
6593
    return sid;
608
}
609
610
/**
611
 * @brief extractReImVectors converts a given ASM component from interleaved to split representation
612
 * @param inputASM
613
 * @param outputASM
614
 * @param asmComponent
615
 */
616
152830
void extractReImVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
617
{
618
    unsigned int i;
619
    float re;
620
    float im;
621
622
19715070
    for (i=0; i<NB_BINS_PER_SM; i++){
623
19562240
        re = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL)    ];
624
19562240
        im = inputASM[ (asmComponent*NB_BINS_PER_SM) + (i * SM_BYTES_PER_VAL) + 1];
625
19562240
        outputASM[ ( asmComponent   *NB_BINS_PER_SM) +  i] = re;
626
19562240
        outputASM[ ((asmComponent+1)*NB_BINS_PER_SM) +  i] = im;
627
    }
628
152830
}
629
630
/**
631
 * @brief copyReVectors copies real part of a given ASM from inputASM to outputASM
632
 * @param inputASM
633
 * @param outputASM
634
 * @param asmComponent
635
 */
636
76415
void copyReVectors( float *inputASM, float *outputASM, unsigned int asmComponent )
637
{
638
    unsigned int i;
639
    float re;
640
641
9857535
    for (i=0; i<NB_BINS_PER_SM; i++){
642
9781120
        re = inputASM[ (asmComponent*NB_BINS_PER_SM) + i];
643
9781120
        outputASM[ (asmComponent*NB_BINS_PER_SM)  +  i] = re;
644
    }
645
76415
}
646
647
/**
648
 * @brief ASM_patch, converts ASM from interleaved to split representation
649
 * @param inputASM
650
 * @param outputASM
651
 * @note inputASM and outputASM must be different, in other words this function can't do in place convertion
652
 * @see extractReImVectors
653
 */
654
15283
void ASM_patch( float *inputASM, float *outputASM )
655
{
656
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1B2);    // b1b2
657
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1B3 );   // b1b3
658
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1E1 );   // b1e1
659
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B1E2 );   // b1e2
660
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B2B3 );   // b2b3
661
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B2E1 );   // b2e1
662
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B2E2 );   // b2e2
663
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B3E1 );   // b3e1
664
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_B3E2 );   // b3e2
665
15283
    extractReImVectors( inputASM, outputASM, ASM_COMP_E1E2 );   // e1e2
666
667
15283
    copyReVectors(inputASM, outputASM, ASM_COMP_B1B1 );     // b1b1
668
15283
    copyReVectors(inputASM, outputASM, ASM_COMP_B2B2 );     // b2b2
669
15283
    copyReVectors(inputASM, outputASM, ASM_COMP_B3B3);      // b3b3
670
15283
    copyReVectors(inputASM, outputASM, ASM_COMP_E1E1);      // e1e1
671
15283
    copyReVectors(inputASM, outputASM, ASM_COMP_E2E2);      // e2e2
672
15283
}
673
674
8251
void ASM_compress_reorganize_and_divide_mask(float *averaged_spec_mat, float *compressed_spec_mat , float divider,
675
                                             unsigned char nbBinsCompressedMatrix, unsigned char nbBinsToAverage,
676
                                             unsigned char ASMIndexStart,
677
                                             unsigned char channel )
678
{
679
    //*************
680
    // input format
681
    // component0[0 .. 127] component1[0 .. 127] .. component24[0 .. 127]
682
    //**************
683
    // output format
684
    // matr0[0 .. 24]      matr1[0 .. 24]       .. matr127[0 .. 24]
685
    //************
686
    // compression
687
    // matr0[0 .. 24]      matr1[0 .. 24]       .. matr11[0 .. 24] => f0 NORM
688
    // matr0[0 .. 24]      matr1[0 .. 24]       .. matr22[0 .. 24] => f0 BURST, SBM
689
690
    int frequencyBin;
691
    int asmComponent;
692
    int offsetASM;
693
    int offsetCompressed;
694
    int offsetFBin;
695
    int fBinMask;
696
    int k;
697
698
    // BUILD DATA
699
214526
    for (asmComponent = 0; asmComponent < NB_VALUES_PER_SM; asmComponent++)
700
    {
701
4419850
        for( frequencyBin = 0; frequencyBin < nbBinsCompressedMatrix; frequencyBin++ )
702
        {
703
4213575
            offsetCompressed =  // NO TIME OFFSET
704
4213575
                    (frequencyBin * NB_VALUES_PER_SM)
705
                    + asmComponent;
706
4213575
            offsetASM =         // NO TIME OFFSET
707
8427150
                    (asmComponent * NB_BINS_PER_SM)
708
                    + ASMIndexStart
709
4213575
                    + (frequencyBin * nbBinsToAverage);
710
8427150
            offsetFBin = ASMIndexStart
711
4213575
                    + (frequencyBin * nbBinsToAverage);
712
4213575
            compressed_spec_mat[ offsetCompressed ] = 0;
713
23057175
            for ( k = 0; k < nbBinsToAverage; k++ )
714
            {
715
18843600
                fBinMask = getFBinMask( offsetFBin + k, channel );
716
37687200
                compressed_spec_mat[offsetCompressed ] = compressed_spec_mat[ offsetCompressed ]
717
18843600
                        + (averaged_spec_mat[ offsetASM + k ] * fBinMask);
718
            }
719
4213575
            if (divider != 0)
720
            {
721
3556325
                compressed_spec_mat[ offsetCompressed ] = compressed_spec_mat[ offsetCompressed ] / (divider * nbBinsToAverage);
722
            }
723
            else
724
            {
725
657250
                compressed_spec_mat[ offsetCompressed ] = INIT_FLOAT;
726
            }
727
        }
728
    }
729
730
8251
}
731
732
18843600
int getFBinMask( int index, unsigned char channel )
733
{
734
    unsigned int indexInChar;
735
    unsigned int indexInTheChar;
736
    int fbin;
737
    unsigned char *sy_lfr_fbins_fx_word1;
738
739
18843600
    sy_lfr_fbins_fx_word1 = parameter_dump_packet.sy_lfr_fbins_f0_word1;
740
741

18843600
    switch(channel)
742
    {
743
    case CHANNELF0:
744
13743400
        sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f0;
745
13743400
        break;
746
    case CHANNELF1:
747
3777800
        sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f1;
748
3777800
        break;
749
    case CHANNELF2:
750
1322400
        sy_lfr_fbins_fx_word1 = fbins_masks.merged_fbins_mask_f2;
751
        break;
752
    default:
753
        PRINTF("ERR *** in getFBinMask, wrong frequency channel")
754
    }
755
756
18843600
    indexInChar = index >> SHIFT_3_BITS;
757
18843600
    indexInTheChar = index - (indexInChar * BITS_PER_BYTE);
758
759
18843600
    fbin = (int) ((sy_lfr_fbins_fx_word1[ BYTES_PER_MASK - 1 - indexInChar] >> indexInTheChar) & 1);
760
761
18843600
    return fbin;
762
}
763
764
/**
765
 * @brief isPolluted returns MATRIX_IS_POLLUTED if there is any overlap between t0:t1 and tbad0:tbad1 ranges
766
 * @param t0 Start acquisition time
767
 * @param t1 End of acquisition time
768
 * @param tbad0 Start time of poluting signal
769
 * @param tbad1 End time of poluting signal
770
 * @return
771
 */
772
786911
unsigned char isPolluted( u_int64_t t0, u_int64_t t1, u_int64_t tbad0, u_int64_t tbad1 )
773
{
774
    unsigned char polluted;
775
776
786911
    polluted = MATRIX_IS_NOT_POLLUTED;
777
778


2927481
    if ( ((tbad0 < t0) && (t0 < tbad1))     // t0 is inside the polluted range
779
714820
         || ((tbad0 < t1) && (t1 < tbad1))  // t1 is inside the polluted range
780
712946
         || ((t0 < tbad0) && (tbad1 < t1))  // the polluted range is inside the signal range
781
712804
         || ((tbad0 < t0) && (t1 < tbad1))) // the signal range is inside the polluted range
782
    {
783
73964
        polluted = MATRIX_IS_POLLUTED;
784
    }
785
786
786911
    return polluted;
787
}
788
789
/**
790
 * @brief acquisitionTimeIsValid checks if the given acquisition time is poluted by PAS
791
 * @param coarseTime Coarse acquisition time of the given SM
792
 * @param fineTime Fine acquisition time of the given ASM
793
 * @param channel Frequency channel to check, will impact SM time footprint
794
 * @return MATRIX_IS_POLLUTED if there is any time overlap between SM and PAS poluting signal
795
 */
796
362127
unsigned char acquisitionTimeIsValid( unsigned int coarseTime, unsigned int fineTime, unsigned char channel)
797
{
798
    u_int64_t t0;
799
    u_int64_t t1;
800
    u_int64_t tc;
801
    u_int64_t tbad0;
802
    u_int64_t tbad1;
803
804
    u_int64_t modulusInFineTime;
805
    u_int64_t offsetInFineTime;
806
    u_int64_t shiftInFineTime;
807
    u_int64_t tbadInFineTime;
808
809
    u_int64_t timecodeReference;
810
811
    unsigned char pasFilteringIsEnabled;
812
    unsigned char ret;
813
814
    // compute acquisition time from caoarseTime and fineTime
815
362127
    t0 = ( ((u_int64_t)coarseTime) <<  SHIFT_2_BYTES ) + (u_int64_t) fineTime;
816
362127
    t1 = t0;
817
362127
    tc = t0;
818
362127
    tbad0 = t0;
819
362127
    tbad1 = t0;
820
821

362127
    switch(channel)
822
    {
823
    case CHANNELF0:
824
308200
        t1 = t0 + ACQUISITION_DURATION_F0;
825
308200
        tc = t0 + HALF_ACQUISITION_DURATION_F0;
826
308200
        break;
827
    case CHANNELF1:
828
50793
        t1 = t0 + ACQUISITION_DURATION_F1;
829
50793
        tc = t0 + HALF_ACQUISITION_DURATION_F1;
830
50793
        break;
831
    case CHANNELF2:
832
3135
        t1 = t0 + ACQUISITION_DURATION_F2;
833
3135
        tc = t0 + HALF_ACQUISITION_DURATION_F2;
834
        break;
835
    default:
836
        break;
837
    }
838
839
    // compute the acquitionTime range
840
362127
    modulusInFineTime   = filterPar.modulus_in_finetime;
841
362127
    offsetInFineTime    = filterPar.offset_in_finetime;
842
362127
    shiftInFineTime     = filterPar.shift_in_finetime;
843
362127
    tbadInFineTime      = filterPar.tbad_in_finetime;
844
362127
    timecodeReference   = INIT_INT;
845
846
362127
    pasFilteringIsEnabled = (filterPar.spare_sy_lfr_pas_filter_enabled & 1); // [0000 0001]
847
362127
    ret = MATRIX_IS_NOT_POLLUTED;
848
849
362127
    if ( (tbadInFineTime == 0) || (pasFilteringIsEnabled == 0) )
850
    {
851
75168
        ret = MATRIX_IS_NOT_POLLUTED;
852
    }
853
    else
854
    {
855
        // INTERSECTION TEST #1
856
286959
        timecodeReference = (tc - (tc % modulusInFineTime)) - modulusInFineTime ;
857
286959
        tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
858
286959
        tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
859
286959
        ret = isPolluted( t0, t1, tbad0, tbad1 );
860
861
        // INTERSECTION TEST #2
862
286958
        if (ret == MATRIX_IS_NOT_POLLUTED)
863
        {
864
286934
            timecodeReference = (tc - (tc % modulusInFineTime)) ;
865
286934
            tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
866
286934
            tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
867
286934
            ret = isPolluted( t0, t1, tbad0, tbad1 );
868
        }
869
870
        // INTERSECTION TEST #3
871
286974
        if (ret == MATRIX_IS_NOT_POLLUTED)
872
        {
873
212922
            timecodeReference = (tc - (tc % modulusInFineTime)) + modulusInFineTime ;
874
212922
            tbad0 = timecodeReference + offsetInFineTime + shiftInFineTime;
875
212922
            tbad1 = timecodeReference + offsetInFineTime + shiftInFineTime + tbadInFineTime;
876
212922
            ret = isPolluted( t0, t1, tbad0, tbad1 );
877
        }
878
    }
879
880
362206
    return ret;
881
}
882
883
2
void init_kcoeff_sbm_from_kcoeff_norm(float *input_kcoeff, float *output_kcoeff, unsigned char nb_bins_norm)
884
{
885
    unsigned char bin;
886
    unsigned char kcoeff;
887
888
26
    for (bin=0; bin<nb_bins_norm; bin++)
889
    {
890
792
        for (kcoeff=0; kcoeff<NB_K_COEFF_PER_BIN; kcoeff++)
891
        {
892
1536
            output_kcoeff[   ( (bin * NB_K_COEFF_PER_BIN) + kcoeff ) * SBM_COEFF_PER_NORM_COEFF       ]
893
768
                    = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
894
1536
            output_kcoeff[ ( ( (bin * NB_K_COEFF_PER_BIN ) + kcoeff) * SBM_COEFF_PER_NORM_COEFF ) + 1 ]
895
768
                    = input_kcoeff[ (bin*NB_K_COEFF_PER_BIN) + kcoeff ];
896
        }
897
    }
898
2
}