static int ntrip_stream_get_req(const struct ntrip_stream_t *stream,
const struct gpsd_errout_t *errout)
{
int dsock;
char buf[BUFSIZ];
dsock = netlib_connectsock(AF_UNSPEC, stream->url, stream->port, "tcp");
if (BAD_SOCKET(dsock)) {
gpsd_log(errout, LOG_ERROR,
"ntrip stream connect error %d\n", dsock);
return -1;
}
gpsd_log(errout, LOG_SPIN,
"netlib_connectsock() returns socket on fd %d\n",
dsock);
(void)snprintf(buf, sizeof(buf),
"GET /%s HTTP/1.1\r\n"
"User-Agent: NTRIP gpsd/%s\r\n"
"Host: %s\r\n"
"Accept: rtk/rtcm, dgps/rtcm\r\n"
"%s"
"Connection: close\r\n"
"\r\n", stream->mountpoint, VERSION, stream->url, stream->authStr);
if (write(dsock, buf, strlen(buf)) != (ssize_t) strlen(buf)) {
gpsd_log(errout, LOG_ERROR,
"ntrip stream write error %d on fd %d during get request\n", errno,
dsock);
(void)close(dsock);
return -1;
}
return dsock;
}
void gpsd_close(struct gps_device_t *session)
{
if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
(void)ioctl(session->gpsdata.gps_fd, (unsigned long)TIOCNXCL);
(void)tcdrain(session->gpsdata.gps_fd);
if (isatty(session->gpsdata.gps_fd) != 0) {
/* force hangup on close on systems that don't do HUPCL properly */
/*@ ignore @*/
(void)cfsetispeed(&session->ttyset, (speed_t) B0);
(void)cfsetospeed(&session->ttyset, (speed_t) B0);
/*@ end @*/
(void)tcsetattr(session->gpsdata.gps_fd, TCSANOW,
&session->ttyset);
}
/* this is the clean way to do it */
session->ttyset_old.c_cflag |= HUPCL;
/*
* Don't revert the serial parameters if we didn't have to mess with
* them the first time. Economical, and avoids tripping over an
* obscure Linux 2.6 kernel bug that disables threaded
* ioctl(TIOCMWAIT) on a device after tcsetattr() is called.
*/
if (cfgetispeed(&session->ttyset_old) != cfgetispeed(&session->ttyset) || (session->ttyset_old.c_cflag & CSTOPB) != (session->ttyset.c_cflag & CSTOPB)) {
/*
* If we revert, keep the most recent baud rate.
* Cuts down on autobaud overhead the next time.
*/
/*@ ignore @*/
(void)cfsetispeed(&session->ttyset_old,
(speed_t) session->gpsdata.dev.baudrate);
(void)cfsetospeed(&session->ttyset_old,
(speed_t) session->gpsdata.dev.baudrate);
/*@ end @*/
(void)tcsetattr(session->gpsdata.gps_fd, TCSANOW,
&session->ttyset_old);
}
gpsd_report(session->context->debug, LOG_SPIN,
"close(%d) in gpsd_close(%s)\n",
session->gpsdata.gps_fd, session->gpsdata.dev.path);
(void)close(session->gpsdata.gps_fd);
session->gpsdata.gps_fd = -1;
}
}
/* zodiac_spew - Takes a message type, an array of data words, and a length
* for the array, and prepends a 5 word header (including checksum).
* The data words are expected to be checksummed.
*/
static ssize_t zodiac_spew(struct gps_device_t *session, unsigned short type,
unsigned short *dat, int dlen)
{
struct header h;
int i;
char buf[BUFSIZ];
h.sync = 0x81ff;
h.id = (unsigned short)type;
h.ndata = (unsigned short)(dlen - 1);
h.flags = 0;
h.csum = zodiac_checksum((unsigned short *)&h, 4);
if (!BAD_SOCKET(session->gpsdata.gps_fd)) {
size_t hlen, datlen;
hlen = sizeof(h);
datlen = sizeof(unsigned short) * dlen;
if (end_write(session->gpsdata.gps_fd, &h, hlen) != (ssize_t) hlen ||
end_write(session->gpsdata.gps_fd, dat,
datlen) != (ssize_t) datlen) {
gpsd_log(&session->context->errout, LOG_RAW,
"Reconfigure write failed\n");
return -1;
}
}
(void)snprintf(buf, sizeof(buf),
"%04x %04x %04x %04x %04x",
h.sync, h.id, h.ndata, h.flags, h.csum);
for (i = 0; i < dlen; i++)
str_appendf(buf, sizeof(buf), " %04x", dat[i]);
gpsd_log(&session->context->errout, LOG_RAW,
"Sent Zodiac packet: %s\n", buf);
return 0;
}
int main(int argc, char *argv[])
{
int option, rc;
struct sockaddr_in localAddr, servAddr;
struct hostent *h;
int n;
for(n=0;n<CLIMB;n++) climb[n]=0.0;
switch (gpsd_units())
{
case imperial:
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_MPH;
speedunits = "mph";
break;
case nautical:
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_KNOTS;
speedunits = "knots";
break;
case metric:
altfactor = 1;
altunits = "m";
speedfactor = MPS_TO_KPH;
speedunits = "kph";
break;
default:
/* leave the default alone */
break;
}
/* Process the options. Print help if requested. */
while ((option = getopt(argc, argv, "Vhl:su:")) != -1) {
switch (option) {
case 'V':
(void)fprintf(stderr, "lcdgs revision " REVISION "\n");
exit(EXIT_SUCCESS);
case 'h':
default:
usage(argv[0]);
break;
case 'l':
switch ( optarg[0] ) {
case 'd':
deg_type = deg_dd;
continue;
case 'm':
deg_type = deg_ddmm;
continue;
case 's':
deg_type = deg_ddmmss;
continue;
default:
(void)fprintf(stderr, "Unknown -l argument: %s\n", optarg);
}
break;
case 's':
sleep(10);
continue;
case 'u':
switch ( optarg[0] ) {
case 'i':
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_MPH;
speedunits = "mph";
continue;
case 'n':
altfactor = METERS_TO_FEET;
altunits = "ft";
speedfactor = MPS_TO_KNOTS;
speedunits = "knots";
continue;
case 'm':
altfactor = 1;
altunits = "m";
speedfactor = MPS_TO_KPH;
speedunits = "kph";
continue;
default:
(void)fprintf(stderr, "Unknown -u argument: %s\n", optarg);
}
}
}
/* Grok the server, port, and device. */
if (optind < argc) {
gpsd_source_spec(argv[optind], &source);
} else
gpsd_source_spec(NULL, &source);
/* Daemonize... */
if (daemon(0, 0) != 0)
(void)fprintf(stderr,
"lcdgps: demonization failed: %s\n",
strerror(errno));
/* Open the stream to gpsd. */
if (gps_open(source.server, source.port, &gpsdata) != 0) {
(void)fprintf( stderr,
"lcdgps: no gpsd running or network error: %d, %s\n",
errno, gps_errstr(errno));
exit(EXIT_FAILURE);
}
/* Connect to LCDd */
h = gethostbyname(LCDDHOST);
if (h==NULL) {
printf("%s: unknown host '%s'\n",argv[0],LCDDHOST);
exit(EXIT_FAILURE);
}
servAddr.sin_family = h->h_addrtype;
memcpy((char *) &servAddr.sin_addr.s_addr, h->h_addr_list[0], h->h_length);
servAddr.sin_port = htons(LCDDPORT);
/* create socket */
sd = socket(AF_INET, SOCK_STREAM, 0);
if (BAD_SOCKET(sd)) {
perror("cannot open socket ");
exit(EXIT_FAILURE);
}
/* bind any port number */
localAddr.sin_family = AF_INET;
localAddr.sin_addr.s_addr = htonl(INADDR_ANY);
localAddr.sin_port = htons(0);
/* coverity[uninit_use_in_call] */
rc = bind(sd, (struct sockaddr *) &localAddr, sizeof(localAddr));
if (rc == -1) {
printf("%s: cannot bind port TCP %u\n",argv[0],LCDDPORT);
perror("error ");
exit(EXIT_FAILURE);
}
/* connect to server */
/* coverity[uninit_use_in_call] */
rc = connect(sd, (struct sockaddr *) &servAddr, sizeof(servAddr));
if (rc == -1) {
perror("cannot connect ");
exit(EXIT_FAILURE);
}
/* Do the initial field label setup. */
reset_lcd();
/* Here's where updates go. */
unsigned int flags = WATCH_ENABLE;
if (source.device != NULL)
flags |= WATCH_DEVICE;
(void)gps_stream(&gpsdata, flags, source.device);
for (;;) { /* heart of the client */
if (!gps_waiting(&gpsdata, 50000000)) {
fprintf( stderr, "lcdgps: error while waiting\n");
exit(EXIT_FAILURE);
} else {
(void)gps_read(&gpsdata);
update_lcd(&gpsdata);
}
}
}
/* in order to save builtin numbers, create a single socket function with
* options socket_request(SockOperation,....) */
xsbBool xsb_socket_request(CTXTdecl)
{
int ecode = 0; /* error code for socket ops */
int timeout_flag;
SOCKET sock_handle;
int domain, portnum;
SOCKADDR_IN socket_addr;
struct linger sock_linger_opt;
int rc;
char *message_buffer = NULL; /* initialized to keep compiler happy */
UInteger msg_len = 0; /* initialized to keep compiler happy */
char char_read;
switch (ptoc_int(CTXTc 1)) {
case SOCKET_ROOT: /* this is the socket() request */
/* socket_request(SOCKET_ROOT,+domain,-socket_fd,-Error,_,_,_)
Currently only AF_INET domain */
domain = (int)ptoc_int(CTXTc 2);
if (!translate_domain(domain, &domain)) {
return FALSE;
}
sock_handle = socket(domain, SOCK_STREAM, IPPROTO_TCP);
/* error handling */
if (BAD_SOCKET(sock_handle)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_REQUEST");
} else {
ecode = SOCK_OK;
}
ctop_int(CTXTc 3, (SOCKET) sock_handle);
return set_error_code(CTXTc ecode, 4, "SOCKET_REQUEST");
case SOCKET_BIND:
/* socket_request(SOCKET_BIND,+domain,+sock_handle,+port,-Error,_,_)
Currently only supports AF_INET */
sock_handle = (SOCKET) ptoc_int(CTXTc 3);
portnum = (int)ptoc_int(CTXTc 4);
domain = (int)ptoc_int(CTXTc 2);
if (!translate_domain(domain, &domain)) {
return FALSE;
}
/* Bind server to the agreed upon port number.
** See commdef.h for the actual port number. */
FillWithZeros(socket_addr);
socket_addr.sin_port = htons((unsigned short)portnum);
socket_addr.sin_family = AF_INET;
#ifndef WIN_NT
socket_addr.sin_addr.s_addr = htonl(INADDR_ANY);
#endif
rc = bind(sock_handle, (PSOCKADDR) &socket_addr, sizeof(socket_addr));
/* error handling */
if (SOCKET_OP_FAILED(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_BIND");
} else
ecode = SOCK_OK;
return set_error_code(CTXTc ecode, 5, "SOCKET_BIND");
case SOCKET_LISTEN:
/* socket_request(SOCKET_LISTEN,+sock_handle,+length,-Error,_,_,_) */
sock_handle = (SOCKET) ptoc_int(CTXTc 2);
rc = listen(sock_handle, (int)ptoc_int(CTXTc 3));
/* error handling */
if (SOCKET_OP_FAILED(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_LISTEN");
} else
ecode = SOCK_OK;
return set_error_code(CTXTc ecode, 4, "SOCKET_LISTEN");
case SOCKET_ACCEPT:
timeout_flag = socket_accept(CTXTc (SOCKET *)&rc, (int)pflags[SYS_TIMER]);
if (timeout_flag == TIMED_OUT) {
return set_error_code(CTXTc TIMEOUT_ERR, 4, "SOCKET_SEND");
} else {
/* error handling */
if (BAD_SOCKET(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_ACCEPT");
sock_handle = rc; /* shut up warning */
} else {
sock_handle = rc; /* accept() returns sock_out */
ecode = SOCK_OK;
}
ctop_int(CTXTc 3, (SOCKET) sock_handle);
return set_error_code(CTXTc ecode, 4, "SOCKET_ACCEPT");
}
case SOCKET_CONNECT: {
/* socket_request(SOCKET_CONNECT,+domain,+sock_handle,+port,
+hostname,-Error) */
timeout_flag = socket_connect(CTXTc &rc, (int)pflags[SYS_TIMER]);
if (timeout_flag == TIMED_OUT) {
return set_error_code(CTXTc TIMEOUT_ERR, 6, "SOCKET_CONNECT");
} else if (timeout_flag == TIMER_SETUP_ERR) {
return set_error_code(CTXTc TIMER_SETUP_ERR, 6, "SOCKET_CONNECT");
} else {
/* error handling */
if (SOCKET_OP_FAILED(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_CONNECT");
/* close, because if connect() fails then socket becomes unusable */
closesocket(ptoc_int(CTXTc 3));
} else {
ecode = SOCK_OK;
}
return set_error_code(CTXTc ecode, 6, "SOCKET_CONNECT");
}
}
case SOCKET_CLOSE:
/* socket_request(SOCKET_CLOSE,+sock_handle,-Error,_,_,_,_) */
sock_handle = (SOCKET)ptoc_int(CTXTc 2);
/* error handling */
rc = closesocket(sock_handle);
if (SOCKET_OP_FAILED(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_CLOSE");
} else
ecode = SOCK_OK;
return set_error_code(CTXTc ecode, 3, "SOCKET_CLOSE");
case SOCKET_RECV:
/* socket_request(SOCKET_RECV,+Sockfd, -Msg, -Error,_,_,_) */
// TODO: consider adding protection against interrupts, EINTR, like
// in socket_get0.
timeout_flag = socket_recv(CTXTc &rc, &message_buffer, &msg_len, (int)pflags[SYS_TIMER]);
if (timeout_flag == TIMED_OUT) {
return set_error_code(CTXTc TIMEOUT_ERR, 4, "SOCKET_SEND");
} else {
/* error handling */
switch (rc) {
case SOCK_OK:
ecode = SOCK_OK;
break;
case SOCK_READMSG_FAILED:
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_RECV");
break;
case SOCK_READMSG_EOF:
ecode = SOCK_EOF;
break;
case SOCK_HEADER_LEN_MISMATCH:
ecode = XSB_SOCKET_ERRORCODE;
break;
default:
xsb_abort("XSB bug: [SOCKET_RECV] invalid return code from readmsg");
}
if (message_buffer != NULL) {
/* use message_buffer+XSB_MSG_HEADER_LENGTH because the first
XSB_MSG_HEADER_LENGTH bytes are for the message length header */
ctop_string(CTXTc 3, (char*)message_buffer+XSB_MSG_HEADER_LENGTH);
mem_dealloc(message_buffer,msg_len,OTHER_SPACE);
} else { /* this happens at the end of a file */
ctop_string(CTXTc 3, (char*)"");
}
return set_error_code(CTXTc ecode, 4, "SOCKET_RECV");
}
case SOCKET_SEND:
/* socket_request(SOCKET_SEND,+Sockfd, +Msg, -Error,_,_,_) */
timeout_flag = socket_send(CTXTc &rc, (int)pflags[SYS_TIMER]);
if (timeout_flag == TIMED_OUT) {
return set_error_code(CTXTc TIMEOUT_ERR, 4, "SOCKET_SEND");
} else {
/* error handling */
if (SOCKET_OP_FAILED(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_SEND");
} else {
ecode = SOCK_OK;
}
return set_error_code(CTXTc ecode, 4, "SOCKET_SEND");
}
case SOCKET_GET0:
/* socket_request(SOCKET_GET0,+Sockfd,-C,-Error,_,_,_) */
message_buffer = &char_read;
timeout_flag = socket_get0(CTXTc &rc, message_buffer, (int)pflags[SYS_TIMER]);
if (timeout_flag == TIMED_OUT) {
return set_error_code(CTXTc TIMEOUT_ERR, 4, "SOCKET_SEND");
} else {
/*error handling */
switch (rc) {
case 1:
ctop_int(CTXTc 3,(unsigned char)message_buffer[0]);
ecode = SOCK_OK;
break;
case 0:
ecode = SOCK_EOF;
break;
default:
ctop_int(CTXTc 3,-1);
perror("SOCKET_GET0");
ecode = XSB_SOCKET_ERRORCODE;
}
return set_error_code(CTXTc ecode, 4, "SOCKET_GET0");
}
case SOCKET_PUT:
/* socket_request(SOCKET_PUT,+Sockfd,+C,-Error_,_,_) */
timeout_flag = socket_put(CTXTc &rc, (int)pflags[SYS_TIMER]);
if (timeout_flag == TIMED_OUT) {
return set_error_code(CTXTc TIMEOUT_ERR, 4, "SOCKET_SEND");
} else {
/* error handling */
if (rc == 1) {
ecode = SOCK_OK;
} else if (SOCKET_OP_FAILED(rc)) {
ecode = XSB_SOCKET_ERRORCODE;
perror("SOCKET_PUT");
}
return set_error_code(CTXTc ecode, 4, "SOCKET_PUT");
}
case SOCKET_SET_OPTION: {
/* socket_request(SOCKET_SET_OPTION,+Sockfd,+OptionName,+Value,_,_,_) */
char *option_name = ptoc_string(CTXTc 3);
sock_handle = (SOCKET)ptoc_int(CTXTc 2);
/* Set the "linger" parameter to a small number of seconds */
if (0==strcmp(option_name,"linger")) {
int linger_time=(int)ptoc_int(CTXTc 4);
if (linger_time < 0) {
sock_linger_opt.l_onoff = FALSE;
sock_linger_opt.l_linger = 0;
} else {
sock_linger_opt.l_onoff = TRUE;
sock_linger_opt.l_linger = linger_time;
}
if (SETSOCKOPT(sock_handle, SOL_SOCKET, SO_LINGER,
&sock_linger_opt, sizeof(sock_linger_opt))
< 0) {
xsb_warn(CTXTc "[SOCKET_SET_OPTION] Cannot set socket linger time");
return FALSE;
}
}else {
xsb_warn(CTXTc "[SOCKET_SET_OPTION] Invalid option, `%s'", option_name);
return FALSE;
}
return TRUE;
}
case SOCKET_SET_SELECT: {
/*socket_request(SOCKET_SET_SELECT,+connection_name,
+R_sockfd,+W_sockfd,+E_sockfd) */
prolog_term R_sockfd, W_sockfd, E_sockfd;
int i, connection_count;
int rmax_fd=0, wmax_fd=0, emax_fd=0;
char *connection_name = ptoc_string(CTXTc 2);
/* bind fds to input arguments */
R_sockfd = reg_term(CTXTc 3);
W_sockfd = reg_term(CTXTc 4);
E_sockfd = reg_term(CTXTc 5);
/* initialize the array of connect_t structure for select call */
init_connections(CTXT);
SYS_MUTEX_LOCK(MUTEX_SOCKETS);
/* check whether the same connection name exists */
for (i=0;i<MAXCONNECT;i++) {
if ((connections[i].empty_flag==FALSE) &&
(strcmp(connection_name,connections[i].connection_name)==0))
xsb_abort("[SOCKET_SET_SELECT] Connection `%s' already exists!",
connection_name);
}
/* check whether there is empty slot left for connection */
if ((connection_count=checkslot())<MAXCONNECT) {
if (connections[connection_count].connection_name == NULL) {
connections[connection_count].connection_name = connection_name;
connections[connection_count].empty_flag = FALSE;
/* call the utility function separately to take the fds in */
list_sockfd(R_sockfd, &connections[connection_count].readset,
&rmax_fd, &connections[connection_count].read_fds,
&connections[connection_count].sizer);
list_sockfd(W_sockfd, &connections[connection_count].writeset,
&wmax_fd, &connections[connection_count].write_fds,
&connections[connection_count].sizew);
list_sockfd(E_sockfd, &connections[connection_count].exceptionset,
&emax_fd,&connections[connection_count].exception_fds,
&connections[connection_count].sizee);
connections[connection_count].maximum_fd =
xsb_max(xsb_max(rmax_fd,wmax_fd), emax_fd);
} else
/* if this one is reached, it is probably a bug */
xsb_abort("[SOCKET_SET_SELECT] All connections are busy!");
} else
xsb_abort("[SOCKET_SET_SELECT] Max number of collections exceeded!");
SYS_MUTEX_UNLOCK(MUTEX_SOCKETS);
return TRUE;
}
case SOCKET_SELECT: {
/* socket_request(SOCKET_SELECT,+connection_name, +timeout
-avail_rsockfds,-avail_wsockfds,
-avail_esockfds,-ecode)
Returns 3 prolog_terms for available socket fds */
prolog_term Avail_rsockfds, Avail_wsockfds, Avail_esockfds;
prolog_term Avail_rsockfds_tail, Avail_wsockfds_tail, Avail_esockfds_tail;
int maxfd;
int i; /* index for connection_count */
char *connection_name = ptoc_string(CTXTc 2);
struct timeval *tv;
prolog_term timeout_term;
int timeout =0;
int connectname_found = FALSE;
int count=0;
SYS_MUTEX_LOCK(MUTEX_SOCKETS);
/* specify the time out */
timeout_term = reg_term(CTXTc 3);
if (isointeger(timeout_term)) {
timeout = (int)oint_val(timeout_term);
/* initialize tv */
tv = (struct timeval *)mem_alloc(sizeof(struct timeval),LEAK_SPACE);
tv->tv_sec = timeout;
tv->tv_usec = 0;
} else
tv = NULL; /* no timeouts */
/* initialize the prolog term */
Avail_rsockfds = p2p_new(CTXT);
Avail_wsockfds = p2p_new(CTXT);
Avail_esockfds = p2p_new(CTXT);
/* bind to output arguments */
Avail_rsockfds = reg_term(CTXTc 4);
Avail_wsockfds = reg_term(CTXTc 5);
Avail_esockfds = reg_term(CTXTc 6);
Avail_rsockfds_tail = Avail_rsockfds;
Avail_wsockfds_tail = Avail_wsockfds;
Avail_esockfds_tail = Avail_esockfds;
/*
// This was wrong. Lists are now made inside test_ready()
c2p_list(CTXTc Avail_rsockfds_tail);
c2p_list(CTXTc Avail_wsockfds_tail);
c2p_list(CTXTc Avail_esockfds_tail);
*/
for (i=0; i < MAXCONNECT; i++) {
/* find the matching connection_name to select */
if(connections[i].empty_flag==FALSE) {
if (strcmp(connection_name, connections[i].connection_name) == 0) {
connectname_found = TRUE;
count = i;
break;
}
}
}
if( i >= MAXCONNECT ) /* if no matching connection_name */
xsb_abort("[SOCKET_SELECT] connection `%s' doesn't exist",
connection_name);
/* compute maxfd for select call */
maxfd = connections[count].maximum_fd + 1;
/* FD_SET all sockets */
set_sockfd( CTXTc count );
/* test whether the socket fd is available */
rc = select(maxfd, &connections[count].readset,
&connections[count].writeset,
&connections[count].exceptionset, tv);
/* error handling */
if (rc == 0) /* timed out */
ecode = TIMEOUT_ERR;
else if (SOCKET_OP_FAILED(rc)) {
perror("SOCKET_SELECT");
ecode = XSB_SOCKET_ERRORCODE;
} else { /* no error */
ecode = SOCK_OK;
/* call the utility function to return the available socket fds */
test_ready(CTXTc &Avail_rsockfds_tail, &connections[count].readset,
connections[count].read_fds,connections[count].sizer);
test_ready(CTXTc &Avail_wsockfds_tail, &connections[count].writeset,
connections[count].write_fds,connections[count].sizew);
test_ready(CTXTc &Avail_esockfds_tail,&connections[count].exceptionset,
connections[count].exception_fds,connections[count].sizee);
}
SYS_MUTEX_UNLOCK(MUTEX_SOCKETS);
if (tv) mem_dealloc((struct timeval *)tv,sizeof(struct timeval),LEAK_SPACE);
SQUASH_LINUX_COMPILER_WARN(connectname_found) ;
return set_error_code(CTXTc ecode, 7, "SOCKET_SELECT");
}
case SOCKET_SELECT_DESTROY: {
/*socket_request(SOCKET_SELECT_DESTROY, +connection_name) */
char *connection_name = ptoc_string(CTXTc 2);
select_destroy(CTXTc connection_name);
return TRUE;
}
default:
xsb_warn(CTXTc "[SOCKET_REQUEST] Invalid socket request %d", (int) ptoc_int(CTXTc 1));
return FALSE;
}
/* This trick would report a bug, if a newly added case
doesn't have a return clause */
xsb_bug("SOCKET_REQUEST case %d has no return clause", ptoc_int(CTXTc 1));
}
/*@-mustfreefresh -type@ -unrecog*/
static /*@null@*/ void *gpsd_ppsmonitor(void *arg)
{
struct gps_device_t *session = (struct gps_device_t *)arg;
struct timeval tv;
struct timespec ts;
#if defined(TIOCMIWAIT)
int cycle, duration, state = 0, laststate = -1, unchanged = 0;
struct timeval pulse[2] = { {0, 0}, {0, 0} };
#endif /* TIOCMIWAIT */
#if defined(HAVE_SYS_TIMEPPS_H)
int kpps_edge = 0; /* 0 = clear edge, 1 = assert edge */
int cycle_kpps, duration_kpps;
struct timespec pulse_kpps[2] = { {0, 0}, {0, 0} };
struct timespec tv_kpps;
pps_info_t pi;
#endif
/* for chrony SOCK interface, which allows nSec timekeeping */
#define SOCK_MAGIC 0x534f434b
struct sock_sample {
struct timeval tv;
double offset;
int pulse;
int leap;
int _pad; /* unused */
int magic; /* must be SOCK_MAGIC */
} sample;
/* chrony must be started first as chrony insists on creating the socket */
/* open the chrony socket */
int chronyfd = -1;
char chrony_path[PATH_MAX];
gpsd_report(LOG_PROG, "PPS Create Thread gpsd_ppsmonitor\n");
/* wait for the device to go active - makes this safe to call early */
while (BAD_SOCKET(session->gpsdata.gps_fd)) {
/* should probably remove this once code is verified */
gpsd_report(LOG_PROG, "PPS thread awaiting device activation\n");
(void)sleep(1);
}
/* Activates PPS support for RS-232 or USB devices only. */
if (!(session->sourcetype == source_rs232 || session->sourcetype == source_usb)) {
gpsd_report(LOG_PROG, "PPS thread deactivation. Not RS-232 or USB device.\n");
(void)ntpshm_free(session->context, session->shmTimeP);
return NULL;
}
if ( 0 == getuid() ) {
/* this case will fire on command-line devices;
* they're opened before priv-dropping. Matters because
* only root can use /var/run.
*/
(void)snprintf(chrony_path, sizeof (chrony_path),
"/var/run/chrony.%s.sock", basename(session->gpsdata.dev.path));
} else {
(void)snprintf(chrony_path, sizeof (chrony_path),
"/tmp/chrony.%s.sock", basename(session->gpsdata.dev.path));
}
if (access(chrony_path, F_OK) != 0) {
gpsd_report(LOG_PROG, "PPS chrony socket %s doesn't exist\n", chrony_path);
} else {
chronyfd = netlib_localsocket(chrony_path, SOCK_DGRAM);
if (chronyfd < 0)
gpsd_report(LOG_PROG, "PPS can not connect chrony socket: %s\n",
chrony_path);
else
gpsd_report(LOG_RAW, "PPS using chrony socket: %s\n", chrony_path);
}
/* end chrony */
#if defined(HAVE_SYS_TIMEPPS_H)
/* some operations in init_kernel_pps() require root privs */
(void)init_kernel_pps( session );
if ( 0 <= session->kernelpps_handle ) {
gpsd_report(LOG_WARN, "KPPS kernel PPS will be used\n");
}
memset( (void *)&pi, 0, sizeof(pps_info_t));
#endif
/* root privileges are not required after this point */
/*
* Wait for status change on any handshake line. The only assumption here
* is that no GPS lights up more than one of these pins. By waiting on
* all of them we remove a configuration switch.
*/
while (!gpsd_ppsmonitor_stop) {
bool ok = false;
char *log = NULL;
#if defined(TIOCMIWAIT)
#define PPS_LINE_TIOC (TIOCM_CD|TIOCM_CAR|TIOCM_RI|TIOCM_CTS)
if (ioctl(session->gpsdata.gps_fd, TIOCMIWAIT, PPS_LINE_TIOC) != 0) {
gpsd_report(LOG_ERROR, "PPS ioctl(TIOCMIWAIT) failed: %d %.40s\n"
, errno, strerror(errno));
break;
}
#endif /* TIOCMIWAIT */
/*@-noeffect@*/
#ifdef HAVE_CLOCK_GETTIME
/* using clock_gettime() here, that is nSec,
* not uSec like gettimeofday */
if ( 0 > clock_gettime(CLOCK_REALTIME, &ts) ) {
/* uh, oh, can not get time! */
gpsd_report(LOG_ERROR, "PPS clock_gettime() failed\n");
break;
}
TSTOTV( &tv, &ts);
#else
if ( 0 > gettimeofday(&tv, NULL) ) {
/* uh, oh, can not get time! */
gpsd_report(LOG_ERROR, "PPS gettimeofday() failed\n");
break;
}
TVTOTS( &ts, &tv);
#endif
/*@+noeffect@*/
#if defined(HAVE_SYS_TIMEPPS_H)
if ( 0 <= session->kernelpps_handle ) {
struct timespec kernelpps_tv;
/* on a quad core 2.4GHz Xeon this removes about 20uS of
* latency, and about +/-5uS of jitter over the other method */
memset( (void *)&kernelpps_tv, 0, sizeof(kernelpps_tv));
if ( 0 > time_pps_fetch(session->kernelpps_handle, PPS_TSFMT_TSPEC
, &pi, &kernelpps_tv)) {
gpsd_report(LOG_ERROR, "KPPS kernel PPS failed\n");
} else {
// find the last edge
if ( pi.assert_timestamp.tv_sec > pi.clear_timestamp.tv_sec ) {
kpps_edge = 1;
tv_kpps = pi.assert_timestamp;
} else if ( pi.assert_timestamp.tv_sec < pi.clear_timestamp.tv_sec ) {
kpps_edge = 0;
tv_kpps = pi.clear_timestamp;
} else if ( pi.assert_timestamp.tv_nsec > pi.clear_timestamp.tv_nsec ) {
kpps_edge = 1;
tv_kpps = pi.assert_timestamp;
} else {
kpps_edge = 0;
tv_kpps = pi.clear_timestamp;
}
gpsd_report(LOG_PROG, "KPPS assert %ld.%09ld, sequence: %ld - "
"clear %ld.%09ld, sequence: %ld\n",
pi.assert_timestamp.tv_sec,
pi.assert_timestamp.tv_nsec,
pi.assert_sequence,
pi.clear_timestamp.tv_sec,
pi.clear_timestamp.tv_nsec,
pi.clear_sequence);
gpsd_report(LOG_PROG, "KPPS data: using %s\n",
kpps_edge ? "assert" : "clear");
#define timediff_kpps(x, y) (int)((x.tv_sec-y.tv_sec)*1000000+((x.tv_nsec-y.tv_nsec)/1000))
cycle_kpps = timediff_kpps(tv_kpps, pulse_kpps[kpps_edge]);
duration_kpps = timediff_kpps(tv_kpps, pulse_kpps[(int)(kpps_edge == 0)]);
if ( 3000000 < duration_kpps ) {
// invisible pulse
duration_kpps = 0;
}
#undef timediff_kpps
gpsd_report(LOG_INF,
"KPPS cycle: %7d, duration: %7d @ %lu.%09lu\n",
cycle_kpps, duration_kpps,
(unsigned long)tv_kpps.tv_sec,
(unsigned long)tv_kpps.tv_nsec);
pulse_kpps[kpps_edge] = tv_kpps;
ok = true;
log = "KPPS";
}
}
#endif /* HAVE_SYS_TIMEPPS_H */
#if defined(TIOCMIWAIT)
ok = false;
log = NULL;
/*@ +ignoresigns */
if (ioctl(session->gpsdata.gps_fd, TIOCMGET, &state) != 0) {
gpsd_report(LOG_ERROR, "PPS ioctl(TIOCMGET) failed\n");
break;
}
/*@ -ignoresigns */
state = (int)((state & PPS_LINE_TIOC) != 0);
/*@ +boolint @*/
#define timediff(x, y) (int)((x.tv_sec-y.tv_sec)*1000000+x.tv_usec-y.tv_usec)
cycle = timediff(tv, pulse[state]);
duration = timediff(tv, pulse[(int)(state == 0)]);
#undef timediff
/*@ -boolint @*/
if (state == laststate) {
/* some pulses may be so short that state never changes */
if (999000 < cycle && 1001000 > cycle) {
duration = 0;
unchanged = 0;
gpsd_report(LOG_RAW,
"PPS pps-detect on %s invisible pulse\n",
session->gpsdata.dev.path);
} else if (++unchanged == 10) {
unchanged = 1;
gpsd_report(LOG_WARN,
"PPS TIOCMIWAIT returns unchanged state, ppsmonitor sleeps 10\n");
(void)sleep(10);
}
} else {
gpsd_report(LOG_RAW, "PPS pps-detect on %s changed to %d\n",
session->gpsdata.dev.path, state);
laststate = state;
unchanged = 0;
}
pulse[state] = tv;
if (unchanged) {
// strange, try again
continue;
}
gpsd_report(LOG_INF, "PPS cycle: %7d, duration: %7d @ %lu.%06lu\n",
cycle, duration,
(unsigned long)tv.tv_sec, (unsigned long)tv.tv_usec);
/* only listen to PPS after 4 consecutive fixes, otherwise time
* will be inaccurate.
* Not sure yet how to handle uBlox UBX_MODE_TMONLY
* Do not use ship_to_ntp here since it is synced to packets
* and this thread is asynchonous to packets */
if ( 3 < session->fixcnt ) {
/*
* The PPS pulse is normally a short pulse with a frequency of
* 1 Hz, and the UTC second is defined by the front edge. But we
* don't know the polarity of the pulse (different receivers
* emit different polarities). The duration variable is used to
* determine which way the pulse is going. The code assumes
* that the UTC second is changing when the signal has not
* been changing for at least 800ms, i.e. it assumes the duty
* cycle is at most 20%.
*
* Some GPSes instead output a square wave that is 0.5 Hz and each
* edge denotes the start of a second.
*
* Some GPSes, like the Globalsat MR-350P, output a 1uS pulse.
* The pulse is so short that TIOCMIWAIT sees a state change
* but by the time TIOCMGET is called the pulse is gone.
*
* A few stupid GPSes, like the Furuno GPSClock, output a 1.0 Hz
* square wave where the leading edge is the start of a second
*
* 5Hz GPS (Garmin 18-5Hz) pulses at 5Hz. Set the pulse length to
* 40ms which gives a 160ms pulse before going high.
*
*/
log = "Unknown error";
if (199000 > cycle) {
// too short to even be a 5Hz pulse
log = "Too short for 5Hz\n";
} else if (201000 > cycle) {
/* 5Hz cycle */
/* looks like 5hz PPS pulse */
if (100000 > duration) {
/* BUG: how does the code know to tell ntpd
* which 1/5 of a second to use?? */
ok = true;
log = "5Hz PPS pulse\n";
}
} else if (999000 > cycle) {
log = "Too long for 5Hz, too short for 1Hz\n";
} else if (1001000 > cycle) {
/* looks like PPS pulse or square wave */
if (0 == duration) {
ok = true;
log = "invisible pulse\n";
} else if (499000 > duration) {
/* end of the short "half" of the cycle */
/* aka the trailing edge */
log = "1Hz trailing edge\n";
} else if (501000 > duration) {
/* looks like 1.0 Hz square wave, ignore trailing edge */
if (state == 1) {
ok = true;
log = "square\n";
}
} else {
/* end of the long "half" of the cycle */
/* aka the leading edge */
ok = true;
log = "1Hz leading edge\n";
}
} else if (1999000 > cycle) {
log = "Too long for 1Hz, too short for 2Hz\n";
} else if (2001000 > cycle) {
/* looks like 0.5 Hz square wave */
if (999000 > duration) {
log = "0.5 Hz square too short duration\n";
} else if (1001000 > duration) {
ok = true;
log = "0.5 Hz square wave\n";
} else {
log = "0.5 Hz square too long duration\n";
}
} else {
log = "Too long for 0.5Hz\n";
}
} else {
/* not a good fix, but a test for an otherwise good PPS
* would go here */
log = "no fix.\n";
}
#endif /* TIOCMIWAIT */
if (ok) {
char *log1 = NULL;
gpsd_report(LOG_RAW, "PPS edge accepted %.100s", log);
/* chrony expects tv-sec since Jan 1970 */
/* FIXME!! offset is double of the error from local time */
sample.pulse = 0;
sample.leap = session->context->leap_notify;
sample.magic = SOCK_MAGIC;
#if defined(HAVE_SYS_TIMEPPS_H)
if ( 0 <= session->kernelpps_handle) {
/* pick the right edge */
if ( kpps_edge ) {
ts = pi.assert_timestamp; /* structure copy */
} else {
ts = pi.clear_timestamp; /* structure copy */
}
TSTOTV( &sample.tv, &ts);
} else
#endif
{
sample.tv = tv; /* structure copy */
}
/* FIXME!! this is wrong if signal is 5Hz or 10Hz instead of PPS */
/* careful, Unix time to nSec is more precision than a double */
sample.offset = 1 + session->last_fixtime - ts.tv_sec;
sample.offset -= ts.tv_nsec / 1e9;
/* was: defined(ONCORE_ENABLE) && defined(BINARY_ENABLE) */
#ifdef __UNUSED__
/*@-noeffect@*/
if (session->device_type == &oncore_binary) {
int pulse_delay_ns = session->driver.oncore.pps_offset_ns;
sample.offset += (double)pulse_delay_ns / 1000000000;
ts.tv_nsec -= pulse_delay_ns;
TS_NORM( &ts );
}
/*@+noeffect@*/
#endif
TSTOTV( &tv, &ts );
if (session->ship_to_ntpd) {
log1 = "accepted";
if ( 0 <= chronyfd ) {
log1 = "accepted chrony sock";
(void)send(chronyfd, &sample, sizeof (sample), 0);
}
(void)ntpshm_pps(session, &tv);
} else {
log1 = "skipped ship_to_ntp=0";
}
gpsd_report(LOG_RAW,
"PPS edge %.20s %lu.%06lu offset %.9f\n",
log1,
(unsigned long)sample.tv.tv_sec,
(unsigned long)sample.tv.tv_usec,
sample.offset);
if (session->context->pps_hook != NULL)
session->context->pps_hook(session, &tv);
} else {
gpsd_report(LOG_RAW, "PPS edge rejected %.100s", log);
}
}
#if defined(HAVE_SYS_TIMEPPS_H)
if (session->kernelpps_handle > 0) {
gpsd_report(LOG_PROG, "PPS descriptor cleaned up\n");
time_pps_destroy(session->kernelpps_handle);
}
#endif
if (chronyfd != -1)
(void)close(chronyfd);
gpsd_report(LOG_PROG, "PPS gpsd_ppsmonitor exited.\n");
return NULL;
}