void write_tcp(struct iface *ifa)
{
struct if_tcp *ift = (struct if_tcp *) ifa->info;
senblk_t *sptr;
/* Porters: MSG_NOSIGNAL / SO_NOSIGPIPE are redundant as of v0.3: We're
* ignorning SIGPIPE so you can forget this if porting to a platform on
* which none of this is supported. Will be removed in a later release.
*/
#ifndef MSG_NOSIGNAL
#define MSG_NOSIGNAL 0
int n=1;
setsockopt(ift->fd,SOL_SOCKET, SO_NOSIGPIPE, (void *)&n, sizeof(int));
#endif
for(;;) {
if ((sptr = next_senblk(ifa->q)) == NULL)
break;
if (senfilter(sptr,ifa->ofilter)) {
senblk_free(sptr,ifa->q);
continue;
}
if ((send(ift->fd,sptr->data,sptr->len,MSG_NOSIGNAL)) <0)
break;
senblk_free(sptr,ifa->q);
}
iface_thread_exit(errno);
}
/*
* Write nmea sentences to serial output
* Args: pointer to interface
* Returns: Nothing. errno supplied to iface_thread_exit()
*/
void write_serial(struct iface *ifa)
{
struct if_serial *ifs = (struct if_serial *) ifa->info;
senblk_t *senblk_p;
int fd=ifs->fd;
int n=0;
char *ptr;
while(n >= 0) {
/* NULL return from next_senblk means the queue has been shut
* down. Time to die */
if ((senblk_p = next_senblk(ifa->q)) == NULL)
break;
if (senfilter(senblk_p,ifa->ofilter)) {
senblk_free(senblk_p,ifa->q);
continue;
}
ptr=senblk_p->data;
while(senblk_p->len) {
if ((n=write(fd,ptr,senblk_p->len)) < 0)
break;
senblk_p->len -=n;
ptr+=n;
}
senblk_free(senblk_p,ifa->q);
}
iface_thread_exit(errno);
}
void write_file(iface_t *ifa)
{
struct if_file *ifc = (struct if_file *) ifa->info;
senblk_t *sptr;
/* ifc->fp will only be NULL if we're opening a FIFO.
*/
if (ifc->fp == NULL) {
if ((ifc->fp=fopen(ifc->filename,"w")) == NULL) {
logerr(errno,"Failed to open FIFO %s for writing\n",ifc->filename);
iface_thread_exit(errno);
}
if ((ifa->q =init_q(ifc->qsize)) == NULL) {
logerr(errno,"Could not create queue for FIFO %s",ifc->filename);
iface_thread_exit(errno);
}
if (setvbuf(ifc->fp,NULL,_IOLBF,0) !=0)
iface_thread_exit(errno);
}
for(;;) {
if ((sptr = next_senblk(ifa->q)) == NULL) {
break;
}
if (senfilter(sptr,ifa->ofilter)) {
senblk_free(sptr,ifa->q);
continue;
}
if (ifc->usereturn == 0) {
sptr->data[sptr->len-2] = '\n';
sptr->data[sptr->len-1] = '\0';
}
if (fputs(sptr->data,ifc->fp) == EOF) {
if (!(ifa->persist && errno == EPIPE) )
break;
if (((ifc->fp=freopen(ifc->filename,"w",ifc->fp)) == NULL) ||
(setvbuf(ifc->fp,NULL,_IOLBF,0) !=0))
break;
}
senblk_free(sptr,ifa->q);
}
iface_thread_exit(errno);
}
/*
* Read from a serial interface
* Args: pointer to interface structure
* Returns: Nothing. errno supplied to iface_thread_exit()
*/
void read_serial(struct iface *ifa)
{
char buf[BUFSIZ]; /* Buffer for serial reads */
char *bptr,*eptr=buf+BUFSIZ,*senptr;
senblk_t sblk;
struct if_serial *ifs = (struct if_serial *) ifa->info;
int nread,cr=0,count=0,overrun=0;
int fd;
senptr=sblk.data;
sblk.src=ifa->id;
fd=ifs->fd;
/* Read up to BUFSIZ data */
while ((ifa->direction != NONE) && (nread=read(fd,buf,BUFSIZ)) > 0) {
/* Process the data we just read */
for(bptr=buf,eptr=buf+nread;bptr<eptr;bptr++) {
/* Copy to our senblk if we haven't exceeded max
* sentence length */
if (count < SENMAX+2) {
++count;
*senptr++=*bptr;
} else
/* if max length exceeded, note that we've overrrun */
++overrun;
if ((*bptr) == '\r') {
/* <CR>: If next char is <LF> that's our sentence */
++cr;
} else {
if (*bptr == '\n' && cr) {
/* <CR><LF>: End of sentence */
if (overrun) {
/* This sentence invalid: discard */
overrun=0;
} else {
/* send the sentence on its way */
sblk.len=count;
if ((!(ifa->checksum && checkcksum(&sblk)) &&
senfilter(&sblk,ifa->ifilter) == 0))
push_senblk(&sblk,ifa->q);
}
/* Reset the sentence */
senptr=sblk.data;
count=0;
}
/* The last char was NOT <CR> */
cr=0;
}
}
}
iface_thread_exit(errno);
}
/*
* Start processing an interface and add it to an iolist, input or output,
* depending on direction
* Args: Pointer to interface structure (cast to void *)
* Returns: Nothing
* We should come into this with SIGUSR1 blocked
*/
void start_interface(void *ptr)
{
iface_t *ifa = (iface_t *)ptr;
iface_t **lptr;
iface_t **iptr;
sigset_t set;
sigemptyset(&set);
sigaddset(&set, SIGUSR1);
pthread_mutex_lock(&ifa->lists->io_mutex);
ifa->tid = pthread_self();
if (pthread_setspecific(ifkey,ptr)) {
perror("Falied to set key");
exit(1);
}
for (iptr=&ifa->lists->initialized;*iptr!=ifa;iptr=&(*iptr)->next)
if (*iptr == NULL) {
perror("interface does not exist on initialized list!");
exit(1);
}
*iptr=(*iptr)->next;
/* We've unlinked from initialized. Exit if we've been told to already */
if (ifa->direction == NONE) {
pthread_mutex_unlock(&ifa->lists->io_mutex);
iface_thread_exit(0);
}
/* Set lptr to point to the input or output list, as appropriate */
lptr=(ifa->direction==IN)?&ifa->lists->inputs:&ifa->lists->outputs;
if (*lptr)
ifa->next=(*lptr);
else
ifa->next=NULL;
(*lptr)=ifa;
if (ifa->lists->initialized == NULL)
pthread_cond_broadcast(&ifa->lists->init_cond);
else
while (ifa->lists->initialized)
pthread_cond_wait(&ifa->lists->init_cond,&ifa->lists->io_mutex);
pthread_mutex_unlock(&ifa->lists->io_mutex);
pthread_sigmask(SIG_UNBLOCK,&set,NULL);
if (ifa->direction == IN) {
ifa->read(ifa);
} else
ifa->write(ifa);
}
void file_read_wrapper(iface_t *ifa)
{
struct if_file *ifc = (struct if_file *) ifa->info;
/* Create FILE stream here to allow for non-blocking opening FIFOs */
if (ifc->fd == -1)
if ((ifc->fd = open(ifc->filename,O_RDONLY)) < 0) {
logerr(errno,"Failed to open FIFO %s for reading\n",ifc->filename);
iface_thread_exit(errno);
}
do_read(ifa);
}
void tcp_server(iface_t *ifa)
{
struct if_tcp *ift=(struct if_tcp *)ifa->info;
int afd;
if (listen(ift->fd,5) == 0) {
while(ifa->direction != NONE) {
if ((afd = accept(ift->fd,NULL,NULL)) < 0)
break;
if (new_tcp_conn(afd,ifa) == NULL)
close(afd);
}
}
iface_thread_exit(errno);
}
void read_tcp(struct iface *ifa)
{
char buf[BUFSIZ];
char *bptr,*eptr=buf+BUFSIZ,*senptr;
senblk_t sblk;
struct if_tcp *ift = (struct if_tcp *) ifa->info;
int nread,cr=0,count=0,overrun=0;
int fd;
senptr=sblk.data;
sblk.src=ifa->id;
fd=ift->fd;
while ((ifa->direction != NONE) && (nread=read(fd,buf,BUFSIZ)) > 0) {
for(bptr=buf,eptr=buf+nread;bptr<eptr;bptr++) {
if (count < SENMAX+2) {
++count;
*senptr++=*bptr;
} else
++overrun;
if ((*bptr) == '\r') {
++cr;
} else {
if (*bptr == '\n' && cr) {
if (overrun) {
overrun=0;
} else {
sblk.len=count;
if (!(ifa->checksum && checkcksum(&sblk)) &&
senfilter(&sblk,ifa->ifilter) == 0)
push_senblk(&sblk,ifa->q);
}
senptr=sblk.data;
count=0;
}
cr=0;
}
}
}
iface_thread_exit(errno);
}
void read_file(iface_t *ifa)
{
struct if_file *ifc = (struct if_file *) ifa->info;
senblk_t sblk;
int len;
int maxread;
char *eptr;
maxread = SENMAX + 2 + ifc->usereturn;
/* Create FILE stream here to allow for non-blocking opening FIFOs */
if (ifc->fp == NULL)
if ((ifc->fp = fopen(ifc->filename,"r")) == NULL) {
logerr(errno,"Failed to open FIFO %s for reading\n",ifc->filename);
iface_thread_exit(errno);
}
sblk.src=ifa->id;
for(;;) {
if (fgets(sblk.data,maxread,ifc->fp) != sblk.data) {
if (feof(ifc->fp) && (ifa->persist)) {
if ((ifc->fp = freopen(ifc->filename,"r",ifc->fp)) == NULL) {
logerr(errno,"Failed to re-open FIFO %s for reading\n",
ifc->filename);
break;
}
continue;
}
break;
}
if ((len = strlen(sblk.data)) == 0) {
break;
}
if (sblk.data[len-1] != '\n') {
logwarn("Line exceeds max sentence length (discarding)");
while ((eptr = fgets(sblk.data,SENBUFSZ,ifc->fp)) == sblk.data) {
if (sblk.data[strlen(sblk.data)-1] == '\n') {
break;
}
}
if (eptr == NULL && (ifa->persist == 0))
break;
continue;
}
if (ifc->usereturn) {
if (sblk.data[len-2] != '\r') {
continue;
}
}
else {
sblk.data[len-1]='\r';
sblk.data[len]='\n';
len++;
}
sblk.len=len;
if (ifa->checksum && checkcksum(&sblk))
continue;
if (senfilter(&sblk,ifa->ifilter))
continue;
push_senblk(&sblk,ifa->q);
}
iface_thread_exit(errno);
}
/* generic read routine
* Args: Interface Pointer
* Returns: nothing
*/
void do_read(iface_t *ifa)
{
senblk_t sblk;
char buf[BUFSIZ];
char tbuf[TAGMAX];
char *bptr,*eptr,*ptr;
int nread,countmax,count=0;
enum sstate senstate;
int nocr=flag_test(ifa,F_NOCR)?1:0;
int loose = (ifa->strict)?0:1;
sblk.src=ifa->id;
senstate=SEN_NODATA;
while ((nread=(*ifa->readbuf)(ifa,buf)) > 0) {
for(bptr=buf,eptr=buf+nread;bptr<eptr;bptr++) {
switch (*bptr) {
case '$':
case '!':
ptr=sblk.data;
countmax=SENMAX-(nocr|loose);
count=1;
*ptr++=*bptr;
senstate=SEN_SENPROC;
continue;
case '\\':
if (senstate==SEN_TAGPROC) {
*ptr++=*bptr;
senstate=SEN_TAGSEEN;
} else {
senstate=SEN_TAGPROC;
ptr=tbuf;
countmax=TAGMAX-1;
*ptr++=*bptr;
count=1;
}
continue;
case '\r':
case '\n':
case '\0':
if (senstate == SEN_SENPROC || senstate == SEN_TAGSEEN) {
if (loose || (nocr && *bptr == '\n')) {
*ptr++='\r';
*ptr='\n';
sblk.len = count+2;
} else {
if ((!nocr) && *bptr == '\r') {
senstate = SEN_CR;
*ptr++=*bptr;
++count;
} else {
senstate = SEN_NODATA;
}
continue;
}
} else if (senstate == SEN_CR) {
if (*bptr != '\n') {
senstate = SEN_NODATA;
continue;
}
*ptr=*bptr;
sblk.len = ++count;
} else {
senstate = SEN_NODATA;
continue;
}
if (!(ifa->checksum && checkcksum(&sblk) && (sblk.len > 0 )) &&
senfilter(&sblk,ifa->ifilter) == 0) {
push_senblk(&sblk,ifa->q);
}
senstate=SEN_NODATA;
continue;
default:
break;
}
if (senstate != SEN_SENPROC && senstate != SEN_TAGPROC) {
if (senstate != SEN_NODATA )
senstate=SEN_NODATA;
continue;
}
if (count++ > countmax) {
senstate=SEN_NODATA;
continue;
}
*ptr++=*bptr;
}
}
iface_thread_exit(errno);
}
void write_file(iface_t *ifa)
{
struct if_file *ifc = (struct if_file *) ifa->info;
senblk_t *sptr;
int usereturn=flag_test(ifa,F_NOCR)?0:1;
int data=0;
int cnt=1;
struct iovec iov[2];
/* ifc->fd will only be < 0 if we're opening a FIFO.
*/
if (ifc->fd < 0) {
if ((ifc->fd=open(ifc->filename,O_WRONLY)) < 0) {
logerr(errno,"Failed to open FIFO %s for writing\n",ifc->filename);
iface_thread_exit(errno);
}
if ((ifa->q =init_q(ifc->qsize)) == NULL) {
logerr(errno,"Could not create queue for FIFO %s",ifc->filename);
iface_thread_exit(errno);
}
}
if (ifa->tagflags) {
if ((iov[0].iov_base=malloc(TAGMAX)) == NULL) {
logerr(errno,"Disabing tag output on interface id %u (%s)",
ifa->id,(ifa->name)?ifa->name:"unlabelled");
ifa->tagflags=0;
} else {
cnt=2;
data=1;
}
}
for(;;) {
if ((sptr = next_senblk(ifa->q)) == NULL) {
break;
}
if (senfilter(sptr,ifa->ofilter)) {
senblk_free(sptr,ifa->q);
continue;
}
if (!usereturn) {
sptr->data[sptr->len-2] = '\n';
sptr->len--;
}
if (ifa->tagflags)
if ((iov[0].iov_len = gettag(ifa,iov[0].iov_base,sptr)) == 0) {
logerr(errno,"Disabing tag output on interface id %u (%s)",
ifa->id,(ifa->name)?ifa->name:"unlabelled");
ifa->tagflags=0;
cnt=1;
data=0;
free(iov[0].iov_base);
}
iov[data].iov_base=sptr->data;
iov[data].iov_len=sptr->len;
if (writev(ifc->fd,iov,cnt) <0) {
if (!(flag_test(ifa,F_PERSIST) && errno == EPIPE) )
break;
if ((ifc->fd=open(ifc->filename,O_WRONLY)) < 0)
break;
}
senblk_free(sptr,ifa->q);
}
if (cnt == 2)
free(iov[0].iov_base);
iface_thread_exit(errno);
}
