From: Chris Johns <chr...@rtems.org> --- rtemsbsd/ptpd/src/dep/datatypes_dep.h | 10 ++ rtemsbsd/ptpd/src/dep/eventtimer.h | 6 +- rtemsbsd/ptpd/src/dep/eventtimer_kqueue.c | 184 ++++++++++++++++++++++ rtemsbsd/ptpd/src/dep/kqueue.c | 80 ++++++++++ rtemsbsd/ptpd/src/dep/net.c | 111 ++++++++++++- rtemsbsd/ptpd/src/dep/ptpd_dep.h | 11 +- rtemsbsd/ptpd/src/protocol.c | 22 ++- rtemsbsd/ptpd/src/ptpd.h | 3 + 8 files changed, 411 insertions(+), 16 deletions(-) create mode 100644 rtemsbsd/ptpd/src/dep/eventtimer_kqueue.c create mode 100644 rtemsbsd/ptpd/src/dep/kqueue.c
diff --git a/rtemsbsd/ptpd/src/dep/datatypes_dep.h b/rtemsbsd/ptpd/src/dep/datatypes_dep.h index 77a645c4..9496ed60 100644 --- a/rtemsbsd/ptpd/src/dep/datatypes_dep.h +++ b/rtemsbsd/ptpd/src/dep/datatypes_dep.h @@ -43,6 +43,16 @@ typedef struct { int ifIndex; } InterfaceInfo; +/** +* \brief Support for kqueue or select + */ +#ifdef HAVE_KQUEUE +#define PTPD_KQUEUE_EVENTS (4) /* 2 for PTP, 2 for PCAP */ +typedef struct kevent PtpNetWaitEvents; +#else +typedef struct fd_set PtpNetWaitEvents; +#endif + /** * \brief Struct describing network transport data */ diff --git a/rtemsbsd/ptpd/src/dep/eventtimer.h b/rtemsbsd/ptpd/src/dep/eventtimer.h index 9d41156d..dc4080df 100644 --- a/rtemsbsd/ptpd/src/dep/eventtimer.h +++ b/rtemsbsd/ptpd/src/dep/eventtimer.h @@ -51,9 +51,11 @@ struct EventTimer { Boolean (*isRunning) (EventTimer* timer); /* implementation data */ -#ifdef PTPD_PTIMERS +#ifdef HAVE_KQUEUE + int timerId; +#elif defined PTPD_PTIMERS timer_t timerId; -#else +#else /* HAVE_KQUEUE */ int32_t itimerInterval; int32_t itimerLeft; #endif /* PTPD_PTIMERS */ diff --git a/rtemsbsd/ptpd/src/dep/eventtimer_kqueue.c b/rtemsbsd/ptpd/src/dep/eventtimer_kqueue.c new file mode 100644 index 00000000..914bbc4a --- /dev/null +++ b/rtemsbsd/ptpd/src/dep/eventtimer_kqueue.c @@ -0,0 +1,184 @@ +/*- + * Copyright (c) 2020 Chris Johns, + * + * 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 AUTHORS ``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 AUTHORS 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. + */ + +/** + * @file eventtimer_kqueue.c + * @date + * + * @brief EventTimer implementation using kqueue timers + * + * Needs support in the kqueue to pass timer events to here. + */ + +#include "../ptpd.h" + +static void eventTimerStart_kqueue(EventTimer *timer, double interval); +static void eventTimerStop_kqueue(EventTimer *timer); +static void eventTimerReset_kqueue(EventTimer *timer); +static void eventTimerShutdown_kqueue(EventTimer *timer); +static Boolean eventTimerIsRunning_kqueue(EventTimer *timer); +static Boolean eventTimerIsExpired_kqueue(EventTimer *timer); + +static int timerInstance; + +void +setupEventTimer(EventTimer *timer) +{ + + struct kevent evset; + + if(timer == NULL) { + return; + } + + memset(timer, 0, sizeof(EventTimer)); + + timer->start = eventTimerStart_kqueue; + timer->stop = eventTimerStop_kqueue; + timer->reset = eventTimerReset_kqueue; + timer->shutdown = eventTimerShutdown_kqueue; + timer->isExpired = eventTimerIsExpired_kqueue; + timer->isRunning = eventTimerIsRunning_kqueue; + + timer->timerId = ++timerInstance; + + EV_SET(&evset, timer->timerId, EVFILT_TIMER, EV_ADD | EV_DISABLE, 0, 0, timer); + + if (kevent(ptpKqueueGet(), &evset, 1, NULL, 0, NULL) < 0) + PERROR("kevent timer add | disable"); + + DBGV("Created kqueue timer %s (%d)", timer->id, timer->timerId); +} + +static void +eventTimerStart_kqueue(EventTimer *timer, double interval) +{ + int kq = ptpKqueueGet(); + if (kq >= 0) { + struct kevent evset; + intptr_t data = interval * 1000000; + + EV_SET(&evset, timer->timerId, + EVFILT_TIMER, EV_ADD | EV_ENABLE | EV_CLEAR, + NOTE_USECONDS, data, timer); + + DBGV("Timer %s start requested at %ld us interval\n", timer->id, data); + + if (kevent(kq, &evset, 1, NULL, 0, NULL) < 0) { + PERROR("kevent timer add | enable"); + return; + } + + DBG2("timerStart: Set timer %s to %f\n", timer->id, interval); + + timer->expired = FALSE; + timer->running = TRUE; + } +} + +static void +eventTimerStop_kqueue(EventTimer *timer) +{ + int kq = ptpKqueueGet(); + if (kq >= 0) { + struct kevent evset; + + EV_SET(&evset, timer->timerId, EVFILT_TIMER, EV_ADD | EV_DISABLE, 0, 0, timer); + + DBGV("Timer %s stop requested\n", timer->id); + + if (kevent(kq, &evset, 1, NULL, 0, NULL) < 0) + PERROR("kevent timer add | disable"); + + timer->running = FALSE; + + DBG2("timerStop: stopped timer %s\n", timer->id); + } +} + +static void +eventTimerReset_kqueue(EventTimer *timer) +{ +} + +static void +eventTimerShutdown_kqueue(EventTimer *timer) +{ + int kq = ptpKqueueGet(); + if (kq >= 0) { + struct kevent evset; + + EV_SET(&evset, timer->timerId, EVFILT_TIMER, EV_DELETE, 0, 0, timer); + + if (kevent(kq, &evset, 1, NULL, 0, NULL) < 0) + PERROR("kevent timer disable"); + + timer->running = FALSE; + + DBG2("timerShutdown: %s\n", timer->id); + } +} + +static Boolean +eventTimerIsRunning_kqueue(EventTimer *timer) +{ + DBG2("timerIsRunning: Timer %s %s running\n", timer->id, + timer->running ? "is" : "is not"); + + return timer->running; +} + +static Boolean +eventTimerIsExpired_kqueue(EventTimer *timer) +{ + + Boolean ret; + + ret = timer->expired; + + DBG2("timerIsExpired: Timer %s %s expired\n", timer->id, + timer->expired ? "is" : "is not"); + + /* the five monkeys experiment */ + if(ret) { + timer->expired = FALSE; + } + + return ret; + +} + +void +startEventTimers(void) +{ + DBG("initTimer\n"); +} + +void +shutdownEventTimers(void) +{ +} diff --git a/rtemsbsd/ptpd/src/dep/kqueue.c b/rtemsbsd/ptpd/src/dep/kqueue.c new file mode 100644 index 00000000..d516c186 --- /dev/null +++ b/rtemsbsd/ptpd/src/dep/kqueue.c @@ -0,0 +1,80 @@ +/*- + * Copyright (c) 2020 Chris Johns, + * + * 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 AUTHORS ``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 AUTHORS 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. + */ + +/** + * @file kqueue.c + * @date + * + * @brief Kqueue support code + * + * The kqueue is shared between the networking and timer code and there + * no shared data in their interfaces so kqueue handle is held here. + */ + +#include "../ptpd.h" + +static int kq = -1; + +int +ptpKqueueGet(void) +{ +#ifdef HAVE_KQUEUE + if(kq < 0) { + kq = kqueue(); + if (kq < 0) + PERROR("kqueue failed"); + } +#endif + return kq; +} + +int +ptpKqueueWait(struct timespec* ts, struct kevent* events, int nevents) +{ + int ret; + + if (kq < 0) { + PERROR("kqueue not initialised"); + return -1; + } + + ret = kevent(kq, NULL, 0, events, nevents, ts); + + if (ret > 0) { + int e; + for (e = 0; e < ret; ++e) { + DBG2("kevent: %d: %2d: %s\n", e, events[e].ident, + events[e].filter == EVFILT_TIMER ? "timer" : "sock"); + if (events[e].filter == EVFILT_TIMER) { + EventTimer* timer = events[e].udata; + timer->expired = TRUE; + } + } + } + + return ret; +} diff --git a/rtemsbsd/ptpd/src/dep/net.c b/rtemsbsd/ptpd/src/dep/net.c index 86db9b83..edee5dc6 100644 --- a/rtemsbsd/ptpd/src/dep/net.c +++ b/rtemsbsd/ptpd/src/dep/net.c @@ -149,6 +149,41 @@ isIpMulticast(struct in_addr in) } */ +/* KQUEUE support to add and delete events */ +static int netKqueueAdd(int fd) +{ + int res = 0; +#ifdef HAVE_KQUEUE + int kq = ptpKqueueGet(); + if (fd >= 0 && kq >= 0) { + struct kevent evset; + EV_SET(&evset, fd, EVFILT_READ, EV_ADD, 0, 0, NULL); + res = kevent(kq, &evset, 1, NULL, 0, NULL); + if (res == -1) + PERROR("kevent sock read add"); + DBG("add sock kevent: %d --> %d\n", fd, kq); + } +#endif + return res; +} + +static int netKqueueDelete(int fd) +{ + int res = 0; +#ifdef HAVE_KQUEUE + int kq = ptpKqueueGet(); + if (fd >= 0 && kq >= 0) { + struct kevent evset; + EV_SET(&evset, fd, EVFILT_READ, EV_DELETE, 0, 0, NULL); + res = kevent(kq, &evset, 1, NULL, 0, NULL); + if (res == -1) + PERROR("kevent sock read delete"); + DBG("delete sock kevent: %d x-> %d\n", fd, kq); + } +#endif + return res; +} + /* shut down the UDP stuff */ Boolean netShutdown(NetPath * netPath) @@ -156,20 +191,26 @@ netShutdown(NetPath * netPath) netShutdownMulticast(netPath); /* Close sockets */ - if (netPath->eventSock >= 0) + if (netPath->eventSock >= 0) { + netKqueueDelete(netPath->eventSock); close(netPath->eventSock); + } netPath->eventSock = -1; - if (netPath->generalSock >= 0) + if (netPath->generalSock >= 0) { + netKqueueDelete(netPath->generalSock); close(netPath->generalSock); + } netPath->generalSock = -1; #ifdef PTPD_PCAP if (netPath->pcapEvent != NULL) { + netKqueueDelete(netPath->pcapEventSock); pcap_close(netPath->pcapEvent); netPath->pcapEventSock = -1; } if (netPath->pcapGeneral != NULL) { + netKqueueDelete(netPath->pcapGeneralSock); pcap_close(netPath->pcapGeneral); netPath->pcapGeneralSock = -1; } @@ -1039,6 +1080,10 @@ netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock) return FALSE; } + if((netKqueueAdd(netPath->eventSock) < 0) + || (netKqueueAdd(netPath->generalSock) < 0)) + return FALSE; + /* let's see if we have another interface left before we die */ if(!testInterface(rtOpts->ifaceName, rtOpts)) { @@ -1127,7 +1172,8 @@ netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock) pcap_get_selectable_fd(netPath->pcapEvent)) < 0) { PERROR("failed to get pcap event fd"); return FALSE; - } + } + netKqueueAdd(netPath->pcapEventSock); if ((netPath->pcapGeneral = pcap_open_live(rtOpts->ifaceName, PACKET_SIZE, promisc, PCAP_TIMEOUT, @@ -1157,14 +1203,17 @@ netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock) PERROR("failed to get pcap general fd"); return FALSE; } + netKqueueAdd(netPath->pcapGeneralSock); } } #endif #ifdef PTPD_PCAP if(rtOpts->transport == IEEE_802_3) { + netKqueueDelete(netPath->eventSock); close(netPath->eventSock); netPath->eventSock = -1; + netKqueueDelete(netPath->generalSock); close(netPath->generalSock); netPath->generalSock = -1; /* TX timestamp is not generated for PCAP mode and Ethernet transport */ @@ -1409,18 +1458,52 @@ netInit(NetPath * netPath, RunTimeOpts * rtOpts, PtpClock * ptpClock) rtOpts->managementAclDenyText, rtOpts->managementAclOrder); } - return TRUE; } /*Check if data has been received*/ +#ifdef HAVE_KQUEUE + +#if defined PTPD_SNMP +#error No KQUEUE support for SNMP +#endif + int -netSelect(TimeInternal * timeout, NetPath * netPath, fd_set *readfds) +netWait(TimeInternal * timeout, NetPath * netPath, PtpNetWaitEvents *readfds) +{ + int ret; + struct timespec ts, *ts_ptr; + + if (timeout) { + if(isTimeInternalNegative(timeout)) { + ERROR("Negative timeout attempted for kevent()\n"); + return -1; + } + ts.tv_sec = timeout->seconds; + ts.tv_nsec = timeout->nanoseconds; + ts_ptr = &ts; + } else { + ts_ptr = NULL; + } + + ret = ptpKqueueWait(ts_ptr, readfds, PTPD_KQUEUE_EVENTS); + + if (ret < 0) { + if (errno == EAGAIN || errno == EINTR) + ret = 0; + } + + return ret; +} + +#else /* HAVE_KQUEUE */ + +int +netWait(TimeInternal * timeout, NetPath * netPath, PtpNetWaitEvents *readfds) { int ret, nfds; struct timeval tv, *tv_ptr; - #if defined PTPD_SNMP extern const RunTimeOpts rtOpts; struct timeval snmp_timer_wait = { 0, 0}; // initialise to avoid unused warnings when SNMP disabled @@ -1499,6 +1582,22 @@ if (rtOpts.snmpEnabled) { return ret; } +#endif /* HAVE_KQUEUE */ + +Boolean netCheckEvent(int fd, PtpNetWaitEvents * readfds) +{ +#ifdef HAVE_KQUEUE + size_t e; + for (e = 0; e < PTPD_KQUEUE_EVENTS; ++e) { + if (readfds[e].ident == fd && readfds[e].filter == EVFILT_READ) + return TRUE; + } + return FALSE; +#else + return FD_ISSET(fd, readfds); +#endif +} + /** * store received data from network to "buf" , get and store the * SO_TIMESTAMP value in "time" for an event message diff --git a/rtemsbsd/ptpd/src/dep/ptpd_dep.h b/rtemsbsd/ptpd/src/dep/ptpd_dep.h index e0bcdf11..2db43f62 100644 --- a/rtemsbsd/ptpd/src/dep/ptpd_dep.h +++ b/rtemsbsd/ptpd/src/dep/ptpd_dep.h @@ -377,7 +377,8 @@ UInteger16 msgPackManagementResponse(Octet * buf,MsgHeader*,MsgManagement*,PtpCl Boolean testInterface(char* ifaceName, const RunTimeOpts* rtOpts); Boolean netInit(NetPath*,RunTimeOpts*,PtpClock*); Boolean netShutdown(NetPath*); -int netSelect(TimeInternal*,NetPath*,fd_set*); +int netWait(TimeInternal*,NetPath*,PtpNetWaitEvents*); +Boolean netCheckEvent(int,PtpNetWaitEvents*); ssize_t netRecvEvent(Octet*,TimeInternal*,NetPath*,int); ssize_t netRecvGeneral(Octet*,NetPath*); ssize_t netSendEvent(Octet*,UInteger16,NetPath*,const RunTimeOpts*,Integer32,TimeInternal*); @@ -389,6 +390,14 @@ Boolean hostLookup(const char* hostname, Integer32* addr); /** \}*/ +/** \name kqueue.c (Kqueue support) */ +/**\{*/ + +int ptpKqueueGet(void); +int ptpKqueueWait(struct timespec*, struct kevent*, int); + +/** \}*/ + #if defined PTPD_SNMP /** \name snmp.c (SNMP subsystem) * -Handle SNMP subsystem*/ diff --git a/rtemsbsd/ptpd/src/protocol.c b/rtemsbsd/ptpd/src/protocol.c index 1514add2..2ca49d8a 100644 --- a/rtemsbsd/ptpd/src/protocol.c +++ b/rtemsbsd/ptpd/src/protocol.c @@ -1450,11 +1450,17 @@ handle(RunTimeOpts *rtOpts, PtpClock *ptpClock) ssize_t length = -1; TimeInternal timeStamp = { 0, 0 }; - fd_set readfds; - +#ifdef HAVE_KQUEUE + PtpNetWaitEvents readfds[PTPD_KQUEUE_EVENTS] = { 0 }; + PtpNetWaitEvents * readfds_ptr = &readfds[0]; +#else + PtpNetWaitEvents readfds; + PtpNetWaitEvents * readfds_ptr = &readfds; FD_ZERO(&readfds); +#endif + if (!ptpClock->message_activity) { - ret = netSelect(NULL, &ptpClock->netPath, &readfds); + ret = netWait(NULL, &ptpClock->netPath, readfds_ptr); if (ret < 0) { PERROR("failed to poll sockets"); ptpClock->counters.messageRecvErrors++; @@ -1471,7 +1477,8 @@ handle(RunTimeOpts *rtOpts, PtpClock *ptpClock) #ifdef PTPD_PCAP if (rtOpts->pcap == TRUE) { - if (ptpClock->netPath.pcapEventSock >=0 && FD_ISSET(ptpClock->netPath.pcapEventSock, &readfds)) { + if (ptpClock->netPath.pcapEventSock >=0 + && netCheckEvent(ptpClock->netPath.pcapEventSock, readfds_ptr)) { length = netRecvEvent(ptpClock->msgIbuf, &timeStamp, &ptpClock->netPath,0); if (length == 0){ /* timeout, return for now */ @@ -1493,7 +1500,8 @@ handle(RunTimeOpts *rtOpts, PtpClock *ptpClock) processMessage(rtOpts, ptpClock, &timeStamp, length); } } - if (ptpClock->netPath.pcapGeneralSock >=0 && FD_ISSET(ptpClock->netPath.pcapGeneralSock, &readfds)) { + if (ptpClock->netPath.pcapGeneralSock >=0 + && netCheckEvent(ptpClock->netPath.pcapGeneralSock, readfds_ptr)) { length = netRecvGeneral(ptpClock->msgIbuf, &ptpClock->netPath); if (length == 0) /* timeout, return for now */ return; @@ -1507,7 +1515,7 @@ handle(RunTimeOpts *rtOpts, PtpClock *ptpClock) } } else { #endif - if (FD_ISSET(ptpClock->netPath.eventSock, &readfds)) { + if (netCheckEvent(ptpClock->netPath.eventSock, readfds_ptr)) { length = netRecvEvent(ptpClock->msgIbuf, &timeStamp, &ptpClock->netPath, 0); if (length < 0) { @@ -1523,7 +1531,7 @@ handle(RunTimeOpts *rtOpts, PtpClock *ptpClock) } } - if (FD_ISSET(ptpClock->netPath.generalSock, &readfds)) { + if (netCheckEvent(ptpClock->netPath.generalSock, readfds_ptr)) { length = netRecvGeneral(ptpClock->msgIbuf, &ptpClock->netPath); if (length < 0) { PERROR("failed to receive on the general socket"); diff --git a/rtemsbsd/ptpd/src/ptpd.h b/rtemsbsd/ptpd/src/ptpd.h index c5287cf5..08090317 100644 --- a/rtemsbsd/ptpd/src/ptpd.h +++ b/rtemsbsd/ptpd/src/ptpd.h @@ -83,6 +83,9 @@ #include <utmp.h> #endif /* HAVE_UTMP_H */ #endif /* HAVE_UTMPX_H */ +#ifdef HAVE_KQUEUE +#include <sys/event.h> +#endif /* HAVE_KQUEUE */ #ifdef HAVE_NET_ETHERNET_H #include <net/ethernet.h> -- 2.25.1 _______________________________________________ devel mailing list devel@rtems.org http://lists.rtems.org/mailman/listinfo/devel