fsw_misc.c
313 lines
| 12.4 KiB
| text/x-c
|
CLexer
/ src / fsw_misc.c
paul@pc-solar1.lab-lpp.local
|
r5 | #include <fsw_misc.h> | ||
#include <fsw_params.h> | ||||
int configure_timer(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider, | ||||
unsigned char interrupt_level, rtems_isr (*timer_isr)() ) | ||||
{ // configure the timer for the waveforms simulation | ||||
paul@pc-solar1.lab-lpp.local
|
r21 | rtems_status_code status; | ||
paul@pc-solar1.lab-lpp.local
|
r5 | rtems_isr_entry old_isr_handler; | ||
paul@pc-solar1.lab-lpp.local
|
r21 | status = rtems_interrupt_catch( timer_isr, interrupt_level, &old_isr_handler) ; // see sparcv8.pdf p.76 for interrupt levels | ||
paul
|
r35 | if (status!=RTEMS_SUCCESSFUL) | ||
paul
|
r31 | { | ||
paul
|
r35 | PRINTF("in configure_timer *** ERR rtems_interrupt_catch\n") | ||
paul
|
r31 | } | ||
paul@pc-solar1.lab-lpp.local
|
r5 | |||
paul
|
r31 | timer_set_clock_divider( gptimer_regs, timer, clock_divider); | ||
return 1; | ||||
} | ||||
int timer_start(gptimer_regs_t *gptimer_regs, unsigned char timer) | ||||
{ | ||||
paul@pc-solar1.lab-lpp.local
|
r5 | gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any | ||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000004; // LD load value from the reload register | ||||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000001; // EN enable the timer | ||||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000002; // RS restart | ||||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000008; // IE interrupt enable | ||||
return 1; | ||||
} | ||||
paul
|
r31 | int timer_stop(gptimer_regs_t *gptimer_regs, unsigned char timer) | ||
{ | ||||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xfffffffe; // EN enable the timer | ||||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl & 0xffffffef; // IE interrupt enable | ||||
gptimer_regs->timer[timer].ctrl = gptimer_regs->timer[timer].ctrl | 0x00000010; // clear pending IRQ if any | ||||
return 1; | ||||
} | ||||
int timer_set_clock_divider(gptimer_regs_t *gptimer_regs, unsigned char timer, unsigned int clock_divider) | ||||
{ | ||||
gptimer_regs->timer[timer].reload = clock_divider; // base clock frequency is 1 MHz | ||||
return 1; | ||||
} | ||||
paul@pc-solar1.lab-lpp.local
|
r23 | void update_spacewire_statistics() | ||
paul@pc-solar1.lab-lpp.local
|
r21 | { | ||
paul@pc-solar1.lab-lpp.local
|
r23 | rtems_status_code status; | ||
spw_stats spacewire_stats_grspw; | ||||
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_GET_STATISTICS, &spacewire_stats_grspw ); | ||||
spacewire_stats.packets_received = spacewire_stats_backup.packets_received | ||||
+ spacewire_stats_grspw.packets_received; | ||||
spacewire_stats.packets_sent = spacewire_stats_backup.packets_sent | ||||
+ spacewire_stats_grspw.packets_sent; | ||||
spacewire_stats.parity_err = spacewire_stats_backup.parity_err | ||||
+ spacewire_stats_grspw.parity_err; | ||||
spacewire_stats.disconnect_err = spacewire_stats_backup.disconnect_err | ||||
+ spacewire_stats_grspw.disconnect_err; | ||||
spacewire_stats.escape_err = spacewire_stats_backup.escape_err | ||||
+ spacewire_stats_grspw.escape_err; | ||||
spacewire_stats.credit_err = spacewire_stats_backup.credit_err | ||||
+ spacewire_stats_grspw.credit_err; | ||||
spacewire_stats.write_sync_err = spacewire_stats_backup.write_sync_err | ||||
+ spacewire_stats_grspw.write_sync_err; | ||||
spacewire_stats.rx_rmap_header_crc_err = spacewire_stats_backup.rx_rmap_header_crc_err | ||||
+ spacewire_stats_grspw.rx_rmap_header_crc_err; | ||||
spacewire_stats.rx_rmap_data_crc_err = spacewire_stats_backup.rx_rmap_data_crc_err | ||||
+ spacewire_stats_grspw.rx_rmap_data_crc_err; | ||||
spacewire_stats.early_ep = spacewire_stats_backup.early_ep | ||||
+ spacewire_stats_grspw.early_ep; | ||||
spacewire_stats.invalid_address = spacewire_stats_backup.invalid_address | ||||
+ spacewire_stats_grspw.invalid_address; | ||||
spacewire_stats.rx_eep_err = spacewire_stats_backup.rx_eep_err | ||||
+ spacewire_stats_grspw.rx_eep_err; | ||||
spacewire_stats.rx_truncated = spacewire_stats_backup.rx_truncated | ||||
+ spacewire_stats_grspw.rx_truncated; | ||||
//spacewire_stats.tx_link_err; | ||||
//**************************** | ||||
// DPU_SPACEWIRE_IF_STATISTICS | ||||
housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[0] = (unsigned char) (spacewire_stats.packets_received >> 8); | ||||
housekeeping_packet.hk_lfr_dpu_spw_pkt_rcv_cnt[1] = (unsigned char) (spacewire_stats.packets_received); | ||||
housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[0] = (unsigned char) (spacewire_stats.packets_sent >> 8); | ||||
housekeeping_packet.hk_lfr_dpu_spw_pkt_sent_cnt[1] = (unsigned char) (spacewire_stats.packets_sent); | ||||
//housekeeping_packet.hk_lfr_dpu_spw_tick_out_cnt; | ||||
//housekeeping_packet.hk_lfr_dpu_spw_last_timc; | ||||
//****************************************** | ||||
// ERROR COUNTERS / SPACEWIRE / LOW SEVERITY | ||||
housekeeping_packet.hk_lfr_dpu_spw_parity = (unsigned char) spacewire_stats.parity_err; | ||||
housekeeping_packet.hk_lfr_dpu_spw_disconnect = (unsigned char) spacewire_stats.disconnect_err; | ||||
housekeeping_packet.hk_lfr_dpu_spw_escape = (unsigned char) spacewire_stats.escape_err; | ||||
housekeeping_packet.hk_lfr_dpu_spw_credit = (unsigned char) spacewire_stats.credit_err; | ||||
housekeeping_packet.hk_lfr_dpu_spw_write_sync = (unsigned char) spacewire_stats.write_sync_err; | ||||
// housekeeping_packet.hk_lfr_dpu_spw_rx_ahb; | ||||
// housekeeping_packet.hk_lfr_dpu_spw_tx_ahb; | ||||
housekeeping_packet.hk_lfr_dpu_spw_header_crc = (unsigned char) spacewire_stats.rx_rmap_header_crc_err; | ||||
housekeeping_packet.hk_lfr_dpu_spw_data_crc = (unsigned char) spacewire_stats.rx_rmap_data_crc_err; | ||||
//********************************************* | ||||
// ERROR COUNTERS / SPACEWIRE / MEDIUM SEVERITY | ||||
housekeeping_packet.hk_lfr_dpu_spw_early_eop = (unsigned char) spacewire_stats.early_ep; | ||||
housekeeping_packet.hk_lfr_dpu_spw_invalid_addr = (unsigned char) spacewire_stats.invalid_address; | ||||
housekeeping_packet.hk_lfr_dpu_spw_eep = (unsigned char) spacewire_stats.rx_eep_err; | ||||
housekeeping_packet.hk_lfr_dpu_spw_rx_too_big = (unsigned char) spacewire_stats.rx_truncated; | ||||
paul@pc-solar1.lab-lpp.local
|
r5 | } | ||
paul@pc-solar1.lab-lpp.local
|
r25 | int send_console_outputs_on_apbuart_port( void ) // Send the console outputs on the apbuart port | ||
paul@pc-solar1.lab-lpp.local
|
r5 | { | ||
paul@pc-solar1.lab-lpp.local
|
r18 | struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) REGS_ADDR_APBUART; | ||
paul@pc-solar1.lab-lpp.local
|
r21 | |||
apbuart_regs->ctrl = apbuart_regs->ctrl & APBUART_CTRL_REG_MASK_DB; | ||||
PRINTF("\n\n\n\n\nIn INIT *** Now the console is on port COM1\n") | ||||
paul@pc-solar1.lab-lpp.local
|
r5 | |||
return 0; | ||||
} | ||||
paul@pc-solar1.lab-lpp.local
|
r17 | int set_apbuart_scaler_reload_register(unsigned int regs, unsigned int value) | ||
{ | ||||
paul@pc-solar1.lab-lpp.local
|
r18 | struct apbuart_regs_str *apbuart_regs = (struct apbuart_regs_str *) regs; | ||
paul@pc-solar1.lab-lpp.local
|
r21 | |||
apbuart_regs->scaler = value; | ||||
paul
|
r35 | BOOT_PRINTF1("OK *** apbuart port scaler reload register set to 0x%x\n", value) | ||
paul@pc-solar1.lab-lpp.local
|
r17 | |||
return 0; | ||||
} | ||||
//************ | ||||
// RTEMS TASKS | ||||
paul@pc-solar1.lab-lpp.local
|
r9 | rtems_task stat_task(rtems_task_argument argument) | ||
paul@pc-solar1.lab-lpp.local
|
r21 | { | ||
paul@pc-solar1.lab-lpp.local
|
r17 | int i; | ||
paul@pc-solar1.lab-lpp.local
|
r21 | int j; | ||
paul@pc-solar1.lab-lpp.local
|
r17 | i = 0; | ||
paul@pc-solar1.lab-lpp.local
|
r21 | j = 0; | ||
paul
|
r35 | BOOT_PRINTF("in STAT *** \n") | ||
paul@pc-solar1.lab-lpp.local
|
r21 | while(1){ | ||
rtems_task_wake_after(1000); | ||||
PRINTF1("%d\n", j) | ||||
paul
|
r33 | if (i == CPU_USAGE_REPORT_PERIOD) { | ||
// #ifdef PRINT_TASK_STATISTICS | ||||
// rtems_cpu_usage_report(); | ||||
// rtems_cpu_usage_reset(); | ||||
// #endif | ||||
paul@pc-solar1.lab-lpp.local
|
r21 | i = 0; | ||
} | ||||
paul@pc-solar1.lab-lpp.local
|
r17 | else i++; | ||
paul@pc-solar1.lab-lpp.local
|
r21 | j++; | ||
} | ||||
paul@pc-solar1.lab-lpp.local
|
r5 | } | ||
paul@pc-solar1.lab-lpp.local
|
r21 | |||
rtems_task hous_task(rtems_task_argument argument) | ||||
paul@pc-solar1.lab-lpp.local
|
r17 | { | ||
paul@pc-solar1.lab-lpp.local
|
r18 | rtems_status_code status; | ||
paul
|
r33 | spw_ioctl_pkt_send spw_ioctl_send; | ||
paul
|
r35 | rtems_id queue_id; | ||
paul
|
r33 | |||
spw_ioctl_send.hlen = 0; | ||||
spw_ioctl_send.hdr = NULL; | ||||
paul
|
r34 | spw_ioctl_send.dlen = PACKET_LENGTH_HK + CCSDS_TC_TM_PACKET_OFFSET + CCSDS_PROTOCOLE_EXTRA_BYTES; | ||
paul
|
r33 | spw_ioctl_send.data = (char*) &housekeeping_packet; | ||
spw_ioctl_send.options = 0; | ||||
paul@pc-solar1.lab-lpp.local
|
r21 | |||
paul
|
r35 | status = rtems_message_queue_ident( misc_name[QUEUE_PKTS], 0, &queue_id ); | ||
if (status != RTEMS_SUCCESSFUL) | ||||
{ | ||||
PRINTF1("in HOUS *** ERR %d\n", status) | ||||
} | ||||
BOOT_PRINTF("in HOUS ***\n") | ||||
paul@pc-solar1.lab-lpp.local
|
r17 | |||
paul@pc-solar1.lab-lpp.local
|
r21 | if (rtems_rate_monotonic_ident( HK_name, &HK_id) != RTEMS_SUCCESSFUL) { | ||
status = rtems_rate_monotonic_create( HK_name, &HK_id ); | ||||
paul@pc-solar1.lab-lpp.local
|
r18 | if( status != RTEMS_SUCCESSFUL ) { | ||
PRINTF1( "rtems_rate_monotonic_create failed with status of %d\n", status ) | ||||
} | ||||
paul@pc-solar1.lab-lpp.local
|
r17 | } | ||
paul@pc-solar1.lab-lpp.local
|
r21 | housekeeping_packet.targetLogicalAddress = CCSDS_DESTINATION_ID; | ||
paul@pc-solar1.lab-lpp.local
|
r23 | housekeeping_packet.protocolIdentifier = CCSDS_PROTOCOLE_ID; | ||
paul
|
r33 | housekeeping_packet.reserved = DEFAULT_RESERVED; | ||
housekeeping_packet.userApplication = CCSDS_USER_APP; | ||||
paul@pc-solar1.lab-lpp.local
|
r23 | housekeeping_packet.packetID[0] = (unsigned char) (TM_PACKET_ID_HK >> 8); | ||
housekeeping_packet.packetID[1] = (unsigned char) (TM_PACKET_ID_HK); | ||||
paul
|
r33 | housekeeping_packet.packetSequenceControl[0] = TM_PACKET_SEQ_CTRL_STANDALONE; | ||
housekeeping_packet.packetSequenceControl[1] = TM_PACKET_SEQ_CNT_DEFAULT; | ||||
housekeeping_packet.packetLength[0] = (unsigned char) (PACKET_LENGTH_HK >> 8); | ||||
housekeeping_packet.packetLength[1] = (unsigned char) (PACKET_LENGTH_HK ); | ||||
housekeeping_packet.spare1_pusVersion_spare2 = DEFAULT_SPARE1_PUSVERSION_SPARE2; | ||||
housekeeping_packet.serviceType = TM_TYPE_HK; | ||||
housekeeping_packet.serviceSubType = TM_SUBTYPE_HK; | ||||
housekeeping_packet.destinationID = TM_DESTINATION_ID_GROUND; | ||||
paul@pc-solar1.lab-lpp.local
|
r17 | |||
status = rtems_rate_monotonic_cancel(HK_id); | ||||
paul@pc-solar1.lab-lpp.local
|
r23 | if( status != RTEMS_SUCCESSFUL ) { | ||
PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_cancel(HK_id) ***code: %d\n", status ) | ||||
} | ||||
else { | ||||
paul
|
r35 | DEBUG_PRINTF("OK *** in HOUS *** rtems_rate_monotonic_cancel(HK_id)\n") | ||
paul@pc-solar1.lab-lpp.local
|
r23 | } | ||
paul@pc-solar1.lab-lpp.local
|
r21 | |||
paul@pc-solar1.lab-lpp.local
|
r17 | while(1){ // launch the rate monotonic task | ||
paul@pc-solar1.lab-lpp.local
|
r21 | status = rtems_rate_monotonic_period( HK_id, HK_PERIOD ); | ||
paul@pc-solar1.lab-lpp.local
|
r23 | if ( status != RTEMS_SUCCESSFUL ) { | ||
paul@pc-solar1.lab-lpp.local
|
r21 | PRINTF1( "ERR *** in HOUS *** rtems_rate_monotonic_period *** code %d\n", status); | ||
paul@pc-solar1.lab-lpp.local
|
r17 | } | ||
paul@pc-solar1.lab-lpp.local
|
r23 | else { | ||
paul
|
r33 | housekeeping_packet.time[0] = (unsigned char) (time_management_regs->coarse_time>>24); | ||
housekeeping_packet.time[1] = (unsigned char) (time_management_regs->coarse_time>>16); | ||||
housekeeping_packet.time[2] = (unsigned char) (time_management_regs->coarse_time>>8); | ||||
housekeeping_packet.time[3] = (unsigned char) (time_management_regs->coarse_time); | ||||
housekeeping_packet.time[4] = (unsigned char) (time_management_regs->fine_time>>8); | ||||
housekeeping_packet.time[5] = (unsigned char) (time_management_regs->fine_time); | ||||
paul@pc-solar1.lab-lpp.local
|
r23 | housekeeping_packet.sid = SID_HK; | ||
update_spacewire_statistics(); | ||||
paul
|
r31 | // SEND PACKET | ||
paul
|
r35 | status = rtems_message_queue_send( queue_id, &spw_ioctl_send, ACTION_MSG_PKTS_SIZE); | ||
paul
|
r33 | if (status != RTEMS_SUCCESSFUL) { | ||
paul
|
r35 | PRINTF1("in HOUS *** ERR %d\n", status) | ||
paul@pc-solar1.lab-lpp.local
|
r17 | } | ||
paul@pc-solar1.lab-lpp.local
|
r21 | } | ||
} | ||||
paul@pc-solar1.lab-lpp.local
|
r5 | |||
paul@pc-solar1.lab-lpp.local
|
r17 | PRINTF("in HOUS *** deleting task\n") | ||
paul@pc-solar1.lab-lpp.local
|
r21 | |||
status = rtems_task_delete( RTEMS_SELF ); // should not return | ||||
printf( "rtems_task_delete returned with status of %d.\n", status ); | ||||
exit( 1 ); | ||||
paul@pc-solar1.lab-lpp.local
|
r16 | } | ||
paul
|
r33 | rtems_task send_task( rtems_task_argument argument) | ||
{ | ||||
rtems_status_code status; // RTEMS status code | ||||
paul
|
r34 | spw_ioctl_pkt_send spw_ioctl_send; // incoming spw_ioctl_pkt_send structure | ||
paul
|
r33 | size_t size; // size of the incoming TC packet | ||
paul
|
r34 | u_int32_t count; | ||
paul
|
r35 | rtems_id queue_id; | ||
paul
|
r33 | |||
paul
|
r35 | status = rtems_message_queue_ident( misc_name[QUEUE_PKTS], 0, &queue_id ); | ||
if (status != RTEMS_SUCCESSFUL) | ||||
{ | ||||
PRINTF1("in SEND *** ERR getting queue id, %d\n", status) | ||||
} | ||||
BOOT_PRINTF("in SEND *** \n") | ||||
paul
|
r33 | |||
while(1) | ||||
{ | ||||
paul
|
r35 | status = rtems_message_queue_receive( queue_id, (char*) &spw_ioctl_send, &size, | ||
RTEMS_WAIT, RTEMS_NO_TIMEOUT ); | ||||
paul
|
r34 | if (status!=RTEMS_SUCCESSFUL) | ||
{ | ||||
paul
|
r35 | PRINTF1("in SEND *** (1) ERR = %d\n", status) | ||
paul
|
r34 | } | ||
paul
|
r33 | else | ||
{ | ||||
paul
|
r35 | if (spw_ioctl_send.hlen == 0) | ||
{ | ||||
status = write( fdSPW, spw_ioctl_send.data, spw_ioctl_send.dlen ); | ||||
if (status == -1){ | ||||
PRINTF2("in SEND *** (2.a) ERR = %d, dlen = %d\n", status, spw_ioctl_send.dlen) | ||||
} | ||||
} | ||||
else | ||||
{ | ||||
status = ioctl( fdSPW, SPACEWIRE_IOCTRL_SEND, spw_ioctl_send ); | ||||
if (status == -1){ | ||||
PRINTF2("in SEND *** (2.b) ERR = %d, dlen = %d\n", status, spw_ioctl_send.dlen) | ||||
PRINTF1(" hlen = %d\n", spw_ioctl_send.hlen) | ||||
} | ||||
paul
|
r33 | } | ||
} | ||||
paul
|
r34 | |||
paul
|
r35 | status = rtems_message_queue_get_number_pending( queue_id, &count ); | ||
paul
|
r34 | if (status != RTEMS_SUCCESSFUL) | ||
{ | ||||
paul
|
r35 | PRINTF1("in SEND *** (3) ERR = %d\n", status) | ||
paul
|
r34 | } | ||
else | ||||
{ | ||||
if (count > maxCount) | ||||
{ | ||||
maxCount = count; | ||||
} | ||||
} | ||||
paul
|
r33 | } | ||
} | ||||
paul
|
r34 | |||
paul
|
r35 | rtems_id get_pkts_queue_id( void ) | ||
{ | ||||
rtems_id queue_id; | ||||
rtems_status_code status; | ||||
status = rtems_message_queue_ident( misc_name[QUEUE_PKTS], 0, &queue_id ); | ||||
if (status != RTEMS_SUCCESSFUL) | ||||
{ | ||||
PRINTF1("in get_pkts_queue_id *** ERR %d\n", status) | ||||
} | ||||
return queue_id; | ||||
} | ||||
paul
|
r34 | |||