From 3b787a204333549c61541958aaaf434f52eeafbd Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Adrien=20Gallou=C3=ABt?= <adrien@gallouet.fr>
Date: Sun, 17 Dec 2017 16:03:00 +0000
Subject: [PATCH] Move ICMP parsing code in src/ip.h
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Signed-off-by: Adrien Gallouët <adrien@gallouet.fr>
---
 src/ip.h   | 40 ++++++++++++++++++++++++++++++++++++++--
 src/main.c | 17 ++++++-----------
 2 files changed, 44 insertions(+), 13 deletions(-)

diff --git a/src/ip.h b/src/ip.h
index 49b1fcd..55b88ca 100644
--- a/src/ip.h
+++ b/src/ip.h
@@ -19,6 +19,42 @@ ip_get_version(const uint8_t *data, size_t size)
     return data[0] >> 4;
 }
 
+static inline uint32_t
+ip_read32(const uint8_t *src)
+{
+    uint32_t ret = src[3];
+    ret |= ((uint32_t)src[2]) << 8;
+    ret |= ((uint32_t)src[1]) << 16;
+    ret |= ((uint32_t)src[0]) << 24;
+    return ret;
+}
+
+static inline uint16_t
+ip_read16(const uint8_t *src)
+{
+    uint16_t ret = src[1];
+    ret |= ((uint16_t)src[0]) << 8;
+    return ret;
+}
+
+static inline int
+ip_get_mtu(struct ip_common *ic, const uint8_t *data, size_t size)
+{
+    if (ic->hdr_size <= 0 || ic->hdr_size + 8 > size)
+        return -1;
+
+    const uint8_t *p = &data[ic->hdr_size];
+
+    if (ic->version == 4 && ic->proto == 1 && p[0] == 3)
+        return ip_read16(&p[6]);
+
+    // not tested..
+    // if (ic->version == 6 && ic->proto == 58 && p[0] == 2)
+    //    return ip_read32(&p[4]);
+
+    return -1;
+}
+
 static inline int
 ip_get_common(struct ip_common *ic, const uint8_t *data, size_t size)
 {
@@ -29,7 +65,7 @@ ip_get_common(struct ip_common *ic, const uint8_t *data, size_t size)
         ic->tc = data[1];
         ic->proto = data[9];
         ic->hdr_size = (data[0] & 0xF) << 2;
-        ic->size = ((data[2] << 8) | data[3]);
+        ic->size = ip_read16(&data[2]);
         if (ic->size >= 20)
             return 0;
         break;
@@ -37,7 +73,7 @@ ip_get_common(struct ip_common *ic, const uint8_t *data, size_t size)
         ic->tc = ((data[0] & 0xF) << 4) | (data[1] >> 4);
         ic->proto = data[6];
         ic->hdr_size = 40;
-        ic->size = ((data[4] << 8) | data[5]) + 40;
+        ic->size = ip_read16(&data[4]) + 40;
         return 0;
     }
 
diff --git a/src/main.c b/src/main.c
index 5391993..3d2cb84 100644
--- a/src/main.c
+++ b/src/main.c
@@ -411,17 +411,12 @@ main(int argc, char **argv)
             socklen_t sl = sizeof(ss);
             ssize_t r = recvfrom(icmp_fd, gt.buf.data, gt.buf.size, 0,
                                  (struct sockaddr *)&ss, &sl);
-            if (r >= 8) {
-                struct ip_common ic;
-                if (!ip_get_common(&ic, gt.buf.data, r) && ic.proto == 1) {
-                    unsigned char *data = &gt.buf.data[ic.hdr_size];
-                    if (data[0] == 3) {
-                        int mtu = (data[6] << 8) | data[7];
-                        if (mtu) {
-                            gt_log("received MTU from ICMP: %i\n", mtu);
-                            mud_set_mtu(mud, GT_MTU(mtu));
-                        }
-                    }
+            struct ip_common ic;
+            if (!ip_get_common(&ic, gt.buf.data, r)) {
+                int mtu = ip_get_mtu(&ic, gt.buf.data, r);
+                if (mtu > 0) {
+                    gt_log("received MTU from ICMP: %i\n", mtu);
+                    mud_set_mtu(mud, GT_MTU(mtu));
                 }
             }
         }