186 lines
6.2 KiB
C
186 lines
6.2 KiB
C
/*-
|
|
* Copyright (c) 2014 Poul-Henning Kamp
|
|
* All rights reserved.
|
|
*
|
|
* 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.
|
|
*
|
|
* 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 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.
|
|
*
|
|
* NTP packet (de)serialization
|
|
* ============================
|
|
*
|
|
* 0 1 2 3
|
|
* 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 0 |LI | VN |Mode | Stratum | Poll | Precision |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 4 | Root Delay |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 8 | Root Dispersion |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 12 | Reference ID |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 16 | |
|
|
* + Reference Timestamp (64) +
|
|
* | |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 24 | |
|
|
* + Origin Timestamp (64) +
|
|
* | |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 32 | |
|
|
* + Receive Timestamp (64) +
|
|
* | |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
* 40 | |
|
|
* + Transmit Timestamp (64) +
|
|
* | |
|
|
* +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
|
|
*
|
|
*/
|
|
|
|
#include <stdlib.h>
|
|
#include <string.h>
|
|
|
|
#include "ntimed.h"
|
|
#include "ntp.h"
|
|
#include "ntimed_endian.h"
|
|
|
|
/*
|
|
* Seconds between 1900 (NTP epoch) and 1970 (UNIX epoch).
|
|
* 17 is the number of leapdays.
|
|
*/
|
|
#define NTP_UNIX (((1970U - 1900U) * 365U + 17U) * 24U * 60U * 60U)
|
|
|
|
/**********************************************************************
|
|
* Picking a NTP packet apart in a safe, byte-order agnostic manner
|
|
*/
|
|
|
|
static void
|
|
ntp64_2ts(struct timestamp *ts, const uint8_t *ptr)
|
|
{
|
|
|
|
INIT_OBJ(ts, TIMESTAMP_MAGIC);
|
|
ts->sec = Be32dec(ptr) - NTP_UNIX;
|
|
ts->frac = (uint64_t)Be32dec(ptr + 4) << 32ULL;
|
|
}
|
|
|
|
static void
|
|
ntp32_2ts(struct timestamp *ts, const uint8_t *ptr)
|
|
{
|
|
|
|
INIT_OBJ(ts, TIMESTAMP_MAGIC);
|
|
ts->sec = Be16dec(ptr);
|
|
ts->frac = (uint64_t)Be16dec(ptr + 2) << 48ULL;
|
|
}
|
|
|
|
|
|
struct ntp_packet *
|
|
NTP_Packet_Unpack(struct ntp_packet *np, void *ptr, ssize_t len)
|
|
{
|
|
uint8_t *p = ptr;
|
|
|
|
AN(ptr);
|
|
if (len != 48) {
|
|
/* XXX: Diagnostic */
|
|
return (NULL);
|
|
}
|
|
|
|
if (np == NULL) {
|
|
ALLOC_OBJ(np, NTP_PACKET_MAGIC);
|
|
AN(np);
|
|
} else {
|
|
INIT_OBJ(np, NTP_PACKET_MAGIC);
|
|
}
|
|
|
|
np->ntp_leap = (enum ntp_leap)(p[0] >> 6);
|
|
np->ntp_version = (p[0] >> 3) & 0x7;
|
|
np->ntp_mode = (enum ntp_mode)(p[0] & 0x07);
|
|
np->ntp_stratum = p[1];
|
|
np->ntp_poll = p[2];
|
|
np->ntp_precision = (int8_t)p[3];
|
|
ntp32_2ts(&np->ntp_delay, p + 4);
|
|
ntp32_2ts(&np->ntp_dispersion, p + 8);
|
|
memcpy(np->ntp_refid, p + 12, 4L);
|
|
ntp64_2ts(&np->ntp_reference, p + 16);
|
|
ntp64_2ts(&np->ntp_origin, p + 24);
|
|
ntp64_2ts(&np->ntp_receive, p + 32);
|
|
ntp64_2ts(&np->ntp_transmit, p + 40);
|
|
return (np);
|
|
}
|
|
|
|
/**********************************************************************
|
|
* Putting a NTP packet apart in a safe, byte-order agnostic manner
|
|
*/
|
|
|
|
static void
|
|
ts_2ntp32(uint8_t *dst, const struct timestamp *ts)
|
|
{
|
|
|
|
CHECK_OBJ_NOTNULL(ts, TIMESTAMP_MAGIC);
|
|
assert(ts->sec < 65536);
|
|
Be16enc(dst, (uint16_t)ts->sec);
|
|
Be16enc(dst + 2, ts->frac >> 48ULL);
|
|
}
|
|
|
|
static void
|
|
ts_2ntp64(uint8_t *dst, const struct timestamp *ts)
|
|
{
|
|
|
|
CHECK_OBJ_NOTNULL(ts, TIMESTAMP_MAGIC);
|
|
Be32enc(dst, ts->sec + NTP_UNIX);
|
|
Be32enc(dst + 4, ts->frac >> 32ULL);
|
|
}
|
|
|
|
size_t
|
|
NTP_Packet_Pack(void *ptr, ssize_t len, struct ntp_packet *np)
|
|
{
|
|
uint8_t *pbuf = ptr;
|
|
|
|
AN(ptr);
|
|
assert(len >= 48);
|
|
CHECK_OBJ_NOTNULL(np, NTP_PACKET_MAGIC);
|
|
assert(np->ntp_version < 8);
|
|
assert(np->ntp_stratum < 15);
|
|
|
|
pbuf[0] = (uint8_t)np->ntp_leap;
|
|
pbuf[0] <<= 3;
|
|
pbuf[0] |= np->ntp_version;
|
|
pbuf[0] <<= 3;
|
|
pbuf[0] |= (uint8_t)np->ntp_mode;
|
|
pbuf[1] = np->ntp_stratum;
|
|
pbuf[2] = np->ntp_poll;
|
|
pbuf[3] = (uint8_t)np->ntp_precision;
|
|
ts_2ntp32(pbuf + 4, &np->ntp_delay);
|
|
ts_2ntp32(pbuf + 8, &np->ntp_dispersion);
|
|
memcpy(pbuf + 12, np->ntp_refid, 4L);
|
|
ts_2ntp64(pbuf + 16, &np->ntp_reference);
|
|
ts_2ntp64(pbuf + 24, &np->ntp_origin);
|
|
ts_2ntp64(pbuf + 32, &np->ntp_receive);
|
|
|
|
TB_Now(&np->ntp_transmit);
|
|
ts_2ntp64(pbuf + 40, &np->ntp_transmit);
|
|
|
|
/* Reverse again, to avoid subsequent trouble from rounding. */
|
|
ntp64_2ts(&np->ntp_transmit, pbuf + 40);
|
|
|
|
return (48);
|
|
}
|