Implement support for QoS / ToS fields.

[liboping.git] / src / liboping.c

diff --git a/src/liboping.c b/src/liboping.c
index 3c1c92a..2e7f288 100644 (file)
--- a/src/liboping.c
+++ b/src/liboping.c
@@ -117,6 +117,7 @@ struct pinghost
        double                   latency;
        uint32_t                 dropped;
        int                      recv_ttl;
+       unsigned                 recv_tos;
        char                    *data;
 
        void                    *context;
@@ -129,6 +130,7 @@ struct pingobj
        double                   timeout;
        int                      ttl;
        int                      addrfamily;
+       unsigned                 tos;
        char                    *data;
 
        struct sockaddr         *srcaddr;
@@ -350,9 +352,10 @@ static pinghost_t *ping_receive_ipv4 (pingobj_t *obj, char *buffer,
                                ident, seq);
        }
 
-       if (ptr != NULL)
+       if (ptr != NULL){
                ptr->recv_ttl = ip_hdr->ip_ttl;
-
+               ptr->recv_tos = ip_hdr->ip_tos;
+       }
        return (ptr);
 }
 
@@ -450,6 +453,7 @@ static int ping_receive_one (pingobj_t *obj, const pinghost_t *ph,
        struct timeval diff;
        pinghost_t *host = NULL;
        int recv_ttl;
+       unsigned recv_tos;
        
        /*
         * Set up the receive buffer..
@@ -495,6 +499,7 @@ static int ping_receive_one (pingobj_t *obj, const pinghost_t *ph,
 
        /* Iterate over all auxiliary data in msghdr */
        recv_ttl = -1;
+       recv_tos = 0xffff;
        for (cmsg = CMSG_FIRSTHDR (&msghdr); /* {{{ */
                        cmsg != NULL;
                        cmsg = CMSG_NXTHDR (&msghdr, cmsg))
@@ -504,6 +509,12 @@ static int ping_receive_one (pingobj_t *obj, const pinghost_t *ph,
                        if (cmsg->cmsg_level != IPPROTO_IP)
                                continue;
 
+                       if (cmsg->cmsg_type == IP_TOS)
+                       {
+                               memcpy (&recv_tos, CMSG_DATA (cmsg),
+                                               sizeof (recv_tos));
+                               dprintf ("TOSv4 = %u;\n", recv_tos);
+                       } else
                        if (cmsg->cmsg_type == IP_TTL)
                        {
                                memcpy (&recv_ttl, CMSG_DATA (cmsg),
@@ -521,6 +532,12 @@ static int ping_receive_one (pingobj_t *obj, const pinghost_t *ph,
                        if (cmsg->cmsg_level != IPPROTO_IPV6)
                                continue;
 
+                       if (cmsg->cmsg_type == IPV6_RECVTCLASS)
+                       {
+                               memcpy (&recv_tos, CMSG_DATA (cmsg),
+                                               sizeof (recv_tos));
+                               dprintf ("TOSv6 = %u;\n", recv_tos);
+                       } else
                        if (cmsg->cmsg_type == IPV6_HOPLIMIT)
                        {
                                memcpy (&recv_ttl, CMSG_DATA (cmsg),
@@ -579,6 +596,8 @@ static int ping_receive_one (pingobj_t *obj, const pinghost_t *ph,
 
        if (recv_ttl >= 0)
                host->recv_ttl = recv_ttl;
+       if (recv_tos != 0xffff)
+               host->recv_tos = recv_tos;
 
        host->latency  = ((double) diff.tv_usec) / 1000.0;
        host->latency += ((double) diff.tv_sec)  * 1000.0;
@@ -931,6 +950,29 @@ static int ping_set_ttl (pinghost_t *ph, int ttl)
        return (ret);
 }
 
+/*
+ * Set the TOS of a socket protocol independently.
+ */
+static int ping_set_tos (pinghost_t *ph, unsigned tos)
+{
+       int ret = -2;
+
+       if (ph->addrfamily == AF_INET)
+       {
+               dprintf ("Setting TP_TOS to %i\n", ttl);
+               ret = setsockopt (ph->fd, IPPROTO_IP, IP_TOS,
+                               &tos, sizeof (tos));
+       }
+       else if (ph->addrfamily == AF_INET6)
+       {
+               dprintf ("Setting IPV6_TCLASS to %i\n", ttl);
+               ret = setsockopt (ph->fd, IPPROTO_IPV6, IPV6_TCLASS,
+                               &tos, sizeof (tos));
+       }
+
+       return (ret);
+}
+
 static int ping_get_ident (void)
 {
        int fd;
@@ -1036,6 +1078,7 @@ pingobj_t *ping_construct (void)
        obj->ttl        = PING_DEF_TTL;
        obj->addrfamily = PING_DEF_AF;
        obj->data       = strdup (PING_DEF_DATA);
+       obj->tos        = 0;
 
        return (obj);
 }
@@ -1081,6 +1124,13 @@ int ping_setopt (pingobj_t *obj, int option, void *value)
 
        switch (option)
        {
+               case PING_OPT_TOS:{
+                       obj->tos=*(unsigned *)value;
+                       pinghost_t *ph;
+                       for (ph = obj->head; ph != NULL; ph = ph->next)
+                               ping_set_tos (ph, obj->tos);
+                       break;
+               }
                case PING_OPT_TIMEOUT:
                        obj->timeout = *((double *) value);
                        if (obj->timeout < 0.0)
@@ -1484,6 +1534,7 @@ int ping_host_add (pingobj_t *obj, const char *host)
        }
 
        ping_set_ttl (ph, obj->ttl);
+       ping_set_tos (ph, obj->tos);
 
        return (0);
 } /* int ping_host_add */
@@ -1662,6 +1713,16 @@ int ping_iterator_get_info (pingobj_iter_t *iter, int info,
                        *((int *) buffer) = iter->recv_ttl;
                        ret = 0;
                        break;
+
+               case PING_INFO_TOS:
+                       ret = ENOMEM;
+                       if (*buffer_len>sizeof(unsigned)) *buffer_len=sizeof(unsigned);
+                       if (!*buffer_len) *buffer_len=1;
+                       if (orig_buffer_len < *buffer_len)
+                               break;
+                       memcpy(buffer,&iter->recv_tos,*buffer_len);
+                       ret = 0;
+                       break;
        }
 
        return (ret);