Support QoS parameters in NWu and N3 and update go-gtp

go.mod

@@ -4,22 +4,19 @@ go 1.14
 
 require (
 	git.cs.nctu.edu.tw/calee/sctp v1.1.0
-	github.com/antonfisher/nested-logrus-formatter v1.3.0
-	github.com/cpuguy83/go-md2man/v2 v2.0.0 // indirect
-	github.com/free5gc/aper v1.0.1
+	github.com/antonfisher/nested-logrus-formatter v1.3.1
+	github.com/free5gc/aper v1.0.2
 	github.com/free5gc/idgenerator v1.0.0
 	github.com/free5gc/logger_conf v1.0.0
 	github.com/free5gc/logger_util v1.0.0
 	github.com/free5gc/ngap v1.0.2
 	github.com/free5gc/path_util v1.0.0
 	github.com/free5gc/version v1.0.0
-	github.com/russross/blackfriday/v2 v2.1.0 // indirect
-	github.com/sirupsen/logrus v1.7.0
+	github.com/sirupsen/logrus v1.8.1
 	github.com/urfave/cli v1.22.5
 	github.com/vishvananda/netlink v1.1.0
-	github.com/vishvananda/netns v0.0.0-20200728191858-db3c7e526aae // indirect
-	github.com/wmnsk/go-gtp v0.7.27
-	golang.org/x/net v0.0.0-20210501142056-aec3718b3fa0
-	golang.org/x/sys v0.0.0-20210603081109-ebe580a85c40
+	github.com/wmnsk/go-gtp v0.8.0
+	golang.org/x/net v0.0.0-20211008194852-3b03d305991f
+	golang.org/x/sys v0.0.0-20211007075335-d3039528d8ac
 	gopkg.in/yaml.v2 v2.4.0
 )

go.sum

@@ -43,6 +43,8 @@ github.com/alecthomas/units v0.0.0-20190924025748-f65c72e2690d/go.mod h1:rBZYJk5
 github.com/antihax/optional v1.0.0/go.mod h1:uupD/76wgC+ih3iEmQUL+0Ugr19nfwCT1kdvxnR2qWY=
 github.com/antonfisher/nested-logrus-formatter v1.3.0 h1:8zixYquU1Odk+vzAaAQPAdRh1ZjmUXNQ1T+dUBvlhVo=
 github.com/antonfisher/nested-logrus-formatter v1.3.0/go.mod h1:6WTfyWFkBc9+zyBaKIqRrg/KwMqBbodBjgbHjDz7zjA=
+github.com/antonfisher/nested-logrus-formatter v1.3.1 h1:NFJIr+pzwv5QLHTPyKz9UMEoHck02Q9L0FP13b/xSbQ=
+github.com/antonfisher/nested-logrus-formatter v1.3.1/go.mod h1:6WTfyWFkBc9+zyBaKIqRrg/KwMqBbodBjgbHjDz7zjA=
 github.com/beorn7/perks v0.0.0-20180321164747-3a771d992973/go.mod h1:Dwedo/Wpr24TaqPxmxbtue+5NUziq4I4S80YR8gNf3Q=
 github.com/beorn7/perks v1.0.0/go.mod h1:KWe93zE9D1o94FZ5RNwFwVgaQK1VOXiVxmqh+CedLV8=
 github.com/beorn7/perks v1.0.1/go.mod h1:G2ZrVWU2WbWT9wwq4/hrbKbnv/1ERSJQ0ibhJ6rlkpw=
@@ -53,9 +55,8 @@ github.com/chzyer/readline v0.0.0-20180603132655-2972be24d48e/go.mod h1:nSuG5e5P
 github.com/chzyer/test v0.0.0-20180213035817-a1ea475d72b1/go.mod h1:Q3SI9o4m/ZMnBNeIyt5eFwwo7qiLfzFZmjNmxjkiQlU=
 github.com/client9/misspell v0.3.4/go.mod h1:qj6jICC3Q7zFZvVWo7KLAzC3yx5G7kyvSDkc90ppPyw=
 github.com/cncf/udpa/go v0.0.0-20191209042840-269d4d468f6f/go.mod h1:M8M6+tZqaGXZJjfX53e64911xZQV5JYwmTeXPW+k8Sc=
+github.com/cpuguy83/go-md2man/v2 v2.0.0-20190314233015-f79a8a8ca69d h1:U+s90UTSYgptZMwQh2aRr3LuazLJIa+Pg3Kc1ylSYVY=
 github.com/cpuguy83/go-md2man/v2 v2.0.0-20190314233015-f79a8a8ca69d/go.mod h1:maD7wRr/U5Z6m/iR4s+kqSMx2CaBsrgA7czyZG/E6dU=
-github.com/cpuguy83/go-md2man/v2 v2.0.0 h1:EoUDS0afbrsXAZ9YQ9jdu/mZ2sXgT1/2yyNng4PGlyM=
-github.com/cpuguy83/go-md2man/v2 v2.0.0/go.mod h1:maD7wRr/U5Z6m/iR4s+kqSMx2CaBsrgA7czyZG/E6dU=
 github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
 github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c=
 github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38=
@@ -67,6 +68,8 @@ github.com/envoyproxy/go-control-plane v0.9.4/go.mod h1:6rpuAdCZL397s3pYoYcLgu1m
 github.com/envoyproxy/protoc-gen-validate v0.1.0/go.mod h1:iSmxcyjqTsJpI2R4NaDN7+kN2VEUnK/pcBlmesArF7c=
 github.com/free5gc/aper v1.0.1 h1:GUUoLYTKTXv9DIH/T5wwxZRBmOJ74ejwNpzkkmrW9BY=
 github.com/free5gc/aper v1.0.1/go.mod h1:1iGSt+WSNrLnsA0U340Nyd8aAqkPAW80Oko/L2CXK2U=
+github.com/free5gc/aper v1.0.2 h1:ZIcD7EV8YICXPS6gnsjGqyN6n6Rggce/V3OiVCFJFhI=
+github.com/free5gc/aper v1.0.2/go.mod h1:3K/m47BIPR2xhBkuHD1unp2LnArVtt3iTI4De0bCqpI=
 github.com/free5gc/http2_util v1.0.0/go.mod h1:GN2BCD8IINjtnAKYGwe+dEeTBRFEv4lQnZblFIIhbdE=
 github.com/free5gc/idgenerator v1.0.0 h1:e55Cn2p8dGCGYv5VoJ6oOsTj3SIVLToNLuS2CDOulZA=
 github.com/free5gc/idgenerator v1.0.0/go.mod h1:+iGY7VUgUX88lqPhGAPGy/b2SxtNJMvymdJbNyjk4HU=
@@ -227,15 +230,17 @@ github.com/prometheus/procfs v0.0.2/go.mod h1:TjEm7ze935MbeOT/UhFTIMYKhuLP4wbCsT
 github.com/prometheus/procfs v0.1.3/go.mod h1:lV6e/gmhEcM9IjHGsFOCxxuZ+z1YqCvr4OA4YeYWdaU=
 github.com/prometheus/procfs v0.6.0/go.mod h1:cz+aTbrPOrUb4q7XlbU9ygM+/jj0fzG6c1xBZuNvfVA=
 github.com/rogpeppe/go-internal v1.3.0/go.mod h1:M8bDsm7K2OlrFYOpmOWEs/qY81heoFRclV5y23lUDJ4=
+github.com/russross/blackfriday/v2 v2.0.1 h1:lPqVAte+HuHNfhJ/0LC98ESWRz8afy9tM/0RK8m9o+Q=
 github.com/russross/blackfriday/v2 v2.0.1/go.mod h1:+Rmxgy9KzJVeS9/2gXHxylqXiyQDYRxCVz55jmeOWTM=
-github.com/russross/blackfriday/v2 v2.1.0 h1:JIOH55/0cWyOuilr9/qlrm0BSXldqnqwMsf35Ld67mk=
-github.com/russross/blackfriday/v2 v2.1.0/go.mod h1:+Rmxgy9KzJVeS9/2gXHxylqXiyQDYRxCVz55jmeOWTM=
+github.com/shurcooL/sanitized_anchor_name v1.0.0 h1:PdmoCO6wvbs+7yrJyMORt4/BmY5IYyJwS/kOiWx8mHo=
 github.com/shurcooL/sanitized_anchor_name v1.0.0/go.mod h1:1NzhyTcUVG4SuEtjjoZeVRXNmyL/1OwPU0+IJeTBvfc=
 github.com/sirupsen/logrus v1.2.0/go.mod h1:LxeOpSwHxABJmUn/MG1IvRgCAasNZTLOkJPxbbu5VWo=
 github.com/sirupsen/logrus v1.4.2/go.mod h1:tLMulIdttU9McNUspp0xgXVQah82FyeX6MwdIuYE2rE=
 github.com/sirupsen/logrus v1.6.0/go.mod h1:7uNnSEd1DgxDLC74fIahvMZmmYsHGZGEOFrfsX/uA88=
 github.com/sirupsen/logrus v1.7.0 h1:ShrD1U9pZB12TX0cVy0DtePoCH97K8EtX+mg7ZARUtM=
 github.com/sirupsen/logrus v1.7.0/go.mod h1:yWOB1SBYBC5VeMP7gHvWumXLIWorT60ONWic61uBYv0=
+github.com/sirupsen/logrus v1.8.1 h1:dJKuHgqk1NNQlqoA6BTlM1Wf9DOH3NBjQyu0h9+AZZE=
+github.com/sirupsen/logrus v1.8.1/go.mod h1:yWOB1SBYBC5VeMP7gHvWumXLIWorT60ONWic61uBYv0=
 github.com/stretchr/objx v0.1.0/go.mod h1:HFkY916IF+rwdDfMAkV7OtwuqBVzrE8GR6GFx+wExME=
 github.com/stretchr/objx v0.1.1/go.mod h1:HFkY916IF+rwdDfMAkV7OtwuqBVzrE8GR6GFx+wExME=
 github.com/stretchr/testify v1.2.2/go.mod h1:a8OnRcib4nhh0OaRAV+Yts87kKdq0PP7pXfy6kDkUVs=
@@ -254,11 +259,10 @@ github.com/urfave/cli v1.22.5 h1:lNq9sAHXK2qfdI8W+GRItjCEkI+2oR4d+MEHy1CKXoU=
 github.com/urfave/cli v1.22.5/go.mod h1:Gos4lmkARVdJ6EkW0WaNv/tZAAMe9V7XWyB60NtXRu0=
 github.com/vishvananda/netlink v1.1.0 h1:1iyaYNBLmP6L0220aDnYQpo1QEV4t4hJ+xEEhhJH8j0=
 github.com/vishvananda/netlink v1.1.0/go.mod h1:cTgwzPIzzgDAYoQrMm0EdrjRUBkTqKYppBueQtXaqoE=
+github.com/vishvananda/netns v0.0.0-20191106174202-0a2b9b5464df h1:OviZH7qLw/7ZovXvuNyL3XQl8UFofeikI1NW1Gypu7k=
 github.com/vishvananda/netns v0.0.0-20191106174202-0a2b9b5464df/go.mod h1:JP3t17pCcGlemwknint6hfoeCVQrEMVwxRLRjXpq+BU=
-github.com/vishvananda/netns v0.0.0-20200728191858-db3c7e526aae h1:4hwBBUfQCFe3Cym0ZtKyq7L16eZUtYKs+BaHDN6mAns=
-github.com/vishvananda/netns v0.0.0-20200728191858-db3c7e526aae/go.mod h1:DD4vA1DwXk04H54A1oHXtwZmA0grkVMdPxx/VGLCah0=
-github.com/wmnsk/go-gtp v0.7.27 h1:2Cj6S/FnooK46Gr/kgrnHcc4R7VyYIiotVpqp41OUIE=
-github.com/wmnsk/go-gtp v0.7.27/go.mod h1:Y0reWDB701yW31+HeZcHfO6dLVRfn/f017vH+7syqrg=
+github.com/wmnsk/go-gtp v0.8.0 h1:KbvPh2nRGrB67w3k80YhIv6NkjKsZn20i0B5wCjhdDs=
+github.com/wmnsk/go-gtp v0.8.0/go.mod h1:Y0reWDB701yW31+HeZcHfO6dLVRfn/f017vH+7syqrg=
 github.com/yuin/goldmark v1.1.25/go.mod h1:3hX8gzYuyVAZsxl0MRgGTJEmQBFcNTphYh9decYSb74=
 github.com/yuin/goldmark v1.1.27/go.mod h1:3hX8gzYuyVAZsxl0MRgGTJEmQBFcNTphYh9decYSb74=
 github.com/yuin/goldmark v1.1.32/go.mod h1:3hX8gzYuyVAZsxl0MRgGTJEmQBFcNTphYh9decYSb74=
@@ -338,6 +342,8 @@ golang.org/x/net v0.0.0-20201209123823-ac852fbbde11 h1:lwlPPsmjDKK0J6eG6xDWd5XPe
 golang.org/x/net v0.0.0-20201209123823-ac852fbbde11/go.mod h1:m0MpNAwzfU5UDzcl9v0D8zg8gWTRqZa9RBIspLL5mdg=
 golang.org/x/net v0.0.0-20210501142056-aec3718b3fa0 h1:6QqBc2UURz4Sbr4IE15uXM8CTQlHnRdtKuogDhwnu2Y=
 golang.org/x/net v0.0.0-20210501142056-aec3718b3fa0/go.mod h1:OJAsFXCWl8Ukc7SiCT/9KSuxbyM7479/AVlXFRxuMCk=
+golang.org/x/net v0.0.0-20211008194852-3b03d305991f h1:1scJEYZBaF48BaG6tYbtxmLcXqwYGSfGcMoStTqkkIw=
+golang.org/x/net v0.0.0-20211008194852-3b03d305991f/go.mod h1:9nx3DQGgdP8bBQD5qxJ1jj9UTztislL4KSBs9R2vV5Y=
 golang.org/x/oauth2 v0.0.0-20180821212333-d2e6202438be/go.mod h1:N/0e6XlmueqKjAGxoOufVs8QHGRruUQn6yWY3a++T0U=
 golang.org/x/oauth2 v0.0.0-20190226205417-e64efc72b421/go.mod h1:gOpvHmFTYa4IltrdGE7lF6nIHvwfUNPOp7c8zoXwtLw=
 golang.org/x/oauth2 v0.0.0-20190604053449-0f29369cfe45/go.mod h1:gOpvHmFTYa4IltrdGE7lF6nIHvwfUNPOp7c8zoXwtLw=
@@ -376,7 +382,6 @@ golang.org/x/sys v0.0.0-20200116001909-b77594299b42/go.mod h1:h1NjWce9XRLGQEsW7w
 golang.org/x/sys v0.0.0-20200122134326-e047566fdf82/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
 golang.org/x/sys v0.0.0-20200202164722-d101bd2416d5/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
 golang.org/x/sys v0.0.0-20200212091648-12a6c2dcc1e4/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
-golang.org/x/sys v0.0.0-20200217220822-9197077df867/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
 golang.org/x/sys v0.0.0-20200223170610-d5e6a3e2c0ae/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
 golang.org/x/sys v0.0.0-20200302150141-5c8b2ff67527/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
 golang.org/x/sys v0.0.0-20200323222414-85ca7c5b95cd/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
@@ -395,6 +400,8 @@ golang.org/x/sys v0.0.0-20210124154548-22da62e12c0c/go.mod h1:h1NjWce9XRLGQEsW7w
 golang.org/x/sys v0.0.0-20210423082822-04245dca01da/go.mod h1:h1NjWce9XRLGQEsW7wpKNCjG9DtNlClVuFLEZdDNbEs=
 golang.org/x/sys v0.0.0-20210603081109-ebe580a85c40 h1:JWgyZ1qgdTaF3N3oxC+MdTV7qvEEgHo3otj+HB5CM7Q=
 golang.org/x/sys v0.0.0-20210603081109-ebe580a85c40/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg=
+golang.org/x/sys v0.0.0-20211007075335-d3039528d8ac h1:oN6lz7iLW/YC7un8pq+9bOLyXrprv2+DKfkJY+2LJJw=
+golang.org/x/sys v0.0.0-20211007075335-d3039528d8ac/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg=
 golang.org/x/term v0.0.0-20201117132131-f5c789dd3221/go.mod h1:Nr5EML6q2oocZ2LXRh80K7BxOlk5/8JxuGnuhpl+muw=
 golang.org/x/term v0.0.0-20201126162022-7de9c90e9dd1/go.mod h1:bj7SfCRtBDWHUb9snDiAeCFNEtKQo2Wmx5Cou7ajbmo=
 golang.org/x/text v0.0.0-20170915032832-14c0d48ead0c/go.mod h1:NqM8EUOU14njkJ3fqMW+pc6Ldnwhi/IjpwHt7yyuwOQ=
@@ -403,6 +410,7 @@ golang.org/x/text v0.3.1-0.20180807135948-17ff2d5776d2/go.mod h1:NqM8EUOU14njkJ3
 golang.org/x/text v0.3.2/go.mod h1:bEr9sfX3Q8Zfm5fL9x+3itogRgK3+ptLWKqgva+5dAk=
 golang.org/x/text v0.3.3/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ=
 golang.org/x/text v0.3.4/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ=
+golang.org/x/text v0.3.6 h1:aRYxNxv6iGQlyVaZmk6ZgYEDa+Jg18DxebPSrd6bg1M=
 golang.org/x/text v0.3.6/go.mod h1:5Zoc/QRtKVWzQhOtBMvqHzDpF6irO9z98xDceosuGiQ=
 golang.org/x/time v0.0.0-20181108054448-85acf8d2951c/go.mod h1:tRJNPiyCQ0inRvYxbN9jk5I+vvW/OXSQhTDSoE431IQ=
 golang.org/x/time v0.0.0-20190308202827-9d24e82272b4/go.mod h1:tRJNPiyCQ0inRvYxbN9jk5I+vvW/OXSQhTDSoE431IQ=

gre/message.go

@@ -0,0 +1,96 @@
+package gre
+
+import "encoding/binary"
+
+// [TS 24.502] 9.3.3 GRE encapsulated user data packet
+const (
+	GREHeaderFieldLength    = 8
+	GREHeaderKeyFieldLength = 4
+)
+
+// Ethertypes Specified by the IETF
+const (
+	IPv4 uint16 = 0x0800
+	IPv6 uint16 = 0x86DD
+)
+
+type GREPacket struct {
+	flags        uint8
+	version      uint8
+	protocolType uint16
+	key          uint32
+	payload      []byte
+}
+
+func (p *GREPacket) Marshal() []byte {
+	packet := make([]byte, GREHeaderFieldLength+len(p.payload))
+
+	packet[0] = p.flags
+	packet[1] = p.version
+	binary.BigEndian.PutUint16(packet[2:4], p.protocolType)
+	binary.BigEndian.PutUint32(packet[4:8], p.key)
+	copy(packet[GREHeaderFieldLength:], p.payload)
+	return packet
+}
+
+func (p *GREPacket) Unmarshal(b []byte) error {
+	p.flags = b[0]
+	p.version = b[1]
+
+	p.protocolType = binary.BigEndian.Uint16(b[2:4])
+
+	offset := 4
+
+	if p.GetKeyFlag() {
+		p.key = binary.BigEndian.Uint32(b[offset : offset+GREHeaderKeyFieldLength])
+		offset += GREHeaderKeyFieldLength
+	}
+
+	p.payload = append(p.payload, b[offset:]...)
+	return nil
+}
+
+func (p *GREPacket) SetPayload(payload []byte, protocolType uint16) {
+	p.payload = payload
+	p.protocolType = protocolType
+}
+
+func (p *GREPacket) GetPayload() ([]byte, uint16) {
+	return p.payload, p.protocolType
+}
+
+func (p *GREPacket) setKeyFlag() {
+	p.flags |= 0x20
+}
+
+func (p *GREPacket) GetKeyFlag() bool {
+	return (p.flags & 0x20) > 0
+}
+
+func (p *GREPacket) setQFI(qfi uint8) {
+	p.key |= (uint32(qfi) & 0x3F) << 24
+}
+
+func (p *GREPacket) setRQI(rqi bool) {
+	if rqi {
+		p.key |= 0x80
+	}
+}
+
+func (p *GREPacket) GetQFI() uint8 {
+	return uint8((p.key >> 24) & 0x3F)
+}
+
+func (p *GREPacket) GetRQI() bool {
+	return (p.key & 0x80) > 0
+}
+
+func (p *GREPacket) GetKeyField() uint32 {
+	return p.key
+}
+
+func (p *GREPacket) SetQoS(qfi uint8, rqi bool) {
+	p.setQFI(qfi)
+	p.setRQI(rqi)
+	p.setKeyFlag()
+}

gtp/handler/handler.go

@@ -0,0 +1,86 @@
+package handler
+
+import (
+	"net"
+	"runtime/debug"
+
+	"github.com/sirupsen/logrus"
+	gtp "github.com/wmnsk/go-gtp/gtpv1"
+	gtpMsg "github.com/wmnsk/go-gtp/gtpv1/message"
+
+	n3iwfContext "github.com/free5gc/n3iwf/context"
+	"github.com/free5gc/n3iwf/gre"
+	gtpQoSMsg "github.com/free5gc/n3iwf/gtp/message"
+	"github.com/free5gc/n3iwf/logger"
+)
+
+var gtpLog *logrus.Entry
+
+func init() {
+	gtpLog = logger.GTPLog
+}
+
+// Parse the fields not supported by go-gtp and forward data to UE.
+func HandleQoSTPDU(c gtp.Conn, senderAddr net.Addr, msg gtpMsg.Message) error {
+	pdu := gtpQoSMsg.QoSTPDUPacket{}
+	if err := pdu.Unmarshal(msg.(*gtpMsg.TPDU)); err != nil {
+		return err
+	}
+
+	forward(pdu)
+	return nil
+}
+
+// Forward user plane packets from N3 to UE with GRE header and new IP header encapsulated
+func forward(packet gtpQoSMsg.QoSTPDUPacket) {
+	defer func() {
+		if p := recover(); p != nil {
+			// Print stack for panic to log. Fatalf() will let program exit.
+			gtpLog.Fatalf("panic: %v\n%s", p, string(debug.Stack()))
+		}
+	}()
+
+	// N3IWF context
+	self := n3iwfContext.N3IWFSelf()
+
+	// Nwu connection in IPv4
+	NWuConn := self.NWuIPv4PacketConn
+
+	// Find UE information
+	ue, ok := self.AllocatedUETEIDLoad(packet.GetTEID())
+	if !ok {
+		gtpLog.Error("UE context not found")
+		return
+	}
+
+	// UE inner IP in IPSec
+	ueInnerIPAddr := ue.IPSecInnerIPAddr
+
+	var (
+		qfi uint8
+		rqi bool
+	)
+
+	// QoS Related Parameter
+	if packet.HasQoS() {
+		qfi, rqi = packet.GetQoSParameters()
+		gtpLog.Tracef("QFI: %v, RQI: %v", qfi, rqi)
+	}
+
+	// Encasulate IPv4 packet with GRE header before forward to UE through IPsec
+	grePacket := gre.GREPacket{}
+
+	// TODO:[24.502(v15.7) 9.3.3 ] The Protocol Type field should be set to zero
+	grePacket.SetPayload(packet.GetPayload(), gre.IPv4)
+	grePacket.SetQoS(qfi, rqi)
+	forwardData := grePacket.Marshal()
+
+	// Send to UE through Nwu
+	if n, err := NWuConn.WriteTo(forwardData, nil, ueInnerIPAddr); err != nil {
+		gtpLog.Errorf("Write to UE failed: %+v", err)
+		return
+	} else {
+		gtpLog.Trace("Forward NWu <- N3")
+		gtpLog.Tracef("Wrote %d bytes", n)
+	}
+}

gtp/message/message.go

@@ -0,0 +1,79 @@
+package message
+
+import (
+	"encoding/hex"
+	"errors"
+
+	gtpMessage "github.com/wmnsk/go-gtp/gtpv1/message"
+
+	"github.com/free5gc/n3iwf/logger"
+)
+
+// [TS 38.415] 5.5.2 Frame format for the PDU Session user plane protocol
+const (
+	DL_PDU_SESSION_INFORMATION_TYPE = 0x00
+	UL_PDU_SESSION_INFORMATION_TYPE = 0x10
+)
+
+type QoSTPDUPacket struct {
+	tPDU *gtpMessage.TPDU
+	qos  bool
+	rqi  bool
+	qfi  uint8
+}
+
+func (p *QoSTPDUPacket) GetPayload() []byte {
+	return p.tPDU.Payload
+}
+
+func (p *QoSTPDUPacket) GetTEID() uint32 {
+	return p.tPDU.TEID()
+}
+
+func (p *QoSTPDUPacket) GetExtensionHeader() []*gtpMessage.ExtensionHeader {
+	return p.tPDU.ExtensionHeaders
+}
+
+func (p *QoSTPDUPacket) HasQoS() bool {
+	return p.qos
+}
+
+func (p *QoSTPDUPacket) GetQoSParameters() (uint8, bool) {
+	return p.qfi, p.rqi
+}
+
+func (p *QoSTPDUPacket) Unmarshal(pdu *gtpMessage.TPDU) error {
+	p.tPDU = pdu
+	if p.tPDU.HasExtensionHeader() {
+		if err := p.unmarshalExtensionHeader(); err != nil {
+			return err
+		}
+	}
+
+	return nil
+}
+
+// [TS 29.281] [TS 38.415]
+// Define GTP extension header
+// [TS 38.415]
+// Define PDU Session User Plane protocol
+func (p *QoSTPDUPacket) unmarshalExtensionHeader() error {
+	for _, eh := range p.tPDU.ExtensionHeaders {
+		switch eh.Type {
+		case gtpMessage.ExtHeaderTypePDUSessionContainer:
+			p.qos = true
+			p.rqi = ((int(eh.Content[1]) >> 6) & 0x1) == 1
+			p.qfi = eh.Content[1] & 0x3F
+			logger.GTPLog.Tracef("Parsed Extension Header: Len=%d, Next Type=%d, Content Dump:\n%s",
+				eh.Length, eh.NextType, hex.Dump(eh.Content))
+		default:
+			logger.GTPLog.Warningf("Unsupported Extension Header Field Value: %x", eh.Type)
+		}
+	}
+
+	if !p.qos {
+		return errors.New("unmarshalExtensionHeader err: no PDUSessionContainer in ExtensionHeaders.")
+	}
+
+	return nil
+}