void
test_HandleKodDemobilize(void)
{
static const char * HOSTNAME = "192.0.2.1";
static const char * REASON = "DENY";
struct pkt rpkt;
sockaddr_u host;
int rpktl;
struct kod_entry * entry;
rpktl = KOD_DEMOBILIZE;
ZERO(rpkt);
memcpy(&rpkt.refid, REASON, 4);
ZERO(host);
host.sa4.sin_family = AF_INET;
host.sa4.sin_addr.s_addr = inet_addr(HOSTNAME);
/* Test that the KOD-entry is added to the database. */
kod_init_kod_db("/dev/null", TRUE);
TEST_ASSERT_EQUAL(1, handle_pkt(rpktl, &rpkt, &host, HOSTNAME));
TEST_ASSERT_EQUAL(1, search_entry(HOSTNAME, &entry));
TEST_ASSERT_EQUAL_MEMORY(REASON, entry->type, 4);
}
TEST_F(mainTest, HandleKodRate) {
pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = KOD_RATE;
EXPECT_EQ(1, handle_pkt(rpktl, &rpkt, &host, ""));
}
TEST_F(mainTest, HandleServerAuthenticationFailure) {
pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = SERVER_AUTH_FAIL;
EXPECT_EQ(1, handle_pkt(rpktl, &rpkt, &host, ""));
}
TEST_F(mainTest, HandleUnusablePacket) {
pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = PACKET_UNUSEABLE;
EXPECT_EQ(1, handle_pkt(rpktl, &rpkt, &host, ""));
}
TEST_F(mainTest, HandleUnusableServer) {
pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = SERVER_UNUSEABLE;
EXPECT_EQ(-1, handle_pkt(rpktl, &rpkt, &host, ""));
}
void
test_HandleKodRate(void) {
struct pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = KOD_RATE;
TEST_ASSERT_EQUAL(1, handle_pkt(rpktl, &rpkt, &host, ""));
}
void
test_HandleServerAuthenticationFailure(void) {
struct pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = SERVER_AUTH_FAIL;
TEST_ASSERT_EQUAL(1, handle_pkt(rpktl, &rpkt, &host, ""));
}
void
test_HandleUnusablePacket(void) {
struct pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = PACKET_UNUSEABLE;
TEST_ASSERT_EQUAL(1, handle_pkt(rpktl, &rpkt, &host, ""));
}
void
test_HandleUnusableServer(void) {
struct pkt rpkt;
sockaddr_u host;
int rpktl;
ZERO(rpkt);
ZERO(host);
rpktl = SERVER_UNUSEABLE;
TEST_ASSERT_EQUAL(-1, handle_pkt(rpktl, &rpkt, &host, ""));
}
TEST_F(mainTest, HandleCorrectPacket) {
pkt rpkt;
sockaddr_u host;
int rpktl;
l_fp now;
// We don't want our testing code to actually change the system clock.
ASSERT_FALSE(ENABLED_OPT(STEP));
ASSERT_FALSE(ENABLED_OPT(SLEW));
get_systime(&now);
HTONL_FP(&now, &rpkt.reftime);
HTONL_FP(&now, &rpkt.org);
HTONL_FP(&now, &rpkt.rec);
HTONL_FP(&now, &rpkt.xmt);
rpktl = LEN_PKT_NOMAC;
ZERO(host);
AF(&host) = AF_INET;
EXPECT_EQ(0, handle_pkt(rpktl, &rpkt, &host, ""));
}
void
test_HandleCorrectPacket(void)
{
struct pkt rpkt;
sockaddr_u host;
int rpktl;
l_fp now;
/* We don't want our testing code to actually change the system clock. */
TEST_ASSERT_FALSE(ENABLED_OPT(STEP));
TEST_ASSERT_FALSE(ENABLED_OPT(SLEW));
get_systime(&now);
HTONL_FP(&now, &rpkt.reftime);
HTONL_FP(&now, &rpkt.org);
HTONL_FP(&now, &rpkt.rec);
HTONL_FP(&now, &rpkt.xmt);
rpktl = LEN_PKT_NOMAC;
ZERO(host);
AF(&host) = AF_INET;
TEST_ASSERT_EQUAL(0, handle_pkt(rpktl, &rpkt, &host, ""));
}
TEST_F(mainTest, HandleKodDemobilize) {
const char * HOSTNAME = "192.0.2.1";
const char * REASON = "DENY";
pkt rpkt;
sockaddr_u host;
int rpktl;
kod_entry * entry;
rpktl = KOD_DEMOBILIZE;
ZERO(rpkt);
memcpy(&rpkt.refid, REASON, 4);
ZERO(host);
host.sa4.sin_family = AF_INET;
host.sa4.sin_addr.s_addr = inet_addr(HOSTNAME);
// Test that the KOD-entry is added to the database.
kod_init_kod_db("/dev/null", TRUE);
EXPECT_EQ(1, handle_pkt(rpktl, &rpkt, &host, HOSTNAME));
ASSERT_EQ(1, search_entry(HOSTNAME, &entry));
EXPECT_TRUE(memcmp(REASON, entry->type, 4) == 0);
}
/* The heart of (S)NTP, exchange NTP packets and compute values to correct the local clock */
int
on_wire (
struct addrinfo *host,
struct addrinfo *bcast
)
{
char addr_buf[INET6_ADDRSTRLEN];
register int try;
SOCKET sock;
struct key *pkt_key = NULL;
int key_id = 0;
struct timeval tv_xmt;
struct pkt x_pkt;
int error, rpktl, handle_pkt_res;
if (ENABLED_OPT(AUTHENTICATION)) {
key_id = (int) atol(OPT_ARG(AUTHENTICATION));
get_key(key_id, &pkt_key);
}
for (try=0; try<5; try++) {
memset(&r_pkt, 0, sizeof rbuf);
error = GETTIMEOFDAY(&tv_xmt, (struct timezone *)NULL);
tv_xmt.tv_sec += JAN_1970;
#ifdef DEBUG
printf("sntp on_wire: Current time sec: %i msec: %i\n", (unsigned int) tv_xmt.tv_sec,
(unsigned int) tv_xmt.tv_usec);
#endif
if (bcast) {
create_socket(&sock, (sockaddr_u *)bcast->ai_addr);
rpktl = recv_bcst_pkt(sock, &r_pkt, sizeof rbuf, (sockaddr_u *)bcast->ai_addr);
closesocket(sock);
} else {
int pkt_len = generate_pkt(&x_pkt, &tv_xmt, key_id, pkt_key);
create_socket(&sock, (sockaddr_u *)host->ai_addr);
sendpkt(sock, (sockaddr_u *)host->ai_addr, &x_pkt, pkt_len);
rpktl = recvpkt(sock, &r_pkt, sizeof rbuf, &x_pkt);
closesocket(sock);
}
handle_pkt_res = handle_pkt(rpktl, &r_pkt, host);
if (handle_pkt_res < 1)
return handle_pkt_res;
}
getnameinfo(host->ai_addr, host->ai_addrlen, addr_buf, sizeof(addr_buf), NULL, 0, NI_NUMERICHOST);
msyslog(LOG_DEBUG, "Received no useable packet from %s!", addr_buf);
return -1;
}
/* Compute the 8 bits for li_vn_mode */
void
set_li_vn_mode (
struct pkt *spkt,
char leap,
char version,
char mode
)
{
if (leap > 3) {
msyslog(LOG_DEBUG, "set_li_vn_mode: leap > 3 using max. 3");
leap = 3;
}
if (mode > 7) {
msyslog(LOG_DEBUG, "set_li_vn_mode: mode > 7, using client mode 3");
mode = 3;
}
spkt->li_vn_mode = leap << 6;
spkt->li_vn_mode |= version << 3;
spkt->li_vn_mode |= mode;
}
/* set_time corrects the local clock by offset with either settimeofday() or by default
* with adjtime()/adjusttimeofday().
*/
int
set_time(
double offset
)
{
struct timeval tp;
if (ENABLED_OPT(SETTOD)) {
GETTIMEOFDAY(&tp, NULL);
tp.tv_sec += (long)offset;
tp.tv_usec += 1e6 * (offset - (long)offset);
NORMALIZE_TIMEVAL(tp);
if (SETTIMEOFDAY(&tp, NULL) < 0) {
msyslog(LOG_ERR, "Time not set: settimeofday(): %m");
return -1;
}
return 0;
}
tp.tv_sec = (long)offset;
tp.tv_usec = 1e6 * (offset - (long)offset);
NORMALIZE_TIMEVAL(tp);
if (ADJTIMEOFDAY(&tp, NULL) < 0) {
msyslog(LOG_ERR, "Time not set: adjtime(): %m");
return -1;
}
return 0;
}
/*
** Socket readable/timeout Callback:
** Read in the packet
** Unicast:
** - close socket
** - decrement n_pending_ntp
** - If packet is good, set the time and "exit"
** Broadcast:
** - If packet is good, set the time and "exit"
*/
void
sock_cb(
evutil_socket_t fd,
short what,
void *ptr
)
{
sockaddr_u sender;
sockaddr_u * psau;
sent_pkt ** p_pktlist;
sent_pkt * spkt;
int rpktl;
int rc;
INSIST(sock4 == fd || sock6 == fd);
TRACE(3, ("sock_cb: event on sock%s:%s%s%s%s\n",
(fd == sock6)
? "6"
: "4",
(what & EV_TIMEOUT) ? " timeout" : "",
(what & EV_READ) ? " read" : "",
(what & EV_WRITE) ? " write" : "",
(what & EV_SIGNAL) ? " signal" : ""));
if (!(EV_READ & what)) {
if (EV_TIMEOUT & what)
timeout_queries();
return;
}
/* Read in the packet */
rpktl = recvdata(fd, &sender, &rbuf, sizeof(rbuf));
if (rpktl < 0) {
msyslog(LOG_DEBUG, "recvfrom error %m");
return;
}
if (sock6 == fd)
p_pktlist = &v6_pkts_list;
else
p_pktlist = &v4_pkts_list;
for (spkt = *p_pktlist; spkt != NULL; spkt = spkt->link) {
psau = &spkt->addr;
if (SOCK_EQ(&sender, psau))
break;
}
if (NULL == spkt) {
msyslog(LOG_WARNING,
"Packet from unexpected source %s dropped",
sptoa(&sender));
return;
}
TRACE(1, ("sock_cb: %s %s\n", spkt->dctx->name,
sptoa(&sender)));
rpktl = process_pkt(&r_pkt, &sender, rpktl, MODE_SERVER,
&spkt->x_pkt, "sock_cb");
TRACE(2, ("sock_cb: process_pkt returned %d\n", rpktl));
/* If this is a Unicast packet, one down ... */
if (!spkt->done && (CTX_UCST & spkt->dctx->flags)) {
dec_pending_ntp(spkt->dctx->name, &spkt->addr);
spkt->done = TRUE;
}
/* If the packet is good, set the time and we're all done */
rc = handle_pkt(rpktl, &r_pkt, &spkt->addr, spkt->dctx->name);
if (0 != rc)
TRACE(1, ("sock_cb: handle_pkt() returned %d\n", rc));
check_exit_conditions();
}
static void parse_rx_data(unsigned char *rx_buff, unsigned len,
void (*handle_pkt)(struct pkt*))
{
// Current packet state
static struct {
struct pkt p;
unsigned int cur_idx;
} pkt;
// Current parser state
unsigned int cur_idx = 0;
// tmp variables
unsigned char *sync_byte_addr;
unsigned int n_bytes;
while (len > cur_idx) {
switch (state) {
case STATE_IDLE:
/*
* Search for 'SYNC_BYTE' byte
*/
sync_byte_addr = (unsigned char *) memchr(
&rx_buff[cur_idx], sync_byte,
(len - cur_idx));
if (sync_byte_addr == NULL)
return; // SYNC not found: drop data
cur_idx = (sync_byte_addr - rx_buff);
cur_idx++; // consume 'SYNC' byte
state = STATE_WAIT_LEN;
break;
case STATE_WAIT_LEN:
/*
* Read 'length' byte and initialize packet
*/
pkt.p.len = rx_buff[cur_idx];
pkt.cur_idx = 0;
cur_idx++; // process 'len' byte
state = STATE_GET_PAYLOAD;
break;
case STATE_GET_PAYLOAD:
/*
* Get missing bytes in payload,
* or as much as available
*/
n_bytes = MIN((pkt.p.len - pkt.cur_idx),
(len - cur_idx));
// copy data in 'pkt'
memcpy(&pkt.p.data[pkt.cur_idx],
&rx_buff[cur_idx], n_bytes);
pkt.cur_idx += n_bytes;
cur_idx += n_bytes;
// Packet full
if (pkt.p.len == pkt.cur_idx) {
/* Handle packet and get back to idle */
DEBUG_PRINT("Got pkt: %d\n", pkt.p.len);
handle_pkt(&pkt.p);
state = STATE_IDLE;
}
break;
default:
// Get out of here
state = STATE_IDLE;
break;
}
}
}
int main(int argc, char *argv[]) {
int opt, ret;
int can;
int nbytes;
struct ifreq ifr;
struct sockaddr_can addr;
struct iovec iov;
struct msghdr msg;
struct canfd_frame frame;
char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
struct sigaction act;
struct timeval timeo;
fd_set rdfs;
verbose = 0;
act.sa_handler = intHandler;
sigaction(SIGINT, &act, NULL);
sigaction(SIGHUP, &act, NULL);
srand(time(NULL));
while ((opt = getopt(argc, argv, "cV:zl:vFh?")) != -1) {
switch(opt) {
case 'c':
keep_spec = 1;
break;
case 'v':
verbose++;
break;
case 'V':
vin = optarg;
break;
case 'F':
no_flow_control = 1;
break;
case 'l':
plogfp = fopen(optarg, "a+");
break;
case 'z':
fuzz_level++;
break;
case 'h':
case '?':
default:
usage(argv[0], NULL);
break;
}
}
if (optind >= argc) usage(argv[0], "You must specify at least one can device");
// Create a new raw CAN socket
can = socket(PF_CAN, SOCK_RAW, CAN_RAW);
if(can < 0) usage(argv[0], "Couldn't create raw socket");
addr.can_family = AF_CAN;
memset(&ifr.ifr_name, 0, sizeof(ifr.ifr_name));
strncpy(ifr.ifr_name, argv[optind], strlen(argv[optind]));
if (verbose) plog("Using CAN interface %s\n", ifr.ifr_name);
if (ioctl(can, SIOCGIFINDEX, &ifr) < 0) {
perror("SIOCGIFINDEX");
exit(1);
}
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(can, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
perror("bind");
return 1;
}
iov.iov_base = &frame;
iov.iov_len = sizeof(frame);
msg.msg_name = &addr;
msg.msg_namelen = sizeof(addr);
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = &ctrlmsg;
msg.msg_controllen = sizeof(ctrlmsg);
msg.msg_flags = 0;
if(verbose) plog("Fuzz level set to: %d\n", fuzz_level);
gettimeofday(&start_tv, NULL);
running = 1;
while(running) {
FD_ZERO(&rdfs);
FD_SET(can, &rdfs);
timeo.tv_sec = 0;
timeo.tv_usec = 10000 * 20; // 20 ms
if ((ret = select(can+1, &rdfs, NULL, NULL, &timeo)) < 0) {
running = 0;
continue;
}
if (FD_ISSET(can, &rdfs)) {
nbytes = recvmsg(can, &msg, 0);
if (nbytes < 0) {
perror("read");
return 1;
}
if ((size_t)nbytes != CAN_MTU) {
fprintf(stderr, "read: incomplete CAN frame\n");
return 1;
}
handle_pkt(can, frame);
}
handle_pending_data(can);
}
plog("Got Interrupt. Shutting down gracefully\n");
if(plogfp) fclose(plogfp);
}
