2 files changed, 54 insertions, 0 deletions

diff --git a/overlays/libdscp/src/Makefile b/overlays/libdscp/src/Makefile
new file mode 100644
index 00000000..e76930d7
--- /dev/null
+++ b/overlays/libdscp/src/Makefile
@@ -0,0 +1,16 @@
+ifeq ($(PREFIX),)
+    PREFIX := /usr/local
+endif
+
+.PHONY: all install
+
+all: libdscp
+
+libdscp:
+        $(CC) \
+                -o $@.so $@.c \
+                -D_GNU_SOURCE \
+                -nostartfiles -shared
+install: libdscp
+        install -d $(DESTDIR)$(PREFIX)/lib/
+        install -m 644 libdscp.so $(DESTDIR)$(PREFIX)/lib/
diff --git a/overlays/libdscp/src/libdscp.c b/overlays/libdscp/src/libdscp.c
new file mode 100644
index 00000000..1bc9a16d
--- /dev/null
+++ b/overlays/libdscp/src/libdscp.c
@@ -0,0 +1,38 @@
+#include <dlfcn.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <arpa/inet.h>
+
+void _init(void);
+int (*sys_connect)(int sockfd, const struct sockaddr *addr, socklen_t addrlen);
+int connect(int sockfd, const struct sockaddr *addr, socklen_t addrlen);
+
+int dscp_val = 0;
+
+void _init(void) {
+  const char* err;
+
+  char* env_dscp;
+
+  env_dscp = getenv("DSCP");
+
+  if (env_dscp)
+    dscp_val = atoi(env_dscp) << 2;
+
+  sys_connect = dlsym(RTLD_NEXT, "connect");
+
+  err = dlerror();
+  if (err != NULL)
+    (void)fprintf(stderr, "libdscp:dlsym (connect): %s\n", err);
+}
+
+int connect(int sockfd, const struct sockaddr *addr, socklen_t addrlen) {
+  int oerrno = errno;
+
+  setsockopt(sockfd, IPPROTO_IP, IP_TOS, &dscp_val, sizeof dscp_val);
+  setsockopt(sockfd, IPPROTO_IPV6, IPV6_TCLASS, &dscp_val, sizeof dscp_val);
+
+  errno = oerrno;
+  return sys_connect(sockfd, addr, addrlen);
+}