/* * 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; }