From 2e969cb5013f39ebc0ad6e5978accc7e9332482e Mon Sep 17 00:00:00 2001 From: Steffen Vogel Date: Sun, 12 Jul 2015 21:10:34 +0200 Subject: [PATCH] thats it for today --- project/Makefile | 2 +- project/probe.c | 105 ++++++------------------------------------- project/tc.c | 114 +++++++++++++++++++++++++++++++++++++++++++++-- project/tc.h | 31 +++++++++++++ project/ts.c | 101 +++++++++++++++++++++++++++++++++++++++++ project/ts.h | 10 +++++ 6 files changed, 267 insertions(+), 96 deletions(-) create mode 100644 project/tc.h create mode 100644 project/ts.c create mode 100644 project/ts.h diff --git a/project/Makefile b/project/Makefile index f314f84..0783dad 100644 --- a/project/Makefile +++ b/project/Makefile @@ -1,6 +1,6 @@ TARGET = netem -OBJS = main.o probe.o +OBJS = main.o probe.o ts.o tc.o CC = gcc CFLAGS = -g -lrt -std=c99 -Wall diff --git a/project/probe.c b/project/probe.c index 99635ce..a3e5ebf 100644 --- a/project/probe.c +++ b/project/probe.c @@ -1,4 +1,5 @@ #define _POSIX_C_SOURCE 199309L +#define _BSD_SOURCE #include #include @@ -7,110 +8,32 @@ #include #include #include -#include - -#include -#include - -#include -#include -#include #include #include #include -#include #include +#include +#include +#include -#define SIOCSHWTSTAMP 0x89b0 -#define SIOCGHWTSTAMP 0x89b1 - -void print_ts(struct timespec *ts) -{ - printf("%lld.%.9ld\n", (long long) ts->tv_sec, ts->tv_nsec); -} - -int if_enable_ts(const char *dev) -{ - struct hwtstamp_config tsc = { - .flags = 0, - .tx_type = HWTSTAMP_TX_ON, - .rx_filter = HWTSTAMP_FILTER_ALL - }; - struct ifreq ifr = { - .ifr_data = (void *) &tsc - }; - - strncpy(ifr.ifr_name, dev, IFNAMSIZ); - - int sd = socket(AF_INET, SOCK_DGRAM, 0); - if (sd < 0) - error(-1, errno, "Failed to create socket"); - - if (ioctl(sd, SIOCSHWTSTAMP, &ifr)) - error(-1, errno, "Failed to enable timestamping"); - - close(sd); - - return 0; -} - -int recvmsg_ts(int sd, struct msghdr *msgh, int flags, struct timespec *ts, int *key, int *id) -{ - char buf[1024]; - msgh->msg_control = &buf; - msgh->msg_controllen = sizeof(buf); - - struct cmsghdr *cmsg; - struct sock_extended_err *serr = NULL; - struct timespec *tss = NULL; - - int ret = recvmsg(sd, msgh, flags); - if (ret >= 0) { - for (cmsg = CMSG_FIRSTHDR(msgh); cmsg != NULL; cmsg = CMSG_NXTHDR(msgh, cmsg)) { - if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) - tss = (struct timespec *) CMSG_DATA(cmsg); - else if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) || - (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) - serr = (struct sock_extended_err *) CMSG_DATA(cmsg); - } - - if (!tss) - return -2; - - for (int i = 0; i < 3; i++) - ts[i] = tss[i]; - - if (serr) { - *key = serr->ee_info; - *id = serr->ee_data; - } - } - - return ret; -} +#include "ts.h" int probe() { - int val; - + char *dev = "lo"; + printf("Start probing\n"); - //if_enable_ts("lo"); - /* Create RAW socket */ int sd = socket(AF_INET, SOCK_RAW, 88); if (sd < 0) error(-1, errno, "Failed to create socket"); - /* Enable kernel / hw timestamping */ - val = SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_TX_SOFTWARE; /* Enable SW timestamps */ -// val |= SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE; /* Enable HW timestamps */ - val |= SOF_TIMESTAMPING_SOFTWARE; //| SOF_TIMESTAMPING_RAW_HARDWARE; /* Report SW and HW timestamps */ -// val |= SOF_TIMESTAMPING_OPT_TSONLY; /* Only return timestamp in cmsg */ - - if (setsockopt(sd, SOL_SOCKET, SO_TIMESTAMPING, (void *) &val, sizeof(val))) + if (ts_enable_if(dev)) + error(-1, errno, "Failed to enable timestamping for interface: %s", dev); + if (ts_enable_sd(sd)) error(-1, errno, "Failed to set SO_TIMESTAMPING"); struct timespec ts[3]; @@ -125,8 +48,8 @@ int probe() inet_aton("8.8.8.8", &sa.sin_addr); - //if (sendto(sd, "test", sizeof("test"), 0, (struct sockaddr*) &sa, sizeof(sa)) == -1) - // error(-1, errno, "Failed to send"); + if (sendto(sd, "test", sizeof("test"), 0, (struct sockaddr*) &sa, sizeof(sa)) == -1) + error(-1, errno, "Failed to send"); enum direction { TX, RX } dir = RX; @@ -141,13 +64,13 @@ retry: if (poll(&pfd, 1, 10000) < 0) goto retry; int id, key; - if (recvmsg_ts(sd, &msgh, (dir == TX) ? MSG_ERRQUEUE : 0, ts, &key, &id) < 0) + if (ts_recvmsg(sd, &msgh, (dir == TX) ? MSG_ERRQUEUE : 0, ts, &key, &id) < 0) error(-1, errno, "Failed to receive TS"); printf("Received key=%d, id=%d\n", key, id); for (int i = 0; i < 3; i++) - print_ts(&ts[i]); + ts_print(&ts[i]); return 0; } diff --git a/project/tc.c b/project/tc.c index 4264110..86af039 100644 --- a/project/tc.c +++ b/project/tc.c @@ -1,12 +1,118 @@ +#include +#include +#include +#include "tc.h" - -int netem_update(struct netem *ne) +struct rtnl_link * tc_get_link(struct nl_sock *sock, const char *dev) { + struct nl_cache *cache; + struct rtnl_link *link; + rtnl_link_alloc_cache(sock, AF_UNSPEC, &cache); + link = rtnl_link_get_by_name(cache, "eth0"); + nl_cache_put(cache); + + return link; } -int netem_create(struct netem *ne) +int tc_init_prio(struct nl_sock *sock) { + q = rtnl_qdisc_alloc(); + + rtnl_tc_set_ifindex(TC_CAST(q), if_index); + rtnl_tc_set_parent(TC_CAST(q), TC_H_ROOT); + rtnl_tc_set_handle(TC_CAST(q), TC_HANDLE(1, 0)); + rtnl_tc_set_kind(TC_CAST(q), "prio"); + + rtnl_qdisc_prio_set_bands (struct rtnl_qdisc *qdisc, int bands); + rtnl_qdisc_prio_set_priomap (struct rtnl_qdisc *qdisc, uint8_t priomap[], int len); + + rtnl_qdisc_add(sock, q, NLM_F_CREATE); + rtnl_qdisc_put(q); +} + +int tc_init_netem(struct nl_sock *sock, struct tc_netem *ne) +{ + q = rtnl_qdisc_alloc(); + + rtnl_tc_set_ifindex(TC_CAST(q), if_index); + rtnl_tc_set_parent(TC_CAST(q), TC_H_ROOT); + rtnl_tc_set_handle(TC_CAST(q), TC_HANDLE(1, 0)); + rtnl_tc_set_kind(TC_CAST(q), "netem"); + + rtnl_netem_set_gap(q, int); + rtnl_netem_set_reorder_probability(q, int); + rtnl_netem_set_reorder_correlation(q, int); + rtnl_netem_set_corruption_probability(q, int); + rtnl_netem_set_corruption_correlation(q, int); + rtnl_netem_set_loss(q, int); + rtnl_netem_set_loss_correlation(q, int); + rtnl_netem_set_duplicate(q, int); + rtnl_netem_set_duplicate_correlation(q, int); + rtnl_netem_set_delay(q, int); + rtnl_netem_set_jitter(q, int); + rtnl_netem_set_delay_correlation(q, int); + rtnl_netem_set_delay_distribution(q, const char *); + + rtnl_qdisc_add(sock, q, NLM_F_CREATE); + rtnl_qdisc_put(q); +} + +int tc_init_classifier(struct nl_sock *sock, int mark) +{ + struct rtnl_cls *c = rtnl_cls_alloc(); + + rtnl_tc_set_link(TC_CAST(c), if_index); + rtnl_tc_set_parent(TC_CAST(c), TC_H_ROOT); + rtnl_tc_set_handle(TC_CAST(c), TC_HANDLE(1, 0)); + rtnl_tc_set_kind(TC_CAST(c), "netem"); + + rtnl_fw_set_classid(c, uint32_t classid); + rtnl_fw_set_mask(c, uint32_t mask); + + rtnl_cls_add(sock, cls, int flags); + rtnl_cls_put(cls); +} + +struct nl_sock * tc_init(int mark) +{ + struct nl_sock *sock; + struct rtnl_link *link; + struct rtnl_qdisc *q; + + /* Create connection to netlink */ + sock = nl_socket_alloc(); + nl_connect(sock, NETLINK_ROUTE); + + if (tc_init_prio(sock)) + error(0, -1, "Failed to setup TC: prio qdisc"); + if (tc_init_classifier(sock, mark)) + error(0, -1, "Failed to setup TC: fw filter"); -} \ No newline at end of file + /* Get interface index */ + int ifindex = tc_get_ifindex; + + /* Setup classifier */ + rtnl_fw_set_classid (struct rtnl_cls *cls, uint32_t classid); + rtnl_fw_set_mask (struct rtnl_cls *cls, uint32_t mask); + rtnl_cls_add(struct nl_sock *sk, struct rtnl_cls *cls, int flags); + + return sock; +} + +int tc_reset(struct nl_sock *) +{ + struct rtnl_qdisc *q; + + /* Restore default qdisc by deleting the root qdisc (see tc-pfifo_fast(8)) */ + q = rtnl_qdisc_alloc(); + rtnl_tc_set_link(TC_CAST(q), link); + rtnl_qdisc_delete(sock, q); + rtnl_qdisc_put(q); + + /* Shutdown */ + nl_close(sock); + nl_socket_free(sock); +} + diff --git a/project/tc.h b/project/tc.h new file mode 100644 index 0000000..0a1a791 --- /dev/null +++ b/project/tc.h @@ -0,0 +1,31 @@ +#include +#include +#include + +struct tc_netem { + int gap; + int reorder_prop; + int reorder_corr; + int corruption_prop; + int corruption_corr; + int loss_prop; + int loss_corr; + int duplication_prob; + int duplication_corr; + int jitter; + int delay; + int delay_corr; + char *delay_distr; +}; + +struct rtnl_link * tc_get_link(struct nl_sock *sock, const char *dev); + +int tc_init_prio(struct nl_sock *sock); + +int tc_init_netem(struct nl_sock *sock, struct tc_netem *ne); + +int tc_init_classifier(struct nl_sock *sock, int mark); + +struct nl_sock * tc_init(int mark); + +int tc_reset(struct nl_sock *); diff --git a/project/ts.c b/project/ts.c new file mode 100644 index 0000000..0396e90 --- /dev/null +++ b/project/ts.c @@ -0,0 +1,101 @@ +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +#define SIOCSHWTSTAMP 0x89b0 +#define SIOCGHWTSTAMP 0x89b1 + +ssize_t ts_sendmsg(int sd, const struct msghdr *msg, int flags, struct timespec *ts) +{ + return sendmsg(sd, msg, flags); +} + +ssize_t ts_recvmsg(int sd, struct msghdr *msgh, int flags, struct timespec *ts, int *key, int *id) +{ + char buf[1024]; + msgh->msg_control = &buf; + msgh->msg_controllen = sizeof(buf); + + struct cmsghdr *cmsg; + struct sock_extended_err *serr = NULL; + struct timespec *tss = NULL; + + int ret = recvmsg(sd, msgh, flags); + if (ret >= 0) { + for (cmsg = CMSG_FIRSTHDR(msgh); cmsg != NULL; cmsg = CMSG_NXTHDR(msgh, cmsg)) { + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SCM_TIMESTAMPING) + tss = (struct timespec *) CMSG_DATA(cmsg); + else if ((cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) || + (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_RECVERR)) + serr = (struct sock_extended_err *) CMSG_DATA(cmsg); + } + + if (!tss) + return -2; + + for (int i = 0; i < 3; i++) + ts[i] = tss[i]; + + if (serr) { + *key = serr->ee_info; + *id = serr->ee_data; + } + } + + return ret; +} + +void ts_print(struct timespec *ts) +{ + printf("%lld.%.9ld\n", (long long) ts->tv_sec, ts->tv_nsec); +} + +int ts_enable_sd(int sd) +{ + int val; + + /* Enable kernel / hw timestamping */ + val = SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_TX_SOFTWARE; /* Enable SW timestamps */ +// val |= SOF_TIMESTAMPING_TX_HARDWARE | SOF_TIMESTAMPING_RX_HARDWARE; /* Enable HW timestamps */ + val |= SOF_TIMESTAMPING_SOFTWARE; //| SOF_TIMESTAMPING_RAW_HARDWARE; /* Report SW and HW timestamps */ +// val |= SOF_TIMESTAMPING_OPT_TSONLY; /* Only return timestamp in cmsg */ + + return setsockopt(sd, SOL_SOCKET, SO_TIMESTAMPING, (void *) &val, sizeof(val)); +} + +int ts_enable_if(const char *dev) +{ + int ret = -1, sd; + + struct hwtstamp_config tsc = { + .flags = 0, + .tx_type = HWTSTAMP_TX_ON, + .rx_filter = HWTSTAMP_FILTER_ALL + }; + struct ifreq ifr = { + .ifr_data = (void *) &tsc + }; + + strncpy(ifr.ifr_name, dev, IFNAMSIZ); + + sd = socket(AF_INET, SOCK_DGRAM, 0); + if (sd >= 0) + ret = ioctl(sd, SIOCSHWTSTAMP, &ifr); + + close(sd); + + return ret; +} + diff --git a/project/ts.h b/project/ts.h new file mode 100644 index 0000000..cb945ff --- /dev/null +++ b/project/ts.h @@ -0,0 +1,10 @@ +ssize_t ts_sendmsg(int sd, const struct msghdr *msgh, int flags, struct timespec *ts); + +ssize_t ts_recvmsg(int sd, struct msghdr *msgh, int flags, struct timespec *ts, int *key, int *id); + +void ts_print(struct timespec *ts); + +int ts_enable_if(const char *dev); + +int ts_enable_sd(int sd); +