fsw_spacewire.c
1598 lines
| 59.7 KiB
| text/x-c
|
CLexer
/ src / fsw_spacewire.c
paul
|
r45 | /** Functions related to the SpaceWire interface. | ||
| * | ||||
| * @file | ||||
| * @author P. LEROY | ||||
| * | ||||
| * A group of functions to handle SpaceWire transmissions: | ||||
| * - configuration of the SpaceWire link | ||||
| * - SpaceWire related interruption requests processing | ||||
| * - transmission of TeleMetry packets by a dedicated RTEMS task | ||||
| * - reception of TeleCommands by a dedicated RTEMS task | ||||
| * | ||||
| */ | ||||
|
paul
|
r35 | #include "fsw_spacewire.h" | ||
|
paul
|
r56 | rtems_name semq_name; | ||
| rtems_id semq_id; | ||||
|
paul
|
r172 | //***************** | ||
| // waveform headers | ||||
| Header_TM_LFR_SCIENCE_CWF_t headerCWF; | ||||
| Header_TM_LFR_SCIENCE_SWF_t headerSWF; | ||||
| Header_TM_LFR_SCIENCE_ASM_t headerASM; | ||||
|
paul
|
r248 | unsigned char previousTimecodeCtr = 0; | ||
| unsigned int *grspwPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_TIME_REGISTER); | ||||
|
paul
|
r45 | //*********** | ||
|
paul
|
r35 | // RTEMS TASK | ||
| rtems_task spiq_task(rtems_task_argument unused) | ||||
| { | ||||
|
paul
|
r46 | /** This RTEMS task is awaken by an rtems_event sent by the interruption subroutine of the SpaceWire driver. | ||
|
paul
|
r45 | * | ||
| * @param unused is the starting argument of the RTEMS task | ||||
| * | ||||
| */ | ||||
|
paul
|
r35 | rtems_event_set event_out; | ||
| rtems_status_code status; | ||||
|
paul
|
r46 | int linkStatus; | ||
| BOOT_PRINTF("in SPIQ *** \n") | ||||
|
paul
|
r35 | |||
| while(true){ | ||||
| rtems_event_receive(SPW_LINKERR_EVENT, RTEMS_WAIT, RTEMS_NO_TIMEOUT, &event_out); // wait for an SPW_LINKERR_EVENT | ||||
|
paul
|
r48 | PRINTF("in SPIQ *** got SPW_LINKERR_EVENT\n") | ||
|
paul
|
r35 | |||
|
paul
|
r51 | // [0] SUSPEND RECV AND SEND TASKS | ||
|
paul
|
r77 | status = rtems_task_suspend( Task_id[ TASKID_RECV ] ); | ||
| if ( status != RTEMS_SUCCESSFUL ) { | ||||
| PRINTF("in SPIQ *** ERR suspending RECV Task\n") | ||||
| } | ||||
| status = rtems_task_suspend( Task_id[ TASKID_SEND ] ); | ||||
| if ( status != RTEMS_SUCCESSFUL ) { | ||||
| PRINTF("in SPIQ *** ERR suspending SEND Task\n") | ||||
| } | ||||
|
paul
|
r47 | |||
| // [1] CHECK THE LINK | ||||
|
paul
|
r77 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (1) | ||
|
paul
|
r46 | if ( linkStatus != 5) { | ||
| PRINTF1("in SPIQ *** linkStatus %d, wait...\n", linkStatus) | ||||
|
paul
|
r77 | status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms | ||
|
paul
|
r46 | } | ||
|
paul
|
r35 | |||
|
paul
|
r47 | // [2] RECHECK THE LINK AFTER SY_LFR_DPU_CONNECT_TIMEOUT | ||
|
paul
|
r253 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status (2) | ||
|
paul
|
r47 | if ( linkStatus != 5 ) // [2.a] not in run state, reset the link | ||
|
paul
|
r46 | { | ||
|
paul
|
r264 | spacewire_read_statistics(); | ||
|
paul
|
r253 | status = spacewire_several_connect_attemps( ); | ||
|
paul
|
r46 | } | ||
|
paul
|
r47 | else // [2.b] in run state, start the link | ||
| { | ||||
|
paul
|
r127 | status = spacewire_stop_and_start_link( fdSPW ); // start the link | ||
|
paul
|
r46 | if ( status != RTEMS_SUCCESSFUL) | ||
| { | ||||
|
paul
|
r197 | PRINTF1("in SPIQ *** ERR spacewire_stop_and_start_link %d\n", status) | ||
|
paul
|
r46 | } | ||
| } | ||||
|
paul
|
r35 | |||
|
paul
|
r47 | // [3] COMPLETE RECOVERY ACTION AFTER SY_LFR_DPU_CONNECT_ATTEMPTS | ||
| if ( status == RTEMS_SUCCESSFUL ) // [3.a] the link is in run state and has been started successfully | ||||
|
paul
|
r46 | { | ||
|
paul
|
r48 | status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 ); | ||
|
paul
|
r46 | if ( status != RTEMS_SUCCESSFUL ) { | ||
| PRINTF("in SPIQ *** ERR resuming SEND Task\n") | ||||
| } | ||||
|
paul
|
r48 | status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 ); | ||
|
paul
|
r46 | if ( status != RTEMS_SUCCESSFUL ) { | ||
| PRINTF("in SPIQ *** ERR resuming RECV Task\n") | ||||
|
paul
|
r35 | } | ||
|
paul
|
r46 | } | ||
|
paul
|
r47 | else // [3.b] the link is not in run state, go in STANDBY mode | ||
|
paul
|
r46 | { | ||
|
paul
|
r237 | status = enter_mode_standby(); | ||
|
paul
|
r253 | if ( status != RTEMS_SUCCESSFUL ) | ||
| { | ||||
|
paul
|
r51 | PRINTF1("in SPIQ *** ERR enter_standby_mode *** code %d\n", status) | ||
|
paul
|
r35 | } | ||
|
paul
|
r254 | { | ||
| updateLFRCurrentMode( LFR_MODE_STANDBY ); | ||||
| } | ||||
| // wake the LINK task up to wait for the link recovery | ||||
| status = rtems_event_send ( Task_id[TASKID_LINK], RTEMS_EVENT_0 ); | ||||
|
paul
|
r77 | status = rtems_task_suspend( RTEMS_SELF ); | ||
|
paul
|
r35 | } | ||
| } | ||||
| } | ||||
|
paul
|
r45 | rtems_task recv_task( rtems_task_argument unused ) | ||
| { | ||||
| /** This RTEMS task is dedicated to the reception of incoming TeleCommands. | ||||
| * | ||||
| * @param unused is the starting argument of the RTEMS task | ||||
| * | ||||
| * The RECV task blocks on a call to the read system call, waiting for incoming SpaceWire data. When unblocked: | ||||
| * 1. It reads the incoming data. | ||||
| * 2. Launches the acceptance procedure. | ||||
| * 3. If the Telecommand is valid, sends it to a dedicated RTEMS message queue. | ||||
| * | ||||
| */ | ||||
| int len; | ||||
| ccsdsTelecommandPacket_t currentTC; | ||||
| unsigned char computed_CRC[ 2 ]; | ||||
| unsigned char currentTC_LEN_RCV[ 2 ]; | ||||
|
paul
|
r68 | unsigned char destinationID; | ||
|
paul
|
r112 | unsigned int estimatedPacketLength; | ||
|
paul
|
r45 | unsigned int parserCode; | ||
| rtems_status_code status; | ||||
| rtems_id queue_recv_id; | ||||
| rtems_id queue_send_id; | ||||
| initLookUpTableForCRC(); // the table is used to compute Cyclic Redundancy Codes | ||||
|
paul
|
r82 | status = get_message_queue_id_recv( &queue_recv_id ); | ||
|
paul
|
r45 | if (status != RTEMS_SUCCESSFUL) | ||
| { | ||||
|
paul
|
r82 | PRINTF1("in RECV *** ERR get_message_queue_id_recv %d\n", status) | ||
|
paul
|
r45 | } | ||
|
paul
|
r82 | status = get_message_queue_id_send( &queue_send_id ); | ||
|
paul
|
r45 | if (status != RTEMS_SUCCESSFUL) | ||
| { | ||||
|
paul
|
r82 | PRINTF1("in RECV *** ERR get_message_queue_id_send %d\n", status) | ||
|
paul
|
r45 | } | ||
| BOOT_PRINTF("in RECV *** \n") | ||||
| while(1) | ||||
| { | ||||
|
paul
|
r46 | len = read( fdSPW, (char*) ¤tTC, CCSDS_TC_PKT_MAX_SIZE ); // the call to read is blocking | ||
|
paul
|
r45 | if (len == -1){ // error during the read call | ||
|
paul
|
r46 | PRINTF1("in RECV *** last read call returned -1, ERRNO %d\n", errno) | ||
|
paul
|
r45 | } | ||
| else { | ||||
| if ( (len+1) < CCSDS_TC_PKT_MIN_SIZE ) { | ||||
|
paul
|
r46 | PRINTF("in RECV *** packet lenght too short\n") | ||
|
paul
|
r45 | } | ||
| else { | ||||
|
paul
|
r112 | estimatedPacketLength = (unsigned int) (len - CCSDS_TC_TM_PACKET_OFFSET - 3); // => -3 is for Prot ID, Reserved and User App bytes | ||
| currentTC_LEN_RCV[ 0 ] = (unsigned char) (estimatedPacketLength >> 8); | ||||
| currentTC_LEN_RCV[ 1 ] = (unsigned char) (estimatedPacketLength ); | ||||
|
paul
|
r45 | // CHECK THE TC | ||
|
paul
|
r112 | parserCode = tc_parser( ¤tTC, estimatedPacketLength, computed_CRC ) ; | ||
|
paul
|
r51 | if ( (parserCode == ILLEGAL_APID) || (parserCode == WRONG_LEN_PKT) | ||
| || (parserCode == INCOR_CHECKSUM) || (parserCode == ILL_TYPE) | ||||
| || (parserCode == ILL_SUBTYPE) || (parserCode == WRONG_APP_DATA) | ||||
| || (parserCode == WRONG_SRC_ID) ) | ||||
|
paul
|
r45 | { // send TM_LFR_TC_EXE_CORRUPTED | ||
|
paul
|
r107 | PRINTF1("TC corrupted received, with code: %d\n", parserCode) | ||
|
paul
|
r51 | if ( !( (currentTC.serviceType==TC_TYPE_TIME) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_TIME) ) | ||
| && | ||||
| !( (currentTC.serviceType==TC_TYPE_GEN) && (currentTC.serviceSubType==TC_SUBTYPE_UPDT_INFO)) | ||||
| ) | ||||
| { | ||||
|
paul
|
r68 | if ( parserCode == WRONG_SRC_ID ) | ||
| { | ||||
| destinationID = SID_TC_GROUND; | ||||
| } | ||||
| else | ||||
| { | ||||
| destinationID = currentTC.sourceID; | ||||
| } | ||||
|
paul
|
r75 | send_tm_lfr_tc_exe_corrupted( ¤tTC, queue_send_id, | ||
| computed_CRC, currentTC_LEN_RCV, | ||||
|
paul
|
r104 | destinationID ); | ||
|
paul
|
r51 | } | ||
|
paul
|
r45 | } | ||
| else | ||||
| { // send valid TC to the action launcher | ||||
| status = rtems_message_queue_send( queue_recv_id, ¤tTC, | ||||
|
paul
|
r112 | estimatedPacketLength + CCSDS_TC_TM_PACKET_OFFSET + 3); | ||
|
paul
|
r45 | } | ||
| } | ||||
| } | ||||
|
paul
|
r197 | |||
| update_queue_max_count( queue_recv_id, &hk_lfr_q_rv_fifo_size_max ); | ||||
|
paul
|
r45 | } | ||
| } | ||||
| rtems_task send_task( rtems_task_argument argument) | ||||
| { | ||||
| /** This RTEMS task is dedicated to the transmission of TeleMetry packets. | ||||
| * | ||||
| * @param unused is the starting argument of the RTEMS task | ||||
| * | ||||
| * The SEND task waits for a message to become available in the dedicated RTEMS queue. When a message arrives: | ||||
| * - if the first byte is equal to CCSDS_DESTINATION_ID, the message is sent as is using the write system call. | ||||
| * - if the first byte is not equal to CCSDS_DESTINATION_ID, the message is handled as a spw_ioctl_pkt_send. After | ||||
| * analyzis, the packet is sent either using the write system call or using the ioctl call SPACEWIRE_IOCTRL_SEND, depending on the | ||||
| * data it contains. | ||||
| * | ||||
| */ | ||||
|
paul
|
r118 | rtems_status_code status; // RTEMS status code | ||
| char incomingData[MSG_QUEUE_SIZE_SEND]; // incoming data buffer | ||||
|
paul
|
r172 | ring_node *incomingRingNodePtr; | ||
| int ring_node_address; | ||||
| char *charPtr; | ||||
|
paul
|
r45 | spw_ioctl_pkt_send *spw_ioctl_send; | ||
| size_t size; // size of the incoming TC packet | ||||
|
paul
|
r197 | rtems_id queue_send_id; | ||
|
paul
|
r194 | unsigned int sid; | ||
|
paul
|
r228 | unsigned char sidAsUnsignedChar; | ||
|
paul
|
r230 | unsigned char type; | ||
|
paul
|
r172 | |||
| incomingRingNodePtr = NULL; | ||||
| ring_node_address = 0; | ||||
| charPtr = (char *) &ring_node_address; | ||||
| sid = 0; | ||||
|
paul
|
r228 | sidAsUnsignedChar = 0; | ||
|
paul
|
r172 | |||
| init_header_cwf( &headerCWF ); | ||||
| init_header_swf( &headerSWF ); | ||||
| init_header_asm( &headerASM ); | ||||
|
paul
|
r45 | |||
|
paul
|
r197 | status = get_message_queue_id_send( &queue_send_id ); | ||
|
paul
|
r45 | if (status != RTEMS_SUCCESSFUL) | ||
| { | ||||
|
paul
|
r82 | PRINTF1("in HOUS *** ERR get_message_queue_id_send %d\n", status) | ||
|
paul
|
r45 | } | ||
| BOOT_PRINTF("in SEND *** \n") | ||||
| while(1) | ||||
| { | ||||
|
paul
|
r197 | status = rtems_message_queue_receive( queue_send_id, incomingData, &size, | ||
|
paul
|
r45 | RTEMS_WAIT, RTEMS_NO_TIMEOUT ); | ||
| if (status!=RTEMS_SUCCESSFUL) | ||||
| { | ||||
| PRINTF1("in SEND *** (1) ERR = %d\n", status) | ||||
| } | ||||
| else | ||||
| { | ||||
|
paul
|
r172 | if ( size == sizeof(ring_node*) ) | ||
| { | ||||
| charPtr[0] = incomingData[0]; | ||||
| charPtr[1] = incomingData[1]; | ||||
| charPtr[2] = incomingData[2]; | ||||
| charPtr[3] = incomingData[3]; | ||||
| incomingRingNodePtr = (ring_node*) ring_node_address; | ||||
| sid = incomingRingNodePtr->sid; | ||||
| if ( (sid==SID_NORM_CWF_LONG_F3) | ||||
| || (sid==SID_BURST_CWF_F2 ) | ||||
| || (sid==SID_SBM1_CWF_F1 ) | ||||
| || (sid==SID_SBM2_CWF_F2 )) | ||||
| { | ||||
| spw_send_waveform_CWF( incomingRingNodePtr, &headerCWF ); | ||||
| } | ||||
| else if ( (sid==SID_NORM_SWF_F0) || (sid== SID_NORM_SWF_F1) || (sid==SID_NORM_SWF_F2) ) | ||||
| { | ||||
| spw_send_waveform_SWF( incomingRingNodePtr, &headerSWF ); | ||||
| } | ||||
| else if ( (sid==SID_NORM_CWF_F3) ) | ||||
| { | ||||
| spw_send_waveform_CWF3_light( incomingRingNodePtr, &headerCWF ); | ||||
| } | ||||
|
paul
|
r196 | else if (sid==SID_NORM_ASM_F0) | ||
| { | ||||
| spw_send_asm_f0( incomingRingNodePtr, &headerASM ); | ||||
| } | ||||
| else if (sid==SID_NORM_ASM_F1) | ||||
|
paul
|
r172 | { | ||
|
paul
|
r196 | spw_send_asm_f1( incomingRingNodePtr, &headerASM ); | ||
| } | ||||
| else if (sid==SID_NORM_ASM_F2) | ||||
| { | ||||
| spw_send_asm_f2( incomingRingNodePtr, &headerASM ); | ||||
|
paul
|
r172 | } | ||
|
paul
|
r194 | else if ( sid==TM_CODE_K_DUMP ) | ||
| { | ||||
| spw_send_k_dump( incomingRingNodePtr ); | ||||
| } | ||||
|
paul
|
r172 | else | ||
| { | ||||
|
paul
|
r227 | PRINTF1("unexpected sid = %d\n", sid); | ||
|
paul
|
r172 | } | ||
| } | ||||
| else if ( incomingData[0] == CCSDS_DESTINATION_ID ) // the incoming message is a ccsds packet | ||||
|
paul
|
r45 | { | ||
|
paul
|
r228 | sidAsUnsignedChar = (unsigned char) incomingData[ PACKET_POS_PA_LFR_SID_PKT ]; | ||
| sid = sidAsUnsignedChar; | ||||
|
paul
|
r230 | type = (unsigned char) incomingData[ PACKET_POS_SERVICE_TYPE ]; | ||
| if (type == TM_TYPE_LFR_SCIENCE) // this is a BP packet, all other types are handled differently | ||||
|
paul
|
r228 | // SET THE SEQUENCE_CNT PARAMETER IN CASE OF BP0 OR BP1 PACKETS | ||
| { | ||||
| increment_seq_counter_source_id( (unsigned char*) &incomingData[ PACKET_POS_SEQUENCE_CNT ], sid ); | ||||
| } | ||||
|
paul
|
r45 | status = write( fdSPW, incomingData, size ); | ||
| if (status == -1){ | ||||
|
paul
|
r48 | PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size) | ||
|
paul
|
r45 | } | ||
| } | ||||
| else // the incoming message is a spw_ioctl_pkt_send structure | ||||
| { | ||||
| spw_ioctl_send = (spw_ioctl_pkt_send*) incomingData; | ||||
|
paul
|
r58 | status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send ); | ||
| if (status == -1){ | ||||
| PRINTF2("in SEND *** (2.b) ERRNO = %d, RTEMS = %d\n", errno, status) | ||||
|
paul
|
r45 | } | ||
| } | ||||
| } | ||||
|
paul
|
r197 | update_queue_max_count( queue_send_id, &hk_lfr_q_sd_fifo_size_max ); | ||
|
paul
|
r45 | } | ||
| } | ||||
|
paul
|
r254 | rtems_task link_task( rtems_task_argument argument ) | ||
|
paul
|
r46 | { | ||
| rtems_event_set event_out; | ||||
| rtems_status_code status; | ||||
| int linkStatus; | ||||
|
paul
|
r254 | BOOT_PRINTF("in LINK ***\n") | ||
|
paul
|
r46 | |||
|
paul
|
r47 | while(1) | ||
| { | ||||
|
paul
|
r46 | // wait for an RTEMS_EVENT | ||
| rtems_event_receive( RTEMS_EVENT_0, | ||||
| RTEMS_WAIT | RTEMS_EVENT_ANY, RTEMS_NO_TIMEOUT, &event_out); | ||||
|
paul
|
r254 | PRINTF("in LINK *** wait for the link\n") | ||
|
paul
|
r184 | status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status | ||
| while( linkStatus != 5) // wait for the link | ||||
|
paul
|
r46 | { | ||
|
paul
|
r184 | status = rtems_task_wake_after( 10 ); // monitor the link each 100ms | ||
| status = ioctl(fdSPW, SPACEWIRE_IOCTRL_GET_LINK_STATUS, &linkStatus); // get the link status | ||||
|
paul
|
r262 | watchdog_reload(); | ||
|
paul
|
r46 | } | ||
|
paul
|
r264 | spacewire_read_statistics(); | ||
|
paul
|
r127 | status = spacewire_stop_and_start_link( fdSPW ); | ||
|
paul
|
r47 | |||
| if (status != RTEMS_SUCCESSFUL) | ||||
|
paul
|
r46 | { | ||
|
paul
|
r254 | PRINTF1("in LINK *** ERR link not started %d\n", status) | ||
|
paul
|
r46 | } | ||
| else | ||||
| { | ||||
|
paul
|
r254 | PRINTF("in LINK *** OK link started\n") | ||
|
paul
|
r46 | } | ||
|
paul
|
r47 | // restart the SPIQ task | ||
|
paul
|
r48 | status = rtems_task_restart( Task_id[TASKID_SPIQ], 1 ); | ||
| if ( status != RTEMS_SUCCESSFUL ) { | ||||
| PRINTF("in SPIQ *** ERR restarting SPIQ Task\n") | ||||
| } | ||||
|
paul
|
r46 | |||
|
paul
|
r48 | // restart RECV and SEND | ||
|
paul
|
r47 | status = rtems_task_restart( Task_id[ TASKID_SEND ], 1 ); | ||
| if ( status != RTEMS_SUCCESSFUL ) { | ||||
|
paul
|
r48 | PRINTF("in SPIQ *** ERR restarting SEND Task\n") | ||
|
paul
|
r47 | } | ||
| status = rtems_task_restart( Task_id[ TASKID_RECV ], 1 ); | ||||
| if ( status != RTEMS_SUCCESSFUL ) { | ||||
|
paul
|
r48 | PRINTF("in SPIQ *** ERR restarting RECV Task\n") | ||
|
paul
|
r47 | } | ||
|
paul
|
r46 | } | ||
| } | ||||
|
paul
|
r45 | //**************** | ||
| // OTHER FUNCTIONS | ||||
|
paul
|
r127 | int spacewire_open_link( void ) // by default, the driver resets the core: [SPW_CTRL_WRITE(pDev, SPW_CTRL_RESET);] | ||
|
paul
|
r46 | { | ||
| /** This function opens the SpaceWire link. | ||||
| * | ||||
| * @return a valid file descriptor in case of success, -1 in case of a failure | ||||
| * | ||||
| */ | ||||
| rtems_status_code status; | ||||
| fdSPW = open(GRSPW_DEVICE_NAME, O_RDWR); // open the device. the open call resets the hardware | ||||
| if ( fdSPW < 0 ) { | ||||
| PRINTF1("ERR *** in configure_spw_link *** error opening "GRSPW_DEVICE_NAME" with ERR %d\n", errno) | ||||
| } | ||||
| else | ||||
| { | ||||
| status = RTEMS_SUCCESSFUL; | ||||
| } | ||||
| return status; | ||||
| } | ||||
| int spacewire_start_link( int fd ) | ||||
| { | ||||
| rtems_status_code status; | ||||
|
paul
|
r127 | status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started | ||
|
paul
|
r47 | // -1 default hardcoded driver timeout | ||
| return status; | ||||
| } | ||||
|
paul
|
r127 | int spacewire_stop_and_start_link( int fd ) | ||
|
paul
|
r47 | { | ||
| rtems_status_code status; | ||||
|
paul
|
r127 | status = ioctl( fd, SPACEWIRE_IOCTRL_STOP); // start fails if link pDev->running != 0 | ||
| status = ioctl( fd, SPACEWIRE_IOCTRL_START, -1); // returns successfuly if the link is started | ||||
|
paul
|
r46 | // -1 default hardcoded driver timeout | ||
| return status; | ||||
| } | ||||
| int spacewire_configure_link( int fd ) | ||||
|
paul
|
r35 | { | ||
|
paul
|
r45 | /** This function configures the SpaceWire link. | ||
| * | ||||
| * @return GR-RTEMS-DRIVER directive status codes: | ||||
| * - 22 EINVAL - Null pointer or an out of range value was given as the argument. | ||||
| * - 16 EBUSY - Only used for SEND. Returned when no descriptors are avialble in non-blocking mode. | ||||
| * - 88 ENOSYS - Returned for SET_DESTKEY if RMAP command handler is not available or if a non-implemented call is used. | ||||
| * - 116 ETIMEDOUT - REturned for SET_PACKET_SIZE and START if the link could not be brought up. | ||||
| * - 12 ENOMEM - Returned for SET_PACKETSIZE if it was unable to allocate the new buffers. | ||||
| * - 5 EIO - Error when writing to grswp hardware registers. | ||||
| * - 2 ENOENT - No such file or directory | ||||
| */ | ||||
|
paul
|
r35 | rtems_status_code status; | ||
| spacewire_set_NP(1, REGS_ADDR_GRSPW); // [N]o [P]ort force | ||||
| spacewire_set_RE(1, REGS_ADDR_GRSPW); // [R]MAP [E]nable, the dedicated call seems to break the no port force configuration | ||||
|
paul
|
r46 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_RXBLOCK, 1); // sets the blocking mode for reception | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
| PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_RXBLOCK\n") | ||||
| } | ||||
|
paul
|
r35 | // | ||
|
paul
|
r46 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_EVENT_ID, Task_id[TASKID_SPIQ]); // sets the task ID to which an event is sent when a | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
|
paul
|
r181 | PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_EVENT_ID\n") // link-error interrupt occurs | ||
|
paul
|
r166 | } | ||
|
paul
|
r35 | // | ||
|
paul
|
r46 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_DISABLE_ERR, 0); // automatic link-disabling due to link-error interrupts | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
| PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_DISABLE_ERR\n") | ||||
| } | ||||
|
paul
|
r35 | // | ||
|
paul
|
r46 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ, 1); // sets the link-error interrupt bit | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
| PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_LINK_ERR_IRQ\n") | ||||
| } | ||||
|
paul
|
r35 | // | ||
|
paul
|
r172 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK, 1); // transmission blocks | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
| PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK\n") | ||||
| } | ||||
|
paul
|
r35 | // | ||
|
paul
|
r48 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL, 1); // transmission blocks when no transmission descriptor is available | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
| PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TXBLOCK_ON_FULL\n") | ||||
| } | ||||
|
paul
|
r35 | // | ||
|
paul
|
r46 | status = ioctl(fd, SPACEWIRE_IOCTRL_SET_TCODE_CTRL, 0x0909); // [Time Rx : Time Tx : Link error : Tick-out IRQ] | ||
|
paul
|
r166 | if (status!=RTEMS_SUCCESSFUL) { | ||
| PRINTF("in SPIQ *** Error SPACEWIRE_IOCTRL_SET_TCODE_CTRL,\n") | ||||
| } | ||||
|
paul
|
r35 | |||
|
paul
|
r46 | return status; | ||
|
paul
|
r35 | } | ||
|
paul
|
r253 | int spacewire_several_connect_attemps( void ) | ||
|
paul
|
r35 | { | ||
|
paul
|
r46 | /** This function is executed by the SPIQ rtems_task wehn it has been awaken by an interruption raised by the SpaceWire driver. | ||
|
paul
|
r45 | * | ||
| * @return RTEMS directive status code: | ||||
| * - RTEMS_UNSATISFIED is returned is the link is not in the running state after 10 s. | ||||
| * - RTEMS_SUCCESSFUL is returned if the link is up before the timeout. | ||||
| * | ||||
| */ | ||||
|
paul
|
r46 | rtems_status_code status_spw; | ||
|
paul
|
r184 | rtems_status_code status; | ||
|
paul
|
r46 | int i; | ||
| for ( i=0; i<SY_LFR_DPU_CONNECT_ATTEMPT; i++ ) | ||||
| { | ||||
| PRINTF1("in spacewire_reset_link *** link recovery, try %d\n", i); | ||||
|
paul
|
r47 | |||
| // CLOSING THE DRIVER AT THIS POINT WILL MAKE THE SEND TASK BLOCK THE SYSTEM | ||||
|
paul
|
r35 | |||
|
paul
|
r184 | status = rtems_task_wake_after( SY_LFR_DPU_CONNECT_TIMEOUT ); // wait SY_LFR_DPU_CONNECT_TIMEOUT 1000 ms | ||
|
paul
|
r127 | status_spw = spacewire_stop_and_start_link( fdSPW ); | ||
|
paul
|
r263 | |||
|
paul
|
r47 | if ( status_spw != RTEMS_SUCCESSFUL ) | ||
|
paul
|
r46 | { | ||
|
paul
|
r47 | PRINTF1("in spacewire_reset_link *** ERR spacewire_start_link code %d\n", status_spw) | ||
|
paul
|
r46 | } | ||
| if ( status_spw == RTEMS_SUCCESSFUL) | ||||
| { | ||||
|
paul
|
r35 | break; | ||
| } | ||||
| } | ||||
|
paul
|
r46 | return status_spw; | ||
|
paul
|
r35 | } | ||
|
paul
|
r45 | void spacewire_set_NP( unsigned char val, unsigned int regAddr ) // [N]o [P]ort force | ||
|
paul
|
r35 | { | ||
|
paul
|
r45 | /** This function sets the [N]o [P]ort force bit of the GRSPW control register. | ||
| * | ||||
| * @param val is the value, 0 or 1, used to set the value of the NP bit. | ||||
| * @param regAddr is the address of the GRSPW control register. | ||||
| * | ||||
| * NP is the bit 20 of the GRSPW control register. | ||||
| * | ||||
| */ | ||||
|
paul
|
r35 | unsigned int *spwptr = (unsigned int*) regAddr; | ||
| if (val == 1) { | ||||
| *spwptr = *spwptr | 0x00100000; // [NP] set the No port force bit | ||||
| } | ||||
| if (val== 0) { | ||||
| *spwptr = *spwptr & 0xffdfffff; | ||||
| } | ||||
| } | ||||
|
paul
|
r45 | void spacewire_set_RE( unsigned char val, unsigned int regAddr ) // [R]MAP [E]nable | ||
|
paul
|
r35 | { | ||
|
paul
|
r45 | /** This function sets the [R]MAP [E]nable bit of the GRSPW control register. | ||
| * | ||||
| * @param val is the value, 0 or 1, used to set the value of the RE bit. | ||||
| * @param regAddr is the address of the GRSPW control register. | ||||
| * | ||||
| * RE is the bit 16 of the GRSPW control register. | ||||
| * | ||||
| */ | ||||
|
paul
|
r35 | unsigned int *spwptr = (unsigned int*) regAddr; | ||
| if (val == 1) | ||||
| { | ||||
| *spwptr = *spwptr | 0x00010000; // [RE] set the RMAP Enable bit | ||||
| } | ||||
| if (val== 0) | ||||
| { | ||||
| *spwptr = *spwptr & 0xfffdffff; | ||||
| } | ||||
| } | ||||
|
paul
|
r264 | void spacewire_read_statistics( void ) | ||
|
paul
|
r45 | { | ||
|
paul
|
r265 | /** This function reads the SpaceWire statistics from the grspw RTEMS driver. | ||
| * | ||||
| * @param void | ||||
| * | ||||
| * @return void | ||||
| * | ||||
| * Once they are read, the counters are stored in a global variable used during the building of the | ||||
| * HK packets. | ||||
| * | ||||
| */ | ||||
|
paul
|
r45 | rtems_status_code status; | ||
|
paul
|
r264 | spw_stats current; | ||
|
paul
|
r263 | |||
|
paul
|
r269 | spacewire_get_last_error(); | ||
|
paul
|
r264 | // read the current statistics | ||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, ¤t ); | ||||
| // clear the counters | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_CLR_STATISTICS ); | ||||
|
paul
|
r263 | |||
| // typedef struct { | ||||
| // unsigned int tx_link_err; // NOT IN HK | ||||
| // unsigned int rx_rmap_header_crc_err; // NOT IN HK | ||||
| // unsigned int rx_rmap_data_crc_err; // NOT IN HK | ||||
| // unsigned int rx_eep_err; | ||||
| // unsigned int rx_truncated; | ||||
| // unsigned int parity_err; | ||||
| // unsigned int escape_err; | ||||
| // unsigned int credit_err; | ||||
| // unsigned int write_sync_err; | ||||
| // unsigned int disconnect_err; | ||||
| // unsigned int early_ep; | ||||
| // unsigned int invalid_address; | ||||
| // unsigned int packets_sent; | ||||
| // unsigned int packets_received; | ||||
| // } spw_stats; | ||||
|
paul
|
r45 | |||
|
paul
|
r263 | // rx_eep_err | ||
|
paul
|
r264 | grspw_stats.rx_eep_err = grspw_stats.rx_eep_err + current.rx_eep_err; | ||
|
paul
|
r263 | // rx_truncated | ||
|
paul
|
r264 | grspw_stats.rx_truncated = grspw_stats.rx_truncated + current.rx_truncated; | ||
|
paul
|
r263 | // parity_err | ||
|
paul
|
r264 | grspw_stats.parity_err = grspw_stats.parity_err + current.parity_err; | ||
|
paul
|
r263 | // escape_err | ||
|
paul
|
r264 | grspw_stats.escape_err = grspw_stats.escape_err + current.escape_err; | ||
|
paul
|
r263 | // credit_err | ||
|
paul
|
r264 | grspw_stats.credit_err = grspw_stats.credit_err + current.credit_err; | ||
|
paul
|
r263 | // write_sync_err | ||
|
paul
|
r264 | grspw_stats.write_sync_err = grspw_stats.write_sync_err + current.write_sync_err; | ||
|
paul
|
r263 | // disconnect_err | ||
|
paul
|
r264 | grspw_stats.disconnect_err = grspw_stats.disconnect_err + current.disconnect_err; | ||
|
paul
|
r263 | // early_ep | ||
|
paul
|
r264 | grspw_stats.early_ep = grspw_stats.early_ep + current.early_ep; | ||
|
paul
|
r263 | // invalid_address | ||
|
paul
|
r264 | grspw_stats.invalid_address = grspw_stats.invalid_address + current.invalid_address; | ||
|
paul
|
r263 | // packets_sent | ||
|
paul
|
r264 | grspw_stats.packets_sent = grspw_stats.packets_sent + current.packets_sent; | ||
|
paul
|
r263 | // packets_received | ||
|
paul
|
r264 | grspw_stats.packets_received= grspw_stats.packets_received + current.packets_received; | ||
|
paul
|
r263 | |||
| } | ||||
| void spacewire_get_last_error( void ) | ||||
| { | ||||
| static spw_stats previous; | ||||
| spw_stats current; | ||||
| rtems_status_code status; | ||||
| unsigned int hk_lfr_last_er_rid; | ||||
| unsigned char hk_lfr_last_er_code; | ||||
| int coarseTime; | ||||
| int fineTime; | ||||
| unsigned char update_hk_lfr_last_er; | ||||
| update_hk_lfr_last_er = 0; | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, ¤t ); | ||||
| // get current time | ||||
| coarseTime = time_management_regs->coarse_time; | ||||
| fineTime = time_management_regs->fine_time; | ||||
| // typedef struct { | ||||
| // unsigned int tx_link_err; // NOT IN HK | ||||
| // unsigned int rx_rmap_header_crc_err; // NOT IN HK | ||||
| // unsigned int rx_rmap_data_crc_err; // NOT IN HK | ||||
| // unsigned int rx_eep_err; | ||||
| // unsigned int rx_truncated; | ||||
| // unsigned int parity_err; | ||||
| // unsigned int escape_err; | ||||
| // unsigned int credit_err; | ||||
| // unsigned int write_sync_err; | ||||
| // unsigned int disconnect_err; | ||||
| // unsigned int early_ep; | ||||
| // unsigned int invalid_address; | ||||
| // unsigned int packets_sent; | ||||
| // unsigned int packets_received; | ||||
| // } spw_stats; | ||||
|
paul
|
r45 | |||
|
paul
|
r263 | // tx_link_err *** no code associated to this field | ||
| // rx_rmap_header_crc_err *** LE *** in HK | ||||
| if (previous.rx_rmap_header_crc_err != current.rx_rmap_header_crc_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_HEADER_CRC; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // rx_rmap_data_crc_err *** LE *** NOT IN HK | ||||
| if (previous.rx_rmap_data_crc_err != current.rx_rmap_data_crc_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_DATA_CRC; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // rx_eep_err | ||||
| if (previous.rx_eep_err != current.rx_eep_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_EEP; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // rx_truncated | ||||
| if (previous.rx_truncated != current.rx_truncated) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_RX_TOO_BIG; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // parity_err | ||||
| if (previous.parity_err != current.parity_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_PARITY; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // escape_err | ||||
| if (previous.parity_err != current.parity_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_ESCAPE; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // credit_err | ||||
| if (previous.credit_err != current.credit_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_CREDIT; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // write_sync_err | ||||
| if (previous.write_sync_err != current.write_sync_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_WRITE_SYNC; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // disconnect_err | ||||
| if (previous.disconnect_err != current.disconnect_err) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_LE_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_DISCONNECT; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // early_ep | ||||
| if (previous.early_ep != current.early_ep) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_EARLY_EOP_EEP; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
| // invalid_address | ||||
| if (previous.invalid_address != current.invalid_address) | ||||
| { | ||||
| hk_lfr_last_er_rid = RID_ME_LFR_DPU_SPW; | ||||
| hk_lfr_last_er_code = CODE_INVALID_ADDRESS; | ||||
| update_hk_lfr_last_er = 1; | ||||
| } | ||||
|
paul
|
r45 | |||
|
paul
|
r263 | // if a field has changed, update the hk_last_er fields | ||
| if (update_hk_lfr_last_er == 1) | ||||
| { | ||||
| update_hk_lfr_last_er_fields( hk_lfr_last_er_rid, hk_lfr_last_er_code ); | ||||
| } | ||||
| previous = current; | ||||
| } | ||||
| void update_hk_lfr_last_er_fields(unsigned int rid, unsigned char code) | ||||
| { | ||||
| unsigned char *coarseTimePtr; | ||||
| unsigned char *fineTimePtr; | ||||
| coarseTimePtr = (unsigned char*) &time_management_regs->coarse_time; | ||||
| fineTimePtr = (unsigned char*) &time_management_regs->fine_time; | ||||
| housekeeping_packet.hk_lfr_last_er_rid[0] = (unsigned char) ((rid & 0xff00) >> 8 ); | ||||
| housekeeping_packet.hk_lfr_last_er_rid[1] = (unsigned char) (rid & 0x00ff); | ||||
| housekeeping_packet.hk_lfr_last_er_code = code; | ||||
| housekeeping_packet.hk_lfr_last_er_time[0] = coarseTimePtr[0]; | ||||
| housekeeping_packet.hk_lfr_last_er_time[1] = coarseTimePtr[1]; | ||||
| housekeeping_packet.hk_lfr_last_er_time[2] = coarseTimePtr[2]; | ||||
| housekeeping_packet.hk_lfr_last_er_time[3] = coarseTimePtr[3]; | ||||
| housekeeping_packet.hk_lfr_last_er_time[4] = fineTimePtr[2]; | ||||
| housekeeping_packet.hk_lfr_last_er_time[5] = fineTimePtr[3]; | ||||
| } | ||||
|
paul
|
r264 | void update_hk_with_grspw_stats( void ) | ||
|
paul
|
r263 | { | ||
|
paul
|
r45 | //**************************** | ||
| // DPU_SPACEWIRE_IF_STATISTICS | ||||
|
paul
|
r264 | housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (grspw_stats.packets_received >> 8); | ||
| housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (grspw_stats.packets_received); | ||||
| housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (grspw_stats.packets_sent >> 8); | ||||
| housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (grspw_stats.packets_sent); | ||||
|
paul
|
r45 | |||
| //****************************************** | ||||
| // ERROR COUNTERS / SPACEWIRE / LOW SEVERITY | ||||
|
paul
|
r264 | housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) grspw_stats.parity_err; | ||
| housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) grspw_stats.disconnect_err; | ||||
| housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) grspw_stats.escape_err; | ||||
| housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) grspw_stats.credit_err; | ||||
| housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) grspw_stats.write_sync_err; | ||||
|
paul
|
r45 | |||
| //********************************************* | ||||
| // ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY | ||||
|
paul
|
r264 | housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) grspw_stats.early_ep; | ||
| housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) grspw_stats.invalid_address; | ||||
| housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) grspw_stats.rx_eep_err; | ||||
| housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) grspw_stats.rx_truncated; | ||||
|
paul
|
r45 | } | ||
|
paul
|
r279 | void spacewire_update_hk_lfr_link_state( unsigned char *hk_lfr_status_word_0 ) | ||
| { | ||||
| unsigned int *statusRegisterPtr; | ||||
| unsigned char linkState; | ||||
| statusRegisterPtr = (unsigned int *) (REGS_ADDR_GRSPW + APB_OFFSET_GRSPW_STATUS_REGISTER); | ||||
| linkState = (unsigned char) ( ( (*statusRegisterPtr) >> 21) & 0x07); // [0000 0111] | ||||
| *hk_lfr_status_word_0 = *hk_lfr_status_word_0 & 0xf8; // [1111 1000] set link state to 0 | ||||
| *hk_lfr_status_word_0 = *hk_lfr_status_word_0 | linkState; // update hk_lfr_dpu_spw_link_state | ||||
| } | ||||
|
paul
|
r248 | void increase_unsigned_char_counter( unsigned char *counter ) | ||
|
paul
|
r35 | { | ||
|
paul
|
r248 | // update the number of valid timecodes that have been received | ||
| if (*counter == 255) | ||||
| { | ||||
| *counter = 0; | ||||
| } | ||||
| else | ||||
| { | ||||
| *counter = *counter + 1; | ||||
| } | ||||
| } | ||||
|
paul
|
r184 | |||
|
paul
|
r248 | unsigned int check_timecode_and_previous_timecode_coherency(unsigned char currentTimecodeCtr) | ||
| { | ||||
|
paul
|
r250 | /** This function checks the coherency between the incoming timecode and the last valid timecode. | ||
| * | ||||
| * @param currentTimecodeCtr is the incoming timecode | ||||
| * | ||||
| * @return returned codes:: | ||||
| * - LFR_DEFAULT | ||||
| * - LFR_SUCCESSFUL | ||||
| * | ||||
| */ | ||||
|
paul
|
r249 | static unsigned char firstTickout = 1; | ||
|
paul
|
r248 | unsigned char ret; | ||
| ret = LFR_DEFAULT; | ||||
|
paul
|
r249 | if (firstTickout == 0) | ||
|
paul
|
r107 | { | ||
|
paul
|
r249 | if (currentTimecodeCtr == 0) | ||
|
paul
|
r248 | { | ||
|
paul
|
r249 | if (previousTimecodeCtr == 63) | ||
| { | ||||
| ret = LFR_SUCCESSFUL; | ||||
| } | ||||
| else | ||||
| { | ||||
| ret = LFR_DEFAULT; | ||||
| } | ||||
|
paul
|
r248 | } | ||
| else | ||||
| { | ||||
|
paul
|
r249 | if (currentTimecodeCtr == (previousTimecodeCtr +1)) | ||
| { | ||||
| ret = LFR_SUCCESSFUL; | ||||
| } | ||||
| else | ||||
| { | ||||
| ret = LFR_DEFAULT; | ||||
| } | ||||
|
paul
|
r248 | } | ||
|
paul
|
r107 | } | ||
| else | ||||
| { | ||||
|
paul
|
r249 | firstTickout = 0; | ||
| ret = LFR_SUCCESSFUL; | ||||
|
paul
|
r225 | } | ||
|
paul
|
r248 | |||
| return ret; | ||||
| } | ||||
| unsigned int check_timecode_and_internal_time_coherency(unsigned char timecode, unsigned char internalTime) | ||||
| { | ||||
| unsigned int ret; | ||||
| ret = LFR_DEFAULT; | ||||
| if (timecode == internalTime) | ||||
| { | ||||
| ret = LFR_SUCCESSFUL; | ||||
| } | ||||
| else | ||||
| { | ||||
| ret = LFR_DEFAULT; | ||||
| } | ||||
| return ret; | ||||
| } | ||||
| void timecode_irq_handler( void *pDev, void *regs, int minor, unsigned int tc ) | ||||
| { | ||||
| // a tickout has been emitted, perform actions on the incoming timecode | ||||
| unsigned char incomingTimecode; | ||||
| unsigned char updateTime; | ||||
| unsigned char internalTime; | ||||
| rtems_status_code status; | ||||
| incomingTimecode = (unsigned char) (grspwPtr[0] & TIMECODE_MASK); | ||||
| updateTime = time_management_regs->coarse_time_load & TIMECODE_MASK; | ||||
| internalTime = time_management_regs->coarse_time & TIMECODE_MASK; | ||||
| housekeeping_packet.hk_lfr_dpu_spw_last_timc = incomingTimecode; | ||||
| // update the number of tickout that have been generated | ||||
| increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt ); | ||||
| //************************** | ||||
| // HK_LFR_TIMECODE_ERRONEOUS | ||||
| // MISSING and INVALID are handled by the timecode_timer_routine service routine | ||||
| if (check_timecode_and_previous_timecode_coherency( incomingTimecode ) == LFR_DEFAULT) | ||||
| { | ||||
|
paul
|
r249 | // this is unexpected but a tickout could have been raised despite of the timecode being erroneous | ||
|
paul
|
r248 | increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_erroneous ); | ||
|
paul
|
r263 | update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_ERRONEOUS ); | ||
|
paul
|
r248 | } | ||
| //************************ | ||||
| // HK_LFR_TIME_TIMECODE_IT | ||||
| // check the coherency between the SpaceWire timecode and the Internal Time | ||||
| if (check_timecode_and_internal_time_coherency( incomingTimecode, internalTime ) == LFR_DEFAULT) | ||||
| { | ||||
| increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_it ); | ||||
|
paul
|
r263 | update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_IT ); | ||
|
paul
|
r248 | } | ||
| //******************** | ||||
| // HK_LFR_TIMECODE_CTR | ||||
| // check the value of the timecode with respect to the last TC_LFR_UPDATE_TIME => SSS-CP-FS-370 | ||||
|
paul
|
r271 | if (oneTcLfrUpdateTimeReceived == 1) | ||
|
paul
|
r248 | { | ||
|
paul
|
r271 | if ( incomingTimecode != updateTime ) | ||
| { | ||||
| increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_time_timecode_ctr ); | ||||
| update_hk_lfr_last_er_fields( RID_LE_LFR_TIME, CODE_TIMECODE_CTR ); | ||||
| } | ||||
|
paul
|
r248 | } | ||
| // launch the timecode timer to detect missing or invalid timecodes | ||||
| previousTimecodeCtr = incomingTimecode; // update the previousTimecodeCtr value | ||||
| status = rtems_timer_fire_after( timecode_timer_id, TIMECODE_TIMER_TIMEOUT, timecode_timer_routine, NULL ); | ||||
|
paul
|
r259 | if (status != RTEMS_SUCCESSFUL) | ||
| { | ||||
| rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_14 ); | ||||
| } | ||||
|
paul
|
r46 | } | ||
|
paul
|
r172 | |||
|
paul
|
r271 | rtems_timer_service_routine timecode_timer_routine( rtems_id timer_id, void *user_data ) | ||
| { | ||||
| static unsigned char initStep = 1; | ||||
| unsigned char currentTimecodeCtr; | ||||
| currentTimecodeCtr = (unsigned char) (grspwPtr[0] & TIMECODE_MASK); | ||||
| if (initStep == 1) | ||||
| { | ||||
| if (currentTimecodeCtr == previousTimecodeCtr) | ||||
| { | ||||
| //************************ | ||||
| // HK_LFR_TIMECODE_MISSING | ||||
| // the timecode value has not changed, no valid timecode has been received, the timecode is MISSING | ||||
| increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing ); | ||||
| update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING ); | ||||
| } | ||||
| else if (currentTimecodeCtr == (previousTimecodeCtr+1)) | ||||
| { | ||||
| // the timecode value has changed and the value is valid, this is unexpected because | ||||
| // the timer should not have fired, the timecode_irq_handler should have been raised | ||||
| } | ||||
| else | ||||
| { | ||||
| //************************ | ||||
| // HK_LFR_TIMECODE_INVALID | ||||
| // the timecode value has changed and the value is not valid, no tickout has been generated | ||||
| // this is why the timer has fired | ||||
| increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_invalid ); | ||||
| update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_INVALID ); | ||||
| } | ||||
| } | ||||
| else | ||||
| { | ||||
| initStep = 1; | ||||
| //************************ | ||||
| // HK_LFR_TIMECODE_MISSING | ||||
| increase_unsigned_char_counter( &housekeeping_packet.hk_lfr_timecode_missing ); | ||||
| update_hk_lfr_last_er_fields( RID_LE_LFR_TIMEC, CODE_MISSING ); | ||||
| } | ||||
| rtems_event_send( Task_id[TASKID_DUMB], RTEMS_EVENT_13 ); | ||||
| } | ||||
|
paul
|
r172 | void init_header_cwf( Header_TM_LFR_SCIENCE_CWF_t *header ) | ||
| { | ||||
|
paul
|
r181 | header->targetLogicalAddress = CCSDS_DESTINATION_ID; | ||
| header->protocolIdentifier = CCSDS_PROTOCOLE_ID; | ||||
| header->reserved = DEFAULT_RESERVED; | ||||
| header->userApplication = CCSDS_USER_APP; | ||||
| header->packetSequenceControl[0]= TM_PACKET_SEQ_CTRL_STANDALONE; | ||||
| header->packetSequenceControl[1]= TM_PACKET_SEQ_CNT_DEFAULT; | ||||
|
paul
|
r179 | header->packetLength[0] = 0x00; | ||
| header->packetLength[1] = 0x00; | ||||
|
paul
|
r172 | // DATA FIELD HEADER | ||
| header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | ||||
| header->serviceType = TM_TYPE_LFR_SCIENCE; // service type | ||||
|
paul
|
r192 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype | ||
|
paul
|
r172 | header->destinationID = TM_DESTINATION_ID_GROUND; | ||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| // AUXILIARY DATA HEADER | ||||
| header->sid = 0x00; | ||||
|
paul
|
r283 | header->pa_bia_status_info = DEFAULT_HKBIA; | ||
|
paul
|
r179 | header->blkNr[0] = 0x00; | ||
| header->blkNr[1] = 0x00; | ||||
|
paul
|
r172 | } | ||
| void init_header_swf( Header_TM_LFR_SCIENCE_SWF_t *header ) | ||||
| { | ||||
| header->targetLogicalAddress = CCSDS_DESTINATION_ID; | ||||
| header->protocolIdentifier = CCSDS_PROTOCOLE_ID; | ||||
| header->reserved = DEFAULT_RESERVED; | ||||
| header->userApplication = CCSDS_USER_APP; | ||||
| header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | ||||
| header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | ||||
| header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | ||||
| header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | ||||
| header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8); | ||||
| header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); | ||||
| // DATA FIELD HEADER | ||||
| header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | ||||
| header->serviceType = TM_TYPE_LFR_SCIENCE; // service type | ||||
|
paul
|
r201 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; // service subtype | ||
|
paul
|
r172 | header->destinationID = TM_DESTINATION_ID_GROUND; | ||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| // AUXILIARY DATA HEADER | ||||
| header->sid = 0x00; | ||||
|
paul
|
r283 | header->pa_bia_status_info = DEFAULT_HKBIA; | ||
|
paul
|
r172 | header->pktCnt = DEFAULT_PKTCNT; // PKT_CNT | ||
| header->pktNr = 0x00; | ||||
| header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8); | ||||
| header->blkNr[1] = (unsigned char) (BLK_NR_CWF ); | ||||
| } | ||||
| void init_header_asm( Header_TM_LFR_SCIENCE_ASM_t *header ) | ||||
| { | ||||
| header->targetLogicalAddress = CCSDS_DESTINATION_ID; | ||||
| header->protocolIdentifier = CCSDS_PROTOCOLE_ID; | ||||
| header->reserved = DEFAULT_RESERVED; | ||||
| header->userApplication = CCSDS_USER_APP; | ||||
| header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | ||||
| header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | ||||
| header->packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | ||||
| header->packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | ||||
| header->packetLength[0] = 0x00; | ||||
| header->packetLength[1] = 0x00; | ||||
| // DATA FIELD HEADER | ||||
| header->spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | ||||
| header->serviceType = TM_TYPE_LFR_SCIENCE; // service type | ||||
|
paul
|
r192 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; // service subtype | ||
|
paul
|
r172 | header->destinationID = TM_DESTINATION_ID_GROUND; | ||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| header->time[0] = 0x00; | ||||
| // AUXILIARY DATA HEADER | ||||
| header->sid = 0x00; | ||||
|
paul
|
r283 | header->pa_bia_status_info = 0x00; | ||
|
paul
|
r172 | header->pa_lfr_pkt_cnt_asm = 0x00; | ||
| header->pa_lfr_pkt_nr_asm = 0x00; | ||||
| header->pa_lfr_asm_blk_nr[0] = 0x00; | ||||
| header->pa_lfr_asm_blk_nr[1] = 0x00; | ||||
| } | ||||
| int spw_send_waveform_CWF( ring_node *ring_node_to_send, | ||||
| Header_TM_LFR_SCIENCE_CWF_t *header ) | ||||
| { | ||||
| /** This function sends CWF CCSDS packets (F2, F1 or F0). | ||||
| * | ||||
| * @param waveform points to the buffer containing the data that will be send. | ||||
| * @param sid is the source identifier of the data that will be sent. | ||||
| * @param headerCWF points to a table of headers that have been prepared for the data transmission. | ||||
| * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | ||||
| * contain information to setup the transmission of the data packets. | ||||
| * | ||||
| * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. | ||||
| * | ||||
| */ | ||||
| unsigned int i; | ||||
| int ret; | ||||
| unsigned int coarseTime; | ||||
| unsigned int fineTime; | ||||
| rtems_status_code status; | ||||
| spw_ioctl_pkt_send spw_ioctl_send_CWF; | ||||
| int *dataPtr; | ||||
| unsigned char sid; | ||||
|
paul
|
r196 | spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF; | ||
|
paul
|
r172 | spw_ioctl_send_CWF.options = 0; | ||
| ret = LFR_DEFAULT; | ||||
| sid = (unsigned char) ring_node_to_send->sid; | ||||
| coarseTime = ring_node_to_send->coarseTime; | ||||
| fineTime = ring_node_to_send->fineTime; | ||||
| dataPtr = (int*) ring_node_to_send->buffer_address; | ||||
|
paul
|
r179 | header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_336 >> 8); | ||
| header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_336 ); | ||||
|
paul
|
r283 | header->pa_bia_status_info = pa_bia_status_info; | ||
|
paul
|
r192 | header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | ||
|
paul
|
r179 | header->blkNr[0] = (unsigned char) (BLK_NR_CWF >> 8); | ||
| header->blkNr[1] = (unsigned char) (BLK_NR_CWF ); | ||||
|
paul
|
r172 | for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF; i++) // send waveform | ||
| { | ||||
| spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF * NB_WORDS_SWF_BLK) ]; | ||||
| spw_ioctl_send_CWF.hdr = (char*) header; | ||||
| // BUILD THE DATA | ||||
| spw_ioctl_send_CWF.dlen = BLK_NR_CWF * NB_BYTES_SWF_BLK; | ||||
| // SET PACKET SEQUENCE CONTROL | ||||
| increment_seq_counter_source_id( header->packetSequenceControl, sid ); | ||||
| // SET SID | ||||
| header->sid = sid; | ||||
| // SET PACKET TIME | ||||
| compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime); | ||||
| // | ||||
| header->time[0] = header->acquisitionTime[0]; | ||||
| header->time[1] = header->acquisitionTime[1]; | ||||
| header->time[2] = header->acquisitionTime[2]; | ||||
| header->time[3] = header->acquisitionTime[3]; | ||||
| header->time[4] = header->acquisitionTime[4]; | ||||
| header->time[5] = header->acquisitionTime[5]; | ||||
| // SET PACKET ID | ||||
| if ( (sid == SID_SBM1_CWF_F1) || (sid == SID_SBM2_CWF_F2) ) | ||||
| { | ||||
| header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2 >> 8); | ||||
| header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_SBM1_SBM2); | ||||
| } | ||||
| else | ||||
| { | ||||
| header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | ||||
| header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | ||||
| } | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF ); | ||||
| if (status != RTEMS_SUCCESSFUL) { | ||||
| ret = LFR_DEFAULT; | ||||
| } | ||||
| } | ||||
| return ret; | ||||
| } | ||||
| int spw_send_waveform_SWF( ring_node *ring_node_to_send, | ||||
| Header_TM_LFR_SCIENCE_SWF_t *header ) | ||||
| { | ||||
| /** This function sends SWF CCSDS packets (F2, F1 or F0). | ||||
| * | ||||
| * @param waveform points to the buffer containing the data that will be send. | ||||
| * @param sid is the source identifier of the data that will be sent. | ||||
| * @param headerSWF points to a table of headers that have been prepared for the data transmission. | ||||
| * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | ||||
| * contain information to setup the transmission of the data packets. | ||||
| * | ||||
| * One group of 2048 samples is sent as 7 consecutive packets, 6 packets containing 340 blocks and 8 packets containing 8 blocks. | ||||
| * | ||||
| */ | ||||
| unsigned int i; | ||||
| int ret; | ||||
| unsigned int coarseTime; | ||||
| unsigned int fineTime; | ||||
| rtems_status_code status; | ||||
| spw_ioctl_pkt_send spw_ioctl_send_SWF; | ||||
| int *dataPtr; | ||||
| unsigned char sid; | ||||
|
paul
|
r196 | spw_ioctl_send_SWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_SWF; | ||
|
paul
|
r172 | spw_ioctl_send_SWF.options = 0; | ||
| ret = LFR_DEFAULT; | ||||
| coarseTime = ring_node_to_send->coarseTime; | ||||
| fineTime = ring_node_to_send->fineTime; | ||||
| dataPtr = (int*) ring_node_to_send->buffer_address; | ||||
| sid = ring_node_to_send->sid; | ||||
|
paul
|
r283 | header->pa_bia_status_info = pa_bia_status_info; | ||
|
paul
|
r192 | header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | ||
|
paul
|
r172 | for (i=0; i<7; i++) // send waveform | ||
| { | ||||
| spw_ioctl_send_SWF.data = (char*) &dataPtr[ (i * BLK_NR_304 * NB_WORDS_SWF_BLK) ]; | ||||
| spw_ioctl_send_SWF.hdr = (char*) header; | ||||
| // SET PACKET SEQUENCE CONTROL | ||||
| increment_seq_counter_source_id( header->packetSequenceControl, sid ); | ||||
| // SET PACKET LENGTH AND BLKNR | ||||
| if (i == 6) | ||||
| { | ||||
| spw_ioctl_send_SWF.dlen = BLK_NR_224 * NB_BYTES_SWF_BLK; | ||||
| header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_224 >> 8); | ||||
| header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_224 ); | ||||
| header->blkNr[0] = (unsigned char) (BLK_NR_224 >> 8); | ||||
| header->blkNr[1] = (unsigned char) (BLK_NR_224 ); | ||||
| } | ||||
| else | ||||
| { | ||||
| spw_ioctl_send_SWF.dlen = BLK_NR_304 * NB_BYTES_SWF_BLK; | ||||
| header->packetLength[0] = (unsigned char) (TM_LEN_SCI_SWF_304 >> 8); | ||||
| header->packetLength[1] = (unsigned char) (TM_LEN_SCI_SWF_304 ); | ||||
| header->blkNr[0] = (unsigned char) (BLK_NR_304 >> 8); | ||||
| header->blkNr[1] = (unsigned char) (BLK_NR_304 ); | ||||
| } | ||||
| // SET PACKET TIME | ||||
| compute_acquisition_time( coarseTime, fineTime, sid, i, header->acquisitionTime ); | ||||
| // | ||||
| header->time[0] = header->acquisitionTime[0]; | ||||
| header->time[1] = header->acquisitionTime[1]; | ||||
| header->time[2] = header->acquisitionTime[2]; | ||||
| header->time[3] = header->acquisitionTime[3]; | ||||
| header->time[4] = header->acquisitionTime[4]; | ||||
| header->time[5] = header->acquisitionTime[5]; | ||||
| // SET SID | ||||
| header->sid = sid; | ||||
| // SET PKTNR | ||||
| header->pktNr = i+1; // PKT_NR | ||||
| // SEND PACKET | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_SWF ); | ||||
| if (status != RTEMS_SUCCESSFUL) { | ||||
| ret = LFR_DEFAULT; | ||||
| } | ||||
| } | ||||
| return ret; | ||||
| } | ||||
| int spw_send_waveform_CWF3_light( ring_node *ring_node_to_send, | ||||
| Header_TM_LFR_SCIENCE_CWF_t *header ) | ||||
| { | ||||
| /** This function sends CWF_F3 CCSDS packets without the b1, b2 and b3 data. | ||||
| * | ||||
| * @param waveform points to the buffer containing the data that will be send. | ||||
| * @param headerCWF points to a table of headers that have been prepared for the data transmission. | ||||
| * @param queue_id is the id of the rtems queue to which spw_ioctl_pkt_send structures will be send. The structures | ||||
| * contain information to setup the transmission of the data packets. | ||||
| * | ||||
| * By default, CWF_F3 packet are send without the b1, b2 and b3 data. This function rebuilds a data buffer | ||||
| * from the incoming data and sends it in 7 packets, 6 containing 340 blocks and 1 one containing 8 blocks. | ||||
| * | ||||
| */ | ||||
| unsigned int i; | ||||
| int ret; | ||||
| unsigned int coarseTime; | ||||
| unsigned int fineTime; | ||||
| rtems_status_code status; | ||||
| spw_ioctl_pkt_send spw_ioctl_send_CWF; | ||||
| char *dataPtr; | ||||
| unsigned char sid; | ||||
|
paul
|
r196 | spw_ioctl_send_CWF.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_CWF; | ||
|
paul
|
r172 | spw_ioctl_send_CWF.options = 0; | ||
| ret = LFR_DEFAULT; | ||||
| sid = ring_node_to_send->sid; | ||||
| coarseTime = ring_node_to_send->coarseTime; | ||||
| fineTime = ring_node_to_send->fineTime; | ||||
| dataPtr = (char*) ring_node_to_send->buffer_address; | ||||
|
paul
|
r179 | header->packetLength[0] = (unsigned char) (TM_LEN_SCI_CWF_672 >> 8); | ||
| header->packetLength[1] = (unsigned char) (TM_LEN_SCI_CWF_672 ); | ||||
|
paul
|
r283 | header->pa_bia_status_info = pa_bia_status_info; | ||
|
paul
|
r192 | header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | ||
|
paul
|
r179 | header->blkNr[0] = (unsigned char) (BLK_NR_CWF_SHORT_F3 >> 8); | ||
| header->blkNr[1] = (unsigned char) (BLK_NR_CWF_SHORT_F3 ); | ||||
|
paul
|
r172 | //********************* | ||
| // SEND CWF3_light DATA | ||||
| for (i=0; i<NB_PACKETS_PER_GROUP_OF_CWF_LIGHT; i++) // send waveform | ||||
| { | ||||
| spw_ioctl_send_CWF.data = (char*) &dataPtr[ (i * BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK) ]; | ||||
| spw_ioctl_send_CWF.hdr = (char*) header; | ||||
| // BUILD THE DATA | ||||
| spw_ioctl_send_CWF.dlen = BLK_NR_CWF_SHORT_F3 * NB_BYTES_CWF3_LIGHT_BLK; | ||||
| // SET PACKET SEQUENCE COUNTER | ||||
| increment_seq_counter_source_id( header->packetSequenceControl, sid ); | ||||
| // SET SID | ||||
| header->sid = sid; | ||||
| // SET PACKET TIME | ||||
| compute_acquisition_time( coarseTime, fineTime, SID_NORM_CWF_F3, i, header->acquisitionTime ); | ||||
| // | ||||
| header->time[0] = header->acquisitionTime[0]; | ||||
| header->time[1] = header->acquisitionTime[1]; | ||||
| header->time[2] = header->acquisitionTime[2]; | ||||
| header->time[3] = header->acquisitionTime[3]; | ||||
| header->time[4] = header->acquisitionTime[4]; | ||||
| header->time[5] = header->acquisitionTime[5]; | ||||
| // SET PACKET ID | ||||
| header->packetID[0] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST >> 8); | ||||
| header->packetID[1] = (unsigned char) (APID_TM_SCIENCE_NORMAL_BURST); | ||||
| // SEND PACKET | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_CWF ); | ||||
| if (status != RTEMS_SUCCESSFUL) { | ||||
| ret = LFR_DEFAULT; | ||||
| } | ||||
| } | ||||
| return ret; | ||||
| } | ||||
|
paul
|
r196 | void spw_send_asm_f0( ring_node *ring_node_to_send, | ||
|
paul
|
r172 | Header_TM_LFR_SCIENCE_ASM_t *header ) | ||
| { | ||||
| unsigned int i; | ||||
| unsigned int length = 0; | ||||
| rtems_status_code status; | ||||
| unsigned int sid; | ||||
|
paul
|
r196 | float *spectral_matrix; | ||
|
paul
|
r172 | int coarseTime; | ||
| int fineTime; | ||||
| spw_ioctl_pkt_send spw_ioctl_send_ASM; | ||||
| sid = ring_node_to_send->sid; | ||||
|
paul
|
r196 | spectral_matrix = (float*) ring_node_to_send->buffer_address; | ||
|
paul
|
r172 | coarseTime = ring_node_to_send->coarseTime; | ||
| fineTime = ring_node_to_send->fineTime; | ||||
|
paul
|
r283 | header->pa_bia_status_info = pa_bia_status_info; | ||
|
paul
|
r192 | header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | ||
|
paul
|
r196 | for (i=0; i<3; i++) | ||
|
paul
|
r172 | { | ||
|
paul
|
r196 | if ((i==0) || (i==1)) | ||
|
paul
|
r172 | { | ||
|
paul
|
r196 | spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_1; | ||
| spw_ioctl_send_ASM.data = (char *) &spectral_matrix[ | ||||
| ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM ) | ||||
|
paul
|
r172 | ]; | ||
|
paul
|
r196 | length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_1; | ||
|
paul
|
r192 | header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; | ||
|
paul
|
r196 | header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_1) >> 8 ); // BLK_NR MSB | ||
| header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_1); // BLK_NR LSB | ||||
| } | ||||
| else | ||||
| { | ||||
| spw_ioctl_send_ASM.dlen = DLEN_ASM_F0_PKT_2; | ||||
| spw_ioctl_send_ASM.data = (char*) &spectral_matrix[ | ||||
| ( (ASM_F0_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F0_1) ) * NB_VALUES_PER_SM ) | ||||
|
paul
|
r172 | ]; | ||
|
paul
|
r196 | length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F0_2; | ||
| header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; | ||||
| header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F0_2) >> 8 ); // BLK_NR MSB | ||||
| header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F0_2); // BLK_NR LSB | ||||
|
paul
|
r172 | } | ||
|
paul
|
r196 | |||
| spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM; | ||||
|
paul
|
r172 | spw_ioctl_send_ASM.hdr = (char *) header; | ||
| spw_ioctl_send_ASM.options = 0; | ||||
| // (2) BUILD THE HEADER | ||||
| increment_seq_counter_source_id( header->packetSequenceControl, sid ); | ||||
| header->packetLength[0] = (unsigned char) (length>>8); | ||||
| header->packetLength[1] = (unsigned char) (length); | ||||
| header->sid = (unsigned char) sid; // SID | ||||
|
paul
|
r196 | header->pa_lfr_pkt_cnt_asm = 3; | ||
| header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); | ||||
| // (3) SET PACKET TIME | ||||
| header->time[0] = (unsigned char) (coarseTime>>24); | ||||
| header->time[1] = (unsigned char) (coarseTime>>16); | ||||
| header->time[2] = (unsigned char) (coarseTime>>8); | ||||
| header->time[3] = (unsigned char) (coarseTime); | ||||
| header->time[4] = (unsigned char) (fineTime>>8); | ||||
| header->time[5] = (unsigned char) (fineTime); | ||||
| // | ||||
| header->acquisitionTime[0] = header->time[0]; | ||||
| header->acquisitionTime[1] = header->time[1]; | ||||
| header->acquisitionTime[2] = header->time[2]; | ||||
| header->acquisitionTime[3] = header->time[3]; | ||||
| header->acquisitionTime[4] = header->time[4]; | ||||
| header->acquisitionTime[5] = header->time[5]; | ||||
| // (4) SEND PACKET | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); | ||||
| if (status != RTEMS_SUCCESSFUL) { | ||||
|
paul
|
r227 | PRINTF1("in ASM_send *** ERR %d\n", (int) status) | ||
|
paul
|
r196 | } | ||
| } | ||||
| } | ||||
| void spw_send_asm_f1( ring_node *ring_node_to_send, | ||||
| Header_TM_LFR_SCIENCE_ASM_t *header ) | ||||
| { | ||||
| unsigned int i; | ||||
| unsigned int length = 0; | ||||
| rtems_status_code status; | ||||
| unsigned int sid; | ||||
| float *spectral_matrix; | ||||
| int coarseTime; | ||||
| int fineTime; | ||||
| spw_ioctl_pkt_send spw_ioctl_send_ASM; | ||||
| sid = ring_node_to_send->sid; | ||||
| spectral_matrix = (float*) ring_node_to_send->buffer_address; | ||||
| coarseTime = ring_node_to_send->coarseTime; | ||||
| fineTime = ring_node_to_send->fineTime; | ||||
|
paul
|
r283 | header->pa_bia_status_info = pa_bia_status_info; | ||
|
paul
|
r196 | header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | ||
| for (i=0; i<3; i++) | ||||
| { | ||||
| if ((i==0) || (i==1)) | ||||
| { | ||||
| spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_1; | ||||
| spw_ioctl_send_ASM.data = (char *) &spectral_matrix[ | ||||
| ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM ) | ||||
| ]; | ||||
| length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_1; | ||||
| header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; | ||||
| header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_1) >> 8 ); // BLK_NR MSB | ||||
| header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_1); // BLK_NR LSB | ||||
| } | ||||
| else | ||||
| { | ||||
| spw_ioctl_send_ASM.dlen = DLEN_ASM_F1_PKT_2; | ||||
| spw_ioctl_send_ASM.data = (char*) &spectral_matrix[ | ||||
| ( (ASM_F1_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F1_1) ) * NB_VALUES_PER_SM ) | ||||
| ]; | ||||
| length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F1_2; | ||||
| header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_6; | ||||
| header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F1_2) >> 8 ); // BLK_NR MSB | ||||
| header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F1_2); // BLK_NR LSB | ||||
| } | ||||
| spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM; | ||||
| spw_ioctl_send_ASM.hdr = (char *) header; | ||||
| spw_ioctl_send_ASM.options = 0; | ||||
| // (2) BUILD THE HEADER | ||||
| increment_seq_counter_source_id( header->packetSequenceControl, sid ); | ||||
| header->packetLength[0] = (unsigned char) (length>>8); | ||||
| header->packetLength[1] = (unsigned char) (length); | ||||
| header->sid = (unsigned char) sid; // SID | ||||
| header->pa_lfr_pkt_cnt_asm = 3; | ||||
| header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); | ||||
| // (3) SET PACKET TIME | ||||
| header->time[0] = (unsigned char) (coarseTime>>24); | ||||
| header->time[1] = (unsigned char) (coarseTime>>16); | ||||
| header->time[2] = (unsigned char) (coarseTime>>8); | ||||
| header->time[3] = (unsigned char) (coarseTime); | ||||
| header->time[4] = (unsigned char) (fineTime>>8); | ||||
| header->time[5] = (unsigned char) (fineTime); | ||||
| // | ||||
| header->acquisitionTime[0] = header->time[0]; | ||||
| header->acquisitionTime[1] = header->time[1]; | ||||
| header->acquisitionTime[2] = header->time[2]; | ||||
| header->acquisitionTime[3] = header->time[3]; | ||||
| header->acquisitionTime[4] = header->time[4]; | ||||
| header->acquisitionTime[5] = header->time[5]; | ||||
| // (4) SEND PACKET | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); | ||||
| if (status != RTEMS_SUCCESSFUL) { | ||||
|
paul
|
r227 | PRINTF1("in ASM_send *** ERR %d\n", (int) status) | ||
|
paul
|
r196 | } | ||
| } | ||||
| } | ||||
| void spw_send_asm_f2( ring_node *ring_node_to_send, | ||||
| Header_TM_LFR_SCIENCE_ASM_t *header ) | ||||
| { | ||||
| unsigned int i; | ||||
| unsigned int length = 0; | ||||
| rtems_status_code status; | ||||
| unsigned int sid; | ||||
| float *spectral_matrix; | ||||
| int coarseTime; | ||||
| int fineTime; | ||||
| spw_ioctl_pkt_send spw_ioctl_send_ASM; | ||||
| sid = ring_node_to_send->sid; | ||||
| spectral_matrix = (float*) ring_node_to_send->buffer_address; | ||||
| coarseTime = ring_node_to_send->coarseTime; | ||||
| fineTime = ring_node_to_send->fineTime; | ||||
|
paul
|
r283 | header->pa_bia_status_info = pa_bia_status_info; | ||
|
paul
|
r196 | header->sy_lfr_common_parameters = parameter_dump_packet.sy_lfr_common_parameters; | ||
| for (i=0; i<3; i++) | ||||
| { | ||||
| spw_ioctl_send_ASM.dlen = DLEN_ASM_F2_PKT; | ||||
| spw_ioctl_send_ASM.data = (char *) &spectral_matrix[ | ||||
| ( (ASM_F2_INDICE_START + (i*NB_BINS_PER_PKT_ASM_F2) ) * NB_VALUES_PER_SM ) | ||||
| ]; | ||||
| length = PACKET_LENGTH_TM_LFR_SCIENCE_ASM_F2; | ||||
| header->serviceSubType = TM_SUBTYPE_LFR_SCIENCE_3; | ||||
| header->pa_lfr_asm_blk_nr[0] = (unsigned char) ( (NB_BINS_PER_PKT_ASM_F2) >> 8 ); // BLK_NR MSB | ||||
| header->pa_lfr_asm_blk_nr[1] = (unsigned char) (NB_BINS_PER_PKT_ASM_F2); // BLK_NR LSB | ||||
| spw_ioctl_send_ASM.hlen = HEADER_LENGTH_TM_LFR_SCIENCE_ASM; | ||||
| spw_ioctl_send_ASM.hdr = (char *) header; | ||||
| spw_ioctl_send_ASM.options = 0; | ||||
| // (2) BUILD THE HEADER | ||||
| increment_seq_counter_source_id( header->packetSequenceControl, sid ); | ||||
| header->packetLength[0] = (unsigned char) (length>>8); | ||||
| header->packetLength[1] = (unsigned char) (length); | ||||
| header->sid = (unsigned char) sid; // SID | ||||
| header->pa_lfr_pkt_cnt_asm = 3; | ||||
|
paul
|
r172 | header->pa_lfr_pkt_nr_asm = (unsigned char) (i+1); | ||
| // (3) SET PACKET TIME | ||||
| header->time[0] = (unsigned char) (coarseTime>>24); | ||||
| header->time[1] = (unsigned char) (coarseTime>>16); | ||||
| header->time[2] = (unsigned char) (coarseTime>>8); | ||||
| header->time[3] = (unsigned char) (coarseTime); | ||||
| header->time[4] = (unsigned char) (fineTime>>8); | ||||
| header->time[5] = (unsigned char) (fineTime); | ||||
| // | ||||
| header->acquisitionTime[0] = header->time[0]; | ||||
| header->acquisitionTime[1] = header->time[1]; | ||||
| header->acquisitionTime[2] = header->time[2]; | ||||
| header->acquisitionTime[3] = header->time[3]; | ||||
| header->acquisitionTime[4] = header->time[4]; | ||||
| header->acquisitionTime[5] = header->time[5]; | ||||
| // (4) SEND PACKET | ||||
| status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, &spw_ioctl_send_ASM ); | ||||
| if (status != RTEMS_SUCCESSFUL) { | ||||
|
paul
|
r227 | PRINTF1("in ASM_send *** ERR %d\n", (int) status) | ||
|
paul
|
r172 | } | ||
| } | ||||
| } | ||||
|
paul
|
r194 | |||
| void spw_send_k_dump( ring_node *ring_node_to_send ) | ||||
| { | ||||
| rtems_status_code status; | ||||
| Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *kcoefficients_dump; | ||||
| unsigned int packetLength; | ||||
| unsigned int size; | ||||
|
paul
|
r227 | PRINTF("spw_send_k_dump\n") | ||
|
paul
|
r194 | |||
| kcoefficients_dump = (Packet_TM_LFR_KCOEFFICIENTS_DUMP_t *) ring_node_to_send->buffer_address; | ||||
| packetLength = kcoefficients_dump->packetLength[0] * 256 + kcoefficients_dump->packetLength[1]; | ||||
| size = packetLength + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES; | ||||
|
paul
|
r227 | PRINTF2("packetLength %d, size %d\n", packetLength, size ) | ||
|
paul
|
r194 | |||
| status = write( fdSPW, (char *) ring_node_to_send->buffer_address, size ); | ||||
| if (status == -1){ | ||||
| PRINTF2("in SEND *** (2.a) ERRNO = %d, size = %d\n", errno, size) | ||||
| } | ||||
| ring_node_to_send->status = 0x00; | ||||
| } | ||||