AnyEvent-FastPing/FastPing.xs

#if defined(__linux) || defined(__FreeBSD__) || defined(__OpenBSD__) || defined(__NetBSD__) || defined(__CYGWIN__)
# define ENABLE_IPV6 1 // if you get compilation problems try to disable IPv6
#else
# define ENABLE_IPV6 0
#endif

#include "EXTERN.h"
#include "perl.h"
#include "XSUB.h"

#include <pthread.h>

#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include <time.h>
#include <poll.h>
#include <unistd.h>
#include <inttypes.h>
#include <fcntl.h>
#include <errno.h>

#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>

#include <netinet/in.h>
#include <arpa/inet.h>

#ifdef __linux
# include <linux/icmp.h>
#endif
#if ENABLE_IPV6 && !defined (__CYGWIN__)
# include <netinet/icmp6.h>
#endif

#define ICMP4_ECHO       8
#define ICMP4_ECHO_REPLY 0
#define ICMP6_ECHO       128
#define ICMP6_ECHO_REPLY 129

#define DRAIN_INTERVAL 1e-6 // how long to wait when sendto returns ENOBUFS, in seconds
#define MIN_INTERVAL   1e-6 // minimum packet send interval, in seconds

#define HDR_SIZE_IP4  20
#define HDR_SIZE_IP6  48

//TODO: xread/xwrite for atomicity? we currently rely on the fact that the pip buffersize divides exactly by pointer sizes

typedef uint8_t addr_tt[16];

static int thr_res[2]; // receive from worker(s)
static int icmp4_fd = -1, icmp6_fd = -1;

/*****************************************************************************/

typedef double tstamp;

static tstamp
NOW (void)
{
  struct timeval tv;

  gettimeofday (&tv, 0);

  return tv.tv_sec + tv.tv_usec * 1e-6;
}

static void
ssleep (tstamp wait)
{
#if defined (__SVR4) && defined (__sun)
  struct timeval tv;

  tv.tv_sec  = wait;
  tv.tv_usec = (wait - tv.tv_sec) * 1e6;

  select (0, 0, 0, 0, &tv);
#elif defined(_WIN32)
  Sleep ((unsigned long)(delay * 1e3));
#else
  struct timespec ts;

  ts.tv_sec  = wait;
  ts.tv_nsec = (wait - ts.tv_sec) * 1e9;

  nanosleep (&ts, 0);
#endif
}

/*****************************************************************************/

typedef struct
{
  uint8_t version_ihl;
  uint8_t tos;
  uint16_t tot_len;

  uint16_t id;
  uint16_t flags;

  uint8_t ttl;
  uint8_t protocol;
  uint16_t cksum;

  uint32_t src;
  uint32_t dst;
} IP4HDR;

#if 0
typedef struct
{
  uint8_t version;
  uint8_t x1, x2, x3;

  uint16_t payload_len;
  uint8_t nxt_hdr;
  uint8_t hop_limit;

  uint8_t src[16];
  uint8_t dst[16];
} IP6HDR;
#endif

/*****************************************************************************/

typedef struct
{
  tstamp next;
  double interval;
  int addrlen;
  addr_tt lo, hi;
  void *items;
} RANGE;

typedef struct
{
  RANGE **ranges;
  int rangecnt, rangemax;

  tstamp interval;
  tstamp maxrtt;
  uint16_t magic1;
  uint16_t magic2;
  uint16_t magic3;

  int id;

  AV *recvq; /* receive queue */
  int nextrecv;
  SV *recvcb;

  pthread_t thrid;
  int running;
} PINGER;

static PINGER **pingers;
static int *pingerfree; /* freelist next */
static int pingercnt;
static int pingermax;
static int firstfree = -1;
static int firstrecv = -1;

/*****************************************************************************/

typedef struct
{
  uint8_t type, code;
  uint16_t cksum;
  uint16_t id, seq;
  uint16_t pinger;
  uint16_t magic;
  uint32_t stamp_hi;
  uint32_t stamp_lo;
} PKT;

static int
pkt_is_valid_for (PKT *pkt, PINGER *pinger)
{
  return pkt->id    == pinger->magic1
      && pkt->seq   == pinger->magic2
      && pkt->magic == pinger->magic3;
}

static void
ts_to_pkt (PKT *pkt, tstamp ts)
{
  /* move 12 bits of seconds into the 32 bit fractional part */
  /* leaving 20 bits subsecond resolution and 44 bits of integers */
  /* (of which 32 are typically usable) */
  ts *= 1. / 4096.;

  pkt->stamp_hi = ts;
  pkt->stamp_lo = (ts - pkt->stamp_hi) * 4294967296.;
}

static tstamp
pkt_to_ts (PKT *pkt)
{
  return pkt->stamp_hi *  4096.
       + pkt->stamp_lo * (4096. / 4294967296.);
}

static void
pkt_cksum (PKT *pkt)
{
  uint_fast32_t sum = -pkt->cksum;
  uint32_t *wp = (uint32_t *)pkt;
  int len = sizeof (*pkt) / 4;

  do
    {
      uint_fast32_t w = *(volatile uint32_t *)wp++;
      sum += (w & 0xffff) + (w >> 16);
    }
  while (len--);

  sum = (sum >> 16) + (sum & 0xffff);   /* add high 16 to low 16 */
  sum += sum >> 16;                     /* add carry */

  pkt->cksum = ~sum;
}

/*****************************************************************************/

static void
range_free (RANGE *self)
{
  free (self);
}

static void
inc_addr (addr_tt *addr)
{
  int len = sizeof (addr_tt) - 1;

  while (!++(*addr)[len])
    --len;
}

/*****************************************************************************/

/* like sendto, but retries on failure */
static void
xsendto (int fd, void *buf, size_t len, int flags, void *sa, int salen)
{
  tstamp wait = 0;

  while (sendto (fd, buf, len, flags, sa, salen) < 0 && errno == ENOBUFS)
    ssleep (wait += DRAIN_INTERVAL);
}

static void
send_ping (PKT *pkt, addr_tt *addr, int addr_len)
{
  pkt->cksum = 0;

  if (addr_len == 4)
    {
      struct sockaddr_in sa;

      pkt->type  = ICMP4_ECHO;
      pkt_cksum (pkt);

      sa.sin_family = AF_INET;
      sa.sin_port   = 0;

      memcpy (&sa.sin_addr,
              sizeof (addr_tt) - sizeof (sa.sin_addr) + (char *)addr,
              sizeof (sa.sin_addr));

      xsendto (icmp4_fd, pkt, sizeof (*pkt), 0, &sa, sizeof (sa));
    }
  else
    {
#if ENABLE_IPV6
      struct sockaddr_in6 sa;

      pkt->type = ICMP6_ECHO;

      sa.sin6_family   = AF_INET6;
      sa.sin6_port     = 0;
      sa.sin6_flowinfo = 0;
      sa.sin6_scope_id = 0;

      memcpy (&sa.sin6_addr,
              sizeof (addr_tt) - sizeof (sa.sin6_addr) + (char *)addr,
              sizeof (sa.sin6_addr));

      xsendto (icmp6_fd, &pkt, sizeof (pkt), 0, &sa, sizeof (sa));
#endif
    }
}

static void
downheap (PINGER *self)
{
  RANGE *elem = self->ranges [0]; /* always exists */
  int Nm1 = self->rangecnt - 1;
  int j;
  int k;

  for (k = 0; ; )
    {
      int j = k * 2 + 1;

      if (j > Nm1)
        break;

      if (j < Nm1
          && self->ranges [j]->next > self->ranges [j + 1]->next)
        ++j;

      if (self->ranges [j]->next >= elem->next)
        break;

      self->ranges [k] = self->ranges [j];

      k = j;
    }

  self->ranges [k] = elem;
}

static void
upheap (PINGER *self, int k)
{
  RANGE *elem = self->ranges [k];

  while (k)
    {
      int j = (k - 1) >> 1;

      if (self->ranges [j]->next <= elem->next)
        break;

      self->ranges [k] = self->ranges [j];

      k = j;
    }

  self->ranges [k] = elem;
}

static void *
ping_proc (void *self_)
{
  PINGER *self = (PINGER *)self_;
  PKT pkt;

  memset (&pkt, 0, sizeof (pkt));

  tstamp now  = NOW ();
  tstamp next = now;

  pkt.code    = 0;
  pkt.id      = self->magic1;
  pkt.seq     = self->magic2;
  pkt.magic   = self->magic3;
  pkt.pinger  = self->id;

  while (self->rangecnt)
    {
      RANGE *range = self->ranges [0];
      int n, k;

      // ranges [0] is always the next range to ping
      tstamp wait = range->next - now;

      // compare with the global frequency limit
      {
        tstamp diff = next - now;

        if (wait < diff)
          wait = diff;
        else if (range)
          next = range->next;
      }

      if (wait > 0.)
        ssleep (wait);

      now = NOW ();

      ts_to_pkt (&pkt, now);

      send_ping (&pkt, &range->lo, range->addrlen);

      if (!memcmp (&range->lo, &range->hi, sizeof (addr_tt)))
        {
          self->ranges [0] = self->ranges [--self->rangecnt];
          range_free (range);
        }
      else
        {
          inc_addr (&range->lo);

          range->next = next;
          range->next += range->interval;

          downheap (self);
        }

      next += self->interval;
    }

  ssleep (self->maxrtt);

  {
    uint16_t id = self->id;

    write (thr_res [1], &id, sizeof (id));
  }

  return 0;
}

/*****************************************************************************/

static void
pinger_start (PINGER *self)
{
  sigset_t fullsigset, oldsigset;
  pthread_attr_t attr;

  if (self->running)
    return;

  sigfillset (&fullsigset);

  pthread_attr_init (&attr);
  pthread_attr_setstacksize (&attr, PTHREAD_STACK_MIN < sizeof (long) * 2048 ? sizeof (long) * 2048 : PTHREAD_STACK_MIN);

  pthread_sigmask (SIG_SETMASK, &fullsigset, &oldsigset);

  if (pthread_create (&self->thrid, &attr, ping_proc, (void *)self))
    croak ("AnyEvent::FastPing: unable to create pinger thread");

  pthread_sigmask (SIG_SETMASK, &oldsigset, 0);

  self->running = 1;
}

static void
pinger_stop (PINGER *self)
{
  if (!self->running)
    return;

  self->running = 0;
  pthread_cancel (self->thrid);
  pthread_join (self->thrid, 0);
}

static void
pinger_init (PINGER *self)
{
  memset (self, 0, sizeof (PINGER));

  if (firstfree >= 0)
    {
      self->id = firstfree;
      firstfree = pingerfree [firstfree];
    }
  else if (pingercnt == 0xffff)
    croak ("unable to create more than 65536 AnyEvent::FastPing objects");
  else
    {
      if (pingercnt == pingermax)
        {
          pingermax = pingermax * 2 + 16;
          pingers    = realloc (pingers   , sizeof (pingers    [0]) * pingermax);
          pingerfree = realloc (pingerfree, sizeof (pingerfree [0]) * pingermax);
        }

      self->id = pingercnt++;
    }

  pingers [self->id] = self;

  self->recvcb   = &PL_sv_undef;
  self->interval = MIN_INTERVAL;
  self->maxrtt   = 0.5;
  self->rangemax = 16;
  self->ranges   = malloc (sizeof (self->ranges [0]) * self->rangemax);
}

static void
pinger_free (PINGER *self)
{
  pinger_stop (self);

  pingers [self->id] = 0;

  SvREFCNT_dec (self->recvq);
  SvREFCNT_dec (self->recvcb);

  pingerfree [self->id] = firstfree;
  firstfree = self->id;

  while (self->rangecnt)
    range_free (self->ranges [--self->rangecnt]);

  free (self->ranges);
}

/*****************************************************************************/

static void
recv_feed (PINGER *self, void *addr, int addrlen, tstamp rtt)
{
  if (!self->recvq)
    {
      /* first seen this round */
      if (!SvOK (self->recvcb))
        return;

      self->recvq = newAV ();

      self->nextrecv = firstrecv;
      firstrecv = self->id;
    }

  {
    AV *pkt = newAV ();

    av_extend (pkt, 2-1);

    AvARRAY (pkt)[0] = newSVpvn (addr, addrlen);
    AvARRAY (pkt)[1] = newSVnv (rtt);
    AvFILLp (pkt) = 2-1;

    av_push (self->recvq, newRV_noinc ((SV *)pkt));
  }
}

static void
recv_flush (void)
{
  if (firstrecv < 0)
    return;

  ENTER;
  SAVETMPS;

  while (firstrecv >= 0)
    {
      dSP;
      PINGER *self = pingers [firstrecv];
      firstrecv = self->nextrecv;

      self->nextrecv = -1;

      PUSHMARK (SP);
      XPUSHs (sv_2mortal (newRV_noinc ((SV *)self->recvq)));
      self->recvq = 0;
      PUTBACK;
      call_sv (self->recvcb, G_DISCARD | G_VOID);
    }

  FREETMPS;
  LEAVE;
}

/*****************************************************************************/

#if 0
static void
feed_reply (AV *res_av)
{
  dSP;
  SV *res = sv_2mortal (newRV_inc ((SV *)res_av));
  int i;

  if (av_len (res_av) < 0)
    return;

  ENTER;
  SAVETMPS;

  for (i = av_len (cbs) + 1; i--; )
    {
      SV *cb = *av_fetch (cbs, i, 1);

      PUSHMARK (SP);
      XPUSHs (res);
      PUTBACK;
      call_sv (cb, G_DISCARD | G_VOID);
    }

  FREETMPS;
  LEAVE;
}
#endif

static void
boot ()
{
  if (pipe (thr_res) < 0)
    croak ("AnyEvent::FastPing: unable to create receive pipe");

  sv_setiv (get_sv ("AnyEvent::FastPing::THR_RES_FD", 1), thr_res [0]);

  icmp4_fd = socket (AF_INET, SOCK_RAW, IPPROTO_ICMP);
  fcntl (icmp4_fd, F_SETFL, O_NONBLOCK);
#ifdef ICMP_FILTER
  {
    struct icmp_filter oval;
    oval.data = 0xffffffff & ~(1 << ICMP4_ECHO_REPLY);
    setsockopt (icmp4_fd, SOL_RAW, ICMP_FILTER, &oval, sizeof oval);
  }
#endif

#if ENABLE_IPV6
  icmp6_fd = socket (AF_INET6, SOCK_RAW, IPPROTO_ICMPV6);
  fcntl (icmp6_fd, F_SETFL, O_NONBLOCK);
# ifdef ICMP6_FILTER
  {
    struct icmp6_filter oval;
    ICMP6_FILTER_SETBLOCKALL (&oval);
    ICMP6_FILTER_SETPASS (ICMP6_ECHO_REPLY, &oval);
    setsockopt (icmp6_fd, IPPROTO_ICMPV6, ICMP6_FILTER, &oval, sizeof oval);
  }
# endif
#endif

  sv_setiv (get_sv ("AnyEvent::FastPing::ICMP4_FD", 1), icmp4_fd);
  sv_setiv (get_sv ("AnyEvent::FastPing::ICMP6_FD", 1), icmp6_fd);
}

MODULE = AnyEvent::FastPing             PACKAGE = AnyEvent::FastPing            PREFIX = pinger_

BOOT:
{
  HV *stash = gv_stashpv ("AnyEvent::FastPing", 1);

  if (sizeof (PKT) & 3)
    croak ("size of PKT structure is not a multiple of 4");

  boot ();

  newCONSTSUB (stash, "ipv4_supported", newSViv (icmp4_fd >= 0));
  newCONSTSUB (stash, "ipv6_supported", newSViv (icmp6_fd >= 0));

  newCONSTSUB (stash, "icmp4_pktsize", newSViv (HDR_SIZE_IP4 + sizeof (PKT)));
  newCONSTSUB (stash, "icmp6_pktsize", newSViv (HDR_SIZE_IP6 + sizeof (PKT)));
}

PROTOTYPES: DISABLE

void
_new (SV *klass, UV magic1, UV magic2, UV magic3)
        PPCODE:
{
        SV *pv = NEWSV (0, sizeof (PINGER));
        PINGER *self = (PINGER *)SvPVX (pv);
        SvPOK_only (pv);
        XPUSHs (sv_2mortal (sv_bless (newRV_noinc (pv), gv_stashpv (SvPV_nolen (klass), 1))));
        pinger_init (self);
        self->magic1 = magic1;
        self->magic2 = magic2;
        self->magic3 = magic3;
}

void
_free (PINGER *self)
        CODE:
        pinger_free (self);

IV
id (PINGER *self, ...)
        CODE:
        RETVAL = self->id;
        OUTPUT:
        RETVAL

void pinger_start (PINGER *self)

void pinger_stop (PINGER *self)

void _stop_id (UV id)
        CODE:
        if (id < pingercnt && pingers [id])
          pinger_stop (pingers [id]);

void interval (PINGER *self, NV interval)
        CODE:
        self->interval = interval > MIN_INTERVAL ? interval : MIN_INTERVAL;

void add_range (PINGER *self, SV *lo_, SV *hi_, NV interval)
        CODE:
{
        STRLEN lo_len, hi_len;
        char *lo = SvPVbyte (lo_, lo_len);
        char *hi = SvPVbyte (hi_, hi_len);
        RANGE *range;

        if (lo_len != hi_len || (lo_len != 4 && lo_len != 16))
          croak ("AnyEvent::FastPing::add_range address range must be specified as two binary IPv4 or IPv6 addresses");

        if (lo_len ==  4 && icmp4_fd < 0) croak ("IPv4 support unavailable");
        if (lo_len == 16 && icmp6_fd < 0) croak ("IPv6 support unavailable");

        range = calloc (1, sizeof (RANGE));

        range->next     = 0;
        range->interval = interval > MIN_INTERVAL ? interval : MIN_INTERVAL;
        range->addrlen  = lo_len;

        memcpy (sizeof (addr_tt) - lo_len + (char *)&range->lo, lo, lo_len);
        memcpy (sizeof (addr_tt) - lo_len + (char *)&range->hi, hi, lo_len);

        if (self->rangecnt == self->rangemax)
          self->ranges = realloc (self->ranges, sizeof (self->ranges [0]) * (self->rangemax <<= 1));

        self->ranges [self->rangecnt] = range;
        upheap (self, self->rangecnt);
        ++self->rangecnt;
}

void
on_recv (PINGER *self, SV *cb)
        CODE:
        SvREFCNT_dec (self->recvcb);
        self->recvcb = newSVsv (cb);

void
max_rtt (PINGER *self, NV maxrtt)
        CODE:
        self->maxrtt = maxrtt;

void
_recv_icmp4 (...)
        CODE:
{
        char buf [512];
        struct sockaddr_in sa;

        for (;;)
          {
            PINGER *pinger;
            IP4HDR *iphdr = (IP4HDR *)buf;
            socklen_t sl = sizeof (sa);
            int len = recvfrom (icmp4_fd, buf, sizeof (buf), MSG_TRUNC, (struct sockaddr *)&sa, &sl);
            int hdrlen, totlen;
            PKT *pkt;

            if (len <= HDR_SIZE_IP4)
              break;

            hdrlen = (iphdr->version_ihl & 15) * 4;
            totlen = ntohs (iphdr->tot_len);

            // packet corrupt?
            if (totlen > len
                || iphdr->protocol != IPPROTO_ICMP
                || hdrlen < HDR_SIZE_IP4 || hdrlen + sizeof (PKT) != totlen)
              continue;

            pkt = (PKT *)(buf + hdrlen);

            if (pkt->type != ICMP4_ECHO_REPLY
                || pkt->pinger >= pingercnt
                || !pingers [pkt->pinger])
              continue;

            pinger = pingers [pkt->pinger];

            if (!pkt_is_valid_for (pkt, pinger))
              continue;

            recv_feed (pinger, &sa.sin_addr, 4, NOW () - pkt_to_ts (pkt));
          }

        recv_flush ();
}

void
_recv_icmp6 (...)
        CODE:
{
        struct sockaddr_in6 sa;
        PKT pkt;

        for (;;)
          {
            PINGER *pinger;
            socklen_t sl = sizeof (sa);
            int len = recvfrom (icmp6_fd, &pkt, sizeof (pkt), MSG_TRUNC, (struct sockaddr *)&sa, &sl);

            if (len != sizeof (PKT))
              break;

            if (pkt.type != ICMP6_ECHO_REPLY
                || pkt.pinger >= pingercnt
                || !pingers [pkt.pinger])
              continue;

            pinger = pingers [pkt.pinger];

            if (!pkt_is_valid_for (&pkt, pinger))
              continue;

            recv_feed (pinger, &sa.sin6_addr, 16, NOW () - pkt_to_ts (&pkt));
          }

        recv_flush ();
}

Revision:	1.10
Committed:	Mon Jan 31 05:35:48 2011 UTC (13 years, 3 months ago) by root
Branch:	MAIN
Changes since 1.9:	+458 -336 lines
Log Message:	halfway?
#	User	Rev	Content
1	root	1.8	#if defined(__linux) \|\| defined(__FreeBSD__) \|\| defined(__OpenBSD__) \|\| defined(__NetBSD__) \|\| defined(__CYGWIN__)
2	root	1.5	# define ENABLE_IPV6 1 // if you get compilation problems try to disable IPv6
3	root	1.2	#else
4	root	1.5	# define ENABLE_IPV6 0
5	root	1.2	#endif
6	root	1.1
7			#include "EXTERN.h"
8			#include "perl.h"
9			#include "XSUB.h"
10
11			#include <pthread.h>
12
13			#include <stdio.h>
14			#include <stdlib.h>
15			#include <string.h>
16
17			#include <time.h>
18			#include <poll.h>
19			#include <unistd.h>
20			#include <inttypes.h>
21			#include <fcntl.h>
22			#include <errno.h>
23
24			#include <sys/types.h>
25			#include <sys/time.h>
26			#include <sys/socket.h>
27
28			#include <netinet/in.h>
29			#include <arpa/inet.h>
30
31			#ifdef __linux
32			# include <linux/icmp.h>
33			#endif
34	root	1.8	#if ENABLE_IPV6 && !defined (__CYGWIN__)
35	root	1.1	# include <netinet/icmp6.h>
36			#endif
37
38			#define ICMP4_ECHO 8
39			#define ICMP4_ECHO_REPLY 0
40			#define ICMP6_ECHO 128
41			#define ICMP6_ECHO_REPLY 129
42
43	root	1.9	#define DRAIN_INTERVAL 1e-6 // how long to wait when sendto returns ENOBUFS, in seconds
44			#define MIN_INTERVAL 1e-6 // minimum packet send interval, in seconds
45	root	1.1
46			#define HDR_SIZE_IP4 20
47			#define HDR_SIZE_IP6 48
48
49	root	1.4	//TODO: xread/xwrite for atomicity? we currently rely on the fact that the pip buffersize divides exactly by pointer sizes
50	root	1.1
51	root	1.8	typedef uint8_t addr_tt[16];
52	root	1.1
53	root	1.10	static int thr_res[2]; // receive from worker(s)
54			static int icmp4_fd = -1, icmp6_fd = -1;
55
56	root	1.9	/*****************************************************************************/
57
58	root	1.1	typedef double tstamp;
59
60	root	1.5	static tstamp
61			NOW (void)
62	root	1.1	{
63			struct timeval tv;
64	root	1.9
65	root	1.1	gettimeofday (&tv, 0);
66	root	1.9
67			return tv.tv_sec + tv.tv_usec * 1e-6;
68	root	1.1	}
69
70	root	1.9	static void
71			ssleep (tstamp wait)
72			{
73			#if defined (__SVR4) && defined (__sun)
74			struct timeval tv;
75
76			tv.tv_sec = wait;
77			tv.tv_usec = (wait - tv.tv_sec) * 1e6;
78
79			select (0, 0, 0, 0, &tv);
80			#elif defined(_WIN32)
81			Sleep ((unsigned long)(delay * 1e3));
82			#else
83			struct timespec ts;
84
85			ts.tv_sec = wait;
86			ts.tv_nsec = (wait - ts.tv_sec) * 1e9;
87
88			nanosleep (&ts, 0);
89			#endif
90			}
91
92			/*****************************************************************************/
93
94	root	1.8	typedef struct
95			{
96	root	1.1	uint8_t version_ihl;
97			uint8_t tos;
98			uint16_t tot_len;
99
100			uint16_t id;
101			uint16_t flags;
102
103			uint8_t ttl;
104			uint8_t protocol;
105			uint16_t cksum;
106
107			uint32_t src;
108			uint32_t dst;
109			} IP4HDR;
110
111	root	1.10	#if 0
112	root	1.8	typedef struct
113			{
114	root	1.1	uint8_t version;
115			uint8_t x1, x2, x3;
116
117			uint16_t payload_len;
118			uint8_t nxt_hdr;
119			uint8_t hop_limit;
120
121			uint8_t src[16];
122			uint8_t dst[16];
123			} IP6HDR;
124	root	1.10	#endif
125	root	1.1
126	root	1.9	/*****************************************************************************/
127
128	root	1.10	typedef struct
129			{
130			tstamp next;
131			double interval;
132			int addrlen;
133			addr_tt lo, hi;
134			void *items;
135			} RANGE;
136
137			typedef struct
138			{
139			RANGE **ranges;
140			int rangecnt, rangemax;
141	root	1.1
142	root	1.10	tstamp interval;
143			tstamp maxrtt;
144			uint16_t magic1;
145			uint16_t magic2;
146			uint16_t magic3;
147
148			int id;
149
150			AV recvq; / receive queue */
151			int nextrecv;
152			SV *recvcb;
153
154			pthread_t thrid;
155			int running;
156			} PINGER;
157
158			static PINGER **pingers;
159			static int pingerfree; / freelist next */
160			static int pingercnt;
161			static int pingermax;
162			static int firstfree = -1;
163			static int firstrecv = -1;
164
165			/*****************************************************************************/
166	root	1.1
167	root	1.8	typedef struct
168			{
169	root	1.1	uint8_t type, code;
170			uint16_t cksum;
171			uint16_t id, seq;
172	root	1.10	uint16_t pinger;
173			uint16_t magic;
174	root	1.9	uint32_t stamp_hi;
175			uint32_t stamp_lo;
176	root	1.1	} PKT;
177
178	root	1.9	static int
179	root	1.10	pkt_is_valid_for (PKT pkt, PINGER pinger)
180	root	1.9	{
181	root	1.10	return pkt->id == pinger->magic1
182			&& pkt->seq == pinger->magic2
183			&& pkt->magic == pinger->magic3;
184	root	1.9	}
185
186			static void
187			ts_to_pkt (PKT *pkt, tstamp ts)
188			{
189			/* move 12 bits of seconds into the 32 bit fractional part */
190			/* leaving 20 bits subsecond resolution and 44 bits of integers */
191			/* (of which 32 are typically usable) */
192			ts *= 1. / 4096.;
193
194			pkt->stamp_hi = ts;
195			pkt->stamp_lo = (ts - pkt->stamp_hi) * 4294967296.;
196			}
197
198			static tstamp
199			pkt_to_ts (PKT *pkt)
200			{
201			return pkt->stamp_hi * 4096.
202			+ pkt->stamp_lo * (4096. / 4294967296.);
203			}
204
205	root	1.10	static void
206			pkt_cksum (PKT *pkt)
207			{
208			uint_fast32_t sum = -pkt->cksum;
209			uint32_t wp = (uint32_t )pkt;
210			int len = sizeof (*pkt) / 4;
211
212			do
213			{
214			uint_fast32_t w = (volatile uint32_t )wp++;
215			sum += (w & 0xffff) + (w >> 16);
216			}
217			while (len--);
218
219			sum = (sum >> 16) + (sum & 0xffff); /* add high 16 to low 16 */
220			sum += sum >> 16; /* add carry */
221
222			pkt->cksum = ~sum;
223			}
224
225	root	1.9	/*****************************************************************************/
226
227	root	1.10	static void
228			range_free (RANGE *self)
229			{
230			free (self);
231			}
232
233			static void
234			inc_addr (addr_tt *addr)
235			{
236			int len = sizeof (addr_tt) - 1;
237	root	1.1
238	root	1.10	while (!++(*addr)[len])
239			--len;
240			}
241	root	1.1
242	root	1.10	/*****************************************************************************/
243	root	1.1
244	root	1.10	/* like sendto, but retries on failure */
245			static void
246			xsendto (int fd, void buf, size_t len, int flags, void sa, int salen)
247	root	1.1	{
248	root	1.10	tstamp wait = 0;
249
250			while (sendto (fd, buf, len, flags, sa, salen) < 0 && errno == ENOBUFS)
251			ssleep (wait += DRAIN_INTERVAL);
252			}
253
254			static void
255			send_ping (PKT pkt, addr_tt addr, int addr_len)
256			{
257			pkt->cksum = 0;
258
259			if (addr_len == 4)
260			{
261			struct sockaddr_in sa;
262
263			pkt->type = ICMP4_ECHO;
264			pkt_cksum (pkt);
265
266			sa.sin_family = AF_INET;
267			sa.sin_port = 0;
268
269			memcpy (&sa.sin_addr,
270			sizeof (addr_tt) - sizeof (sa.sin_addr) + (char *)addr,
271			sizeof (sa.sin_addr));
272
273			xsendto (icmp4_fd, pkt, sizeof (*pkt), 0, &sa, sizeof (sa));
274			}
275			else
276			{
277			#if ENABLE_IPV6
278			struct sockaddr_in6 sa;
279
280			pkt->type = ICMP6_ECHO;
281	root	1.1
282	root	1.10	sa.sin6_family = AF_INET6;
283			sa.sin6_port = 0;
284			sa.sin6_flowinfo = 0;
285			sa.sin6_scope_id = 0;
286	root	1.1
287	root	1.10	memcpy (&sa.sin6_addr,
288			sizeof (addr_tt) - sizeof (sa.sin6_addr) + (char *)addr,
289			sizeof (sa.sin6_addr));
290
291			xsendto (icmp6_fd, &pkt, sizeof (pkt), 0, &sa, sizeof (sa));
292			#endif
293			}
294			}
295
296			static void
297			downheap (PINGER *self)
298			{
299			RANGE elem = self->ranges [0]; / always exists */
300			int Nm1 = self->rangecnt - 1;
301			int j;
302			int k;
303
304			for (k = 0; ; )
305			{
306			int j = k * 2 + 1;
307
308			if (j > Nm1)
309			break;
310
311			if (j < Nm1
312			&& self->ranges [j]->next > self->ranges [j + 1]->next)
313			++j;
314
315			if (self->ranges [j]->next >= elem->next)
316			break;
317
318			self->ranges [k] = self->ranges [j];
319
320			k = j;
321			}
322	root	1.1
323	root	1.10	self->ranges [k] = elem;
324	root	1.1	}
325
326			static void
327	root	1.10	upheap (PINGER *self, int k)
328	root	1.1	{
329	root	1.10	RANGE *elem = self->ranges [k];
330	root	1.1
331	root	1.10	while (k)
332	root	1.1	{
333	root	1.10	int j = (k - 1) >> 1;
334
335			if (self->ranges [j]->next <= elem->next)
336			break;
337
338			self->ranges [k] = self->ranges [j];
339	root	1.1
340	root	1.10	k = j;
341	root	1.1	}
342	root	1.10
343			self->ranges [k] = elem;
344	root	1.1	}
345
346			static void *
347	root	1.10	ping_proc (void *self_)
348	root	1.1	{
349	root	1.10	PINGER self = (PINGER )self_;
350	root	1.1	PKT pkt;
351
352			memset (&pkt, 0, sizeof (pkt));
353
354	root	1.10	tstamp now = NOW ();
355			tstamp next = now;
356
357			pkt.code = 0;
358			pkt.id = self->magic1;
359			pkt.seq = self->magic2;
360			pkt.magic = self->magic3;
361			pkt.pinger = self->id;
362	root	1.1
363	root	1.10	while (self->rangecnt)
364	root	1.1	{
365	root	1.10	RANGE *range = self->ranges [0];
366			int n, k;
367	root	1.1
368	root	1.10	// ranges [0] is always the next range to ping
369			tstamp wait = range->next - now;
370	root	1.6
371	root	1.10	// compare with the global frequency limit
372			{
373			tstamp diff = next - now;
374
375			if (wait < diff)
376			wait = diff;
377			else if (range)
378			next = range->next;
379			}
380
381			if (wait > 0.)
382			ssleep (wait);
383
384			now = NOW ();
385
386			ts_to_pkt (&pkt, now);
387
388			send_ping (&pkt, &range->lo, range->addrlen);
389
390			if (!memcmp (&range->lo, &range->hi, sizeof (addr_tt)))
391	root	1.1	{
392	root	1.10	self->ranges [0] = self->ranges [--self->rangecnt];
393			range_free (range);
394			}
395			else
396			{
397			inc_addr (&range->lo);
398
399			range->next = next;
400			range->next += range->interval;
401
402			downheap (self);
403	root	1.1	}
404
405	root	1.10	next += self->interval;
406			}
407
408			ssleep (self->maxrtt);
409
410			{
411			uint16_t id = self->id;
412	root	1.1
413	root	1.10	write (thr_res [1], &id, sizeof (id));
414			}
415
416			return 0;
417			}
418
419			/*****************************************************************************/
420
421			static void
422			pinger_start (PINGER *self)
423			{
424			sigset_t fullsigset, oldsigset;
425			pthread_attr_t attr;
426
427			if (self->running)
428			return;
429
430			sigfillset (&fullsigset);
431	root	1.1
432	root	1.10	pthread_attr_init (&attr);
433			pthread_attr_setstacksize (&attr, PTHREAD_STACK_MIN < sizeof (long) * 2048 ? sizeof (long) * 2048 : PTHREAD_STACK_MIN);
434
435			pthread_sigmask (SIG_SETMASK, &fullsigset, &oldsigset);
436
437			if (pthread_create (&self->thrid, &attr, ping_proc, (void *)self))
438			croak ("AnyEvent::FastPing: unable to create pinger thread");
439
440			pthread_sigmask (SIG_SETMASK, &oldsigset, 0);
441
442			self->running = 1;
443			}
444
445			static void
446			pinger_stop (PINGER *self)
447			{
448			if (!self->running)
449			return;
450
451			self->running = 0;
452			pthread_cancel (self->thrid);
453			pthread_join (self->thrid, 0);
454			}
455
456			static void
457			pinger_init (PINGER *self)
458			{
459			memset (self, 0, sizeof (PINGER));
460	root	1.1
461	root	1.10	if (firstfree >= 0)
462			{
463			self->id = firstfree;
464			firstfree = pingerfree [firstfree];
465			}
466			else if (pingercnt == 0xffff)
467			croak ("unable to create more than 65536 AnyEvent::FastPing objects");
468			else
469			{
470			if (pingercnt == pingermax)
471	root	1.1	{
472	root	1.10	pingermax = pingermax * 2 + 16;
473			pingers = realloc (pingers , sizeof (pingers [0]) * pingermax);
474			pingerfree = realloc (pingerfree, sizeof (pingerfree [0]) * pingermax);
475			}
476
477			self->id = pingercnt++;
478			}
479
480			pingers [self->id] = self;
481
482			self->recvcb = &PL_sv_undef;
483			self->interval = MIN_INTERVAL;
484			self->maxrtt = 0.5;
485			self->rangemax = 16;
486			self->ranges = malloc (sizeof (self->ranges [0]) * self->rangemax);
487			}
488
489			static void
490			pinger_free (PINGER *self)
491			{
492			pinger_stop (self);
493
494			pingers [self->id] = 0;
495
496			SvREFCNT_dec (self->recvq);
497			SvREFCNT_dec (self->recvcb);
498
499			pingerfree [self->id] = firstfree;
500			firstfree = self->id;
501
502			while (self->rangecnt)
503			range_free (self->ranges [--self->rangecnt]);
504
505			free (self->ranges);
506			}
507
508			/*****************************************************************************/
509
510			static void
511			recv_feed (PINGER self, void addr, int addrlen, tstamp rtt)
512			{
513			if (!self->recvq)
514			{
515			/* first seen this round */
516			if (!SvOK (self->recvcb))
517			return;
518
519			self->recvq = newAV ();
520
521			self->nextrecv = firstrecv;
522			firstrecv = self->id;
523			}
524	root	1.1
525	root	1.10	{
526			AV *pkt = newAV ();
527
528			av_extend (pkt, 2-1);
529
530			AvARRAY (pkt)[0] = newSVpvn (addr, addrlen);
531			AvARRAY (pkt)[1] = newSVnv (rtt);
532			AvFILLp (pkt) = 2-1;
533	root	1.1
534	root	1.10	av_push (self->recvq, newRV_noinc ((SV *)pkt));
535			}
536			}
537	root	1.1
538	root	1.10	static void
539			recv_flush (void)
540			{
541			if (firstrecv < 0)
542			return;
543
544			ENTER;
545			SAVETMPS;
546	root	1.1
547	root	1.10	while (firstrecv >= 0)
548			{
549			dSP;
550			PINGER *self = pingers [firstrecv];
551			firstrecv = self->nextrecv;
552	root	1.1
553	root	1.10	self->nextrecv = -1;
554	root	1.1
555	root	1.10	PUSHMARK (SP);
556			XPUSHs (sv_2mortal (newRV_noinc ((SV *)self->recvq)));
557			self->recvq = 0;
558			PUTBACK;
559			call_sv (self->recvcb, G_DISCARD \| G_VOID);
560	root	1.1	}
561
562	root	1.10	FREETMPS;
563			LEAVE;
564	root	1.1	}
565
566	root	1.10	/*****************************************************************************/
567
568			#if 0
569	root	1.1	static void
570			feed_reply (AV *res_av)
571			{
572			dSP;
573			SV res = sv_2mortal (newRV_inc ((SV )res_av));
574			int i;
575
576	root	1.6	if (av_len (res_av) < 0)
577			return;
578
579	root	1.1	ENTER;
580			SAVETMPS;
581
582			for (i = av_len (cbs) + 1; i--; )
583			{
584			SV cb = av_fetch (cbs, i, 1);
585
586			PUSHMARK (SP);
587			XPUSHs (res);
588			PUTBACK;
589			call_sv (cb, G_DISCARD \| G_VOID);
590			}
591
592			FREETMPS;
593			LEAVE;
594			}
595	root	1.10	#endif
596	root	1.1
597			static void
598			boot ()
599			{
600	root	1.10	if (pipe (thr_res) < 0)
601			croak ("AnyEvent::FastPing: unable to create receive pipe");
602	root	1.1
603	root	1.10	sv_setiv (get_sv ("AnyEvent::FastPing::THR_RES_FD", 1), thr_res [0]);
604	root	1.1
605			icmp4_fd = socket (AF_INET, SOCK_RAW, IPPROTO_ICMP);
606	root	1.2	fcntl (icmp4_fd, F_SETFL, O_NONBLOCK);
607	root	1.1	#ifdef ICMP_FILTER
608			{
609			struct icmp_filter oval;
610			oval.data = 0xffffffff & ~(1 << ICMP4_ECHO_REPLY);
611			setsockopt (icmp4_fd, SOL_RAW, ICMP_FILTER, &oval, sizeof oval);
612			}
613			#endif
614
615	root	1.5	#if ENABLE_IPV6
616	root	1.1	icmp6_fd = socket (AF_INET6, SOCK_RAW, IPPROTO_ICMPV6);
617	root	1.2	fcntl (icmp6_fd, F_SETFL, O_NONBLOCK);
618	root	1.1	# ifdef ICMP6_FILTER
619			{
620			struct icmp6_filter oval;
621			ICMP6_FILTER_SETBLOCKALL (&oval);
622			ICMP6_FILTER_SETPASS (ICMP6_ECHO_REPLY, &oval);
623			setsockopt (icmp6_fd, IPPROTO_ICMPV6, ICMP6_FILTER, &oval, sizeof oval);
624			}
625			# endif
626			#endif
627
628			sv_setiv (get_sv ("AnyEvent::FastPing::ICMP4_FD", 1), icmp4_fd);
629			sv_setiv (get_sv ("AnyEvent::FastPing::ICMP6_FD", 1), icmp6_fd);
630			}
631
632	root	1.10	MODULE = AnyEvent::FastPing PACKAGE = AnyEvent::FastPing PREFIX = pinger_
633	root	1.1
634			BOOT:
635			{
636			HV *stash = gv_stashpv ("AnyEvent::FastPing", 1);
637
638	root	1.9	if (sizeof (PKT) & 3)
639			croak ("size of PKT structure is not a multiple of 4");
640
641	root	1.1	boot ();
642
643			newCONSTSUB (stash, "ipv4_supported", newSViv (icmp4_fd >= 0));
644			newCONSTSUB (stash, "ipv6_supported", newSViv (icmp6_fd >= 0));
645
646			newCONSTSUB (stash, "icmp4_pktsize", newSViv (HDR_SIZE_IP4 + sizeof (PKT)));
647			newCONSTSUB (stash, "icmp6_pktsize", newSViv (HDR_SIZE_IP6 + sizeof (PKT)));
648			}
649
650			PROTOTYPES: DISABLE
651
652	root	1.10	void
653			_new (SV *klass, UV magic1, UV magic2, UV magic3)
654			PPCODE:
655			{
656			SV *pv = NEWSV (0, sizeof (PINGER));
657			PINGER self = (PINGER )SvPVX (pv);
658			SvPOK_only (pv);
659			XPUSHs (sv_2mortal (sv_bless (newRV_noinc (pv), gv_stashpv (SvPV_nolen (klass), 1))));
660			pinger_init (self);
661			self->magic1 = magic1;
662			self->magic2 = magic2;
663			self->magic3 = magic3;
664			}
665
666			void
667			_free (PINGER *self)
668			CODE:
669			pinger_free (self);
670
671			IV
672			id (PINGER *self, ...)
673	root	1.1	CODE:
674	root	1.10	RETVAL = self->id;
675			OUTPUT:
676			RETVAL
677
678			void pinger_start (PINGER *self)
679
680			void pinger_stop (PINGER *self)
681	root	1.6
682	root	1.10	void _stop_id (UV id)
683			CODE:
684			if (id < pingercnt && pingers [id])
685			pinger_stop (pingers [id]);
686	root	1.1
687	root	1.10	void interval (PINGER *self, NV interval)
688			CODE:
689			self->interval = interval > MIN_INTERVAL ? interval : MIN_INTERVAL;
690	root	1.1
691	root	1.10	void add_range (PINGER self, SV lo_, SV *hi_, NV interval)
692			CODE:
693			{
694			STRLEN lo_len, hi_len;
695			char *lo = SvPVbyte (lo_, lo_len);
696			char *hi = SvPVbyte (hi_, hi_len);
697			RANGE *range;
698	root	1.1
699	root	1.10	if (lo_len != hi_len \|\| (lo_len != 4 && lo_len != 16))
700			croak ("AnyEvent::FastPing::add_range address range must be specified as two binary IPv4 or IPv6 addresses");
701	root	1.1
702	root	1.10	if (lo_len == 4 && icmp4_fd < 0) croak ("IPv4 support unavailable");
703			if (lo_len == 16 && icmp6_fd < 0) croak ("IPv6 support unavailable");
704	root	1.1
705	root	1.10	range = calloc (1, sizeof (RANGE));
706	root	1.1
707	root	1.10	range->next = 0;
708			range->interval = interval > MIN_INTERVAL ? interval : MIN_INTERVAL;
709			range->addrlen = lo_len;
710	root	1.1
711	root	1.10	memcpy (sizeof (addr_tt) - lo_len + (char *)&range->lo, lo, lo_len);
712			memcpy (sizeof (addr_tt) - lo_len + (char *)&range->hi, hi, lo_len);
713	root	1.1
714	root	1.10	if (self->rangecnt == self->rangemax)
715			self->ranges = realloc (self->ranges, sizeof (self->ranges [0]) * (self->rangemax <<= 1));
716	root	1.1
717	root	1.10	self->ranges [self->rangecnt] = range;
718			upheap (self, self->rangecnt);
719			++self->rangecnt;
720	root	1.1	}
721
722	root	1.10	void
723			on_recv (PINGER self, SV cb)
724	root	1.1	CODE:
725	root	1.10	SvREFCNT_dec (self->recvcb);
726			self->recvcb = newSVsv (cb);
727	root	1.1
728	root	1.10	void
729			max_rtt (PINGER *self, NV maxrtt)
730			CODE:
731			self->maxrtt = maxrtt;
732	root	1.1
733			void
734			_recv_icmp4 (...)
735			CODE:
736			{
737			char buf [512];
738			struct sockaddr_in sa;
739	root	1.9
740	root	1.1	for (;;)
741			{
742	root	1.10	PINGER *pinger;
743	root	1.6	IP4HDR iphdr = (IP4HDR )buf;
744	root	1.10	socklen_t sl = sizeof (sa);
745	root	1.2	int len = recvfrom (icmp4_fd, buf, sizeof (buf), MSG_TRUNC, (struct sockaddr *)&sa, &sl);
746	root	1.6	int hdrlen, totlen;
747			PKT *pkt;
748	root	1.1
749			if (len <= HDR_SIZE_IP4)
750			break;
751
752	root	1.6	hdrlen = (iphdr->version_ihl & 15) * 4;
753			totlen = ntohs (iphdr->tot_len);
754	root	1.1
755			// packet corrupt?
756	root	1.9	if (totlen > len
757	root	1.1	\|\| iphdr->protocol != IPPROTO_ICMP
758			\|\| hdrlen < HDR_SIZE_IP4 \|\| hdrlen + sizeof (PKT) != totlen)
759			continue;
760
761	root	1.6	pkt = (PKT *)(buf + hdrlen);
762	root	1.1
763	root	1.10	if (pkt->type != ICMP4_ECHO_REPLY
764			\|\| pkt->pinger >= pingercnt
765			\|\| !pingers [pkt->pinger])
766	root	1.1	continue;
767
768	root	1.10	pinger = pingers [pkt->pinger];
769
770			if (!pkt_is_valid_for (pkt, pinger))
771			continue;
772	root	1.1
773	root	1.10	recv_feed (pinger, &sa.sin_addr, 4, NOW () - pkt_to_ts (pkt));
774	root	1.1	}
775
776	root	1.10	recv_flush ();
777	root	1.1	}
778
779			void
780			_recv_icmp6 (...)
781			CODE:
782			{
783			struct sockaddr_in6 sa;
784			PKT pkt;
785	root	1.9
786	root	1.1	for (;;)
787			{
788	root	1.10	PINGER *pinger;
789			socklen_t sl = sizeof (sa);
790	root	1.2	int len = recvfrom (icmp6_fd, &pkt, sizeof (pkt), MSG_TRUNC, (struct sockaddr *)&sa, &sl);
791	root	1.1
792			if (len != sizeof (PKT))
793			break;
794
795	root	1.10	if (pkt.type != ICMP6_ECHO_REPLY
796			\|\| pkt.pinger >= pingercnt
797			\|\| !pingers [pkt.pinger])
798	root	1.1	continue;
799
800	root	1.10	pinger = pingers [pkt.pinger];
801
802			if (!pkt_is_valid_for (&pkt, pinger))
803			continue;
804	root	1.1
805	root	1.10	recv_feed (pinger, &sa.sin6_addr, 16, NOW () - pkt_to_ts (&pkt));
806	root	1.1	}
807
808	root	1.10	recv_flush ();
809	root	1.1	}
810