sntpslib.c

VXWORKS源代码

*
* ERRNO: N/A
*
* INTERNAL
* Floating-point calculations can't be used because some boards (notably
* the SPARC architectures) disable software floating point by default to
* speed up context switching. These boards abort with an exception when
* floating point operations are encountered.
*/

ULONG sntpsNsecToFraction
    (
    ULONG nsecs 	/* nanoseconds to convert to binary fraction */
    )
    {
    ULONG factor = 294967296;  /* Partial conversion factor from base 10 */
    ULONG divisor = 10;        /* Initial exponent for mantissa. */
    ULONG mask = 100000000;    /* Pulls digits of factor from left to right. */
    int loop;
    ULONG fraction = 0;
    BOOL shift = FALSE;        /* Shifted to avoid overflow? */


    /*
     * Adjust large values so that no intermediate calculation exceeds
     * 32 bits. (This test is overkill, since the fourth MSB can be set
     * sometimes, but it's fast).
     */

    if (nsecs & 0xF0000000)
        {
        nsecs >>= 4;     /* Exclude rightmost hex digit. */
        shift = TRUE;
        }

    /*
     * In order to increase portability, the following conversion avoids
     * floating point operations, so it is somewhat obscure.
     *
     * A one nanosecond increase corresponds to increasing the NTP fractional 
     * part by (2^32)/1E9. Multiplying the number of nanoseconds by that value
     * (4.294967286) produces the NTP fractional part.
     *
     * The above constant is separated into integer and decimal parts to avoid
     * overflow. The mask variable selects each digit from the decimal part
     * sequentially, and the divisor shifts the digit the appropriate number
     * of decimal places.
     */

    fraction += nsecs * 4;              /* Handle integer part of conversion */

    for (loop = 0; loop < 9; loop++)    /* Nine digits in mantissa */
        {
        fraction += nsecs * (factor/mask)/divisor;
        factor %= mask;    /* Remove most significant digit from the factor. */
        mask /= 10;        /* Reduce length of mask by one. */
        divisor *= 10;     /* Increase shift by one decimal place. */
        }

    /* Scale result upwards if value was adjusted before processing. */

    if (shift)
        fraction <<= 4;

    return (fraction);
    }

/*******************************************************************************
*
* sntpsConfigSet - change SNTP server broadcast settings
*
* This routine alters the configuration of the SNTP server when operating
* in broadcast mode.  A <setting> value of SNTPS_DELAY interprets the contents
* of <pValue> as the new 16-bit broadcast interval.  When <setting> equals
* SNTPS_ADDRESS, <pValue> should provide the string representation of an
* IP broadcast or multicast address (for example, "224.0.1.1").  Any changed 
* settings will take effect after the current broadcast interval is 
* completed and the corresponding NTP message is sent.
*
* RETURNS: OK or ERROR.
*
* ERRNO:
*  S_sntpsLib_INVALID_PARAMETER
*/

STATUS sntpsConfigSet
    (
    int 	setting,     /* configuration option to change */
    void * 	pValue       /* new value for parameter */
    )
    {
    struct in_addr target;
    short interval;
    int result = OK;

    /* Don't change settings if message transmission in progress. */

    semTake (sntpsMutexSem, WAIT_FOREVER);

    if (setting == SNTPS_ADDRESS)
        {
        target.s_addr = inet_addr ( (char *)pValue);
        if (target.s_addr == ERROR)
            {
            errnoSet (S_sntpsLib_INVALID_PARAMETER);
            result = ERROR; 
            }
        else
            sntpsDstAddr.s_addr = target.s_addr;
        }
    else if (setting == SNTPS_DELAY)
        {
        interval = (short) (*( (int *)pValue));
        sntpsInterval = interval;
        }
    else
        {
        errnoSet (S_sntpsLib_INVALID_PARAMETER);
        result = ERROR;
        }

    semGive (sntpsMutexSem);

    return (result);
    }

/*******************************************************************************
*
* sntpsStart - execute the SNTP server
*
* This routine monitors the specified SNTP/NTP port for incoming requests from
* clients and transmits replies containing the NTP timestamp obtained from
* the hook provided by the user. If the server executes in broadcast mode,
* this routine also schedules the transmission of NTP messages at the assigned
* broadcast interval. It is the entry point for the SNTP server task and should
* only be called internally.
*
* RETURNS: N/A
*
* ERRNO: N/A
*
* NOMANUAL
*/

LOCAL void sntpsStart (void)
    {
    SNTP_PACKET sntpRequest;     /* SNTP request received from client */
    SNTP_PACKET sntpReply;       /* buffer for server reply */
 
    struct sockaddr_in srcAddr;  /* address of requesting SNTP/NTP client */
    struct sockaddr_in dstAddr;  /* target address of transmission */
    int sntpSocket;
    int result;
    int addrLen;
    SNTP_TIMESTAMP refTime;
    BOOL unsync;

    if (!sntpsInitialized)    /* Sanity check to force initialization. */
        return;

    addrLen = sizeof (dstAddr);

    /* Set address information. */

    bzero ( (char *)&srcAddr, sizeof(srcAddr));
    bzero ( (char *)&dstAddr, sizeof(dstAddr));
    srcAddr.sin_addr.s_addr = INADDR_ANY;
    srcAddr.sin_family = AF_INET;
    srcAddr.sin_port = sntpsPort;
  
    /* Create UDP socket and bind to the SNTP port. */
  
    sntpSocket = socket (AF_INET, SOCK_DGRAM, 0);
    if (sntpSocket == -1) 
        return;

    result = bind (sntpSocket, (struct sockaddr *)&srcAddr, sizeof (srcAddr));
    if (result == -1)
        {
        close (sntpSocket);
        return;
        }
 
    /* 
     * The use of sntpsInterval below doesn't need to be guarded from a call to
     * sntpsConfigSet() because this routine is called during system startup.
     */

    if (sntpsMode == SNTP_ACTIVE)
        wdStart (sntpsTimer, sntpsInterval * sysClkRateGet (),
                 (FUNCPTR)netJobAdd, (int)sntpsMsgSend);
    FOREVER 
        {
        result = recvfrom (sntpSocket, (caddr_t)&sntpRequest, 
                           sizeof (sntpRequest), 0, 
                           (struct sockaddr *)&dstAddr, &addrLen);
        if (result == -1)
            continue;

        semTake (sntpsMutexSem, WAIT_FOREVER);   /* Lock out clock changes. */

        /* Can't transmit messages if no access to clock is provided. */

        if (!sntpsClockReady || sntpsClockHookRtn == NULL)
            {
            semGive (sntpsMutexSem);
            continue;
            }

        /* All timestamp fields are zero by default. */

        bzero ( (char *)&sntpReply, sizeof (sntpReply));

        /* Retrieve the current clock ID, precision, and NTP timestamp. */

        sntpReply.precision = sntpsPrecision;
        sntpReply.referenceIdentifier = sntpsClockId;

        unsync = FALSE;
        result = (* sntpsClockHookRtn) (SNTPS_TIME, &refTime);
        if (result == ERROR)
            unsync = TRUE;

        semGive (sntpsMutexSem);

        /* Set the leap indicator and version number. */

        sntpReply.leapVerMode = 0;

        if (unsync)
            {
            sntpReply.stratum = SNTP_STRATUM_0;
            sntpReply.leapVerMode |= SNTP_LI_3;
            }
        else
            {
            sntpReply.stratum = SNTP_STRATUM_1;
            sntpReply.leapVerMode |= SNTP_LI_0;
            }

        sntpReply.leapVerMode |= (sntpRequest.leapVerMode & SNTP_VN_MASK);

        /* Set mode to server for client response, or to symmetric passive. */

        if ( (sntpRequest.leapVerMode & SNTP_MODE_MASK) == SNTP_MODE_3)
            sntpReply.leapVerMode |= SNTP_MODE_4;
        else
            sntpReply.leapVerMode |= SNTP_MODE_2;

        /* Copy the poll field from the request. */

        sntpReply.poll = sntpRequest.poll;

        /* 
         * Leave the root delay and root dispersion fields at zero.
         * Set the timestamp fields and send the message.
         */

        if (!unsync)
            {
            sntpReply.referenceTimestampSec = htonl (refTime.seconds);
            sntpReply.referenceTimestampFrac = htonl (refTime.fraction);
      
            sntpReply.receiveTimestampSec = sntpReply.referenceTimestampSec;
            sntpReply.receiveTimestampFrac = sntpReply.referenceTimestampFrac;

            sntpReply.transmitTimestampSec = sntpReply.referenceTimestampSec;
            sntpReply.transmitTimestampFrac = sntpReply.referenceTimestampFrac;

            /* The originate timestamp contains the request transmit time. */

            sntpReply.originateTimestampSec = sntpRequest.transmitTimestampSec;
            sntpReply.originateTimestampFrac =
                                             sntpRequest.transmitTimestampFrac;
            }

        result = sendto (sntpSocket, (caddr_t)&sntpReply, sizeof (sntpReply), 
                          0, (struct sockaddr *)&dstAddr, sizeof (dstAddr));
        }

    /* Not reached. */

    close (sntpSocket);
    return;
    }

/*******************************************************************************
*
* sntpsMsgSend - transmit an unsolicited NTP message
*
* This routine sends an NTP message to the assigned destination address when 
* the broadcast interval has elapsed.  It is called by watchdog timers set
* during initialization, and should not be used directly.
*
* RETURNS: N/A
*
* ERRNO: N/A
*
* NOMANUAL
*/

void sntpsMsgSend (void)
    {
    SNTP_PACKET sntpReply;
    int result;
    int optval;
    short interval;
    SNTP_TIMESTAMP refTime;
    int sntpSocket;
    struct sockaddr_in dstAddr;

    if (!sntpsInitialized)    /* Sanity check to prevent direct calls. */
        return;

    /* Lock out clock and configuration changes. */

    semTake (sntpsMutexSem, WAIT_FOREVER);

    /* Can't transmit messages if no access to clock is provided. */

    if (!sntpsClockReady || sntpsClockHookRtn == NULL)
        {
        semGive (sntpsMutexSem);
        wdStart (sntpsTimer, sntpsInterval * sysClkRateGet (),
                 (FUNCPTR)netJobAdd, (int)sntpsMsgSend);
        return;
        }

    interval = sntpsInterval;    /* Save current broadcast interval. */

    /* Retrieve the current clock ID, precision, and NTP timestamp. */

    sntpReply.precision = sntpsPrecision;
    sntpReply.referenceIdentifier = sntpsClockId;

    result = (* sntpsClockHookRtn) (SNTPS_TIME, &refTime);
    if (result == ERROR)
        {
        semGive (sntpsMutexSem);
        wdStart (sntpsTimer, sntpsInterval * sysClkRateGet (),
                 (FUNCPTR)netJobAdd, (int)sntpsMsgSend);
        return;
        }

    /* Assign target address for outgoing message. */

    bzero ( (char *)&dstAddr, sizeof(dstAddr));
    dstAddr.sin_addr.s_addr = sntpsDstAddr.s_addr;
    dstAddr.sin_family = AF_INET;
    dstAddr.sin_port = sntpsPort;

    semGive (sntpsMutexSem);
  
    /* Create UDP socket for transmission. */
  
    sntpSocket = socket (AF_INET, SOCK_DGRAM, 0);
    if (sntpSocket == -1) 
        {
        wdStart (sntpsTimer, interval * sysClkRateGet (),
                 (FUNCPTR)netJobAdd, (int)sntpsMsgSend);
        return;
        }

    /* Enable broadcast option for socket. */

    optval = 1;
    result = setsockopt (sntpSocket, SOL_SOCKET, SO_BROADCAST, (char *)&optval,
                         sizeof (optval));
    if (result == ERROR)
        {
        close (sntpSocket);
        wdStart (sntpsTimer, interval * sysClkRateGet (),
                 (FUNCPTR)netJobAdd, (int)sntpsMsgSend);
        return;
        }

    /* 
     * Set the common values for outgoing NTP messages - root delay
     * and root dispersion are 0. 
     */
  
    bzero ((char *)&sntpReply, sizeof (sntpReply));
    sntpReply.stratum = SNTP_STRATUM_1;
      
    /* Set the leap indicator, version number and mode. */

    sntpReply.leapVerMode |= SNTP_LI_0;
    sntpReply.leapVerMode |= SNTP_VN_3;
    sntpReply.leapVerMode |= SNTP_MODE_5;

    /* Set the poll field: find the nearest integral power of two. */

    sntpReply.poll = sntpsLog2Get (interval);

    /* Set the timestamp fields and send the message. */

    sntpReply.referenceTimestampSec = htonl (refTime.seconds);
    sntpReply.referenceTimestampFrac = htonl (refTime.fraction);
      
    sntpReply.receiveTimestampSec = sntpReply.referenceTimestampSec;
    sntpReply.receiveTimestampFrac = sntpReply.referenceTimestampFrac;

    sntpReply.transmitTimestampSec = sntpReply.referenceTimestampSec;
    sntpReply.transmitTimestampFrac = sntpReply.referenceTimestampFrac;

    sntpReply.originateTimestampSec = sntpReply.referenceTimestampSec;
    sntpReply.originateTimestampFrac = sntpReply.referenceTimestampFrac;

    result = sendto (sntpSocket, (caddr_t)&sntpReply, sizeof (sntpReply), 0, 
                     (struct sockaddr *)&dstAddr, sizeof (dstAddr));

    close (sntpSocket);

    /* Schedule a new transmission after the broadcast interval. */

    wdStart (sntpsTimer, interval * sysClkRateGet (),
             (FUNCPTR)netJobAdd, (int)sntpsMsgSend);
    return;
    }