C++ L_SUB函数代码示例

::testing::AssertionResult
AssertFpClose::operator()(
	const char* m_expr,
	const char* n_expr,
	const l_fp & m,
	const l_fp & n
	)
{
	l_fp diff;

	if (L_ISGEQ(&m, &n)) {
		diff = m;
		L_SUB(&diff, &n);
	} else {
		diff = n;
		L_SUB(&diff, &m);
	}
	if (L_ISGEQ(&limit, &diff))
		return ::testing::AssertionSuccess();

	return ::testing::AssertionFailure()
	    << m_expr << " which is " << l_fp_wrap(m)
	    << "\nand\n"
	    << n_expr << " which is " << l_fp_wrap(n)
	    << "\nare not close; diff=" << l_fp_wrap(diff);
}

bool
parse_timedout(
	       parse_t *parseio,
	       timestamp_t *tstamp,
	       struct timespec *del
	       )
{
	struct timespec delta;

	l_fp delt;

	delt = tstamp->fp;
	L_SUB(&delt, &parseio->parse_lastchar.fp);
	delta = lfp_uintv_to_tspec(delt);
	if (cmp_tspec(delta, *del) == TIMESPEC_GREATER_THAN)
	{
		parseprintf(DD_PARSE, ("parse: timedout: TRUE\n"));
		return true;
	}
	else
	{
		parseprintf(DD_PARSE, ("parse: timedout: FALSE\n"));
		return false;
	}
}

l_fp
l_fp_subtract(const l_fp first, const l_fp second)
{
	l_fp temp = first;
	L_SUB(&temp, &second);

	return temp;
}

static void
gpsd_parse(
	peerT      * const peer ,
	const l_fp * const rtime)
{
	clockprocT * const pp = peer->procptr;
	gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;

	json_ctx     jctx;
	const char * clsid;
	l_fp         tmpfp;

        DPRINTF(2, ("GPSD_JSON(%d): gpsd_parse: time %s '%s'\n",
                    up->unit, ulfptoa(rtime, 6), up->buffer));

	/* See if we can grab anything potentially useful */
	if (!json_parse_record(&jctx, up->buffer))
		return;

	/* Now dispatch over the objects we know */
	clsid = json_object_lookup_string_default(
		&jctx, 0, "class", "-bad-repy-");

	up->tc_recv += 1;
	if (!strcmp("VERSION", clsid))
		process_version(peer, &jctx, rtime);
	else if (!strcmp("TPV", clsid))
		process_tpv(peer, &jctx, rtime);
	else if (!strcmp("PPS", clsid))
		process_pps(peer, &jctx, rtime);
	else if (!strcmp("WATCH", clsid))
		process_watch(peer, &jctx, rtime);
	else
		return; /* nothing we know about... */

	/* now aggregate TPV and PPS -- no PPS? just use TPV...*/
	if (up->fl_tpv) {
		/* TODO: also check remote receive time stamps */
		tmpfp = up->tpv_local;
		L_SUB(&tmpfp, &up->pps_local);

		if (up->fl_pps && 0 == tmpfp.l_ui) {
			refclock_process_offset(
				pp, up->tpv_stamp, up->pps_recvt, 0.0);
			if (up->ppscount < PPS_MAXCOUNT)
				up->ppscount += 1;
		} else {
			refclock_process_offset(
				pp, up->tpv_stamp, up->tpv_recvt, 0.0);
			if (up->ppscount > 0)
				up->ppscount -= 1;
		}
		up->fl_pps   = 0;
		up->fl_tpv   = 0;
		up->tc_good += 1;
	}
}

static void
process_pps(
	peerT      * const peer ,
	json_ctx   * const jctx ,
	const l_fp * const rtime)
{
	clockprocT * const pp = peer->procptr;
	gpsd_unitT * const up = (gpsd_unitT *)pp->unitptr;

	struct timespec ts;
		
	errno = 0;
	ts.tv_sec = (time_t)json_object_lookup_int(
		jctx, 0, "clock_sec");
	if (up->fl_nsec)
		ts.tv_nsec = json_object_lookup_int(
			jctx, 0, "clock_nsec");
	else
		ts.tv_nsec = json_object_lookup_int(
			jctx, 0, "clock_musec") * 1000;

	if (0 != errno)
		goto fail;

	up->pps_local = *rtime;
	/* get fudged receive time */
	up->pps_recvt = tspec_stamp_to_lfp(ts);
	L_SUB(&up->pps_recvt, &up->pps_fudge);

	/* map to next full second as reference time stamp */
	up->pps_stamp = up->pps_recvt;
	L_ADDUF(&up->pps_stamp, 0x80000000u);
	up->pps_stamp.l_uf = 0;
	
	pp->lastrec = up->pps_stamp;

	DPRINTF(2, ("GPSD_JSON(%d): process_pps, stamp='%s', recvt='%s'\n", 
		    up->unit,
		    gmprettydate(&up->pps_stamp),
		    gmprettydate(&up->pps_recvt)));
	
	/* When we have a time pulse, clear the TPV flag: the
	 * PPS is only valid for the >NEXT< TPV value!
	 */
	up->fl_pps = -1;
	up->fl_tpv =  0;
	return;

  fail:
	DPRINTF(2, ("GPSD_JSON(%d): process_pps FAILED, nsec=%d stamp='%s', recvt='%s'\n",
		    up->unit, up->fl_nsec,
		    gmprettydate(&up->pps_stamp),
		    gmprettydate(&up->pps_recvt)));
	up->tc_breply += 1;
}

static int burst_reset(mapidflib_function_instance_t* instance) {
	struct burst_inst_struct *internal_data_ptr;
	internal_data_ptr = (struct burst_inst_struct *) (instance->internal_data);

	L_SUB(internal_data_ptr->last_pkt_ts, internal_data_ptr->last_pkt_ts);
	internal_data_ptr->burst_bytes = 0;
	internal_data_ptr->burst_packets = 0;
	memset(instance->result.data, 0, instance->def->shm_size);

	return 0;
}

bool
AssertFpClose(const l_fp m, const l_fp n, const l_fp limit) {
	l_fp diff;

	if (L_ISGEQ(&m, &n)) {
		diff = m;
		L_SUB(&diff, &n);
	} else {
		diff = n;
		L_SUB(&diff, &m);
	}
	if (L_ISGEQ(&limit, &diff)){
		return TRUE;
	}
	else {
		printf("m_expr which is %s \nand\nn_expr which is %s\nare not close; diff=%susec\n", lfptoa(&m, 10), lfptoa(&n, 10), lfptoa(&diff, 10)); 
		//printf("m_expr which is %d.%d \nand\nn_expr which is %d.%d\nare not close; diff=%d.%dusec\n", m.l_uf, m.Ul_i, n.l_uf, n.Ul_i, diff.l_uf, diff.Ul_i); 
		return FALSE;
	}
}

bool AssertFpClose(const l_fp m,const l_fp n, const l_fp limit)
{
	l_fp diff;

	if (L_ISGEQ(&m, &n)) {
		diff = m;
		L_SUB(&diff, &n);
	} else {
		diff = n;
		L_SUB(&diff, &m);
	}
	if (L_ISGEQ(&limit, &diff)){
		return TRUE;
	}
	else {
		//<< m_expr << " which is " << l_fp_wrap(m)
		//<< "\nand\n"
		//<< n_expr << " which is " << l_fp_wrap(n)
		//<< "\nare not close; diff=" << l_fp_wrap(diff);
		return FALSE;
	}
}

/*
 * leitch_process - process a pile of samples from the clock
 *
 * This routine uses a three-stage median filter to calculate offset and
 * dispersion. reduce jitter. The dispersion is calculated as the span
 * of the filter (max - min), unless the quality character (format 2) is
 * non-blank, in which case the dispersion is calculated on the basis of
 * the inherent tolerance of the internal radio oscillator, which is
 * +-2e-5 according to the radio specifications.
 */
static void
leitch_process(
	struct leitchunit *leitch
	)
{
	l_fp off;
	l_fp tmp_fp;
      /*double doffset;*/

	off = leitch->reftime1;
	L_SUB(&off,&leitch->codetime1);
	tmp_fp = leitch->reftime2;
	L_SUB(&tmp_fp,&leitch->codetime2);
	if (L_ISGEQ(&off,&tmp_fp))
	    off = tmp_fp;
	tmp_fp = leitch->reftime3;
	L_SUB(&tmp_fp,&leitch->codetime3);

	if (L_ISGEQ(&off,&tmp_fp))
	    off = tmp_fp;
      /*LFPTOD(&off, doffset);*/
	refclock_receive(leitch->peer);
}

/*
 * refclock_process_offset - update median filter
 *
 * This routine uses the given offset and timestamps to construct a new
 * entry in the median filter circular buffer. Samples that overflow the
 * filter are quietly discarded.
 */
void
refclock_process_offset(
	struct refclockproc *pp,
	l_fp offset,
	l_fp lastrec,
	double fudge
	)
{
	double doffset;

	pp->lastref = offset;
	pp->lastrec = lastrec;
	L_SUB(&offset, &lastrec);
	LFPTOD(&offset, doffset);
	SAMPLE(doffset + fudge);
}

/*
 * refclock_process_offset - update median filter
 *
 * This routine uses the given offset and timestamps to construct a new
 * entry in the median filter circular buffer. Samples that overflow the
 * filter are quietly discarded.
 */
void
refclock_process_offset(
	struct refclockproc *pp,	/* refclock structure pointer */
	l_fp lasttim,			/* last timecode timestamp */
	l_fp lastrec,			/* last receive timestamp */
	double fudge
	)
{
	l_fp lftemp;
	double doffset;

	pp->lastrec = lastrec;
	lftemp = lasttim;
	L_SUB(&lftemp, &lastrec);
	LFPTOD(&lftemp, doffset);
	SAMPLE(doffset + fudge);
}

/*
 * cvt_trimtsip
 *
 * convert TSIP type format
 */
static unsigned long
cvt_trimtsip(
	     unsigned char *buffer,
	     int            size,
	     struct format *format,
	     clocktime_t   *clock_time,
	     void          *local
	     )
{
        register struct trimble *t = (struct trimble *)local; /* get local data space */
#define mb(_X_) (buffer[2+(_X_)]) /* shortcut for buffer access */
	register u_char cmd;

	clock_time->flags = 0;

	if (!t) {
		return CVT_NONE;		/* local data not allocated - sigh! */
	}

	if ((size < 4) ||
	    (buffer[0]      != DLE) ||
	    (buffer[size-1] != ETX) ||
	    (buffer[size-2] != DLE))
	{
		printf("TRIMBLE BAD packet, size %d:\n", size);
		return CVT_NONE;
	}
	else
	{
		unsigned char *bp;
		cmd = buffer[1];

		    switch(cmd)
		    {
		    case CMD_RCURTIME:
			    {			/* GPS time */
				    l_fp secs;
				    int   week = getshort((unsigned char *)&mb(4));
				    l_fp utcoffset;
				    l_fp gpstime;

				    bp = &mb(0);
				    if (fetch_ieee754(&bp, IEEE_SINGLE, &secs, trim_offsets) != IEEE_OK)
					    return CVT_FAIL|CVT_BADFMT;

				    if ((secs.l_i <= 0) ||
					(t->t_utcknown == 0))
				    {
					    clock_time->flags = PARSEB_POWERUP;
					    return CVT_OK;
				    }
				    if (week < GPSWRAP) {
					    week += GPSWEEKS;
				    }

				    /* time OK */

				    /* fetch UTC offset */
				    bp = &mb(6);
				    if (fetch_ieee754(&bp, IEEE_SINGLE, &utcoffset, trim_offsets) != IEEE_OK)
					    return CVT_FAIL|CVT_BADFMT;

				    L_SUB(&secs, &utcoffset); /* adjust GPS time to UTC time */

				    gpstolfp((unsigned short)week, (unsigned short)0,
					     secs.l_ui, &gpstime);

				    gpstime.l_uf = secs.l_uf;

				    clock_time->utctime = gpstime.l_ui - JAN_1970;

				    TSFTOTVU(gpstime.l_uf, clock_time->usecond);

				    if (t->t_leap == ADDSECOND)
					clock_time->flags |= PARSEB_LEAPADD;

				    if (t->t_leap == DELSECOND)
					clock_time->flags |= PARSEB_LEAPDEL;

				    switch (t->t_operable)
				      {
				      case STATUS_SYNC:
					clock_time->flags &= ~(PARSEB_POWERUP|PARSEB_NOSYNC);
					break;

				      case STATUS_UNSAFE:
					clock_time->flags |= PARSEB_NOSYNC;
					break;

				      case STATUS_BAD:
					clock_time->flags |= PARSEB_NOSYNC|PARSEB_POWERUP;
					break;
				      }

				    if (t->t_mode == 0)
//.........这里部分代码省略.........

void
offset_calculation(
	struct pkt *rpkt,
	int rpktl,
	struct timeval *tv_dst,
	double *offset,
	double *precision,
	double *synch_distance
	)
{
	l_fp p_rec, p_xmt, p_ref, p_org, tmp, dst;
	u_fp p_rdly, p_rdsp;
	double t21, t34, delta;

	/* Convert timestamps from network to host byte order */
	p_rdly = NTOHS_FP(rpkt->rootdelay);
	p_rdsp = NTOHS_FP(rpkt->rootdisp);
	NTOHL_FP(&rpkt->reftime, &p_ref);
	NTOHL_FP(&rpkt->org, &p_org);
	NTOHL_FP(&rpkt->rec, &p_rec);
	NTOHL_FP(&rpkt->xmt, &p_xmt);

	*precision = LOGTOD(rpkt->precision);

	TRACE(3, ("offset_calculation: LOGTOD(rpkt->precision): %f\n", *precision));

	/* Compute offset etc. */
	tmp = p_rec;
	L_SUB(&tmp, &p_org);
	LFPTOD(&tmp, t21);
	TVTOTS(tv_dst, &dst);
	dst.l_ui += JAN_1970;
	tmp = p_xmt;
	L_SUB(&tmp, &dst);
	LFPTOD(&tmp, t34);
	*offset = (t21 + t34) / 2.;
	delta = t21 - t34;

	// synch_distance is:
	// (peer->delay + peer->rootdelay) / 2 + peer->disp
	// + peer->rootdisp + clock_phi * (current_time - peer->update)
	// + peer->jitter;
	//
	// and peer->delay = fabs(peer->offset - p_offset) * 2;
	// and peer->offset needs history, so we're left with
	// p_offset = (t21 + t34) / 2.;
	// peer->disp = 0; (we have no history to augment this)
	// clock_phi = 15e-6; 
	// peer->jitter = LOGTOD(sys_precision); (we have no history to augment this)
	// and ntp_proto.c:set_sys_tick_precision() should get us sys_precision.
	//
	// so our answer seems to be:
	//
	// (fabs(t21 + t34) + peer->rootdelay) / 3.
	// + 0 (peer->disp)
	// + peer->rootdisp
	// + 15e-6 (clock_phi)
	// + LOGTOD(sys_precision)

	INSIST( FPTOD(p_rdly) >= 0. );
#if 1
	*synch_distance = (fabs(t21 + t34) + FPTOD(p_rdly)) / 3.
		+ 0.
		+ FPTOD(p_rdsp)
		+ 15e-6
		+ 0.	/* LOGTOD(sys_precision) when we can get it */
		;
	INSIST( *synch_distance >= 0. );
#else
	*synch_distance = (FPTOD(p_rdly) + FPTOD(p_rdsp))/2.0;
#endif

#ifdef DEBUG
	if (debug > 3) {
		printf("sntp rootdelay: %f\n", FPTOD(p_rdly));
		printf("sntp rootdisp: %f\n", FPTOD(p_rdsp));
		printf("sntp syncdist: %f\n", *synch_distance);

		pkt_output(rpkt, rpktl, stdout);

		printf("sntp offset_calculation: rpkt->reftime:\n");
		l_fp_output(&p_ref, stdout);
		printf("sntp offset_calculation: rpkt->org:\n");
		l_fp_output(&p_org, stdout);
		printf("sntp offset_calculation: rpkt->rec:\n");
		l_fp_output(&p_rec, stdout);
		printf("sntp offset_calculation: rpkt->xmt:\n");
		l_fp_output(&p_xmt, stdout);
	}
#endif

	TRACE(3, ("sntp offset_calculation:\trec - org t21: %.6f\n"
		  "\txmt - dst t34: %.6f\tdelta: %.6f\toffset: %.6f\n",
		  t21, t34, delta, *offset));

	return;
}

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

//.........这里部分代码省略.........
# endif		/* !HAVE_IO_COMPLETION_PORT */

# ifdef DEBUG_TIMING
		{
			l_fp pts;
			l_fp tsa, tsb;
			int bufcount = 0;

			get_systime(&pts);
			tsa = pts;
# endif
			rbuf = get_full_recv_buffer();
			while (rbuf != NULL) {
				if (alarm_flag) {
					was_alarmed = TRUE;
					alarm_flag = FALSE;
				}
				UNBLOCK_IO_AND_ALARM();

				if (was_alarmed) {
					/* avoid timer starvation during lengthy I/O handling */
					timer();
					was_alarmed = FALSE;
				}

				/*
				 * Call the data procedure to handle each received
				 * packet.
				 */
				if (rbuf->receiver != NULL) {
# ifdef DEBUG_TIMING
					l_fp dts = pts;

					L_SUB(&dts, &rbuf->recv_time);
					DPRINTF(2, ("processing timestamp delta %s (with prec. fuzz)\n", lfptoa(&dts, 9)));
					collect_timing(rbuf, "buffer processing delay", 1, &dts);
					bufcount++;
# endif
					(*rbuf->receiver)(rbuf);
				} else {
					msyslog(LOG_ERR, "fatal: receive buffer callback NULL");
					abort();
				}

				BLOCK_IO_AND_ALARM();
				freerecvbuf(rbuf);
				rbuf = get_full_recv_buffer();
			}
# ifdef DEBUG_TIMING
			get_systime(&tsb);
			L_SUB(&tsb, &tsa);
			if (bufcount) {
				collect_timing(NULL, "processing", bufcount, &tsb);
				DPRINTF(2, ("processing time for %d buffers %s\n", bufcount, lfptoa(&tsb, 9)));
			}
		}
# endif

		/*
		 * Go around again
		 */

# ifdef HAVE_DNSREGISTRATION
		if (mdnsreg && (current_time - mdnsreg ) > 60 && mdnstries && sys_leap != LEAP_NOTINSYNC) {
			mdnsreg = current_time;
			msyslog(LOG_INFO, "Attempting to register mDNS");