From ae636fa84edb1430ba2f2c1243db4e5cf9a6e5f7 Mon Sep 17 00:00:00 2001
From: Alex Tsai <alex.tsai@protonmail.com>
Date: Tue, 20 Apr 2021 17:51:20 +0800
Subject: [PATCH] fix icmp

---
 server/server.go | 42 ++++++++++++++++++++++++++++++++----------
 1 file changed, 32 insertions(+), 10 deletions(-)

diff --git a/server/server.go b/server/server.go
index f57005f..9ab8093 100644
--- a/server/server.go
+++ b/server/server.go
@@ -45,7 +45,12 @@ func Start(config config.Config) {
 			continue
 		}
 		iface.Write(b)
-		key := srcAddr(b) + "-" + dstAddr(b)
+		srcAddr := srcAddr(b)
+		dstAddr := dstAddr(b)
+		if srcAddr == "" || dstAddr == "" {
+			continue
+		}
+		key := srcAddr + "-" + dstAddr
 		forwarder.connCache.Set(key, cliAddr, cache.DefaultExpiration)
 	}
 }
@@ -66,7 +71,12 @@ func (f *Forwarder) forward(iface *water.Interface, conn *net.UDPConn) {
 		if !waterutil.IsIPv4(b) {
 			continue
 		}
-		key := dstAddr(b) + "-" + srcAddr(b)
+		dstAddr := dstAddr(b)
+		srcAddr := srcAddr(b)
+		if dstAddr == "" || srcAddr == "" {
+			continue
+		}
+		key := dstAddr + "-" + srcAddr
 		v, ok := f.connCache.Get(key)
 		if ok {
 			// encrypt data
@@ -77,15 +87,27 @@ func (f *Forwarder) forward(iface *water.Interface, conn *net.UDPConn) {
 }
 
 func srcAddr(b []byte) string {
-	ip := waterutil.IPv4Source(b)
-	port := waterutil.IPv4SourcePort(b)
-	addr := fmt.Sprintf("%s:%d", ip.To4().String(), port)
-	return addr
+	if waterutil.IPv4Protocol(b) == waterutil.UDP || waterutil.IPv4Protocol(b) == waterutil.TCP {
+		ip := waterutil.IPv4Source(b)
+		port := waterutil.IPv4SourcePort(b)
+		addr := fmt.Sprintf("%s:%d", ip.To4().String(), port)
+		return addr
+	} else if waterutil.IPv4Protocol(b) == waterutil.ICMP {
+		ip := waterutil.IPv4Source(b)
+		return ip.To4().String()
+	}
+	return ""
 }
 
 func dstAddr(b []byte) string {
-	ip := waterutil.IPv4Destination(b)
-	port := waterutil.IPv4DestinationPort(b)
-	addr := fmt.Sprintf("%s:%d", ip.To4().String(), port)
-	return addr
+	if waterutil.IPv4Protocol(b) == waterutil.UDP || waterutil.IPv4Protocol(b) == waterutil.TCP {
+		ip := waterutil.IPv4Destination(b)
+		port := waterutil.IPv4DestinationPort(b)
+		addr := fmt.Sprintf("%s:%d", ip.To4().String(), port)
+		return addr
+	} else if waterutil.IPv4Protocol(b) == waterutil.ICMP {
+		ip := waterutil.IPv4Destination(b)
+		return ip.To4().String()
+	}
+	return ""
 }