LCOV - code coverage report
Current view: top level - src/hw - wf_handler.c (source / functions) Hit Total Coverage
Test: trace.info Lines: 221 493 44.8 %
Date: 2023-02-20 11:47:12 Functions: 20 31 64.5 %

          Line data    Source code
       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 and tasks related to waveform packet generation.
      26             :  *
      27             :  * @file
      28             :  * @author P. LEROY
      29             :  *
      30             :  * A group of functions to handle waveforms, in snapshot or continuous format.\n
      31             :  *
      32             :  */
      33             : 
      34             : #include "hw/wf_handler.h"
      35             : #include "fsw_compile_warnings.h"
      36             : #include "fsw_debug.h"
      37             : #include "fsw_misc.h"
      38             : #include "hw/lfr_regs.h"
      39             : #include "lfr_common_headers/fsw_params.h"
      40             : 
      41             : //***************
      42             : // waveform rings
      43             : // F0
      44             : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
      45             : ring_node waveform_ring_f0[NB_RING_NODES_F0] = { { 0 } };
      46             : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
      47             : ring_node* current_ring_node_f0 = NULL;
      48             : ring_node* ring_node_to_send_swf_f0 = NULL;
      49             : // F1
      50             : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
      51             : ring_node waveform_ring_f1[NB_RING_NODES_F1] = { { 0 } };
      52             : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
      53             : ring_node* current_ring_node_f1 = NULL;
      54             : ring_node* ring_node_to_send_swf_f1 = NULL;
      55             : ring_node* ring_node_to_send_cwf_f1 = NULL;
      56             : // F2
      57             : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
      58             : ring_node waveform_ring_f2[NB_RING_NODES_F2] = { { 0 } };
      59             : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
      60             : ring_node* current_ring_node_f2 = NULL;
      61             : ring_node* ring_node_to_send_swf_f2 = NULL;
      62             : ring_node* ring_node_to_send_cwf_f2 = NULL;
      63             : // F3
      64             : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
      65             : ring_node waveform_ring_f3[NB_RING_NODES_F3] = { { 0 } };
      66             : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
      67             : ring_node* current_ring_node_f3 = NULL;
      68             : ring_node* ring_node_to_send_cwf_f3 = NULL;
      69             : char wf_cont_f3_light[NB_SAMPLES_PER_SNAPSHOT * NB_BYTES_CWF3_LIGHT_BLK] = { 0 };
      70             : 
      71             : bool extractSWF1 = false;
      72             : bool extractSWF2 = false;
      73             : bool swf0_ready_flag_f1 = false;
      74             : bool swf0_ready_flag_f2 = false;
      75             : bool swf1_ready = false;
      76             : bool swf2_ready = false;
      77             : 
      78             : int swf1_extracted[(NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK)] = { 0 };
      79             : int swf2_extracted[(NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK)] = { 0 };
      80             : 
      81             : DISABLE_MISSING_FIELD_INITIALIZER_WARNING
      82             : ring_node ring_node_swf1_extracted = { 0 };
      83             : ring_node ring_node_swf2_extracted = { 0 };
      84             : ENABLE_MISSING_FIELD_INITIALIZER_WARNING
      85             : 
      86             : typedef enum resynchro_state_t
      87             : {
      88             :     MEASURE,
      89             :     CORRECTION
      90             : } resynchro_state;
      91             : 
      92             : //*********************
      93             : // Interrupt SubRoutine
      94             : 
      95           0 : ring_node* getRingNodeToSendCWF(unsigned char frequencyChannel)
      96             : {
      97             :     ring_node* node;
      98             : 
      99          22 :     node = NULL;
     100          22 :     switch (frequencyChannel)
     101             :     {
     102             :         case CHANNELF1:
     103          22 :             node = ring_node_to_send_cwf_f1;
     104           0 :             break;
     105             :         case CHANNELF2:
     106           0 :             node = ring_node_to_send_cwf_f2;
     107           0 :             break;
     108             :         case CHANNELF3:
     109           0 :             node = ring_node_to_send_cwf_f3;
     110             :             break;
     111             :         default:
     112             :             break;
     113             :     }
     114             : 
     115          22 :     return node;
     116             : }
     117             : 
     118           0 : ring_node* getRingNodeToSendSWF(unsigned char frequencyChannel)
     119             : {
     120             :     ring_node* node;
     121             : 
     122           0 :     node = NULL;
     123           0 :     switch (frequencyChannel)
     124             :     {
     125             :         case CHANNELF0:
     126           0 :             node = ring_node_to_send_swf_f0;
     127           0 :             break;
     128             :         case CHANNELF1:
     129           0 :             node = ring_node_to_send_swf_f1;
     130           0 :             break;
     131             :         case CHANNELF2:
     132           0 :             node = ring_node_to_send_swf_f2;
     133             :             break;
     134             :         default:
     135             :             break;
     136             :     }
     137             : 
     138           0 :     return node;
     139             : }
     140             : 
     141           3 : void reset_extractSWF(void)
     142             : {
     143           3 :     extractSWF1 = false;
     144           3 :     extractSWF2 = false;
     145           3 :     swf0_ready_flag_f1 = false;
     146           3 :     swf0_ready_flag_f2 = false;
     147           3 :     swf1_ready = false;
     148           3 :     swf2_ready = false;
     149           3 : }
     150             : 
     151          22 : void waveforms_isr_f3(void)
     152             : {
     153          22 :     if ((lfrCurrentMode == LFR_MODE_NORMAL)
     154             :         || (lfrCurrentMode
     155             :             == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet
     156             :         || (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2))
     157             :     { // in modes other than STANDBY and BURST, send the CWF_F3 data
     158             :         //***
     159             :         // F3
     160          22 :         if ((waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR)
     161             :         { // [1100 0000] check the f3 full bits
     162           0 :             ring_node_to_send_cwf_f3 = current_ring_node_f3->previous;
     163           0 :             current_ring_node_f3 = current_ring_node_f3->next;
     164           0 :             if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0)
     165             :             { // [0100 0000] f3 buffer 0 is full
     166           0 :                 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time;
     167           0 :                 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time;
     168           0 :                 waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address;
     169           0 :                 waveform_picker_regs->status
     170           0 :                     = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000]
     171             :             }
     172           0 :             else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1)
     173             :             { // [1000 0000] f3 buffer 1 is full
     174           0 :                 ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time;
     175           0 :                 ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time;
     176           0 :                 waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address;
     177           0 :                 waveform_picker_regs->status
     178           0 :                     = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000]
     179             :             }
     180           0 :             if (rtems_event_send(Task_id[TASKID_CWF3], RTEMS_EVENT_0) != RTEMS_SUCCESSFUL)
     181             :             {
     182             :                 DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_0));
     183             :             }
     184             :         }
     185             :     }
     186          22 : }
     187             : 
     188           0 : void waveforms_isr_burst(void)
     189             : {
     190             :     unsigned char status;
     191             : 
     192           0 :     status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2)
     193             :         >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2
     194             : 
     195           0 :     switch (status)
     196             :     {
     197             :         case BIT_WFP_BUFFER_0:
     198           0 :             ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
     199           0 :             ring_node_to_send_cwf_f2->packet_id = SID_BURST_CWF_F2;
     200           0 :             ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
     201           0 :             ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
     202           0 :             current_ring_node_f2 = current_ring_node_f2->next;
     203           0 :             waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
     204           0 :             if (rtems_event_send(Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST) != RTEMS_SUCCESSFUL)
     205             :             {
     206             :                 DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_0));
     207             :             }
     208           0 :             waveform_picker_regs->status
     209           0 :                 = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
     210           0 :             break;
     211             :         case BIT_WFP_BUFFER_1:
     212           0 :             ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
     213           0 :             ring_node_to_send_cwf_f2->packet_id = SID_BURST_CWF_F2;
     214           0 :             ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
     215           0 :             ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
     216           0 :             current_ring_node_f2 = current_ring_node_f2->next;
     217           0 :             waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
     218           0 :             if (rtems_event_send(Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST) != RTEMS_SUCCESSFUL)
     219             :             {
     220             :                 DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_0));
     221             :             }
     222           0 :             waveform_picker_regs->status
     223           0 :                 = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
     224             :             break;
     225             :         default:
     226             :             break;
     227             :     }
     228           0 : }
     229             : 
     230          22 : void waveform_isr_normal_sbm1_sbm2(void)
     231             : {
     232             :     //***
     233             :     // F0
     234          22 :     if ((waveform_picker_regs->status & BITS_WFP_STATUS_F0)
     235             :         != INIT_CHAR) // [0000 0011] check the f0 full bits
     236             :     {
     237           0 :         swf0_ready_flag_f1 = true;
     238           0 :         swf0_ready_flag_f2 = true;
     239           0 :         ring_node_to_send_swf_f0 = current_ring_node_f0->previous;
     240           0 :         current_ring_node_f0 = current_ring_node_f0->next;
     241           0 :         if ((waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0)
     242             :         {
     243             : 
     244           0 :             ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time;
     245           0 :             ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time;
     246           0 :             waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address;
     247           0 :             waveform_picker_regs->status
     248           0 :                 = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001]
     249             :         }
     250           0 :         else if ((waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1)
     251             :         {
     252           0 :             ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time;
     253           0 :             ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time;
     254           0 :             waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address;
     255           0 :             waveform_picker_regs->status
     256           0 :                 = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010]
     257             :         }
     258             :         // send an event to the WFRM task for resynchro activities
     259           0 :         DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH));
     260           0 :         DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_CALI], RTEMS_EVENT_CAL_SWEEP_WAKE));
     261             :     }
     262             : 
     263             :     //***
     264             :     // F1
     265          22 :     if ((waveform_picker_regs->status & BITS_WFP_STATUS_F1) != INIT_CHAR)
     266             :     { // [0000 1100] check the f1 full bits
     267             :         // (1) change the receiving buffer for the waveform picker
     268          22 :         ring_node_to_send_cwf_f1 = current_ring_node_f1->previous;
     269          22 :         current_ring_node_f1 = current_ring_node_f1->next;
     270          22 :         if ((waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0)
     271             :         {
     272          12 :             ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time;
     273          12 :             ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time;
     274          12 :             waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address;
     275          24 :             waveform_picker_regs->status
     276          12 :                 = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0
     277             :         }
     278          10 :         else if ((waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1)
     279             :         {
     280          10 :             ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time;
     281          10 :             ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time;
     282          10 :             waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address;
     283          20 :             waveform_picker_regs->status
     284          10 :                 = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0
     285             :         }
     286             :         // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if
     287             :         // needed)
     288          22 :         DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2));
     289             :     }
     290             : 
     291             :     //***
     292             :     // F2
     293          22 :     if ((waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR)
     294             :     { // [0011 0000] check the f2 full bit
     295             :         // (1) change the receiving buffer for the waveform picker
     296           0 :         ring_node_to_send_cwf_f2 = current_ring_node_f2->previous;
     297           0 :         ring_node_to_send_cwf_f2->packet_id = SID_SBM2_CWF_F2;
     298           0 :         current_ring_node_f2 = current_ring_node_f2->next;
     299           0 :         if ((waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0)
     300             :         {
     301           0 :             ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time;
     302           0 :             ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time;
     303           0 :             waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address;
     304           0 :             waveform_picker_regs->status
     305           0 :                 = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000]
     306             :         }
     307           0 :         else if ((waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1)
     308             :         {
     309           0 :             ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time;
     310           0 :             ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time;
     311           0 :             waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address;
     312           0 :             waveform_picker_regs->status
     313           0 :                 = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000]
     314             :         }
     315             :         // (2) send an event for the waveforms transmission
     316           0 :         DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2));
     317             :     }
     318          22 : }
     319             : 
     320          22 : rtems_isr waveforms_isr(rtems_vector_number vector)
     321             : {
     322             :     /** This is the interrupt sub routine called by the waveform picker core.
     323             :      *
     324             :      * This ISR launch different actions depending mainly on two pieces of information:
     325             :      * 1. the values read in the registers of the waveform picker.
     326             :      * 2. the current LFR mode.
     327             :      *
     328             :      */
     329             : 
     330             :     // STATUS
     331             :     // new error        error buffer full
     332             :     // 15 14 13 12      11 10 9  8
     333             :     // f3 f2 f1 f0      f3 f2 f1 f0
     334             :     //
     335             :     // ready buffer
     336             :     // 7    6    5    4    3    2    1    0
     337             :     // f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0
     338             :     IGNORE_UNUSED_PARAMETER(vector);
     339             : 
     340          22 :     waveforms_isr_f3();
     341             : 
     342             :     //*************************************************
     343             :     // copy the status bits in the housekeeping packets
     344          22 :     housekeeping_packet.hk_lfr_vhdl_iir_cal
     345          22 :         = (unsigned char)((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE);
     346             : 
     347          22 :     if ((waveform_picker_regs->status & BYTE0_MASK)
     348             :         != INIT_CHAR) // [1111 1111 0000 0000] check the error bits
     349             :     {
     350             :         DEBUG_CHECK_STATUS(send_event_dumb_task(RTEMS_EVENT_10));
     351             :     }
     352             : 
     353          22 :     switch (lfrCurrentMode)
     354             :     {
     355             :         //********
     356             :         // STANDBY
     357             :         case LFR_MODE_STANDBY:
     358             :             break;
     359             :             //**************************
     360             :             // LFR NORMAL, SBM1 and SBM2
     361             :         case LFR_MODE_NORMAL:
     362             :         case LFR_MODE_SBM1:
     363             :         case LFR_MODE_SBM2:
     364          22 :             waveform_isr_normal_sbm1_sbm2();
     365          22 :             break;
     366             :             //******
     367             :             // BURST
     368             :         case LFR_MODE_BURST:
     369           0 :             waveforms_isr_burst();
     370             :             break;
     371             :             //********
     372             :             // DEFAULT
     373             :         default:
     374             :             break;
     375             :     }
     376          22 : }
     377             : 
     378             : //************
     379             : // RTEMS TASKS
     380             : 
     381           3 : LFR_NO_RETURN rtems_task wfrm_task(
     382             :     rtems_task_argument argument) // used with the waveform picker VHDL IP
     383             : {
     384             :     /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode.
     385             :      *
     386             :      * @param unused is the starting argument of the RTEMS task
     387             :      *
     388             :      * The following data packets are sent by this task:
     389             :      * - TM_LFR_SCIENCE_NORMAL_SWF_F0
     390             :      * - TM_LFR_SCIENCE_NORMAL_SWF_F1
     391             :      * - TM_LFR_SCIENCE_NORMAL_SWF_F2
     392             :      *
     393             :      */
     394             : 
     395             :     IGNORE_UNUSED_PARAMETER(argument);
     396             : 
     397           3 :     rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
     398           3 :     rtems_id queue_id = RTEMS_ID_NONE;
     399             :     ring_node* ring_node_swf1_extracted_ptr;
     400             :     ring_node* ring_node_swf2_extracted_ptr;
     401             : 
     402           3 :     ring_node_swf1_extracted_ptr = &ring_node_swf1_extracted;
     403           3 :     ring_node_swf2_extracted_ptr = &ring_node_swf2_extracted;
     404             : 
     405           3 :     DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
     406             : 
     407             :     BOOT_PRINTF("in WFRM ***\n");
     408             : 
     409             :     while (1)
     410             :     {
     411             :         // wait for an RTEMS_EVENT
     412           3 :         rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH,
     413             :             RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
     414             : 
     415           0 :         if (event_out == RTEMS_EVENT_MODE_NORMAL)
     416             :         {
     417             :             DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n");
     418           0 :             ring_node_to_send_swf_f0->packet_id = SID_NORM_SWF_F0;
     419           0 :             ring_node_swf1_extracted_ptr->packet_id = SID_NORM_SWF_F1;
     420           0 :             ring_node_swf2_extracted_ptr->packet_id = SID_NORM_SWF_F2;
     421           0 :             DEBUG_CHECK_STATUS(
     422             :                 rtems_message_queue_send(queue_id, &ring_node_to_send_swf_f0, sizeof(ring_node*)));
     423           0 :             DEBUG_CHECK_STATUS(rtems_message_queue_send(
     424             :                 queue_id, &ring_node_swf1_extracted_ptr, sizeof(ring_node*)));
     425           0 :             DEBUG_CHECK_STATUS(rtems_message_queue_send(
     426             :                 queue_id, &ring_node_swf2_extracted_ptr, sizeof(ring_node*)));
     427             :         }
     428           0 :         if (event_out == RTEMS_EVENT_SWF_RESYNCH)
     429             :         {
     430           0 :             snapshot_resynchronization((unsigned char*)&ring_node_to_send_swf_f0->coarseTime);
     431             :         }
     432             :     }
     433             : }
     434             : 
     435           3 : LFR_NO_RETURN rtems_task cwf3_task(
     436             :     rtems_task_argument argument) // used with the waveform picker VHDL IP
     437             : {
     438             :     /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3.
     439             :      *
     440             :      * @param unused is the starting argument of the RTEMS task
     441             :      *
     442             :      * The following data packet is sent by this task:
     443             :      * - TM_LFR_SCIENCE_NORMAL_CWF_F3
     444             :      *
     445             :      */
     446             : 
     447             :     IGNORE_UNUSED_PARAMETER(argument);
     448             : 
     449           3 :     rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
     450           3 :     rtems_id queue_id = RTEMS_ID_NONE;
     451             :     ring_node ring_node_cwf3_light;
     452             :     ring_node* ring_node_to_send_cwf;
     453             : 
     454             : 
     455           3 :     DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
     456             : 
     457           3 :     ring_node_to_send_cwf_f3->packet_id = SID_NORM_CWF_LONG_F3;
     458             : 
     459             :     // init the ring_node_cwf3_light structure
     460           3 :     ring_node_cwf3_light.buffer_address = wf_cont_f3_light;
     461           3 :     ring_node_cwf3_light.coarseTime = INIT_CHAR;
     462           3 :     ring_node_cwf3_light.fineTime = INIT_CHAR;
     463           3 :     ring_node_cwf3_light.next = NULL;
     464           3 :     ring_node_cwf3_light.previous = NULL;
     465           3 :     ring_node_cwf3_light.packet_id = SID_NORM_CWF_F3;
     466           3 :     ring_node_cwf3_light.status = INIT_CHAR;
     467             : 
     468             :     BOOT_PRINTF("in CWF3 ***\n");
     469             : 
     470             :     while (1)
     471             :     {
     472             :         // wait for an RTEMS_EVENT
     473           3 :         rtems_event_receive(
     474             :             RTEMS_EVENT_0, RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out);
     475           0 :         if ((lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_SBM1)
     476           0 :             || (lfrCurrentMode == LFR_MODE_SBM2))
     477             :         {
     478           0 :             ring_node_to_send_cwf = getRingNodeToSendCWF(CHANNELF3);
     479           0 :             if ((parameter_dump_packet.sy_lfr_n_cwf_long_f3 & BIT_CWF_LONG_F3) == BIT_CWF_LONG_F3)
     480             :             {
     481             :                 LFR_PRINTF("send CWF_LONG_F3\n");
     482           0 :                 ring_node_to_send_cwf_f3->packet_id = SID_NORM_CWF_LONG_F3;
     483           0 :                 DEBUG_CHECK_STATUS(
     484             :                     rtems_message_queue_send(queue_id, &ring_node_to_send_cwf, sizeof(ring_node*)));
     485             :             }
     486             :             else
     487             :             {
     488             :                 LFR_PRINTF("send CWF_F3 (light)\n");
     489           0 :                 send_waveform_CWF3_light(ring_node_to_send_cwf, &ring_node_cwf3_light, queue_id);
     490             :             }
     491             :         }
     492             :         else
     493             :         {
     494             :             LFR_PRINTF("in CWF3 *** lfrCurrentMode is %d, no data will be sent\n", lfrCurrentMode);
     495             :         }
     496             :     }
     497             : }
     498             : 
     499           3 : LFR_NO_RETURN rtems_task cwf2_task(rtems_task_argument argument) // ONLY USED IN BURST AND SBM2
     500             : {
     501             :     /** This RTEMS task is dedicated to the transmission of continuous waveforms at f2.
     502             :      *
     503             :      * @param unused is the starting argument of the RTEMS task
     504             :      *
     505             :      * The following data packet is sent by this function:
     506             :      * - TM_LFR_SCIENCE_BURST_CWF_F2
     507             :      * - TM_LFR_SCIENCE_SBM2_CWF_F2
     508             :      *
     509             :      */
     510             : 
     511             :     IGNORE_UNUSED_PARAMETER(argument);
     512             : 
     513           3 :     rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
     514           3 :     rtems_id queue_id = RTEMS_ID_NONE;
     515             :     ring_node* ring_node_to_send;
     516             :     unsigned long long int acquisitionTimeF0_asLong;
     517             : 
     518           3 :     acquisitionTimeF0_asLong = INIT_CHAR;
     519             : 
     520           3 :     DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
     521             : 
     522             :     BOOT_PRINTF("in CWF2 ***\n");
     523             : 
     524             :     while (1)
     525             :     {
     526             :         // wait for an RTEMS_EVENT// send the snapshot when built
     527           3 :         DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_SBM2));
     528           3 :         DEBUG_CHECK_STATUS(rtems_event_receive(RTEMS_EVENT_MODE_NORM_S1_S2 | RTEMS_EVENT_MODE_BURST,
     529             :             RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out));
     530           0 :         ring_node_to_send = getRingNodeToSendCWF(CHANNELF2);
     531           0 :         if (event_out == RTEMS_EVENT_MODE_BURST)
     532             :         { // data are sent whatever the transition time
     533           0 :             DEBUG_CHECK_STATUS(
     534             :                 rtems_message_queue_send(queue_id, &ring_node_to_send, sizeof(ring_node*)));
     535             :         }
     536           0 :         else if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
     537             :         {
     538           0 :             if (lfrCurrentMode == LFR_MODE_SBM2
     539           0 :                 && time_management_regs->coarse_time >= lastValidEnterModeTime)
     540             :             {
     541           0 :                 DEBUG_CHECK_STATUS(
     542             :                     rtems_message_queue_send(queue_id, &ring_node_to_send, sizeof(ring_node*)));
     543             :             }
     544             :             // launch snapshot extraction if needed
     545           0 :             if (extractSWF2 == true)
     546             :             {
     547           0 :                 ring_node_to_send_swf_f2 = ring_node_to_send_cwf_f2;
     548             :                 // extract the snapshot
     549           0 :                 build_snapshot_from_ring(ring_node_to_send_swf_f2, CHANNELF2,
     550             :                     acquisitionTimeF0_asLong, &ring_node_swf2_extracted, swf2_extracted);
     551           0 :                 extractSWF2 = false;
     552           0 :                 swf2_ready = true; // once the snapshot at f2 is ready the CWF1 task will send an
     553             :                                    // event to WFRM
     554             :             }
     555           0 :             if (swf0_ready_flag_f2 == true)
     556             :             {
     557           0 :                 extractSWF2 = true;
     558             :                 // record the acquition time of the f0 snapshot to use to build the snapshot at f2
     559           0 :                 acquisitionTimeF0_asLong
     560           0 :                     = get_acquisition_time((unsigned char*)&ring_node_to_send_swf_f0->coarseTime);
     561           0 :                 swf0_ready_flag_f2 = false;
     562             :             }
     563             :         }
     564             :     }
     565             : }
     566             : 
     567           3 : LFR_NO_RETURN rtems_task cwf1_task(rtems_task_argument argument) // ONLY USED IN SBM1
     568             : {
     569             :     /** This RTEMS task is dedicated to the transmission of continuous waveforms at f1.
     570             :      *
     571             :      * @param unused is the starting argument of the RTEMS task
     572             :      *
     573             :      * The following data packet is sent by this function:
     574             :      * - TM_LFR_SCIENCE_SBM1_CWF_F1
     575             :      *
     576             :      */
     577             : 
     578             :     IGNORE_UNUSED_PARAMETER(argument);
     579             : 
     580           3 :     rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
     581           3 :     rtems_id queue_id = RTEMS_ID_NONE;
     582             : 
     583             :     ring_node* ring_node_to_send_cwf;
     584             : 
     585           3 :     DEBUG_CHECK_STATUS(get_message_queue_id_send(&queue_id));
     586             : 
     587             :     BOOT_PRINTF("in CWF1 ***\n");
     588             : 
     589             :     while (1)
     590             :     {
     591             :         // wait for an RTEMS_EVENT
     592          25 :         rtems_event_receive(RTEMS_EVENT_MODE_NORM_S1_S2, RTEMS_WAIT | RTEMS_EVENT_ANY,
     593             :             RTEMS_NO_TIMEOUT, &event_out);
     594          22 :         ring_node_to_send_cwf = getRingNodeToSendCWF(1);
     595          22 :         ring_node_to_send_cwf_f1->packet_id = SID_SBM1_CWF_F1;
     596          29 :         if (lfrCurrentMode == LFR_MODE_SBM1
     597          29 :             && time_management_regs->coarse_time >= lastValidEnterModeTime)
     598             :         {
     599           7 :             DEBUG_CHECK_STATUS(
     600             :                 rtems_message_queue_send(queue_id, &ring_node_to_send_cwf, sizeof(ring_node*)));
     601             :         }
     602             :         // launch snapshot extraction if needed
     603          22 :         if (extractSWF1 == true)
     604             :         {
     605           0 :             ring_node_to_send_swf_f1 = ring_node_to_send_cwf;
     606             :             // launch the snapshot extraction
     607           0 :             DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_SWBD], RTEMS_EVENT_MODE_NORM_S1_S2));
     608           0 :             extractSWF1 = false;
     609             :         }
     610          22 :         if (swf0_ready_flag_f1 == true)
     611             :         {
     612           0 :             extractSWF1 = true;
     613           0 :             swf0_ready_flag_f1 = false; // this step shall be executed only one time
     614             :         }
     615          22 :         if ((swf1_ready == true) && (swf2_ready == true)) // swf_f1 is ready after the extraction
     616             :         {
     617           0 :             DEBUG_CHECK_STATUS(rtems_event_send(Task_id[TASKID_WFRM], RTEMS_EVENT_MODE_NORMAL));
     618           0 :             swf1_ready = false;
     619           0 :             swf2_ready = false;
     620             :         }
     621             :     }
     622             : }
     623             : 
     624           1 : LFR_NO_RETURN rtems_task swbd_task(rtems_task_argument argument)
     625             : {
     626             :     /** This RTEMS task is dedicated to the building of snapshots from different continuous
     627             :      * waveforms buffers.
     628             :      *
     629             :      * @param unused is the starting argument of the RTEMS task
     630             :      *
     631             :      */
     632             : 
     633             :     IGNORE_UNUSED_PARAMETER(argument);
     634             : 
     635           1 :     rtems_event_set event_out = EVENT_SETS_NONE_PENDING;
     636           1 :     unsigned long long int acquisitionTimeF0_asLong = 0;
     637             : 
     638             :     BOOT_PRINTF("in SWBD ***\n");
     639             : 
     640             :     while (1)
     641             :     {
     642             :         // wait for an RTEMS_EVENT
     643           1 :         rtems_event_receive(RTEMS_EVENT_MODE_NORM_S1_S2, RTEMS_WAIT | RTEMS_EVENT_ANY,
     644             :             RTEMS_NO_TIMEOUT, &event_out);
     645           0 :         if (event_out == RTEMS_EVENT_MODE_NORM_S1_S2)
     646             :         {
     647           0 :             acquisitionTimeF0_asLong
     648           0 :                 = get_acquisition_time((unsigned char*)&ring_node_to_send_swf_f0->coarseTime);
     649           0 :             build_snapshot_from_ring(ring_node_to_send_swf_f1, CHANNELF1, acquisitionTimeF0_asLong,
     650             :                 &ring_node_swf1_extracted, swf1_extracted);
     651           0 :             swf1_ready = true; // the snapshot has been extracted and is ready to be sent
     652             :         }
     653             :         else
     654             :         {
     655             :             LFR_PRINTF("in SWBD *** unexpected rtems event received %x\n", (int)event_out);
     656             :         }
     657             :     }
     658             : }
     659             : 
     660             : //******************
     661             : // general functions
     662             : 
     663           1 : void WFP_init_rings(void)
     664             : {
     665             :     // F0 RING
     666           1 :     init_ring(waveform_ring_f0, NB_RING_NODES_F0, wf_buffer_f0, WFRM_BUFFER);
     667             :     // F1 RING
     668           1 :     init_ring(waveform_ring_f1, NB_RING_NODES_F1, wf_buffer_f1, WFRM_BUFFER);
     669             :     // F2 RING
     670           1 :     init_ring(waveform_ring_f2, NB_RING_NODES_F2, wf_buffer_f2, WFRM_BUFFER);
     671             :     // F3 RING
     672           1 :     init_ring(waveform_ring_f3, NB_RING_NODES_F3, wf_buffer_f3, WFRM_BUFFER);
     673             : 
     674           1 :     ring_node_swf1_extracted.buffer_address = swf1_extracted;
     675           1 :     ring_node_swf2_extracted.buffer_address = swf2_extracted;
     676             : 
     677             :     DEBUG_PRINTF("waveform_ring_f0 @%x\n", (unsigned int)waveform_ring_f0);
     678             :     DEBUG_PRINTF("waveform_ring_f1 @%x\n", (unsigned int)waveform_ring_f1);
     679             :     DEBUG_PRINTF("waveform_ring_f2 @%x\n", (unsigned int)waveform_ring_f2);
     680             :     DEBUG_PRINTF("waveform_ring_f3 @%x\n", (unsigned int)waveform_ring_f3);
     681             :     DEBUG_PRINTF("wf_buffer_f0 @%x\n", (unsigned int)wf_buffer_f0);
     682             :     DEBUG_PRINTF("wf_buffer_f1 @%x\n", (unsigned int)wf_buffer_f1);
     683             :     DEBUG_PRINTF("wf_buffer_f2 @%x\n", (unsigned int)wf_buffer_f2);
     684             :     DEBUG_PRINTF("wf_buffer_f3 @%x\n", (unsigned int)wf_buffer_f3);
     685           1 : }
     686             : 
     687           4 : void WFP_reset_current_ring_nodes(void)
     688             : {
     689           4 :     current_ring_node_f0 = waveform_ring_f0[0].next;
     690           4 :     current_ring_node_f1 = waveform_ring_f1[0].next;
     691           4 :     current_ring_node_f2 = waveform_ring_f2[0].next;
     692           4 :     current_ring_node_f3 = waveform_ring_f3[0].next;
     693             : 
     694           4 :     ring_node_to_send_swf_f0 = waveform_ring_f0;
     695           4 :     ring_node_to_send_swf_f1 = waveform_ring_f1;
     696           4 :     ring_node_to_send_swf_f2 = waveform_ring_f2;
     697             : 
     698           4 :     ring_node_to_send_cwf_f1 = waveform_ring_f1;
     699           4 :     ring_node_to_send_cwf_f2 = waveform_ring_f2;
     700           4 :     ring_node_to_send_cwf_f3 = waveform_ring_f3;
     701           4 : }
     702             : 
     703           0 : int send_waveform_CWF3_light(
     704             :     ring_node* ring_node_to_send, ring_node* ring_node_cwf3_light, rtems_id queue_id)
     705             : {
     706             :     /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data.
     707             :      *
     708             :      * @param waveform points to the buffer containing the data that will be send.
     709             :      * @param headerCWF points to a table of headers that have been prepared for the data
     710             :      * transmission.
     711             :      * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be
     712             :      * send. The structures contain information to setup the transmission of the data packets.
     713             :      *
     714             :      * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a
     715             :      * data buffer from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1
     716             :      * one containing 8 blocks.
     717             :      *
     718             :      */
     719             : 
     720           0 :     int ret = LFR_SUCCESSFUL;
     721             :     rtems_status_code status;
     722             : 
     723             :     const char* sample;
     724           0 :     const int* const dataPtr = (int*)ring_node_to_send->buffer_address;
     725             : 
     726           0 :     ring_node_cwf3_light->coarseTime = ring_node_to_send->coarseTime;
     727           0 :     ring_node_cwf3_light->fineTime = ring_node_to_send->fineTime;
     728             : 
     729             :     //**********************
     730             :     // BUILD CWF3_light DATA
     731           0 :     for (unsigned int i = 0; i < NB_SAMPLES_PER_SNAPSHOT; i++)
     732             :     {
     733           0 :         sample = (const char*)&dataPtr[i * NB_WORDS_SWF_BLK];
     734           0 :         for (unsigned int j = 0; j < CWF_BLK_SIZE; j++)
     735             :         {
     736           0 :             wf_cont_f3_light[(i * NB_BYTES_CWF3_LIGHT_BLK) + j] = sample[j];
     737             :         }
     738             :     }
     739             : 
     740             :     // SEND PACKET
     741           0 :     status = rtems_message_queue_send(queue_id, &ring_node_cwf3_light, sizeof(ring_node*));
     742           0 :     if (status != RTEMS_SUCCESSFUL)
     743             :     {
     744           0 :         ret = LFR_DEFAULT;
     745             :     }
     746             : 
     747           0 :     return ret;
     748             : }
     749             : 
     750          56 : void compute_acquisition_time(unsigned int coarseTime, unsigned int fineTime, unsigned int sid,
     751             :     unsigned char pa_lfr_pkt_nr, unsigned char* acquisitionTime)
     752             : {
     753             :     unsigned long long int acquisitionTimeAsLong;
     754             :     unsigned char localAcquisitionTime[BYTES_PER_TIME];
     755          56 :     double deltaT = 0.;
     756             : 
     757          56 :     localAcquisitionTime[BYTE_0] = (unsigned char)(coarseTime >> SHIFT_3_BYTES);
     758          56 :     localAcquisitionTime[BYTE_1] = (unsigned char)(coarseTime >> SHIFT_2_BYTES);
     759          56 :     localAcquisitionTime[BYTE_2] = (unsigned char)(coarseTime >> SHIFT_1_BYTE);
     760          56 :     localAcquisitionTime[BYTE_3] = (unsigned char)(coarseTime);
     761          56 :     localAcquisitionTime[BYTE_4] = (unsigned char)(fineTime >> SHIFT_1_BYTE);
     762          56 :     localAcquisitionTime[BYTE_5] = (unsigned char)(fineTime);
     763             : 
     764         112 :     acquisitionTimeAsLong = ((unsigned long long int)localAcquisitionTime[BYTE_0] << SHIFT_5_BYTES)
     765          56 :         + ((unsigned long long int)localAcquisitionTime[BYTE_1] << SHIFT_4_BYTES)
     766          56 :         + ((unsigned long long int)localAcquisitionTime[BYTE_2] << SHIFT_3_BYTES)
     767          56 :         + ((unsigned long long int)localAcquisitionTime[BYTE_3] << SHIFT_2_BYTES)
     768          56 :         + ((unsigned long long int)localAcquisitionTime[BYTE_4] << SHIFT_1_BYTE)
     769          56 :         + ((unsigned long long int)localAcquisitionTime[BYTE_5]);
     770             : 
     771          56 :     switch (sid)
     772             :     {
     773             :         case SID_NORM_SWF_F0:
     774           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_304 * T0_IN_FINETIME;
     775           0 :             break;
     776             : 
     777             :         case SID_NORM_SWF_F1:
     778           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_304 * T1_IN_FINETIME;
     779           0 :             break;
     780             : 
     781             :         case SID_NORM_SWF_F2:
     782           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_304 * T2_IN_FINETIME;
     783           0 :             break;
     784             : 
     785             :         case SID_SBM1_CWF_F1:
     786          56 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T1_IN_FINETIME;
     787          56 :             break;
     788             : 
     789             :         case SID_SBM2_CWF_F2:
     790           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T2_IN_FINETIME;
     791           0 :             break;
     792             : 
     793             :         case SID_BURST_CWF_F2:
     794           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T2_IN_FINETIME;
     795           0 :             break;
     796             : 
     797             :         case SID_NORM_CWF_F3:
     798           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF_SHORT_F3 * T3_IN_FINETIME;
     799           0 :             break;
     800             : 
     801             :         case SID_NORM_CWF_LONG_F3:
     802           0 :             deltaT = ((double)(pa_lfr_pkt_nr)) * BLK_NR_CWF * T3_IN_FINETIME;
     803           0 :             break;
     804             : 
     805             :         default:
     806             :             LFR_PRINTF("in compute_acquisition_time *** ERR unexpected sid %d\n", sid);
     807           0 :             deltaT = 0.;
     808             :             break;
     809             :     }
     810             : 
     811          56 :     acquisitionTimeAsLong = acquisitionTimeAsLong + (unsigned long long int)deltaT;
     812             :     //
     813          56 :     acquisitionTime[BYTE_0] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_5_BYTES);
     814          56 :     acquisitionTime[BYTE_1] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_4_BYTES);
     815          56 :     acquisitionTime[BYTE_2] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_3_BYTES);
     816          56 :     acquisitionTime[BYTE_3] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_2_BYTES);
     817          56 :     acquisitionTime[BYTE_4] = (unsigned char)(acquisitionTimeAsLong >> SHIFT_1_BYTE);
     818          56 :     acquisitionTime[BYTE_5] = (unsigned char)(acquisitionTimeAsLong);
     819          56 : }
     820             : 
     821           0 : void build_snapshot_from_ring(ring_node* ring_node_to_send, unsigned char frequencyChannel,
     822             :     unsigned long long int acquisitionTimeF0_asLong, ring_node* ring_node_swf_extracted,
     823             :     int* swf_extracted)
     824             : {
     825             :     unsigned int i;
     826             :     unsigned int node;
     827             :     unsigned long long int centerTime_asLong;
     828             :     unsigned long long int acquisitionTime_asLong;
     829             :     unsigned long long int bufferAcquisitionTime_asLong;
     830             :     const unsigned char* ptr1;
     831             :     unsigned char* ptr2;
     832             :     const unsigned char* timeCharPtr;
     833             :     unsigned char nb_ring_nodes;
     834             :     unsigned long long int frequency_asLong;
     835             :     // set to default value (Don_Initialisation_P2)
     836           0 :     unsigned long long int nbTicksPerSample_asLong = TICKS_PER_T2;
     837             :     long long int nbSamplesPart1_asLong;
     838           0 :     unsigned long long int sampleOffset_asLong = 0;
     839             : 
     840           0 :     unsigned int deltaT_F0 = DELTAT_F0;
     841           0 :     unsigned int deltaT_F1 = DELTAT_F1;
     842           0 :     unsigned long long int deltaT_F2 = DELTAT_F2;
     843             : 
     844             :     // (1) get the f0 acquisition time => the value is passed in argument
     845             : 
     846             :     // (2) compute the central reference time
     847           0 :     centerTime_asLong = acquisitionTimeF0_asLong + deltaT_F0;
     848           0 :     acquisitionTime_asLong = centerTime_asLong; // set to default value (Don_Initialisation_P2)
     849           0 :     bufferAcquisitionTime_asLong = centerTime_asLong; // set to default value
     850             :                                                       // (Don_Initialisation_P2)
     851             : 
     852             :     // (3) compute the acquisition time of the current snapshot
     853           0 :     switch (frequencyChannel)
     854             :     {
     855             :         case CHANNELF1: // 1 is for F1 = 4096 Hz
     856           0 :             acquisitionTime_asLong = centerTime_asLong - deltaT_F1;
     857           0 :             nb_ring_nodes = NB_RING_NODES_F1;
     858           0 :             frequency_asLong = FREQ_F1;
     859           0 :             nbTicksPerSample_asLong = TICKS_PER_T1; // 65536 / 4096;
     860           0 :             break;
     861             :         case CHANNELF2: // 2 is for F2 = 256 Hz
     862           0 :             acquisitionTime_asLong = centerTime_asLong - deltaT_F2;
     863           0 :             nb_ring_nodes = NB_RING_NODES_F2;
     864           0 :             frequency_asLong = FREQ_F2;
     865           0 :             nbTicksPerSample_asLong = TICKS_PER_T2; // 65536 / 256;
     866           0 :             break;
     867             :         default:
     868           0 :             acquisitionTime_asLong = centerTime_asLong;
     869           0 :             nb_ring_nodes = 0;
     870           0 :             frequency_asLong = FREQ_F2;
     871           0 :             nbTicksPerSample_asLong = TICKS_PER_T2;
     872             :             break;
     873             :     }
     874             : 
     875             :     //*****************************************************************************
     876             :     // (4) search the ring_node with the acquisition time <= acquisitionTime_asLong
     877           0 :     node = 0;
     878           0 :     while (node < nb_ring_nodes)
     879             :     {
     880           0 :         bufferAcquisitionTime_asLong
     881           0 :             = get_acquisition_time((unsigned char*)&ring_node_to_send->coarseTime);
     882           0 :         if (bufferAcquisitionTime_asLong <= acquisitionTime_asLong)
     883             :         {
     884           0 :             node = nb_ring_nodes;
     885             :         }
     886             :         else
     887             :         {
     888           0 :             node = node + 1;
     889           0 :             ring_node_to_send = ring_node_to_send->previous;
     890             :         }
     891             :     }
     892             : 
     893             :     // (5) compute the number of samples to take in the current buffer
     894           0 :     sampleOffset_asLong
     895           0 :         = ((acquisitionTime_asLong - bufferAcquisitionTime_asLong) * frequency_asLong)
     896             :         >> SHIFT_2_BYTES;
     897           0 :     nbSamplesPart1_asLong = NB_SAMPLES_PER_SNAPSHOT - sampleOffset_asLong;
     898             : 
     899             :     // (6) compute the final acquisition time
     900           0 :     acquisitionTime_asLong
     901           0 :         = bufferAcquisitionTime_asLong + (sampleOffset_asLong * nbTicksPerSample_asLong);
     902             : 
     903             :     // (7) copy the acquisition time at the beginning of the extrated snapshot
     904           0 :     ptr1 = (unsigned char*)&acquisitionTime_asLong;
     905             :     // fine time
     906           0 :     ptr2 = (unsigned char*)&ring_node_swf_extracted->fineTime;
     907           0 :     ptr2[BYTE_2] = ptr1[BYTE_4 + OFFSET_2_BYTES];
     908           0 :     ptr2[BYTE_3] = ptr1[BYTE_5 + OFFSET_2_BYTES];
     909             :     // coarse time
     910           0 :     ptr2 = (unsigned char*)&ring_node_swf_extracted->coarseTime;
     911           0 :     ptr2[BYTE_0] = ptr1[BYTE_0 + OFFSET_2_BYTES];
     912           0 :     ptr2[BYTE_1] = ptr1[BYTE_1 + OFFSET_2_BYTES];
     913           0 :     ptr2[BYTE_2] = ptr1[BYTE_2 + OFFSET_2_BYTES];
     914           0 :     ptr2[BYTE_3] = ptr1[BYTE_3 + OFFSET_2_BYTES];
     915             : 
     916             :     // re set the synchronization bit
     917           0 :     timeCharPtr = (unsigned char*)&ring_node_to_send->coarseTime;
     918           0 :     ptr2[0] = ptr2[0] | (timeCharPtr[0] & SYNC_BIT); // [1000 0000]
     919             : 
     920           0 :     if ((nbSamplesPart1_asLong > NB_SAMPLES_PER_SNAPSHOT) | (nbSamplesPart1_asLong < 0))
     921             :     {
     922           0 :         nbSamplesPart1_asLong = 0;
     923             :     }
     924             :     // copy the part 1 of the snapshot in the extracted buffer
     925           0 :     for (i = 0; i < (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK); i++)
     926             :     {
     927           0 :         swf_extracted[i] = ((
     928           0 :             int*)ring_node_to_send->buffer_address)[i + (sampleOffset_asLong * NB_WORDS_SWF_BLK)];
     929             :     }
     930             :     // copy the part 2 of the snapshot in the extracted buffer
     931           0 :     ring_node_to_send = ring_node_to_send->next;
     932           0 :     for (i = (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK);
     933           0 :          i < (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK); i++)
     934             :     {
     935           0 :         swf_extracted[i] = ((
     936           0 :             int*)ring_node_to_send->buffer_address)[i - (nbSamplesPart1_asLong * NB_WORDS_SWF_BLK)];
     937             :     }
     938           0 : }
     939             : 
     940           0 : double computeCorrection(const unsigned char* const timePtr)
     941             : {
     942             :     unsigned long long int acquisitionTime;
     943             :     unsigned long long int centerTime;
     944             :     unsigned long long int previousTick;
     945             :     unsigned long long int nextTick;
     946             :     unsigned long long int deltaPreviousTick;
     947             :     unsigned long long int deltaNextTick;
     948             :     double deltaPrevious_ms;
     949             :     double deltaNext_ms;
     950           0 :     double correctionInF2 = 0.; // set to default value (Don_Initialisation_P2)
     951             : 
     952             : 
     953             :     // get acquisition time in fine time ticks
     954           0 :     acquisitionTime = get_acquisition_time(timePtr);
     955             : 
     956             :     // compute center time
     957           0 :     centerTime = acquisitionTime + DELTAT_F0; // (2048. / 24576. / 2.) * 65536. = 2730.667;
     958           0 :     previousTick = centerTime - (centerTime & INT16_ALL_F);
     959           0 :     nextTick = previousTick + (unsigned long long)TICKS_PER_S;
     960             : 
     961           0 :     deltaPreviousTick = centerTime - previousTick;
     962           0 :     deltaNextTick = nextTick - centerTime;
     963             : 
     964           0 :     deltaPrevious_ms = (((double)deltaPreviousTick) / TICKS_PER_S) * MS_PER_S;
     965           0 :     deltaNext_ms = (((double)deltaNextTick) / TICKS_PER_S) * MS_PER_S;
     966             : 
     967             :     LFR_PRINTF(
     968             :         "    delta previous = %.3f ms, delta next = %.2f ms\n", deltaPrevious_ms, deltaNext_ms);
     969             : 
     970             :     // which tick is the closest?
     971           0 :     if (deltaPreviousTick > deltaNextTick)
     972             :     {
     973             :         // the snapshot center is just before the second => increase delta_snapshot
     974           0 :         correctionInF2 = +(deltaNext_ms * FREQ_F2 / MS_PER_S);
     975             :     }
     976             :     else
     977             :     {
     978             :         // the snapshot center is just after the second => decrease delta_snapshot
     979           0 :         correctionInF2 = -(deltaPrevious_ms * FREQ_F2 / MS_PER_S);
     980             :     }
     981             : 
     982             :     LFR_PRINTF("    correctionInF2 = %.2f\n", correctionInF2);
     983             : 
     984           0 :     return correctionInF2;
     985             : }
     986             : 
     987           0 : void applyCorrection(double correction)
     988             : {
     989           0 :     int correctionInt = 0;
     990             : 
     991           0 :     if (correction >= 0.)
     992             :     {
     993           0 :         if ((ONE_TICK_CORR_INTERVAL_0_MIN < correction)
     994           0 :             && (correction < ONE_TICK_CORR_INTERVAL_0_MAX))
     995             :         {
     996           0 :             correctionInt = ONE_TICK_CORR;
     997             :         }
     998             :         else
     999             :         {
    1000           0 :             correctionInt = (int)((double)CORR_MULT * floor(correction));
    1001             :         }
    1002             :     }
    1003             :     else
    1004             :     {
    1005           0 :         if ((ONE_TICK_CORR_INTERVAL_1_MIN < correction)
    1006           0 :             && (correction < ONE_TICK_CORR_INTERVAL_1_MAX))
    1007             :         {
    1008           0 :             correctionInt = -ONE_TICK_CORR;
    1009             :         }
    1010             :         else
    1011             :         {
    1012           0 :             correctionInt = (int)((double)CORR_MULT * ceil(correction));
    1013             :         }
    1014             :     }
    1015           0 :     waveform_picker_regs->delta_snapshot = waveform_picker_regs->delta_snapshot + correctionInt;
    1016           0 : }
    1017             : 
    1018           0 : void snapshot_resynchronization(const unsigned char* const timePtr)
    1019             : {
    1020             :     /** This function compute a correction to apply on delta_snapshot.
    1021             :      *
    1022             :      *
    1023             :      * @param timePtr is a pointer to the acquisition time of the snapshot being considered.
    1024             :      *
    1025             :      * @return void
    1026             :      *
    1027             :      */
    1028             : 
    1029             :     static double correction = 0.;
    1030             :     static resynchro_state state = MEASURE;
    1031             :     static unsigned int nbSnapshots = 0;
    1032             : 
    1033           0 :     switch (state)
    1034             :     {
    1035             : 
    1036             :         case MEASURE:
    1037             :             // ********
    1038             :             LFR_PRINTF("MEASURE === %d\n", nbSnapshots);
    1039           0 :             state = CORRECTION;
    1040           0 :             correction = computeCorrection(timePtr);
    1041             :             LFR_PRINTF("MEASURE === correction = %.2f\n", correction);
    1042           0 :             applyCorrection(correction);
    1043             :             LFR_PRINTF("MEASURE === delta_snapshot = %u\n",
    1044             :                 (unsigned int)waveform_picker_regs->delta_snapshot);
    1045             :             //****
    1046           0 :             break;
    1047             : 
    1048             :         case CORRECTION:
    1049             :             //************
    1050             :             LFR_PRINTF("CORRECTION === %u\n", nbSnapshots);
    1051           0 :             state = MEASURE;
    1052           0 :             computeCorrection(timePtr);
    1053             :             set_wfp_delta_snapshot();
    1054             :             LFR_PRINTF("CORRECTION === delta_snapshot = %u\n",
    1055             :                 (unsigned int)waveform_picker_regs->delta_snapshot);
    1056             :             //****
    1057             :             break;
    1058             : 
    1059             :         default:
    1060             :             break;
    1061             :     }
    1062             : 
    1063           0 :     nbSnapshots++;
    1064           0 : }
    1065             : 
    1066             : //**************
    1067             : // wfp registers
    1068           3 : void reset_wfp_burst_enable(void)
    1069             : {
    1070             :     /** This function resets the waveform picker burst_enable register.
    1071             :      *
    1072             :      * The burst bits [f2 f1 f0] and the enable bits [f3 f2 f1 f0] are set to 0.
    1073             :      *
    1074             :      */
    1075             : 
    1076             :     // [1000 000] burst f2, f1, f0     enable f3, f2, f1, f0
    1077          14 :     waveform_picker_regs->run_burst_enable
    1078           7 :         = waveform_picker_regs->run_burst_enable & RST_BITS_RUN_BURST_EN;
    1079           3 : }
    1080             : 
    1081           3 : void reset_wfp_status(void)
    1082             : {
    1083             :     /** This function resets the waveform picker status register.
    1084             :      *
    1085             :      * All status bits are set to 0 [new_err full_err full].
    1086             :      *
    1087             :      */
    1088             : 
    1089           7 :     waveform_picker_regs->status = INT16_ALL_F;
    1090           3 : }
    1091             : 
    1092           0 : void reset_wfp_buffer_addresses(void)
    1093             : {
    1094             :     // F0
    1095           4 :     waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->previous->buffer_address; // 0x08
    1096           4 :     waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; // 0x0c
    1097             :     // F1
    1098           4 :     waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->previous->buffer_address; // 0x10
    1099           4 :     waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; // 0x14
    1100             :     // F2
    1101           4 :     waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->previous->buffer_address; // 0x18
    1102           4 :     waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; // 0x1c
    1103             :     // F3
    1104           4 :     waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->previous->buffer_address; // 0x20
    1105           4 :     waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; // 0x24
    1106           0 : }
    1107             : 
    1108           4 : void reset_waveform_picker_regs(void)
    1109             : {
    1110             :     /** This function resets the waveform picker module registers.
    1111             :      *
    1112             :      * The registers affected by this function are located at the following offset addresses:
    1113             :      * - 0x00 data_shaping
    1114             :      * - 0x04 run_burst_enable
    1115             :      * - 0x08 addr_data_f0
    1116             :      * - 0x0C addr_data_f1
    1117             :      * - 0x10 addr_data_f2
    1118             :      * - 0x14 addr_data_f3
    1119             :      * - 0x18 status
    1120             :      * - 0x1C delta_snapshot
    1121             :      * - 0x20 delta_f0
    1122             :      * - 0x24 delta_f0_2
    1123             :      * - 0x28 delta_f1 (obsolet parameter)
    1124             :      * - 0x2c delta_f2
    1125             :      * - 0x30 nb_data_by_buffer
    1126             :      * - 0x34 nb_snapshot_param
    1127             :      * - 0x38 start_date
    1128             :      * - 0x3c nb_word_in_buffer
    1129             :      *
    1130             :      */
    1131             : 
    1132           4 :     set_wfp_data_shaping(); // 0x00 *** R1 R0 SP1 SP0 BW
    1133             : 
    1134             :     reset_wfp_burst_enable(); // 0x04 *** [run *** burst f2, f1, f0 *** enable f3, f2, f1, f0 ]
    1135             : 
    1136             :     reset_wfp_buffer_addresses();
    1137             : 
    1138             :     reset_wfp_status(); // 0x18
    1139             : 
    1140             :     set_wfp_delta_snapshot(); // 0x1c *** 300 s => 0x12bff
    1141             : 
    1142           4 :     set_wfp_delta_f0_f0_2(); // 0x20, 0x24
    1143             : 
    1144             :     // the parameter delta_f1 [0x28] is not used anymore
    1145             : 
    1146           4 :     set_wfp_delta_f2(); // 0x2c
    1147             : 
    1148             :     DEBUG_PRINTF("delta_snapshot %x\n", waveform_picker_regs->delta_snapshot);
    1149             :     DEBUG_PRINTF("delta_f0 %x\n", waveform_picker_regs->delta_f0);
    1150             :     DEBUG_PRINTF("delta_f0_2 %x\n", waveform_picker_regs->delta_f0_2);
    1151             :     DEBUG_PRINTF("delta_f1 %x\n", waveform_picker_regs->delta_f1);
    1152             :     DEBUG_PRINTF("delta_f2 %x\n", waveform_picker_regs->delta_f2);
    1153             :     // 2688 = 8 * 336
    1154           4 :     waveform_picker_regs->nb_data_by_buffer
    1155             :         = DFLT_WFP_NB_DATA_BY_BUFFER; // 0x30 *** 2688 - 1 => nb samples -1
    1156           4 :     waveform_picker_regs->snapshot_param = DFLT_WFP_SNAPSHOT_PARAM; // 0x34 *** 2688 => nb samples
    1157           4 :     waveform_picker_regs->start_date = COARSE_TIME_MASK;
    1158             :     //
    1159             :     // coarse time and fine time registers are not initialized, they are volatile
    1160             :     //
    1161           4 :     waveform_picker_regs->buffer_length
    1162             :         = DFLT_WFP_BUFFER_LENGTH; // buffer length in burst = 3 * 2688 / 16 = 504 = 0x1f8
    1163           4 : }
    1164             : 
    1165           4 : void set_wfp_data_shaping(void)
    1166             : {
    1167             :     /** This function sets the data_shaping register of the waveform picker module.
    1168             :      *
    1169             :      * The value is read from one field of the parameter_dump_packet structure:\n
    1170             :      * bw_sp0_sp1_r0_r1
    1171             :      *
    1172             :      */
    1173             : 
    1174             :     unsigned char data_shaping;
    1175             : 
    1176             :     // get the parameters for the data shaping [BW SP0 SP1 R0 R1] in sy_lfr_common1 and configure
    1177             :     // the register waveform picker : [R1 R0 SP1 SP0 BW]
    1178             : 
    1179           4 :     data_shaping = parameter_dump_packet.sy_lfr_common_parameters;
    1180             : 
    1181           8 :     waveform_picker_regs->data_shaping = ((data_shaping & BIT_5) >> SHIFT_5_BITS) // BW
    1182           4 :         + ((data_shaping & BIT_4) >> SHIFT_3_BITS) // SP0
    1183           4 :         + ((data_shaping & BIT_3) >> 1) // SP1
    1184           4 :         + ((data_shaping & BIT_2) << 1) // R0
    1185           4 :         + ((data_shaping & BIT_1) << SHIFT_3_BITS) // R1
    1186           4 :         + ((data_shaping & BIT_0) << SHIFT_5_BITS); // R2
    1187           4 : }
    1188             : 
    1189           3 : void set_wfp_burst_enable_register(unsigned char mode)
    1190             : {
    1191             :     /** This function sets the waveform picker burst_enable register depending on the mode.
    1192             :      *
    1193             :      * @param mode is the LFR mode to launch.
    1194             :      *
    1195             :      * The burst bits shall be before the enable bits.
    1196             :      *
    1197             :      */
    1198             : 
    1199             :     // [0000 0000] burst f2, f1, f0 enable f3 f2 f1 f0
    1200             :     // the burst bits shall be set first, before the enable bits
    1201           3 :     switch (mode)
    1202             :     {
    1203             :         case LFR_MODE_NORMAL:
    1204             :         case LFR_MODE_SBM1:
    1205             :         case LFR_MODE_SBM2:
    1206           2 :             waveform_picker_regs->run_burst_enable
    1207             :                 = RUN_BURST_ENABLE_SBM2; // [0110 0000] enable f2 and f1 burst
    1208           2 :             waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable
    1209             :                 | BITS_WFP_ENABLE_ALL; // [1111] enable f3 f2 f1 f0
    1210           2 :             break;
    1211             :         case LFR_MODE_BURST:
    1212           1 :             waveform_picker_regs->run_burst_enable
    1213             :                 = RUN_BURST_ENABLE_BURST; // [0100 0000] f2 burst enabled
    1214           1 :             waveform_picker_regs->run_burst_enable = waveform_picker_regs->run_burst_enable
    1215             :                 | BITS_WFP_ENABLE_BURST; // [1100] enable f3 and f2
    1216           1 :             break;
    1217             :         default:
    1218           0 :             waveform_picker_regs->run_burst_enable
    1219             :                 = INIT_CHAR; // [0000 0000] no burst enabled, no waveform enabled
    1220             :             break;
    1221             :     }
    1222           3 : }
    1223             : 
    1224           0 : void set_wfp_delta_snapshot(void)
    1225             : {
    1226             :     /** This function sets the delta_snapshot register of the waveform picker module.
    1227             :      *
    1228             :      * The value is read from two (unsigned char) of the parameter_dump_packet structure:
    1229             :      * - sy_lfr_n_swf_p[0]
    1230             :      * - sy_lfr_n_swf_p[1]
    1231             :      *
    1232             :      */
    1233             : 
    1234             :     unsigned int delta_snapshot;
    1235             :     unsigned int delta_snapshot_in_T2;
    1236             : 
    1237           8 :     delta_snapshot = (parameter_dump_packet.sy_lfr_n_swf_p[0] * CONST_256)
    1238           4 :         + parameter_dump_packet.sy_lfr_n_swf_p[1];
    1239             : 
    1240           4 :     delta_snapshot_in_T2 = delta_snapshot * (unsigned int)FREQ_F2;
    1241           4 :     waveform_picker_regs->delta_snapshot = delta_snapshot_in_T2 - 1; // max 4 bytes
    1242           0 : }
    1243             : 
    1244           4 : void set_wfp_delta_f0_f0_2(void)
    1245             : {
    1246             :     unsigned int delta_snapshot;
    1247             :     unsigned int nb_samples_per_snapshot;
    1248             :     float delta_f0_in_float;
    1249             : 
    1250           4 :     delta_snapshot = waveform_picker_regs->delta_snapshot;
    1251           8 :     nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256)
    1252           4 :         + parameter_dump_packet.sy_lfr_n_swf_l[1];
    1253           8 :     delta_f0_in_float = (float)(((double)nb_samples_per_snapshot / 2.)
    1254           4 :         * ((1. / FREQ_F2) - (1. / FREQ_F0)) * FREQ_F2);
    1255             : 
    1256           4 :     waveform_picker_regs->delta_f0 = delta_snapshot - (int)floor(delta_f0_in_float);
    1257           4 :     waveform_picker_regs->delta_f0_2 = DFLT_WFP_DELTA_F0_2;
    1258           4 : }
    1259             : 
    1260           0 : void set_wfp_delta_f1(void)
    1261             : {
    1262             :     /** Sets the value of the delta_f1 parameter
    1263             :      *
    1264             :      * @param void
    1265             :      *
    1266             :      * @return void
    1267             :      *
    1268             :      * delta_f1 is not used, the snapshots are extracted from CWF_F1 waveforms.
    1269             :      *
    1270             :      */
    1271             : 
    1272             :     unsigned int delta_snapshot;
    1273             :     unsigned int nb_samples_per_snapshot;
    1274             :     float delta_f1_in_float;
    1275             : 
    1276           0 :     delta_snapshot = waveform_picker_regs->delta_snapshot;
    1277           0 :     nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256)
    1278           0 :         + parameter_dump_packet.sy_lfr_n_swf_l[1];
    1279           0 :     delta_f1_in_float = (float)(((double)nb_samples_per_snapshot / 2.)
    1280           0 :         * ((1. / FREQ_F2) - (1. / FREQ_F1)) * FREQ_F2);
    1281             : 
    1282           0 :     waveform_picker_regs->delta_f1 = delta_snapshot - (int)floor(delta_f1_in_float);
    1283           0 : }
    1284             : 
    1285           4 : void set_wfp_delta_f2(void) // parameter not used, only delta_f0 and delta_f0_2 are used
    1286             : {
    1287             :     /** Sets the value of the delta_f2 parameter
    1288             :      *
    1289             :      * @param void
    1290             :      *
    1291             :      * @return void
    1292             :      *
    1293             :      * delta_f2 is used only for the first snapshot generation, even when the snapshots are
    1294             :      * extracted from CWF_F2 waveforms (see lpp_waveform_snapshot_controler.vhd for details).
    1295             :      *
    1296             :      */
    1297             : 
    1298             :     unsigned int delta_snapshot;
    1299             :     unsigned int nb_samples_per_snapshot;
    1300             : 
    1301           4 :     delta_snapshot = waveform_picker_regs->delta_snapshot;
    1302           8 :     nb_samples_per_snapshot = (parameter_dump_packet.sy_lfr_n_swf_l[0] * CONST_256)
    1303           4 :         + parameter_dump_packet.sy_lfr_n_swf_l[1];
    1304             : 
    1305           4 :     waveform_picker_regs->delta_f2 = delta_snapshot - (nb_samples_per_snapshot / 2) - 1;
    1306           4 : }
    1307             : 
    1308             : //*****************
    1309             : // local parameters
    1310             : 
    1311         104 : void increment_seq_counter_source_id(unsigned char* packet_sequence_control, unsigned char sid)
    1312             : {
    1313             :     /** This function increments the parameter "sequence_cnt" depending on the sid passed in
    1314             :      * argument.
    1315             :      *
    1316             :      * @param packet_sequence_control is a pointer toward the parameter sequence_cnt to update.
    1317             :      * @param sid is the source identifier of the packet being updated.
    1318             :      *
    1319             :      * REQ-LFR-SRS-5240 / SSS-CP-FS-590
    1320             :      * The sequence counters shall wrap around from 2^14 to zero.
    1321             :      * The sequence counter shall start at zero at startup.
    1322             :      *
    1323             :      * REQ-LFR-SRS-5239 / SSS-CP-FS-580
    1324             :      * All TM_LFR_SCIENCE_ packets are sent to ground, i.e. destination id = 0
    1325             :      *
    1326             :      */
    1327             : 
    1328         104 :     unsigned short* sequence_cnt = NULL;
    1329             :     unsigned short segmentation_grouping_flag;
    1330             :     unsigned short new_packet_sequence_control;
    1331         104 :     rtems_mode initial_mode_set = RTEMS_DEFAULT_MODES;
    1332         104 :     rtems_mode current_mode_set = RTEMS_DEFAULT_MODES;
    1333             : 
    1334             :     //******************************************
    1335             :     // CHANGE THE MODE OF THE CALLING RTEMS TASK
    1336         104 :     DEBUG_CHECK_STATUS(rtems_task_mode(RTEMS_NO_PREEMPT, RTEMS_PREEMPT_MASK, &initial_mode_set));
    1337             : 
    1338         814 :     if ((sid == SID_NORM_SWF_F0) || (sid == SID_NORM_SWF_F1) || (sid == SID_NORM_SWF_F2)
    1339         208 :         || (sid == SID_NORM_CWF_F3) || (sid == SID_NORM_CWF_LONG_F3) || (sid == SID_BURST_CWF_F2)
    1340         208 :         || (sid == SID_NORM_ASM_F0) || (sid == SID_NORM_ASM_F1) || (sid == SID_NORM_ASM_F2)
    1341         205 :         || (sid == SID_NORM_BP1_F0) || (sid == SID_NORM_BP1_F1) || (sid == SID_NORM_BP1_F2)
    1342         190 :         || (sid == SID_NORM_BP2_F0) || (sid == SID_NORM_BP2_F1) || (sid == SID_NORM_BP2_F2)
    1343         187 :         || (sid == SID_BURST_BP1_F0) || (sid == SID_BURST_BP2_F0) || (sid == SID_BURST_BP1_F1)
    1344             :         || (sid == SID_BURST_BP2_F1))
    1345             :     {
    1346          15 :         sequence_cnt = &sequenceCounters_SCIENCE_NORMAL_BURST;
    1347             :     }
    1348         284 :     else if ((sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) || (sid == SID_SBM1_BP1_F0)
    1349         106 :         || (sid == SID_SBM1_BP2_F0) || (sid == SID_SBM2_BP1_F0) || (sid == SID_SBM2_BP2_F0)
    1350           4 :         || (sid == SID_SBM2_BP1_F1) || (sid == SID_SBM2_BP2_F1))
    1351             :     {
    1352          89 :         sequence_cnt = &sequenceCounters_SCIENCE_SBM1_SBM2;
    1353             :     }
    1354             :     else
    1355             :     {
    1356           0 :         sequence_cnt = (unsigned short*)NULL;
    1357             :         LFR_PRINTF(
    1358             :             "in increment_seq_counter_source_id *** ERR apid_destid %d not known\n", (int)sid);
    1359             :     }
    1360             : 
    1361         104 :     if (sequence_cnt != NULL)
    1362             :     {
    1363         104 :         segmentation_grouping_flag = TM_PACKET_SEQ_CTRL_STANDALONE << SHIFT_1_BYTE;
    1364         104 :         *sequence_cnt = (*sequence_cnt) & SEQ_CNT_MASK;
    1365             : 
    1366         104 :         new_packet_sequence_control = segmentation_grouping_flag | (*sequence_cnt);
    1367             : 
    1368         104 :         packet_sequence_control[0] = (unsigned char)(new_packet_sequence_control >> SHIFT_1_BYTE);
    1369         104 :         packet_sequence_control[1] = (unsigned char)(new_packet_sequence_control);
    1370             : 
    1371             :         // increment the sequence counter
    1372         104 :         if (*sequence_cnt < SEQ_CNT_MAX)
    1373             :         {
    1374         104 :             *sequence_cnt = *sequence_cnt + 1;
    1375             :         }
    1376             :         else
    1377             :         {
    1378           0 :             *sequence_cnt = 0;
    1379             :         }
    1380             :     }
    1381             : 
    1382             :     //*************************************
    1383             :     // RESTORE THE MODE OF THE CALLING TASK
    1384         104 :     DEBUG_CHECK_STATUS(rtems_task_mode(initial_mode_set, RTEMS_PREEMPT_MASK, &current_mode_set));
    1385         104 : }

Generated by: LCOV version 1.14