gpsd.c

来自「gpsd, a popular GPS daemon.」· C语言 代码 · 共 1,824 行 · 第 1/4 页

static void notify_watchers(struct gps_device_t *device, char *sentence, ...)
/* notify all clients watching a given device of an event */
{
    struct subscriber_t *sub;
    va_list ap;
    char buf[BUFSIZ];

    va_start(ap, sentence) ;
    (void)vsnprintf(buf, sizeof(buf), sentence, ap);
    va_end(ap);

    for (sub = subscribers; sub < subscribers + MAXSUBSCRIBERS; sub++)
	if (sub->watcher != 0 && sub->device == device)
	    (void)throttled_write(sub, buf, (ssize_t)strlen(buf));
}

static void raw_hook(struct gps_data_t *ud,
		     char *sentence, size_t len, int level)
/* hook to be executed on each incoming packet */
{
    struct subscriber_t *sub;

    for (sub = subscribers; sub < subscribers + MAXSUBSCRIBERS; sub++) {
	/* copy raw NMEA sentences from GPS to clients in raw mode */
	if (sub->raw == level &&
	    sub->device!=NULL &&
	    strcmp(ud->gps_device, sub->device->gpsdata.gps_device)==0)
	    (void)throttled_write(sub, sentence, (ssize_t)len);
    }
}

/*@ -globstate @*/
static /*@null@*/ /*@observer@*/struct gps_device_t *find_device(char *device_name)
/* find the channel block for an existing device name */
{
    struct gps_device_t *chp;

    for (chp = channels; chp < channels + MAXDEVICES; chp++)
	if (allocated_channel(chp) && strcmp(chp->gpsdata.gps_device, device_name)==0)
	    return chp;
    return NULL;
}

/*@ -statictrans @*/
static /*@null@*/ struct gps_device_t *open_device(char *device_name)
/* open and initialize a new channel block */
{
    struct gps_device_t *chp;

    /* special case: source may be a URL to differential-GPS service */
    if (dgnss_url(device_name)) {
	int dsock = dgnss_open(&context, device_name);
	if (dsock >= 0) {
	    FD_SET(dsock, &all_fds);
	    adjust_max_fd(dsock, true);
	}
	/* shaky, but only 0 versus nonzero is tested */
	return &channels[0];
    }

    /* normal case: set up GPS service */
    for (chp = channels; chp < channels + MAXDEVICES; chp++)
	if (!allocated_channel(chp)){
	    goto found;
	}
    return NULL;
found:
    gpsd_init(chp, &context, device_name);
    chp->gpsdata.raw_hook = raw_hook;
    /*
     * Bring the device all the way so we'll sniff packets from it and
     * discover up front whether it's a GPS source or an RTCM source.
     * Otherwise clients trying to bind to a specific type won't know
     * what source types are actually available.  If we're in nowait mode
     * the device has to be configured now; otherwise, it can wait.
     */
    if (gpsd_activate(chp, nowait) < 0)
	return NULL;
    FD_SET(chp->gpsdata.gps_fd, &all_fds);
    adjust_max_fd(chp->gpsdata.gps_fd, true);
    return chp;
}
/*@ +statictrans @*/
/*@ +globstate @*/

static bool allocation_policy(struct gps_device_t *channel,
			      struct subscriber_t *user,
			      double most_recent)
{
#ifdef __UNUSED__
    /* only allocate devices that we know the packet type of */
    if (channel->packet_type == BAD_PACKET)
	return false;
#endif /* __UNUSED__ */
    /* maybe we have already bound a more recently active device */
    if (user->device!=NULL && channel->gpsdata.sentence_time < most_recent)
	return false;
    gpsd_report(LOG_PROG, "User requires %d, channel type is %d\n", user->requires, channel->packet.type);
    /* we might have type constraints */
    if (user->requires == ANY)
	return true;
    else if (user->requires==RTCM104 && (channel->packet.type==RTCM_PACKET))
	return true;
    else if (user->requires == GPS
	     && (channel->packet.type!=RTCM_PACKET) && (channel->packet.type!=BAD_PACKET))
	return true;
    else
	return false;
}

/*@ -branchstate -usedef -globstate @*/
static bool assign_channel(struct subscriber_t *user)
{
    bool was_unassigned = (user->device == NULL);
    /* if subscriber has no device... */
    if (was_unassigned) {
	double most_recent = 0;
	struct gps_device_t *channel;

	gpsd_report(LOG_PROG, "client(%d): assigning channel...\n", user-subscribers);
	/* ...connect him to the most recently active device */
	/*@ -mustfreeonly @*/
	for(channel = channels; channel<channels+MAXDEVICES; channel++)
	    if (allocated_channel(channel)) {
		if (allocation_policy(channel, user, most_recent)) {
		    user->device = channel;
		    most_recent = channel->gpsdata.sentence_time;
		}
	    }
	/*@ +mustfreeonly @*/
    }

    if (user->device == NULL) {
	gpsd_report(LOG_ERROR, "client(%d): channel assignment failed.\n", user-subscribers);
	return false;
    }

    /* and open that device */
    if (user->device->gpsdata.gps_fd != -1)
	gpsd_report(LOG_PROG,"client(%d): channel %d already active.\n",
		    user-subscribers, user->device->gpsdata.gps_fd);
    else {
	if (gpsd_activate(user->device, true) < 0) {

	    gpsd_report(LOG_ERROR, "client(%d): channel activation failed.\n", user-subscribers);
	    return false;
	} else {
	    gpsd_report(LOG_RAW, "flagging descriptor %d in assign_channel\n", user->device->gpsdata.gps_fd);
	    FD_SET(user->device->gpsdata.gps_fd, &all_fds);
	    adjust_max_fd(user->device->gpsdata.gps_fd, true);
	    if (user->watcher && !user->tied) {
		(void)write(user->fd, "GPSD,F=", 7);
		(void)write(user->fd,
			    user->device->gpsdata.gps_device,
			    strlen(user->device->gpsdata.gps_device));
		(void)write(user->fd, "\r\n", 2);
	    }
	}
    }

    if (user->watcher && was_unassigned) {
	char buf[NMEA_MAX];
	(void)snprintf(buf, sizeof(buf), "GPSD,X=%f,I=%s\r\n",
		       timestamp(), gpsd_id(user->device));
	(void)write(user->fd, buf, strlen(buf));
    }
    return true;
}
/*@ +branchstate +usedef +globstate @*/

#ifdef RTCM104_SERVICE
static int handle_rtcm_request(struct subscriber_t* sub UNUSED, char *buf UNUSED, int buflen UNUSED)
/* interpret a client request; cfd is the socket back to the client */
{
    return 0;	/* not actually interpreting these yet */
}
#endif /* RTCM104_SERVICE */

static /*@ observer @*/ char *snarfline(char *p, /*@out@*/char **out)
/* copy the rest of the command line, before CR-LF */
{
    char *q;
    static char	stash[BUFSIZ];

    /*@ -temptrans -mayaliasunique @*/
    for (q = p; isprint(*p) && !isspace(*p) && /*@i@*/(p-q < BUFSIZ-1); p++)
	continue;
    (void)memcpy(stash, q, (size_t)(p-q));
    stash[p-q] = '\0';
    *out = stash;
    return p;
    /*@ +temptrans +mayaliasunique @*/
}

static bool privileged_user(struct subscriber_t *who)
/* is this user privileged to change the GPS's behavior? */
{
    struct subscriber_t *sub;
    int subscribercount = 0;

    /* grant user privilege if he's the only one listening to the device */
    for (sub = subscribers; sub < subscribers + MAXSUBSCRIBERS; sub++)
	if (sub->device == who->device)
	    subscribercount++;
    return (subscribercount == 1);
}

static int handle_gpsd_request(struct subscriber_t* sub, char *buf, int buflen)
/* interpret a client request; cfd is the socket back to the client */
{
    char reply[BUFSIZ], phrase[BUFSIZ], *p, *stash;
    int i, j;
    struct gps_device_t *newchan;

    (void)strlcpy(reply, "GPSD", BUFSIZ);
    p = buf;
    while (*p != '\0' && p - buf < buflen) {
	phrase[0] = '\0';
	switch (toupper(*p++)) {
	case 'A':
	    if (assign_channel(sub) && have_fix(sub) && sub->fixbuffer.mode == MODE_3D)
		(void)snprintf(phrase, sizeof(phrase), ",A=%.3f",
			sub->fixbuffer.altitude);
	    else
		(void)strlcpy(phrase, ",A=?", BUFSIZ);
	    break;
	case 'B':		/* change baud rate (SiRF/Zodiac only) */
#ifndef FIXED_PORT_SPEED
	    if (assign_channel(sub) && sub->device->device_type!=NULL && *p=='=' && privileged_user(sub) && !context.readonly) {
		i = atoi(++p);
		while (isdigit(*p)) p++;
#ifdef ALLOW_RECONFIGURE
		if (sub->device->device_type->speed_switcher)
		    if (sub->device->device_type->speed_switcher(sub->device, (unsigned)i)) {
			/*
			 * Allow the control string time to register at the
			 * GPS before we do the baud rate switch, which
			 * effectively trashes the UART's buffer.
			 *
			 * This definitely fails below 40 milliseconds on a
			 * BU-303b. 50ms is also verified by Chris Kuethe on
			 *	Pharos iGPS360 + GSW 2.3.1ES + prolific
			 *	Rayming TN-200 + GSW 2.3.1 + ftdi
			 *	Rayming TN-200 + GSW 2.3.2 + ftdi
			 * so it looks pretty solid.
			 *
			 * The minimum delay time is probably constant
			 * across any given type of UART.
			 */
			(void)tcdrain(sub->device->gpsdata.gps_fd);
			(void)usleep(50000);
			gpsd_set_speed(sub->device, (speed_t)i,
				(unsigned char)sub->device->gpsdata.parity,
				sub->device->gpsdata.stopbits);
		    }
#endif /* ALLOW_RECONFIGURE */
	    }
#endif /* FIXED_PORT_SPEED */
	    if (sub->device) {
		if ( sub->device->gpsdata.parity == 0 ) {
			/* zero parity breaks the next snprintf */
			sub->device->gpsdata.parity = (unsigned)'N';
		}
		(void)snprintf(phrase, sizeof(phrase), ",B=%d %d %c %u",
		    (int)gpsd_get_speed(&sub->device->ttyset),
			9 - sub->device->gpsdata.stopbits,
			(int)sub->device->gpsdata.parity,
			sub->device->gpsdata.stopbits);
	    } else {
		(void)strlcpy(phrase, ",B=?", BUFSIZ);
	    }
	    break;
	case 'C':
	    if (!assign_channel(sub) || sub->device->device_type==NULL)
		(void)strlcpy(phrase, ",C=?", BUFSIZ);
	    else {
		struct gps_type_t *dev = sub->device->device_type;
		double mincycle = (dev->cycle_chars * 10.0) / sub->device->gpsdata.baudrate;
		if (*p == '=' && privileged_user(sub)) {
		    double cycle = strtod(++p, &p);
		    if (cycle >= mincycle)
			if (dev->rate_switcher != NULL)
			    if (dev->rate_switcher(sub->device, cycle))
				dev->cycle = cycle;
		}
		if (dev->rate_switcher == NULL)
		    (void)snprintf(phrase, sizeof(phrase),
				   ",C=%.2f", dev->cycle);
		else
		    (void)snprintf(phrase, sizeof(phrase),
				   ",C=%.2f %.2f", dev->cycle, mincycle);
	    }
	    break;
	case 'D':
	    (void)strlcpy(phrase, ",D=", BUFSIZ);
	    if (assign_channel(sub) && isnan(sub->fixbuffer.time)==0)
		(void)unix_to_iso8601(sub->fixbuffer.time,
				phrase+3, sizeof(phrase)-3);
	    else
		(void)strlcat(phrase, "?", BUFSIZ);
	    break;
	case 'E':
	    (void)strlcpy(phrase, ",E=", BUFSIZ);
	    if (assign_channel(sub) && have_fix(sub)) {
		if (finite(sub->device->gpsdata.epe))
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   "%.2f", sub->device->gpsdata.epe * (CEP50_SIGMA/GPSD_CONFIDENCE));
		else
		    (void)strlcat(phrase, "?", sizeof(phrase));
		if (finite(sub->device->gpsdata.fix.eph))
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.2f", sub->device->gpsdata.fix.eph * (CEP50_SIGMA/GPSD_CONFIDENCE));
		else
		    (void)strlcat(phrase, " ?", sizeof(phrase));
		if (finite(sub->device->gpsdata.fix.epv))
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.2f", sub->device->gpsdata.fix.epv * (CEP50_SIGMA/GPSD_CONFIDENCE));
		else
		    (void)strlcat(phrase, " ?", sizeof(phrase));
	    } else
		(void)strlcat(phrase, "?", sizeof(phrase));
	    break;
	case 'F':
	    /*@ -branchstate @*/
	    if (*p == '=') {
		p = snarfline(++p, &stash);
		gpsd_report(LOG_INF,"<= client(%d): switching to %s\n",sub_index(sub),stash);
		if ((newchan = find_device(stash))) {
		    /*@i@*/sub->device = newchan;
		    sub->tied = true;
		}
	    }
	    /*@ +branchstate @*/
	    if (sub->device != NULL)
		(void)snprintf(phrase, sizeof(phrase), ",F=%s",
			 sub->device->gpsdata.gps_device);
	    else
		(void)strlcpy(phrase, ",F=?", BUFSIZ);
	    break;
	case 'G':
	    if (*p == '=') {
		gpsd_report(LOG_INF,"<= client(%d): requesting data type %s\n",sub_index(sub),++p);
		if (strncasecmp(p, "rtcm104", 7) == 0)
		    sub->requires = RTCM104;
		else if (strncasecmp(p, "gps", 3) == 0)
		    sub->requires = GPS;
		else
		    sub->requires = ANY;
		p += strcspn(p, ",\r\n");
	    }
	    (void)assign_channel(sub);
	    if (sub->device==NULL||sub->device->packet.type==BAD_PACKET)
		(void)strlcpy(phrase, ",G=?", BUFSIZ);
	    else if (sub->device->packet.type == RTCM_PACKET)
		(void)snprintf(phrase, sizeof(phrase), ",G=RTCM104");
	    else
		(void)snprintf(phrase, sizeof(phrase), ",G=GPS");
	    break;
	case 'I':
	    if (assign_channel(sub) && sub->device->device_type!=NULL) {
		(void)snprintf(phrase, sizeof(phrase), ",I=%s",
			       gpsd_id(sub->device));
	    } else
		(void)strlcpy(phrase, ",I=?", BUFSIZ);
	    break;
	case 'J':
	    if (*p == '=') ++p;
	    if (*p == '1' || *p == '+') {
		sub->buffer_policy = nocasoc;
		p++;
	    } else if (*p == '0' || *p == '-') {
		sub->buffer_policy = casoc;
		p++;
	    }
	    (void)snprintf(phrase, sizeof(phrase), ",J=%u", sub->buffer_policy);
	    break;
	case 'K':
	    for (j = i = 0; i < MAXDEVICES; i++)
		if (allocated_channel(&channels[i]))
		    j++;
	    (void)snprintf(phrase, sizeof(phrase), ",K=%d ", j);
	    for (i = 0; i < MAXDEVICES; i++) {
		if (allocated_channel(&channels[i]) && strlen(phrase)+strlen(channels[i].gpsdata.gps_device)+1 < sizeof(phrase)) {
		    (void)strlcat(phrase, channels[i].gpsdata.gps_device, BUFSIZ);
		    (void)strlcat(phrase, " ", BUFSIZ);
		}
	    }
	    phrase[strlen(phrase)-1] = '\0';
	    break;
	case 'L':
	    (void)snprintf(phrase, sizeof(phrase), ",L=3 " VERSION " abcdefgijklmnopqrstuvwxyz");	//h
	    break;
	case 'M':
	    if (!assign_channel(sub) && (!sub->device || sub->fixbuffer.mode == MODE_NOT_SEEN))
		(void)strlcpy(phrase, ",M=?", BUFSIZ);
	    else
		(void)snprintf(phrase, sizeof(phrase), ",M=%d", sub->fixbuffer.mode);
	    break;
	case 'N':
	    if (!assign_channel(sub) || sub->device->device_type == NULL)
		(void)strlcpy(phrase, ",N=?", BUFSIZ);
	    else if (!sub->device->device_type->mode_switcher)
		(void)strlcpy(phrase, ",N=0", BUFSIZ);
#ifdef ALLOW_RECONFIGURE
	    else if (privileged_user(sub) && !context.readonly) {
		if (*p == '=') ++p;
		if (*p == '1' || *p == '+') {
		    sub->device->device_type->mode_switcher(sub->device, 1);
		    p++;
		} else if (*p == '0' || *p == '-') {
		    sub->device->device_type->mode_switcher(sub->device, 0);
		    p++;
		}
	    }
#endif /* ALLOW_RECONFIGURE */
	    if (!sub->device)
		(void)snprintf(phrase, sizeof(phrase), ",N=?");
	    else
		(void)snprintf(phrase, sizeof(phrase), ",N=%u", sub->device->gpsdata.driver_mode);
	    break;
	case 'O':
	    if (!assign_channel(sub) || !have_fix(sub))
		(void)strlcpy(phrase, ",O=?", BUFSIZ);
	    else {
		(void)snprintf(phrase, sizeof(phrase), ",O=%s",
			       sub->device->gpsdata.tag[0]!='\0' ? sub->device->gpsdata.tag : "-");
		if (isnan(sub->fixbuffer.time)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.3f",
				   sub->fixbuffer.time);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.ept)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.3f",
				   sub->fixbuffer.ept);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.latitude)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.6f",
				   sub->fixbuffer.latitude);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.longitude)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.6f",
				   sub->fixbuffer.longitude);
		else