138 lines
4.7 KiB
C
138 lines
4.7 KiB
C
#include <string.h>
|
|
|
|
#include "x502tstp.h"
|
|
|
|
typedef enum {
|
|
STREAM_IN_WRD_ADC = 0,
|
|
STREAM_IN_WRD_DIN = 1,
|
|
STREAM_IN_WRD_MSG = 2,
|
|
STREAM_IN_WRD_USR = 3,
|
|
STREAM_IN_WRD_TSP = 4,
|
|
} t_stream_in_wrd_type;
|
|
|
|
#define STREAM_IN_WRD_TYPE(wrd) wrd & 0x80000000 ? STREAM_IN_WRD_ADC : \
|
|
(wrd & 0xFF000000) == 0x0 ? STREAM_IN_WRD_DIN : \
|
|
(wrd & 0xF0000000) == 0x40000000 ? STREAM_IN_WRD_TSP : \
|
|
((wrd & 0xFF000000)>>24) == 0x01 ? STREAM_IN_WRD_MSG : STREAM_IN_WRD_USR
|
|
|
|
static uint64_t get_secssec(uint32_t wrd1, uint32_t wrd2, uint32_t wrd3) {
|
|
uint64_t wrd1_sec = TSP_WRD1_GET_SSEC(wrd1);
|
|
uint64_t wrd2_sec = TSP_WRD2_GET_SECSSEC(wrd2);
|
|
uint64_t wrd3_sec = TSP_WRD3_GET_SEC(wrd3);
|
|
uint64_t ret;
|
|
|
|
ret = wrd1_sec | wrd2_sec | wrd3_sec;
|
|
return ret;
|
|
//return TSP_WRD3_GET_SEC(wrd3) | TSP_WRD2_GET_SECSSEC(wrd2) | TSP_WRD1_GET_SSEC(wrd1);
|
|
}
|
|
|
|
#if 0
|
|
static int32_t get_ssec(uint32_t wrd1, uint32_t wrd2, uint32_t wrd3) {
|
|
int32_t ret;
|
|
ret = (TSP_WRD2_GET_SSEC(wrd2) << TSP_WRD1_SSEC_LEN) | TSP_WRD1_GET_SSEC(wrd1);
|
|
if(ret < 0) {
|
|
fprintf(stderr, "unexpected get_ssec ret=%d\n", ret);
|
|
while(1);
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
static uint32_t get_sec(uint32_t wrd2) {
|
|
return ((wrd2 & TSP_WRD2_SEC_MASK) >> TSP_WRD2_SSEC_LEN);
|
|
}
|
|
|
|
#endif
|
|
|
|
X502_EXPORT(bool) X502_tstp_get_lock(t_x502_tstp_state *tstp_state) {
|
|
if (TSP_WRD1_IS_LOCK(tstp_state->wrd[1])) {
|
|
return true;
|
|
} else {
|
|
return false;
|
|
}
|
|
}
|
|
|
|
X502_EXPORT(void) X502_tstp_process_wrd(t_x502_tstp_state *tstp_state, uint32_t wrd) {
|
|
t_stream_in_wrd_type type = STREAM_IN_WRD_TYPE(wrd);
|
|
if (type == STREAM_IN_WRD_TSP) {
|
|
if (TSP_WRD_NUM(wrd) == 3) {
|
|
uint64_t cur_tspt_time;
|
|
|
|
tstp_state->wrd[3] = wrd;
|
|
|
|
cur_tspt_time = get_secssec(tstp_state->wrd[1], tstp_state->wrd[2], tstp_state->wrd[3]);
|
|
|
|
if (TSP_WRD1_IS_FMARK(tstp_state->wrd[1])) {
|
|
tstp_state->tstp_start_time = cur_tspt_time;
|
|
#ifdef DEBUG
|
|
if (tstp_state->tstp_mark_rcvd) {
|
|
fprintf(stderr, "fmark && tstp_mark_rcv unexpected!\n");
|
|
}
|
|
#endif
|
|
tstp_state->tstp_mark_rcvd = true;
|
|
}
|
|
tstp_state->adcwrds_after_tstp = 0;
|
|
tstp_state->dinwrds_after_tstp = 0;
|
|
tstp_state->wrds_after_tstp = 0;
|
|
|
|
if (cur_tspt_time < tstp_state->last_tstp_time) {
|
|
#ifdef DEBUG
|
|
fprintf(stderr, "cur = %" PRIu64 " last = %" PRIu64 "\n", cur_tspt_time, tstp_state->last_tstp_time);
|
|
fprintf(stderr, "cur_tstp < last_tstp unexpected!\n");
|
|
#endif
|
|
return;
|
|
}
|
|
|
|
//printf("tstp time = %d.%09d\n", TSTP_SECSSEC_TO_SEC(cur_tspt_time), TSTP_SSEC_TO_NSEC(cur_tspt_time));
|
|
|
|
tstp_state->last_tstp_time = cur_tspt_time;
|
|
} else {
|
|
if (TSP_WRD_NUM(wrd) == 0) {
|
|
tstp_state->wrd[0] = wrd;
|
|
} else
|
|
if (TSP_WRD_NUM(wrd) == 1) {
|
|
tstp_state->wrd[1] = wrd;
|
|
} else
|
|
if (TSP_WRD_NUM(wrd) == 2) {
|
|
tstp_state->wrd[2] = wrd;
|
|
}
|
|
tstp_state->wrds_after_tstp++;
|
|
}
|
|
} else {
|
|
if (type == STREAM_IN_WRD_DIN) {
|
|
tstp_state->dinwrds_after_tstp++;
|
|
} else
|
|
if (type == STREAM_IN_WRD_ADC) {
|
|
tstp_state->adcwrds_after_tstp++;
|
|
}
|
|
tstp_state->wrds_after_tstp++;
|
|
}
|
|
tstp_state->processed_wrds++;
|
|
tstp_state->cur_wrd = wrd;
|
|
}
|
|
|
|
X502_EXPORT(void) X502_tstp_init(t_x502_tstp_state *tstp_state, uint32_t adc_freq, uint32_t din_freq) {
|
|
memset(tstp_state, 0, sizeof(*tstp_state));
|
|
|
|
tstp_state->adc_freq = adc_freq;
|
|
tstp_state->din_freq = din_freq;
|
|
}
|
|
|
|
X502_EXPORT(void) X502_tstp_get_curwrd_time(t_x502_tstp_state *tstp_state, t_x502_tstptime *ret) {
|
|
t_x502_tstptime wrd_time;
|
|
t_stream_in_wrd_type type = STREAM_IN_WRD_TYPE(tstp_state->cur_wrd);
|
|
|
|
if (type == STREAM_IN_WRD_DIN) {
|
|
wrd_time = tstp_state->last_tstp_time + ((double)(tstp_state->dinwrds_after_tstp - 1) / tstp_state->din_freq) * (1U << 31);
|
|
} else
|
|
if (type == STREAM_IN_WRD_ADC) {
|
|
wrd_time = tstp_state->last_tstp_time + ((double)(tstp_state->adcwrds_after_tstp - 1)/ tstp_state->adc_freq) * (1U << 31);
|
|
} else {
|
|
// чтобы посчитать время слова отличного от АЦП и DIN нужно знать частоту их появления
|
|
wrd_time = tstp_state->last_tstp_time + ((double)(tstp_state->adcwrds_after_tstp - 1)/ tstp_state->adc_freq) * (1U << 31);
|
|
wrd_time += tstp_state->last_tstp_time + ((double)(tstp_state->dinwrds_after_tstp - 1) / tstp_state->din_freq) * (1U << 31);
|
|
}
|
|
if (ret) {
|
|
*ret = wrd_time;
|
|
}
|
|
}
|