wf_handler.c
1343 lines
| 50.0 KiB
| text/x-c
|
CLexer
/ src / wf_handler.c
|
r40 | /** Functions and tasks related to waveform packet generation. | |
* | |||
* @file | |||
* @author P. LEROY | |||
* | |||
* A group of functions to handle waveforms, in snapshot or continuous format.\n | |||
* | |||
*/ | |||
#include "wf_handler.h" | |||
|
r5 | ||
|
r171 | //*************** | |
// waveform rings | |||
// F0 | |||
|
r92 | ring_node waveform_ring_f0[NB_RING_NODES_F0]; | |
ring_node *current_ring_node_f0; | |||
ring_node *ring_node_to_send_swf_f0; | |||
|
r171 | // F1 | |
ring_node waveform_ring_f1[NB_RING_NODES_F1]; | |||
|
r87 | ring_node *current_ring_node_f1; | |
ring_node *ring_node_to_send_swf_f1; | |||
ring_node *ring_node_to_send_cwf_f1; | |||
|
r171 | // F2 | |
ring_node waveform_ring_f2[NB_RING_NODES_F2]; | |||
|
r87 | ring_node *current_ring_node_f2; | |
ring_node *ring_node_to_send_swf_f2; | |||
ring_node *ring_node_to_send_cwf_f2; | |||
|
r171 | // F3 | |
ring_node waveform_ring_f3[NB_RING_NODES_F3]; | |||
|
r131 | ring_node *current_ring_node_f3; | |
ring_node *ring_node_to_send_cwf_f3; | |||
|
r181 | char wf_cont_f3_light[ (NB_SAMPLES_PER_SNAPSHOT) * NB_BYTES_CWF3_LIGHT_BLK ]; | |
|
r87 | ||
|
r238 | bool extractSWF1 = false; | |
bool extractSWF2 = false; | |||
bool swf0_ready_flag_f1 = false; | |||
bool swf0_ready_flag_f2 = false; | |||
bool swf1_ready = false; | |||
bool swf2_ready = false; | |||
|
r105 | ||
|
r238 | int swf1_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ]; | |
int swf2_extracted[ (NB_SAMPLES_PER_SNAPSHOT * NB_WORDS_SWF_BLK) ]; | |||
ring_node ring_node_swf1_extracted; | |||
ring_node ring_node_swf2_extracted; | |||
|
r106 | ||
|
r256 | typedef enum resynchro_state_t | |
{ | |||
|
r267 | MEASURE, | |
CORRECTION | |||
|
r256 | } resynchro_state; | |
|
r106 | //********************* | |
// Interrupt SubRoutine | |||
|
r179 | ring_node * getRingNodeToSendCWF( unsigned char frequencyChannel) | |
{ | |||
ring_node *node; | |||
node = NULL; | |||
switch ( frequencyChannel ) { | |||
|
r318 | case CHANNELF1: | |
|
r179 | node = ring_node_to_send_cwf_f1; | |
break; | |||
|
r318 | case CHANNELF2: | |
|
r179 | node = ring_node_to_send_cwf_f2; | |
break; | |||
|
r318 | case CHANNELF3: | |
|
r179 | node = ring_node_to_send_cwf_f3; | |
break; | |||
default: | |||
break; | |||
} | |||
return node; | |||
} | |||
ring_node * getRingNodeToSendSWF( unsigned char frequencyChannel) | |||
{ | |||
ring_node *node; | |||
node = NULL; | |||
switch ( frequencyChannel ) { | |||
|
r318 | case CHANNELF0: | |
|
r179 | node = ring_node_to_send_swf_f0; | |
break; | |||
|
r318 | case CHANNELF1: | |
|
r179 | node = ring_node_to_send_swf_f1; | |
break; | |||
|
r318 | case CHANNELF2: | |
|
r179 | node = ring_node_to_send_swf_f2; | |
break; | |||
default: | |||
break; | |||
} | |||
return node; | |||
} | |||
|
r106 | void reset_extractSWF( void ) | |
{ | |||
|
r238 | extractSWF1 = false; | |
extractSWF2 = false; | |||
swf0_ready_flag_f1 = false; | |||
swf0_ready_flag_f2 = false; | |||
swf1_ready = false; | |||
swf2_ready = false; | |||
|
r106 | } | |
|
r105 | ||
|
r172 | inline void waveforms_isr_f3( void ) | |
|
r170 | { | |
rtems_status_code spare_status; | |||
if ( (lfrCurrentMode == LFR_MODE_NORMAL) || (lfrCurrentMode == LFR_MODE_BURST) // in BURST the data are used to place v, e1 and e2 in the HK packet | |||
|| (lfrCurrentMode == LFR_MODE_SBM1) || (lfrCurrentMode == LFR_MODE_SBM2) ) | |||
{ // in modes other than STANDBY and BURST, send the CWF_F3 data | |||
|
r171 | //*** | |
// F3 | |||
|
r318 | if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F3) != INIT_CHAR ) { // [1100 0000] check the f3 full bits | |
|
r171 | ring_node_to_send_cwf_f3 = current_ring_node_f3->previous; | |
current_ring_node_f3 = current_ring_node_f3->next; | |||
|
r318 | if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_0) == BIT_WFP_BUF_F3_0){ // [0100 0000] f3 buffer 0 is full | |
|
r171 | ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_0_coarse_time; | |
ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_0_fine_time; | |||
waveform_picker_regs->addr_data_f3_0 = current_ring_node_f3->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_0; // [1000 1000 0100 0000] | |
|
r171 | } | |
|
r318 | else if ((waveform_picker_regs->status & BIT_WFP_BUF_F3_1) == BIT_WFP_BUF_F3_1){ // [1000 0000] f3 buffer 1 is full | |
|
r171 | ring_node_to_send_cwf_f3->coarseTime = waveform_picker_regs->f3_1_coarse_time; | |
ring_node_to_send_cwf_f3->fineTime = waveform_picker_regs->f3_1_fine_time; | |||
waveform_picker_regs->addr_data_f3_1 = current_ring_node_f3->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F3_1; // [1000 1000 1000 0000] | |
|
r171 | } | |
|
r170 | if (rtems_event_send( Task_id[TASKID_CWF3], RTEMS_EVENT_0 ) != RTEMS_SUCCESSFUL) { | |
|
r171 | spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ); | |
|
r170 | } | |
} | |||
} | |||
} | |||
|
r172 | inline void waveforms_isr_burst( void ) | |
|
r170 | { | |
|
r179 | unsigned char status; | |
|
r170 | rtems_status_code spare_status; | |
|
r318 | status = (waveform_picker_regs->status & BITS_WFP_STATUS_F2) >> SHIFT_WFP_STATUS_F2; // [0011 0000] get the status bits for f2 | |
|
r179 | ||
switch(status) | |||
{ | |||
|
r318 | case BIT_WFP_BUFFER_0: | |
|
r185 | ring_node_to_send_cwf_f2 = current_ring_node_f2->previous; | |
ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2; | |||
|
r179 | ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time; | |
ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time; | |||
|
r185 | current_ring_node_f2 = current_ring_node_f2->next; | |
|
r179 | waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address; | |
|
r170 | if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) { | |
|
r171 | spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ); | |
|
r170 | } | |
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000] | |
|
r179 | break; | |
|
r318 | case BIT_WFP_BUFFER_1: | |
|
r185 | ring_node_to_send_cwf_f2 = current_ring_node_f2->previous; | |
ring_node_to_send_cwf_f2->sid = SID_BURST_CWF_F2; | |||
|
r179 | ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time; | |
ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time; | |||
|
r185 | current_ring_node_f2 = current_ring_node_f2->next; | |
|
r179 | waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; | |
if (rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_BURST ) != RTEMS_SUCCESSFUL) { | |||
spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_0 ); | |||
} | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000] | |
|
r179 | break; | |
default: | |||
break; | |||
|
r170 | } | |
} | |||
|
r238 | inline void waveform_isr_normal_sbm1_sbm2( void ) | |
|
r170 | { | |
rtems_status_code status; | |||
//*** | |||
|
r238 | // F0 | |
|
r318 | if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F0) != INIT_CHAR ) // [0000 0011] check the f0 full bits | |
|
r238 | { | |
swf0_ready_flag_f1 = true; | |||
swf0_ready_flag_f2 = true; | |||
ring_node_to_send_swf_f0 = current_ring_node_f0->previous; | |||
current_ring_node_f0 = current_ring_node_f0->next; | |||
|
r318 | if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_0) == BIT_WFP_BUFFER_0) | |
|
r238 | { | |
ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_0_coarse_time; | |||
ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_0_fine_time; | |||
waveform_picker_regs->addr_data_f0_0 = current_ring_node_f0->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_0; // [0001 0001 0000 0001] | |
|
r238 | } | |
|
r318 | else if ( (waveform_picker_regs->status & BIT_WFP_BUFFER_1) == BIT_WFP_BUFFER_1) | |
|
r238 | { | |
ring_node_to_send_swf_f0->coarseTime = waveform_picker_regs->f0_1_coarse_time; | |||
ring_node_to_send_swf_f0->fineTime = waveform_picker_regs->f0_1_fine_time; | |||
waveform_picker_regs->addr_data_f0_1 = current_ring_node_f0->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F0_1; // [0001 0001 0000 0010] | |
|
r238 | } | |
|
r267 | // send an event to the WFRM task for resynchro activities | |
status = rtems_event_send( Task_id[TASKID_WFRM], RTEMS_EVENT_SWF_RESYNCH ); | |||
|
r238 | } | |
//*** | |||
|
r170 | // F1 | |
|
r318 | if ( (waveform_picker_regs->status & 0x0c) != INIT_CHAR ) { // [0000 1100] check the f1 full bits | |
|
r170 | // (1) change the receiving buffer for the waveform picker | |
|
r171 | ring_node_to_send_cwf_f1 = current_ring_node_f1->previous; | |
current_ring_node_f1 = current_ring_node_f1->next; | |||
|
r318 | if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_0) == BIT_WFP_BUF_F1_0) | |
|
r171 | { | |
ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_0_coarse_time; | |||
ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_0_fine_time; | |||
waveform_picker_regs->addr_data_f1_0 = current_ring_node_f1->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_0; // [0010 0010 0000 0100] f1 bits = 0 | |
|
r171 | } | |
|
r318 | else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F1_1) == BIT_WFP_BUF_F1_1) | |
|
r171 | { | |
ring_node_to_send_cwf_f1->coarseTime = waveform_picker_regs->f1_1_coarse_time; | |||
ring_node_to_send_cwf_f1->fineTime = waveform_picker_regs->f1_1_fine_time; | |||
waveform_picker_regs->addr_data_f1_1 = current_ring_node_f1->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F1_1; // [0010 0010 0000 1000] f1 bits = 0 | |
|
r171 | } | |
|
r170 | // (2) send an event for the the CWF1 task for transmission (and snapshot extraction if needed) | |
|
r238 | status = rtems_event_send( Task_id[TASKID_CWF1], RTEMS_EVENT_MODE_NORM_S1_S2 ); | |
|
r170 | } | |
//*** | |||
// F2 | |||
|
r318 | if ( (waveform_picker_regs->status & BITS_WFP_STATUS_F2) != INIT_CHAR ) { // [0011 0000] check the f2 full bit | |
|
r170 | // (1) change the receiving buffer for the waveform picker | |
|
r181 | ring_node_to_send_cwf_f2 = current_ring_node_f2->previous; | |
ring_node_to_send_cwf_f2->sid = SID_SBM2_CWF_F2; | |||
current_ring_node_f2 = current_ring_node_f2->next; | |||
|
r318 | if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_0) == BIT_WFP_BUF_F2_0) | |
|
r171 | { | |
ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_0_coarse_time; | |||
ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_0_fine_time; | |||
waveform_picker_regs->addr_data_f2_0 = current_ring_node_f2->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_0; // [0100 0100 0001 0000] | |
|
r171 | } | |
|
r318 | else if ( (waveform_picker_regs->status & BIT_WFP_BUF_F2_1) == BIT_WFP_BUF_F2_1) | |
|
r171 | { | |
ring_node_to_send_cwf_f2->coarseTime = waveform_picker_regs->f2_1_coarse_time; | |||
ring_node_to_send_cwf_f2->fineTime = waveform_picker_regs->f2_1_fine_time; | |||
waveform_picker_regs->addr_data_f2_1 = current_ring_node_f2->buffer_address; | |||
|
r318 | waveform_picker_regs->status = waveform_picker_regs->status & RST_WFP_F2_1; // [0100 0100 0010 0000] | |
|
r171 | } | |
|
r170 | // (2) send an event for the waveforms transmission | |
|
r238 | status = rtems_event_send( Task_id[TASKID_CWF2], RTEMS_EVENT_MODE_NORM_S1_S2 ); | |
|
r170 | } | |
} | |||
|
r21 | rtems_isr waveforms_isr( rtems_vector_number vector ) | |
{ | |||
|
r45 | /** This is the interrupt sub routine called by the waveform picker core. | |
* | |||
* This ISR launch different actions depending mainly on two pieces of information: | |||
* 1. the values read in the registers of the waveform picker. | |||
* 2. the current LFR mode. | |||
* | |||
*/ | |||
|
r21 | ||
|
r170 | // STATUS | |
// new error error buffer full | |||
// 15 14 13 12 11 10 9 8 | |||
// f3 f2 f1 f0 f3 f2 f1 f0 | |||
// | |||
// ready buffer | |||
// 7 6 5 4 3 2 1 0 | |||
// f3_1 f3_0 f2_1 f2_0 f1_1 f1_0 f0_1 f0_0 | |||
|
r166 | rtems_status_code spare_status; | |
|
r95 | ||
|
r170 | waveforms_isr_f3(); | |
|
r265 | //************************************************* | |
// copy the status bits in the housekeeping packets | |||
housekeeping_packet.hk_lfr_vhdl_iir_cal = | |||
|
r318 | (unsigned char) ((waveform_picker_regs->status & BYTE0_MASK) >> SHIFT_1_BYTE); | |
|
r265 | ||
|
r318 | if ( (waveform_picker_regs->status & BYTE0_MASK) != INIT_CHAR) // [1111 1111 0000 0000] check the error bits | |
|
r170 | { | |
|
r171 | spare_status = rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_10 ); | |
|
r32 | } | |
|
r33 | switch(lfrCurrentMode) | |
|
r21 | { | |
|
r238 | //******** | |
// STANDBY | |||
case LFR_MODE_STANDBY: | |||
|
r33 | break; | |
|
r246 | //************************** | |
// LFR NORMAL, SBM1 and SBM2 | |||
|
r238 | case LFR_MODE_NORMAL: | |
case LFR_MODE_SBM1: | |||
case LFR_MODE_SBM2: | |||
waveform_isr_normal_sbm1_sbm2(); | |||
|
r95 | break; | |
|
r246 | //****** | |
// BURST | |||
|
r238 | case LFR_MODE_BURST: | |
|
r170 | waveforms_isr_burst(); | |
|
r95 | break; | |
|
r246 | //******** | |
// DEFAULT | |||
|
r238 | default: | |
|
r33 | break; | |
|
r21 | } | |
} | |||
|
r106 | //************ | |
// RTEMS TASKS | |||
|
r18 | rtems_task wfrm_task(rtems_task_argument argument) //used with the waveform picker VHDL IP | |
{ | |||
|
r45 | /** This RTEMS task is dedicated to the transmission of snapshots of the NORMAL mode. | |
* | |||
* @param unused is the starting argument of the RTEMS task | |||
* | |||
* The following data packets are sent by this task: | |||
* - TM_LFR_SCIENCE_NORMAL_SWF_F0 | |||
* - TM_LFR_SCIENCE_NORMAL_SWF_F1 | |||
* - TM_LFR_SCIENCE_NORMAL_SWF_F2 | |||
* | |||
*/ | |||
|
r5 | rtems_event_set event_out; | |
|
r35 | rtems_id queue_id; | |
|
r82 | rtems_status_code status; | |
|
r238 | ring_node *ring_node_swf1_extracted_ptr; | |
ring_node *ring_node_swf2_extracted_ptr; | |||
|
r172 | ||
|
r320 | event_out = EVENT_SETS_NONE_PENDING; | |
queue_id = RTEMS_ID_NONE; | |||
|
r238 | ring_node_swf1_extracted_ptr = (ring_node *) &ring_node_swf1_extracted; | |
ring_node_swf2_extracted_ptr = (ring_node *) &ring_node_swf2_extracted; | |||
|
r168 | ||
|
r82 | status = get_message_queue_id_send( &queue_id ); | |
if (status != RTEMS_SUCCESSFUL) | |||
{ | |||
|
r246 | PRINTF1("in WFRM *** ERR get_message_queue_id_send %d\n", status); | |
|
r82 | } | |
|
r35 | ||
|
r246 | BOOT_PRINTF("in WFRM ***\n"); | |
|
r18 | ||
|
r246 | while(1){ | |
|
r21 | // wait for an RTEMS_EVENT | |
|
r267 | rtems_event_receive(RTEMS_EVENT_MODE_NORMAL | RTEMS_EVENT_SWF_RESYNCH, | |
|
r21 | RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); | |
|
r247 | ||
|
r35 | if (event_out == RTEMS_EVENT_MODE_NORMAL) | |
{ | |||
|
r246 | DEBUG_PRINTF("WFRM received RTEMS_EVENT_MODE_SBM2\n"); | |
ring_node_to_send_swf_f0->sid = SID_NORM_SWF_F0; | |||
|
r238 | ring_node_swf1_extracted_ptr->sid = SID_NORM_SWF_F1; | |
ring_node_swf2_extracted_ptr->sid = SID_NORM_SWF_F2; | |||
status = rtems_message_queue_send( queue_id, &ring_node_to_send_swf_f0, sizeof( ring_node* ) ); | |||
status = rtems_message_queue_send( queue_id, &ring_node_swf1_extracted_ptr, sizeof( ring_node* ) ); | |||
status = rtems_message_queue_send( queue_id, &ring_node_swf2_extracted_ptr, sizeof( ring_node* ) ); | |||
|
r106 | } | |
|
r267 | if (event_out == RTEMS_EVENT_SWF_RESYNCH) | |
{ | |||
snapshot_resynchronization( (unsigned char *) &ring_node_to_send_swf_f0->coarseTime ); | |||
} | |||
|
r5 | } | |
} | |||
|
r17 | ||
|
r32 | rtems_task cwf3_task(rtems_task_argument argument) //used with the waveform picker VHDL IP | |
{ | |||
|
r45 | /** This RTEMS task is dedicated to the transmission of continuous waveforms at f3. | |
* | |||
* @param unused is the starting argument of the RTEMS task | |||
* | |||
* The following data packet is sent by this task: | |||
* - TM_LFR_SCIENCE_NORMAL_CWF_F3 | |||
* | |||
*/ | |||
|
r32 | rtems_event_set event_out; | |
|
r35 | rtems_id queue_id; | |
|
r82 | rtems_status_code status; | |
|
r172 | ring_node ring_node_cwf3_light; | |
|
r205 | ring_node *ring_node_to_send_cwf; | |
|
r32 | ||
|
r320 | event_out = EVENT_SETS_NONE_PENDING; | |
queue_id = RTEMS_ID_NONE; | |||
|
r82 | status = get_message_queue_id_send( &queue_id ); | |
if (status != RTEMS_SUCCESSFUL) | |||
{ | |||
PRINTF1("in CWF3 *** ERR get_message_queue_id_send %d\n", status) | |||
} | |||
|
r35 | ||
|
r172 | ring_node_to_send_cwf_f3->sid = SID_NORM_CWF_LONG_F3; | |
// init the ring_node_cwf3_light structure | |||
ring_node_cwf3_light.buffer_address = (int) wf_cont_f3_light; | |||
|
r318 | ring_node_cwf3_light.coarseTime = INIT_CHAR; | |
ring_node_cwf3_light.fineTime = INIT_CHAR; | |||
|
r172 | ring_node_cwf3_light.next = NULL; | |
ring_node_cwf3_light.previous = NULL; | |||
ring_node_cwf3_light.sid = SID_NORM_CWF_F3; | |||
|
r318 | ring_node_cwf3_light.status = INIT_CHAR; | |
|
r172 | ||
|
r292 | BOOT_PRINTF("in CWF3 ***\n"); | |
|