parsestreams.c

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

		putnext(q, mp);
		return putctl1(WR(q)->q_next, M_CTL, (mode == M_PARSE) ? MC_SERVICEIMM :
			       MC_SERVICEDEF);
	}
	else
	{
		parseprintf(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
	)
{
	register parsestream_t *parse;
	static int notice = 0;
  
	parseprintf(DD_OPEN,("parse: OPEN\n")); 
  
	if (sflag != MODOPEN)
	{			/* open only for modules */
		parseprintf(DD_OPEN,("parse: OPEN - FAILED - not MODOPEN\n")); 
		return OPENFAIL;
	}

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

#ifdef VDDRV
	parsebusy++;
#endif
  
	q->q_ptr = (caddr_t)kmem_alloc(sizeof(parsestream_t));
	if (q->q_ptr == (caddr_t)0)
	{
		parseprintf(DD_OPEN,("parse: OPEN - FAILED - no memory\n")); 
#ifdef VDDRV
		parsebusy--;
#endif
		return OPENFAIL;
	}
	WR(q)->q_ptr = q->q_ptr;
  
	parse = (parsestream_t *)(void *)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;

	if (!parse_ioinit(&parse->parse_io))
	{
		/*
		 * ok guys - beat it
		 */
		kmem_free((caddr_t)parse, sizeof(parsestream_t));
#ifdef VDDRV
		parsebusy--;
#endif
		return OPENFAIL;
	}

	if (setup_stream(q, M_PARSE))
	{
		(void) init_linemon(q);	/* hook up PPS ISR routines if possible */

		parseprintf(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)
		{
#ifdef VDDRV
			printf("%s: Copyright (C) 1991-2005, Frank Kardel\n", parsesync_vd.Drv_name);
#else
			printf("%s: Copyright (C) 1991-2005, Frank Kardel\n", "parsestreams.c,v 4.11 2005/04/16 17:32:10 kardel RELEASE_20050508_A");
#endif
			notice = 1;
		}

		return MODOPEN;
	}
	else
	{
		kmem_free((caddr_t)parse, sizeof(parsestream_t));

#ifdef VDDRV
		parsebusy--;
#endif
		return OPENFAIL;
	}
}

/*ARGSUSED*/
static int
parseclose(
	queue_t *q,
	int flags
	)
{
	register parsestream_t *parse = (parsestream_t *)(void *)q->q_ptr;
	register unsigned long s;
  
	parseprintf(DD_CLOSE,("parse: CLOSE\n"));
  
	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;

#ifdef VDDRV
	parsebusy--;
#endif
	return 0;
}

/*
 * move unrecognized stuff upward
 */
static int
parsersvc(
	queue_t *q
	)
{
	mblk_t *mp;
  
	while ((mp = getq(q)))
	{
		if (canput(q->q_next) || (mp->b_datap->db_type > QPCTL))
		{
			putnext(q, mp);
			parseprintf(DD_RSVC,("parse: RSVC - putnext\n"));
		}
		else
		{
			putbq(q, mp);
			parseprintf(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,
	register mblk_t *mp
	)
{
	register int ok = 1;
	register mblk_t *datap;
	register struct iocblk *iocp;
	parsestream_t         *parse = (parsestream_t *)(void *)q->q_ptr;
  
	parseprintf(DD_WPUT,("parse: parsewput\n"));
  
	switch (mp->b_datap->db_type)
	{
	    default:
		putnext(q, mp);
		break;
      
	    case M_IOCTL:
		    iocp = (struct iocblk *)(void *)mp->b_rptr;
		switch (iocp->ioc_cmd)
		{
		    default:
			parseprintf(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 *)(void *)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 *)(void *)mp->b_cont->b_rptr;

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

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

				    case PARSEIOC_SETCS:
					parseprintf(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;
			}
			parseprintf(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(
       register 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;
	  
			*mp = (*mp)->b_cont;
			freeb(mmp);
		}
	}
	return (unsigned)~0;
}

/*
 * convert incoming data
 */
static int
parserput(
	queue_t *q,
	mblk_t *mp
	)
{
	unsigned char type;
  
	switch (type = mp->b_datap->db_type)
	{
	    default:
		/*
		 * anything we don't know will be put on queue
		 * the service routine will move it to the next one
		 */
		parseprintf(DD_RPUT,("parse: parserput - forward type 0x%x\n", type));
		if (canput(q->q_next) || (mp->b_datap->db_type > QPCTL))
		{
			putnext(q, mp);
		}
		else
		    putq(q, mp);
		break;
      
	    case M_BREAK:
	    case M_DATA:
		    {
			    register parsestream_t * parse = (parsestream_t *)(void *)q->q_ptr;
			    register mblk_t *nmp;
			    register unsigned long ch;
			    timestamp_t ctime;

			    /*
			     * get time on packet delivery
			     */
			    uniqtime(&ctime.tv);

			    if (!(parse->parse_status & PARSE_ENABLE))
			    {
				    parseprintf(DD_RPUT,("parse: parserput - parser disabled - forward type 0x%x\n", type));
				    if (canput(q->q_next) || (mp->b_datap->db_type > QPCTL))
				    {
					    putnext(q, mp);
				    }
				    else
					putq(q, mp);
			    }
			    else
			    {
				    parseprintf(DD_RPUT,("parse: parserput - M_%s\n", (type == M_DATA) ? "DATA" : "BREAK"));

				    if (type == M_DATA)
				    {
					    /*
					     * parse packet looking for start an end characters
					     */
					    while (mp != (mblk_t *)NULL)
					    {
						    ch = rdchar(&mp);
						    if (ch != ~0 && parse_ioread(&parse->parse_io, (unsigned int)ch, &ctime))
						    {
							    /*
							     * up up and away (hopefully ...)
							     * don't press it if resources are tight or nobody wants it
							     */
							    nmp = (mblk_t *)NULL;
							    if (canput(parse->parse_queue->q_next) && (nmp = allocb(sizeof(parsetime_t), BPRI_MED)))
							    {
								    bcopy((caddr_t)&parse->parse_io.parse_dtime, (caddr_t)nmp->b_rptr, sizeof(parsetime_t));
								    nmp->b_wptr += sizeof(parsetime_t);
								    putnext(parse->parse_queue, nmp);
							    }
							    else
								if (nmp) freemsg(nmp);
							    parse_iodone(&parse->parse_io);
						    }
					    }	
				    }
				    else
				    {
					    if (parse_ioread(&parse->parse_io, (unsigned int)0, &ctime))
					    {
						    /*
						     * up up and away (hopefully ...)
						     * don't press it if resources are tight or nobody wants it
						     */
						    nmp = (mblk_t *)NULL;
						    if (canput(parse->parse_queue->q_next) && (nmp = allocb(sizeof(parsetime_t), BPRI_MED)))
						    {
							    bcopy((caddr_t)&parse->parse_io.parse_dtime, (caddr_t)nmp->b_rptr, sizeof(parsetime_t));
							    nmp->b_wptr += sizeof(parsetime_t);
							    putnext(parse->parse_queue, nmp);
						    }
						    else
							if (nmp) freemsg(nmp);
						    parse_iodone(&parse->parse_io);
					    }
					    freemsg(mp);
				    }
				    break;
			    }
		    }

		    /*
		     * CD PPS support for non direct ISR hack
		     */
	    case M_HANGUP:
	    case M_UNHANGUP:
		    {
			    register parsestream_t * parse = (parsestream_t *)(void *)q->q_ptr;
			    timestamp_t ctime;
			    register mblk_t *nmp;
			    register int status = cd_invert ^ (type == M_UNHANGUP);

			    uniqtime(&ctime.tv);
	
			    parseprintf(DD_RPUT,("parse: parserput - M_%sHANGUP\n", (type == M_HANGUP) ? "" : "UN"));

			    if ((parse->parse_status & PARSE_ENABLE) &&
				parse_iopps(&parse->parse_io, (int)(status ? SYNC_ONE : SYNC_ZERO), &ctime))
			    {
				    nmp = (mblk_t *)NULL;
				    if (canput(parse->parse_queue->q_next) && (nmp = allocb(sizeof(parsetime_t), BPRI_MED)))
				    {
					    bcopy((caddr_t)&parse->parse_io.parse_dtime, (caddr_t)nmp->b_rptr, sizeof(parsetime_t));
					    nmp->b_wptr += sizeof(parsetime_t);
					    putnext(parse->parse_queue, nmp);
				    }
				    else
					if (nmp) freemsg(nmp);
				    parse_iodone(&parse->parse_io);
				    freemsg(mp);
			    }
			    else
				if (canput(q->q_next) || (mp->b_datap->db_type > QPCTL))
				{
					putnext(q, mp);
				}
				else
				    putq(q, mp);
	
			    if (status)