gpsd.c

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

		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.altitude)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.2f",
				   sub->fixbuffer.altitude);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.eph)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				  " %.2f",  sub->fixbuffer.eph);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.epv)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.2f",  sub->fixbuffer.epv);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.track)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.4f %.3f",
				   sub->fixbuffer.track,
				   sub->fixbuffer.speed);
		else
		    (void)strlcat(phrase, " ? ?", BUFSIZ);
		if (isnan(sub->fixbuffer.climb)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.3f",
				   sub->fixbuffer.climb);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.epd)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.4f",
				   sub->fixbuffer.epd);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.eps)==0)
		    (void)snprintf(phrase+strlen(phrase),
			     sizeof(phrase)-strlen(phrase),
			     " %.2f", sub->fixbuffer.eps);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (isnan(sub->fixbuffer.epc)==0)
		    (void)snprintf(phrase+strlen(phrase),
			     sizeof(phrase)-strlen(phrase),
			     " %.2f", sub->fixbuffer.epc);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
		if (sub->fixbuffer.mode > 0)
		    (void)snprintf(phrase+strlen(phrase),
			     sizeof(phrase)-strlen(phrase),
			     " %d", sub->fixbuffer.mode);
		else
		    (void)strlcat(phrase, " ?", BUFSIZ);
	    }
	    break;
	case 'P':
	    if (assign_channel(sub) && have_fix(sub))
		(void)snprintf(phrase, sizeof(phrase), ",P=%.6f %.6f",
			sub->fixbuffer.latitude,
			sub->fixbuffer.longitude);
	    else
		(void)strlcpy(phrase, ",P=?", BUFSIZ);
	    break;
	case 'Q':
#define ZEROIZE(x)	(isnan(x)!=0 ? 0.0 : x)
	    if (assign_channel(sub) &&
		(isnan(sub->device->gpsdata.pdop)==0
		 || isnan(sub->device->gpsdata.hdop)==0
		 || isnan(sub->device->gpsdata.vdop)==0))
		(void)snprintf(phrase, sizeof(phrase), ",Q=%d %.2f %.2f %.2f %.2f %.2f",
			sub->device->gpsdata.satellites_used,
			ZEROIZE(sub->device->gpsdata.pdop),
			ZEROIZE(sub->device->gpsdata.hdop),
			ZEROIZE(sub->device->gpsdata.vdop),
			ZEROIZE(sub->device->gpsdata.tdop),
			ZEROIZE(sub->device->gpsdata.gdop));
	    else
		(void)strlcpy(phrase, ",Q=?", BUFSIZ);
#undef ZEROIZE
	    break;
	case 'R':
	    if (*p == '=') ++p;
	    if (*p == '2') {
		(void)assign_channel(sub);
		sub->raw = 2;
		gpsd_report(LOG_INF, "client(%d) turned on super-raw mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",R=2");
		p++;
	    } else if (*p == '1' || *p == '+') {
		(void)assign_channel(sub);
		sub->raw = 1;
		gpsd_report(LOG_INF, "client(%d) turned on raw mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",R=1");
		p++;
	    } else if (*p == '0' || *p == '-') {
		sub->raw = 0;
		gpsd_report(LOG_INF, "client(%d) turned off raw mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",R=0");
		p++;
	    } else if (sub->raw) {
		sub->raw = 0;
		gpsd_report(LOG_INF, "client(%d) turned off raw mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",R=0");
	    } else {
		(void)assign_channel(sub);
		sub->raw = 1;
		gpsd_report(LOG_INF, "client(%d) turned on raw mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",R=1");
	    }
	    break;
	case 'S':
	    if (assign_channel(sub))
		(void)snprintf(phrase, sizeof(phrase), ",S=%d", sub->device->gpsdata.status);
	    else
		(void)strlcpy(phrase, ",S=?", BUFSIZ);
	    break;
	case 'T':
	    if (assign_channel(sub) && have_fix(sub) && isnan(sub->fixbuffer.track)==0)
		(void)snprintf(phrase, sizeof(phrase), ",T=%.4f", sub->fixbuffer.track);
	    else
		(void)strlcpy(phrase, ",T=?", BUFSIZ);
	    break;
	case 'U':
	    if (assign_channel(sub) && have_fix(sub) && sub->fixbuffer.mode == MODE_3D)
		(void)snprintf(phrase, sizeof(phrase), ",U=%.3f", sub->fixbuffer.climb);
	    else
		(void)strlcpy(phrase, ",U=?", BUFSIZ);
	    break;
	case 'V':
	    if (assign_channel(sub) && have_fix(sub) && isnan(sub->fixbuffer.speed)==0)
		(void)snprintf(phrase, sizeof(phrase), ",V=%.3f", sub->fixbuffer.speed * MPS_TO_KNOTS);
	    else
		(void)strlcpy(phrase, ",V=?", BUFSIZ);
	    break;
	case 'W':
	    if (*p == '=') ++p;
	    if (*p == '1' || *p == '+') {
		sub->watcher = true;
		(void)assign_channel(sub);
		(void)snprintf(phrase, sizeof(phrase), ",W=1");
		p++;
	    } else if (*p == '0' || *p == '-') {
		sub->watcher = false;
		(void)snprintf(phrase, sizeof(phrase), ",W=0");
		p++;
	    } else if (sub->watcher!=0) {
		sub->watcher = false;
		(void)snprintf(phrase, sizeof(phrase), ",W=0");
	    } else {
		sub->watcher = true;
		(void)assign_channel(sub);
		gpsd_report(LOG_INF, "client(%d) turned on watching\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",W=1");
	    }
	    break;
	case 'X':
	    if (assign_channel(sub) && sub->device != NULL)
		(void)snprintf(phrase, sizeof(phrase), ",X=%f", sub->device->gpsdata.online);
	    else
		(void)strlcpy(phrase, ",X=?", BUFSIZ);
	    break;
	case 'Y':
	    if (assign_channel(sub) && sub->device->gpsdata.satellites > 0) {
		int used, reported = 0;
		(void)strlcpy(phrase, ",Y=", BUFSIZ);
		if (sub->device->gpsdata.tag[0] != '\0')
		    (void)strlcat(phrase, sub->device->gpsdata.tag, BUFSIZ);
		else
		    (void)strlcat(phrase, "-", BUFSIZ);
		if (isnan(sub->device->gpsdata.sentence_time)==0)
		    (void)snprintf(phrase+strlen(phrase),
				   sizeof(phrase)-strlen(phrase),
				   " %.3f ",
				   sub->device->gpsdata.sentence_time);
		else
		    (void)strlcat(phrase, " ? ", BUFSIZ);
		/* insurance against flaky drivers */
		for (i = 0; i < sub->device->gpsdata.satellites; i++)
		    if (sub->device->gpsdata.PRN[i])
			reported++;
		(void)snprintf(phrase+strlen(phrase),
			       sizeof(phrase)-strlen(phrase),
			       "%d:", reported);
		for (i = 0; i < sub->device->gpsdata.satellites; i++) {
		    used = 0;
		    for (j = 0; j < sub->device->gpsdata.satellites_used; j++)
			if (sub->device->gpsdata.used[j] == sub->device->gpsdata.PRN[i]) {
			    used = 1;
			    break;
			}
		    if (sub->device->gpsdata.PRN[i]) {
			(void)snprintf(phrase+strlen(phrase),
				      sizeof(phrase)-strlen(phrase),
				      "%d %d %d %d %d:",
				      sub->device->gpsdata.PRN[i],
				      sub->device->gpsdata.elevation[i],sub->device->gpsdata.azimuth[i],
				      sub->device->gpsdata.ss[i],
				      used);
		    }
		}
		if (sub->device->gpsdata.satellites != reported)
		    gpsd_report(LOG_WARN,"Satellite count %d != PRN count %d\n",
				sub->device->gpsdata.satellites, reported);
	    } else
		(void)strlcpy(phrase, ",Y=?", BUFSIZ);
	    break;
	case 'Z':
	    (void)assign_channel(sub);
	    if (*p == '=') ++p;
	    if (sub->device == NULL) {
		(void)snprintf(phrase, sizeof(phrase), ",Z=?");
		p++;
	    } else if (*p == '1' || *p == '+') {
		sub->device->gpsdata.profiling = true;
		gpsd_report(LOG_INF, "client(%d) turned on profiling mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",Z=1");
		p++;
	    } else if (*p == '0' || *p == '-') {
		sub->device->gpsdata.profiling = false;
		gpsd_report(LOG_INF, "client(%d) turned off profiling mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",Z=0");
		p++;
	    } else {
		sub->device->gpsdata.profiling = !sub->device->gpsdata.profiling;
		gpsd_report(LOG_INF, "client(%d) toggled profiling mode\n", sub_index(sub));
		(void)snprintf(phrase, sizeof(phrase), ",Z=%d",
			       (int)sub->device->gpsdata.profiling);
	    }
	    break;
	case '$':
	    if (!assign_channel(sub))
		(void)strlcpy(phrase, ",$=?", BUFSIZ);
	    else if (sub->device->gpsdata.sentence_time!=0)
		(void)snprintf(phrase, sizeof(phrase), ",$=%s %d %lf %lf %lf %lf %lf %lf",
			sub->device->gpsdata.tag,
			(int)sub->device->gpsdata.sentence_length,
			sub->device->gpsdata.sentence_time,
			sub->device->gpsdata.d_xmit_time - sub->device->gpsdata.sentence_time,
			sub->device->gpsdata.d_recv_time - sub->device->gpsdata.sentence_time,
			sub->device->gpsdata.d_decode_time - sub->device->gpsdata.sentence_time,
			sub->device->poll_times[sub_index(sub)] - sub->device->gpsdata.sentence_time,
			timestamp() - sub->device->gpsdata.sentence_time);
	    else
		(void)snprintf(phrase, sizeof(phrase), ",$=%s %d 0 %lf %lf %lf %lf %lf",
			sub->device->gpsdata.tag,
			(int)sub->device->gpsdata.sentence_length,
			sub->device->gpsdata.d_xmit_time,
			sub->device->gpsdata.d_recv_time - sub->device->gpsdata.d_xmit_time,
			sub->device->gpsdata.d_decode_time - sub->device->gpsdata.d_xmit_time,
			sub->device->poll_times[sub_index(sub)] - sub->device->gpsdata.d_xmit_time,
			timestamp() - sub->device->gpsdata.d_xmit_time);
	    break;
	case '\r': case '\n':
	    goto breakout;
	}
	if (strlen(reply) + strlen(phrase) < sizeof(reply) - 1)
	    (void)strlcat(reply, phrase, BUFSIZ);
	else
	    return -1;	/* Buffer would overflow.  Just return an error */
    }
 breakout:
    (void)strlcat(reply, "\r\n", BUFSIZ);

    return (int)throttled_write(sub, reply, (ssize_t)strlen(reply));
}

static void handle_control(int sfd, char *buf)
/* handle privileged commands coming through the control socket */
{
    char	*p, *stash, *eq;
    struct gps_device_t	*chp;
    int cfd;

    if (buf[0] == '-') {
	p = snarfline(buf+1, &stash);
	gpsd_report(LOG_INF, "<= control(%d): removing %s\n", sfd, stash);
	if ((chp = find_device(stash))) {
	    if (chp->gpsdata.gps_fd > 0) {
		FD_CLR(chp->gpsdata.gps_fd, &all_fds);
		adjust_max_fd(chp->gpsdata.gps_fd, false);
	    }
	    notify_watchers(chp, "X=0\r\n");
	    for (cfd = 0; cfd < MAXSUBSCRIBERS; cfd++)
		if (subscribers[cfd].device == chp)
		    subscribers[cfd].device = NULL;
	    gpsd_wrap(chp);
	    chp->gpsdata.gps_fd = -1;	/* device is already disconnected */
	    /*@i@*/free_channel(chp);	/* modifying observer storage */
	    (void)write(sfd, "OK\n", 3);
	} else
	    (void)write(sfd, "ERROR\n", 6);
    } else if (buf[0] == '+') {
	p = snarfline(buf+1, &stash);
	if (find_device(stash)) {
	    gpsd_report(LOG_INF,"<= control(%d): %s already active \n", sfd, stash);
		(void)write(sfd, "ERROR\n", 6);
	} else {
	    gpsd_report(LOG_INF,"<= control(%d): adding %s \n", sfd, stash);
	    if (open_device(stash))
		(void)write(sfd, "OK\n", 3);
	    else
		(void)write(sfd, "ERROR\n", 6);
	}
    } else if (buf[0] == '!') {
	p = snarfline(buf+1, &stash);
	eq = strchr(stash, '=');
	if (eq == NULL) {
	    gpsd_report(LOG_WARN,"<= control(%d): ill-formed command \n", sfd);
	    (void)write(sfd, "ERROR\n", 3);
	} else {
	    *eq++ = '\0';
	    if ((chp = find_device(stash))) {
		gpsd_report(LOG_INF,"<= control(%d): writing to %s \n", sfd, stash);
		(void)write(chp->gpsdata.gps_fd, eq, strlen(eq));
		(void)write(sfd, "OK\n", 3);
	    } else {
		gpsd_report(LOG_INF,"<= control(%d): %s not active \n", sfd, stash);
		(void)write(sfd, "ERROR\n", 6);
	    }
	}
    }
}

/*@ -mustfreefresh @*/
int main(int argc, char *argv[])
{
    static char *pid_file = NULL;
    static int st, csock = -1;
    static gps_mask_t changed;
    static char *gpsd_service = NULL;
#ifdef RTCM104_SERVICE
    static char *rtcm_service = NULL;
    static int nsock;
#endif /* RTCM104_SERVICE */
    static char *control_socket = NULL;
    struct gps_device_t *channel;
    struct sockaddr_in fsin;
    fd_set rfds, control_fds;
    int i, option, msock, cfd, dfd;
    bool go_background = true;
    struct timeval tv;
#ifdef RTCM104_SERVICE
    struct gps_device_t *gps;
#endif /* RTCM104_SERVICE */
    struct subscriber_t *sub;

#ifdef PPS_ENABLE
    pthread_mutex_init(&report_mutex, NULL);
#endif /* PPS_ENABLE */

#ifdef HAVE_SETLOCALE
    (void)setlocale(LC_NUMERIC, "C");
#endif
    debuglevel = 0;
    while ((option = getopt(argc, argv, "F:D:S:bhNnP:V"
#ifdef RTCM104_SERVICE
			    "R:"
#endif /* RTCM104_SERVICE */
		)) != -1) {
	switch (option) {
	case 'D':
	    debuglevel = (int) strtol(optarg, 0, 0);
	    break;
	case 'F':
	    control_socket = optarg;
	    break;
	case 'N':
	    go_background = false;
	    break;
	case 'b':
	    context.readonly = true;
	    break;
#ifdef RTCM104_SERVICE
	case 'R':
	    rtcm_service = optarg;
	    break;
#endif /* RTCM104_SERVICE */
	case 'S':
	    gpsd_service = optarg;
	    break;
	case 'n':
	    nowait = true;
	    break;
	case 'P':
	    pid_file = optarg;
	    break;
	case 'V':
	    (void)printf("gpsd %s\n", VERSION);
	    exit(0);
	case 'h': case '?':
	default:
	    usage();
	    exit(0);
	}
    }

#ifdef FIXED_PORT_SPEED
    /* Asssume that if we're running with FIXED_PORT_SPEED we're some sort
     * of embedded configuration where we don't want to wait for connect */
    nowait = true;
#endif

    if (!control_socket && optind >= argc) {
	gpsd_report(LOG_ERROR, "can't run with neither control socket nor devices\n");
	exit(1);
    }

    /*
     * Control socket has to be created before we go background in order to
     * avoid a race condition in which hotplug scripts can try opening
     * the socket before it's created.
     */
    if (control_socket) {
	(void)unlink(control_socket);
	if ((csock = filesock(control_socket)) < 0) {
	    gpsd_report(LOG_ERROR,"control socket create failed, netlib error %d\n",csock);
	    exit(2);
	}
	FD_SET(csock, &all_fds);
	adjust_max_fd(csock, true);
	gpsd_report(LOG_PROG, "control socket opened at %s\n", control_socket);
    }

    if (go_background)
	(void)daemonize();

    if (pid_file) {
	FILE *fp;

	if ((fp = fopen(pid_file, "w")) != NULL) {
	    (void)fprintf(fp, "%u\n", (unsigned int)getpid());
	    (void)fclose(fp);
	} else {
	    gpsd_report(LOG_ERROR, "Cannot create PID file: %s.\n", pid_file);
	}
    }

    openlog("gpsd", LOG_PID, LOG_USER);
    gpsd_report(LOG_INF, "launching (Version %s)\n", VERSION);
    /*@ -observertrans @*/
    if (!gpsd_service)
	gpsd_service = getservbyname("gpsd", "tcp") ? "gpsd" : DEFAULT_GPSD_PORT;
    /*@ +observertrans @*/
    if ((msock = passivesock(gpsd_service, "tcp", QLEN)) < 0) {
	gpsd_report(LOG_ERR,"command socket create failed, netlib error %d\n",msock);
	exit(2);
    }
    gpsd_report(LOG_INF, "listening on port %s\n", gpsd_service);
#ifdef RTCM104_SERVICE