/*
* refclock_process - process a sample from the clock
* refclock_process_f - refclock_process with other than time1 fudge
*
* This routine converts the timecode in the form days, hours, minutes,
* seconds and milliseconds/microseconds to internal timestamp format,
* then constructs a new entry in the median filter circular buffer.
* Return success (1) if the data are correct and consistent with the
* converntional calendar.
*
* Important for PPS users: Normally, the pp->lastrec is set to the
* system time when the on-time character is received and the pp->year,
* ..., pp->second decoded and the seconds fraction pp->nsec in
* nanoseconds). When a PPS offset is available, pp->nsec is forced to
* zero and the fraction for pp->lastrec is set to the PPS offset.
*/
int
refclock_process_f(
struct refclockproc *pp, /* refclock structure pointer */
double fudge
)
{
l_fp offset, ltemp;
/*
* Compute the timecode timestamp from the days, hours, minutes,
* seconds and milliseconds/microseconds of the timecode. Use
* clocktime() for the aggregate seconds and the msec/usec for
* the fraction, when present. Note that this code relies on the
* filesystem time for the years and does not use the years of
* the timecode.
*/
if (!clocktime(pp->day, pp->hour, pp->minute, pp->second, GMT,
pp->lastrec.l_ui, &pp->yearstart, &offset.l_ui))
return (0);
offset.l_uf = 0;
DTOLFP(pp->nsec / 1e9, <emp);
L_ADD(&offset, <emp);
refclock_process_offset(pp, offset, pp->lastrec, fudge);
return (1);
}
l_fp
l_fp_add(const l_fp first, const l_fp second)
{
l_fp temp = first;
L_ADD(&temp, &second);
return temp;
}
void Pred_lt_6 (
INT16 exc[], /* in/out: excitation buffer */
INT16 T0, /* input : integer pitch lag */
INT16 frac, /* input : fraction of lag */
INT16 L_subfr /* input : subframe size */
)
{
INT16 i, j, k;
INT16 *x0, *x1, *x2;
const INT16 *c1, *c2;
//INT32 s;
register INT32 s_hi=0;
register UINT32 s_lo=0;
VPP_EFR_PROFILE_FUNCTION_ENTER(Pred_lt_6);
x0 = &exc[-T0];
frac = NEGATE(frac);
if (frac < 0)
{
frac = ADD(frac, UP_SAMP);
x0--;
}
for (j = 0; j < L_subfr; j++)
{
x1 = x0++;
x2 = x0;
c1 = &inter_6[frac];
c2 = &inter_6[SUB (UP_SAMP, frac)];
s_lo = 0;
for (i = 0, k = 0; i < L_INTERPOL; i++, k += UP_SAMP)
{
//s = L_MAC(s, x1[-i], c1[k]);
VPP_MLA16(s_hi, s_lo, x1[-i], c1[k]);
//s = L_MAC(s, x2[i], c2[k]);
VPP_MLA16(s_hi, s_lo, x2[i], c2[k]);
}
//exc[j] = round(s);
//exc[j] = ROUND(VPP_SCALE64_TO_16(s_hi, s_lo));
exc[j] = L_SHR_D(L_ADD((INT32)s_lo,0x4000),15);
}
VPP_EFR_PROFILE_FUNCTION_EXIT(Pred_lt_6);
return;
}
/*
* chu_audio_receive - receive data from the audio device
*/
static void
chu_audio_receive(
struct recvbuf *rbufp /* receive buffer structure pointer */
)
{
struct chuunit *up;
struct refclockproc *pp;
struct peer *peer;
double sample; /* codec sample */
u_char *dpt; /* buffer pointer */
int bufcnt; /* buffer counter */
l_fp ltemp; /* l_fp temp */
peer = rbufp->recv_peer;
pp = peer->procptr;
up = pp->unitptr;
/*
* Main loop - read until there ain't no more. Note codec
* samples are bit-inverted.
*/
DTOLFP((double)rbufp->recv_length / SECOND, <emp);
L_SUB(&rbufp->recv_time, <emp);
up->timestamp = rbufp->recv_time;
dpt = rbufp->recv_buffer;
for (bufcnt = 0; bufcnt < rbufp->recv_length; bufcnt++) {
sample = up->comp[~*dpt++ & 0xff];
/*
* Clip noise spikes greater than MAXAMP. If no clips,
* increase the gain a tad; if the clips are too high,
* decrease a tad.
*/
if (sample > MAXAMP) {
sample = MAXAMP;
up->clipcnt++;
} else if (sample < -MAXAMP) {
sample = -MAXAMP;
up->clipcnt++;
}
chu_rf(peer, sample);
L_ADD(&up->timestamp, &up->tick);
/*
* Once each second ride gain.
*/
up->seccnt = (up->seccnt + 1) % SECOND;
if (up->seccnt == 0) {
chu_gain(peer);
}
}
/*
* Set the input port and monitor gain for the next buffer.
*/
if (pp->sloppyclockflag & CLK_FLAG2)
up->port = 2;
else
up->port = 1;
if (pp->sloppyclockflag & CLK_FLAG3)
up->mongain = MONGAIN;
else
up->mongain = 0;
}
/*
* sendrequest - format and send a request packet
*
* Historically, ntpdc has used a fixed-size request packet regardless
* of the actual payload size. When authenticating, the timestamp, key
* ID, and digest have been placed just before the end of the packet.
* With the introduction in late 2009 of support for authenticated
* ntpdc requests using larger 20-octet digests (vs. 16 for MD5), we
* come up four bytes short.
*
* To maintain interop while allowing for larger digests, the behavior
* is unchanged when using 16-octet digests. For larger digests, the
* timestamp, key ID, and digest are placed immediately following the
* request payload, with the overall packet size variable. ntpd can
* distinguish 16-octet digests by the overall request size being
* REQ_LEN_NOMAC + 4 + 16 with the auth bit enabled. When using a
* longer digest, that request size should be avoided.
*
* With the form used with 20-octet and larger digests, the timestamp,
* key ID, and digest are located by ntpd relative to the start of the
* packet, and the size of the digest is then implied by the packet
* size.
*/
static int
sendrequest(
int implcode,
int reqcode,
int auth,
u_int qitems,
size_t qsize,
char *qdata
)
{
struct req_pkt qpkt;
size_t datasize;
size_t reqsize;
u_long key_id;
l_fp ts;
l_fp * ptstamp;
int maclen;
char * pass;
memset(&qpkt, 0, sizeof(qpkt));
qpkt.rm_vn_mode = RM_VN_MODE(0, 0, 0);
qpkt.implementation = (u_char)implcode;
qpkt.request = (u_char)reqcode;
datasize = qitems * qsize;
if (datasize && qdata != NULL) {
memcpy(qpkt.data, qdata, datasize);
qpkt.err_nitems = ERR_NITEMS(0, qitems);
qpkt.mbz_itemsize = MBZ_ITEMSIZE(qsize);
} else {
qpkt.err_nitems = ERR_NITEMS(0, 0);
qpkt.mbz_itemsize = MBZ_ITEMSIZE(qsize); /* allow for optional first item */
}
if (!auth || (keyid_entered && info_auth_keyid == 0)) {
qpkt.auth_seq = AUTH_SEQ(0, 0);
return sendpkt(&qpkt, req_pkt_size);
}
if (info_auth_keyid == 0) {
key_id = getkeyid("Keyid: ");
if (!key_id) {
fprintf(stderr, "Invalid key identifier\n");
return 1;
}
info_auth_keyid = key_id;
}
if (!authistrusted(info_auth_keyid)) {
pass = getpass_keytype(info_auth_keytype);
if ('\0' == pass[0]) {
fprintf(stderr, "Invalid password\n");
return 1;
}
authusekey(info_auth_keyid, info_auth_keytype,
(u_char *)pass);
authtrust(info_auth_keyid, 1);
}
qpkt.auth_seq = AUTH_SEQ(1, 0);
if (info_auth_hashlen > 16) {
/*
* Only ntpd which expects REQ_LEN_NOMAC plus maclen
* octets in an authenticated request using a 16 octet
* digest (that is, a newer ntpd) will handle digests
* larger than 16 octets, so for longer digests, do
* not attempt to shorten the requests for downlevel
* ntpd compatibility.
*/
if (REQ_LEN_NOMAC != req_pkt_size)
return 1;
reqsize = REQ_LEN_HDR + datasize + sizeof(*ptstamp);
/* align to 32 bits */
reqsize = (reqsize + 3) & ~3;
} else
reqsize = req_pkt_size;
ptstamp = (void *)((char *)&qpkt + reqsize);
ptstamp--;
get_systime(&ts);
L_ADD(&ts, &delay_time);
HTONL_FP(&ts, ptstamp);
maclen = authencrypt(info_auth_keyid, (void *)&qpkt, reqsize);
if (!maclen) {
fprintf(stderr, "Key not found\n");
return 1;
} else if (maclen != (info_auth_hashlen + sizeof(keyid_t))) {
fprintf(stderr,
"%d octet MAC, %lu expected with %lu octet digest\n",
maclen, (u_long)(info_auth_hashlen + sizeof(keyid_t)),
(u_long)info_auth_hashlen);
return 1;
}
return sendpkt(&qpkt, reqsize + maclen);
}
/******************************************************************************
*
* Function: tsync_poll()
* Description: Retrieve time from the TSYNC device.
*
* Parameters:
* IN: unit - not used.
* *peer - pointer to this reference clock's peer structure
* Returns: none.
*
*******************************************************************************/
static void tsync_poll(int unit, struct peer *peer)
{
char device[32];
struct refclockproc *pp;
struct calendar jt;
TsyncUnit *up;
unsigned char synch;
double seconds;
int err;
int err1;
int err2;
int err3;
int i;
int j;
unsigned int itAllocationLength;
unsigned int itAllocationLength1;
unsigned int itAllocationLength2;
NtpTimeObj TimeContext;
BoardObj hBoard;
char timeRef[TSYNC_REF_LEN + 1];
char ppsRef [TSYNC_REF_LEN + 1];
TIME_SCALE tmscl = TIME_SCALE_UTC;
LeapSecondObj leapSec;
ioctl_trans_di *it;
ioctl_trans_di *it1;
ioctl_trans_di *it2;
l_fp offset;
l_fp ltemp;
ReferenceObj * pRefObj;
/* Construct the device name */
sprintf(device, "%s%d", DEVICE, (int)peer->refclkunit);
printf("Polling device number %d...\n", (int)peer->refclkunit);
/* Open the TSYNC device */
hBoard.file_descriptor = open(device, O_RDONLY | O_NDELAY, 0777);
/* If error opening TSYNC device... */
if (hBoard.file_descriptor < 0)
{
msyslog(LOG_ERR, "Couldn't open device");
return;
}
/* If error while initializing the board... */
if (ioctl(hBoard.file_descriptor, IOCTL_TPRO_OPEN, &hBoard) < 0)
{
msyslog(LOG_ERR, "Couldn't initialize device");
close(hBoard.file_descriptor);
return;
}
/* Allocate memory for ioctl message */
itAllocationLength =
(sizeof(ioctl_trans_di) - DI_PAYLOADS_STARTER_LENGTH) +
TSYNC_REF_IN_LEN + TSYNC_REF_MAX_OUT_LEN;
it = (ioctl_trans_di*)alloca(itAllocationLength);
if (it == NULL) {
msyslog(LOG_ERR, "Couldn't allocate transaction memory - Reference");
return;
}
/* Build SS_GetRef ioctl message */
it->dest = TSYNC_REF_DEST_ID;
it->iid = TSYNC_REF_IID;
it->inPayloadOffset = TSYNC_REF_IN_PYLD_OFF;
it->inLength = TSYNC_REF_IN_LEN;
it->outPayloadOffset = TSYNC_REF_OUT_PYLD_OFF;
it->maxOutLength = TSYNC_REF_MAX_OUT_LEN;
it->actualOutLength = 0;
it->status = 0;
memset(it->payloads, 0, TSYNC_REF_MAX_OUT_LEN);
/* Read the reference from the TSYNC-PCI device */
err = ioctl(hBoard.file_descriptor,
IOCTL_TSYNC_GET,
(char *)it);
/* Allocate memory for ioctl message */
itAllocationLength1 =
(sizeof(ioctl_trans_di) - DI_PAYLOADS_STARTER_LENGTH) +
TSYNC_TMSCL_IN_LEN + TSYNC_TMSCL_MAX_OUT_LEN;
it1 = (ioctl_trans_di*)alloca(itAllocationLength1);
if (it1 == NULL) {
msyslog(LOG_ERR, "Couldn't allocate transaction memory - Time Scale");
return;
}
/* Build CS_GetTimeScale ioctl message */
it1->dest = TSYNC_TMSCL_DEST_ID;
it1->iid = TSYNC_TMSCL_IID;
it1->inPayloadOffset = TSYNC_TMSCL_IN_PYLD_OFF;
it1->inLength = TSYNC_TMSCL_IN_LEN;
it1->outPayloadOffset = TSYNC_TMSCL_OUT_PYLD_OFF;
it1->maxOutLength = TSYNC_TMSCL_MAX_OUT_LEN;
it1->actualOutLength = 0;
it1->status = 0;
memset(it1->payloads, 0, TSYNC_TMSCL_MAX_OUT_LEN);
/* Read the Time Scale info from the TSYNC-PCI device */
err1 = ioctl(hBoard.file_descriptor,
IOCTL_TSYNC_GET,
(char *)it1);
/* Allocate memory for ioctl message */
itAllocationLength2 =
(sizeof(ioctl_trans_di) - DI_PAYLOADS_STARTER_LENGTH) +
TSYNC_LEAP_IN_LEN + TSYNC_LEAP_MAX_OUT_LEN;
it2 = (ioctl_trans_di*)alloca(itAllocationLength2);
if (it2 == NULL) {
msyslog(LOG_ERR, "Couldn't allocate transaction memory - Leap Second");
return;
}
/* Build CS_GetLeapSec ioctl message */
it2->dest = TSYNC_LEAP_DEST_ID;
it2->iid = TSYNC_LEAP_IID;
it2->inPayloadOffset = TSYNC_LEAP_IN_PYLD_OFF;
it2->inLength = TSYNC_LEAP_IN_LEN;
it2->outPayloadOffset = TSYNC_LEAP_OUT_PYLD_OFF;
it2->maxOutLength = TSYNC_LEAP_MAX_OUT_LEN;
it2->actualOutLength = 0;
it2->status = 0;
memset(it2->payloads, 0, TSYNC_LEAP_MAX_OUT_LEN);
/* Read the leap seconds info from the TSYNC-PCI device */
err2 = ioctl(hBoard.file_descriptor,
IOCTL_TSYNC_GET,
(char *)it2);
pp = peer->procptr;
up = (TsyncUnit*)pp->unitptr;
/* Read the time from the TSYNC-PCI device */
err3 = ioctl(hBoard.file_descriptor,
IOCTL_TPRO_GET_NTP_TIME,
(char *)&TimeContext);
/* Close the TSYNC device */
close(hBoard.file_descriptor);
// Check for errors
if ((err < 0) ||(err1 < 0) || (err2 < 0) || (err3 < 0) ||
(it->status != 0) || (it1->status != 0) || (it2->status != 0) ||
(it->actualOutLength != TSYNC_REF_OUT_LEN) ||
(it1->actualOutLength != TSYNC_TMSCL_OUT_LEN) ||
(it2->actualOutLength != TSYNC_LEAP_OUT_LEN)) {
refclock_report(peer, CEVNT_FAULT);
return;
}
// Extract reference identifiers from ioctl payload
memset(timeRef, '\0', sizeof(timeRef));
memset(ppsRef, '\0', sizeof(ppsRef));
pRefObj = (void *)it->payloads;
memcpy(timeRef, pRefObj->time, TSYNC_REF_LEN);
memcpy(ppsRef, pRefObj->pps, TSYNC_REF_LEN);
// Extract the Clock Service Time Scale and convert to correct byte order
memcpy(&tmscl, ((TIME_SCALE*)(it1->payloads)), sizeof(tmscl));
tmscl = ntohl(tmscl);
// Extract leap second info from ioctl payload and perform byte swapping
for (i = 0; i < (sizeof(leapSec) / 4); i++)
{
for (j = 0; j < 4; j++)
{
((unsigned char*)&leapSec)[(i * 4) + j] =
((unsigned char*)(it2->payloads))[(i * 4) + (3 - j)];
}
}
// Determine time reference ID from reference name
for (i = 0; RefIdLookupTbl[i].pRef != NULL; i++)
{
// Search RefID table
if (strstr(timeRef, RefIdLookupTbl[i].pRef) != NULL)
{
// Found the matching string
break;
}
}
// Determine pps reference ID from reference name
for (j = 0; RefIdLookupTbl[j].pRef != NULL; j++)
{
// Search RefID table
if (strstr(ppsRef, RefIdLookupTbl[j].pRef) != NULL)
{
// Found the matching string
break;
}
}
// Determine synchronization state from flags
synch = (TimeContext.timeObj.flags == 0x4) ? 1 : 0;
// Pull seconds information from time object
seconds = (double) (TimeContext.timeObj.secsDouble);
seconds /= (double) 1000000.0;
/*
** Convert the number of microseconds to double and then place in the
** peer's last received long floating point format.
*/
DTOLFP(((double)TimeContext.tv.tv_usec / 1000000.0), &pp->lastrec);
/*
** The specTimeStamp is the number of seconds since 1/1/1970, while the
** peer's lastrec time should be compatible with NTP which is seconds since
** 1/1/1900. So Add the number of seconds between 1900 and 1970 to the
** specTimeStamp and place in the peer's lastrec long floating point struct.
*/
pp->lastrec.Ul_i.Xl_ui += (unsigned int)TimeContext.tv.tv_sec +
SECONDS_1900_TO_1970;
pp->polls++;
/*
** set the reference clock object
*/
sprintf(pp->a_lastcode, "%03d %02d:%02d:%02.6f",
TimeContext.timeObj.days, TimeContext.timeObj.hours,
TimeContext.timeObj.minutes, seconds);
pp->lencode = strlen (pp->a_lastcode);
pp->day = TimeContext.timeObj.days;
pp->hour = TimeContext.timeObj.hours;
pp->minute = TimeContext.timeObj.minutes;
pp->second = (int) seconds;
seconds = (seconds - (double) (pp->second / 1.0)) * 1000000000;
pp->nsec = (long) seconds;
/*
** calculate year start
*/
jt.year = TimeContext.timeObj.year;
jt.yearday = 1;
jt.monthday = 1;
jt.month = 1;
jt.hour = 0;
jt.minute = 0;
jt.second = 0;
pp->yearstart = caltontp(&jt);
// Calculate and report reference clock offset
offset.l_ui = (long)(((pp->day - 1) * 24) + pp->hour + GMT);
offset.l_ui = (offset.l_ui * 60) + (long)pp->minute;
offset.l_ui = (offset.l_ui * 60) + (long)pp->second;
offset.l_ui = offset.l_ui + (long)pp->yearstart;
offset.l_uf = 0;
DTOLFP(pp->nsec / 1e9, <emp);
L_ADD(&offset, <emp);
refclock_process_offset(pp, offset, pp->lastrec,
pp->fudgetime1);
// KTS in sync
if (synch) {
// Subtract leap second info by one second to determine effective day
ApplyTimeOffset(&(leapSec.utcDate), -1);
// If there is a leap second today and the KTS is using a time scale
// which handles leap seconds then
if ((tmscl != TIME_SCALE_GPS) && (tmscl != TIME_SCALE_TAI) &&
(leapSec.utcDate.year == (unsigned int)TimeContext.timeObj.year) &&
(leapSec.utcDate.doy == (unsigned int)TimeContext.timeObj.days))
{
// If adding a second
if (leapSec.offset == 1)
{
pp->leap = LEAP_ADDSECOND;
}
// Else if removing a second
else if (leapSec.offset == -1)
{
pp->leap = LEAP_DELSECOND;
}
// Else report no leap second pending (no handling of offsets
// other than +1 or -1)
else
{
pp->leap = LEAP_NOWARNING;
}
}
// Else report no leap second pending
else
{
pp->leap = LEAP_NOWARNING;
}
peer->leap = pp->leap;
refclock_report(peer, CEVNT_NOMINAL);
// If reference name reported, then not in holdover
if ((RefIdLookupTbl[i].pRef != NULL) &&
(RefIdLookupTbl[j].pRef != NULL))
{
// Determine if KTS being synchronized by host (identified as
// "LOCL")
if ((strcmp(RefIdLookupTbl[i].pRefId, TSYNC_REF_LOCAL) == 0) ||
(strcmp(RefIdLookupTbl[j].pRefId, TSYNC_REF_LOCAL) == 0))
{
// Clear prefer flag
peer->flags &= ~FLAG_PREFER;
// Set reference clock stratum level as unusable
pp->stratum = STRATUM_UNSPEC;
peer->stratum = pp->stratum;
// If a valid peer is available
if ((sys_peer != NULL) && (sys_peer != peer))
{
// Store reference peer stratum level and ID
up->refStratum = sys_peer->stratum;
up->refId = addr2refid(&sys_peer->srcadr);
}
}
else
{
// Restore prefer flag
peer->flags |= up->refPrefer;
// Store reference stratum as local clock
up->refStratum = TSYNC_LCL_STRATUM;
strncpy((char *)&up->refId, RefIdLookupTbl[j].pRefId,
TSYNC_REF_LEN);
// Set reference clock stratum level as local clock
pp->stratum = TSYNC_LCL_STRATUM;
peer->stratum = pp->stratum;
}
// Update reference name
strncpy((char *)&pp->refid, RefIdLookupTbl[j].pRefId,
TSYNC_REF_LEN);
peer->refid = pp->refid;
}
// Else in holdover
else
{
// Restore prefer flag
peer->flags |= up->refPrefer;
// Update reference ID to saved ID
pp->refid = up->refId;
peer->refid = pp->refid;
// Update stratum level to saved stratum level
pp->stratum = up->refStratum;
peer->stratum = pp->stratum;
}
}
// Else KTS not in sync
else {
// Place local identifier in peer RefID
strncpy((char *)&pp->refid, TSYNC_REF_LOCAL, TSYNC_REF_LEN);
peer->refid = pp->refid;
// Report not in sync
pp->leap = LEAP_NOTINSYNC;
peer->leap = pp->leap;
}
if (pp->coderecv == pp->codeproc) {
refclock_report(peer, CEVNT_TIMEOUT);
return;
}
record_clock_stats(&peer->srcadr, pp->a_lastcode);
refclock_receive(peer);
/* Increment the number of times the reference has been polled */
pp->polls++;
} /* End - tsync_poll() */
int
step_systime(
double step
)
{
time_t pivot; /* for ntp era unfolding */
struct timeval timetv, tvlast, tvdiff;
struct timespec timets;
struct calendar jd;
l_fp fp_ofs, fp_sys; /* offset and target system time in FP */
/*
* Get pivot time for NTP era unfolding. Since we don't step
* very often, we can afford to do the whole calculation from
* scratch. And we're not in the time-critical path yet.
*/
#if SIZEOF_TIME_T > 4
/*
* This code makes sure the resulting time stamp for the new
* system time is in the 2^32 seconds starting at 1970-01-01,
* 00:00:00 UTC.
*/
pivot = 0x80000000;
#if USE_COMPILETIME_PIVOT
/*
* Add the compile time minus 10 years to get a possible target
* area of (compile time - 10 years) to (compile time + 126
* years). This should be sufficient for a given binary of
* NTPD.
*/
if (ntpcal_get_build_date(&jd)) {
jd.year -= 10;
pivot += ntpcal_date_to_time(&jd);
} else {
msyslog(LOG_ERR,
"step-systime: assume 1970-01-01 as build date");
}
#else
UNUSED_LOCAL(jd);
#endif /* USE_COMPILETIME_PIVOT */
#else
UNUSED_LOCAL(jd);
/* This makes sure the resulting time stamp is on or after
* 1969-12-31/23:59:59 UTC and gives us additional two years,
* from the change of NTP era in 2036 to the UNIX rollover in
* 2038. (Minus one second, but that won't hurt.) We *really*
* need a longer 'time_t' after that! Or a different baseline,
* but that would cause other serious trouble, too.
*/
pivot = 0x7FFFFFFF;
#endif
/* get the complete jump distance as l_fp */
DTOLFP(sys_residual, &fp_sys);
DTOLFP(step, &fp_ofs);
L_ADD(&fp_ofs, &fp_sys);
/* ---> time-critical path starts ---> */
/* get the current time as l_fp (without fuzz) and as struct timeval */
get_ostime(&timets);
fp_sys = tspec_stamp_to_lfp(timets);
tvlast.tv_sec = timets.tv_sec;
tvlast.tv_usec = (timets.tv_nsec + 500) / 1000;
/* get the target time as l_fp */
L_ADD(&fp_sys, &fp_ofs);
/* unfold the new system time */
timetv = lfp_stamp_to_tval(fp_sys, &pivot);
/* now set new system time */
if (ntp_set_tod(&timetv, NULL) != 0) {
msyslog(LOG_ERR, "step-systime: %m");
return FALSE;
}
/* <--- time-critical path ended with 'ntp_set_tod()' <--- */
sys_residual = 0;
lamport_violated = (step < 0);
if (step_callback)
(*step_callback)();
#ifdef NEED_HPUX_ADJTIME
/*
* CHECKME: is this correct when called by ntpdate?????
*/
_clear_adjtime();
#endif
/*
* FreeBSD, for example, has:
* struct utmp {
* char ut_line[UT_LINESIZE];
* char ut_name[UT_NAMESIZE];
* char ut_host[UT_HOSTSIZE];
* long ut_time;
* };
* and appends line="|", name="date", host="", time for the OLD
* and appends line="{", name="date", host="", time for the NEW
* to _PATH_WTMP .
*
* Some OSes have utmp, some have utmpx.
*/
/*
* Write old and new time entries in utmp and wtmp if step
* adjustment is greater than one second.
*
* This might become even Uglier...
*/
tvdiff = abs_tval(sub_tval(timetv, tvlast));
if (tvdiff.tv_sec > 0) {
#ifdef HAVE_UTMP_H
struct utmp ut;
#endif
#ifdef HAVE_UTMPX_H
struct utmpx utx;
#endif
#ifdef HAVE_UTMP_H
ZERO(ut);
#endif
#ifdef HAVE_UTMPX_H
ZERO(utx);
#endif
/* UTMP */
#ifdef UPDATE_UTMP
# ifdef HAVE_PUTUTLINE
# ifndef _PATH_UTMP
# define _PATH_UTMP UTMP_FILE
# endif
utmpname(_PATH_UTMP);
ut.ut_type = OLD_TIME;
strlcpy(ut.ut_line, OTIME_MSG, sizeof(ut.ut_line));
ut.ut_time = tvlast.tv_sec;
setutent();
pututline(&ut);
ut.ut_type = NEW_TIME;
strlcpy(ut.ut_line, NTIME_MSG, sizeof(ut.ut_line));
ut.ut_time = timetv.tv_sec;
setutent();
pututline(&ut);
endutent();
# else /* not HAVE_PUTUTLINE */
# endif /* not HAVE_PUTUTLINE */
#endif /* UPDATE_UTMP */
/* UTMPX */
#ifdef UPDATE_UTMPX
# ifdef HAVE_PUTUTXLINE
utx.ut_type = OLD_TIME;
strlcpy(utx.ut_line, OTIME_MSG, sizeof(utx.ut_line));
utx.ut_tv = tvlast;
setutxent();
pututxline(&utx);
utx.ut_type = NEW_TIME;
strlcpy(utx.ut_line, NTIME_MSG, sizeof(utx.ut_line));
utx.ut_tv = timetv;
setutxent();
pututxline(&utx);
endutxent();
# else /* not HAVE_PUTUTXLINE */
# endif /* not HAVE_PUTUTXLINE */
#endif /* UPDATE_UTMPX */
/* WTMP */
#ifdef UPDATE_WTMP
# ifdef HAVE_PUTUTLINE
# ifndef _PATH_WTMP
# define _PATH_WTMP WTMP_FILE
# endif
utmpname(_PATH_WTMP);
ut.ut_type = OLD_TIME;
strlcpy(ut.ut_line, OTIME_MSG, sizeof(ut.ut_line));
ut.ut_time = tvlast.tv_sec;
setutent();
pututline(&ut);
ut.ut_type = NEW_TIME;
strlcpy(ut.ut_line, NTIME_MSG, sizeof(ut.ut_line));
ut.ut_time = timetv.tv_sec;
setutent();
pututline(&ut);
endutent();
# else /* not HAVE_PUTUTLINE */
# endif /* not HAVE_PUTUTLINE */
#endif /* UPDATE_WTMP */
/* WTMPX */
#ifdef UPDATE_WTMPX
# ifdef HAVE_PUTUTXLINE
utx.ut_type = OLD_TIME;
utx.ut_tv = tvlast;
strlcpy(utx.ut_line, OTIME_MSG, sizeof(utx.ut_line));
# ifdef HAVE_UPDWTMPX
updwtmpx(WTMPX_FILE, &utx);
# else /* not HAVE_UPDWTMPX */
# endif /* not HAVE_UPDWTMPX */
# else /* not HAVE_PUTUTXLINE */
# endif /* not HAVE_PUTUTXLINE */
# ifdef HAVE_PUTUTXLINE
utx.ut_type = NEW_TIME;
utx.ut_tv = timetv;
strlcpy(utx.ut_line, NTIME_MSG, sizeof(utx.ut_line));
# ifdef HAVE_UPDWTMPX
updwtmpx(WTMPX_FILE, &utx);
# else /* not HAVE_UPDWTMPX */
# endif /* not HAVE_UPDWTMPX */
# else /* not HAVE_PUTUTXLINE */
# endif /* not HAVE_PUTUTXLINE */
#endif /* UPDATE_WTMPX */
}
return TRUE;
}
/*
* get_systime - return system time in NTP timestamp format.
*/
void
get_systime(
l_fp *now /* system time */
)
{
static struct timespec ts_prev; /* prior os time */
static l_fp lfp_prev; /* prior result */
static double dfuzz_prev; /* prior fuzz */
struct timespec ts; /* seconds and nanoseconds */
struct timespec ts_min; /* earliest permissible */
struct timespec ts_lam; /* lamport fictional increment */
struct timespec ts_prev_log; /* for msyslog only */
double dfuzz;
double ddelta;
l_fp result;
l_fp lfpfuzz;
l_fp lfpdelta;
get_ostime(&ts);
DEBUG_REQUIRE(systime_init_done);
ENTER_GET_SYSTIME_CRITSEC();
/*
* After default_get_precision() has set a nonzero sys_fuzz,
* ensure every reading of the OS clock advances by at least
* sys_fuzz over the prior reading, thereby assuring each
* fuzzed result is strictly later than the prior. Limit the
* necessary fiction to 1 second.
*/
if (!USING_SIGIO()) {
ts_min = add_tspec_ns(ts_prev, sys_fuzz_nsec);
if (cmp_tspec(ts, ts_min) < 0) {
ts_lam = sub_tspec(ts_min, ts);
if (ts_lam.tv_sec > 0 && !lamport_violated) {
msyslog(LOG_ERR,
"get_systime Lamport advance exceeds one second (%.9f)",
ts_lam.tv_sec +
1e-9 * ts_lam.tv_nsec);
exit(1);
}
if (!lamport_violated)
ts = ts_min;
}
ts_prev_log = ts_prev;
ts_prev = ts;
} else {
/*
* Quiet "ts_prev_log.tv_sec may be used uninitialized"
* warning from x86 gcc 4.5.2.
*/
ZERO(ts_prev_log);
}
/* convert from timespec to l_fp fixed-point */
result = tspec_stamp_to_lfp(ts);
/*
* Add in the fuzz.
*/
dfuzz = ntp_random() * 2. / FRAC * sys_fuzz;
DTOLFP(dfuzz, &lfpfuzz);
L_ADD(&result, &lfpfuzz);
/*
* Ensure result is strictly greater than prior result (ignoring
* sys_residual's effect for now) once sys_fuzz has been
* determined.
*/
if (!USING_SIGIO()) {
if (!L_ISZERO(&lfp_prev) && !lamport_violated) {
if (!L_ISGTU(&result, &lfp_prev) &&
sys_fuzz > 0.) {
msyslog(LOG_ERR, "ts_prev %s ts_min %s",
tspectoa(ts_prev_log),
tspectoa(ts_min));
msyslog(LOG_ERR, "ts %s", tspectoa(ts));
msyslog(LOG_ERR, "sys_fuzz %ld nsec, prior fuzz %.9f",
sys_fuzz_nsec, dfuzz_prev);
msyslog(LOG_ERR, "this fuzz %.9f",
dfuzz);
lfpdelta = lfp_prev;
L_SUB(&lfpdelta, &result);
LFPTOD(&lfpdelta, ddelta);
msyslog(LOG_ERR,
"prev get_systime 0x%x.%08x is %.9f later than 0x%x.%08x",
lfp_prev.l_ui, lfp_prev.l_uf,
ddelta, result.l_ui, result.l_uf);
}
}
lfp_prev = result;
dfuzz_prev = dfuzz;
if (lamport_violated)
lamport_violated = FALSE;
}
LEAVE_GET_SYSTIME_CRITSEC();
*now = result;
}
/*
* vme_poll - called by the transmit procedure
*/
static void
vme_poll(
int unit,
struct peer *peer
)
{
struct vmedate *tptr;
struct vmeunit *vme;
l_fp tstmp;
time_t tloc;
struct tm *tadr;
long ltemp;
if (unit >= MAXUNITS) {
msyslog(LOG_ERR, "vme_poll: unit %d invalid", unit);
return;
}
if (!unitinuse[unit]) {
msyslog(LOG_ERR, "vme_poll: unit %d not in use", unit);
return;
}
vme = vmeunits[unit]; /* Here is the structure */
vme->polls++;
tptr = &vme->vmedata;
if ((tptr = get_gpsvme_time()) == NULL ) {
vme_report_event(vme, CEVNT_BADREPLY);
return;
}
get_systime(&vme->lastrec);
vme->lasttime = current_time;
/*
* Get VME time and convert to timestamp format.
* The year must come from the system clock.
*/
/*
time(&tloc);
tadr = gmtime(&tloc);
tptr->year = (unsigned short)(tadr->tm_year + 1900);
*/
sprintf(vme->lastcode,
"%3.3d %2.2d:%2.2d:%2.2d.%.6d %1d\0",
tptr->doy, tptr->hr, tptr->mn,
tptr->sec, tptr->frac, tptr->status);
record_clock_stats(&(vme->peer->srcadr), vme->lastcode);
vme->lencode = (u_short) strlen(vme->lastcode);
vme->day = tptr->doy;
vme->hour = tptr->hr;
vme->minute = tptr->mn;
vme->second = tptr->sec;
vme->nsec = tptr->frac * 1000;
#ifdef DEBUG
if (debug)
printf("vme: %3d %02d:%02d:%02d.%06ld %1x\n",
vme->day, vme->hour, vme->minute, vme->second,
vme->nsec, tptr->status);
#endif
if (tptr->status ) { /* Status 0 is locked to ref., 1 is not */
vme_report_event(vme, CEVNT_BADREPLY);
return;
}
/*
* Now, compute the reference time value. Use the heavy
* machinery for the seconds and the millisecond field for the
* fraction when present. If an error in conversion to internal
* format is found, the program declares bad data and exits.
* Note that this code does not yet know how to do the years and
* relies on the clock-calendar chip for sanity.
*/
if (!clocktime(vme->day, vme->hour, vme->minute,
vme->second, GMT, vme->lastrec.l_ui,
&vme->yearstart, &vme->lastref.l_ui)) {
vme->baddata++;
vme_report_event(vme, CEVNT_BADTIME);
msyslog(LOG_ERR, "refclock_gpsvme: bad data!!");
return;
}
vme->lastref.l_uf = 0;
DTOLFP(vme->nsec / 1e9, <emp);
L_ADD(&vme->lastrec, <emp);
tstmp = vme->lastref;
L_SUB(&tstmp, &vme->lastrec);
vme->coderecv++;
L_ADD(&tstmp, &(fudgefactor[vme->unit]));
vme->lastref = vme->lastrec;
refclock_receive(vme->peer);
}
/*
**
** Function: LsptoA()
**
** Description: Converts LSP frequencies to LPC coefficients
** for a subframe. Sum and difference
** polynomials are computed from the LSP
** frequencies (which are the roots of these
** polynomials). The LPC coefficients are then
** computed by adding the sum and difference
** polynomials.
**
** Links to text: Sections 2.7, 3.3
**
** Arguments:
**
** Word16 Lsp[] LSP frequencies (10 words)
**
** Outputs:
**
** Word16 Lsp[] LPC coefficients (10 words)
**
** Return value: None
**
*/
void LsptoA( Word16 *Lsp )
{
int i,j ;
Word32 Acc0,Acc1 ;
Word16 Tmp ;
Word32 P[LpcOrder/2+1] ;
Word32 Q[LpcOrder/2+1] ;
/*
* Compute the cosines of the LSP frequencies by table lookup and
* linear interpolation
*/
for ( i = 0 ; i < LpcOrder ; i ++ ) {
/*
* Do the table lookup using bits [15:7] of the LSP frequency
*/
j = (int) shr( Lsp[i], (Word16) 7 ) ;
Acc0 = L_deposit_h( CosineTable[j] ) ;
/*
* Do the linear interpolations using bits [6:0] of the LSP
* frequency
*/
Tmp = sub(CosineTable[j+1], CosineTable[j] ) ;
Acc0 = L_mac( Acc0, Tmp, add( shl( (Word16)(Lsp[i] & 0x007f) ,
(Word16)8 ), (Word16) 0x0080 ) ) ;
Acc0 = L_shl( Acc0, (Word16) 1 ) ;
Lsp[i] = negate( round( Acc0 ) ) ;
}
/*
* Compute the sum and difference polynomials with the real roots
* removed. These are computed by polynomial multiplication as
* follows. Let the sum polynomial be P(z). Define the elementary
* polynomials P_i(z) = 1 - 2cos(w_i) z^{-1} + z^{-2}, for 1<=i<=
* 5, where {w_i} are the LSP frequencies corresponding to the sum
* polynomial. Then P(z) = P_1(z)P_2(z)...P_5(z). Similarly
* the difference polynomial Q(z) = Q_1(z)Q_2(z)...Q_5(z).
*/
/*
* Initialize the arrays with the coefficients of the product
* P_1(z)P_2(z) and Q_1(z)Q_2(z). Scale by 1/8.
*/
P[0] = (Word32) 0x10000000L ;
P[1] = L_mult( Lsp[0], (Word16) 0x2000 ) ;
P[1] = L_mac( P[1], Lsp[2], (Word16) 0x2000 ) ;
P[2] = L_mult( Lsp[0], Lsp[2] ) ;
P[2] = L_shr( P[2], (Word16) 1 ) ;
P[2] = L_add( P[2], (Word32) 0x20000000L ) ;
Q[0] = (Word32) 0x10000000L ;
Q[1] = L_mult( Lsp[1], (Word16) 0x2000 ) ;
Q[1] = L_mac( Q[1], Lsp[3], (Word16) 0x2000 ) ;
Q[2] = L_mult( Lsp[1], Lsp[3] ) ;
Q[2] = L_shr( Q[2], (Word16) 1 ) ;
Q[2] = L_add( Q[2], (Word32) 0x20000000L ) ;
/*
* Compute the intermediate polynomials P_1(z)P_2(z)...P_i(z) and
* Q_1(z)Q_2(z)...Q_i(z), for i = 2, 3, 4. Each intermediate
* polynomial is symmetric, so only the coefficients up to i+1 need
* by computed. Scale by 1/2 each iteration for a total of 1/8.
*/
for ( i = 2 ; i < LpcOrder/2 ; i ++ ) {
/* Compute coefficient (i+1) */
Acc0 = P[i] ;
Acc0 = L_mls( Acc0, Lsp[2*i+0] ) ;
// Acc0 = L_add( Acc0, P[i-1] ) ;
L_ADD(Acc0, P[i-1], Acc0);
P[i+1] = Acc0 ;
Acc1 = Q[i] ;
Acc1 = L_mls( Acc1, Lsp[2*i+1] ) ;
// Acc1 = L_add( Acc1, Q[i-1] ) ;
L_ADD(Acc1, Q[i-1], Acc1);
Q[i+1] = Acc1 ;
/* Compute coefficients i, i-1, ..., 2 */
for ( j = i ; j >= 2 ; j -- ) {
Acc0 = P[j-1] ;
Acc0 = L_mls( Acc0, Lsp[2*i+0] ) ;
Acc0 = L_add( Acc0, L_shr(P[j], (Word16) 1 ) ) ;
Acc0 = L_add( Acc0, L_shr(P[j-2], (Word16) 1 ) ) ;
P[j] = Acc0 ;
Acc1 = Q[j-1] ;
Acc1 = L_mls( Acc1, Lsp[2*i+1] ) ;
Acc1 = L_add( Acc1, L_shr(Q[j], (Word16) 1 ) ) ;
Acc1 = L_add( Acc1, L_shr(Q[j-2], (Word16) 1 ) ) ;
Q[j] = Acc1 ;
}
/* Compute coefficients 1, 0 */
P[0] = L_shr( P[0], (Word16) 1 ) ;
Q[0] = L_shr( Q[0], (Word16) 1 ) ;
Acc0 = L_deposit_h( Lsp[2*i+0] ) ;
Acc0 = L_shr( Acc0, (Word16) i ) ;
Acc0 = L_add( Acc0, P[1] ) ;
Acc0 = L_shr( Acc0, (Word16) 1 ) ;
P[1] = Acc0 ;
Acc1 = L_deposit_h( Lsp[2*i+1] ) ;
Acc1 = L_shr( Acc1, (Word16) i ) ;
Acc1 = L_add( Acc1, Q[1] ) ;
Acc1 = L_shr( Acc1, (Word16) 1 ) ;
Q[1] = Acc1 ;
}
/*
* Convert the sum and difference polynomials to LPC coefficients
* The LPC polynomial is the sum of the sum and difference
* polynomials with the real zeros factored in: A(z) = 1/2 {P(z) (1
* + z^{-1}) + Q(z) (1 - z^{-1})}. The LPC coefficients are scaled
* here by 16; the overall scale factor for the LPC coefficients
* returned by this function is therefore 1/4.
*/
for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {
Acc0 = P[i] ;
Acc0 = L_add( Acc0, P[i+1] ) ;
Acc0 = L_sub( Acc0, Q[i] ) ;
Acc0 = L_add( Acc0, Q[i+1] ) ;
Acc0 = L_shl( Acc0, (Word16) 3 ) ;
Lsp[i] = negate( round( Acc0 ) ) ;
Acc1 = P[i] ;
Acc1 = L_add( Acc1, P[i+1] ) ;
Acc1 = L_add( Acc1, Q[i] ) ;
Acc1 = L_sub( Acc1, Q[i+1] ) ;
Acc1 = L_shl( Acc1, (Word16) 3 ) ;
Lsp[LpcOrder-1-i] = negate( round( Acc1 ) ) ;
}
}
/*
**
** Function: AtoLsp()
**
** Description: Transforms 10 LPC coefficients to the 10
** corresponding LSP frequencies for a subframe.
** This transformation is done once per frame,
** for subframe 3 only. The transform algorithm
** generates sum and difference polynomials from
** the LPC coefficients. It then evaluates the
** sum and difference polynomials at uniform
** intervals of pi/256 along the unit circle.
** Intervals where a sign change occurs are
** interpolated to find the zeros of the
** polynomials, which are the LSP frequencies.
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 *LspVect Empty Buffer
** Word16 Lpc[] Unquantized LPC coefficients (10 words)
** Word16 PrevLsp[] LSP frequencies from the previous frame (10 words)
**
** Outputs:
**
** Word16 LspVect[] LSP frequencies for the current frame (10 words)
**
** Return value: None
**
**/
void AtoLsp( Word16 *LspVect, Word16 *Lpc, Word16 *PrevLsp )
{
int i,j,k ;
Word32 Lpq[LpcOrder+2] ;
Word16 Spq[LpcOrder+2] ;
Word16 Exp ;
Word16 LspCnt ;
Word32 PrevVal,CurrVal ;
Word32 Acc0,Acc1 ;
/*
* Perform a bandwidth expansion on the LPC coefficients. This
* scales the poles of the LPC synthesis filter by a factor of
* 0.994.
*/
for ( i = 0 ; i < LpcOrder ; i ++ )
LspVect[i] = mult_r( Lpc[i], BandExpTable[i] ) ;
/*
* Compute the sum and difference polynomials with the roots at z =
* -1 (sum) or z = +1 (difference) removed. Let these polynomials
* be P(z) and Q(z) respectively, and let their coefficients be
* {p_i} amd {q_i}. The coefficients are stored in the array Lpq[]
* as follows: p_0, q_0, p_1, q_1, ..., p_5, q_5. There is no need
* to store the other coefficients because of symmetry.
*/
/*
* Set p_0 = q_0 = 1. The LPC coefficients are already scaled by
* 1/4. P(z) and Q(z) are scaled by an additional scaling factor of
* 1/16, for an overall factor of 1/64 = 0x02000000L.
*/
Lpq[0] = Lpq[1] = (Word32) 0x02000000L ;
/*
* This loop computes the coefficients of P(z) and Q(z). The long
* division (to remove the real zeros) is done recursively.
*/
for ( i = 0 ; i < LpcOrder/2 ; i ++ ) {
/* P(z) */
Acc0 = L_negate( Lpq[2*i+0] ) ;
Acc1 = L_deposit_h( LspVect[i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Lpq[2*i+2] = Acc0 ;
/* Q(z) */
Acc0 = Lpq[2*i+1] ;
Acc1 = L_deposit_h( LspVect[i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
Acc0 = L_sub( Acc0, Acc1 ) ;
Acc1 = L_deposit_h( LspVect[LpcOrder-1-i] ) ;
Acc1 = L_shr( Acc1, (Word16) 4 ) ;
// Acc0 = L_add( Acc0, Acc1 ) ;
L_ADD(Acc0, Acc1, Acc0);
Lpq[2*i+3] = Acc0 ;
}
/*
* Divide p_5 and q_5 by 2 for proper weighting during polynomial
* evaluation.
*/
Lpq[LpcOrder+0] = L_shr( Lpq[LpcOrder+0], (Word16) 1 ) ;
Lpq[LpcOrder+1] = L_shr( Lpq[LpcOrder+1], (Word16) 1 ) ;
/*
* Normalize the polynomial coefficients and convert to shorts
*/
/* Find the maximum */
Acc1 = L_abs( Lpq[0] ) ;
for ( i = 1 ; i < LpcOrder+2 ; i ++ ) {
Acc0 = L_abs( Lpq[i] ) ;
if ( Acc0 > Acc1 )
Acc1 = Acc0 ;
}
/* Compute the normalization factor */
Exp = norm_l( Acc1 ) ;
/* Normalize and convert to shorts */
for ( i = 0 ; i < LpcOrder+2 ; i ++ ) {
Acc0 = L_shl( Lpq[i], Exp ) ;
Spq[i] = round( Acc0 ) ;
}
/*
* Initialize the search loop
*/
/*
* The variable k is a flag that indicates which polynomial (sum or
* difference) the algorithm is currently evaluating. Start with
* the sum.
*/
k = 0 ;
/* Evaluate the sum polynomial at frequency zero */
PrevVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
PrevVal = L_mac( PrevVal, Spq[2*j], CosineTable[0] ) ;
/*
* Search loop. Evaluate P(z) and Q(z) at uniform intervals of
* pi/256 along the unit circle. Check for zero crossings. The
* zeros of P(w) and Q(w) alternate, so only one of them need by
* evaluated at any given step.
*/
LspCnt = (Word16) 0 ;
for ( i = 1 ; i < CosineTableSize/2 ; i ++ ) {
/* Evaluate the selected polynomial */
CurrVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
/* Check for a sign change, indicating a zero crossing */
if ( (CurrVal ^ PrevVal) < (Word32) 0 ) {
/*
* Interpolate to find the bottom 7 bits of the
* zero-crossing frequency
*/
Acc0 = L_abs( CurrVal ) ;
Acc1 = L_abs( PrevVal ) ;
// Acc0 = L_add( Acc0, Acc1 ) ;
L_ADD(Acc0, Acc1, Acc0);
/* Normalize the sum */
Exp = norm_l( Acc0 ) ;
Acc0 = L_shl( Acc0, Exp ) ;
Acc1 = L_shl( Acc1, Exp ) ;
Acc1 = L_shr( Acc1, (Word16) 8 ) ;
LspVect[LspCnt] = div_l( Acc1, extract_h( Acc0 ) ) ;
/*
* Add the upper part of the zero-crossing frequency,
* i.e. bits 7-15
*/
Exp = shl( (Word16) (i-1), (Word16) 7 ) ;
LspVect[LspCnt] = add( LspVect[LspCnt], Exp ) ;
LspCnt ++ ;
/* Check if all zeros have been found */
if ( LspCnt == (Word16) LpcOrder )
break ;
/*
* Switch the pointer between sum and difference polynomials
*/
k ^= 1 ;
/*
* Evaluate the new polynomial at the current frequency
*/
CurrVal = (Word32) 0 ;
for ( j = 0 ; j <= LpcOrder/2 ; j ++ )
CurrVal = L_mac( CurrVal, Spq[LpcOrder-2*j+k],
CosineTable[i*j%CosineTableSize] ) ;
}
/* Update the previous value */
PrevVal = CurrVal ;
}
/*
* Check if all 10 zeros were found. If not, ignore the results of
* the search and use the previous frame's LSP frequencies instead.
*/
if ( LspCnt != (Word16) LpcOrder ) {
for ( j = 0 ; j < LpcOrder ; j ++ )
LspVect[j] = PrevLsp[j] ;
}
return ;
}
/*
**
** Function: Lsp_Svq()
**
** Description: Performs the search of the VQ tables to find
** the optimum LSP indices for all three bands.
** For each band, the search finds the index which
** minimizes the weighted squared error between
** the table entry and the target.
**
** Links to text: Section 2.5
**
** Arguments:
**
** Word16 Tv[] VQ target vector (10 words)
** Word16 Wvect[] VQ weight vector (10 words)
**
** Outputs: None
**
** Return value:
**
** Word32 Long word packed with the 3 VQ indices. Band 0
** corresponds to bits [23:16], band 1 corresponds
** to bits [15:8], and band 2 corresponds to bits [7:0].
**
*/
Word32 Lsp_Svq( Word16 *Tv, Word16 *Wvect )
{
int i,j,k ;
Word32 Rez,Indx ;
Word32 Acc0,Acc1 ;
Word16 Tmp[LpcOrder] ;
Word16 *LspQntPnt ;
/*
* Initialize the return value
*/
Rez = (Word32) 0 ;
/*
* Quantize each band separately
*/
for ( k = 0 ; k < LspQntBands ; k ++ ) {
/*
* Search over the entire VQ table to find the index that
* minimizes the error.
*/
/* Initialize the search */
Acc1 = (Word32) -1 ;
Indx = (Word32) 0 ;
LspQntPnt = BandQntTable[k] ;
for ( i = 0 ; i < LspCbSize ; i ++ ) {
/*
* Generate the metric, which is the negative error with the
* constant component removed.
*/
for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
Tmp[j] = mult_r( Wvect[BandInfoTable[k][0]+j],
LspQntPnt[j] ) ;
Acc0 = (Word32) 0 ;
for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
Acc0 = L_mac( Acc0, Tv[BandInfoTable[k][0]+j], Tmp[j] ) ;
Acc0 = L_shl( Acc0, (Word16) 1 ) ;
for ( j = 0 ; j < BandInfoTable[k][1] ; j ++ )
Acc0 = L_msu( Acc0, LspQntPnt[j], Tmp[j] ) ;
LspQntPnt += BandInfoTable[k][1] ;
/*
* Compare the metric to the previous maximum and select the
* new index
*/
if ( Acc0 > Acc1 ) {
Acc1 = Acc0 ;
Indx = (Word32) i ;
}
}
/*
* Pack the result with the optimum index for this band
*/
Rez = L_shl( Rez, (Word16) LspCbBits ) ;
// Rez = L_add( Rez, Indx ) ;
L_ADD(Rez, Indx, Rez);
}
return Rez ;
}
static int burst_process(mapidflib_function_instance_t *instance,
MAPI_UNUSED unsigned char* dev_pkt,
MAPI_UNUSED unsigned char* link_pkt,
mapid_pkthdr_t* pkt_head) {
struct burst_inst_struct *internal_data_ptr;
int cat;
// count with l_fp
l_fp pkt_ts_min, pkt_ts_max, pkt_ts_exa, len_ts_min, len_ts_max, len_ts_exa;
// store final values in ull
unsigned long long pkt_ts_ull_min, pkt_ts_ull_max, pkt_ts_ull_exa;
// gap
unsigned long long gap_len_frac;
double gap_len_s;
unsigned long long gap_len_b;
// Debugging {{{
// just ascii art, not code
#ifdef __BURST_DEBUG
l_fp delta, pkt_ts, len_ts, assumed_ts;
double d_i, d_f;
#endif
#ifdef __BURST_DEBUG_LAG_LEAD
l_fp laglead;
#ifndef __BURST_DEBUG
l_fp len_ts, assumed_ts;
double d_i, d_f;
#endif
#endif
#ifdef __BURST_DEBUG_LAG_LEAD_MAX
unsigned long long assumed_ts_ull, len_ts_ull, laglead_ull;
#ifndef __BURST_DEBUG_LAG_LEAD
l_fp laglead;
#ifndef __BURST_DEBUG
l_fp len_ts;
double d_i, d_f;
#endif
#endif
#endif
// }}}
internal_data_ptr = (struct burst_inst_struct *) (instance->internal_data);
// count min / max ts
// start with last_pkt_ts
pkt_ts_max = internal_data_ptr->last_pkt_ts;
pkt_ts_min = internal_data_ptr->last_pkt_ts;
pkt_ts_exa = internal_data_ptr->last_pkt_ts;
// add wlen, iatime, +-
DTOLFP(internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s - internal_data_ptr->lead_s, len_ts_min);
DTOLFP(internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s + internal_data_ptr->lag_s, len_ts_max);
DTOLFP(internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s, len_ts_exa);
L_ADD(pkt_ts_min, len_ts_min);
L_ADD(pkt_ts_max, len_ts_max);
L_ADD(pkt_ts_exa, len_ts_exa);
// Debugging {{{
#ifdef __BURST_DEBUG_WARNINGS
unsigned long long lastullts;
LFPTOULL(internal_data_ptr->last_pkt_ts, lastullts);
if(pkt_head->ts == lastullts) printf("* OMG, same timestamp as previous. Deja vu or what? Timestamps does not seem to be very reliable.\n");
if(pkt_head->ts < lastullts) printf("* OMG^2, timestamp smaller than previous. Timestamps does not seem to be very reliable.\n");
#endif
#ifdef __BURST_DEBUG
printf(">>> [ pkt_head->wlen: %d (B) ]\n", pkt_head->wlen);
ULLTOLFP(pkt_head->ts, delta);
L_SUB(delta, internal_data_ptr->last_pkt_ts);
LFPTODD(delta, d_i, d_f);
printf("with delt/\\: %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", delta.l_ui, delta.l_uf, d_i, d_f);
//////printf("count with me: %u * %023.012f = %023.012f a+ %023.012f = %023.012f ok???\n", internal_data_ptr->last_pkt_wlen, internal_data_ptr->B_s, internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s, internal_data_ptr->iatime_s, internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s);
assumed_ts = internal_data_ptr->last_pkt_ts;
DTOLFP(internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s, len_ts);
L_ADD(assumed_ts, len_ts);
LFPTODD(assumed_ts, d_i, d_f);
printf("assumed_ts=: %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", assumed_ts.l_ui, assumed_ts.l_uf, d_i, d_f);
ULLTOLFP(pkt_head->ts, pkt_ts);
LFPTODD(pkt_ts, d_i, d_f);
printf("current_ts=: %012lu.%012lu (ntp) == %010.0f + %.012f (sec) %llu\n", pkt_ts.l_ui, pkt_ts.l_uf, d_i, d_f, pkt_head->ts);
//
//LFPTODD(pkt_ts_min, d_i, d_f);
//printf(" -: %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", pkt_ts_min.l_ui, pkt_ts_min.l_uf, d_i, d_f);
//
//LFPTODD(pkt_ts_max, d_i, d_f);
//printf(" +: %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", pkt_ts_max.l_ui, pkt_ts_max.l_uf, d_i, d_f);
#endif
#ifdef __BURST_DEBUG_LAG_LEAD
assumed_ts = internal_data_ptr->last_pkt_ts;
DTOLFP(internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s, len_ts);
L_ADD(assumed_ts, len_ts);
ULLTOLFP(pkt_head->ts, laglead);
L_SUB(laglead, assumed_ts);
LFPTODD(laglead, d_i, d_f);
printf("so lag/lead: %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", laglead.l_ui, laglead.l_uf, d_i, d_f);
#endif
#ifdef __BURST_DEBUG_LAG_LEAD_MAX
LFPTOULL(internal_data_ptr->last_pkt_ts, assumed_ts_ull);
DTOLFP(internal_data_ptr->last_pkt_wlen * internal_data_ptr->B_s + internal_data_ptr->iatime_s, len_ts);
LFPTOULL(len_ts, len_ts_ull);
assumed_ts_ull += len_ts_ull;
laglead_ull = pkt_head->ts - assumed_ts_ull;
if(pkt_head->ts >= assumed_ts_ull) { // lag
if(laglead_ull > internal_data_ptr->lagmax) { // update
if(internal_data_ptr->initialized == 0) { // first time
internal_data_ptr->initialized = 1; // initialize
}
else { // update
internal_data_ptr->lagmax = laglead_ull;
ULLTOLFP(laglead_ull, laglead);
LFPTODD(laglead, d_i, d_f);
printf("lagmax : %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", laglead.l_ui, laglead.l_uf, d_i, d_f);
}
}
}
else { // lead
if(laglead_ull < internal_data_ptr->leadmax) { // update
if(internal_data_ptr->initialized == 0) { // first time
internal_data_ptr->initialized = 1; // initialize
}
else {
internal_data_ptr->leadmax = laglead_ull;
ULLTOLFP(laglead_ull, laglead);
LFPTODD(laglead, d_i, d_f);
printf("leadmax : %012lu.%012lu (ntp) == %010.0f + %.012f (sec)\n", laglead.l_ui, laglead.l_uf, d_i, d_f);
}
}
}
#endif
// }}}
// resolve bursts
LFPTOULL(pkt_ts_min, pkt_ts_ull_min);
LFPTOULL(pkt_ts_max, pkt_ts_ull_max);
LFPTOULL(pkt_ts_exa, pkt_ts_ull_exa);
// not burst
if(pkt_head->ts < pkt_ts_ull_min || pkt_head->ts > pkt_ts_ull_max) {
// count measurement into results
if(internal_data_ptr->burst_bytes < (unsigned long) internal_data_ptr->min) {
// Debugging {{{
#ifdef __BURST_VERBOSE
printf("process: I am sed :-( I wish I have [0.x] to save this burst into.\n");
#endif
// }}}
cat = 0;
}
else cat = ((internal_data_ptr->burst_bytes - internal_data_ptr->min) / internal_data_ptr->step) + 1; // +1: category 0 is reserved for <0, min>
if(cat > internal_data_ptr->cats) {
// Debugging {{{
#ifdef __BURST_VERBOSE
printf("process: I am sed :-( I wish I have [%d] to save this burst into.\n", cat);
#endif
// }}}
cat = internal_data_ptr->cats;
}
((burst_category_t *)instance->result.data)[cat].bytes += internal_data_ptr->burst_bytes;
((burst_category_t *)instance->result.data)[cat].packets += internal_data_ptr->burst_packets;
if(internal_data_ptr->burst_bytes > 0) // not the very first packet
((burst_category_t *)instance->result.data)[cat].bursts++;
// Debugging {{{
#ifdef __BURST_VERBOSE
printf("process: save [%d / %d] += %lu B (= %lu B), += %lu pkts (= %lu pkts), +1 burst (= %lu bursts)\n", cat, internal_data_ptr->cats, internal_data_ptr->burst_bytes, ((burst_category_t *)instance->result.data)[cat].bytes, internal_data_ptr->burst_packets, ((burst_category_t *)instance->result.data)[cat].packets, ((burst_category_t *)instance->result.data)[cat].bursts);
#endif
// }}}
// and start new measurement
internal_data_ptr->burst_bytes = 0; // reset collectors
internal_data_ptr->burst_packets = 0;
// NEW: store inter-burst gap size too
if(internal_data_ptr->last_pkt_wlen) { // if not first packet
if(pkt_head->ts > pkt_ts_ull_exa) {
gap_len_frac = pkt_head->ts - pkt_ts_ull_exa;
ULLTOD(gap_len_frac, gap_len_s);
gap_len_b = (unsigned long long) (gap_len_s / internal_data_ptr->B_s);
}
else gap_len_b = 0;
if(gap_len_b < (unsigned) internal_data_ptr->min) {
cat = 0;
} else cat = ((gap_len_b - internal_data_ptr->min) / internal_data_ptr->step) + 1; // +1: category 0 is reserved for <0, min>
if(cat > internal_data_ptr->cats) {
cat = internal_data_ptr->cats;
}
((burst_category_t *)instance->result.data)[cat].gap_bytes += gap_len_b;
if(internal_data_ptr->burst_bytes > 0) // not the very first packet
((burst_category_t *)instance->result.data)[cat].gaps++;
}
}
else { // if one bursts, show goes on
// Debugging {{{
#ifdef __BURST_VERBOSE
printf("process: collect [x] (BURST detected)\n");
#endif
// }}}
}
// count current into collector
internal_data_ptr->burst_bytes += (unsigned long) pkt_head->wlen;
internal_data_ptr->burst_packets++;
// save pktinfo for next generation
ULLTOLFP(pkt_head->ts, internal_data_ptr->last_pkt_ts);
internal_data_ptr->last_pkt_wlen = pkt_head->wlen;
return 1;
}
LFP& LFP::operator+=(const LFP & rhs)
{
L_ADD(&_v, &rhs._v);
return *this;
}
void Syn_filt (
INT16 a[], /* (i) : a[m+1] prediction coefficients (m=10) */
INT16 x[], /* (i) : input signal */
INT16 y[], /* (o) : output signal */
INT16 lg, /* (i) : size of filtering */
INT16 mem[], /* (i/o) : memory associated with this filtering. */
INT16 update /* (i) : 0=no update, 1=update of memory. */
)
{
INT16 i;
INT16* p_x = x;
INT16*p_a;
//INT32 s;
INT16 tmp[80]={0}; /* This is usually done by memory allocation (lg+m) */
INT16 *yy;
register INT32 s_hi=0;
register UINT32 s_lo=0;
VPP_EFR_PROFILE_FUNCTION_ENTER(Syn_filt);
/* Copy mem[] to yy[] */
yy = tmp;
/*for (i = 0; i < m; i++)
{
*yy++ = mem[i];
}*/
*yy++ = mem[0];
*yy++ = mem[1];
*yy++ = mem[2];
*yy++ = mem[3];
*yy++ = mem[4];
*yy++ = mem[5];
*yy++ = mem[6];
*yy++ = mem[7];
*yy++ = mem[8];
*yy++ = mem[9];
/* Do the filtering. */
for (i = 0; i < lg; i++)
{
//s = L_mult (x[i], a[0]);
//s = L_MULT(x[i], a[0]);
p_a = &a[0];
VPP_MLX16(s_hi,s_lo,(*p_x++), (*p_a++));
//for (j = 1; j <= m; j++)
// {
//s = L_msu (s, a[j], yy[-j]);
//s = L_MSU (s, a[j], yy[-j]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-1]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-2]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-3]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-4]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-5]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-6]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-7]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-8]);
VPP_MLA16(s_hi,s_lo,(*p_a++), -yy[-9]);
VPP_MLA16(s_hi,s_lo,(*p_a), -yy[-10]);
// }
//s = L_shl (s, 3);
//s = VPP_SCALE64_TO_16(s_hi,s_lo);
//s = L_SHL_SAT(s, 3);
//*yy++ = ROUND(s);
*yy++ = (INT16)(L_SHR_D(L_ADD((INT32)s_lo,0x800),12));
}
for (i = 0; i < lg; i++)
{
y[i] = tmp[i + m];
}
/* Update of memory if update==1 */
if (update != 0)
{
// for (i = 0; i < m; i++)
// {
mem[0] = y[lg - m ];
mem[1] = y[lg - m + 1];
mem[2] = y[lg - m + 2];
mem[3] = y[lg - m + 3];
mem[4] = y[lg - m + 4];
mem[5] = y[lg - m + 5];
mem[6] = y[lg - m + 6];
mem[7] = y[lg - m + 7];
mem[8] = y[lg - m + 8];
mem[9] = y[lg - m + 9];
// }
}
VPP_EFR_PROFILE_FUNCTION_EXIT(Syn_filt);
return;
}
