parsesolaris.c

网络时间协议NTP 源码 版本v4.2.0b 该源码用于linux平台下

/*
 * /src/NTP/ntp4-dev/libparse/parsesolaris.c,v 4.11 2005/04/16 17:32:10 kardel RELEASE_20050508_A
 *  
 * parsesolaris.c,v 4.11 2005/04/16 17:32:10 kardel RELEASE_20050508_A
 *
 * STREAMS module for reference clocks
 *
 * Copyright (c) 1995-2005 by Frank Kardel <kardel <AT> ntp.org>
 * Copyright (c) 1989-1994 by Frank Kardel, Friedrich-Alexander Universit鋞 Erlangen-N黵nberg, Germany
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 * 1. Redistributions of source code must retain the above copyright
 *    notice, this list of conditions and the following disclaimer.
 * 2. Redistributions in binary form must reproduce the above copyright
 *    notice, this list of conditions and the following disclaimer in the
 *    documentation and/or other materials provided with the distribution.
 * 3. Neither the name of the author nor the names of its contributors
 *    may be used to endorse or promote products derived from this software
 *    without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
 * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
 * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
 * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
 * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
 * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
 * SUCH DAMAGE.
 *
 */

#define _KERNEL			/* it is a _KERNEL module */

#ifndef lint
static char rcsid[] = "parsesolaris.c,v 4.11 2005/04/16 17:32:10 kardel RELEASE_20050508_A";
#endif

#include <sys/types.h>
#include <sys/conf.h>
#include <sys/errno.h>
#include <sys/time.h>
#include <sys/termios.h>
#include <sys/stream.h>
#include <sys/strtty.h>
#include <sys/stropts.h>
#include <sys/modctl.h>
#include <sys/ddi.h>
#include <sys/sunddi.h>
#ifdef __GNUC__ /* makes it compile on Solaris 2.6 - acc doesn't like it -- GREAT! */
#include <stdarg.h>
#endif

#include "ntp_fp.h"
#include "parse.h"
#include <sys/parsestreams.h>

/*--------------- loadable driver section -----------------------------*/

static struct streamtab parseinfo;

static struct fmodsw fmod_templ =
{
	"parse",			/* module name */
	&parseinfo,			/* module information */
	D_NEW|D_MP|D_MTQPAIR,		/* exclusive for q pair */
	/* lock ptr */
};

extern struct mod_ops mod_strmodops;

static struct modlstrmod modlstrmod = 
{
	&mod_strmodops,		/* a STREAMS module */
	"PARSE      - NTP reference",	/* name this baby - keep room for revision number */
	&fmod_templ
};

static struct modlinkage modlinkage =
{
	MODREV_1,
	{
		&modlstrmod,
		NULL
	}
};

/*
 * module management routines
 */
/*ARGSUSED*/
int
_init(
     void
     )
{
	static char revision[] = "4.6";
	char *s, *S; 
	char *t;
  
#ifndef lint
	t = rcsid;
#endif
	
	/*
	 * copy RCS revision into Drv_name
	 *
	 * are we forcing RCS here to do things it was not built for ?
	 */
	s = revision;
	if (*s == '$')
	{
		/*
		 * skip "$Revision: "
		 * if present. - not necessary on a -kv co (cvs export)
		 */
		while (*s && (*s != ' '))
		{
			s++;
		}
		if (*s == ' ') s++;
	}
	  
	t = modlstrmod.strmod_linkinfo; 
	while (*t && (*t != ' '))
	{
		t++;
	}
	if (*t == ' ') t++;
	  
	S = s;
	while (*S && (((*S >= '0') && (*S <= '9')) || (*S == '.')))
	{
		S++;
	}
	  
	if (*s && *t && (S > s))
	{
		if (strlen(t) >= (S - s))
		{
			(void) strncpy(t, s, (unsigned)(S - s));
		}
	}
	return (mod_install(&modlinkage));
}

/*ARGSUSED*/
int
_info(
      struct modinfo *modinfop
      )
{
	return (mod_info(&modlinkage, modinfop));
}

/*ARGSUSED*/
int
_fini(
      void
      )
{
	if (mod_remove(&modlinkage) != DDI_SUCCESS)
	{
		return EBUSY;
	}
	else
	    return DDI_SUCCESS;
}

/*--------------- stream module definition ----------------------------*/

static int parseopen  P((queue_t *, dev_t *, int, int, cred_t *));
static int parseclose P((queue_t *, int));
static int parsewput  P((queue_t *, mblk_t *));
static int parserput  P((queue_t *, mblk_t *));
static int parsersvc  P((queue_t *));

static struct module_info driverinfo =
{
	0,				/* module ID number */
	fmod_templ.f_name,		/* module name - why repeated here ? compat ?*/
	0,				/* minimum accepted packet size */
	INFPSZ,				/* maximum accepted packet size */
	1,				/* high water mark - flow control */
	0				/* low water mark - flow control */
};

static struct qinit rinit =	/* read queue definition */
{
	parserput,			/* put procedure */
	parsersvc,			/* service procedure */
	parseopen,			/* open procedure */
	parseclose,			/* close procedure */
	NULL,				/* admin procedure - NOT USED FOR NOW */
	&driverinfo,			/* information structure */
	NULL				/* statistics */
};

static struct qinit winit =	/* write queue definition */
{
	parsewput,			/* put procedure */
	NULL,				/* service procedure */
	NULL,				/* open procedure */
	NULL,				/* close procedure */
	NULL,				/* admin procedure - NOT USED FOR NOW */
	&driverinfo,			/* information structure */
	NULL				/* statistics */
};

static struct streamtab parseinfo =	/* stream info element for parse driver */
{
	&rinit,			/* read queue */
	&winit,			/* write queue */
	NULL,				/* read mux */
	NULL				/* write mux */
};

/*--------------- driver data structures ----------------------------*/

/*
 * we usually have an inverted signal - but you
 * can change this to suit your needs
 */
int cd_invert = 1;		/* invert status of CD line - PPS support via CD input */

#ifdef PARSEDEBUG
int parsedebug = ~0;
#else
int parsedebug = 0;
#endif

/*--------------- module implementation -----------------------------*/

#define TIMEVAL_USADD(_X_, _US_) do {\
	(_X_)->tv_usec += (_US_);\
	if ((_X_)->tv_usec >= 1000000)\
	{\
	    (_X_)->tv_sec++;\
	    (_X_)->tv_usec -= 1000000;\
	}\
     } while (0)

static int init_linemon P((queue_t *));
static void close_linemon P((queue_t *, queue_t *));

#define M_PARSE		0x0001
#define M_NOPARSE	0x0002

void
ntp_memset(
	char *a,
	int x,
	int c
	)
{
	while (c-- > 0)
	    *a++ = x;
}

static void
pprintf(
	int lev,
	const char *form,
	...
	)
{
	va_list ap;

	va_start(ap, form);

	if (lev & parsedebug)
	    vcmn_err(CE_CONT, (char *)form, ap);

	va_end(ap);
}

static int
setup_stream(
	     queue_t *q,
	     int mode
	     )
{
	register mblk_t *mp;

	pprintf(DD_OPEN,"parse: SETUP_STREAM - setting up stream for q=%x\n", q);

	mp = allocb(sizeof(struct stroptions), BPRI_MED);
	if (mp)
	{
		struct stroptions *str = (struct stroptions *)mp->b_wptr;

		str->so_flags   = SO_READOPT|SO_HIWAT|SO_LOWAT|SO_ISNTTY;
		str->so_readopt = (mode == M_PARSE) ? RMSGD : RNORM;
		str->so_hiwat   = (mode == M_PARSE) ? sizeof(parsetime_t) : 256;
		str->so_lowat   = 0;
		mp->b_datap->db_type = M_SETOPTS;
		mp->b_wptr     += sizeof(struct stroptions);
		if (!q)
		    panic("NULL q - strange");
		putnext(q, mp);
		return putctl1(WR(q)->q_next, M_CTL, (mode == M_PARSE) ? MC_SERVICEIMM :
			       MC_SERVICEDEF);
	}
	else
	{
		pprintf(DD_OPEN, "parse: setup_stream - FAILED - no MEMORY for allocb\n"); 
		return 0;
	}
}

/*ARGSUSED*/
static int
parseopen(
	  queue_t *q,
	  dev_t *dev,
	  int flag,
	  int sflag,
	  cred_t *credp
	  )
{
	register parsestream_t *parse;
	static int notice = 0;
  
	pprintf(DD_OPEN, "parse: OPEN - q=%x\n", q); 
  
	if (sflag != MODOPEN)
	{			/* open only for modules */
		pprintf(DD_OPEN, "parse: OPEN - FAILED - not MODOPEN\n"); 
		return EIO;
	}

	if (q->q_ptr != (caddr_t)NULL)
	{
		pprintf(DD_OPEN, "parse: OPEN - FAILED - EXCLUSIVE ONLY\n"); 
		return EBUSY;
	}

	q->q_ptr = (caddr_t)kmem_alloc(sizeof(parsestream_t), KM_SLEEP);
	if (q->q_ptr == (caddr_t)0)
	{
		return ENOMEM;
	}

	pprintf(DD_OPEN, "parse: OPEN - parse area q=%x, q->q_ptr=%x\n", q, q->q_ptr); 
	WR(q)->q_ptr = q->q_ptr;
	pprintf(DD_OPEN, "parse: OPEN - WQ parse area q=%x, q->q_ptr=%x\n", WR(q), WR(q)->q_ptr); 
  
	parse = (parsestream_t *) q->q_ptr;
	bzero((caddr_t)parse, sizeof(*parse));
	parse->parse_queue     = q;
	parse->parse_status    = PARSE_ENABLE;
	parse->parse_ppsclockev.tv.tv_sec  = 0;
	parse->parse_ppsclockev.tv.tv_usec = 0;
	parse->parse_ppsclockev.serial     = 0;

	qprocson(q);

	pprintf(DD_OPEN, "parse: OPEN - initializing io subsystem q=%x\n", q); 

	if (!parse_ioinit(&parse->parse_io))
	{
		/*
		 * ok guys - beat it
		 */
		qprocsoff(q);

		kmem_free((caddr_t)parse, sizeof(parsestream_t));

		return EIO;
	}

	pprintf(DD_OPEN, "parse: OPEN - initializing stream q=%x\n", q); 

	if (setup_stream(q, M_PARSE))
	{
		(void) init_linemon(q);	/* hook up PPS ISR routines if possible */
		pprintf(DD_OPEN, "parse: OPEN - SUCCEEDED\n"); 

		/*
		 * I know that you know the delete key, but you didn't write this
		 * code, did you ? - So, keep the message in here.
		 */
		if (!notice)
		{
		  cmn_err(CE_CONT, "?%s: Copyright (c) 1993-2005, Frank Kardel\n", modlstrmod.strmod_linkinfo);
			notice = 1;
		}

		return 0;
	}
	else
	{
		qprocsoff(q);

		kmem_free((caddr_t)parse, sizeof(parsestream_t));

		return EIO;
	}
}

/*ARGSUSED*/
static int
parseclose(
	   queue_t *q,
	   int flags
	   )
{
	register parsestream_t *parse = (parsestream_t *)q->q_ptr;
	register unsigned long s;
  
	pprintf(DD_CLOSE, "parse: CLOSE\n");
  
	qprocsoff(q);

	s = splhigh();
  
	if (parse->parse_dqueue)
	    close_linemon(parse->parse_dqueue, q);
	parse->parse_dqueue = (queue_t *)0;

	(void) splx(s);
      
	parse_ioend(&parse->parse_io);

	kmem_free((caddr_t)parse, sizeof(parsestream_t));

	q->q_ptr = (caddr_t)NULL;
	WR(q)->q_ptr = (caddr_t)NULL;

	return 0;
}

/*
 * move unrecognized stuff upward
 */
static int
parsersvc(
	  queue_t *q
	  )
{
	mblk_t *mp;
  
	while ((mp = getq(q)))
	{
		if (canputnext(q) || (mp->b_datap->db_type > QPCTL))
		{
			putnext(q, mp);
			pprintf(DD_RSVC, "parse: RSVC - putnext\n");
		}
		else
		{
			putbq(q, mp);
			pprintf(DD_RSVC, "parse: RSVC - flow control wait\n");
			break;
		}
	}
	return 0;
}

/*
 * do ioctls and
 * send stuff down - dont care about
 * flow control
 */
static int
parsewput(
	  queue_t *q,
	  mblk_t *mp
	  )
{
	register int ok = 1;
	register mblk_t *datap;
	register struct iocblk *iocp;
	parsestream_t         *parse = (parsestream_t *)q->q_ptr;
  
	pprintf(DD_WPUT, "parse: parsewput\n");
  
	switch (mp->b_datap->db_type)
	{
	    default:
		putnext(q, mp);
		break;
      
	    case M_IOCTL:
		iocp = (struct iocblk *)mp->b_rptr;
		switch (iocp->ioc_cmd)
		{
		    default:
			pprintf(DD_WPUT, "parse: parsewput - forward M_IOCTL\n");
			putnext(q, mp);
			break;

		    case CIOGETEV:
			/*
			 * taken from Craig Leres ppsclock module (and modified)
			 */
			datap = allocb(sizeof(struct ppsclockev), BPRI_MED);
			if (datap == NULL || mp->b_cont)
			{
				mp->b_datap->db_type = M_IOCNAK;
				iocp->ioc_error = (datap == NULL) ? ENOMEM : EINVAL;
				if (datap != NULL)
				    freeb(datap);
				qreply(q, mp);
				break;
			}

			mp->b_cont = datap;
			*(struct ppsclockev *)datap->b_wptr = parse->parse_ppsclockev;
			datap->b_wptr +=
				sizeof(struct ppsclockev) / sizeof(*datap->b_wptr);
			mp->b_datap->db_type = M_IOCACK;
			iocp->ioc_count = sizeof(struct ppsclockev);
			qreply(q, mp);
			break;
	  
		    case PARSEIOC_ENABLE:
		    case PARSEIOC_DISABLE:
			    {
				    parse->parse_status = (parse->parse_status & (unsigned)~PARSE_ENABLE) |
					    (iocp->ioc_cmd == PARSEIOC_ENABLE) ?
					    PARSE_ENABLE : 0;
				    if (!setup_stream(RD(q), (parse->parse_status & PARSE_ENABLE) ?
						      M_PARSE : M_NOPARSE))
				    {
					    mp->b_datap->db_type = M_IOCNAK;
				    }
				    else
				    {
					    mp->b_datap->db_type = M_IOCACK;
				    }
				    qreply(q, mp);
				    break;
			    }	    

		    case PARSEIOC_TIMECODE:
		    case PARSEIOC_SETFMT:
		    case PARSEIOC_GETFMT:
		    case PARSEIOC_SETCS:
			if (iocp->ioc_count == sizeof(parsectl_t))
			{
				parsectl_t *dct = (parsectl_t *)mp->b_cont->b_rptr;

				switch (iocp->ioc_cmd)
				{
				    case PARSEIOC_TIMECODE:
					pprintf(DD_WPUT, "parse: parsewput - PARSEIOC_TIMECODE\n");
					ok = parse_timecode(dct, &parse->parse_io);
					break;
		  
				    case PARSEIOC_SETFMT:
					pprintf(DD_WPUT, "parse: parsewput - PARSEIOC_SETFMT\n");
					ok = parse_setfmt(dct, &parse->parse_io);
					break;

				    case PARSEIOC_GETFMT:
					pprintf(DD_WPUT, "parse: parsewput - PARSEIOC_GETFMT\n");
					ok = parse_getfmt(dct, &parse->parse_io);
					break;

				    case PARSEIOC_SETCS:
					pprintf(DD_WPUT, "parse: parsewput - PARSEIOC_SETCS\n");
					ok = parse_setcs(dct, &parse->parse_io);
					break;
				}
				mp->b_datap->db_type = ok ? M_IOCACK : M_IOCNAK;
			}
			else
			{
				mp->b_datap->db_type = M_IOCNAK;
			}
			pprintf(DD_WPUT, "parse: parsewput qreply - %s\n", (mp->b_datap->db_type == M_IOCNAK) ? "M_IOCNAK" : "M_IOCACK");
			qreply(q, mp);
			break;
		}
	}
	return 0;
}

/*
 * read characters from streams buffers
 */
static unsigned long
rdchar(
       mblk_t **mp
       )
{
	while (*mp != (mblk_t *)NULL)
	{
		if ((*mp)->b_wptr - (*mp)->b_rptr)
		{
			return (unsigned long)(*(unsigned char *)((*mp)->b_rptr++));
		}
		else
		{
			register mblk_t *mmp = *mp;