ladder charts

Related: SYS#5599
Change-Id: Ie8c1b1fa50cd4f1569f0fcb02d42c713e6e49cad

Makefile.am

@@ -16,6 +16,12 @@ SUBDIRS = \
 	contrib \
 	$(NULL)
 
+pkgconfigdir = $(libdir)/pkgconfig
+pkgconfig_DATA = \
+	libosmo-gtlv.pc \
+	libosmo-pfcp.pc \
+	$(NULL)
+
 BUILT_SOURCES = $(top_srcdir)/.version
 EXTRA_DIST = \
 	     .version \

configure.ac

@@ -42,6 +42,9 @@ AC_SUBST(LIBRARY_DL)
 PKG_CHECK_MODULES(LIBOSMOCORE, libosmocore >= 1.5.0)
 PKG_CHECK_MODULES(LIBOSMOVTY, libosmovty >= 1.5.0)
 PKG_CHECK_MODULES(LIBOSMOCTRL, libosmoctrl >= 1.5.0)
+PKG_CHECK_MODULES(LIBGTPNL, libgtpnl >= 1.2.0)
+PKG_CHECK_MODULES(LIBNFTNL, libnftnl >= 1.2.1)
+PKG_CHECK_MODULES(LIBNFTABLES, libnftables >= 1.0.2)
 
 dnl checks for header files
 AC_HEADER_STDC
@@ -195,6 +198,8 @@ dnl Generate the output
 AM_CONFIG_HEADER(config.h)
 
 AC_OUTPUT(
+    libosmo-gtlv.pc
+    libosmo-pfcp.pc
     include/Makefile
     include/osmocom/Makefile
     include/osmocom/gtlv/Makefile
@@ -204,14 +209,17 @@ AC_OUTPUT(
     src/libosmo-gtlv/Makefile
     src/libosmo-pfcp/Makefile
     src/osmo-upf/Makefile
+    src/osmo-pfcp-tool/Makefile
     tests/Makefile
     tests/atlocal
     tests/libosmo-gtlv/Makefile
     tests/libosmo-gtlv/test_gtlv_gen/Makefile
     tests/libosmo-gtlv/test_tliv/Makefile
+    tests/libosmo-pfcp/Makefile
     doc/Makefile
     doc/examples/Makefile
     doc/manuals/Makefile
+    doc/charts/Makefile
     contrib/Makefile
     contrib/systemd/Makefile
     Makefile)

contrib/osmo-pfcp-tool-scripts/assoc_setup.vty

@@ -0,0 +1,3 @@
+pfcp-peer 127.0.0.1
+ tx assoc-setup-req
+ sleep 1

contrib/osmo-pfcp-tool-scripts/assoc_setup_retrans.vty

@@ -0,0 +1,8 @@
+pfcp-peer 127.0.0.1
+ tx assoc-setup-req
+ sleep 3
+ retrans req
+ sleep 5
+ retrans req
+ sleep 1
+ retrans req

contrib/osmo-pfcp-tool-scripts/encaps_plus_tunmap.vty

@@ -0,0 +1,30 @@
+# ACCESS                HOP                           CORE
+#                       session 23 = tunmap           session 42 = encaps/decaps
+# GTP 127.0.0.13        127.0.0.12                    127.0.0.11
+# TEID l:23 r:123 <---> r:23 l:123 | l:142 r:42 <---> r:142 l:42 | 192.168.100.42
+#
+# Run two UPF, one listening on / sending from 127.0.0.11, the other on 127.0.0.12.
+# (Each has to match on the sender address of incoming GTP packets.)
+
+timer pfcp x23 0
+
+pfcp-peer 127.0.0.11
+ tx assoc-setup-req
+ sleep 1
+ session endecaps 42
+  ue ip 192.168.100.42
+  gtp access ip 127.0.0.12
+  gtp access teid local 42 remote 142
+  tx session-est-req
+ sleep 1
+
+pfcp-peer 127.0.0.12
+ tx assoc-setup-req
+ sleep 1
+ session tunmap 23
+  gtp core ip 127.0.0.11
+  gtp core teid local 142 remote 42
+  gtp access ip 127.0.0.13
+  gtp access teid local 123 remote 23
+  tx session-est-req
+ sleep 1

contrib/osmo-pfcp-tool-scripts/endecaps_session_est.vty

@@ -0,0 +1,8 @@
+timer pfcp x23 0
+pfcp-peer 127.0.0.1
+ tx assoc-setup-req
+ sleep 1
+ session endecaps
+  tx session-est-req forw
+  sleep 5
+  tx session-del-req

contrib/osmo-pfcp-tool-scripts/heartbeat.vty

@@ -0,0 +1,10 @@
+pfcp-peer 127.0.0.1
+ tx heartbeat
+ sleep 2
+ tx heartbeat
+ sleep 2
+ tx heartbeat
+ sleep 2
+ tx heartbeat
+ sleep 2
+ tx heartbeat

contrib/osmo-pfcp-tool-scripts/osmo-pfcp-tool.cfg

@@ -0,0 +1,5 @@
+log stderr
+ logging level set-all info
+
+local-addr 127.0.0.2
+listen

contrib/osmo-pfcp-tool-scripts/osmo-upf-11.cfg

@@ -0,0 +1,27 @@
+log stderr
+ logging filter all 1
+ logging color 1
+ logging print level 1
+ logging print category 1
+ logging print category-hex 0
+ logging print file basename last
+ logging print extended-timestamp 1
+ logging level set-all notice
+ logging level set-all info
+ logging level session debug
+ logging level nft debug
+ logging level gtp debug
+#logging level set-all debug
+
+line vty
+ bind 127.0.0.11
+ctrl
+ bind 127.0.0.11
+
+timer pfcp x24 5000
+pfcp
+ local-addr 127.0.0.11
+gtp
+ dev create apn11 127.0.0.11
+nft
+ table-name osmo-upf-11

contrib/osmo-pfcp-tool-scripts/osmo-upf-12.cfg

@@ -0,0 +1,27 @@
+log stderr
+ logging filter all 1
+ logging color 1
+ logging print level 1
+ logging print category 1
+ logging print category-hex 0
+ logging print file basename last
+ logging print extended-timestamp 1
+ logging level set-all notice
+ logging level set-all info
+ logging level session debug
+ logging level nft debug
+ logging level gtp debug
+#logging level set-all debug
+
+line vty
+ bind 127.0.0.12
+ctrl
+ bind 127.0.0.12
+
+timer pfcp x24 5000
+pfcp
+ local-addr 127.0.0.12
+gtp
+ dev create apn12 127.0.0.12
+nft
+ table-name osmo-upf-12

contrib/osmo-pfcp-tool-scripts/session_est_without_assoc.vty

@@ -0,0 +1,4 @@
+timer pfcp x23 0
+pfcp-peer 127.0.0.1
+ session endecaps
+  tx session-est-req

contrib/osmo-pfcp-tool-scripts/session_mod.vty

@@ -0,0 +1,14 @@
+timer pfcp x23 0
+pfcp-peer 127.0.0.1
+ tx assoc-setup-req
+ sleep 1
+ session
+  tx session-est-req drop
+  sleep 3
+  tx session-mod-req forw
+  sleep 5
+  tx session-mod-req drop
+  sleep 3
+  tx session-mod-req forw
+  sleep 3
+  tx session-del-req

contrib/osmo-pfcp-tool-scripts/tunmap_session_est.vty

@@ -0,0 +1,8 @@
+timer pfcp x23 0
+pfcp-peer 127.0.0.1
+ tx assoc-setup-req
+ sleep 1
+ session tunmap
+  tx session-est-req
+  sleep 5
+  tx session-del-req

contrib/osmo-upf.spec.in

@@ -32,6 +32,7 @@ BuildRequires:  pkgconfig >= 0.20
 %if 0%{?suse_version}
 BuildRequires:  systemd-rpm-macros
 %endif
+BuildRequires:  pkgconfig(libgtpnl) >= 1.2.0
 BuildRequires:  pkgconfig(libosmocore) >= 1.6.0
 BuildRequires:  pkgconfig(libosmoctrl) >= 1.6.0
 BuildRequires:  pkgconfig(libosmovty) >= 1.6.0

contrib/set_cap_net_admin.sh

@@ -0,0 +1,4 @@
+#!/bin/sh
+set -x
+bin="${1:-$(which osmo-upf)}"
+setcap cap_net_admin+pe "$bin"

debian/control

@@ -12,6 +12,7 @@ Build-Depends: debhelper (>=9),
                pkg-config,
                python3-minimal,
                libtalloc-dev,
+               libgtpnl-dev (>= 1.2.0),
                libosmocore-dev (>= 1.6.0),
                osmo-gsm-manuals-dev (>= 1.2.0)
 Standards-Version: 3.9.8

doc/Makefile.am

@@ -1,4 +1,5 @@
 SUBDIRS = \
 	examples \
 	manuals \
+	charts \
 	$(NULL)

doc/charts/Makefile.am

@@ -0,0 +1,28 @@
+msc: \
+	$(builddir)/pfcp_msgs.png \
+	$(builddir)/pfcp_msgs_gtp.png \
+	$(NULL)
+
+dot: \
+	$(builddir)/pfcp_overview.png \
+	$(builddir)/pfcp_cp_peer_fsm.png \
+	$(builddir)/pfcp_up_peer_fsm.png \
+	$(builddir)/pfcp_heartbeat_fsm.png \
+	$(builddir)/pfcp_cp_session_fsm.png \
+	$(builddir)/pfcp_up_session_fsm.png \
+	$(builddir)/pfcp_and_gtp.png \
+	$(NULL)
+
+$(builddir)/%.png: $(srcdir)/%.msc
+	mscgen -T png -o $@ $<
+
+$(builddir)/%.png: $(srcdir)/%.dot
+	dot -Tpng $< > $@
+
+$(srcdir)/%.msc: $(srcdir)/%.ladder
+	@which ladder_to_msc.py || (echo 'PLEASE POINT YOUR $$PATH AT libosmocore/contrib/ladder_to_msc.py' && false)
+	ladder_to_msc.py -i $< -o $@
+
+.PHONY: poll
+poll:
+	while true; do $(MAKE) msc dot; sleep 1; done

doc/charts/pfcp_and_gtp.dot

@@ -0,0 +1,20 @@
+digraph G {
+rankdir=LR
+labelloc=t; label="PFCP and GTP"
+
+SGSN [label="SGSN\n123.44.0.9"]
+SGWC [label="SGW-C\n123.44.05"]
+subgraph cluster_UPF {
+ label="OsmoUPF";
+ SGWU [label="SGW-U\n123.44.0.6"];
+ GTPk [label="kernel GTP\n123.44.0.6"]
+}
+
+SGSN -> SGWC [label="S4\nGTPv2-C"]
+SGWC -> SGWU [label="Sxa\nPFCP\nSession Establishment:\n"]
+SGSN -> GTPk [label="S4\nGTPv1-U",dir=both]
+
+MS [label="MS\n192.168.104.176"]
+MS -> SGSN [dir=both]
+
+}

doc/charts/pfcp_cp_peer_fsm.dot

@@ -0,0 +1,39 @@
+digraph G {
+rankdir=TB
+labelloc=t; label="PFCP CP peer FSM\nControl Plane side, managing association with remote UP peer"
+
+cp [label="CP function",shape="box"]
+
+cp -> WAIT_ASSOC_SETUP_RESP [label="cp_peer_associate()"]
+
+txrx [label="PFCP socket",shape="box"]
+WAIT_ASSOC_SETUP_RESP -> txrx [label="tx_assoc_setup_req()",style=dotted]
+txrx -> WAIT_ASSOC_SETUP_RESP [label="EV_RX_ASSOC_SETUP_RESP",style=dotted]
+WAIT_ASSOC_SETUP_RESP -> ASSOCIATED [label="Assoc Setup Resp"]
+
+WAIT_ASSOC_SETUP_RESP -> WAIT_ASSOC_SETUP_RESP [label="retry"]
+
+heartbeat [label="PFCP heartbeat FSM",shape=box3d]
+ASSOCIATED -> heartbeat [label="alloc()",style=dotted]
+heartbeat -> ASSOCIATED [label="EV_HEARTBEAT_FAILURE",style=dotted]
+
+txrx2 [label="PFCP socket",shape="box"]
+txrx2 -> ASSOCIATED [label="EV_RX_ASSOC_UPDATE_REQ\n3GPP TS 29.244 6.2.7.3.1",style=dotted]
+GRACEFUL_RELEASE -> txrx2 [label="tx_assoc_update_resp()",style=dotted]
+
+cp_session [label="PFCP CP session FSM",shape=box3d]
+cp -> ASSOCIATED [label="cp_peer_session_create()",style=dotted]
+ASSOCIATED -> cp_session [label="cp_session_create()",style=dotted]
+cp -> cp_session [style=invisible,arrowhead=none]
+
+ASSOCIATED -> GRACEFUL_RELEASE [label="Association Update\nindicating graceful release"]
+
+cp -> ASSOCIATED [label="cp_peer_release()",style=dotted]
+ASSOCIATED -> term [label="cp_peer_release()\nHeartbeat failure"]
+
+ASSOCIATED -> WAIT_ASSOC_SETUP_RESP [label="Heartbeat failure"]
+
+GRACEFUL_RELEASE -> term
+term [shape="octagon"]
+
+}

doc/charts/pfcp_cp_session_fsm.dot

@@ -0,0 +1,28 @@
+digraph G {
+rankdir=TB
+labelloc=t; label="PFCP CP session FSM"
+
+cp [label="CP function",shape=box]
+cp -> WAIT_ESTABLISHMENT_RESP [label="cp_session_create(cp_peer)\niff cp_peer in state ASSOCIATED"]
+
+txrx [label="PFCP socket",shape=box]
+
+WAIT_ESTABLISHMENT_RESP -> txrx [label="tx_session_est_req()",style=dotted]
+txrx -> WAIT_ESTABLISHMENT_RESP [label="EV_RX_SESSION_EST_RESP",style=dotted]
+
+WAIT_ESTABLISHMENT_RESP -> ESTABLISHED [label="Est Resp"]
+
+cp -> ESTABLISHED [label="cp_session_modify()",style=dotted]
+ESTABLISHED -> WAIT_MODIFICATION_RESP [label="cp_session_modify()"]
+WAIT_MODIFICATION_RESP -> txrx [label="tx_session_mod_req()",style=dotted]
+txrx -> WAIT_MODIFICATION_RESP [label="EV_RX_SESSION_MOD_RESP",style=dotted,constraint=false]
+WAIT_MODIFICATION_RESP -> ESTABLISHED [label="Mod Resp"]
+
+cp -> ESTABLISHED [label="cp_session_delete()",style=dotted]
+ESTABLISHED -> WAIT_DELETION_RESP [label="cp_session_delete()"]
+WAIT_DELETION_RESP -> txrx [label="tx_session_del_req()",style=dotted]
+txrx -> WAIT_DELETION_RESP [label="EV_RX_SESSION_DEL_RESP",style=dotted,constraint=false]
+WAIT_DELETION_RESP -> term
+term [shape="octagon"]
+
+}

doc/charts/pfcp_heartbeat_fsm.dot

@@ -0,0 +1,21 @@
+digraph G {
+rankdir=TB
+labelloc=t; label="PFCP heartbeat FSM"
+
+peer [label="PFCP CP/UP peer FSM",shape=box3d]
+txrx [label="PFCP socket",shape=box]
+
+peer -> IDLE [label="alloc()"]
+IDLE -> WAIT_HEARTBEAT_RESP -> IDLE
+WAIT_HEARTBEAT_RESP -> term
+term [shape="octagon"]
+
+WAIT_HEARTBEAT_RESP -> txrx [label="tx_heartbeat_req()",style=dotted]
+txrx -> WAIT_HEARTBEAT_RESP [label="HEARTBEAT_EV_RX_RESP",style=dotted]
+
+term -> peer [label="PEER_EV_HEARTBEAT_FAILURE",style=dotted]
+
+txrx2 [label="PFCP socket",shape=box]
+txrx2 -> txrx2 [label="rx Heartbeat Req\ntx Heartbeat Resp",style=dotted]
+
+}

doc/charts/pfcp_msgs.ladder

@@ -0,0 +1,73 @@
+{hscale=1}
+upf = User Plane function
+cpf = Control Plane function
+
+cpf () .	Look up UPF,
+		pick any one of the available
+		IP addrs for the UPF
+
+...
+upf <> cpf	not yet associated
+upf () cpf	reject any session related msgs
+...
+
+upf < cpf	PFCP Association Setup Request
+		CP function Node Id, features
+upf > cpf	PFCP Association Setup Response
+		UP function Node Id, features
+upf <> cpf	associated
+upf () cpf	start Heartbeat checking
+...
+upf < cpf	Heartbeat Request
+upf > cpf	Heartbeat Response
+...
+upf > cpf	Heartbeat Request
+upf < cpf	Heartbeat Response
+...
+
+upf < cpf	Session Establishment Request
+		CP Node-Id
+		CP F-SEID
+		1+ Packet Detection Rule(s)
+		1+ Forward Action Rule(s)
+upf > cpf	Session Establishment Response
+
+upf < cpf	Session Modification Request
+upf > cpf	Session Modification Response
+
+upf < cpf	Session Deletion Request
+upf > cpf	Session Deletion Response
+
+...
+upf () cpf	F-SEID: accept any other IP addrs than peer's Node Id
+...
+
+---		Graceful release initiated by CP
+
+upf < cpf	Association Update Request
+		with PFCP Association Release Preparation Start = 1
+upf > cpf	Association Update Response
+upf > cpf	Session Report Request
+		to report non-zero usage reports,
+		at least one message per PFCP Session
+upf < cpf	Association Release Request
+upf > cpf	Association Release Response
+
+---		Graceful release initiated by UP
+
+upf > cpf	Association Update Request
+		with PFCP Association Release Preparation = 1
+cpf <> .	refrain from establishing sessions
+upf < cpf	Association Update Response
+upf < cpf	Session Deletion Request(s)
+		to collect usage reports
+		per session
+upf > cpf	Session Deletion Response(s)
+cpf () .	wait Graceful Release Period
+upf < cpf	Association Release Request
+upf > cpf	Association Release Response
+
+---		Release (immediate)
+
+upf < cpf	Association Release Request
+upf > cpf	Association Release Response

doc/charts/pfcp_msgs_gtp.ladder

@@ -0,0 +1,142 @@
+{hscale=1}
+sgsn =	SGSN
+	123.44.0.9
+sgwc =	SGW-C
+	123.44.0.5
+sgwu =	SGW-U
+	123.44.0.6
+pgwc =	PGW-C
+	123.44.0.7
+pgwu =	PGW-U
+	123.44.0.8
+
+sgsn <-> sgwc	S4-C GTPv2-C
+sgwc <-> sgwu	Sxa PFCP
+sgsn <-> sgwu	S4-U GTPv1-U
+sgwc <-> pgwc	S5-C GTPv2-C
+pgwc <-> pgwu	Sxb PFCP
+sgwu <-> pgwu	S5-U GTPv1-U
+
+...
+
+sgsn -> sgwc	GTP Create Session Request
+sgsn [] sgwc	F-TEID S11 = 123.44.0.9,0x004
+		F-TEID S5 = 123.44.0.7,0x000
+		PDN addr alloc = IPv4 192.168.100.2
+
+|||
+|||
+
+sgwc -> sgwu	PFCP Session Establishment Request
+sgwc [] sgwu	2x Create PDR
+		F-TEID = CHOOSE
+		FAR = NOCP,BUFF
+
+|||
+|||
+
+sgwc <- sgwu	PFCP Session Establishment Response
+sgwc [] sgwu	Created PDR F-TEID 123.44.0.6,0x015
+		Created PDR F-TEID 123.44.0.6,0x016
+
+|||
+|||
+
+sgwc -> pgwc	GTP Create Session Request
+sgwc [] pgwc	F-TEID S5 = 123.44.0.5,0x00b
+		PDN addr alloc = IPv4 192.168.100.2
+		Bearer Ctx: F-TEID S5 = 123.44.0.6,0x015
+
+|||
+|||
+
+pgwc -> pgwu	PFCP Session Establishment Request
+pgwc [] pgwu	Create PDR 1:
+		PDI: src-iface Core, UE IPv4 192.168.100.2
+		FAR-1: FORW, dst-iface Access,
+		hdr creation: GTP-U 123.44.0.6,0x015
+
+		Create PDR 2:
+		PDI: src-iface Access, F-TEID = CHOOSE id:05
+		hdr removal: GTP-U
+		FAR-2: FORW, dst-iface Core
+
+		Create PDR 3:
+		PDI: src-iface CP-function, F-TEID = CHOOSE
+		hdr removal: GTP-U
+		FAR-1
+
+		Create PDR 4:
+		PDI: src-iface Access, F-TEID = CHOOSE id:05, SDF Filter
+		hdr removal: GTP-U
+		FAR-3: FORW, dst-iface CP-Function,
+		hdr creation: GTP-U 123.44.0.7,0x00b
+
+|||
+
+pgwc <- pgwu	PFCP Session Establishment Response
+pgwc [] pgwu	Created PDR-1
+		Created PDR-2: F-TEID = 123.44.0.8,0x01e
+		Created PDR-3: F-TEID = 123.44.0.8,0x01f
+		Created PDR-4: F-TEID = 123.44.0.8,0x01e
+
+|||
+|||
+
+sgwc <- pgwc	GTP Create Session Response
+sgwc [] pgwc	TEID: 0x00b
+		F-TEID: 123.44.0.7,0x00b
+		PDN Addr: 192.168.100.2
+		Bearer Ctx: F-TEID S5 123.44.0.8,0x01e
+
+|||
+|||
+
+sgwc -> sgwu	PFCP Session Modification Request
+sgwc [] sgwu	Update FAR-2: FORW, dst-iface Core,
+		hdr creation GTP-U 123.44.0.8,0x01e
+
+|||
+
+sgwc <- sgwu	PFCP Session Modification Response
+
+|||
+|||
+
+sgsn <- sgwc	GTP Create Session Response
+sgsn [] sgwc	TEID: 0x004
+		F-TEID S11/S4: 123.44.0.5,0x007
+		F-TEID S5/S8: 123.44.0.7,0x00b
+		PDN Addr: 192.168.100.2
+		Bearer Ctx:
+		F-TEID S1-U: 123.44.0.6,0x016
+		F-TEID S5/S8: 123.44.0.8,0x01e
+
+|||
+|||
+
+sgsn -> sgwc	GTP Modify Bearer Request
+sgsn [] sgwc	TEID: 0x007
+		Bearer Ctx:
+		F-TEID S1-U: 192.168.104.167,0x32adb2ad
+
+|||
+|||
+
+sgwc -> sgwu	PFCP Session Modification Request
+sgwc [] sgwu	Update FAR-1: FORW, dst-iface Access,
+		hdr creation: GTP-U 192.168.104.167,0x32adb2ad
+
+|||
+|||
+
+
+sgwc <- sgwu	PFCP Session Modification Response
+
+|||
+
+sgsn <- sgwc	GTP Modify Bearer Response
+sgsn [] sgwc	TEID: 0x004
+		Bearer Ctx:
+		F-TEID S1-U: 192.168.104.167,0x32adb2ad
+

doc/charts/pfcp_overview.dot

@@ -0,0 +1,23 @@
+digraph G {
+rankdir=TB
+labelloc=t; label="PFCP Overview\n3GPP TS 29.244 3.1, 5.8.1"
+
+subgraph cluster_N1_CP {
+	label="Node: Control Plane function";style=dotted
+	N1_E_CP [label="CP Entity"]
+}
+
+subgraph cluster_N2_UP {
+	label="Node: User Plane function\nNode ID: my-userplane.com\n(FQDN may provide multiple PFCP Entities)";style=dotted
+	N2_E_UP [label="UP Entity\n8.7.6.1"]
+	N2_E_UP2 [label="UP Entity\n8.7.6.2"]
+}
+
+subgraph cluster_N3_UP {
+	label="Node: User Plane function\nNode ID: 1.2.3.4\n(IP address means only one PFCP Entity)";style=dotted
+	N3_E_UP [label="UP Entity\n1.2.3.4\n(osmo-upf)"]
+}
+
+N1_E_CP -> N3_E_UP [label="PFCP Request"]
+N1_E_CP -> N2_E_UP
+}

doc/charts/pfcp_up_peer_fsm.dot

@@ -0,0 +1,27 @@
+digraph G {
+rankdir=TB
+labelloc=t; label="PFCP UP peer FSM\nUser Plane side, managing association with remote CP peer"
+
+txrx [label="PFCP socket",shape="box"]
+
+txrx -> NOT_ASSOCIATED [label="rx PFCP msg from\nnew remote IP"]
+txrx -> NOT_ASSOCIATED [label="EV_RX_ASSOC_SETUP_REQ",style=dotted]
+
+NOT_ASSOCIATED -> ASSOCIATED [label="Assoc Setup Req",shape="box"]
+
+heartbeat [label="PFCP heartbeat FSM",shape=box3d]
+ASSOCIATED -> heartbeat [label="alloc()",style=dotted]
+heartbeat -> ASSOCIATED [label="EV_HEARTBEAT_FAILURE",style=dotted]
+
+txrx -> ASSOCIATED [label="EV_RX_SESSION_EST_REQ",style=dotted]
+up_session [label="PFCP UP session FSM",shape=box3d]
+ASSOCIATED -> up_session [label="up_session_create()",style=dotted]
+
+txrx -> ASSOCIATED [label="EV_RX_ASSOC_UPD_REQ",style=dotted]
+ASSOCIATED -> GRACEFUL_RELEASE [label="Association Update\nindicating graceful release"]
+
+ASSOCIATED -> term [label="Heartbeat failure"]
+GRACEFUL_RELEASE -> term
+term [shape="octagon"]
+
+}

doc/charts/pfcp_up_session_fsm.dot

@@ -0,0 +1,21 @@
+digraph G {
+rankdir=TB
+labelloc=t; label="PFCP UP session FSM"
+
+peer [label="PFCP UP peer FSM",shape=box3d]
+peer -> ESTABLISHED [label="rx_session_est_req()"]
+
+txrx [label="PFCP socket",shape="box"]
+txrx2 [label="PFCP socket",shape="box"]
+
+txrx -> ESTABLISHED [label="EV_RX_SESSION_MOD_REQ",style=dotted]
+ESTABLISHED -> txrx [label="tx_session_mod_resp()",style=dotted,constraint=false]
+ESTABLISHED -> ESTABLISHED [label="Mod"]
+
+txrx2 -> ESTABLISHED [label="EV_RX_SESSION_DEL_REQ",style=dotted]
+ESTABLISHED -> txrx2 [label="tx_session_del_resp()",style=dotted,constraint=false]
+
+ESTABLISHED -> term [label="Deletion"]
+term [shape="octagon"]
+
+}

doc/examples/osmo-upf/osmo-upf-create-dev.cfg

@@ -0,0 +1,16 @@
+log stderr
+ logging filter all 1
+ logging color 1
+ logging print level 1
+ logging print category 1
+ logging print category-hex 0
+ logging print file basename last
+ logging print extended-timestamp 1
+ logging level set-all info
+#logging level set-all debug
+
+timer pfcp x24 5000
+pfcp
+ local-addr 127.0.0.1
+gtp
+ dev create apn23

doc/examples/osmo-upf/osmo-upf.cfg

@@ -6,4 +6,10 @@ log stderr
  logging print category-hex 0
  logging print file basename last
  logging print extended-timestamp 1
+ logging level set-all debug
  logging level set-all notice
+ logging level set-all info
+
+timer pfcp x24 5000
+pfcp
+ local-addr 127.0.0.1

include/osmocom/pfcp/Makefile.am

@@ -1,5 +1,9 @@
 pfcp_HEADERS = \
+	pfcp_endpoint.h \
+	pfcp_heartbeat_fsm.h \
 	pfcp_ies_custom.h \
+	pfcp_ies_auto.h \
+	pfcp_msg.h \
 	pfcp_proto.h \
 	pfcp_strs.h \
 	$(NULL)

include/osmocom/pfcp/pfcp_endpoint.h

@@ -0,0 +1,101 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <osmocom/core/socket.h>
+#include <osmocom/core/select.h>
+#include <osmocom/core/tdef.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+
+struct osmo_pfcp_endpoint;
+struct osmo_fsm_inst;
+
+#define OSMO_PFCP_TIMER_T1 -22
+#define OSMO_PFCP_TIMER_N1 -23
+#define OSMO_PFCP_TIMER_KEEP_RESP -24
+#define OSMO_PFCP_TIMER_ASSOC_RETRY -26
+#define OSMO_PFCP_TIMER_GRACEFUL_REL -27
+
+extern struct osmo_tdef osmo_pfcp_tdefs[];
+
+/* Ownership of m remains with the caller / m will be deallocated by the caller */
+typedef void (*osmo_pfcp_endpoint_cb)(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m);
+
+/* Send/receive PFCP messages to/from remote PFCP endpoints. */
+struct osmo_pfcp_endpoint {
+	struct {
+		/* Local address */
+		struct osmo_sockaddr local_addr;
+		/* Local PFCP Node ID, as sent in outgoing messages' Node ID IE */
+		struct osmo_pfcp_ie_node_id local_node_id;
+
+		/* Timer definitions to use, if any. See t1_ms, keep_resp_ms. Use osmo_pfcp_tdefs by default. It is
+		 * convenient to add osmo_pfcp_tdefs as one of your program's osmo_tdef_group entries and call
+		 * osmo_tdef_vty_init() to expose PFCP timers on the VTY. */
+		const struct osmo_tdef *tdefs;
+	} cfg;
+
+	/* PFCP socket */
+	struct osmo_fd pfcp_fd;
+
+	/* The time at which this endpoint last restarted, as seconds since unix epoch. */
+	uint32_t recovery_time_stamp;
+
+	/* State for determining the next sequence number for transmitting a request message */
+	uint32_t seq_nr_state;
+
+	/* This function is called just after decoding and before handling the message.
+	 * This function may set ctx.peer_fi and ctx.session_fi, used for logging context during message decoding.
+	 * The caller may also use these fi pointers to reduce lookup iterations in rx_msg().
+	 */
+	osmo_pfcp_endpoint_cb set_msg_ctx;
+
+	/* Callback to receive single incoming PFCP messages from a remote peer, already decoded. */
+	osmo_pfcp_endpoint_cb rx_msg;
+
+	/* application-private data */
+	void *priv;
+
+	/* All transmitted messages that are still pending, list of osmo_pfcp_queue_entry.
+	 * For a transmitted Request message, wait for a matching Response from a remote peer; if none arrives,
+	 * retransmit (see n1 and t1_ms).
+	 * For a transmitted Response message, keep it in the queue for a fixed amount of time. If the peer retransmits
+	 * the original Request, do not dispatch the Request, but respond with the queued message directly.
+	 */
+	struct llist_head retrans_queue;
+};
+
+struct osmo_pfcp_endpoint *osmo_pfcp_endpoint_create(void *ctx, void *priv);
+int osmo_pfcp_endpoint_bind(struct osmo_pfcp_endpoint *ep);
+void osmo_pfcp_endpoint_close(struct osmo_pfcp_endpoint *ep);
+void osmo_pfcp_endpoint_free(struct osmo_pfcp_endpoint **ep);
+
+uint32_t osmo_pfcp_endpoint_next_seq_nr(struct osmo_pfcp_endpoint *ep);
+
+int osmo_pfcp_endpoint_tx(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m);
+int osmo_pfcp_endpoint_tx_data(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m);
+int osmo_pfcp_endpoint_tx_heartbeat_req(struct osmo_pfcp_endpoint *ep, const struct osmo_sockaddr *remote_addr);
+
+void osmo_pfcp_endpoint_invalidate_ctx(struct osmo_pfcp_endpoint *ep, struct osmo_fsm_inst *deleted_fi);

include/osmocom/pfcp/pfcp_heartbeat_fsm.h

@@ -0,0 +1,40 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+struct osmo_fsm_inst;
+struct osmo_tdef;
+
+enum osmo_pfcp_heartbeat_fsm_event {
+	/* Dispatch this with a struct osmo_pfcp_msg* as data argument whenever a Heartbeat Response matching this
+	 * instance is received. Typically a PFCP Peer responds to a request sent from here. */
+	OSMO_PFCP_HEARTBEAT_EV_RX_RESP,
+	/* Dispatch this with a struct osmo_pfcp_msg* as data argument whenever a Heartbeat Request matching this
+	 * instance is received. Typically a PFCP Peer on its own accord sent a Heartbeat Request. */
+	OSMO_PFCP_HEARTBEAT_EV_RX_REQ,
+};
+
+struct osmo_fsm_inst *osmo_pfcp_heartbeat_alloc(struct osmo_fsm_inst *parent_fi,
+						uint32_t parent_event_tx_heartbeat, uint32_t parent_event_term,
+						struct osmo_tdef *tdefs);

include/osmocom/pfcp/pfcp_ies_custom.h

@@ -48,6 +48,9 @@ struct osmo_pfcp_ie_node_id {
 	};
 };
 
+int osmo_pfcp_ie_node_id_to_str_buf(char *buf, size_t buflen, const struct osmo_pfcp_ie_node_id *node_id);
+char *osmo_pfcp_ie_node_id_to_str_c(void *ctx, const struct osmo_pfcp_ie_node_id *node_id);
+
 bool osmo_pfcp_bits_get(const uint8_t *bits, unsigned int bitpos);
 void osmo_pfcp_bits_set(uint8_t *bits, unsigned int bitpos, bool val);
 int osmo_pfcp_bits_to_str_buf(char *buf, size_t buflen, const uint8_t *bits, const struct value_string *bit_strs);
@@ -73,7 +76,7 @@ struct osmo_pfcp_ie_up_function_features {
  *     printf("%s\n", osmo_pfcp_bits_to_str_c(x.bits, osmo_pfcp_cp_feature_strs));
  */
 struct osmo_pfcp_ie_cp_function_features {
-	uint8_t bits[3];
+	uint8_t bits[1];
 };
 
 /* 3GPP TS 29.244 8.2.37 */
@@ -84,6 +87,7 @@ struct osmo_pfcp_ie_f_seid {
 
 void osmo_pfcp_ie_f_seid_set(struct osmo_pfcp_ie_f_seid *f_seid, uint64_t seid,
 			     const struct osmo_sockaddr *remote_addr);
+int osmo_pfcp_ie_f_seid_cmp(const struct osmo_pfcp_ie_f_seid *a, const struct osmo_pfcp_ie_f_seid *b);
 
 /* 3GPP TS 29.244 8.3.2 */
 struct osmo_pfcp_ie_f_teid {

include/osmocom/pfcp/pfcp_msg.h

@@ -0,0 +1,205 @@
+/* PFCP message encoding and decoding */
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <inttypes.h>
+
+#include <osmocom/core/socket.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/core/fsm.h>
+
+#include <osmocom/pfcp/pfcp_proto.h>
+#include <osmocom/pfcp/pfcp_ies_auto.h>
+#include <osmocom/pfcp/pfcp_strs.h>
+
+struct msgb;
+struct osmo_t16l16v_ie;
+struct osmo_pfcp_msg;
+
+#define OSMO_PFCP_MSGB_ALLOC_SIZE 2048
+
+#define OSMO_LOG_PFCP_MSG_SRC(M, LEVEL, file, line, FMT, ARGS...) do { \
+		struct osmo_fsm_inst *_fi = (M) ? ((M)->ctx.session_fi ?: (M)->ctx.peer_fi) : NULL; \
+		enum osmo_pfcp_cause *cause = osmo_pfcp_msg_cause(M); \
+		if ((M)->h.seid_present) { \
+			LOGPFSMSLSRC(_fi, DLPFCP, LEVEL, file, line, \
+				     "%s%s PFCP seq-%u SEID-0x%"PRIx64" %s%s%s: " FMT, \
+				     _fi ? "" : osmo_sockaddr_to_str_c(OTC_SELECT, &(M)->remote_addr), \
+				     (M)->rx ? "-rx->" : "<-tx-", (M)->h.sequence_nr, \
+				     (M)->h.seid, \
+				     osmo_pfcp_message_type_str((M)->h.message_type), cause ? ": " : "", \
+				     cause ? osmo_pfcp_cause_str(*cause) : "", ##ARGS); \
+		} else { \
+			LOGPFSMSLSRC(_fi, DLPFCP, LEVEL, file, line, \
+				     "%s%s PFCP seq-%u %s%s%s: " FMT, \
+				     _fi ? "" : osmo_sockaddr_to_str_c(OTC_SELECT, &(M)->remote_addr), \
+				     (M)->rx ? "-rx->" : "<-tx-", (M)->h.sequence_nr, \
+				     osmo_pfcp_message_type_str((M)->h.message_type), cause ? ": " : "", \
+				     cause ? osmo_pfcp_cause_str(*cause) : "", ##ARGS); \
+		} \
+	} while (0)
+
+#define OSMO_LOG_PFCP_MSG(M, LEVEL, FMT, ARGS...) \
+	OSMO_LOG_PFCP_MSG_SRC(M, LEVEL, __FILE__, __LINE__, FMT, ##ARGS)
+
+/* Return the next PFCP transmit sequence number based on the given sequence state var. */
+static inline uint32_t osmo_pfcp_next_seq(uint32_t *seq_state)
+{
+	(*seq_state)++;
+	(*seq_state) &= 0xffffff;
+	return *seq_state;
+}
+
+struct osmo_pfcp_header_parsed {
+	uint8_t version;
+	enum osmo_pfcp_message_type message_type;
+	uint32_t sequence_nr;
+	bool priority_present;
+	uint8_t priority;
+	bool seid_present;
+	uint64_t seid;
+};
+
+/* For PFCP requests, notify when a PFCP response has arrived, or when the PFCP response timed out.
+ * When rx_resp == NULL, receiving a response timed out or the response could not be decoded.
+ * On error, errmsg may convey a human readable error message.
+ * Return 1 to also pass rx_resp to osmo_pfcp_endpoint->rx_msg(), return 0 to mark rx_resp handled and not pass it to
+ * rx_msg() (to save lookup iterations). Return negative on error, rx_resp is dropped.
+ * Find in req the original osmo_pfcp_msg instance; in req->ctx.priv, arbitrary user data may be passed.
+ * For example:
+ *
+ *  static int on_foo_resp(struct osmo_pfcp_msg *req, struct osmo_pfcp_msg *rx_resp, const char *errmsg)
+ *  {
+ *          struct something *obj = req->ctx.priv;
+ *          if (!rx_resp) {
+ *                  handle_error();
+ *                  return 0;
+ *          }
+ *          handle_response(obj, rx_resp);
+ *          return 0;
+ *  }
+ *
+ *  int do_request(struct something *obj)
+ *  {
+ *          struct osmo_pfcp_msg *req;
+ *          req = osmo_pfcp_msg_alloc_tx(pfcp_ep, &upf_addr, &pfcp_ep->cfg.local_node_id, NULL, OSMO_PFCP_MSGT_FOO);
+ *          req->h.seid_present = true;
+ *          req->h.seid = remote_seid;
+ *          req->ies.foo... = ...;
+ *          req->ctx.on_resp = on_foo_resp;
+ *          req->ctx.priv = obj;
+ *          return osmo_pfcp_endpoint_tx(pfcp_ep, req);
+ *  }
+ */
+typedef int (*osmo_pfcp_resp_cb)(struct osmo_pfcp_msg *req, struct osmo_pfcp_msg *rx_resp, const char *errmsg);
+
+struct osmo_pfcp_msg {
+	/* Peer's remote address. Received from this peer, or should be sent to this peer. */
+	struct osmo_sockaddr remote_addr;
+	/* True when this message was received from a remote; false when this message is going to be sent. */
+	bool rx;
+	/* True when this message is a Response message type; false if Request. This is set by
+	 * osmo_pfcp_msg_decode() for received messages, and by osmo_pfcp_msg_alloc_tx */
+	bool is_response;
+
+	struct osmo_pfcp_header_parsed h;
+
+	int ofs_cause;
+	int ofs_node_id;
+
+	/* The union of decoded IEs from all supported PFCP message types.  The union and its structure is defined in
+	 * pfcp_ies_auto.h, which is generated by gen__pfcp_ies_auto.c.
+	 */
+	union osmo_pfcp_ies ies;
+
+	/* Context information about this message, used for logging */
+	struct {
+		/* Peer FSM instance that this message is received from / sent to. This can be set in the
+		 * osmo_pfcp_endpoint->set_msg_ctx() implementation, up to the caller. If present, this is used for
+		 * logging context, and can also be used by the caller to reduce lookup iterations. */
+		struct osmo_fsm_inst *peer_fi;
+		struct osmo_use_count *peer_use_count;
+		const char *peer_use_token;
+
+		/* Session FSM instance that this message is received from / sent to. This can be set in the
+		 * osmo_pfcp_endpoint->set_msg_ctx() implementation, up to the caller. If present, this is used for
+		 * logging context, and can also be used by the caller to reduce lookup iterations. */
+		struct osmo_fsm_inst *session_fi;
+		struct osmo_use_count *session_use_count;
+		const char *session_use_token;
+
+		osmo_pfcp_resp_cb resp_cb;
+		void *priv;
+	} ctx;
+
+	/* When a message gets encoded, the encoded packet is cached here for possible retransmissions. */
+	struct msgb *encoded;
+};
+
+/* Given a &osmo_pfcp_msg->ies pointer, return the &osmo_pfcp_msg.
+ * In the TLV API, only the 'ies' union is passed around as argument. This macro is useful in error callbacks to obtain
+ * the related osmo_pfcp_msg and thus the logging context pointers (ctx.peer_fi and ctx.session_fi). */
+#define OSMO_PFCP_MSG_FOR_IES(IES_P) ((struct osmo_pfcp_msg *)((char *)IES_P - offsetof(struct osmo_pfcp_msg, ies)))
+
+bool osmo_pfcp_msgtype_is_response(enum osmo_pfcp_message_type message_type);
+
+int osmo_pfcp_ie_f_teid_to_str_buf(char *buf, size_t len, const struct osmo_pfcp_ie_f_teid *ft);
+char *osmo_pfcp_ie_f_teid_to_str_c(void *ctx, const struct osmo_pfcp_ie_f_teid *ft);
+
+int osmo_pfcp_msg_encode(struct msgb *msg, const struct osmo_pfcp_msg *pfcp_msg);
+
+int osmo_pfcp_msg_decode_header(struct osmo_gtlv_load *tlv, struct osmo_pfcp_msg *m,
+				const struct msgb *msg);
+int osmo_pfcp_msg_decode_tlv(struct osmo_pfcp_msg *m, struct osmo_gtlv_load *tlv);
+
+struct osmo_pfcp_msg *osmo_pfcp_msg_alloc_rx(void *ctx, const struct osmo_sockaddr *remote_addr);
+struct osmo_pfcp_msg *osmo_pfcp_msg_alloc_tx(void *ctx, const struct osmo_sockaddr *remote_addr,
+					     const struct osmo_pfcp_ie_node_id *local_node_id,
+					     const struct osmo_pfcp_msg *in_reply_to,
+					     enum osmo_pfcp_message_type msg_type);
+
+void osmo_pfcp_msg_invalidate_ctx(struct osmo_pfcp_msg *m, struct osmo_fsm_inst *deleted_fi);
+
+void osmo_pfcp_msg_free(struct osmo_pfcp_msg *m);
+
+int osmo_pfcp_ie_node_id_from_osmo_sockaddr(struct osmo_pfcp_ie_node_id *node_id, const struct osmo_sockaddr *os);
+int osmo_pfcp_ie_node_id_to_osmo_sockaddr(const struct osmo_pfcp_ie_node_id *node_id, struct osmo_sockaddr *os);
+
+#define OSMO_PFCP_MSG_MEMB(M, OFS) ((OFS) <= 0 ? NULL : (void *)((uint8_t *)(M) + OFS))
+
+static inline enum osmo_pfcp_cause *osmo_pfcp_msg_cause(const struct osmo_pfcp_msg *m)
+{
+	return OSMO_PFCP_MSG_MEMB(m, m->ofs_cause);
+}
+
+static inline struct osmo_pfcp_ie_node_id *osmo_pfcp_msg_node_id(const struct osmo_pfcp_msg *m)
+{
+	return OSMO_PFCP_MSG_MEMB(m, m->ofs_node_id);
+}
+
+int osmo_pfcp_msg_to_str_buf(char *buf, size_t buflen, const struct osmo_pfcp_msg *m);
+char *osmo_pfcp_msg_to_str_c(void *ctx, const struct osmo_pfcp_msg *m);

include/osmocom/upf/Makefile.am

@@ -1,3 +1,10 @@
 noinst_HEADERS = \
+	up_endpoint.h \
+	up_peer.h \
+	up_session.h \
 	upf.h \
+	upf_gtp.h \
+	upf_gtpu_echo.h \
+	upf_nft.h \
+	up_gtp_action.h \
 	$(NULL)

include/osmocom/upf/up_endpoint.h

@@ -0,0 +1,48 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <osmocom/core/linuxlist.h>
+
+struct osmo_pfcp_msg;
+struct osmo_pfcp_endpoint;
+struct osmo_sockaddr;
+
+#define UP_USE_MSG_RX "msg-rx"
+#define UP_USE_MSG_TX "msg-tx"
+
+struct up_endpoint {
+	struct osmo_pfcp_endpoint *pfcp_ep;
+
+	struct llist_head peers;
+
+	uint64_t next_seid_state;
+	uint32_t next_teid_state;
+};
+
+struct up_endpoint *up_endpoint_init(void *ctx, const struct osmo_sockaddr *local_addr);
+void up_endpoint_free(struct up_endpoint **ep);
+
+uint64_t up_endpoint_next_seid(struct up_endpoint *ep);
+uint32_t up_endpoint_next_teid(struct up_endpoint *ep);

include/osmocom/upf/up_gtp_action.h

@@ -0,0 +1,73 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/fsm.h>
+
+#include <osmocom/upf/up_session.h>
+#include <osmocom/upf/upf_gtp.h>
+#include <osmocom/upf/upf_nft.h>
+
+#define LOG_UP_GTP_ACTION(A, LEVEL, FMT, ARGS...) \
+	LOGP(DGTP, LEVEL, "%s: " FMT, up_gtp_action_to_str_c(OTC_SELECT, A), ##ARGS)
+
+struct up_session;
+
+enum up_gtp_action_kind {
+	UP_GTP_DROP,
+	UP_GTP_U_ENDECAPS,
+	UP_GTP_U_TUNMAP,
+};
+
+struct up_gtp_action {
+	struct llist_head entry;
+	struct up_session *session;
+
+	uint16_t pdr_core;
+	uint16_t pdr_access;
+
+	enum up_gtp_action_kind kind;
+	union {
+		/* En-/De-capsulate GTP: add/remove a GTP header and forward the GTP payload from/to plain IP. */
+		struct upf_gtp_tun_desc endecaps;
+
+		/* Tunnel-map GTP: translate from one TEID to another and forward */
+		struct upf_nft_tunmap_desc tunmap;
+	};
+
+	/* volatile loop variable to match up wanted and actually present GTP actions */
+	void *handle;
+};
+
+int up_gtp_action_cmp(const struct up_gtp_action *a, const struct up_gtp_action *b);
+
+int up_gtp_action_enable(struct up_gtp_action *a);
+int up_gtp_action_disable(struct up_gtp_action *a);
+
+int up_gtp_action_to_str_buf(char *buf, size_t buflen, const struct up_gtp_action *a);
+char *up_gtp_action_to_str_c(void *ctx, const struct up_gtp_action *a);

include/osmocom/upf/up_peer.h

@@ -0,0 +1,78 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/hashtable.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/use_count.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+
+enum up_peer_event {
+	UP_PEER_EV_RX_ASSOC_SETUP_REQ,
+	UP_PEER_EV_RX_ASSOC_UPD_REQ,
+	UP_PEER_EV_RX_ASSOC_REL_REQ,
+	UP_PEER_EV_RX_SESSION_EST_REQ,
+	UP_PEER_EV_HEARTBEAT_FAILURE,
+	UP_PEER_EV_USE_COUNT_ZERO,
+	UP_PEER_EV_SESSION_TERM,
+};
+
+struct up_peer {
+	struct llist_head entry;
+
+	struct osmo_fsm_inst *fi;
+	struct up_endpoint *up_endpoint;
+
+	/* peer's remote address */
+	struct osmo_sockaddr remote_addr;
+	struct osmo_pfcp_ie_node_id remote_node_id;
+	uint32_t remote_recovery_timestamp;
+
+	struct osmo_pfcp_ie_up_function_features local_up_features;
+	struct osmo_pfcp_ie_cp_function_features peer_cp_features;
+
+	uint32_t next_seq_nr;
+
+	struct osmo_fsm_inst *heartbeat_fi;
+
+	struct osmo_use_count use_count;
+	struct osmo_use_count_entry use_count_buf[5];
+
+	DECLARE_HASHTABLE(sessions_by_up_seid, 6);
+	DECLARE_HASHTABLE(sessions_by_cp_seid, 6);
+};
+
+struct up_peer *up_peer_find_or_add(struct up_endpoint *up_ep, const struct osmo_sockaddr *remote_addr);
+struct up_peer *up_peer_find(struct up_endpoint *up_ep, const struct osmo_sockaddr *remote_addr);
+
+void up_peer_set_msg_ctx(struct up_peer *peer, struct osmo_pfcp_msg *m);
+
+char *up_peer_remote_addr_str(struct up_peer *peer);
+
+struct osmo_pfcp_msg *up_peer_init_tx(struct up_peer *peer, struct osmo_pfcp_msg *in_reply_to,
+				      enum osmo_pfcp_message_type message_type);
+
+void up_peer_free(struct up_peer *peer);

include/osmocom/upf/up_session.h

@@ -0,0 +1,117 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <osmocom/core/hashtable.h>
+#include <osmocom/core/use_count.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+
+struct osmo_fsm_inst;
+struct osmo_pfcp_msg;
+struct up_peer;
+
+enum up_session_fsm_event {
+	UP_SESSION_EV_RX_SESSION_EST_REQ,
+	UP_SESSION_EV_RX_SESSION_MOD_REQ,
+	UP_SESSION_EV_RX_SESSION_DEL_REQ,
+	UP_SESSION_EV_USE_COUNT_ZERO,
+};
+
+enum up_session_kind {
+	UP_SESSION_DROP,
+	UP_SESSION_GTP_U_ENDECAPS,
+	UP_SESSION_GTP_U_FORW,
+};
+
+struct up_session {
+	struct hlist_node node_by_up_seid;
+	struct hlist_node node_by_cp_seid;
+
+	struct osmo_fsm_inst *fi;
+	struct up_peer *up_peer;
+
+	struct osmo_pfcp_ie_f_seid cp_f_seid;
+	uint64_t up_seid;
+
+	struct osmo_use_count use_count;
+	struct osmo_use_count_entry use_count_buf[8];
+
+	struct llist_head pdrs;
+	struct llist_head fars;
+	struct llist_head chosen_f_teids;
+
+	struct llist_head active_gtp_actions;
+};
+
+struct up_session *up_session_find_or_add(struct up_peer *peer, const struct osmo_pfcp_ie_f_seid *cp_f_seid,
+					  const struct osmo_pfcp_ie_f_seid *up_f_seid);
+struct up_session *up_session_find_by_up_seid(struct up_peer *peer, uint64_t up_seid);
+struct up_session *up_session_find_by_cp_f_seid(struct up_peer *peer, const struct osmo_pfcp_ie_f_seid *cp_f_seid);
+struct up_session *up_session_find_by_local_teid(struct up_peer *peer, uint32_t teid);
+
+void up_session_set_msg_ctx(struct up_session *session, struct osmo_pfcp_msg *m);
+
+char *up_session_gtp_status(struct up_session *session);
+bool up_session_is_active(struct up_session *session);
+bool up_session_is_fully_active(struct up_session *session, int *active_p, int *inactive_p);
+
+int up_session_discard(struct up_session *session);
+
+int up_session_to_str_buf(char *buf, size_t buflen, struct up_session *session);
+char *up_session_to_str_c(void *ctx, struct up_session *session);
+
+struct pdr {
+	struct llist_head entry;
+	struct up_session *session;
+
+	struct osmo_pfcp_ie_create_pdr desc;
+	struct osmo_pfcp_ie_f_teid *local_f_teid;
+	struct osmo_pfcp_ie_f_teid _local_f_teid_buf;
+
+	struct far *far;
+
+	bool rx_decaps;
+	bool forw_encaps;
+	bool forw_to_core;
+	bool forw_from_core;
+
+	struct pdr *reverse_pdr;
+	bool active;
+
+	char *inactive_reason;
+};
+
+int pdr_to_str_buf(char *buf, size_t buflen, const struct pdr *pdr);
+char *pdr_to_str_c(void *ctx, const struct pdr *pdr);
+
+struct far {
+	struct llist_head entry;
+	struct up_session *session;
+
+	struct osmo_pfcp_ie_create_far desc;
+	bool active;
+};

include/osmocom/upf/up_session_to_gtp.c

@@ -0,0 +1,23 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+

include/osmocom/upf/upf.h

@@ -24,12 +24,92 @@
 
 #pragma once
 
+#include <osmocom/core/tdef.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/select.h>
+#include <osmocom/core/linuxlist.h>
+
+struct osmo_tdef;
 struct ctrl_handle;
 
+struct upf_gtp_dev;
+struct nft_ctx;
+
+#define UPF_PFCP_LISTEN_DEFAULT "0.0.0.0"
+
+extern struct osmo_tdef_group g_upf_tdef_groups[];
+
+struct pfcp_vty_cfg {
+	char *local_addr;
+	uint16_t local_port;
+};
+
+struct gtp_vty_cfg_dev {
+	struct llist_head entry;
+
+	/* If true, osmo-upf creates the GTP device on startup. If false, the GTP device was created by the user, and we
+	 * just plug into it. */
+	bool create;
+
+	/* GTP device name as shown by 'ip link', e.g. 'apn0' */
+	char *dev_name;
+
+	/* Which address the GTP device should listen on.
+	 * When create == true, the GTP kernel module is created to listen on this address.
+	 * Also used to listen for GTP ECHO requests using libgtp. */
+	char *local_addr;
+};
+
+struct gtp_vty_cfg {
+	/* list of struct gtp_vty_cfg_dev, GTP devices as in the config file. The actual GTP devices in use are in
+	 * g_upf->gtp.devs. */
+	struct llist_head devs;
+};
+
 struct g_upf {
 	struct ctrl_handle *ctrl;
+
+	struct {
+		struct pfcp_vty_cfg vty_cfg;
+		struct up_endpoint *ep;
+	} pfcp;
+
+	/* Tunnel encaps decaps via GTP kernel module */
+	struct {
+		/* GTP devices as in osmo-upf.cfg */
+		struct gtp_vty_cfg vty_cfg;
+
+		/* GTP devices actually in use, list of struct upf_gtp_dev. */
+		struct llist_head devs;
+
+		struct mnl_socket *nl;
+		int32_t genl_id;
+
+		uint8_t recovery_count;
+	} gtp;
+
+	/* Tunnel forwarding via linux netfilter */
+	struct {
+		struct nft_ctx *nft_ctx;
+		char *table_name;
+		int priority;
+		uint32_t next_id_state;
+	} nft;
 };
 
 extern struct g_upf *g_upf;
 
+enum upf_log_subsys {
+	DREF,
+	DPEER,
+	DSESSION,
+	DGTP,
+	DNFT,
+};
+
 void g_upf_alloc(void *ctx);
+void upf_vty_init();
+int upf_pfcp_listen();
+
+int upf_gtp_devs_open();
+void upf_gtp_devs_close();

include/osmocom/upf/upf_gtp.h

@@ -0,0 +1,85 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/select.h>
+#include <osmocom/core/logging.h>
+
+#define LOG_GTP_DEV(DEV, LEVEL, FMT, ARGS...) \
+	LOGP(DGTP, LEVEL, "%s: " FMT, upf_gtp_dev_to_str_c(OTC_SELECT, (DEV)), ##ARGS)
+
+#define PORT_GTP0_C 3386
+#define PORT_GTP0_U 3386
+
+#define PORT_GTP1_C 2123
+#define PORT_GTP1_U 2152
+
+struct upf_gtp_dev {
+	struct llist_head entry;
+
+	/* If true, osmo-upf created this GTP device on startup and will destroy it on program exit. If false, the user
+	 * has created the device and osmo-upf will not destroy it. */
+	bool created;
+
+	char *name;
+	struct {
+		bool enabled;
+		struct osmo_sockaddr local_addr;
+		struct osmo_fd ofd;
+	} gtpv0;
+	struct {
+		struct osmo_sockaddr local_addr;
+		struct osmo_fd ofd;
+	} gtpv1;
+	bool sgsn_mode;
+
+	uint32_t ifidx;
+
+	struct llist_head tunnels;
+};
+
+struct upf_gtp_tun_desc {
+	uint32_t local_teid;
+	uint32_t remote_teid;
+	struct osmo_sockaddr ue_addr;
+	struct osmo_sockaddr gtp_remote_addr;
+};
+
+int upf_gtp_tun_desc_cmp(const struct upf_gtp_tun_desc *a, const struct upf_gtp_tun_desc *b);
+
+int upf_gtp_genl_open();
+void upf_gtp_genl_close();
+
+int upf_gtp_dev_open(const char *name, bool create_gtp_dev, const char *local_addr, bool listen_for_gtpv0,
+		     bool sgsn_mode);
+struct upf_gtp_dev *upf_gtp_dev_find_by_name(const char *name);
+struct upf_gtp_dev *upf_gtp_dev_first();
+
+int upf_gtp_dev_tunnel_add(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *t);
+bool upf_gtp_dev_is_tunnel_active(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *t);
+int upf_gtp_dev_tunnel_del(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *t);
+
+int upf_gtp_dev_to_str_buf(char *buf, size_t buflen, const struct upf_gtp_dev *dev);
+char *upf_gtp_dev_to_str_c(void *ctx, const struct upf_gtp_dev *dev);

include/osmocom/upf/upf_gtpu_echo.h

@@ -0,0 +1,4 @@
+/* GTP-U ECHO implementation for osmo-upf */
+#pragma once
+
+int upf_gtpu_echo_setup(struct upf_gtp_dev *dev);

include/osmocom/upf/upf_nft.h

@@ -0,0 +1,50 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+#include <osmocom/core/socket.h>
+
+#define NFT_CHAIN_NAME_PREFIX_TUNMAP "tunmap"
+
+struct upf_nft_tunmap_desc {
+	struct {
+		struct osmo_sockaddr gtp_remote_addr;
+		uint32_t local_teid;
+		uint32_t remote_teid;
+	} access;
+	struct {
+		struct osmo_sockaddr gtp_remote_addr;
+		uint32_t local_teid;
+		uint32_t remote_teid;
+	} core;
+	uint32_t id;
+};
+
+int upf_nft_init();
+int upf_nft_free();
+
+int upf_nft_tunmap_create(struct upf_nft_tunmap_desc *tunmap);
+int upf_nft_tunmap_delete(struct upf_nft_tunmap_desc *tunmap);

libosmo-gtlv.pc.in

@@ -0,0 +1,10 @@
+prefix=@prefix@
+exec_prefix=@exec_prefix@
+libdir=@libdir@
+includedir=@includedir@
+
+Name: Osmocom Generic TLV Library
+Description: C Utility Library
+Version: @VERSION@
+Libs: -L${libdir} -losmo-gtlv
+Cflags: -I${includedir}/

libosmo-pfcp.pc.in

@@ -0,0 +1,10 @@
+prefix=@prefix@
+exec_prefix=@exec_prefix@
+libdir=@libdir@
+includedir=@includedir@
+
+Name: Osmocom PFCP library
+Description: C Utility Library that implements the PFCP protocol and endpoint
+Version: @VERSION@
+Libs: -L${libdir} -losmo-pfcp
+Cflags: -I${includedir}/

src/Makefile.am

@@ -2,4 +2,5 @@ SUBDIRS = \
 	libosmo-gtlv \
 	libosmo-pfcp \
 	osmo-upf \
+	osmo-pfcp-tool \
 	$(NULL)

src/libosmo-gtlv/Makefile.am

@@ -16,11 +16,11 @@ AM_LDFLAGS = \
 	$(COVERAGE_LDFLAGS) \
 	$(NULL)
 
-noinst_LIBRARIES = \
-	libosmo-gtlv.a \
+lib_LTLIBRARIES = \
+	libosmo-gtlv.la \
 	$(NULL)
 
-libosmo_gtlv_a_SOURCES = \
+libosmo_gtlv_la_SOURCES = \
 	gtlv.c \
 	gtlv_dec_enc.c \
 	gtlv_gen.c \

src/libosmo-pfcp/Makefile.am

@@ -19,12 +19,15 @@ AM_LDFLAGS = \
 	$(COVERAGE_LDFLAGS) \
 	$(NULL)
 
-noinst_LIBRARIES = \
-	libosmo-pfcp.a \
+lib_LTLIBRARIES = \
+	libosmo-pfcp.la \
 	$(NULL)
 
-libosmo_pfcp_a_SOURCES = \
+libosmo_pfcp_la_SOURCES = \
+	pfcp_endpoint.c \
+	pfcp_heartbeat_fsm.c \
 	pfcp_ies_custom.c \
+	pfcp_msg.c \
 	pfcp_strs.c \
 	\
 	pfcp_ies_auto.c \
@@ -54,7 +57,7 @@ gen__pfcp_ies_auto_SOURCES = \
 	$(NULL)
 
 gen__pfcp_ies_auto_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(COVERAGE_LDFLAGS) \
 	$(NULL)

src/libosmo-pfcp/pfcp_endpoint.c

@@ -0,0 +1,453 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <errno.h>
+#include <unistd.h>
+#include <time.h>
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/timer.h>
+#include <osmocom/core/tdef.h>
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
+#include <osmocom/pfcp/pfcp_msg.h>
+
+/*! Entry of pfcp_endpoint message queue of PFCP messages, for re-transsions. */
+struct osmo_pfcp_queue_entry {
+	/* entry in per-peer list of messages waiting for a response */
+	struct llist_head entry;
+	/* back-pointer */
+	struct osmo_pfcp_endpoint *ep;
+	/* message we have transmitted */
+	struct osmo_pfcp_msg *m;
+	/* T1 timer: How long to wait for response before retransmitting */
+	struct osmo_timer_list t1;
+	/* N1: number of pending re-transmissions */
+	unsigned int n1_remaining;
+};
+
+/* find a matching osmo_pfcp_queue_entry for given rx_hdr */
+static struct osmo_pfcp_queue_entry *
+osmo_pfcp_queue_find(struct llist_head *queue, const struct osmo_pfcp_msg *rx)
+{
+	struct osmo_pfcp_queue_entry *qe;
+	/* It's important to match only a Request to a Response and vice versa, because the remote peer makes its own
+	 * sequence_nr. There could be a collision of sequence_nr. But as long as all Requests look for a Response and
+	 * vice versa, the sequence_nr scopes don't overlap. */
+	llist_for_each_entry(qe, queue, entry) {
+		if (qe->m->is_response != rx->is_response
+		    && qe->m->h.sequence_nr == rx->h.sequence_nr)
+			return qe;
+	}
+	return NULL;
+}
+
+/* clean up and deallocate the given osmo_pfcp_queue_entry */
+static void osmo_pfcp_queue_del(struct osmo_pfcp_queue_entry *qe)
+{
+	/* see also the talloc destructor */
+	talloc_free(qe);
+}
+
+int osmo_pfcp_queue_destructor(struct osmo_pfcp_queue_entry *qe)
+{
+	osmo_timer_del(&qe->t1);
+	llist_del(&qe->entry);
+	return 0;
+}
+
+struct osmo_tdef osmo_pfcp_tdefs[] = {
+	{ .T = -19, .default_val = 15, .unit = OSMO_TDEF_S,
+	  .desc = "PFCP Heartbeat Request period, how long to wait between issuing requests"
+	},
+	{ .T = -20, .default_val = 15, .unit = OSMO_TDEF_S,
+	  .desc = "PFCP Heartbeat Response timeout, the time after which to regard a non-responding peer as disconnected"
+	},
+	{ .T = -21, .default_val = 15, .unit = OSMO_TDEF_S,
+	  .desc = "PFCP peer graceful shutdown timeout, how long to keep the peer's state after a peer requested"
+		  " graceful shutdown"
+	},
+	{ .T = OSMO_PFCP_TIMER_T1, .default_val = 3000, .unit = OSMO_TDEF_MS,
+	  .desc = "PFCP request timeout, how long after a missing response to retransmit a PFCP request"
+	},
+	{ .T = OSMO_PFCP_TIMER_N1, .default_val = 3, .unit = OSMO_TDEF_CUSTOM,
+	  .desc = "Number of PFCP request retransmission attempts"
+	},
+	{ .T = OSMO_PFCP_TIMER_KEEP_RESP, .default_val = 10000, .unit = OSMO_TDEF_MS,
+	  .desc = "PFCP response timeout, how long to keep a response, in case its same request is retransmitted by the peer"
+	},
+	{ .T = OSMO_PFCP_TIMER_ASSOC_RETRY, .default_val = 15, .unit = OSMO_TDEF_S,
+	  .desc = "Idle time between attempts of PFCP Association Setup (CPF)"
+	},
+	{ .T = OSMO_PFCP_TIMER_GRACEFUL_REL, .default_val = 15, .unit = OSMO_TDEF_S,
+	  .desc = "PFCP graceful release timeout"
+	},
+	{}
+};
+
+struct osmo_pfcp_endpoint *osmo_pfcp_endpoint_create(void *ctx, void *priv)
+{
+	struct osmo_pfcp_endpoint *ep = talloc_zero(ctx, struct osmo_pfcp_endpoint);
+	uint32_t unix_time;
+	if (!ep)
+		return NULL;
+
+	INIT_LLIST_HEAD(&ep->retrans_queue);
+
+	ep->cfg.tdefs = osmo_pfcp_tdefs;
+	ep->priv = priv;
+	ep->pfcp_fd.fd = -1;
+
+	/* time() returns seconds since 1970 (UNIX epoch), but the recovery_time_stamp is coded in the NTP format, which is
+	 * seconds since 1900, the NTP era 0. 2208988800L is the offset between UNIX epoch and NTP era 0.
+	 * TODO: what happens when we enter NTP era 1? Is it sufficient to integer-wrap? */
+	unix_time = time(NULL);
+	ep->recovery_time_stamp = unix_time + 2208988800L;
+	LOGP(DLPFCP, LOGL_NOTICE, "PFCP endpoint: recovery timestamp = 0x%08x (%u seconds since UNIX epoch,"
+	     " which is %u seconds since NTP era 0; IETF RFC 5905)\n",
+	     ep->recovery_time_stamp, unix_time, ep->recovery_time_stamp);
+
+	return ep;
+}
+
+uint32_t osmo_pfcp_endpoint_next_seq_nr(struct osmo_pfcp_endpoint *ep)
+{
+	ep->seq_nr_state++;
+	ep->seq_nr_state &= 0xffffff;
+	return ep->seq_nr_state;
+}
+
+static unsigned int ep_n1(struct osmo_pfcp_endpoint *ep)
+{
+	return osmo_tdef_get(ep->cfg.tdefs, OSMO_PFCP_TIMER_N1, OSMO_TDEF_CUSTOM, -1);
+}
+
+unsigned int ep_t1(struct osmo_pfcp_endpoint *ep)
+{
+	return osmo_tdef_get(ep->cfg.tdefs, OSMO_PFCP_TIMER_T1, OSMO_TDEF_MS, -1);
+}
+
+unsigned int ep_keep_resp(struct osmo_pfcp_endpoint *ep)
+{
+	return osmo_tdef_get(ep->cfg.tdefs, OSMO_PFCP_TIMER_KEEP_RESP, OSMO_TDEF_MS, -1);
+}
+
+/* Return true to keep the message in the queue, false for dropping from the queue. */
+static bool pfcp_queue_retrans(struct osmo_pfcp_queue_entry *qe)
+{
+	struct osmo_pfcp_endpoint *endpoint = qe->ep;
+	unsigned int t1_ms = ep_t1(endpoint);
+	struct osmo_pfcp_msg *m = qe->m;
+	int rc;
+
+	/* re-transmit */
+	if (qe->n1_remaining)
+		qe->n1_remaining--;
+	OSMO_LOG_PFCP_MSG(m, LOGL_DEBUG, "re-sending (%u attempts remaining)\n", qe->n1_remaining);
+
+	rc = osmo_pfcp_endpoint_tx_data(endpoint, m);
+	/* If encoding failed, it cannot ever succeed. Drop the queue entry. */
+	if (rc)
+		return false;
+	/* if no more attempts remaining, drop from queue */
+	if (!qe->n1_remaining)
+		return false;
+	/* re-schedule timer, keep in queue */
+	osmo_timer_schedule(&qe->t1, t1_ms/1000, t1_ms%1000);
+	return true;
+}
+
+/* T1 for a given queue entry has expired */
+static void pfcp_queue_timer_cb(void *data)
+{
+	struct osmo_pfcp_queue_entry *qe = data;
+	bool keep;
+
+	if (qe->m->is_response) {
+		/* The response has waited in the queue for any retransmissions of its initiating request. Now that time
+		 * has passed and the response can be dropped from the queue. */
+		keep = false;
+	} else {
+		/* The request is still here, which means it has not received a response from the remote side.
+		 * Retransmit the request. */
+		keep = pfcp_queue_retrans(qe);
+	}
+
+	if (keep)
+		return;
+	/* Drop the queue entry. No more retransmissions. */
+	if (!qe->m->is_response && qe->m->ctx.resp_cb)
+		qe->m->ctx.resp_cb(qe->m, NULL, "PFCP retransmissions elapsed, no response received");
+	osmo_pfcp_queue_del(qe);
+}
+
+/* Directly encode and transmit the message, without storing in the retrans_queue. */
+int osmo_pfcp_endpoint_tx_data_no_logging(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	int rc;
+
+	if (!m->encoded) {
+		/* Allocate msgb as child of the message m, so that when m gets deallocated at the end of
+		 * retransmission queueing, the msgb gets deallocated with it. */
+		m->encoded = msgb_alloc_c(m, OSMO_PFCP_MSGB_ALLOC_SIZE, "PFCP-tx");
+		OSMO_ASSERT(m->encoded);
+		rc = osmo_pfcp_msg_encode(m->encoded, m);
+		if (rc) {
+			msgb_free(m->encoded);
+			m->encoded = NULL;
+			return rc;
+		}
+	}
+
+	rc = sendto(ep->pfcp_fd.fd, msgb_data(m->encoded), msgb_length(m->encoded), 0,
+		    (struct sockaddr*)&m->remote_addr, sizeof(m->remote_addr));
+	if (rc != msgb_length(m->encoded)) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "sendto() failed: rc = %d != length %u\n",
+				  rc, msgb_length(m->encoded));
+		return -EIO;
+	}
+	return 0;
+}
+
+int osmo_pfcp_endpoint_tx_data(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	OSMO_LOG_PFCP_MSG(m, LOGL_INFO, "sending\n");
+	return osmo_pfcp_endpoint_tx_data_no_logging(ep, m);
+}
+
+int osmo_pfcp_endpoint_tx_heartbeat_req(struct osmo_pfcp_endpoint *ep, const struct osmo_sockaddr *remote_addr)
+{
+	struct osmo_pfcp_msg *tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, remote_addr, NULL, NULL,
+							  OSMO_PFCP_MSGT_HEARTBEAT_REQ);
+	tx->ies.heartbeat_req.recovery_time_stamp = ep->recovery_time_stamp;
+	tx->h.sequence_nr = osmo_pfcp_endpoint_next_seq_nr(ep);
+	return osmo_pfcp_endpoint_tx_data(ep, tx);
+}
+
+/* add a given msgb to the queue of per-peer messages waiting for a response */
+static int osmo_pfcp_endpoint_retrans_queue_add(struct osmo_pfcp_endpoint *endpoint, struct osmo_pfcp_msg *m)
+{
+	struct osmo_pfcp_queue_entry *qe;
+	unsigned int n1 = ep_n1(endpoint);
+	unsigned int t1_ms = ep_t1(endpoint);
+	unsigned int keep_resp_ms = ep_keep_resp(endpoint);
+	unsigned int timeout = m->is_response ? keep_resp_ms : t1_ms;
+
+	LOGP(DLPFCP, LOGL_DEBUG, "retransmit unanswered Requests %u x %ums; keep sent Responses for %ums\n",
+	     n1, t1_ms, keep_resp_ms);
+	/* If there are no retransmissions or no timeout, it makes no sense to add to the queue. */
+	if (!n1 || !t1_ms) {
+		if (!m->is_response && m->ctx.resp_cb)
+			m->ctx.resp_cb(m, NULL, "PFCP timeout is zero, cannot wait for a response");
+		return 0;
+	}
+
+	qe = talloc(endpoint, struct osmo_pfcp_queue_entry);
+	OSMO_ASSERT(qe);
+	*qe = (struct osmo_pfcp_queue_entry){
+		.ep = endpoint,
+		.m = m,
+		.n1_remaining = m->is_response ? 0 : n1,
+	};
+	talloc_steal(qe, m);
+	llist_add_tail(&qe->entry, &endpoint->retrans_queue);
+	talloc_set_destructor(qe, osmo_pfcp_queue_destructor);
+
+	osmo_timer_setup(&qe->t1, pfcp_queue_timer_cb, qe);
+	osmo_timer_schedule(&qe->t1, timeout/1000, timeout%1000);
+	return 0;
+}
+
+/* Transmit a PFCP message.
+ * Store the message in the local message queue for possible retransmissions.
+ * On success, return zero, and pass ownership of m to ep. ep deallocates m when all retransmissions are done / a reply
+ * has been received.
+ * On error, return nonzero, and immediately deallocate m. */
+int osmo_pfcp_endpoint_tx(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	struct osmo_pfcp_ie_node_id *node_id;
+	int rc;
+	if (!m->is_response)
+		m->h.sequence_nr = osmo_pfcp_endpoint_next_seq_nr(ep);
+	node_id = osmo_pfcp_msg_node_id(m);
+	if (node_id)
+		*node_id = ep->cfg.local_node_id;
+
+	rc = osmo_pfcp_endpoint_tx_data(ep, m);
+	if (rc) {
+		if (!m->is_response && m->ctx.resp_cb)
+			m->ctx.resp_cb(m, NULL, "Failed to transmit request");
+		osmo_pfcp_msg_free(m);
+		return rc;
+	}
+	osmo_pfcp_endpoint_retrans_queue_add(ep, m);
+	return 0;
+}
+
+static void osmo_pfcp_endpoint_handle_rx(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	bool dispatch_rx = true;
+	struct osmo_pfcp_queue_entry *prev_msg;
+
+	OSMO_LOG_PFCP_MSG(m, LOGL_INFO, "received\n");
+
+	if (m->h.message_type == OSMO_PFCP_MSGT_HEARTBEAT_REQ) {
+		/* Directly answer with a Heartbeat Response. */
+		struct osmo_pfcp_msg *resp = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, NULL, m, OSMO_PFCP_MSGT_HEARTBEAT_RESP);
+		resp->ies.heartbeat_resp.recovery_time_stamp = ep->recovery_time_stamp;
+		osmo_pfcp_endpoint_tx_data(ep, resp);
+		/* Still also dispatch the Rx event to the peer. */
+	}
+
+	/* If this is receiving a response, search for matching request that is now completed.
+	 * If this is receiving a request, search for a matching response that can be retransmitted.
+	 * Either way see if a matching sequence_nr is already in the queue. */
+	prev_msg = osmo_pfcp_queue_find(&ep->retrans_queue, m);
+	if (prev_msg) {
+		if (m->is_response && !prev_msg->m->is_response) {
+			/* Got a response, the original request is now ACKed and can be dropped from the retransmission
+			 * queue. */
+			if (prev_msg->m->ctx.resp_cb) {
+				int rc = prev_msg->m->ctx.resp_cb(prev_msg->m, m, NULL);
+				/* Only dispatch the response to rx_msg() when resp_cb() asks for it with rc == 1
+				 * (or when there is no resp_cb()). */
+				if (rc != 1)
+					dispatch_rx = false;
+			}
+			osmo_pfcp_queue_del(prev_msg);
+		} else if (!m->is_response && prev_msg->m->is_response) {
+			/* Got a request, but we have already sent a response to this same request earlier. Retransmit
+			 * the same response, and don't dispatch the msg rx. Keep our response queued in case the
+			 * request is retransmitted yet another time. */
+			OSMO_LOG_PFCP_MSG(prev_msg->m, LOGL_INFO, "re-sending cached response\n");
+			osmo_pfcp_endpoint_tx_data_no_logging(ep, prev_msg->m);
+			dispatch_rx = false;
+		}
+	}
+
+	if (dispatch_rx)
+		ep->rx_msg(ep, m);
+}
+
+/* call-back for PFCP socket file descriptor */
+static int osmo_pfcp_fd_cb(struct osmo_fd *ofd, unsigned int what)
+{
+	int rc;
+	struct osmo_pfcp_endpoint *ep = ofd->data;
+
+	if (what & OSMO_FD_READ) {
+		struct osmo_sockaddr remote;
+		socklen_t remote_len = sizeof(remote);
+		struct msgb *msg = msgb_alloc_c(OTC_SELECT, OSMO_PFCP_MSGB_ALLOC_SIZE, "PFCP-rx");
+		if (!msg)
+			return -ENOMEM;
+
+		msg->l3h = msg->tail;
+		rc = recvfrom(ofd->fd, msg->tail, msgb_tailroom(msg), 0, (struct sockaddr *)&remote, &remote_len);
+		if (rc <= 0)
+			return -EIO;
+		msgb_put(msg, rc);
+
+		OSMO_ASSERT(ep->rx_msg);
+
+		/* This may be a bundle of PFCP messages. Parse and receive each message received, by shifting l4h
+		 * through the message bundle. */
+		msg->l4h = msg->l3h;
+		while (msgb_l4len(msg)) {
+			struct osmo_gtlv_load tlv;
+			struct osmo_pfcp_msg *m = osmo_pfcp_msg_alloc_rx(OTC_SELECT, &remote);
+			m->encoded = msg;
+
+			rc = osmo_pfcp_msg_decode_header(&tlv, m, msg);
+			if (rc < 0)
+				break;
+			msg->l4h += rc;
+
+			rc = osmo_pfcp_msg_decode_tlv(m, &tlv);
+			if (rc == 0) {
+				/* Populate message context to point at peer and session, if applicable */
+				if (ep->set_msg_ctx)
+					ep->set_msg_ctx(ep, m);
+
+				osmo_pfcp_endpoint_handle_rx(ep, m);
+			}
+			osmo_pfcp_msg_free(m);
+		}
+		msgb_free(msg);
+	}
+	return 0;
+}
+
+/*! bind a PFCP endpoint to its configured address (ep->cfg.local_addr).
+ * \return 0 on success, negative on error. */
+int osmo_pfcp_endpoint_bind(struct osmo_pfcp_endpoint *ep)
+{
+	int rc;
+	/* close the existing socket, if any */
+	osmo_pfcp_endpoint_close(ep);
+
+	if (!ep->rx_msg) {
+		LOGP(DLPFCP, LOGL_ERROR, "missing rx_msg cb at osmo_pfcp_endpoint\n");
+		return -EINVAL;
+	}
+
+	/* create the new socket, binding to configured local address */
+	ep->pfcp_fd.cb = osmo_pfcp_fd_cb;
+	ep->pfcp_fd.data = ep;
+	rc = osmo_sock_init_osa_ofd(&ep->pfcp_fd, SOCK_DGRAM, IPPROTO_UDP, &ep->cfg.local_addr, NULL, OSMO_SOCK_F_BIND);
+	if (rc < 0)
+		return rc;
+	return 0;
+}
+
+void osmo_pfcp_endpoint_close(struct osmo_pfcp_endpoint *ep)
+{
+	struct osmo_pfcp_queue_entry *qe;
+	while ((qe = llist_first_entry_or_null(&ep->retrans_queue, struct osmo_pfcp_queue_entry, entry))) {
+		osmo_pfcp_queue_del(qe);
+	}
+
+	if (ep->pfcp_fd.fd != -1) {
+		osmo_fd_unregister(&ep->pfcp_fd);
+		close(ep->pfcp_fd.fd);
+		ep->pfcp_fd.fd = -1;
+	}
+}
+
+void osmo_pfcp_endpoint_free(struct osmo_pfcp_endpoint **ep)
+{
+	if (!*ep)
+		return;
+	osmo_pfcp_endpoint_close(*ep);
+	talloc_free(*ep);
+	*ep = NULL;
+}
+
+/* Call osmo_pfcp_msg_invalidate_ctx(deleted_fi) on all queued osmo_pfcp_msg instances in the retrans_queue. */
+void osmo_pfcp_endpoint_invalidate_ctx(struct osmo_pfcp_endpoint *ep, struct osmo_fsm_inst *deleted_fi)
+{
+	struct osmo_pfcp_queue_entry *qe;
+	llist_for_each_entry(qe, &ep->retrans_queue, entry) {
+		osmo_pfcp_msg_invalidate_ctx(qe->m, deleted_fi);
+	}
+}

src/libosmo-pfcp/pfcp_heartbeat_fsm.c

@@ -0,0 +1,179 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/fsm.h>
+#include <osmocom/core/tdef.h>
+
+#include <osmocom/pfcp/pfcp_heartbeat_fsm.h>
+
+enum heartbeat_fsm_state {
+	HEARTBEAT_ST_IDLE,
+	HEARTBEAT_ST_WAIT_RESP,
+};
+
+static const struct value_string heartbeat_fsm_event_names[] = {
+	OSMO_VALUE_STRING(OSMO_PFCP_HEARTBEAT_EV_RX_RESP),
+	OSMO_VALUE_STRING(OSMO_PFCP_HEARTBEAT_EV_RX_REQ),
+	{}
+};
+
+static struct osmo_fsm heartbeat_fsm;
+
+static const struct osmo_tdef_state_timeout heartbeat_fsm_timeouts[32] = {
+	[HEARTBEAT_ST_IDLE] = { .T = 0 },
+	[HEARTBEAT_ST_WAIT_RESP] = { .T = 0 },
+};
+
+/* Transition to a state, using the T timer defined in heartbeat_fsm_timeouts.
+ * Assumes local variable fi exists. */
+#define heartbeat_state_chg(state) \
+	osmo_tdef_fsm_inst_state_chg(fi, state, \
+				     heartbeat_fsm_timeouts, \
+				     ((struct heartbeat*)(fi->priv))->tdefs, \
+				     5)
+
+struct heartbeat {
+	struct osmo_fsm_inst *fi;
+	uint32_t parent_event_tx_heartbeat;
+	struct osmo_tdef *tdefs;
+};
+
+struct osmo_fsm_inst *osmo_pfcp_heartbeat_alloc(struct osmo_fsm_inst *parent_fi,
+						uint32_t parent_event_tx_heartbeat, uint32_t parent_event_term,
+						struct osmo_tdef *tdefs)
+{
+	struct heartbeat *heartbeat;
+
+	struct osmo_fsm_inst *fi = osmo_fsm_inst_alloc_child(&heartbeat_fsm, parent_fi, parent_event_term);
+	OSMO_ASSERT(fi);
+
+	heartbeat = talloc(fi, struct heartbeat);
+	OSMO_ASSERT(heartbeat);
+	fi->priv = heartbeat;
+	*heartbeat = (struct heartbeat){
+		.fi = fi,
+		.parent_event_tx_heartbeat = parent_event_tx_heartbeat,
+		.tdefs = tdefs,
+	};
+
+	return heartbeat->fi;
+}
+
+static int heartbeat_fsm_timer_cb(struct osmo_fsm_inst *fi)
+{
+	/* Return 1 to terminate FSM instance, 0 to keep running */
+	switch (fi->state) {
+	case HEARTBEAT_ST_IDLE:
+		/* Time for another heartbeat request */
+		heartbeat_state_chg(HEARTBEAT_ST_WAIT_RESP);
+		return 0;
+	case HEARTBEAT_ST_WAIT_RESP:
+		/* Response did not arrive. Emit parent_event_term to the parent_fi. */
+		return 1;
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void pfcp_heartbeat_idle_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	switch (event) {
+
+	case OSMO_PFCP_HEARTBEAT_EV_RX_RESP:
+		/* A retransmission? */
+	case OSMO_PFCP_HEARTBEAT_EV_RX_REQ:
+		/* Either way, if we've seen any Heartbeat message from the peer, consider a Heartbeat to have succeeded
+		 * and restart the idle timeout. */
+		heartbeat_state_chg(HEARTBEAT_ST_IDLE);
+		return;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void pfcp_heartbeat_wait_resp_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+	struct heartbeat *heartbeat = fi->priv;
+	/* Let the caller's implementation figure out how exactly to encode the Heartbeat Request and send it.
+	 * Just dispatching events here. */
+	osmo_fsm_inst_dispatch(fi->proc.parent, heartbeat->parent_event_tx_heartbeat, NULL);
+}
+
+static void pfcp_heartbeat_wait_resp_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	switch (event) {
+
+	case OSMO_PFCP_HEARTBEAT_EV_RX_RESP:
+		heartbeat_state_chg(HEARTBEAT_ST_IDLE);
+		break;
+
+	case OSMO_PFCP_HEARTBEAT_EV_RX_REQ:
+		/* Doesn't matter whether the peer is also requesting, still waiting for the response to our own
+		 * request. */
+		break;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+#define S(x)    (1 << (x))
+
+static const struct osmo_fsm_state heartbeat_fsm_states[] = {
+	[HEARTBEAT_ST_IDLE] = {
+		.name = "idle",
+		.in_event_mask = 0
+			,
+		.out_state_mask = 0
+			| S(HEARTBEAT_ST_WAIT_RESP)
+			,
+		.action = pfcp_heartbeat_idle_action,
+	},
+	[HEARTBEAT_ST_WAIT_RESP] = {
+		.name = "wait_resp",
+		.in_event_mask = 0
+			| S(OSMO_PFCP_HEARTBEAT_EV_RX_RESP)
+			| S(OSMO_PFCP_HEARTBEAT_EV_RX_REQ)
+			,
+		.out_state_mask = 0
+			,
+		.onenter = pfcp_heartbeat_wait_resp_onenter,
+		.action = pfcp_heartbeat_wait_resp_action,
+	},
+};
+
+static struct osmo_fsm pfcp_heartbeat_fsm = {
+	.name = "pfcp_heartbeat",
+	.states = heartbeat_fsm_states,
+	.num_states = ARRAY_SIZE(heartbeat_fsm_states),
+	.log_subsys = DLPFCP,
+	.event_names = heartbeat_fsm_event_names,
+	.timer_cb = heartbeat_fsm_timer_cb,
+};
+
+static __attribute__((constructor)) void pfcp_heartbeat_fsm_register(void)
+{
+	OSMO_ASSERT(osmo_fsm_register(&pfcp_heartbeat_fsm) == 0);
+}

src/libosmo-pfcp/pfcp_ies_custom.c

@@ -34,12 +34,13 @@
 
 #include <osmocom/pfcp/pfcp_ies_custom.h>
 #include <osmocom/pfcp/pfcp_strs.h>
+#include <osmocom/pfcp/pfcp_msg.h>
 
 /* Assumes presence of local variable osmo_pfcp_msg *m. m->log_ctx may be NULL. */
 #define RETURN_ERROR(RC, FMT, ARGS...) \
 	do {\
 		OSMO_ASSERT(decoded_struct); \
-		LOGP(DLPFCP, LOGL_ERROR, FMT " (%d: %s)\n", ##ARGS, RC, \
+		OSMO_LOG_PFCP_MSG(OSMO_PFCP_MSG_FOR_IES(decoded_struct), LOGL_ERROR, FMT " (%d: %s)\n", ##ARGS, RC, \
 				  strerror((RC) > 0 ? (RC) : -(RC))); \
 		return RC; \
 	} while (0)
@@ -88,6 +89,31 @@ void osmo_pfcp_ie_f_seid_set(struct osmo_pfcp_ie_f_seid *f_seid, uint64_t seid,
 	osmo_pfcp_ip_addrs_set(&f_seid->ip_addr, remote_addr);
 }
 
+int osmo_pfcp_ie_f_seid_cmp(const struct osmo_pfcp_ie_f_seid *a, const struct osmo_pfcp_ie_f_seid *b)
+{
+	int rc;
+	if (a == b)
+		return 0;
+	if (!a)
+		return -1;
+	if (!b)
+		return 1;
+	/* It suffices if one of the IP addresses match */
+	if (a->ip_addr.v4_present && b->ip_addr.v4_present)
+		rc = osmo_sockaddr_cmp(&a->ip_addr.v4, &b->ip_addr.v4);
+	else if (a->ip_addr.v6_present && b->ip_addr.v6_present)
+		rc = osmo_sockaddr_cmp(&a->ip_addr.v6, &b->ip_addr.v6);
+	else
+		rc = (a->ip_addr.v4_present ? -1 : 1);
+	if (rc)
+		return rc;
+	if (a->seid < b->seid)
+		return -1;
+	if (a->seid > b->seid)
+		return 1;
+	return 0;
+}
+
 int osmo_pfcp_dec_cause(void *decoded_struct, void *decode_to, const struct osmo_gtlv_load *tlv)
 {
 	enum osmo_pfcp_cause *cause = decode_to;
@@ -317,9 +343,8 @@ int osmo_pfcp_enc_node_id(struct osmo_gtlv_put *tlv, const void *decoded_struct,
 	return 0;
 }
 
-int osmo_pfcp_enc_to_str_node_id(char *buf, size_t buflen, const void *encode_from)
+int osmo_pfcp_ie_node_id_to_str_buf(char *buf, size_t buflen, const struct osmo_pfcp_ie_node_id *node_id)
 {
-	const struct osmo_pfcp_ie_node_id *node_id = encode_from;
 	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
 
 	switch (node_id->type) {
@@ -343,6 +368,16 @@ int osmo_pfcp_enc_to_str_node_id(char *buf, size_t buflen, const void *encode_fr
 	return sb.chars_needed;
 }
 
+char *osmo_pfcp_ie_node_id_to_str_c(void *ctx, const struct osmo_pfcp_ie_node_id *node_id)
+{
+	OSMO_NAME_C_IMPL(ctx, 64, "ERROR", osmo_pfcp_ie_node_id_to_str_buf, node_id)
+}
+
+int osmo_pfcp_enc_to_str_node_id(char *buf, size_t buflen, const void *encode_from)
+{
+	return osmo_pfcp_ie_node_id_to_str_buf(buf, buflen, encode_from);
+}
+
 bool osmo_pfcp_bits_get(const uint8_t *bits, unsigned int bitpos)
 {
 	unsigned int bytenum = bitpos / 8;

src/libosmo-pfcp/pfcp_msg.c

@@ -0,0 +1,506 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <osmocom/core/endian.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/fsm.h>
+#include <osmocom/core/use_count.h>
+#include <osmocom/core/bitvec.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+#include <osmocom/gtlv/gtlv_dec_enc.h>
+
+/* Assumes presence of local variable osmo_pfcp_msg *m. m->log_ctx may be NULL. */
+#define RETURN_ERROR(RC, FMT, ARGS...) \
+	do {\
+		OSMO_ASSERT(m); \
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, FMT " (%d: %s)\n", ##ARGS, RC, strerror((RC) > 0 ? (RC) : -(RC))); \
+		return RC; \
+	} while (0)
+
+bool osmo_pfcp_msgtype_is_response(enum osmo_pfcp_message_type message_type)
+{
+	switch (message_type) {
+	case OSMO_PFCP_MSGT_HEARTBEAT_RESP:
+	case OSMO_PFCP_MSGT_PFD_MGMT_RESP:
+	case OSMO_PFCP_MSGT_ASSOC_SETUP_RESP:
+	case OSMO_PFCP_MSGT_ASSOC_UPDATE_RESP:
+	case OSMO_PFCP_MSGT_ASSOC_RELEASE_RESP:
+	case OSMO_PFCP_MSGT_VERSION_NOT_SUPP_RESP:
+	case OSMO_PFCP_MSGT_NODE_REPORT_RESP:
+	case OSMO_PFCP_MSGT_SESSION_SET_DEL_RESP:
+	case OSMO_PFCP_MSGT_SESSION_EST_RESP:
+	case OSMO_PFCP_MSGT_SESSION_MOD_RESP:
+	case OSMO_PFCP_MSGT_SESSION_DEL_RESP:
+	case OSMO_PFCP_MSGT_SESSION_REP_RESP:
+		return true;
+	default:
+		return false;
+	}
+}
+
+struct osmo_pfcp_header_common {
+	uint8_t seid_present:1,
+		message_priority_present:1,
+		follow_on:1,
+		spare:2,
+		version:3;
+	uint8_t message_type;
+	uint16_t message_length;
+} __attribute__ ((packed));
+
+struct osmo_pfcp_header_no_seid {
+	struct osmo_pfcp_header_common c;
+	uint8_t sequence_nr[3];
+	uint8_t spare;
+} __attribute__ ((packed));
+
+struct osmo_pfcp_header_seid {
+	struct osmo_pfcp_header_common c;
+	uint64_t session_endpoint_identifier;
+	uint8_t sequence_nr[3];
+	uint8_t message_priority:4,
+		spare:4;
+} __attribute__ ((packed));
+
+int osmo_pfcp_ie_node_id_from_osmo_sockaddr(struct osmo_pfcp_ie_node_id *node_id, const struct osmo_sockaddr *os)
+{
+	switch (os->u.sa.sa_family) {
+	case AF_INET:
+		node_id->type = OSMO_PFCP_NODE_ID_T_IPV4;
+		break;
+	case AF_INET6:
+		node_id->type = OSMO_PFCP_NODE_ID_T_IPV6;
+		break;
+	default:
+		return -ENOTSUP;
+	}
+	node_id->ip = *os;
+	return 0;
+}
+
+int osmo_pfcp_ie_node_id_to_osmo_sockaddr(const struct osmo_pfcp_ie_node_id *node_id, struct osmo_sockaddr *os)
+{
+	switch (node_id->type) {
+	case OSMO_PFCP_NODE_ID_T_IPV4:
+		if (os->u.sa.sa_family != AF_INET)
+			return -EINVAL;
+		break;
+	case OSMO_PFCP_NODE_ID_T_IPV6:
+		if (os->u.sa.sa_family != AF_INET6)
+			return -EINVAL;
+		break;
+	default:
+		return -ENOTSUP;
+	}
+	*os = node_id->ip;
+	return 0;
+}
+
+static int pfcp_header_set_message_length(struct osmo_pfcp_header_common *c, unsigned int header_and_payload_len)
+{
+	if (header_and_payload_len < sizeof(struct osmo_pfcp_header_common))
+		return -EINVAL;
+	if (header_and_payload_len - sizeof(struct osmo_pfcp_header_common) > UINT16_MAX)
+		return -EMSGSIZE;
+	osmo_store16be(header_and_payload_len - sizeof(struct osmo_pfcp_header_common),
+		       &c->message_length);
+	return 0;
+}
+
+static unsigned int pfcp_header_get_message_length(const struct osmo_pfcp_header_common *c)
+{
+	unsigned int len = osmo_load16be(&c->message_length);
+	return len + sizeof(struct osmo_pfcp_header_common);
+}
+
+/*! Encode and append the given PFCP header to a msgb.
+ * \param[out] msg message buffer to which to push the header.
+ * \param[in] to-be-encoded representation of PFCP header. */
+static int enc_pfcp_header(struct msgb *msg, const struct osmo_pfcp_msg *m)
+{
+	const struct osmo_pfcp_header_parsed *parsed = &m->h;
+	struct osmo_pfcp_header_seid *h_seid;
+	struct osmo_pfcp_header_no_seid *h_no_seid;
+	struct osmo_pfcp_header_common *c;
+	int rc;
+
+	if (!parsed->seid_present) {
+		h_no_seid = (struct osmo_pfcp_header_no_seid *)msgb_put(msg, sizeof(struct osmo_pfcp_header_no_seid));
+		c = &h_no_seid->c;
+	} else {
+		h_seid = (struct osmo_pfcp_header_seid *)msgb_put(msg, sizeof(struct osmo_pfcp_header_seid));
+		c = &h_seid->c;
+	}
+
+	*c = (struct osmo_pfcp_header_common){
+		.version = parsed->version,
+		.message_priority_present = (parsed->priority_present ? 1 : 0),
+		.seid_present = (parsed->seid_present ? 1 : 0),
+		.message_type = parsed->message_type,
+	};
+
+	/* Put a preliminary length reflecting only the header, until it is updated later in osmo_pfcp_msg_encode(). */
+	rc = pfcp_header_set_message_length(c, parsed->seid_present ? sizeof(struct osmo_pfcp_header_seid)
+								    : sizeof(struct osmo_pfcp_header_no_seid));
+	if (rc)
+		RETURN_ERROR(rc, "Problem with PFCP message length");
+
+	if (!parsed->seid_present) {
+		osmo_store32be_ext(parsed->sequence_nr, h_no_seid->sequence_nr, 3);
+		if (parsed->priority_present)
+			RETURN_ERROR(-EINVAL, "Message Priority can only be present when the SEID is also present");
+	} else {
+		osmo_store64be(parsed->seid, &h_seid->session_endpoint_identifier);
+		osmo_store32be_ext(parsed->sequence_nr, h_seid->sequence_nr, 3);
+		if (parsed->priority_present)
+			h_seid->message_priority = parsed->priority;
+	}
+
+	return 0;
+}
+
+static void osmo_pfcp_msg_set_memb_ofs(struct osmo_pfcp_msg *m)
+{
+	const struct osmo_gtlv_coding *mc = osmo_pfcp_get_msg_coding(m->h.message_type);
+	m->ofs_cause = 0;
+	m->ofs_node_id = 0;
+	if (!mc)
+		return;
+	for (; !osmo_gtlv_coding_end(mc) && (m->ofs_cause == 0 || m->ofs_node_id == 0); mc++) {
+		if (mc->ti.tag == OSMO_PFCP_IEI_CAUSE)
+			m->ofs_cause = offsetof(struct osmo_pfcp_msg, ies) + mc->memb_ofs;
+		if (mc->ti.tag == OSMO_PFCP_IEI_NODE_ID)
+			m->ofs_node_id = offsetof(struct osmo_pfcp_msg, ies) + mc->memb_ofs;
+	}
+};
+
+/*! Decode a single PFCP message's header.
+ *
+ * If msg->l4h is non-NULL, decode at msgb_l4(msg). If l4h is NULL, decode at msgb_l3(msg).
+ * In case of bundled PFCP messages, decode only one message and return the offset to the next message in the buffer.
+ * Hence, to decode a message bundle, increment msg->l4h until all messages are decoded:
+ *
+ *   msg->l4h = msg->l3h;
+ *   while (msgb_l4len(msg)) {
+ *           struct osmo_pfcp_msg m;
+ *           struct osmo_gtlv_load tlv;
+ *           int rc;
+ *           rc = osmo_pfcp_msg_decode_header(&tlv, &m, msg);
+ *           if (rc < 0)
+ *                  error();
+ *           msg->l4h += rc;
+ *
+ *           if (osmo_pfcp_msg_decode_tlv(&m, &tlv))
+ *                  error();
+ *           handle(&m);
+ *   }
+ *
+ * \param[out] tlv  Return TLV start pointer and length in tlv->src.*.
+ * \param[inout] m  Place the decoded data in m->h; use m->ctx.* as logging context.
+ * \param[in] msg  PFCP data to parse, possibly containing a PFCP message bundle.
+ * \return total single PFCP message length (<= data_len) on success, negative on error.
+ */
+int osmo_pfcp_msg_decode_header(struct osmo_gtlv_load *tlv, struct osmo_pfcp_msg *m,
+				const struct msgb *msg)
+{
+	struct osmo_pfcp_header_parsed *parsed = &m->h;
+	const uint8_t *pfcp_msg_data;
+	unsigned int pfcp_msg_data_len;
+	unsigned int header_len;
+	unsigned int message_length;
+	const struct osmo_pfcp_header_common *c;
+
+	if (msg->l4h) {
+		pfcp_msg_data = msgb_l4(msg);
+		pfcp_msg_data_len = msgb_l4len(msg);
+	} else {
+		pfcp_msg_data = msgb_l3(msg);
+		pfcp_msg_data_len = msgb_l3len(msg);
+	}
+
+	if (!pfcp_msg_data || !pfcp_msg_data_len)
+		RETURN_ERROR(-EINVAL, "No Layer 3 data in this message buffer");
+
+	if (pfcp_msg_data_len < sizeof(struct osmo_pfcp_header_common))
+		RETURN_ERROR(-EINVAL, "Message too short for PFCP header: %u", pfcp_msg_data_len);
+
+	c = (void *)pfcp_msg_data;
+
+	header_len = (c->seid_present ? sizeof(struct osmo_pfcp_header_seid) : sizeof(struct osmo_pfcp_header_no_seid));
+	if (pfcp_msg_data_len < header_len)
+		RETURN_ERROR(-EINVAL, "Message too short for PFCP header: %u", pfcp_msg_data_len);
+
+	*parsed = (struct osmo_pfcp_header_parsed){
+		.version = c->version,
+		.priority_present = (bool)c->message_priority_present,
+		.seid_present = (bool)c->seid_present,
+		.message_type = c->message_type,
+	};
+
+	m->is_response = osmo_pfcp_msgtype_is_response(parsed->message_type);
+	osmo_pfcp_msg_set_memb_ofs(m);
+
+	message_length = pfcp_header_get_message_length(c);
+	if (message_length > pfcp_msg_data_len)
+		RETURN_ERROR(-EMSGSIZE,
+			     "The header's indicated total message length %u is larger than the received data %u",
+			     message_length, pfcp_msg_data_len);
+
+	/* T16L16V payload data and len */
+	*tlv = (struct osmo_gtlv_load){
+		.cfg = &osmo_t16l16v_cfg,
+		.src = {
+			.data = pfcp_msg_data + header_len,
+			.len = message_length - header_len,
+		},
+	};
+
+	if (c->follow_on) {
+		/* Another PFCP message should follow */
+		if (pfcp_msg_data_len - message_length < sizeof(struct osmo_pfcp_header_common))
+			OSMO_LOG_PFCP_MSG(m, LOGL_INFO,
+					  "PFCP message indicates more messages should follow in the bundle,"
+					  " but remaining size %u is too short", pfcp_msg_data_len - message_length);
+	} else {
+		/* No more PFCP message should follow in the bundle */
+		if (pfcp_msg_data_len > message_length)
+			OSMO_LOG_PFCP_MSG(m, LOGL_INFO, "Surplus data after PFCP message: %u",
+					  pfcp_msg_data_len - message_length);
+	}
+
+	if (!parsed->seid_present) {
+		const struct osmo_pfcp_header_no_seid *h_no_seid = (void *)pfcp_msg_data;
+		parsed->sequence_nr = osmo_load32be_ext_2(h_no_seid->sequence_nr, 3);
+		if (parsed->priority_present)
+			RETURN_ERROR(-EINVAL, "Message Priority can only be present when the SEID is also present");
+	} else {
+		const struct osmo_pfcp_header_seid *h_seid = (void *)pfcp_msg_data;
+		parsed->seid = osmo_load64be(&h_seid->session_endpoint_identifier);
+		parsed->sequence_nr = osmo_load32be_ext_2(h_seid->sequence_nr, 3);
+		if (parsed->priority_present)
+			parsed->priority = h_seid->message_priority;
+	}
+
+	return message_length;
+}
+
+void osmo_pfcp_msg_err_cb(void *data, void *decoded_struct, const char *file, int line, const char *fmt, ...)
+{
+	va_list ap;
+	if (log_check_level(DLPFCP, LOGL_ERROR)) {
+		char *errmsg;
+
+		va_start(ap, fmt);
+		errmsg = talloc_vasprintf(OTC_SELECT, fmt, ap);
+		va_end(ap);
+		OSMO_LOG_PFCP_MSG_SRC((struct osmo_pfcp_msg *)data, LOGL_ERROR, file, line, "%s", errmsg);
+	}
+}
+
+int osmo_pfcp_msg_decode_tlv(struct osmo_pfcp_msg *m, struct osmo_gtlv_load *tlv)
+{
+	return osmo_pfcp_ies_decode(&m->ies, tlv, false, m->h.message_type, osmo_pfcp_msg_err_cb, m, osmo_pfcp_iei_strs);
+}
+
+static int osmo_pfcp_msg_encode_tlv(struct msgb *msg, const struct osmo_pfcp_msg *m)
+{
+	struct osmo_gtlv_put tlv = {
+		.cfg = &osmo_t16l16v_cfg,
+		.dst = msg,
+	};
+	return osmo_pfcp_ies_encode(&tlv, &m->ies, m->h.message_type, osmo_pfcp_msg_err_cb, (void *)m, osmo_pfcp_iei_strs);
+}
+
+/* Append the encoded PFCP message to the message buffer.
+ *
+ * If msg->l3h is NULL, point it at the start of the encoded message.
+ * Always point msg->l4h at the start of the newly encoded message.
+ * Hence, in a message bundle, msg->l3h always points at the first PFCP message, while msg->l4h always points at the
+ * last PFCP message.
+ *
+ * When adding a PFCP message to a bundle, set the Follow On (FO) flag of the previously last message to 1, and of the
+ * newly encoded, now last message as 0.
+ *
+ * To log errors to a specific osmo_fsm_inst, point m->log_ctx to that instance before calling this function. Otherwise
+ * set log_ctx = NULL.
+ *
+ * \return 0 on success, negative on error. */
+int osmo_pfcp_msg_encode(struct msgb *msg, const struct osmo_pfcp_msg *m)
+{
+	struct osmo_pfcp_header_common *c;
+	int rc;
+
+	/* Forming a bundle? If yes, set the Follow On flag of the currently last message to 1 */
+	if (msg->l4h && msgb_l4len(msg)) {
+		c = msgb_l4(msg);
+		c->follow_on = 1;
+	}
+	/* Make sure l3h points at the first PFCP message in a message bundle */
+	if (!msg->l3h)
+		msg->l3h = msg->tail;
+	/* Make sure l4h points at the last PFCP message in a message bundle */
+	msg->l4h = msg->tail;
+	c = (void *)msg->tail;
+
+	rc = enc_pfcp_header(msg, m);
+	if (rc)
+		return rc;
+
+	rc = osmo_pfcp_msg_encode_tlv(msg, m);
+	if (rc)
+		return rc;
+
+	/* Update the header's message_length */
+	rc = pfcp_header_set_message_length(c, msgb_l4len(msg));
+	if (rc)
+		RETURN_ERROR(rc, "Problem with PFCP message length");
+	return 0;
+}
+
+static int osmo_pfcp_msg_destructor(struct osmo_pfcp_msg *m);
+
+static struct osmo_pfcp_msg *_osmo_pfcp_msg_alloc(void *ctx, const struct osmo_sockaddr *remote_addr)
+{
+	struct osmo_pfcp_msg *m = talloc(ctx, struct osmo_pfcp_msg);
+	*m = (struct osmo_pfcp_msg){
+		.remote_addr = *remote_addr,
+		.h = {
+			.version = 1,
+		},
+	};
+	talloc_set_destructor(m, osmo_pfcp_msg_destructor);
+	return m;
+}
+
+struct osmo_pfcp_msg *osmo_pfcp_msg_alloc_rx(void *ctx, const struct osmo_sockaddr *remote_addr)
+{
+	struct osmo_pfcp_msg *rx = _osmo_pfcp_msg_alloc(ctx, remote_addr);
+	rx->rx = true;
+	return rx;
+}
+
+struct osmo_pfcp_msg *osmo_pfcp_msg_alloc_tx(void *ctx, const struct osmo_sockaddr *remote_addr,
+					     const struct osmo_pfcp_ie_node_id *node_id,
+					     const struct osmo_pfcp_msg *in_reply_to,
+					     enum osmo_pfcp_message_type msg_type)
+{
+	struct osmo_pfcp_msg *tx;
+	if (!remote_addr && in_reply_to)
+		remote_addr = &in_reply_to->remote_addr;
+	OSMO_ASSERT(remote_addr);
+	tx = _osmo_pfcp_msg_alloc(ctx, remote_addr);
+	tx->is_response = osmo_pfcp_msgtype_is_response(msg_type);
+	tx->h.message_type = msg_type;
+	if (in_reply_to)
+		tx->h.sequence_nr = in_reply_to->h.sequence_nr;
+	osmo_pfcp_msg_set_memb_ofs(tx);
+
+	/* Write the local node id data to the correct tx->ies.* member. */
+	if (node_id) {
+		struct osmo_pfcp_ie_node_id *tx_node_id = osmo_pfcp_msg_node_id(tx);
+		if (tx_node_id)
+			*tx_node_id = *node_id;
+	}
+	return tx;
+}
+
+static int osmo_pfcp_msg_destructor(struct osmo_pfcp_msg *m)
+{
+	OSMO_LOG_PFCP_MSG(m, LOGL_DEBUG, "discarding\n");
+	if (m->ctx.session_use_count)
+		osmo_use_count_get_put(m->ctx.session_use_count, m->ctx.session_use_token, -1);
+	m->ctx.session_fi = NULL;
+	m->ctx.session_use_count = NULL;
+	m->ctx.session_use_token = NULL;
+
+	if (m->ctx.peer_use_count)
+		osmo_use_count_get_put(m->ctx.peer_use_count, m->ctx.peer_use_token, -1);
+	m->ctx.peer_fi = NULL;
+	m->ctx.peer_use_count = NULL;
+	m->ctx.peer_use_token = NULL;
+	return 0;
+}
+
+void osmo_pfcp_msg_free(struct osmo_pfcp_msg *m)
+{
+	if (!m)
+		return;
+	talloc_free(m);
+}
+
+int osmo_pfcp_ip_addrs_set(struct osmo_pfcp_ip_addrs *dst, const struct osmo_sockaddr *addr)
+{
+	switch (addr->u.sas.ss_family) {
+	case AF_INET:
+		dst->v4_present = true;
+		dst->v4 = *addr;
+		return 0;
+	case AF_INET6:
+		dst->v6_present = true;
+		dst->v6 = *addr;
+		return 0;
+	default:
+		return -ENOTSUP;
+	}
+}
+
+/* If a osmo_fsm_inst placed in m->ctx deallocates before the osmo_pfcp_msg, call this function, to make sure to avoid
+ * use after free. Alternatively use m->ctx.*_use_count to make sure the FSM inst does not deallocate before the
+ * osmo_pfcp_msg is discarded from the resend queue. */
+void osmo_pfcp_msg_invalidate_ctx(struct osmo_pfcp_msg *m, struct osmo_fsm_inst *deleted_fi)
+{
+	if (m->ctx.session_fi == deleted_fi) {
+		m->ctx.session_fi = NULL;
+		m->ctx.session_use_count = NULL;
+		m->ctx.session_use_token = NULL;
+	}
+	if (m->ctx.peer_fi == deleted_fi) {
+		m->ctx.peer_fi = NULL;
+		m->ctx.peer_use_count = NULL;
+		m->ctx.peer_use_token = NULL;
+	}
+}
+
+int osmo_pfcp_msg_to_str_buf(char *buf, size_t buflen, const struct osmo_pfcp_msg *m)
+{
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+	OSMO_STRBUF_PRINTF(sb, "PFCPv%u %s hdr={seq=%u", m->h.version, osmo_pfcp_message_type_str(m->h.message_type),
+			   m->h.sequence_nr);
+	if (m->h.priority_present)
+		OSMO_STRBUF_PRINTF(sb, " prio=%u", m->h.priority);
+	if (m->h.seid_present)
+		OSMO_STRBUF_PRINTF(sb, " SEID=0x%"PRIx64, m->h.seid);
+	OSMO_STRBUF_PRINTF(sb, "} ies={");
+	OSMO_STRBUF_APPEND(sb, osmo_pfcp_ies_encode_to_str, &m->ies, m->h.message_type, osmo_pfcp_iei_strs);
+	OSMO_STRBUF_PRINTF(sb, " }");
+	return sb.chars_needed;
+}
+
+char *osmo_pfcp_msg_to_str_c(void *ctx, const struct osmo_pfcp_msg *m)
+{
+	OSMO_NAME_C_IMPL(ctx, 256, "ERROR", osmo_pfcp_msg_to_str_buf, m)
+}

src/osmo-pfcp-tool/Makefile.am

@@ -0,0 +1,41 @@
+AM_CPPFLAGS = \
+	$(all_includes) \
+	-I$(top_srcdir)/include \
+	-I$(top_builddir)/include \
+	-I$(top_builddir) \
+	$(NULL)
+
+AM_CFLAGS = \
+	-Wall \
+	$(LIBOSMOCORE_CFLAGS) \
+	$(LIBOSMOVTY_CFLAGS) \
+	$(LIBOSMOCTRL_CFLAGS) \
+	$(COVERAGE_CFLAGS) \
+	$(NULL)
+
+AM_LDFLAGS = \
+	$(COVERAGE_LDFLAGS) \
+	$(NULL)
+
+noinst_HEADERS = \
+	pfcp_tool.h \
+	$(NULL)
+
+bin_PROGRAMS = \
+	osmo-pfcp-tool \
+	$(NULL)
+
+osmo_pfcp_tool_SOURCES = \
+	osmo_pfcp_tool_main.c \
+	pfcp_tool.c \
+	pfcp_tool_vty.c \
+	$(NULL)
+
+osmo_pfcp_tool_LDADD = \
+	$(top_builddir)/src/libosmo-pfcp/libosmo-pfcp.la \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
+	$(LIBOSMOCORE_LIBS) \
+	$(LIBOSMOVTY_LIBS) \
+	$(LIBOSMOCTRL_LIBS) \
+	$(COVERAGE_LDFLAGS) \
+	$(NULL)

src/osmo-pfcp-tool/osmo_pfcp_tool_main.c

@@ -0,0 +1,371 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <osmocom/core/application.h>
+#include <osmocom/core/signal.h>
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/fsm.h>
+#include <osmocom/core/stats.h>
+#include <osmocom/core/msgb.h>
+#include <osmocom/vty/logging.h>
+#include <osmocom/vty/command.h>
+#include <osmocom/vty/misc.h>
+#include <osmocom/vty/cpu_sched_vty.h>
+#include <osmocom/vty/telnet_interface.h>
+#include <osmocom/vty/ports.h>
+#include <osmocom/vty/tdef_vty.h>
+#include <osmocom/ctrl/control_if.h>
+#include <osmocom/ctrl/control_vty.h>
+#include <osmocom/ctrl/ports.h>
+#include <osmocom/core/sockaddr_str.h>
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
+
+#include "pfcp_tool.h"
+
+#define _GNU_SOURCE
+#include <getopt.h>
+
+/* build switches from the configure script */
+#include "config.h"
+
+#include <signal.h>
+#include <stdio.h>
+#include <string.h>
+
+extern void *tall_vty_ctx;
+
+void *tall_pfcp_tool_ctx = NULL;
+static int quit = 0;
+
+static struct {
+	const char *config_file;
+	int daemonize;
+	enum vty_ref_gen_mode vty_ref_gen_mode;
+	const char *command_file;
+} pfcp_tool_cmdline_config = {
+	.config_file = "osmo-pfcp-tool.cfg",
+	.vty_ref_gen_mode = VTY_REF_GEN_MODE_DEFAULT,
+};
+
+static void print_usage()
+{
+	printf("Usage: osmo-pfcp-tool [command-file.vty]\n       telnet localhost %d\n", OSMO_VTY_PORT_PFCP_TOOL);
+}
+
+static void print_help()
+{
+	const struct value_string *vty_ref_gen_mode_name;
+
+	printf("Some useful options:\n");
+	printf("  -h --help                  This text.\n");
+	printf("  -D --daemonize             Fork the process into a background daemon.\n");
+	printf("  -c --config-file filename  The config file to use, for logging etc.\n");
+	printf("  -V --version               Print the version of OsmoMSC.\n");
+
+	printf("\nVTY reference generation:\n");
+	printf("     --vty-ref-xml           Generate the VTY reference XML output and exit.\n");
+	printf("     --vty-ref-mode MODE     Mode for --vty-ref-xml:\n");
+	/* List all VTY ref gen modes */
+	for (vty_ref_gen_mode_name = vty_ref_gen_mode_names; vty_ref_gen_mode_name->str; vty_ref_gen_mode_name++)
+		printf("                    %s: %s\n",
+		       vty_ref_gen_mode_name->str,
+		       get_value_string(vty_ref_gen_mode_desc, vty_ref_gen_mode_name->value));
+}
+
+static void handle_long_options(const char *prog_name, const int long_option)
+{
+	switch (long_option) {
+	case 1:
+		pfcp_tool_cmdline_config.vty_ref_gen_mode = get_string_value(vty_ref_gen_mode_names, optarg);
+		if (pfcp_tool_cmdline_config.vty_ref_gen_mode < 0) {
+			fprintf(stderr, "%s: Unknown VTY reference generation mode: '%s'\n", prog_name, optarg);
+			exit(2);
+		}
+		break;
+	case 2:
+		fprintf(stderr, "Generating the VTY reference in mode '%s' (%s)\n",
+			get_value_string(vty_ref_gen_mode_names, pfcp_tool_cmdline_config.vty_ref_gen_mode),
+			get_value_string(vty_ref_gen_mode_desc, pfcp_tool_cmdline_config.vty_ref_gen_mode));
+		vty_dump_xml_ref_mode(stdout, pfcp_tool_cmdline_config.vty_ref_gen_mode);
+		exit(0);
+	default:
+		fprintf(stderr, "%s: error parsing cmdline options\n", prog_name);
+		exit(2);
+	}
+}
+
+static void handle_options(int argc, char **argv)
+{
+	while (1) {
+		int option_index = 0, c;
+		static int long_option = 0;
+		static struct option long_options[] = {
+			{"help", 0, 0, 'h'},
+			{"daemonize", 0, 0, 'D'},
+			{"config-file", 1, 0, 'c'},
+			{"version", 0, 0, 'V' },
+			{"vty-ref-mode", 1, &long_option, 1},
+			{"vty-ref-xml", 0, &long_option, 2},
+			{0, 0, 0, 0}
+		};
+
+		c = getopt_long(argc, argv, "hDc:V", long_options, &option_index);
+		if (c == -1)
+			break;
+
+		switch (c) {
+		case 'h':
+			print_usage();
+			print_help();
+			exit(0);
+		case 0:
+			handle_long_options(argv[0], long_option);
+			break;
+		case 'D':
+			pfcp_tool_cmdline_config.daemonize = 1;
+			break;
+		case 'c':
+			pfcp_tool_cmdline_config.config_file = optarg;
+			break;
+		case 'V':
+			print_version(1);
+			exit(0);
+			break;
+		default:
+			/* catch unknown options *as well as* missing arguments. */
+			fprintf(stderr, "%s: Error in command line options. Exiting.\n", argv[0]);
+			exit(-1);
+		}
+	}
+
+	if (argc > optind) {
+		pfcp_tool_cmdline_config.command_file = argv[optind];
+		optind++;
+	}
+
+	if (argc > optind) {
+		fprintf(stderr, "%s: Unsupported positional arguments on command line\n", argv[optind]);
+		exit(2);
+	}
+}
+
+static void signal_handler(int signum)
+{
+	fprintf(stdout, "signal %u received\n", signum);
+
+	switch (signum) {
+	case SIGINT:
+	case SIGTERM:
+		LOGP(DLGLOBAL, LOGL_NOTICE, "Terminating due to signal %d\n", signum);
+		quit++;
+		break;
+	case SIGABRT:
+		osmo_generate_backtrace();
+		/* in case of abort, we want to obtain a talloc report and
+		 * then run default SIGABRT handler, who will generate coredump
+		 * and abort the process. abort() should do this for us after we
+		 * return, but program wouldn't exit if an external SIGABRT is
+		 * received.
+		 */
+		talloc_report(tall_vty_ctx, stderr);
+		talloc_report_full(tall_pfcp_tool_ctx, stderr);
+		signal(SIGABRT, SIG_DFL);
+		raise(SIGABRT);
+		break;
+	case SIGUSR1:
+		talloc_report(tall_vty_ctx, stderr);
+		talloc_report_full(tall_pfcp_tool_ctx, stderr);
+		break;
+	case SIGUSR2:
+		talloc_report_full(tall_vty_ctx, stderr);
+		break;
+	default:
+		break;
+	}
+}
+
+static const char * const osmo_pfcp_tool_copyright =
+	"OsmoPFCPTool - Osmocom Packet Forwarding Control Protocol tool for testing\r\n"
+	"Copyright (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>\r\n"
+	"License AGPLv3+: GNU AGPL version 3 or later <http://gnu.org/licenses/agpl-3.0.html>\r\n"
+	"This is free software: you are free to change and redistribute it.\r\n"
+	"There is NO WARRANTY, to the extent permitted by law.\r\n";
+
+static struct vty_app_info pfcp_tool_vty_app_info = {
+	.name = "osmo-pfcp-tool",
+	.version = PACKAGE_VERSION,
+	.copyright = osmo_pfcp_tool_copyright,
+};
+
+static const struct log_info_cat pfcp_tool_default_categories[] = {
+};
+
+const struct log_info log_info = {
+	.cat = pfcp_tool_default_categories,
+	.num_cat = ARRAY_SIZE(pfcp_tool_default_categories),
+};
+
+int pfcp_tool_mainloop()
+{
+	log_reset_context();
+	osmo_select_main_ctx(0);
+
+	/* If the user hits Ctrl-C the third time, just terminate immediately. */
+	if (quit >= 3)
+		return 1;
+
+	/* Has SIGTERM been received (and not yet been handled)? */
+	if (quit && !osmo_select_shutdown_requested()) {
+		osmo_signal_dispatch(SS_L_GLOBAL, S_L_GLOBAL_SHUTDOWN, NULL);
+
+		/* Request write-only mode in osmo_select_main_ctx() */
+		osmo_select_shutdown_request();
+		/* continue the main select loop until all write queues are serviced. */
+	}
+	return 0;
+}
+
+int main(int argc, char **argv)
+{
+	int rc;
+
+	/* Track the use of talloc NULL memory contexts */
+	talloc_enable_null_tracking();
+
+	osmo_fsm_set_dealloc_ctx(OTC_SELECT);
+
+	tall_pfcp_tool_ctx = talloc_named_const(NULL, 1, "osmo-pfcp-tool");
+	pfcp_tool_vty_app_info.tall_ctx = tall_pfcp_tool_ctx;
+
+	msgb_talloc_ctx_init(tall_pfcp_tool_ctx, 0);
+	osmo_signal_talloc_ctx_init(tall_pfcp_tool_ctx);
+
+	osmo_init_logging2(tall_pfcp_tool_ctx, &log_info);
+	log_set_print_category_hex(osmo_stderr_target, 0);
+	log_set_print_category(osmo_stderr_target, 1);
+	log_set_print_level(osmo_stderr_target, 1);
+	log_set_print_filename2(osmo_stderr_target, LOG_FILENAME_BASENAME);
+	log_set_print_filename_pos(osmo_stderr_target, LOG_FILENAME_POS_LINE_END);
+	log_set_print_extended_timestamp(osmo_stderr_target, 1);
+
+	osmo_fsm_log_timeouts(true);
+	osmo_fsm_log_addr(true);
+
+	osmo_stats_init(tall_pfcp_tool_ctx);
+
+	g_pfcp_tool_alloc(tall_pfcp_tool_ctx);
+
+	/* For --version, vty_init() must be called before handling options */
+	vty_init(&pfcp_tool_vty_app_info);
+
+	ctrl_vty_init(tall_pfcp_tool_ctx);
+	logging_vty_add_cmds();
+	osmo_talloc_vty_add_cmds();
+	osmo_cpu_sched_vty_init(tall_pfcp_tool_ctx);
+	osmo_fsm_vty_add_cmds();
+	osmo_tdef_vty_groups_init(CONFIG_NODE, g_pfcp_tool_tdef_groups);
+
+	pfcp_tool_vty_init_cfg();
+
+	/* Parse options */
+	handle_options(argc, argv);
+
+	if (pfcp_tool_cmdline_config.config_file) {
+		rc = vty_read_config_file(pfcp_tool_cmdline_config.config_file, NULL);
+		if (rc < 0) {
+			LOGP(DLGLOBAL, LOGL_ERROR, "Failed to parse the config file: '%s'\n",
+			     pfcp_tool_cmdline_config.config_file);
+		}
+	}
+
+	/* start telnet, after reading config for vty_get_bind_addr() */
+	rc = telnet_init_dynif(tall_pfcp_tool_ctx, &g_pfcp_tool, vty_get_bind_addr(), OSMO_VTY_PORT_PFCP_TOOL);
+	if (rc < 0)
+		return 2;
+
+	/* start control interface, after reading config for ctrl_vty_get_bind_addr() */
+	g_pfcp_tool->ctrl = ctrl_interface_setup_dynip(g_pfcp_tool, ctrl_vty_get_bind_addr(), OSMO_CTRL_PORT_PFCP_TOOL, NULL);
+	if (!g_pfcp_tool->ctrl) {
+		fprintf(stderr, "Failed to initialize control interface. Exiting.\n");
+		return -1;
+	}
+
+	signal(SIGINT, &signal_handler);
+	signal(SIGTERM, &signal_handler);
+	signal(SIGABRT, &signal_handler);
+	signal(SIGUSR1, &signal_handler);
+	signal(SIGUSR2, &signal_handler);
+	osmo_init_ignore_signals();
+
+	if (pfcp_tool_cmdline_config.daemonize) {
+		rc = osmo_daemonize();
+		if (rc < 0) {
+			perror("Error during daemonize");
+			return 6;
+		}
+	}
+
+	pfcp_tool_mainloop();
+
+	pfcp_tool_vty_init_cmds();
+
+	if (pfcp_tool_cmdline_config.command_file) {
+		printf("Reading '%s'\n", pfcp_tool_cmdline_config.command_file);
+		rc = vty_read_config_file(pfcp_tool_cmdline_config.command_file, NULL);
+		if (rc < 0) {
+			LOGP(DLGLOBAL, LOGL_FATAL, "Failed to parse the command file: '%s'\n",
+			     pfcp_tool_cmdline_config.command_file);
+			return 1;
+		}
+		printf("Done reading '%s', waiting for retransmission queue...\n",
+		       pfcp_tool_cmdline_config.command_file);
+		do {
+			if (pfcp_tool_mainloop())
+				break;
+		} while (!llist_empty(&g_pfcp_tool->ep->retrans_queue));
+		printf("Done\n");
+	} else {
+		printf("Listening for commands on VTY...\n");
+		do {
+			if (pfcp_tool_mainloop())
+				break;
+		} while (!osmo_select_shutdown_done());
+	}
+
+	osmo_pfcp_endpoint_free(&g_pfcp_tool->ep);
+
+	log_fini();
+
+	/* Report the heap state of talloc contexts, then free, so both ASAN and Valgrind are happy... */
+	//talloc_report_full(tall_pfcp_tool_ctx, stderr);
+	talloc_free(tall_pfcp_tool_ctx);
+
+	//talloc_report_full(tall_vty_ctx, stderr);
+	talloc_free(tall_vty_ctx);
+
+	//talloc_report_full(NULL, stderr);
+	talloc_disable_null_tracking();
+	return 0;
+}

src/osmo-pfcp-tool/pfcp_tool.c

@@ -0,0 +1,176 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/talloc.h>
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
+
+#include "pfcp_tool.h"
+
+struct g_pfcp_tool *g_pfcp_tool = NULL;
+
+struct osmo_tdef_group g_pfcp_tool_tdef_groups[] = {
+	{ .name = "pfcp", .tdefs = osmo_pfcp_tdefs, .desc = "PFCP" },
+	{}
+};
+
+void g_pfcp_tool_alloc(void *ctx)
+{
+	OSMO_ASSERT(g_pfcp_tool == NULL);
+	g_pfcp_tool = talloc_zero(ctx, struct g_pfcp_tool);
+
+	*g_pfcp_tool = (struct g_pfcp_tool){
+		.vty_cfg = {
+			.local_ip = talloc_strdup(g_pfcp_tool, "0.0.0.0"),
+			.local_port = OSMO_PFCP_PORT,
+		},
+	};
+
+	INIT_LLIST_HEAD(&g_pfcp_tool->peers);
+}
+
+struct pfcp_tool_peer *pfcp_tool_peer_find(const struct osmo_sockaddr *remote_addr)
+{
+	struct pfcp_tool_peer *peer;
+	llist_for_each_entry(peer, &g_pfcp_tool->peers, entry) {
+		if (osmo_sockaddr_cmp(&peer->remote_addr, remote_addr) == 0)
+			return peer;
+	}
+	return NULL;
+}
+
+struct pfcp_tool_peer *pfcp_tool_peer_find_or_create(const struct osmo_sockaddr *remote_addr)
+{
+	struct pfcp_tool_peer *peer = pfcp_tool_peer_find(remote_addr);
+	if (peer)
+		return peer;
+
+	peer = talloc_zero(g_pfcp_tool, struct pfcp_tool_peer);
+	peer->remote_addr = *remote_addr;
+	peer->next_seid_state = 0x1234567;
+	INIT_LLIST_HEAD(&peer->sessions);
+	llist_add(&peer->entry, &g_pfcp_tool->peers);
+	return peer;
+}
+
+struct pfcp_tool_session *pfcp_tool_session_find(struct pfcp_tool_peer *peer, uint64_t cp_seid)
+{
+	struct pfcp_tool_session *session;
+	llist_for_each_entry(session, &peer->sessions, entry) {
+		if (session->cp_seid == cp_seid)
+			return session;
+	}
+	return NULL;
+}
+
+struct pfcp_tool_session *pfcp_tool_session_find_or_create(struct pfcp_tool_peer *peer, uint64_t cp_seid,
+							   enum up_gtp_action_kind gtp_action)
+{
+	struct pfcp_tool_session *session = pfcp_tool_session_find(peer, cp_seid);
+	if (session)
+		return session;
+
+	session = talloc(peer, struct pfcp_tool_session);
+	*session = (struct pfcp_tool_session){
+		.peer = peer,
+		.cp_seid = cp_seid,
+		.gtp_action = gtp_action,
+	};
+	llist_add(&session->entry, &peer->sessions);
+	return session;
+}
+
+static void rx_assoc_setup_resp(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	if (m->ies.assoc_setup_resp.up_function_features_present)
+		OSMO_LOG_PFCP_MSG(m, LOGL_NOTICE, "Associated. UP Peer features: %s\n",
+				  osmo_pfcp_bits_to_str_c(OTC_SELECT,
+							  m->ies.assoc_setup_resp.up_function_features.bits,
+							  osmo_pfcp_up_feature_strs));
+
+	if (m->ies.assoc_setup_resp.cp_function_features_present)
+		OSMO_LOG_PFCP_MSG(m, LOGL_NOTICE, "Associated. CP Peer features: %s\n",
+				  osmo_pfcp_bits_to_str_c(OTC_SELECT,
+							  m->ies.assoc_setup_resp.cp_function_features.bits,
+							  osmo_pfcp_cp_feature_strs));
+}
+
+static void rx_session_est_resp(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	struct pfcp_tool_peer *peer;
+	struct pfcp_tool_session *session;
+	enum osmo_pfcp_cause *cause = osmo_pfcp_msg_cause(m);
+	if (!cause) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Session Establishment Response should contain a Cause\n");
+		return;
+	}
+	if (*cause != OSMO_PFCP_CAUSE_REQUEST_ACCEPTED) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Peer responds that Session Establishment failed\n");
+		return;
+	}
+	if (!m->h.seid_present) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Session Establishment Response should contain a SEID\n");
+		return;
+	}
+	if (!m->ies.session_est_resp.up_f_seid_present) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Session Establishment Response without UP F-SEID\n");
+		return;
+	}
+	peer = pfcp_tool_peer_find(&m->remote_addr);
+	if (!peer)
+		return;
+	session = pfcp_tool_session_find(peer, m->h.seid);
+	if (!session)
+		return;
+	session->up_f_seid = m->ies.session_est_resp.up_f_seid;
+}
+
+void pfcp_tool_rx_msg(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	switch (m->h.message_type) {
+	case OSMO_PFCP_MSGT_ASSOC_SETUP_RESP:
+		rx_assoc_setup_resp(ep, m);
+		break;
+	case OSMO_PFCP_MSGT_SESSION_EST_RESP:
+		rx_session_est_resp(ep, m);
+		break;
+	default: break;
+	}
+}
+
+int peer_tx(struct pfcp_tool_peer *peer, struct osmo_pfcp_msg *m)
+{
+	int rc;
+	rc = osmo_pfcp_endpoint_tx(g_pfcp_tool->ep, m);
+	if (m->is_response)
+		peer->last_resp = *m;
+	else
+		peer->last_req = *m;
+	return rc;
+}
+
+uint64_t peer_new_seid(struct pfcp_tool_peer *peer)
+{
+	return peer->next_seid_state++;
+}

src/osmo-pfcp-tool/pfcp_tool.h

@@ -0,0 +1,106 @@
+/* Global definitions for osmo-pfcp-tool */
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#pragma once
+
+#include <osmocom/core/linuxlist.h>
+#include <osmocom/core/tdef.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/sockaddr_str.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+
+#include <osmocom/upf/up_gtp_action.h>
+
+struct osmo_tdef;
+struct ctrl_handle;
+
+extern struct osmo_tdef g_pfcp_tool_tdefs[];
+extern struct osmo_tdef_group g_pfcp_tool_tdef_groups[];
+
+struct pfcp_tool_peer {
+	struct llist_head entry;
+
+	struct osmo_sockaddr remote_addr;
+	struct osmo_pfcp_msg last_req;
+	struct osmo_pfcp_msg last_resp;
+
+	uint64_t next_seid_state;
+
+	struct llist_head sessions;
+};
+
+struct pfcp_tool_teid_pair {
+	uint32_t local;
+	uint32_t remote;
+};
+
+struct pfcp_tool_session {
+	struct llist_head entry;
+
+	enum up_gtp_action_kind gtp_action;
+
+	struct pfcp_tool_peer *peer;
+	uint64_t cp_seid;
+	struct osmo_pfcp_ie_f_seid up_f_seid;
+
+	struct {
+		struct pfcp_tool_teid_pair teid;
+		struct osmo_sockaddr_str gtp_ip;
+	} access;
+
+	struct {
+		struct pfcp_tool_teid_pair teid;
+		struct osmo_sockaddr_str gtp_ip;
+		struct osmo_sockaddr_str ue_addr;
+	} core;
+};
+
+struct g_pfcp_tool {
+	struct ctrl_handle *ctrl;
+
+	struct {
+		char *local_ip;
+		uint16_t local_port;
+	} vty_cfg;
+
+	struct osmo_pfcp_endpoint *ep;
+	struct llist_head peers;
+};
+
+extern struct g_pfcp_tool *g_pfcp_tool;
+
+void g_pfcp_tool_alloc(void *ctx);
+void pfcp_tool_vty_init_cfg();
+void pfcp_tool_vty_init_cmds();
+
+int pfcp_tool_mainloop();
+
+struct pfcp_tool_peer *pfcp_tool_peer_find_or_create(const struct osmo_sockaddr *remote_addr);
+struct pfcp_tool_session *pfcp_tool_session_find_or_create(struct pfcp_tool_peer *peer, uint64_t cp_seid,
+							   enum up_gtp_action_kind kind);
+void pfcp_tool_rx_msg(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m);
+
+int peer_tx(struct pfcp_tool_peer *peer, struct osmo_pfcp_msg *m);
+uint64_t peer_new_seid(struct pfcp_tool_peer *peer);

src/osmo-pfcp-tool/pfcp_tool_vty.c

@@ -0,0 +1,785 @@
+/* osmo-pfcp-tool interface to quagga VTY */
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdlib.h>
+#include <unistd.h>
+
+#include <osmocom/core/sockaddr_str.h>
+#include <osmocom/core/socket.h>
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
+#include <osmocom/pfcp/pfcp_msg.h>
+
+#include <osmocom/vty/vty.h>
+#include <osmocom/vty/command.h>
+
+#include "pfcp_tool.h"
+
+enum pfcp_tool_vty_node {
+	PEER_NODE = _LAST_OSMOVTY_NODE + 1,
+	SESSION_NODE,
+};
+
+DEFUN(c_local_addr, c_local_addr_cmd,
+      "local-addr IP_ADDR",
+      "Set the local IP address to bind on for PFCP; see also 'listen'\n"
+      "IP address\n")
+{
+	if (g_pfcp_tool->ep != NULL) {
+		vty_out(vty, "Already listening on %s%s",
+			osmo_sockaddr_to_str_c(OTC_SELECT, &g_pfcp_tool->ep->cfg.local_addr),
+			VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	osmo_talloc_replace_string(g_pfcp_tool, &g_pfcp_tool->vty_cfg.local_ip, argv[0]);
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(c_listen, c_listen_cmd,
+      "listen",
+      "Bind local PFCP port and listen; see also 'local-addr'\n")
+{
+	struct osmo_sockaddr_str local_addr;
+	int rc;
+
+	OSMO_ASSERT(g_pfcp_tool);
+	if (g_pfcp_tool->ep != NULL) {
+		vty_out(vty, "Already listening on %s%s",
+			osmo_sockaddr_to_str_c(OTC_SELECT, &g_pfcp_tool->ep->cfg.local_addr),
+			VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	g_pfcp_tool->ep = osmo_pfcp_endpoint_create(g_pfcp_tool, g_pfcp_tool);
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Failed to allocate PFCP endpoint.%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	g_pfcp_tool->ep->rx_msg = pfcp_tool_rx_msg;
+	g_pfcp_tool->ep->seq_nr_state = rand();
+
+	/* Translate address string from VTY config to osmo_sockaddr: first read into osmo_sockaddr_str, then write to
+	 * osmo_sockaddr. */
+	osmo_sockaddr_str_from_str(&local_addr, g_pfcp_tool->vty_cfg.local_ip,
+				   g_pfcp_tool->vty_cfg.local_port);
+	osmo_sockaddr_str_to_sockaddr(&local_addr, &g_pfcp_tool->ep->cfg.local_addr.u.sas);
+
+	/* Store this address as the local PFCP Node Id */
+	osmo_pfcp_ie_node_id_from_osmo_sockaddr(&g_pfcp_tool->ep->cfg.local_node_id, &g_pfcp_tool->ep->cfg.local_addr);
+
+	rc = osmo_pfcp_endpoint_bind(g_pfcp_tool->ep);
+	if (rc) {
+		vty_out(vty, "Failed to bind PFCP endpoint on %s: %s%s\n",
+			osmo_sockaddr_to_str_c(OTC_SELECT, &g_pfcp_tool->ep->cfg.local_addr), strerror(rc),
+			VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+DEFUN(c_sleep, c_sleep_cmd,
+	"sleep <0-999999> [<0-999>]",
+	"Let some time pass\n"
+	"Seconds to wait\n")
+{
+	int secs = atoi(argv[0]);
+	int msecs = 0;
+	struct osmo_timer_list t = {};
+	if (argc > 1)
+		msecs = atoi(argv[1]);
+
+	vty_out(vty, "zzZ %d.%03ds...%s", secs, msecs, VTY_NEWLINE);
+	vty_flush(vty);
+
+	osmo_timer_setup(&t, NULL, NULL);
+	osmo_timer_schedule(&t, secs, msecs * 1000);
+
+	/* Still operate the message pump while waiting for time to pass */
+	while (t.active && !osmo_select_shutdown_done()) {
+		if (pfcp_tool_mainloop())
+			break;
+	}
+
+	osmo_timer_del(&t);
+	vty_out(vty, "...zzZ %d.%03ds%s", secs, msecs, VTY_NEWLINE);
+	vty_flush(vty);
+	return CMD_SUCCESS;
+}
+
+static struct cmd_node peer_node = {
+	PEER_NODE,
+	"%s(peer)# ",
+	1,
+};
+
+DEFUN(peer, peer_cmd,
+      "pfcp-peer REMOTE_ADDR",
+      "Enter the 'peer' node for the given remote address\n"
+      "Remote PFCP peer's IP address\n")
+{
+	struct pfcp_tool_peer *peer;
+	struct osmo_sockaddr_str remote_addr_str;
+	struct osmo_sockaddr remote_addr;
+
+	osmo_sockaddr_str_from_str(&remote_addr_str, argv[0], OSMO_PFCP_PORT);
+	osmo_sockaddr_str_to_sockaddr(&remote_addr_str, (struct sockaddr_storage*)&remote_addr);
+
+	peer = pfcp_tool_peer_find_or_create(&remote_addr);
+
+	vty->index = peer;
+	vty->node = PEER_NODE;
+
+	return CMD_SUCCESS;
+}
+
+#define TX_STR "Send a PFCP message to a peer\n"
+
+DEFUN(peer_tx_heartbeat, peer_tx_heartbeat_cmd,
+      "tx heartbeat",
+      TX_STR "Send a Heartbeat Request\n")
+{
+	struct pfcp_tool_peer *peer = vty->index;
+	int rc;
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	vty_out(vty, "Tx Heartbeat Request to %s%s",
+		osmo_sockaddr_to_str_c(OTC_SELECT, &peer->remote_addr), VTY_NEWLINE);
+
+	rc = osmo_pfcp_endpoint_tx_heartbeat_req(g_pfcp_tool->ep, &peer->remote_addr);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+DEFUN(peer_tx_assoc_setup_req, peer_tx_assoc_setup_req_cmd,
+      "tx assoc-setup-req",
+      TX_STR "Send an Association Setup Request\n")
+{
+	struct pfcp_tool_peer *peer = vty->index;
+	int rc;
+	struct osmo_pfcp_msg *m;
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	m = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr, &g_pfcp_tool->ep->cfg.local_node_id, NULL,
+				   OSMO_PFCP_MSGT_ASSOC_SETUP_REQ);
+	m->ies.assoc_setup_req.recovery_time_stamp = g_pfcp_tool->ep->recovery_time_stamp;
+
+	m->ies.assoc_setup_req.cp_function_features_present = true;
+	osmo_pfcp_bits_set(m->ies.assoc_setup_req.cp_function_features.bits, OSMO_PFCP_CP_FEAT_BUNDL, true);
+
+	rc = peer_tx(peer, m);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+DEFUN(peer_retrans_req, peer_retrans_req_cmd,
+      "retrans (req|resp)",
+      "Retransmit the last sent message\n" "Retransmit the last sent PFCP Request\n"
+      "Retransmit the last sent PFCP Response\n")
+{
+	struct pfcp_tool_peer *peer = vty->index;
+	int rc;
+	struct osmo_pfcp_msg *m;
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	m = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr, &g_pfcp_tool->ep->cfg.local_node_id, NULL, 0);
+	if (strcmp(argv[0], "req") == 0)
+		*m = peer->last_req;
+	else
+		*m = peer->last_resp;
+
+	OSMO_LOG_PFCP_MSG(m, LOGL_DEBUG, "retrans %s\n", argv[0]);
+
+	rc = osmo_pfcp_endpoint_tx_data(g_pfcp_tool->ep, m);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+static struct cmd_node session_node = {
+	SESSION_NODE,
+	"%s(session)# ",
+	1,
+};
+
+DEFUN(session, session_cmd,
+      "session [(endecaps|tunmap)] [<0-18446744073709551615>]",
+      "Enter the 'session' node for the given SEID\n"
+      "Set up GTP tunnel encapsulation/decapsulation (default)\n"
+      "Set up GTP tunnel mapping\n"
+      "local Session Endpoint ID\n")
+{
+	struct pfcp_tool_peer *peer = vty->index;
+	struct pfcp_tool_session *session;
+	enum up_gtp_action_kind gtp_action = UP_GTP_U_ENDECAPS;
+
+	if (argc > 0 && !strcmp(argv[0], "tunmap"))
+		gtp_action = UP_GTP_U_TUNMAP;
+
+	if (argc > 1)
+		session = pfcp_tool_session_find_or_create(peer, atoll(argv[1]), gtp_action);
+	else
+		session = pfcp_tool_session_find_or_create(peer, peer_new_seid(peer), gtp_action);
+
+	vty->index = session;
+	vty->node = SESSION_NODE;
+
+	return CMD_SUCCESS;
+}
+
+DEFUN(s_ue, s_ue_cmd,
+      "ue ip A.B.C.D",
+      "Setup the UE as it appears towards the Core network in plain IP traffic\n"
+      "IP address assigned to the UE\n")
+{
+	struct pfcp_tool_session *session = vty->index;
+	if (osmo_sockaddr_str_from_str2(&session->core.ue_addr, argv[0])) {
+		vty_out(vty, "Error setting UE IP address%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+DEFUN(s_teid, s_teid_cmd,
+      "gtp (access|core) teid local <0-4294967295> remote <0-4294967295>",
+      "Setup TEID used in GTP\n"
+      "Set the TEIDs towards the ACCESS network (towards the radio network and the actual UE)\n"
+      "Set the TEIDs towards the CORE network (towards the internet)\n"
+      "Local TEID, which the UPF expects to see in incoming GTP packets\n"
+      "Local TEID, when 0 tell the UPF to choose (PFCP: FAR F-TEID: CHOOSE=1)\n"
+      "Remote TEID, which the UPF sends out in GTP packets\n"
+      "Remote TEID, which the GTP peer has assigned for itself\n")
+{
+	struct pfcp_tool_session *session = vty->index;
+	struct pfcp_tool_teid_pair *dst;
+	if (!strcmp(argv[0], "access"))
+		dst = &session->access.teid;
+	else
+		dst = &session->core.teid;
+	*dst = (struct pfcp_tool_teid_pair){
+		.local = atoi(argv[1]),
+		.remote = atoi(argv[2]),
+	};
+	return CMD_SUCCESS;
+}
+
+DEFUN(s_gtp, s_gtp_cmd,
+      "gtp (access|core) ip A.B.C.D",
+      "Setup GTP peer\n"
+      "Set the GTP peer towards the ACCESS network (towards the radio network and the actual UE)\n"
+      "Set the GTP peer towards the CORE network (towards the internet)\n"
+      "Set the GTP peer IP address, where to send GTP packets to / receive GTP packets from\n"
+      "GTP peer IP address\n")
+{
+	struct pfcp_tool_session *session = vty->index;
+	struct osmo_sockaddr_str *dst;
+	if (!strcmp(argv[0], "access"))
+		dst = &session->access.gtp_ip;
+	else
+		dst = &session->core.gtp_ip;
+	if (osmo_sockaddr_str_from_str2(dst, argv[1])) {
+		vty_out(vty, "Error setting GTP IP address%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+int session_endecaps_tx_est_req(struct vty *vty, const char **argv, int argc)
+{
+	struct pfcp_tool_session *session = vty->index;
+	struct pfcp_tool_peer *peer = session->peer;
+	int rc;
+	struct osmo_pfcp_msg *m;
+	struct osmo_pfcp_ie_f_teid f_teid_access_local;
+	struct osmo_pfcp_ie_outer_header_creation ohc_access;
+	struct osmo_pfcp_ie_apply_action aa = {};
+	struct osmo_sockaddr ue_addr;
+	struct osmo_pfcp_ie_f_seid cp_f_seid;
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	if (argc > 0 && !strcmp("drop", argv[0]))
+		osmo_pfcp_bits_set(aa.bits, OSMO_PFCP_APPLY_ACTION_DROP, true);
+	else
+		osmo_pfcp_bits_set(aa.bits, OSMO_PFCP_APPLY_ACTION_FORW, true);
+
+	if (osmo_sockaddr_str_to_sockaddr(&session->core.ue_addr, &ue_addr.u.sas)) {
+		vty_out(vty, "Error in UE IP%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	if (session->access.teid.local == 0) {
+		f_teid_access_local = (struct osmo_pfcp_ie_f_teid){
+			.choose_flag = true,
+			.choose = {
+				.ipv4_addr = true,
+			},
+		};
+	} else {
+		f_teid_access_local = (struct osmo_pfcp_ie_f_teid){
+			.fixed = {
+				.teid = session->access.teid.local,
+				.ip_addr = {
+					.v4_present = true,
+					.v4 = g_pfcp_tool->ep->cfg.local_addr,
+				},
+			},
+		};
+		if (osmo_sockaddr_str_to_sockaddr(&session->access.gtp_ip, &f_teid_access_local.fixed.ip_addr.v4.u.sas)) {
+			vty_out(vty, "Error in GTP IP towards Access%s", VTY_NEWLINE);
+			return CMD_WARNING;
+		}
+	}
+
+	ohc_access = (struct osmo_pfcp_ie_outer_header_creation){
+		.teid_present = true,
+		.teid = session->access.teid.remote,
+		.ip_addr.v4_present = true,
+	};
+	osmo_pfcp_bits_set(ohc_access.desc_bits, OSMO_PFCP_OUTER_HEADER_CREATION_GTP_U_UDP_IPV4, true);
+	if (osmo_sockaddr_str_to_sockaddr(&session->access.gtp_ip, &ohc_access.ip_addr.v4.u.sas)) {
+		vty_out(vty, "Error in GTP IP towards Access%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	cp_f_seid = (struct osmo_pfcp_ie_f_seid){
+		.seid = session->cp_seid,
+	};
+	osmo_pfcp_ip_addrs_set(&cp_f_seid.ip_addr, &g_pfcp_tool->ep->cfg.local_addr);
+
+	m = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr, &g_pfcp_tool->ep->cfg.local_node_id, NULL,
+				   OSMO_PFCP_MSGT_SESSION_EST_REQ);
+	m->h.seid_present = true;
+	/* the UPF has yet to assign a SEID for itself, no matter what SEID we (the CPF) use for this session */
+	m->h.seid = 0;
+	/* GTP encapsulation decapsulation: remove header from ACCESS to CORE, add header from CORE towards ACCESS */
+	m->ies.session_est_req = (struct osmo_pfcp_msg_session_est_req){
+		.node_id = m->ies.session_est_req.node_id,
+		.cp_f_seid_present = true,
+		.cp_f_seid = cp_f_seid,
+		.create_pdr_count = 2,
+		.create_pdr = {
+			{
+				.pdr_id = 1,
+				.precedence = 255,
+				.pdi = {
+					.source_iface = OSMO_PFCP_SOURCE_IFACE_CORE,
+					.ue_ip_address_present = true,
+					.ue_ip_address = {
+						.ip_is_destination = true,
+						.ip_addr = {
+							.v4_present = true,
+							.v4 = ue_addr,
+						},
+					},
+				},
+				.far_id_present = true,
+				.far_id = 1,
+			},
+			{
+				.pdr_id = 2,
+				.precedence = 255,
+				.pdi = {
+					.source_iface = OSMO_PFCP_SOURCE_IFACE_ACCESS,
+					.local_f_teid_present = true,
+					.local_f_teid = f_teid_access_local,
+				},
+				.outer_header_removal_present = true,
+				.outer_header_removal = {
+					.desc = OSMO_PFCP_OUTER_HEADER_REMOVAL_GTP_U_UDP_IPV4,
+				},
+				.far_id_present = true,
+				.far_id = 2,
+			},
+		},
+		.create_far_count = 2,
+		.create_far = {
+			{
+				.far_id = 1,
+				.forw_params_present = true,
+				.forw_params = {
+					.destination_iface = OSMO_PFCP_DEST_IFACE_ACCESS,
+					.outer_header_creation_present = true,
+					.outer_header_creation = ohc_access,
+				},
+				.apply_action = aa,
+			},
+			{
+				.far_id = 2,
+				.forw_params_present = true,
+				.forw_params = {
+					.destination_iface = OSMO_PFCP_DEST_IFACE_CORE,
+				},
+				.apply_action = aa,
+			},
+		},
+	};
+
+	rc = peer_tx(peer, m);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+int session_tunmap_tx_est_req(struct vty *vty, const char **argv, int argc)
+{
+	struct pfcp_tool_session *session = vty->index;
+	struct pfcp_tool_peer *peer = session->peer;
+	int rc;
+	struct osmo_pfcp_msg *m;
+
+	struct osmo_pfcp_ie_f_seid cp_f_seid;
+
+	struct osmo_pfcp_ie_f_teid f_teid_access_local;
+	struct osmo_pfcp_ie_outer_header_creation ohc_access;
+
+	struct osmo_pfcp_ie_f_teid f_teid_core_local;
+	struct osmo_pfcp_ie_outer_header_creation ohc_core;
+
+	struct osmo_pfcp_ie_apply_action aa = {};
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	if (argc > 0 && !strcmp("drop", argv[0]))
+		osmo_pfcp_bits_set(aa.bits, OSMO_PFCP_APPLY_ACTION_DROP, true);
+	else
+		osmo_pfcp_bits_set(aa.bits, OSMO_PFCP_APPLY_ACTION_FORW, true);
+
+	if (session->access.teid.local == 0) {
+		f_teid_access_local = (struct osmo_pfcp_ie_f_teid){
+			.choose_flag = true,
+			.choose = {
+				.ipv4_addr = true,
+			},
+		};
+	} else {
+		f_teid_access_local = (struct osmo_pfcp_ie_f_teid){
+			.fixed = {
+				.teid = session->access.teid.local,
+				.ip_addr = {
+					.v4_present = true,
+					.v4 = g_pfcp_tool->ep->cfg.local_addr,
+				},
+			},
+		};
+		if (osmo_sockaddr_str_to_sockaddr(&session->access.gtp_ip, &f_teid_access_local.fixed.ip_addr.v4.u.sas)) {
+			vty_out(vty, "Error in GTP IP towards Access%s", VTY_NEWLINE);
+			return CMD_WARNING;
+		}
+	}
+
+	ohc_access = (struct osmo_pfcp_ie_outer_header_creation){
+		.teid_present = true,
+		.teid = session->access.teid.remote,
+		.ip_addr.v4_present = true,
+	};
+	osmo_pfcp_bits_set(ohc_access.desc_bits, OSMO_PFCP_OUTER_HEADER_CREATION_GTP_U_UDP_IPV4, true);
+	if (osmo_sockaddr_str_to_sockaddr(&session->access.gtp_ip, &ohc_access.ip_addr.v4.u.sas)) {
+		vty_out(vty, "Error in GTP IP towards Access%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	if (session->core.teid.local == 0) {
+		f_teid_core_local = (struct osmo_pfcp_ie_f_teid){
+			.choose_flag = true,
+			.choose = {
+				.ipv4_addr = true,
+			},
+		};
+	} else {
+		f_teid_core_local = (struct osmo_pfcp_ie_f_teid){
+			.fixed = {
+				.teid = session->core.teid.local,
+				.ip_addr = {
+					.v4_present = true,
+					.v4 = g_pfcp_tool->ep->cfg.local_addr,
+				},
+			},
+		};
+		if (osmo_sockaddr_str_to_sockaddr(&session->core.gtp_ip, &f_teid_core_local.fixed.ip_addr.v4.u.sas)) {
+			vty_out(vty, "Error in GTP IP towards Core%s", VTY_NEWLINE);
+			return CMD_WARNING;
+		}
+	}
+	ohc_core = (struct osmo_pfcp_ie_outer_header_creation){
+		.teid_present = true,
+		.teid = session->core.teid.remote,
+		.ip_addr.v4_present = true,
+	};
+	osmo_pfcp_bits_set(ohc_core.desc_bits, OSMO_PFCP_OUTER_HEADER_CREATION_GTP_U_UDP_IPV4, true);
+	if (osmo_sockaddr_str_to_sockaddr(&session->core.gtp_ip, &ohc_core.ip_addr.v4.u.sas)) {
+		vty_out(vty, "Error in GTP IP towards Core%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	cp_f_seid = (struct osmo_pfcp_ie_f_seid){
+		.seid = session->cp_seid,
+	};
+	osmo_pfcp_ip_addrs_set(&cp_f_seid.ip_addr, &g_pfcp_tool->ep->cfg.local_addr);
+
+	m = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr, &g_pfcp_tool->ep->cfg.local_node_id, NULL,
+				   OSMO_PFCP_MSGT_SESSION_EST_REQ);
+	m->h.seid_present = true;
+	m->h.seid = 0;
+	/* GTP tunmap: remove header from both directions, and add header in both directions */
+	m->ies.session_est_req = (struct osmo_pfcp_msg_session_est_req){
+		.node_id = m->ies.session_est_req.node_id,
+		.cp_f_seid_present = true,
+		.cp_f_seid = cp_f_seid,
+		.create_pdr_count = 2,
+		.create_pdr = {
+			{
+				.pdr_id = 1,
+				.precedence = 255,
+				.pdi = {
+					.source_iface = OSMO_PFCP_SOURCE_IFACE_CORE,
+					.local_f_teid_present = true,
+					.local_f_teid = f_teid_core_local,
+				},
+				.outer_header_removal_present = true,
+				.outer_header_removal = {
+					.desc = OSMO_PFCP_OUTER_HEADER_REMOVAL_GTP_U_UDP_IPV4,
+				},
+				.far_id_present = true,
+				.far_id = 1,
+			},
+			{
+				.pdr_id = 2,
+				.precedence = 255,
+				.pdi = {
+					.source_iface = OSMO_PFCP_SOURCE_IFACE_ACCESS,
+					.local_f_teid_present = true,
+					.local_f_teid = f_teid_access_local,
+				},
+				.outer_header_removal_present = true,
+				.outer_header_removal = {
+					.desc = OSMO_PFCP_OUTER_HEADER_REMOVAL_GTP_U_UDP_IPV4,
+				},
+				.far_id_present = true,
+				.far_id = 2,
+			},
+		},
+		.create_far_count = 2,
+		.create_far = {
+			{
+				.far_id = 1,
+				.forw_params_present = true,
+				.forw_params = {
+					.destination_iface = OSMO_PFCP_DEST_IFACE_ACCESS,
+					.outer_header_creation_present = true,
+					.outer_header_creation = ohc_access,
+				},
+				.apply_action = aa,
+			},
+			{
+				.far_id = 2,
+				.forw_params_present = true,
+				.forw_params = {
+					.destination_iface = OSMO_PFCP_DEST_IFACE_CORE,
+					.outer_header_creation_present = true,
+					.outer_header_creation = ohc_core,
+				},
+				.apply_action = aa,
+			},
+		},
+	};
+
+	rc = peer_tx(peer, m);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+DEFUN(session_tx_est_req, session_tx_est_req_cmd,
+      "tx session-est-req [(forw|drop)]",
+      TX_STR "Send a Session Establishment Request\n"
+      "Set FAR to FORW = 1 (default)\n"
+      "Set FAR to DROP = 1\n")
+{
+	struct pfcp_tool_session *session = vty->index;
+	switch (session->gtp_action) {
+	case UP_GTP_U_ENDECAPS:
+		return session_endecaps_tx_est_req(vty, argv, argc);
+	case UP_GTP_U_TUNMAP:
+		return session_tunmap_tx_est_req(vty, argv, argc);
+	default:
+		vty_out(vty, "unknown gtp action%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+}
+
+DEFUN(session_tx_mod_req, session_tx_mod_req_cmd,
+      "tx session-mod-req far [(forw|drop)]",
+      TX_STR "Send a Session Modification Request\n"
+      "Set FAR to FORW = 1\n"
+      "Set FAR to DROP = 1\n")
+{
+	struct pfcp_tool_session *session = vty->index;
+	struct pfcp_tool_peer *peer = session->peer;
+	int rc;
+	struct osmo_pfcp_msg *m;
+	struct osmo_pfcp_ie_apply_action aa = {};
+	struct osmo_pfcp_ie_f_seid cp_f_seid;
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	if (argc > 0 && !strcmp("drop", argv[0]))
+		osmo_pfcp_bits_set(aa.bits, OSMO_PFCP_APPLY_ACTION_DROP, true);
+	else
+		osmo_pfcp_bits_set(aa.bits, OSMO_PFCP_APPLY_ACTION_FORW, true);
+
+	cp_f_seid = (struct osmo_pfcp_ie_f_seid){
+		.seid = session->cp_seid,
+	};
+	osmo_pfcp_ip_addrs_set(&cp_f_seid.ip_addr, &g_pfcp_tool->ep->cfg.local_addr);
+
+	m = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr, &g_pfcp_tool->ep->cfg.local_node_id, NULL,
+				   OSMO_PFCP_MSGT_SESSION_MOD_REQ);
+	m->h.seid_present = true;
+	m->h.seid = session->up_f_seid.seid;
+	m->ies.session_mod_req = (struct osmo_pfcp_msg_session_mod_req){
+		.cp_f_seid_present = true,
+		.cp_f_seid = cp_f_seid,
+		.upd_far_count = 2,
+		.upd_far = {
+			{
+				.far_id = 1,
+				.apply_action_present = true,
+				.apply_action = aa,
+			},
+			{
+				.far_id = 2,
+				.apply_action_present = true,
+				.apply_action = aa,
+			},
+		},
+	};
+
+	rc = peer_tx(peer, m);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+DEFUN(session_tx_del_req, session_tx_del_req_cmd,
+      "tx session-del-req",
+      TX_STR "Send a Session Deletion Request\n")
+{
+	struct pfcp_tool_session *session = vty->index;
+	struct pfcp_tool_peer *peer = session->peer;
+	int rc;
+	struct osmo_pfcp_msg *m;
+
+	if (!g_pfcp_tool->ep) {
+		vty_out(vty, "Endpoint not configured%s", VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+
+	m = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr, &g_pfcp_tool->ep->cfg.local_node_id, NULL,
+				   OSMO_PFCP_MSGT_SESSION_DEL_REQ);
+	m->h.seid_present = true;
+	m->h.seid = session->up_f_seid.seid;
+
+	rc = peer_tx(peer, m);
+	if (rc) {
+		vty_out(vty, "Failed to transmit: %s%s", strerror(-rc), VTY_NEWLINE);
+		return CMD_WARNING;
+	}
+	return CMD_SUCCESS;
+}
+
+static void install_ve_and_config(struct cmd_element *cmd)
+{
+	install_element_ve(cmd);
+	install_element(CONFIG_NODE, cmd);
+}
+
+void pfcp_tool_vty_init_cfg()
+{
+	OSMO_ASSERT(g_pfcp_tool != NULL);
+
+	install_ve_and_config(&c_local_addr_cmd);
+	install_ve_and_config(&c_listen_cmd);
+}
+
+void pfcp_tool_vty_init_cmds()
+{
+	OSMO_ASSERT(g_pfcp_tool != NULL);
+
+	install_ve_and_config(&c_sleep_cmd);
+
+	install_ve_and_config(&peer_cmd);
+	install_node(&peer_node, NULL);
+
+	install_element(PEER_NODE, &c_sleep_cmd);
+	install_element(PEER_NODE, &peer_tx_heartbeat_cmd);
+	install_element(PEER_NODE, &peer_tx_assoc_setup_req_cmd);
+	install_element(PEER_NODE, &peer_retrans_req_cmd);
+
+	install_element(PEER_NODE, &session_cmd);
+	install_node(&session_node, NULL);
+	install_element(SESSION_NODE, &c_sleep_cmd);
+	install_element(SESSION_NODE, &session_tx_est_req_cmd);
+	install_element(SESSION_NODE, &session_tx_mod_req_cmd);
+	install_element(SESSION_NODE, &session_tx_del_req_cmd);
+	install_element(SESSION_NODE, &s_ue_cmd);
+	install_element(SESSION_NODE, &s_gtp_cmd);
+	install_element(SESSION_NODE, &s_teid_cmd);
+}

src/osmo-upf/Makefile.am

@@ -1,6 +1,7 @@
 AM_CPPFLAGS = \
 	$(all_includes) \
 	-I$(top_srcdir)/include \
+	-I$(top_builddir)/include \
 	-I$(top_builddir) \
 	$(NULL)
 
@@ -9,10 +10,16 @@ AM_CFLAGS = \
 	$(LIBOSMOCORE_CFLAGS) \
 	$(LIBOSMOVTY_CFLAGS) \
 	$(LIBOSMOCTRL_CFLAGS) \
+	$(LIBGTPNL_CFLAGS) \
+	$(LIBNFTNL_CFLAGS) \
+	$(LIBNFTABLES_CFLAGS) \
 	$(COVERAGE_CFLAGS) \
 	$(NULL)
 
 AM_LDFLAGS = \
+	$(LIBGTPNL_LDFLAGS) \
+	$(LIBNFTNL_LDFLAGS) \
+	$(LIBNFTABLES_LDFLAGS) \
 	$(COVERAGE_LDFLAGS) \
 	$(NULL)
 
@@ -22,12 +29,25 @@ bin_PROGRAMS = \
 
 osmo_upf_SOURCES = \
 	osmo_upf_main.c \
+	up_endpoint.c \
+	up_gtp_action.c \
+	up_peer.c \
+	up_session.c \
 	upf.c \
+	upf_gtp.c \
+	upf_gtpu_echo.c \
+	upf_nft.c \
+	upf_vty.c \
 	$(NULL)
 
 osmo_upf_LDADD = \
+	$(top_builddir)/src/libosmo-pfcp/libosmo-pfcp.la \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(LIBOSMOVTY_LIBS) \
 	$(LIBOSMOCTRL_LIBS) \
+	$(LIBGTPNL_LIBS) \
+	$(LIBNFTNL_LIBS) \
+	$(LIBNFTABLES_LIBS) \
 	$(COVERAGE_LDFLAGS) \
 	$(NULL)

src/osmo-upf/osmo_upf_main.c

@@ -34,11 +34,17 @@
 #include <osmocom/vty/cpu_sched_vty.h>
 #include <osmocom/vty/telnet_interface.h>
 #include <osmocom/vty/ports.h>
+#include <osmocom/vty/tdef_vty.h>
 #include <osmocom/ctrl/control_if.h>
 #include <osmocom/ctrl/control_vty.h>
 #include <osmocom/ctrl/ports.h>
 
+#include <osmocom/pfcp/pfcp_endpoint.h>
+
 #include <osmocom/upf/upf.h>
+#include <osmocom/upf/up_endpoint.h>
+#include <osmocom/upf/upf_gtp.h>
+#include <osmocom/upf/upf_nft.h>
 
 #define _GNU_SOURCE
 #include <getopt.h>
@@ -48,6 +54,7 @@
 
 #include <signal.h>
 #include <stdio.h>
+#include <string.h>
 
 extern void *tall_vty_ctx;
 
@@ -207,6 +214,36 @@ static struct vty_app_info upf_vty_app_info = {
 };
 
 static const struct log_info_cat upf_default_categories[] = {
+	[DREF] = {
+		.name = "DREF",
+		.description = "Reference Counting",
+		.enabled = 0, .loglevel = LOGL_NOTICE,
+		.color = OSMO_LOGCOLOR_DARKGREY,
+	},
+	[DPEER] = {
+		.name = "DPEER",
+		.description = "PFCP peer association",
+		.enabled = 0, .loglevel = LOGL_NOTICE,
+		.color = OSMO_LOGCOLOR_YELLOW,
+	},
+	[DSESSION] = {
+		.name = "DSESSION",
+		.description = "PFCP sessions",
+		.enabled = 0, .loglevel = LOGL_NOTICE,
+		.color = OSMO_LOGCOLOR_BLUE,
+	},
+	[DGTP] = {
+		.name = "DGTP",
+		.description = "GTP tunneling",
+		.enabled = 0, .loglevel = LOGL_NOTICE,
+		.color = OSMO_LOGCOLOR_PURPLE,
+	},
+	[DNFT] = {
+		.name = "DNFT",
+		.description = "GTP forwarding rules via linux netfilter",
+		.enabled = 0, .loglevel = LOGL_NOTICE,
+		.color = OSMO_LOGCOLOR_PURPLE,
+	},
 };
 
 const struct log_info log_info = {
@@ -217,19 +254,27 @@ const struct log_info log_info = {
 int main(int argc, char **argv)
 {
 	int rc;
+	void *tall_infra_ctx;
 
 	/* Track the use of talloc NULL memory contexts */
 	talloc_enable_null_tracking();
 
 	osmo_fsm_set_dealloc_ctx(OTC_SELECT);
 
-	tall_upf_ctx = talloc_named_const(NULL, 1, "osmo-upf");
+	tall_infra_ctx = talloc_named_const(NULL, 1, "osmo-upf");
+	tall_upf_ctx = talloc_named_const(tall_infra_ctx, 1, "osmo-upf-main");
 	upf_vty_app_info.tall_ctx = tall_upf_ctx;
 
 	msgb_talloc_ctx_init(tall_upf_ctx, 0);
 	osmo_signal_talloc_ctx_init(tall_upf_ctx);
 
-	osmo_init_logging2(tall_upf_ctx, &log_info);
+	osmo_init_logging2(tall_infra_ctx, &log_info);
+	log_set_print_category_hex(osmo_stderr_target, 0);
+	log_set_print_category(osmo_stderr_target, 1);
+	log_set_print_level(osmo_stderr_target, 1);
+	log_set_print_filename2(osmo_stderr_target, LOG_FILENAME_BASENAME);
+	log_set_print_filename_pos(osmo_stderr_target, LOG_FILENAME_POS_LINE_END);
+	log_set_print_extended_timestamp(osmo_stderr_target, 1);
 
 	osmo_fsm_log_timeouts(true);
 	osmo_fsm_log_addr(true);
@@ -246,6 +291,9 @@ int main(int argc, char **argv)
 	osmo_talloc_vty_add_cmds();
 	osmo_cpu_sched_vty_init(tall_upf_ctx);
 
+	upf_vty_init();
+	osmo_tdef_vty_groups_init(CONFIG_NODE, g_upf_tdef_groups);
+
 	/* Parse options */
 	handle_options(argc, argv);
 
@@ -283,6 +331,18 @@ int main(int argc, char **argv)
 		}
 	}
 
+	if (upf_gtp_genl_open())
+		return -1;
+
+	if (upf_gtp_devs_open())
+		return -1;
+
+	if (upf_nft_init())
+		return -1;
+
+	if (upf_pfcp_listen())
+		return -1;
+
 	do {
 		log_reset_context();
 		osmo_select_main_ctx(0);
@@ -301,12 +361,22 @@ int main(int argc, char **argv)
 		}
 	} while (!osmo_select_shutdown_done());
 
-	log_fini();
+	up_endpoint_free(&g_upf->pfcp.ep);
+	upf_gtp_devs_close();
+
+	upf_gtp_genl_close();
+
+	upf_nft_free();
 
 	/* Report the heap state of talloc contexts, then free, so both ASAN and Valgrind are happy... */
 	talloc_report_full(tall_upf_ctx, stderr);
 	talloc_free(tall_upf_ctx);
 
+	log_fini();
+
+	talloc_report_full(tall_infra_ctx, stderr);
+	talloc_free(tall_infra_ctx);
+
 	talloc_report_full(tall_vty_ctx, stderr);
 	talloc_free(tall_vty_ctx);
 

src/osmo-upf/up_endpoint.c

@@ -0,0 +1,357 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
+#include <osmocom/pfcp/pfcp_msg.h>
+#include <osmocom/pfcp/pfcp_heartbeat_fsm.h>
+
+#include <osmocom/upf/up_endpoint.h>
+#include <osmocom/upf/up_peer.h>
+#include <osmocom/upf/up_session.h>
+
+static void up_endpoint_set_msg_ctx(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	struct up_endpoint *up_ep = ep->priv;
+	struct up_peer *peer;
+
+	/* From the remote address, find the matching peer instance */
+	if (!m->ctx.peer_fi) {
+		peer = up_peer_find(up_ep, &m->remote_addr);
+		if (peer) {
+			up_peer_set_msg_ctx(peer, m);
+		}
+	} else {
+		peer = m->ctx.peer_fi->priv;
+	}
+
+	/* Find a session, if the header is parsed yet and contains a SEID */
+	if (peer && !m->ctx.session_fi && m->h.seid_present) {
+		struct up_session *session;
+		session = up_session_find_by_up_seid(peer, m->h.seid);
+		if (session) {
+			up_session_set_msg_ctx(session, m);
+		}
+	}
+}
+
+static void up_ep_rx_not_impl_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m,
+				  enum osmo_pfcp_message_type resp_msgt,
+				  const struct osmo_pfcp_ie_node_id *node_id, enum osmo_pfcp_cause cause)
+{
+	struct osmo_pfcp_msg *tx;
+	enum osmo_pfcp_cause *tx_cause;
+	OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "message type not implemented\n");
+	tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, &up_ep->pfcp_ep->cfg.local_node_id, m, resp_msgt);
+	tx_cause = osmo_pfcp_msg_cause(tx);
+	if (tx_cause)
+		*tx_cause = cause;
+	osmo_pfcp_endpoint_tx(up_ep->pfcp_ep, tx);
+	return;
+}
+
+static void up_ep_rx_heartbeat_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	struct up_peer *peer = m->ctx.peer_fi ? m->ctx.peer_fi->priv : NULL;
+	/* osmo_pfcp_endpoint_handle_rx() has already taken care of the heartbeat response. Just dispatch the event
+	 * here. */
+	/* If the peer is not associated / not known, we don't care that a heartbeat happened. */
+	if (!peer || !peer->heartbeat_fi)
+		return;
+	osmo_fsm_inst_dispatch(peer->heartbeat_fi, OSMO_PFCP_HEARTBEAT_EV_RX_REQ, (void*)m);
+}
+
+static void up_ep_rx_heartbeat_resp(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	struct up_peer *peer = m->ctx.peer_fi ? m->ctx.peer_fi->priv : NULL;
+	if (!peer) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_INFO, "Heartbeat response from unknown peer %s\n",
+				  osmo_sockaddr_to_str_c(OTC_SELECT, &m->remote_addr));
+		return;
+	}
+	if (!peer->heartbeat_fi) {
+		OSMO_LOG_PFCP_MSG(m, LOGL_INFO, "Heartbeat response, but peer is not associated %s\n",
+				  osmo_sockaddr_to_str_c(OTC_SELECT, &m->remote_addr));
+		return;
+	}
+	osmo_fsm_inst_dispatch(peer->heartbeat_fi, OSMO_PFCP_HEARTBEAT_EV_RX_RESP, (void*)m);
+}
+
+static void up_ep_rx_pfd_mgmt_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	up_ep_rx_not_impl_req(up_ep, m, OSMO_PFCP_MSGT_PFD_MGMT_RESP, NULL, OSMO_PFCP_CAUSE_SERVICE_NOT_SUPPORTED);
+}
+
+static void up_ep_rx_assoc_setup_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	struct up_peer *peer = m->ctx.peer_fi ? m->ctx.peer_fi->priv : NULL;
+	if (!peer) {
+		peer = up_peer_find_or_add(up_ep, &m->remote_addr);
+		OSMO_ASSERT(peer);
+	}
+	osmo_fsm_inst_dispatch(peer->fi, UP_PEER_EV_RX_ASSOC_SETUP_REQ, (void*)m);
+}
+
+static void up_ep_rx_assoc_upd_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	if (!m->ctx.peer_fi) {
+		struct osmo_pfcp_msg *tx;
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Peer is not associated, cannot update association\n");
+		tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, &up_ep->pfcp_ep->cfg.local_node_id, m, OSMO_PFCP_MSGT_ASSOC_UPDATE_RESP);
+		/* FIXME set node_id, cause */
+		osmo_pfcp_endpoint_tx(up_ep->pfcp_ep, tx);
+		return;
+	}
+	osmo_fsm_inst_dispatch(m->ctx.peer_fi, UP_PEER_EV_RX_ASSOC_UPD_REQ, (void*)m);
+}
+
+static void up_ep_rx_assoc_rel_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	if (!m->ctx.peer_fi) {
+		struct osmo_pfcp_msg *tx;
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Peer is not associated. Sending ACK response anyway\n");
+		tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, &up_ep->pfcp_ep->cfg.local_node_id, m, OSMO_PFCP_MSGT_ASSOC_RELEASE_RESP);
+		/* FIXME set node_id, cause */
+		osmo_pfcp_endpoint_tx(up_ep->pfcp_ep, tx);
+		return;
+	}
+	osmo_fsm_inst_dispatch(m->ctx.peer_fi, UP_PEER_EV_RX_ASSOC_REL_REQ, (void*)m);
+}
+
+static void up_ep_rx_node_report_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	up_ep_rx_not_impl_req(up_ep, m, OSMO_PFCP_MSGT_NODE_REPORT_RESP, NULL /* FIXME? */,
+			      OSMO_PFCP_CAUSE_SERVICE_NOT_SUPPORTED);
+}
+
+static void up_ep_rx_session_set_del_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	up_ep_rx_not_impl_req(up_ep, m, OSMO_PFCP_MSGT_SESSION_SET_DEL_RESP,
+			      &up_ep->pfcp_ep->cfg.local_node_id, OSMO_PFCP_CAUSE_SERVICE_NOT_SUPPORTED);
+}
+
+static void up_ep_rx_session_est_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	if (!m->ctx.peer_fi) {
+		struct osmo_pfcp_msg *tx;
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Peer is not associated, cannot establish session\n");
+		tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, &up_ep->pfcp_ep->cfg.local_node_id, m, OSMO_PFCP_MSGT_SESSION_EST_RESP);
+		tx->ies.session_est_resp.cause = OSMO_PFCP_CAUSE_NO_ESTABLISHED_PFCP_ASSOC;
+		osmo_pfcp_endpoint_tx(up_ep->pfcp_ep, tx);
+		return;
+	}
+	osmo_fsm_inst_dispatch(m->ctx.peer_fi, UP_PEER_EV_RX_SESSION_EST_REQ, (void*)m);
+}
+
+static void up_ep_rx_session_mod_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	if (!m->ctx.session_fi) {
+		/* Session not found. */
+		struct osmo_pfcp_msg *tx;
+		tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, &up_ep->pfcp_ep->cfg.local_node_id, m,
+					    OSMO_PFCP_MSGT_SESSION_MOD_RESP);
+		if (!m->ctx.peer_fi) {
+			/* Not even the peer is associated. */
+			OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Peer is not associated, cannot modify session\n");
+			tx->ies.session_mod_resp.cause = OSMO_PFCP_CAUSE_NO_ESTABLISHED_PFCP_ASSOC;
+		} else {
+			OSMO_LOG_PFCP_MSG(m, LOGL_ERROR,
+					  "No established session with SEID=0x%"PRIx64", cannot modify\n",
+					  m->h.seid);
+			tx->ies.session_mod_resp.cause = OSMO_PFCP_CAUSE_SESSION_CTX_NOT_FOUND;
+		}
+		osmo_pfcp_endpoint_tx(up_ep->pfcp_ep, tx);
+		return;
+	}
+	osmo_fsm_inst_dispatch(m->ctx.session_fi, UP_SESSION_EV_RX_SESSION_MOD_REQ, (void*)m);
+}
+
+static void up_ep_rx_session_del_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	if (!m->ctx.session_fi) {
+		/* Session not found. */
+		struct osmo_pfcp_msg *tx;
+		tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, NULL, &up_ep->pfcp_ep->cfg.local_node_id, m, OSMO_PFCP_MSGT_SESSION_DEL_RESP);
+		if (!m->ctx.peer_fi) {
+			/* Not even the peer is associated. */
+			OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Peer is not associated, cannot delete session\n");
+			tx->ies.session_del_resp.cause = OSMO_PFCP_CAUSE_NO_ESTABLISHED_PFCP_ASSOC;
+		} else {
+			OSMO_LOG_PFCP_MSG(m, LOGL_ERROR,
+					  "No established session with SEID=0x%"PRIx64", cannot delete\n",
+					  m->h.seid);
+			tx->ies.session_del_resp.cause = OSMO_PFCP_CAUSE_SESSION_CTX_NOT_FOUND;
+		}
+		osmo_pfcp_endpoint_tx(up_ep->pfcp_ep, tx);
+		return;
+	}
+	osmo_fsm_inst_dispatch(m->ctx.session_fi, UP_SESSION_EV_RX_SESSION_DEL_REQ, (void*)m);
+}
+
+static void up_ep_rx_session_rep_req(struct up_endpoint *up_ep, const struct osmo_pfcp_msg *m)
+{
+	up_ep_rx_not_impl_req(up_ep, m, OSMO_PFCP_MSGT_SESSION_REP_RESP, NULL, OSMO_PFCP_CAUSE_SERVICE_NOT_SUPPORTED);
+}
+
+static void up_endpoint_rx_cb(struct osmo_pfcp_endpoint *ep, struct osmo_pfcp_msg *m)
+{
+	struct up_endpoint *up_ep = ep->priv;
+
+	switch (m->h.message_type) {
+	case OSMO_PFCP_MSGT_HEARTBEAT_REQ:
+		up_ep_rx_heartbeat_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_HEARTBEAT_RESP:
+		up_ep_rx_heartbeat_resp(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_PFD_MGMT_REQ:
+		up_ep_rx_pfd_mgmt_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_ASSOC_SETUP_REQ:
+		up_ep_rx_assoc_setup_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_ASSOC_UPDATE_REQ:
+		up_ep_rx_assoc_upd_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_ASSOC_RELEASE_REQ:
+		up_ep_rx_assoc_rel_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_NODE_REPORT_REQ:
+		up_ep_rx_node_report_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_SESSION_SET_DEL_REQ:
+		up_ep_rx_session_set_del_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_SESSION_EST_REQ:
+		up_ep_rx_session_est_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_SESSION_MOD_REQ:
+		up_ep_rx_session_mod_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_SESSION_DEL_REQ:
+		up_ep_rx_session_del_req(up_ep, m);
+		return;
+	case OSMO_PFCP_MSGT_SESSION_REP_REQ:
+		up_ep_rx_session_rep_req(up_ep, m);
+		return;
+	default:
+		OSMO_LOG_PFCP_MSG(m, LOGL_ERROR, "Unknown message type\n");
+		return;
+	}
+}
+
+struct up_endpoint *up_endpoint_init(void *ctx, const struct osmo_sockaddr *local_addr)
+{
+	int rc;
+	struct up_endpoint *up_ep;
+	up_ep = talloc_zero(ctx, struct up_endpoint);
+	INIT_LLIST_HEAD(&up_ep->peers);
+
+	up_ep->pfcp_ep = osmo_pfcp_endpoint_create(up_ep, up_ep);
+	up_ep->pfcp_ep->cfg.local_addr = *local_addr;
+
+	up_ep->pfcp_ep->set_msg_ctx = up_endpoint_set_msg_ctx;
+	up_ep->pfcp_ep->rx_msg = up_endpoint_rx_cb;
+
+	osmo_pfcp_ie_node_id_from_osmo_sockaddr(&up_ep->pfcp_ep->cfg.local_node_id, local_addr);
+
+	rc = osmo_pfcp_endpoint_bind(up_ep->pfcp_ep);
+	if (rc) {
+		talloc_free(up_ep);
+		return NULL;
+	}
+	return up_ep;
+}
+
+static struct up_session *up_endpoint_find_session(struct up_endpoint *ep, uint64_t up_seid)
+{
+		struct up_peer *peer;
+		llist_for_each_entry(peer, &ep->peers, entry) {
+			struct up_session *session = up_session_find_by_up_seid(peer, up_seid);
+			if (session)
+				return session;
+		}
+		return NULL;
+}
+
+static struct up_session *up_endpoint_find_session_by_local_teid(struct up_endpoint *ep, uint32_t teid)
+{
+		struct up_peer *peer;
+		llist_for_each_entry(peer, &ep->peers, entry) {
+			struct up_session *session = up_session_find_by_local_teid(peer, teid);
+			if (session)
+				return session;
+		}
+		return NULL;
+}
+
+static uint64_t up_endpoint_inc_seid(struct up_endpoint *ep)
+{
+	ep->next_seid_state++;
+	if (!ep->next_seid_state)
+		ep->next_seid_state++;
+	return ep->next_seid_state;
+}
+
+uint64_t up_endpoint_next_seid(struct up_endpoint *ep)
+{
+	uint64_t sanity;
+	for (sanity = 2342; sanity; sanity--) {
+		uint64_t next_seid = up_endpoint_inc_seid(ep);
+		if (up_endpoint_find_session(ep, next_seid))
+			continue;
+		return next_seid;
+	}
+	return 0;
+}
+
+static uint32_t up_endpoint_inc_teid(struct up_endpoint *ep)
+{
+	ep->next_teid_state++;
+	if (!ep->next_teid_state)
+		ep->next_teid_state++;
+	return ep->next_teid_state;
+}
+
+uint32_t up_endpoint_next_teid(struct up_endpoint *ep)
+{
+	uint32_t sanity;
+	for (sanity = 2342; sanity; sanity--) {
+		uint32_t next_teid = up_endpoint_inc_teid(ep);
+		if (up_endpoint_find_session_by_local_teid(ep, next_teid))
+			continue;
+		return next_teid;
+	}
+	return 0;
+}
+
+void up_endpoint_free(struct up_endpoint **_ep)
+{
+	struct up_peer *peer;
+	struct up_endpoint *ep = *_ep;
+
+	while ((peer = llist_first_entry_or_null(&ep->peers, struct up_peer, entry)))
+		up_peer_free(peer);
+
+	osmo_pfcp_endpoint_free(&ep->pfcp_ep);
+	*_ep = NULL;
+}

src/osmo-upf/up_gtp_action.c

@@ -0,0 +1,186 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <errno.h>
+#include <string.h>
+
+#include <osmocom/core/utils.h>
+
+#include <osmocom/upf/upf.h>
+#include <osmocom/upf/up_gtp_action.h>
+#include <osmocom/upf/up_peer.h>
+#include <osmocom/upf/up_session.h>
+
+int up_gtp_action_cmp(const struct up_gtp_action *a, const struct up_gtp_action *b)
+{
+	int cmp;
+	if (a == b)
+		return 0;
+	if (!a)
+		return -1;
+	if (!b)
+		return 1;
+
+#define CMP_RET(MEMB) do { \
+		int _cmp = OSMO_CMP(a->MEMB, b->MEMB); \
+		if (_cmp) \
+			return _cmp; \
+	} while (0)
+
+	CMP_RET(kind);
+
+	switch (a->kind) {
+	case UP_GTP_U_ENDECAPS:
+		CMP_RET(endecaps.local_teid);
+		CMP_RET(endecaps.remote_teid);
+		cmp = osmo_sockaddr_cmp(&a->endecaps.gtp_remote_addr, &b->endecaps.gtp_remote_addr);
+		if (cmp)
+			return cmp;
+		cmp = osmo_sockaddr_cmp(&a->endecaps.ue_addr, &b->endecaps.ue_addr);
+		if (cmp)
+			return cmp;
+		break;
+
+	case UP_GTP_U_TUNMAP:
+		CMP_RET(tunmap.access.local_teid);
+		CMP_RET(tunmap.access.remote_teid);
+		CMP_RET(tunmap.core.local_teid);
+		CMP_RET(tunmap.core.remote_teid);
+		break;
+	default:
+		break;
+	}
+	return 0;
+}
+
+static int up_gtp_action_enable_disable(struct up_gtp_action *a, bool enable)
+{
+	struct upf_gtp_dev *gtp_dev;
+	int rc;
+
+	switch (a->kind) {
+	case UP_GTP_U_ENDECAPS:
+		/* use the first available GTP device.
+		 * TODO: select by interface name?
+		 */
+		gtp_dev = upf_gtp_dev_first();
+		if (!gtp_dev) {
+			LOG_UP_GTP_ACTION(a, LOGL_ERROR, "No GTP device open, cannot %s\n", enable ? "enable" : "disable");
+			return -EIO;
+		}
+
+		if (enable)
+			rc = upf_gtp_dev_tunnel_add(gtp_dev, &a->endecaps);
+		else
+			rc = upf_gtp_dev_tunnel_del(gtp_dev, &a->endecaps);
+		if (rc) {
+			LOG_UP_GTP_ACTION(a, LOGL_ERROR, "Failed to %s GTP tunnel: %d %s\n",
+					  enable ? "enable" : "disable", rc, strerror(-rc));
+			return rc;
+		}
+		LOG_UP_GTP_ACTION(a, LOGL_NOTICE, "%s GTP tunnel\n", enable ? "Enabled" : "Disabled");
+		return 0;
+
+	case UP_GTP_U_TUNMAP:
+		if (enable && a->tunmap.id != 0) {
+			LOG_UP_GTP_ACTION(a, LOGL_ERROR,
+					  "Cannot enable: nft GTP tunnel mapping rule has been enabled before"
+					  " as " NFT_CHAIN_NAME_PREFIX_TUNMAP "%u\n", a->tunmap.id);
+			return -EALREADY;
+		}
+		if (!enable && a->tunmap.id == 0) {
+			LOG_UP_GTP_ACTION(a, LOGL_ERROR,
+					  "Cannot disable: nft GTP tunnel mapping rule has not been enabled"
+					  " (no " NFT_CHAIN_NAME_PREFIX_TUNMAP " id)\n");
+			return -ENOENT;
+		}
+		if (enable)
+			rc = upf_nft_tunmap_create(&a->tunmap);
+		else
+			rc = upf_nft_tunmap_delete(&a->tunmap);
+		if (rc) {
+			LOG_UP_GTP_ACTION(a, LOGL_ERROR,
+					  "Failed to %s nft GTP tunnel mapping " NFT_CHAIN_NAME_PREFIX_TUNMAP "%u:"
+					  " %d %s\n", enable ? "enable" : "disable", a->tunmap.id, rc, strerror(-rc));
+			return rc;
+		}
+		LOG_UP_GTP_ACTION(a, LOGL_NOTICE, "%s nft GTP tunnel mapping " NFT_CHAIN_NAME_PREFIX_TUNMAP "%u\n",
+				  enable ? "Enabled" : "Disabled", a->tunmap.id);
+		if (!enable)
+			a->tunmap.id = 0;
+		return 0;
+
+	default:
+		LOG_UP_GTP_ACTION(a, LOGL_ERROR, "Invalid action\n");
+		return -ENOTSUP;
+	}
+}
+
+int up_gtp_action_enable(struct up_gtp_action *a)
+{
+	return up_gtp_action_enable_disable(a, true);
+}
+
+int up_gtp_action_disable(struct up_gtp_action *a)
+{
+	return up_gtp_action_enable_disable(a, false);
+}
+
+int up_gtp_action_to_str_buf(char *buf, size_t buflen, const struct up_gtp_action *a)
+{
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+	switch (a->kind) {
+	case UP_GTP_U_ENDECAPS:
+		OSMO_STRBUF_PRINTF(sb, "GTP:endecaps GTP-access:");
+		OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &a->endecaps.gtp_remote_addr);
+		OSMO_STRBUF_PRINTF(sb, " TEID-r:0x%"PRIx32" TEID-l:0x%"PRIx32" IP-core:",
+				   a->endecaps.remote_teid, a->endecaps.local_teid);
+		OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &a->endecaps.ue_addr);
+		break;
+	case UP_GTP_U_TUNMAP:
+		OSMO_STRBUF_PRINTF(sb, "GTP:tunmap GTP-access:");
+		OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &a->tunmap.access.gtp_remote_addr);
+		OSMO_STRBUF_PRINTF(sb, " TEID-access-r:0x%"PRIx32" TEID-access-l:0x%"PRIx32" GTP-core:",
+				   a->tunmap.access.remote_teid, a->tunmap.access.local_teid);
+		OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &a->tunmap.core.gtp_remote_addr);
+		OSMO_STRBUF_PRINTF(sb, " TEID-core-r:0x%"PRIx32" TEID-core-l:0x%"PRIx32,
+				   a->tunmap.core.remote_teid, a->tunmap.core.local_teid);
+		break;
+	case UP_GTP_DROP:
+		OSMO_STRBUF_PRINTF(sb, "GTP:drop");
+		break;
+	default:
+		OSMO_STRBUF_PRINTF(sb, "GTP:?");
+		break;
+	}
+	if (a->session)
+		OSMO_STRBUF_PRINTF(sb, " PFCP-peer:%s SEID-l:0x%"PRIx64" PDR:%d,%d",
+				   up_peer_remote_addr_str(a->session->up_peer),
+				   a->session->up_seid, a->pdr_core, a->pdr_access);
+	return sb.chars_needed;
+}
+
+char *up_gtp_action_to_str_c(void *ctx, const struct up_gtp_action *a)
+{
+	OSMO_NAME_C_IMPL(ctx, 128, "ERROR", up_gtp_action_to_str_buf, a)
+}

src/osmo-upf/up_peer.c

@@ -0,0 +1,545 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <errno.h>
+
+#include <osmocom/core/fsm.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/tdef.h>
+#include <osmocom/core/utils.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+#include <osmocom/pfcp/pfcp_endpoint.h>
+
+#include <osmocom/upf/upf.h>
+#include <osmocom/upf/up_peer.h>
+#include <osmocom/upf/up_endpoint.h>
+#include <osmocom/upf/up_session.h>
+
+enum up_peer_fsm_state {
+	UP_PEER_ST_NOT_ASSOCIATED,
+	UP_PEER_ST_ASSOCIATED,
+	UP_PEER_ST_GRACEFUL_RELEASE,
+	UP_PEER_ST_WAIT_USE_COUNT,
+};
+
+static const struct value_string up_peer_fsm_event_names[] = {
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_ASSOC_SETUP_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_ASSOC_UPD_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_ASSOC_REL_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_SESSION_EST_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_HEARTBEAT_FAILURE),
+	OSMO_VALUE_STRING(UP_PEER_EV_USE_COUNT_ZERO),
+	OSMO_VALUE_STRING(UP_PEER_EV_SESSION_TERM),
+	{}
+};
+
+static struct osmo_fsm up_peer_fsm;
+
+static const struct osmo_tdef_state_timeout up_peer_fsm_timeouts[32] = {
+	[UP_PEER_ST_GRACEFUL_RELEASE] = { .T = -21 },
+};
+
+/* Transition to a state, using the T timer defined in up_peer_fsm_timeouts.
+ * Assumes local variable fi exists. */
+#define up_peer_fsm_state_chg(state) \
+	osmo_tdef_fsm_inst_state_chg(fi, state, \
+				     up_peer_fsm_timeouts, \
+				     osmo_pfcp_tdefs, \
+				     5)
+
+static int up_peer_use_cb(struct osmo_use_count_entry *e, int32_t old_use_count, const char *file, int line)
+{
+	struct up_peer *peer = e->use_count->talloc_object;
+	int32_t total;
+	int level;
+
+	if (!e->use)
+		return -EINVAL;
+
+	total = osmo_use_count_total(&peer->use_count);
+
+	if (total == 0
+	    || (total == 1 && old_use_count == 0 && e->count == 1))
+		level = LOGL_INFO;
+	else
+		level = LOGL_DEBUG;
+
+	LOGPFSMSLSRC(peer->fi, DREF, level, file, line,
+		     "%s %s: now used by %s\n",
+		     (e->count - old_use_count) > 0? "+" : "-", e->use,
+		     osmo_use_count_to_str_c(OTC_SELECT, &peer->use_count));
+
+	if (e->count < 0)
+		return -ERANGE;
+
+	if (total == 0)
+		osmo_fsm_inst_dispatch(peer->fi, UP_PEER_EV_USE_COUNT_ZERO, NULL);
+	return 0;
+}
+
+char *up_peer_remote_addr_str(struct up_peer *peer)
+{
+	struct osmo_sockaddr remote_addr = peer->remote_addr;
+#if 1
+	/* Zero the port, it is not interesting information. The port for PFCP is defined fixed, and there is no use
+	 * printing it in the logs */
+	osmo_sockaddr_set_port(&remote_addr.u.sa, 0);
+#endif
+	return osmo_sockaddr_to_str_c(OTC_SELECT, &remote_addr);
+}
+
+static void up_peer_update_id(struct up_peer *peer)
+{
+	osmo_fsm_inst_update_id_f_sanitize(peer->fi, '-', "%s", up_peer_remote_addr_str(peer));
+	LOGPFSML(peer->fi, LOGL_DEBUG, "Updated id\n");
+}
+
+static struct up_peer *up_peer_add(struct up_endpoint *up_endpoint, const struct osmo_sockaddr *remote_addr)
+{
+	struct up_peer *peer;
+
+	struct osmo_fsm_inst *fi = osmo_fsm_inst_alloc(&up_peer_fsm, up_endpoint, NULL, LOGL_DEBUG, NULL);
+	OSMO_ASSERT(fi);
+
+	peer = talloc(fi, struct up_peer);
+	OSMO_ASSERT(peer);
+	fi->priv = peer;
+
+	*peer = (struct up_peer) {
+		.fi = fi,
+		.up_endpoint = up_endpoint,
+		.remote_addr = *remote_addr,
+		.heartbeat_fi = NULL /* FIXME */,
+		.use_count = {
+			.talloc_object = peer,
+			.use_cb = up_peer_use_cb,
+		},
+	};
+	osmo_use_count_make_static_entries(&peer->use_count, peer->use_count_buf, ARRAY_SIZE(peer->use_count_buf));
+	hash_init(peer->sessions_by_up_seid);
+	hash_init(peer->sessions_by_cp_seid);
+
+	osmo_pfcp_bits_set(peer->local_up_features.bits, OSMO_PFCP_UP_FEAT_BUNDL, true);
+	osmo_pfcp_bits_set(peer->local_up_features.bits, OSMO_PFCP_UP_FEAT_RTTL, true);
+	osmo_pfcp_bits_set(peer->local_up_features.bits, OSMO_PFCP_UP_FEAT_FTUP, true);
+
+	up_peer_update_id(peer);
+
+	llist_add(&peer->entry, &up_endpoint->peers);
+	return peer;
+}
+
+struct up_peer *up_peer_find(struct up_endpoint *up_endpoint, const struct osmo_sockaddr *remote_addr)
+{
+	struct up_peer *peer;
+	llist_for_each_entry(peer, &up_endpoint->peers, entry) {
+		if (osmo_sockaddr_cmp(&peer->remote_addr, remote_addr))
+			continue;
+		return peer;
+	}
+	return NULL;
+}
+
+struct up_peer *up_peer_find_or_add(struct up_endpoint *up_endpoint, const struct osmo_sockaddr *remote_addr)
+{
+	struct up_peer *peer = up_peer_find(up_endpoint, remote_addr);
+	if (peer)
+		return peer;
+	return up_peer_add(up_endpoint, remote_addr);
+}
+
+int up_peer_tx(struct up_peer *peer, struct osmo_pfcp_msg *m)
+{
+	return osmo_pfcp_endpoint_tx(peer->up_endpoint->pfcp_ep, m);
+}
+
+static int up_peer_fsm_timer_cb(struct osmo_fsm_inst *fi)
+{
+	//struct up_peer *peer = fi->priv;
+	/* Return 1 to terminate FSM instance, 0 to keep running */
+	return 1;
+}
+
+void up_peer_set_msg_ctx(struct up_peer *peer, struct osmo_pfcp_msg *m)
+{
+	OSMO_ASSERT(!m->ctx.peer_fi);
+
+	m->ctx.peer_fi = peer->fi;
+	m->ctx.peer_use_count = &peer->use_count;
+	m->ctx.peer_use_token = (m->rx ? UP_USE_MSG_RX : UP_USE_MSG_TX);
+	osmo_use_count_get_put(m->ctx.peer_use_count, m->ctx.peer_use_token, 1);
+}
+
+struct osmo_pfcp_msg *up_peer_init_tx(struct up_peer *peer, struct osmo_pfcp_msg *in_reply_to,
+				      enum osmo_pfcp_message_type message_type)
+{
+	struct osmo_pfcp_msg *tx = osmo_pfcp_msg_alloc_tx(OTC_SELECT, &peer->remote_addr,
+							  &peer->up_endpoint->pfcp_ep->cfg.local_node_id,
+							  in_reply_to, message_type);
+	up_peer_set_msg_ctx(peer, tx);
+	return tx;
+}
+
+static int up_peer_tx_assoc_setup_resp(struct up_peer *peer, struct osmo_pfcp_msg *m, enum osmo_pfcp_cause cause)
+{
+	struct osmo_pfcp_msg *resp;
+
+	resp = up_peer_init_tx(peer, m, OSMO_PFCP_MSGT_ASSOC_SETUP_RESP);
+
+	resp->ies.assoc_setup_resp = (struct osmo_pfcp_msg_assoc_setup_resp) {
+		.cause = cause,
+		.recovery_time_stamp = g_upf->pfcp.ep->pfcp_ep->recovery_time_stamp,
+		.up_function_features_present = true,
+		.up_function_features = peer->local_up_features,
+	};
+	resp->ies.assoc_setup_resp.recovery_time_stamp = g_upf->pfcp.ep->pfcp_ep->recovery_time_stamp;
+
+	if (osmo_pfcp_endpoint_tx(peer->up_endpoint->pfcp_ep, resp)) {
+		OSMO_LOG_PFCP_MSG(resp, LOGL_ERROR, "Error sending response, cannot associate with peer\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static int up_peer_tx_assoc_rel_resp(struct up_peer *peer, struct osmo_pfcp_msg *m, enum osmo_pfcp_cause cause)
+{
+	struct osmo_pfcp_msg *resp;
+
+	resp = up_peer_init_tx(peer, m, OSMO_PFCP_MSGT_ASSOC_RELEASE_RESP);
+
+	resp->ies.assoc_release_resp = (struct osmo_pfcp_msg_assoc_release_resp) {
+		.cause = cause,
+	};
+
+	if (osmo_pfcp_endpoint_tx(peer->up_endpoint->pfcp_ep, resp)) {
+		OSMO_LOG_PFCP_MSG(resp, LOGL_ERROR, "Error sending response\n");
+		return -EIO;
+	}
+	return 0;
+}
+
+static void up_peer_clear_sessions(struct up_peer *peer)
+{
+	struct up_session *session;
+	int bkt;
+	struct hlist_node *tmp;
+	int count = 0;
+	hash_for_each_safe(peer->sessions_by_up_seid, bkt, tmp, session, node_by_up_seid) {
+		count += up_session_discard(session);
+	}
+	if (count)
+		LOGPFSML(peer->fi, LOGL_NOTICE, "terminated %d sessions\n", count);
+}
+
+static void up_peer_rx_assoc_setup_req(struct up_peer *peer, struct osmo_pfcp_msg *m)
+{
+	struct osmo_fsm_inst *fi = peer->fi;
+	enum osmo_pfcp_cause cause = OSMO_PFCP_CAUSE_REQUEST_ACCEPTED;
+
+	if (fi->state == UP_PEER_ST_ASSOCIATED) {
+		/* Retransmissions of the ACK response happen in pfcp_endpoint.c. So if we get this, it is a genuine
+		 * duplicate association setup request. We could reject it. But why. Just "replace" with the new
+		 * association. Continue. */
+		/* If the peer has restarted, it has forgotten about all sessions. */
+		if (peer->remote_recovery_timestamp != m->ies.assoc_setup_req.recovery_time_stamp) {
+			LOGPFSML(fi, LOGL_NOTICE, "another Association Setup Request, with different Recovery Timestamp."
+				 " Clearing sessions, sending ACK.\n");
+			up_peer_clear_sessions(peer);
+		} else {
+			LOGPFSML(fi, LOGL_NOTICE, "another Association Setup Request, with same Recovery Timestamp."
+				 " Keeping sessions, sending ACK.\n");
+		}
+	} else if (up_peer_fsm_state_chg(UP_PEER_ST_ASSOCIATED)) {
+		/* Not allowed to transition to ST_ASSOCIATED */
+		cause = OSMO_PFCP_CAUSE_REQUEST_REJECTED;
+	} else {
+		/* Successfully transitioned to ST_ASSOCIATED */
+		peer->remote_recovery_timestamp = m->ies.assoc_setup_req.recovery_time_stamp;
+		peer->remote_node_id = m->ies.assoc_setup_req.node_id;
+		if (m->ies.assoc_setup_req.cp_function_features_present)
+			peer->peer_cp_features = m->ies.assoc_setup_req.cp_function_features;
+	}
+
+	if (up_peer_tx_assoc_setup_resp(peer, m, cause)
+	    || cause != OSMO_PFCP_CAUSE_REQUEST_ACCEPTED)
+		up_peer_fsm_state_chg(UP_PEER_ST_WAIT_USE_COUNT);
+
+	LOGPFSML(fi, LOGL_NOTICE, "Peer associated, Node-Id=%s. Local UP features: [%s]; Peer CP features: [%s]\n",
+		 osmo_pfcp_ie_node_id_to_str_c(OTC_SELECT, &peer->remote_node_id),
+		 osmo_pfcp_bits_to_str_c(OTC_SELECT, peer->local_up_features.bits, osmo_pfcp_up_feature_strs),
+		 osmo_pfcp_bits_to_str_c(OTC_SELECT, peer->peer_cp_features.bits, osmo_pfcp_cp_feature_strs));
+}
+
+static void up_peer_rx_assoc_rel_req(struct up_peer *peer, struct osmo_pfcp_msg *m)
+{
+	struct osmo_fsm_inst *fi = peer->fi;
+	up_peer_tx_assoc_rel_resp(peer, m, OSMO_PFCP_CAUSE_REQUEST_ACCEPTED);
+	up_peer_fsm_state_chg(UP_PEER_ST_WAIT_USE_COUNT);
+}
+
+static void up_peer_rx_session_est_req(struct up_peer *peer, struct osmo_pfcp_msg *m)
+{
+	enum osmo_pfcp_cause cause = OSMO_PFCP_CAUSE_REQUEST_ACCEPTED;
+	struct osmo_pfcp_msg *resp;
+	struct up_session *session = up_session_find_or_add(peer, &m->ies.session_est_req.cp_f_seid, NULL);
+
+	if (!session) {
+		cause = OSMO_PFCP_CAUSE_NO_RESOURCES_AVAILABLE;
+		goto nack_response;
+	}
+
+	up_session_set_msg_ctx(session, m);
+
+	if (osmo_fsm_inst_dispatch(session->fi, UP_SESSION_EV_RX_SESSION_EST_REQ, m)) {
+		cause = OSMO_PFCP_CAUSE_REQUEST_REJECTED;
+		goto nack_response;
+	}
+	return;
+
+nack_response:
+	resp = up_peer_init_tx(peer, m, OSMO_PFCP_MSGT_SESSION_EST_RESP);
+	resp->h.seid = m->ies.session_est_req.cp_f_seid.seid;
+	resp->h.seid_present = true;
+	resp->ies.session_est_resp = (struct osmo_pfcp_msg_session_est_resp){
+		.cause = cause,
+	};
+	osmo_pfcp_endpoint_tx(peer->up_endpoint->pfcp_ep, resp);
+}
+
+static void up_peer_not_associated_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	struct up_peer *peer = fi->priv;
+
+	switch (event) {
+
+	case UP_PEER_EV_RX_ASSOC_SETUP_REQ:
+		up_peer_rx_assoc_setup_req(peer, data);
+		break;
+
+	case UP_PEER_EV_USE_COUNT_ZERO:
+		/* Not associated and no pending messages. discard peer. */
+		up_peer_fsm_state_chg(UP_PEER_ST_WAIT_USE_COUNT);
+		return;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void up_peer_associated_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	struct up_peer *peer = fi->priv;
+
+	switch (event) {
+
+	case UP_PEER_EV_RX_ASSOC_SETUP_REQ:
+		up_peer_rx_assoc_setup_req(peer, data);
+		break;
+
+	case UP_PEER_EV_RX_ASSOC_UPD_REQ:
+		// FIXME
+		break;
+
+	case UP_PEER_EV_RX_SESSION_EST_REQ:
+		up_peer_rx_session_est_req(peer, data);
+		break;
+
+	case UP_PEER_EV_HEARTBEAT_FAILURE:
+		// FIXME
+		break;
+
+	case UP_PEER_EV_USE_COUNT_ZERO:
+		/* Stay associated. */
+		return;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void up_peer_associated_onleave(struct osmo_fsm_inst *fi, uint32_t next_state)
+{
+	struct up_peer *peer = fi->priv;
+	if (next_state != UP_PEER_ST_ASSOCIATED)
+		LOGPFSML(fi, LOGL_NOTICE, "Peer %s released\n",
+			 osmo_pfcp_ie_node_id_to_str_c(OTC_SELECT, &peer->remote_node_id));
+}
+
+static void up_peer_graceful_release_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+	//struct up_peer *peer = fi->priv;
+	// FIXME
+}
+
+static void up_peer_graceful_release_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	struct up_peer *peer = fi->priv;
+
+	switch (event) {
+
+	case UP_PEER_EV_HEARTBEAT_FAILURE:
+		up_peer_fsm_state_chg(UP_PEER_ST_WAIT_USE_COUNT);
+		break;
+
+	case UP_PEER_EV_USE_COUNT_ZERO:
+		/* When there are still sessions, stay around. */
+		if (!hash_empty(peer->sessions_by_up_seid))
+			return;
+		up_peer_fsm_state_chg(UP_PEER_ST_WAIT_USE_COUNT);
+		return;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void up_peer_wait_use_count_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+	struct up_peer *peer = fi->priv;
+	up_peer_clear_sessions(peer);
+	if (!osmo_use_count_total(&peer->use_count))
+		osmo_fsm_inst_term(fi, OSMO_FSM_TERM_REGULAR, NULL);
+}
+
+static void up_peer_wait_use_count_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	struct up_peer *peer = fi->priv;
+	switch (event) {
+
+	case UP_PEER_EV_USE_COUNT_ZERO:
+		osmo_fsm_inst_term(fi, OSMO_FSM_TERM_REGULAR, NULL);
+		return;
+
+	case UP_PEER_EV_RX_ASSOC_SETUP_REQ:
+		up_peer_rx_assoc_setup_req(peer, data);
+		break;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+#define S(x)    (1 << (x))
+
+static const struct osmo_fsm_state up_peer_fsm_states[] = {
+	[UP_PEER_ST_NOT_ASSOCIATED] = {
+		.name = "NOT_ASSOCIATED",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_RX_ASSOC_SETUP_REQ)
+			| S(UP_PEER_EV_USE_COUNT_ZERO)
+			,
+		.out_state_mask = 0
+			| S(UP_PEER_ST_ASSOCIATED)
+			| S(UP_PEER_ST_WAIT_USE_COUNT)
+			,
+		.action = up_peer_not_associated_action,
+	},
+	[UP_PEER_ST_ASSOCIATED] = {
+		.name = "ASSOCIATED",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_RX_ASSOC_SETUP_REQ)
+			| S(UP_PEER_EV_RX_ASSOC_UPD_REQ)
+			| S(UP_PEER_EV_RX_SESSION_EST_REQ)
+			| S(UP_PEER_EV_HEARTBEAT_FAILURE)
+			| S(UP_PEER_EV_USE_COUNT_ZERO)
+			,
+		.out_state_mask = 0
+			| S(UP_PEER_ST_ASSOCIATED)
+			| S(UP_PEER_ST_GRACEFUL_RELEASE)
+			| S(UP_PEER_ST_WAIT_USE_COUNT)
+			,
+		.action = up_peer_associated_action,
+		.onleave = up_peer_associated_onleave,
+	},
+	[UP_PEER_ST_GRACEFUL_RELEASE] = {
+		.name = "GRACEFUL_RELEASE",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_HEARTBEAT_FAILURE)
+			| S(UP_PEER_EV_USE_COUNT_ZERO)
+			,
+		.out_state_mask = 0
+			| S(UP_PEER_ST_WAIT_USE_COUNT)
+			,
+		.onenter = up_peer_graceful_release_onenter,
+		.action = up_peer_graceful_release_action,
+	},
+	[UP_PEER_ST_WAIT_USE_COUNT] = {
+		.name = "WAIT_USE_COUNT",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_USE_COUNT_ZERO)
+			| S(UP_PEER_EV_RX_ASSOC_SETUP_REQ)
+			,
+		.out_state_mask = 0
+			| S(UP_PEER_ST_ASSOCIATED)
+			,
+		.onenter = up_peer_wait_use_count_onenter,
+		.action = up_peer_wait_use_count_action,
+	},
+};
+
+void up_peer_fsm_cleanup(struct osmo_fsm_inst *fi, enum osmo_fsm_term_cause cause)
+{
+	struct up_peer *peer = fi->priv;
+	LOGPFSML(fi, LOGL_NOTICE, "Peer removed\n");
+	llist_del(&peer->entry);
+}
+
+static void up_peer_allstate_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	switch (event) {
+	case UP_PEER_EV_SESSION_TERM:
+		/* ignore */
+		return;
+	case UP_PEER_EV_RX_ASSOC_REL_REQ:
+		up_peer_rx_assoc_rel_req(fi->priv, data);
+		return;
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static struct osmo_fsm up_peer_fsm = {
+	.name = "up_peer",
+	.log_subsys = DPEER,
+	.states = up_peer_fsm_states,
+	.num_states = ARRAY_SIZE(up_peer_fsm_states),
+	.event_names = up_peer_fsm_event_names,
+	.timer_cb = up_peer_fsm_timer_cb,
+	.cleanup = up_peer_fsm_cleanup,
+	.allstate_event_mask = 0
+		| S(UP_PEER_EV_RX_ASSOC_REL_REQ)
+		| S(UP_PEER_EV_SESSION_TERM)
+		,
+	.allstate_action = up_peer_allstate_action,
+};
+
+static __attribute__((constructor)) void up_peer_fsm_register(void)
+{
+	OSMO_ASSERT(osmo_fsm_register(&up_peer_fsm) == 0);
+}
+
+void up_peer_free(struct up_peer *peer)
+{
+	osmo_fsm_inst_term(peer->fi, OSMO_FSM_TERM_REGULAR, NULL);
+}

src/osmo-upf/up_peer_fsm.c

@@ -0,0 +1,196 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <osmocom/core/utils.h>
+#include <osmocom/core/fsm.h>
+
+#include <osmocom/upf/up_peer.h>
+
+enum up_peer_fsm_state {
+	UP_PEER_ST_NOT_ASSOCIATED,
+	UP_PEER_ST_ASSOCIATED,
+	UP_PEER_ST_GRACEFUL_RELEASE,
+};
+
+static const struct value_string up_peer_fsm_event_names[] = {
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_ASSOC_SETUP_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_ASSOC_UPD_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_RX_SESSION_EST_REQ),
+	OSMO_VALUE_STRING(UP_PEER_EV_HEARTBEAT_FAILURE),
+	{}
+};
+
+static struct osmo_fsm up_peer_fsm;
+
+static const struct osmo_tdef_state_timeout up_peer_fsm_timeouts[32] = {
+	[UP_PEER_ST_GRACEFUL_RELEASE] = { .T = -21 },
+};
+
+/* Transition to a state, using the T timer defined in up_peer_fsm_timeouts.
+ * Assumes local variable fi exists. */
+#define up_peer_fsm_state_chg(state) \
+	osmo_tdef_fsm_inst_state_chg(fi, state, \
+				     up_peer_fsm_timeouts, \
+				     g_upf_tdefs, \
+				     5)
+
+struct up_peer *up_peer_alloc(struct osmo_fsm_inst *parent_fi, uint32_t parent_event_term)
+{
+	struct up_peer *up_peer;
+
+	struct osmo_fsm_inst *fi = osmo_fsm_inst_alloc_child(&up_peer_fsm, parent_fi, parent_event_term);
+	OSMO_ASSERT(fi);
+
+	up_peer = talloc(fi, struct up_peer);
+	OSMO_ASSERT(up_peer);
+	fi->priv = up_peer;
+	*up_peer = (struct up_peer){
+		.fi = fi,
+	};
+
+	return up_peer;
+}
+
+static int up_peer_fsm_timer_cb(struct osmo_fsm_inst *fi)
+{
+	//struct up_peer *up_peer = fi->priv;
+	/* Return 1 to terminate FSM instance, 0 to keep running */
+	return 1;
+}
+
+static void up_peer_not_associated_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	//struct up_peer *up_peer = fi->priv;
+
+	switch (event) {
+
+	case UP_PEER_EV_RX_ASSOC_SETUP_REQ:
+		// FIXME
+		break;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void up_peer_associated_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+	//struct up_peer *up_peer = fi->priv;
+	// FIXME
+}
+
+static void up_peer_associated_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	//struct up_peer *up_peer = fi->priv;
+
+	switch (event) {
+
+	case UP_PEER_EV_RX_ASSOC_UPD_REQ:
+		// FIXME
+		break;
+
+	case UP_PEER_EV_RX_SESSION_EST_REQ:
+		// FIXME
+		break;
+
+	case UP_PEER_EV_HEARTBEAT_FAILURE:
+		// FIXME
+		break;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+static void up_peer_graceful_release_onenter(struct osmo_fsm_inst *fi, uint32_t prev_state)
+{
+	//struct up_peer *up_peer = fi->priv;
+	// FIXME
+}
+
+static void up_peer_graceful_release_action(struct osmo_fsm_inst *fi, uint32_t event, void *data)
+{
+	//struct up_peer *up_peer = fi->priv;
+
+	switch (event) {
+
+	case UP_PEER_EV_HEARTBEAT_FAILURE:
+		// FIXME
+		break;
+
+	default:
+		OSMO_ASSERT(false);
+	}
+}
+
+#define S(x)    (1 << (x))
+
+static const struct osmo_fsm_state up_peer_fsm_states[] = {
+	[UP_PEER_ST_NOT_ASSOCIATED] = {
+		.name = "not_associated",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_RX_ASSOC_SETUP_REQ)
+			,
+		.out_state_mask = 0
+			| S(UP_PEER_ST_ASSOCIATED)
+			,
+		.action = up_peer_not_associated_action,
+	},
+	[UP_PEER_ST_ASSOCIATED] = {
+		.name = "associated",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_RX_ASSOC_UPD_REQ)
+			| S(UP_PEER_EV_RX_SESSION_EST_REQ)
+			| S(UP_PEER_EV_HEARTBEAT_FAILURE)
+			,
+		.out_state_mask = 0
+			| S(UP_PEER_ST_GRACEFUL_RELEASE)
+			,
+		.onenter = up_peer_associated_onenter,
+		.action = up_peer_associated_action,
+	},
+	[UP_PEER_ST_GRACEFUL_RELEASE] = {
+		.name = "graceful_release",
+		.in_event_mask = 0
+			| S(UP_PEER_EV_HEARTBEAT_FAILURE)
+			,
+		.out_state_mask = 0
+			,
+		.onenter = up_peer_graceful_release_onenter,
+		.action = up_peer_graceful_release_action,
+	},
+};
+
+static struct osmo_fsm up_peer_fsm = {
+	.name = "up_peer",
+	.states = up_peer_fsm_states,
+	.num_states = ARRAY_SIZE(up_peer_fsm_states),
+	.log_subsys = DSESSION,
+	.event_names = up_peer_fsm_event_names,
+	.timer_cb = up_peer_fsm_timer_cb,
+};
+
+static __attribute__((constructor)) void up_peer_fsm_register(void)
+{
+	OSMO_ASSERT(osmo_fsm_register(&up_peer_fsm) == 0);
+}

src/osmo-upf/upf.c

@@ -23,13 +23,76 @@
 
 #include <osmocom/core/utils.h>
 #include <osmocom/core/talloc.h>
+#include <osmocom/core/sockaddr_str.h>
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
 
 #include <osmocom/upf/upf.h>
+#include <osmocom/upf/up_endpoint.h>
+#include <osmocom/upf/upf_gtp.h>
 
 struct g_upf *g_upf = NULL;
 
+struct osmo_tdef_group g_upf_tdef_groups[] = {
+	{ "pfcp", "PFCP endpoint timers", osmo_pfcp_tdefs, },
+	{}
+};
+
 void g_upf_alloc(void *ctx)
 {
 	OSMO_ASSERT(g_upf == NULL);
 	g_upf = talloc_zero(ctx, struct g_upf);
+
+	*g_upf = (struct g_upf){
+		.pfcp = {
+			.vty_cfg = {
+				.local_addr = talloc_strdup(g_upf, UPF_PFCP_LISTEN_DEFAULT),
+				.local_port = OSMO_PFCP_PORT,
+			},
+		},
+		.nft = {
+			.priority = -300,
+		},
+		.gtp = {
+			/* TODO: recovery count state file; use lower byte of current time, poor person's random. */
+			.recovery_count = time(NULL),
+		},
+	};
+
+	INIT_LLIST_HEAD(&g_upf->gtp.vty_cfg.devs);
+	INIT_LLIST_HEAD(&g_upf->gtp.devs);
+}
+
+int upf_pfcp_listen()
+{
+	struct osmo_sockaddr_str local_addr_str;
+	struct osmo_sockaddr local_addr;
+
+	OSMO_ASSERT(g_upf);
+	OSMO_ASSERT(g_upf->pfcp.ep == NULL);
+
+	/* Translate address string from VTY config to osmo_sockaddr: first read into osmo_sockaddr_str, then write to
+	 * osmo_sockaddr. */
+	osmo_sockaddr_str_from_str(&local_addr_str, g_upf->pfcp.vty_cfg.local_addr, g_upf->pfcp.vty_cfg.local_port);
+	osmo_sockaddr_str_to_sockaddr(&local_addr_str, &local_addr.u.sas);
+	LOGP(DLPFCP, LOGL_NOTICE, "PFCP: Listening on %s\n", osmo_sockaddr_to_str_c(OTC_SELECT, &local_addr));
+
+	g_upf->pfcp.ep = up_endpoint_init(g_upf, &local_addr);;
+	if (!g_upf->pfcp.ep) {
+		fprintf(stderr, "Failed to allocate PFCP endpoint.\n");
+		return -1;
+	}
+	return 0;
+}
+
+int upf_gtp_devs_open()
+{
+	struct gtp_vty_cfg *c = &g_upf->gtp.vty_cfg;
+	struct gtp_vty_cfg_dev *d;
+
+	llist_for_each_entry(d, &c->devs, entry) {
+		if (upf_gtp_dev_open(d->dev_name, d->create, d->local_addr, false, false))
+			return -1;
+	}
+	return 0;
 }

src/osmo-upf/upf_gtp.c

@@ -0,0 +1,461 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdbool.h>
+#include <errno.h>
+#include <string.h>
+#include <net/if.h>
+#include <linux/gtp.h>
+
+#include <libgtpnl/gtpnl.h>
+#include <libgtpnl/gtp.h>
+
+#include <osmocom/core/socket.h>
+#include <osmocom/core/logging.h>
+#include <osmocom/core/sockaddr_str.h>
+
+#include <osmocom/upf/upf.h>
+#include <osmocom/upf/upf_gtp.h>
+#include <osmocom/upf/upf_gtpu_echo.h>
+
+#define LOG_GTP_TUN(TUN, LEVEL, FMT, ARGS...) \
+	LOGP(DGTP, LEVEL, "%s: " FMT, upf_gtp_tun_to_str_c(OTC_SELECT, (TUN)), ##ARGS)
+
+int upf_gtp_dev_to_str_buf(char *buf, size_t buflen, const struct upf_gtp_dev *dev)
+{
+	uint16_t v0_port;
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+	OSMO_STRBUF_PRINTF(sb, "%s", dev->name ? : "null");
+	if (dev->name && dev->ifidx)
+		OSMO_STRBUF_PRINTF(sb, " [%u]", dev->ifidx);
+	if (dev->sgsn_mode)
+		OSMO_STRBUF_PRINTF(sb, " (SGSN)");
+	OSMO_STRBUF_PRINTF(sb, " ");
+	OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &dev->gtpv1.local_addr);
+	v0_port = osmo_sockaddr_port(&dev->gtpv0.local_addr.u.sa);
+	if (dev->gtpv0.enabled && v0_port)
+		OSMO_STRBUF_PRINTF(sb, "/%u", v0_port);
+	return sb.chars_needed;
+}
+
+char *upf_gtp_dev_to_str_c(void *ctx, const struct upf_gtp_dev *dev)
+{
+	OSMO_NAME_C_IMPL(ctx, 64, "ERROR", upf_gtp_dev_to_str_buf, dev)
+}
+
+struct upf_gtp_dev *upf_gtp_dev_find_by_name(const char *name)
+{
+	struct upf_gtp_dev *dev;
+	llist_for_each_entry(dev, &g_upf->gtp.devs, entry) {
+		if (!strcmp(name, dev->name))
+			return dev;
+	}
+	return NULL;
+}
+
+struct upf_gtp_dev *upf_gtp_dev_first()
+{
+	return llist_first_entry_or_null(&g_upf->gtp.devs, struct upf_gtp_dev, entry);
+}
+
+/* Tell the kernel to remove the GTP device. Called implicitly by talloc_free() (see upf_gtp_dev_destruct()). */
+static int upf_gtp_dev_delete(struct upf_gtp_dev *dev)
+{
+	int rc;
+	if (!dev->name)
+		return 0;
+	rc = gtp_dev_destroy(dev->name);
+	if (rc < 0) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "Error while deleting device: %s\n", strerror(errno));
+		return rc;
+	}
+	LOG_GTP_DEV(dev, LOGL_NOTICE, "Deleted GTP device\n");
+	dev->name = NULL;
+	return 0;
+}
+
+static int upf_gtp_dev_destruct(struct upf_gtp_dev *dev);
+
+static struct upf_gtp_dev *upf_gtp_dev_alloc(const char *name, const char *local_addr)
+{
+	struct upf_gtp_dev *dev = upf_gtp_dev_find_by_name(name);
+	struct osmo_sockaddr_str addr_conv;
+	local_addr = local_addr ? : "0.0.0.0";
+	if (dev) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "Device already exists. Cannot create %s %s\n", name, local_addr);
+		return NULL;
+	}
+	dev = talloc(g_upf, struct upf_gtp_dev);
+	*dev = (struct upf_gtp_dev){
+		.name = talloc_strdup(dev, name),
+		.gtpv0.ofd.fd = -1,
+		.gtpv1.ofd.fd = -1,
+	};
+	INIT_LLIST_HEAD(&dev->tunnels);
+
+	osmo_sockaddr_str_from_str(&addr_conv, local_addr, PORT_GTP0_U);
+
+	osmo_sockaddr_str_to_sockaddr(&addr_conv, &dev->gtpv0.local_addr.u.sas);
+
+	addr_conv.port = PORT_GTP1_U;
+	osmo_sockaddr_str_to_sockaddr(&addr_conv, &dev->gtpv1.local_addr.u.sas);
+
+	/* Need to add to list before setting up the destructor. A talloc_free() does automagically remove from the
+	 * list. */
+	llist_add(&dev->entry, &g_upf->gtp.devs);
+
+	talloc_set_destructor(dev, upf_gtp_dev_destruct);
+
+	return dev;
+}
+
+static int dev_resolve_ifidx(struct upf_gtp_dev *dev)
+{
+	int rc;
+
+	dev->ifidx = if_nametoindex(dev->name);
+	if (dev->ifidx == 0) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "No such device: '%s'\n", dev->name);
+		talloc_free(dev);
+		return -EIO;
+	}
+	/* Let's try something to see if talking to the device works. */
+	errno = 0;
+	rc = gtp_list_tunnel(g_upf->gtp.genl_id, g_upf->gtp.nl);
+	if (errno)
+		rc = -errno;
+	else if (rc)
+		rc = -EINVAL;
+	if (rc) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "Failed to open GTP device: %s\n", strerror(-rc));
+		talloc_free(dev);
+		return rc;
+	}
+
+	LOG_GTP_DEV(dev, LOGL_NOTICE, "GTP device ready (ifidx=%u)\n", dev->ifidx);
+	return 0;
+}
+
+int upf_gtp_dev_open(const char *name, bool create_gtp_dev, const char *local_addr, bool listen_for_gtpv0, bool sgsn_mode)
+{
+	struct osmo_sockaddr any = {
+		.u.sin = {
+			.sin_family = AF_INET,
+			.sin_port = 0,
+			.sin_addr = {
+				.s_addr = INADDR_ANY,
+			},
+		},
+	};
+	int rc;
+	struct upf_gtp_dev *dev = upf_gtp_dev_alloc(name, local_addr);
+	if (!dev)
+		return -EIO;
+
+	dev->sgsn_mode = sgsn_mode;
+
+	if (listen_for_gtpv0) {
+		rc = osmo_sock_init_osa_ofd(&dev->gtpv0.ofd, SOCK_DGRAM, 0, &dev->gtpv0.local_addr, &any,
+					    OSMO_SOCK_F_BIND);
+		if (rc < 0) {
+			LOG_GTP_DEV(dev, LOGL_ERROR, "Cannot bind GTPv0 on %s (rc=%d)\n",
+				    osmo_sockaddr_to_str_c(OTC_SELECT, &dev->gtpv0.local_addr), rc);
+			talloc_free(dev);
+			return -EIO;
+		}
+		dev->gtpv0.enabled = true;
+		LOG_GTP_DEV(dev, LOGL_DEBUG, "GTPv0 bound\n");
+	}
+
+	/* GTPv1 */
+	rc = osmo_sock_init_osa_ofd(&dev->gtpv1.ofd, SOCK_DGRAM, 0, &dev->gtpv1.local_addr, &any,
+				    OSMO_SOCK_F_BIND);
+	if (rc < 0) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "Cannot bind GTPv1 (rc=%d)\n", rc);
+		talloc_free(dev);
+		return -EIO;
+	}
+	LOG_GTP_DEV(dev, LOGL_DEBUG, "GTPv1 bound\n");
+
+	if (create_gtp_dev) {
+		int gtp0_fd = listen_for_gtpv0 ? dev->gtpv0.ofd.fd : -1;
+		int gtp1_fd = dev->gtpv1.ofd.fd;
+		if (dev->sgsn_mode)
+			rc = gtp_dev_create_sgsn(-1, dev->name, gtp0_fd, gtp1_fd);
+		else
+			rc = gtp_dev_create(-1, dev->name, gtp0_fd, gtp1_fd);
+		if (rc < 0) {
+			LOG_GTP_DEV(dev, LOGL_ERROR, "Cannot create GTP device: rc=%d\n", rc);
+			/* name = NULL: signal to the destructor that it does not need to delete the device */
+			dev->name = NULL;
+			talloc_free(dev);
+			return -EIO;
+		}
+
+		LOG_GTP_DEV(dev, LOGL_NOTICE, "created GTP device\n");
+		dev->created = true;
+	}
+
+	rc = dev_resolve_ifidx(dev);
+	if (rc) {
+		talloc_free(dev);
+		return -EIO;
+	}
+
+	upf_gtpu_echo_setup(dev);
+
+	return 0;
+}
+
+void upf_gtp_devs_close()
+{
+	struct upf_gtp_dev *dev;
+	while ((dev = llist_first_entry_or_null(&g_upf->gtp.devs, struct upf_gtp_dev, entry)))
+		talloc_free(dev);
+}
+
+void upf_gtp_genl_close()
+{
+	if (!g_upf->gtp.nl)
+		return;
+	genl_socket_close(g_upf->gtp.nl);
+	g_upf->gtp.nl = NULL;
+	g_upf->gtp.genl_id = -1;
+
+	LOGP(DGTP, LOGL_NOTICE, "Closed mnl_socket\n");
+}
+
+/* Open an MNL socket which allows to create and remove GTP devices (requires CAP_NET_ADMIN). */
+int upf_gtp_genl_open()
+{
+	if (g_upf->gtp.nl && g_upf->gtp.genl_id >= 0)
+		return 0;
+
+	if (g_upf->gtp.nl)
+		upf_gtp_genl_close();
+
+	g_upf->gtp.nl = genl_socket_open();
+	if (!g_upf->gtp.nl) {
+		LOGP(DGTP, LOGL_ERROR, "Cannot open mnl_socket: %s\n", strerror(errno));
+		return -EIO;
+	}
+
+	g_upf->gtp.genl_id = genl_lookup_family(g_upf->gtp.nl, "gtp");
+	if (g_upf->gtp.genl_id < 0) {
+		LOGP(DGTP, LOGL_ERROR, "genl family 'gtp' not found\n");
+		return -ENOTSUP;
+	}
+
+	LOGP(DGTP, LOGL_NOTICE, "Opened mnl_socket\n");
+	return 0;
+}
+
+struct upf_gtp_tun {
+	struct llist_head entry;
+
+	struct upf_gtp_dev *dev;
+	struct upf_gtp_tun_desc desc;
+	bool active;
+};
+
+static int upf_gtp_tun_to_str_buf(char *buf, size_t buflen, const struct upf_gtp_tun *tun)
+{
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+	OSMO_STRBUF_PRINTF(sb, "%s:tun{TEID=l:0x%x,r:0x%x UE=", tun->dev->name, tun->desc.local_teid,
+			   tun->desc.remote_teid);
+	OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &tun->desc.ue_addr);
+	OSMO_STRBUF_PRINTF(sb, " GTP-dst=");
+	OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, &tun->desc.gtp_remote_addr);
+	OSMO_STRBUF_PRINTF(sb, "}");
+	return sb.chars_needed;
+}
+
+static char *upf_gtp_tun_to_str_c(void *ctx, const struct upf_gtp_tun *tun)
+{
+	OSMO_NAME_C_IMPL(ctx, 64, "ERROR", upf_gtp_tun_to_str_buf, tun)
+}
+
+static int upf_gtp_tun_deactivate(struct upf_gtp_tun *tun);
+
+static int upf_gtp_tun_destruct(struct upf_gtp_tun *tun)
+{
+	if (tun->active)
+		upf_gtp_tun_deactivate(tun);
+	llist_del(&tun->entry);
+	return 0;
+}
+
+static struct upf_gtp_tun *upf_gtp_tun_alloc(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *desc)
+{
+	struct upf_gtp_tun *tun = talloc(dev, struct upf_gtp_tun);
+	OSMO_ASSERT(tun);
+	*tun = (struct upf_gtp_tun){
+		.dev = dev,
+		.desc = *desc,
+	};
+	llist_add(&tun->entry, &dev->tunnels);
+	talloc_set_destructor(tun, upf_gtp_tun_destruct);
+	return tun;
+}
+
+static struct gtp_tunnel *upf_gtp_tun_to_gtp_tunnel(struct upf_gtp_tun *tun)
+{
+	struct gtp_tunnel *t;
+
+	if (tun->desc.ue_addr.u.sas.ss_family != AF_INET || tun->desc.gtp_remote_addr.u.sas.ss_family != AF_INET) {
+		LOG_GTP_TUN(tun, LOGL_ERROR, "Only capabale of IPv4\n");
+		return NULL;
+	}
+
+	t = gtp_tunnel_alloc();
+	OSMO_ASSERT(t);
+	gtp_tunnel_set_ifidx(t, tun->dev->ifidx);
+	gtp_tunnel_set_version(t, GTP_V1);
+	gtp_tunnel_set_i_tei(t, tun->desc.local_teid);
+	gtp_tunnel_set_o_tei(t, tun->desc.remote_teid);
+	gtp_tunnel_set_ms_ip4(t, &tun->desc.ue_addr.u.sin.sin_addr);
+	gtp_tunnel_set_sgsn_ip4(t, &tun->desc.gtp_remote_addr.u.sin.sin_addr);
+	return t;
+}
+
+int upf_gtp_tun_activate(struct upf_gtp_tun *tun)
+{
+	int rc;
+	struct gtp_tunnel *t;
+
+	if (tun->active)
+		return -EALREADY;
+
+	t = upf_gtp_tun_to_gtp_tunnel(tun);
+	if (!t)
+		return -ENOTSUP;
+
+	errno = 0;
+	rc = gtp_add_tunnel(g_upf->gtp.genl_id, g_upf->gtp.nl, t);
+	if (errno) {
+		rc = -errno;
+	} else if (rc) {
+		rc = -EINVAL;
+	} else {
+		tun->active = true;
+	}
+
+	gtp_tunnel_free(t);
+	return rc;
+}
+
+static struct upf_gtp_tun *upf_gtp_dev_tunnel_find(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *tun_desc)
+{
+	struct upf_gtp_tun *tun;
+	llist_for_each_entry(tun, &dev->tunnels, entry) {
+		if (upf_gtp_tun_desc_cmp(tun_desc, &tun->desc))
+			continue;
+		return tun;
+	}
+	return NULL;
+}
+
+int upf_gtp_dev_tunnel_add(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *tun_desc)
+{
+	struct upf_gtp_tun *tun;
+	tun = upf_gtp_dev_tunnel_find(dev, tun_desc);
+	if (!tun)
+		tun = upf_gtp_tun_alloc(dev, tun_desc);
+	if (tun->active)
+		return 0;
+	return upf_gtp_tun_activate(tun);
+}
+
+int upf_gtp_dev_tunnel_del(struct upf_gtp_dev *dev, const struct upf_gtp_tun_desc *tun_desc)
+{
+	struct upf_gtp_tun *tun;
+	int rc;
+	tun = upf_gtp_dev_tunnel_find(dev, tun_desc);
+	if (!tun)
+		return 0;
+	if (tun->active) {
+		rc = upf_gtp_tun_deactivate(tun);
+		if (rc)
+			return rc;
+	}
+	talloc_free(tun);
+	return 0;
+}
+
+static int upf_gtp_tun_deactivate(struct upf_gtp_tun *tun)
+{
+	int rc;
+	struct gtp_tunnel *t;
+
+	if (!tun->active) {
+		LOG_GTP_TUN(tun, LOGL_ERROR, "Cannot deactivate, not active\n");
+		return -EINVAL;
+	}
+
+	t = upf_gtp_tun_to_gtp_tunnel(tun);
+	if (!t)
+		return -EINVAL;
+
+	rc = gtp_del_tunnel(g_upf->gtp.genl_id, g_upf->gtp.nl, t);
+	if (rc)
+		LOG_GTP_TUN(tun, LOGL_ERROR, "Failed to delete tunnel: %d %s\n", rc, strerror(rc));
+	else
+		tun->active = false;
+
+	gtp_tunnel_free(t);
+	return rc;
+}
+
+static int upf_gtp_dev_destruct(struct upf_gtp_dev *dev)
+{
+	struct upf_gtp_tun *t;
+	/* Destruct and clean up all active tunnels before deleting the device */
+	while ((t = llist_first_entry_or_null(&dev->tunnels, struct upf_gtp_tun, entry)))
+		talloc_free(t);
+	llist_del(&dev->entry);
+	osmo_fd_close(&dev->gtpv0.ofd);
+	osmo_fd_close(&dev->gtpv1.ofd);
+	if (dev->created)
+		upf_gtp_dev_delete(dev);
+	return 0;
+}
+
+int upf_gtp_tun_desc_cmp(const struct upf_gtp_tun_desc *a, const struct upf_gtp_tun_desc *b)
+{
+	if (a == b)
+		return 0;
+	if (!a)
+		return -1;
+	if (!b)
+		return 1;
+
+#define CMP_RET(MEMB) do { \
+		int _cmp = OSMO_CMP(a->MEMB, b->MEMB); \
+		if (_cmp) \
+			return _cmp; \
+	} while (0)
+
+	CMP_RET(local_teid);
+	CMP_RET(remote_teid);
+	return osmo_sockaddr_cmp(&a->gtp_remote_addr, &b->gtp_remote_addr);
+}

src/osmo-upf/upf_gtpu_echo.c

@@ -0,0 +1,154 @@
+/* GTP-U ECHO implementation for osmo-upf */
+
+#include <stdint.h>
+#include <errno.h>
+
+#include <osmocom/core/endian.h>
+#include <osmocom/core/socket.h>
+#include <osmocom/core/msgb.h>
+
+#include <osmocom/upf/upf.h>
+#include <osmocom/upf/upf_gtp.h>
+
+#define GTP1U_PORT	2152
+
+enum gtp1u_msgt {
+	GTP1U_MSGTYPE_ECHO_REQ = 1,
+	GTP1U_MSGTYPE_ECHO_RSP = 2,
+};
+
+enum gtp1u_iei {
+	GTP1U_IEI_RECOVERY = 14,
+};
+
+/* 3GPP TS 29.281 */
+struct gtp1u_hdr {
+#if OSMO_IS_LITTLE_ENDIAN
+	uint8_t pn:1, /*< N-PDU Number flag */
+		s:1, /*< Sequence number flag */
+		e:1, /*< Extension header flag */
+		spare:1,
+		pt:1, /*< Protocol Type: GTP=1, GTP'=0 */
+		version:3; /*< Version: 1 */
+#elif OSMO_IS_BIG_ENDIAN
+	uint8_t version:3, pt:1, spare:1, e:1, s:1, pn:1;
+#endif
+	uint8_t msg_type;
+	uint16_t length;
+	uint32_t tei; /*< 05 - 08 Tunnel Endpoint ID */
+	union {
+		uint8_t data1[0];
+		struct {
+			uint16_t seq_nr;
+			uint8_t n_pdu_nr;
+			uint8_t next_ext_type;
+		} ext;
+	};
+	uint8_t data2[0];
+} __attribute__((packed));
+
+static int rx_echo_req(struct upf_gtp_dev *dev, const struct osmo_sockaddr *remote, const struct gtp1u_hdr *rx_h)
+{
+	struct msgb *msg;
+	struct gtp1u_hdr *tx_h;
+	int rc;
+
+	if (!rx_h->s) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "rx GTPv1-U ECHO REQ without sequence nr\n");
+		return -1;
+	}
+
+	msg = msgb_alloc_headroom(1024, 128, "GTP-echo-resp");
+	tx_h = (void*)msgb_put(msg, sizeof(*tx_h));
+
+	*tx_h = (struct gtp1u_hdr){
+		/* 3GPP TS 29.281 5.1 defines that the ECHO REQ & RESP shall contain a sequence nr */
+		.s = 1,
+		.pt = 1,
+		.version = 1,
+		.msg_type = GTP1U_MSGTYPE_ECHO_RSP,
+		.ext = {
+			.seq_nr = rx_h->ext.seq_nr,
+		},
+	};
+
+	OSMO_ASSERT(msg->tail == tx_h->data2);
+
+	/* ECHO RESPONSE shall contain a recovery counter */
+	msgb_put_u8(msg, GTP1U_IEI_RECOVERY);
+	msgb_put_u8(msg, g_upf->gtp.recovery_count);
+
+	osmo_store16be(msg->tail - tx_h->data1, &tx_h->length);
+
+	rc = sendto(dev->gtpv1.ofd.fd, msgb_data(msg), msgb_length(msg), 0, &remote->u.sa, sizeof(*remote));
+	if (rc < 0) {
+		int err = errno;
+		LOG_GTP_DEV(dev, LOGL_ERROR, "GTP1-U sendto(len=%d, to=%s): %s\n", msgb_length(msg),
+			    osmo_sockaddr_to_str(remote), strerror(err));
+	}
+	return rc;
+}
+
+int upf_gtpu_echo_read_cb(struct osmo_fd *ofd, unsigned int what)
+{
+	struct upf_gtp_dev *dev = ofd->data;
+
+	ssize_t sz;
+	uint8_t buf[4096];
+	struct osmo_sockaddr remote;
+	socklen_t remote_len = sizeof(remote);
+	const struct gtp1u_hdr *h;
+	uint16_t h_length;
+
+	if ((sz = recvfrom(dev->gtpv1.ofd.fd, buf, sizeof(buf), 0, &remote.u.sa, &remote_len)) < 0) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "recvfrom() failed: %s\n", strerror(errno));
+		return -1;
+	}
+	if (sz == 0) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "recvfrom() yields zero bytes\n");
+		return -1;
+	}
+
+	/* A GTPv1-U header of size 8 is valid, but this code expects to handle only ECHO REQUEST messages. These are
+	 * required to have a sequence number, hence this check here consciously uses the full sizeof(*h) == 12. */
+	if (sz < sizeof(*h)) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "rx GTP packet smaller than the GTPv1-U header + sequence nr: %zd < %zu\n",
+			    sz, sizeof(*h));
+		return -1;
+	}
+
+	h = (const struct gtp1u_hdr *)buf;
+	if (h->version != 1) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "rx GTP v%u: only GTP version 1 supported\n", h->version);
+		return -1;
+	}
+
+	h_length = osmo_load16be(&h->length);
+	if (offsetof(struct gtp1u_hdr, data1) + h_length > sz) {
+		LOG_GTP_DEV(dev, LOGL_ERROR, "rx GTP: header + h.length = %zu > received bytes = %zd\n",
+			    offsetof(struct gtp1u_hdr, data1) + h_length, sz);
+		return -1;
+	}
+
+	switch (h->msg_type) {
+	case GTP1U_MSGTYPE_ECHO_REQ:
+		return rx_echo_req(dev, &remote, h);
+	default:
+		LOG_GTP_DEV(dev, LOGL_ERROR, "rx: GTPv1-U message type %u not supported\n", h->msg_type);
+		return -1;
+	}
+
+	return 0;
+}
+
+int upf_gtpu_echo_setup(struct upf_gtp_dev *dev)
+{
+	if (dev->gtpv1.ofd.fd == -1) {
+		LOGP(DGTP, LOGL_ERROR, "Cannot setup GTP-U ECHO: GTP-v1 socket not initialized\n");
+		return -EINVAL;
+	}
+
+	dev->gtpv1.ofd.cb = upf_gtpu_echo_read_cb;
+	dev->gtpv1.ofd.data = dev;
+	return osmo_fd_register(&dev->gtpv1.ofd);
+}

src/osmo-upf/upf_nft.c

@@ -0,0 +1,207 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <errno.h>
+#include <nftables/libnftables.h>
+
+#include <osmocom/core/talloc.h>
+#include <osmocom/core/logging.h>
+
+#include <osmocom/upf/upf.h>
+#include <osmocom/upf/upf_nft.h>
+
+static char *upf_nft_ruleset_table_create(void *ctx, const char *table_name)
+{
+	return talloc_asprintf(ctx, "add table inet %s\n", table_name);
+}
+
+static int upf_nft_run(const char *ruleset)
+{
+	int rc;
+	if (!g_upf->nft.nft_ctx) {
+		rc = upf_nft_init();
+		if (rc)
+			return rc;
+	}
+
+	rc = nft_run_cmd_from_buffer(g_upf->nft.nft_ctx, ruleset);
+	if (rc < 0) {
+		LOGP(DNFT, LOGL_ERROR, "error running nft ruleset: rc=%d ruleset=%s\n",
+		     rc, osmo_quote_str_c(OTC_SELECT, ruleset, -1));
+		return -EIO;
+	}
+	return 0;
+}
+
+int upf_nft_init()
+{
+	int rc;
+	g_upf->nft.nft_ctx = nft_ctx_new(NFT_CTX_DEFAULT);
+	if (!g_upf->nft.nft_ctx) {
+		LOGP(DNFT, LOGL_ERROR, "cannot allocate libnftables nft_ctx\n");
+		return -EIO;
+	}
+
+	if (!g_upf->nft.table_name)
+		g_upf->nft.table_name = talloc_strdup(g_upf, "osmo-upf");
+
+	rc = upf_nft_run(upf_nft_ruleset_table_create(OTC_SELECT, g_upf->nft.table_name));
+	if (rc) {
+		LOGP(DNFT, LOGL_ERROR, "Failed to create nft table %s\n",
+		     osmo_quote_str_c(OTC_SELECT, g_upf->nft.table_name, -1));
+		return rc;
+	}
+	LOGP(DNFT, LOGL_NOTICE, "Created nft table %s\n", osmo_quote_str_c(OTC_SELECT, g_upf->nft.table_name, -1));
+	return 0;
+}
+
+int upf_nft_free()
+{
+	if (!g_upf->nft.nft_ctx)
+		return 0;
+	nft_ctx_free(g_upf->nft.nft_ctx);
+	g_upf->nft.nft_ctx = NULL;
+	return 0;
+}
+
+struct upf_nft_args_peer {
+	/* The source IP address in packets received from this peer */
+	const struct osmo_sockaddr *addr;
+	/* The TEID that we send to the peer in GTP packets. */
+	uint32_t teid_remote;
+	/* The TEID that the peer sends to us in GTP packets. */
+	uint32_t teid_local;
+};
+
+struct upf_nft_args {
+	/* global table name */
+	const char *table_name;
+	/* chain name for this specific tunnel mapping */
+	uint32_t chain_id;
+	int priority;
+
+	struct upf_nft_args_peer peer_a;
+	struct upf_nft_args_peer peer_b;
+};
+
+static int tunmap_single_direction(char *buf, size_t buflen,
+				   const struct upf_nft_args *args,
+				   const struct upf_nft_args_peer *from_peer,
+				   const struct upf_nft_args_peer *to_peer)
+{
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+	OSMO_STRBUF_PRINTF(sb, "add rule inet %s " NFT_CHAIN_NAME_PREFIX_TUNMAP "%u", args->table_name, args->chain_id);
+
+	/* Match only UDP packets */
+	OSMO_STRBUF_PRINTF(sb, " meta l4proto udp");
+
+	/* Match on packets coming in from from_peer */
+	OSMO_STRBUF_PRINTF(sb, " ip saddr ");
+	OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, from_peer->addr);
+
+	/* Match on the TEID in the header */
+	OSMO_STRBUF_PRINTF(sb, " @ih,32,32 0x%08x", from_peer->teid_local);
+
+	/* Change destination address to to_peer */
+	OSMO_STRBUF_PRINTF(sb, " ip daddr set ");
+	OSMO_STRBUF_APPEND(sb, osmo_sockaddr_to_str_buf2, to_peer->addr);
+
+	/* Change the TEID in the header to the one to_peer expects */
+	OSMO_STRBUF_PRINTF(sb, " @ih,32,32 set 0x%08x", to_peer->teid_remote);
+
+	OSMO_STRBUF_PRINTF(sb, " counter\n");
+
+	return sb.chars_needed;
+}
+
+static int upf_nft_ruleset_tunmap_create_buf(char *buf, size_t buflen, const struct upf_nft_args *args)
+{
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+
+	/* Add a chain for this tunnel mapping */
+	OSMO_STRBUF_PRINTF(sb, "add chain inet %s " NFT_CHAIN_NAME_PREFIX_TUNMAP "%u { type filter hook prerouting priority %d; }\n",
+			   args->table_name, args->chain_id, args->priority);
+
+	/* Forwarding from peer_a to peer_b */
+	OSMO_STRBUF_APPEND(sb, tunmap_single_direction, args, &args->peer_a, &args->peer_b);
+	/* And from peer_b to peer_a */
+	OSMO_STRBUF_APPEND(sb, tunmap_single_direction, args, &args->peer_b, &args->peer_a);
+
+	return sb.chars_needed;
+}
+
+static char *upf_nft_ruleset_tunmap_create_c(void *ctx, const struct upf_nft_args *args)
+{
+	OSMO_NAME_C_IMPL(ctx, 512, "ERROR", upf_nft_ruleset_tunmap_create_buf, args)
+}
+
+static int upf_nft_ruleset_tunmap_delete_buf(char *buf, size_t buflen, const struct upf_nft_args *args)
+{
+	struct osmo_strbuf sb = { .buf = buf, .len = buflen };
+	OSMO_STRBUF_PRINTF(sb, "delete chain inet %s " NFT_CHAIN_NAME_PREFIX_TUNMAP "%u\n",
+			   args->table_name, args->chain_id);
+	return sb.chars_needed;
+}
+
+static char *upf_nft_ruleset_tunmap_delete_c(void *ctx, const struct upf_nft_args *args)
+{
+	OSMO_NAME_C_IMPL(ctx, 64, "ERROR", upf_nft_ruleset_tunmap_delete_buf, args)
+}
+
+static void upf_nft_args_from_tunmap_desc(struct upf_nft_args *args, const struct upf_nft_tunmap_desc *tunmap)
+{
+	*args = (struct upf_nft_args){
+		.table_name = g_upf->nft.table_name,
+		.chain_id = tunmap->id,
+		.priority = g_upf->nft.priority,
+		.peer_a = {
+			.addr = &tunmap->access.gtp_remote_addr,
+			.teid_remote = tunmap->access.remote_teid,
+			.teid_local = tunmap->access.local_teid,
+		},
+		.peer_b = {
+			.addr = &tunmap->core.gtp_remote_addr,
+			.teid_remote = tunmap->core.remote_teid,
+			.teid_local = tunmap->core.local_teid,
+		},
+	};
+}
+
+int upf_nft_tunmap_create(struct upf_nft_tunmap_desc *tunmap)
+{
+	struct upf_nft_args args;
+
+	/* Give this tunnel mapping a new id, returned to the caller so that the tunnel mapping can be deleted later */
+	g_upf->nft.next_id_state++;
+	tunmap->id = g_upf->nft.next_id_state;
+
+	upf_nft_args_from_tunmap_desc(&args, tunmap);
+	return upf_nft_run(upf_nft_ruleset_tunmap_create_c(OTC_SELECT, &args));
+}
+
+int upf_nft_tunmap_delete(struct upf_nft_tunmap_desc *tunmap)
+{
+	struct upf_nft_args args;
+	upf_nft_args_from_tunmap_desc(&args, tunmap);
+	return upf_nft_run(upf_nft_ruleset_tunmap_delete_c(OTC_SELECT, &args));
+}

src/osmo-upf/upf_vty.c

@@ -0,0 +1,326 @@
+/* OsmoUpf interface to quagga VTY */
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <osmocom/core/sockaddr_str.h>
+#include <osmocom/core/socket.h>
+
+#include <osmocom/pfcp/pfcp_endpoint.h>
+#include <osmocom/pfcp/pfcp_msg.h>
+
+#include <osmocom/vty/vty.h>
+#include <osmocom/vty/command.h>
+
+#include <osmocom/upf/upf.h>
+#include <osmocom/upf/upf_gtp.h>
+#include <osmocom/upf/up_endpoint.h>
+#include <osmocom/upf/up_peer.h>
+#include <osmocom/upf/up_session.h>
+#include <osmocom/upf/up_gtp_action.h>
+
+enum upf_vty_node {
+	PFCP_NODE = _LAST_OSMOVTY_NODE + 1,
+	GTP_NODE,
+	NFT_NODE,
+};
+
+static struct cmd_node cfg_pfcp_node = {
+	PFCP_NODE,
+	"%s(config-pfcp)# ",
+	1,
+};
+
+#define pfcp_vty (g_upf->pfcp.vty_cfg)
+#define gtp_vty (g_upf->gtp.vty_cfg)
+
+DEFUN(cfg_pfcp, cfg_pfcp_cmd,
+      "pfcp",
+      "Enter the PFCP configuration node\n")
+{
+	vty->node = PFCP_NODE;
+	return CMD_SUCCESS;
+}
+
+static int config_write_pfcp(struct vty *vty)
+{
+	vty_out(vty, "pfcp%s", VTY_NEWLINE);
+	if (strcmp(UPF_PFCP_LISTEN_DEFAULT, pfcp_vty.local_addr))
+		vty_out(vty, " local-addr %s%s", pfcp_vty.local_addr, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_pfcp_local_addr, cfg_pfcp_local_addr_cmd,
+      "local-addr IP_ADDR",
+      "Set the local IP address to bind on for PFCP\n"
+      "IP address\n")
+{
+	osmo_talloc_replace_string(g_upf, &pfcp_vty.local_addr, argv[0]);
+	return CMD_SUCCESS;
+}
+
+static struct cmd_node cfg_gtp_node = {
+	GTP_NODE,
+	"%s(config-gtp)# ",
+	1,
+};
+
+DEFUN(cfg_gtp, cfg_gtp_cmd,
+      "gtp",
+      "Enter the 'gtp' node to configure GTP kernel module usage\n")
+{
+	vty->node = GTP_NODE;
+	return CMD_SUCCESS;
+}
+
+static int config_write_gtp(struct vty *vty)
+{
+	struct gtp_vty_cfg_dev *d;
+	vty_out(vty, "gtp%s", VTY_NEWLINE);
+
+	llist_for_each_entry(d, &gtp_vty.devs, entry) {
+		if (d->create) {
+			vty_out(vty, " dev create %s", d->dev_name);
+			if (d->local_addr)
+				vty_out(vty, " %s", d->local_addr);
+			vty_out(vty, "%s", VTY_NEWLINE);
+		} else {
+			vty_out(vty, " dev use %s%s", d->dev_name, VTY_NEWLINE);
+		}
+	}
+	return CMD_SUCCESS;
+}
+
+#define DEV_STR "Configure the GTP device to use for encaps/decaps.\n"
+
+DEFUN(cfg_gtp_dev_create, cfg_gtp_dev_create_cmd,
+      "dev create DEVNAME [LISTEN_ADDR]",
+      DEV_STR
+      "create a new GTP device. Will listen on GTPv1 port " OSMO_STRINGIFY_VAL(PORT_GTP1_U)
+      " and GTPv0 port " OSMO_STRINGIFY_VAL(PORT_GTP0_U) " on the specified interface, or on ANY if LISTEN_ADDR is"
+      " omitted.\n"
+      "device name, e.g. 'apn0'\n"
+      "IPv4 or IPv6 address to listen on, omit for ANY\n")
+{
+	struct gtp_vty_cfg_dev *d = talloc_zero(g_upf, struct gtp_vty_cfg_dev);
+	d->create = true;
+	d->dev_name = talloc_strdup(d, argv[0]);
+	if (argc > 1)
+		d->local_addr = talloc_strdup(d, argv[1]);
+	llist_add(&d->entry, &gtp_vty.devs);
+	vty_out(vty, "Added GTP device %s (create new)%s", d->dev_name, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_gtp_dev_use, cfg_gtp_dev_use_cmd,
+      "dev use DEVNAME",
+      DEV_STR
+      "use an existing GTP device, e.g. created by 'gtp-link'\n"
+      "device name, e.g. 'apn0'\n")
+{
+	struct gtp_vty_cfg_dev *d = talloc_zero(g_upf, struct gtp_vty_cfg_dev);
+	d->create = false;
+	d->dev_name = talloc_strdup(d, argv[0]);
+	llist_add(&d->entry, &gtp_vty.devs);
+	vty_out(vty, "Added GTP device %s (use existing)%s", d->dev_name, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_gtp_dev_del, cfg_gtp_dev_del_cmd,
+      "dev delete DEVNAME",
+      DEV_STR
+      "Remove a GTP device from the configuration, and delete the device if it was created here.\n"
+      "device name, e.g. 'apn0'\n")
+{
+	const char *dev_name = argv[0];
+	struct gtp_vty_cfg_dev *d;
+	struct upf_gtp_dev *dev;
+
+	/* remove from VTY cfg */
+	llist_for_each_entry(d, &gtp_vty.devs, entry) {
+		if (strcmp(d->dev_name, dev_name))
+			continue;
+		llist_del(&d->entry);
+		break;
+	}
+
+	/* close device (and possibly delete from system, via talloc destructor) */
+	dev = upf_gtp_dev_find_by_name(dev_name);
+	if (dev)
+		talloc_free(dev);
+	return CMD_SUCCESS;
+}
+
+static struct cmd_node cfg_nft_node = {
+	NFT_NODE,
+	"%s(config-nft)# ",
+	1,
+};
+
+DEFUN(cfg_nft, cfg_nft_cmd,
+      "nft",
+      "Enter the 'nft' node to configure nftables usage\n")
+{
+	vty->node = NFT_NODE;
+	return CMD_SUCCESS;
+}
+
+static int config_write_nft(struct vty *vty)
+{
+	vty_out(vty, "nft%s", VTY_NEWLINE);
+
+	if (g_upf->nft.table_name && strcmp(g_upf->nft.table_name, "osmo-upf"))
+		vty_out(vty, " table-name %s%s", g_upf->nft.table_name, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+DEFUN(cfg_nft_table_name, cfg_nft_table_name_cmd,
+      "table-name TABLE_NAME",
+      "Set the nft inet table name to create and place GTP tunnel forwarding chains in"
+      " (as in 'nft add table inet foo'). If multiple instances of osmo-upf are running on the same system, each"
+      " osmo-upf must have its own table name. Otherwise the names of created forwarding chains will collide.\n"
+      "nft inet table name\n")
+{
+	osmo_talloc_replace_string(g_upf, &g_upf->nft.table_name, argv[0]);
+	return CMD_SUCCESS;
+}
+
+DEFUN(show_pdr, show_pdr_cmd,
+      "show pdr",
+      SHOW_STR
+      "List all sessions' PDR and FAR status\n")
+{
+	struct up_peer *peer;
+	int active_count = 0;
+	int inactive_count = 0;
+	llist_for_each_entry(peer, &g_upf->pfcp.ep->peers, entry) {
+		struct up_session *session;
+		int bkt;
+		hash_for_each(peer->sessions_by_up_seid, bkt, session, node_by_up_seid) {
+			struct pdr *pdr;
+			llist_for_each_entry(pdr, &session->pdrs, entry) {
+				if (!pdr->active) {
+					vty_out(vty, "%s: inactive: %s%s%s%s",
+						session->fi->id, pdr_to_str_c(OTC_SELECT, pdr),
+						pdr->inactive_reason ? ": " : "",
+						pdr->inactive_reason ? : "",
+						VTY_NEWLINE);
+					inactive_count++;
+				} else {
+					vty_out(vty, "%s: active: %s%s",
+						session->fi->id, pdr_to_str_c(OTC_SELECT, pdr),
+						VTY_NEWLINE);
+					active_count++;
+				}
+			}
+		}
+	}
+	vty_out(vty, "(%d of %d active)%s", active_count, active_count + inactive_count, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+DEFUN(show_gtp, show_gtp_cmd,
+      "show gtp",
+      SHOW_STR
+      "Active GTP tunnels and forwardings\n")
+{
+	struct up_peer *peer;
+	int count = 0;
+
+	if (!upf_gtp_dev_first()) {
+		vty_out(vty, "No GTP device open%s", VTY_NEWLINE);
+		return CMD_SUCCESS;
+	}
+
+	llist_for_each_entry(peer, &g_upf->pfcp.ep->peers, entry) {
+		struct up_session *session;
+		int bkt;
+		hash_for_each(peer->sessions_by_up_seid, bkt, session, node_by_up_seid) {
+			struct up_gtp_action *a;
+			llist_for_each_entry(a, &session->active_gtp_actions, entry) {
+				vty_out(vty, "%s%s", up_gtp_action_to_str_c(OTC_SELECT, a), VTY_NEWLINE);
+				count++;
+			}
+		}
+	}
+	vty_out(vty, "(%d active)%s", count, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+DEFUN(show_session, show_session_cmd,
+      "show session",
+      SHOW_STR
+      "PFCP Session status\n")
+{
+	struct up_peer *peer;
+	int inactive_count = 0;
+	int active_count = 0;
+	int fully_active_count = 0;
+
+	llist_for_each_entry(peer, &g_upf->pfcp.ep->peers, entry) {
+		struct up_session *session;
+		int bkt;
+		hash_for_each(peer->sessions_by_up_seid, bkt, session, node_by_up_seid) {
+			vty_out(vty, "%s %s%s",
+				up_session_to_str_c(OTC_SELECT, session),
+				up_session_gtp_status(session), VTY_NEWLINE);
+			if (up_session_is_active(session)) {
+				if (up_session_is_fully_active(session, NULL, NULL))
+					fully_active_count++;
+				else
+					active_count++;
+			} else {
+				inactive_count++;
+			}
+		}
+	}
+	vty_out(vty, "(%d fully-active + %d partially active + %d inactive)%s",
+		fully_active_count, active_count, inactive_count, VTY_NEWLINE);
+	return CMD_SUCCESS;
+}
+
+void upf_vty_init()
+{
+	OSMO_ASSERT(g_upf != NULL);
+
+	install_element_ve(&show_pdr_cmd);
+	install_element_ve(&show_gtp_cmd);
+	install_element_ve(&show_session_cmd);
+
+	install_node(&cfg_pfcp_node, config_write_pfcp);
+	install_element(CONFIG_NODE, &cfg_pfcp_cmd);
+
+	install_element(PFCP_NODE, &cfg_pfcp_local_addr_cmd);
+
+	install_node(&cfg_gtp_node, config_write_gtp);
+	install_element(CONFIG_NODE, &cfg_gtp_cmd);
+
+	install_element(GTP_NODE, &cfg_gtp_dev_create_cmd);
+	install_element(GTP_NODE, &cfg_gtp_dev_use_cmd);
+	install_element(GTP_NODE, &cfg_gtp_dev_del_cmd);
+
+	install_node(&cfg_nft_node, config_write_nft);
+	install_element(CONFIG_NODE, &cfg_nft_cmd);
+
+	install_element(NFT_NODE, &cfg_nft_table_name_cmd);
+}
+

tests/Makefile.am

@@ -1,5 +1,6 @@
 SUBDIRS = \
 	libosmo-gtlv \
+	libosmo-pfcp \
 	$(NULL)
 
 # The `:;' works around a Bash 3.2 bug when the output is not writeable.

tests/libosmo-gtlv/Makefile.am

@@ -28,7 +28,7 @@ gtlv_test_SOURCES = \
 	$(NULL)
 
 gtlv_test_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(NULL)
 
@@ -37,7 +37,7 @@ gtlv_dec_enc_test_SOURCES = \
 	$(NULL)
 
 gtlv_dec_enc_test_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(NULL)
 

tests/libosmo-gtlv/test_gtlv_gen/Makefile.am

@@ -35,7 +35,7 @@ gen__myproto_ies_auto_SOURCES = \
 	$(NULL)
 
 gen__myproto_ies_auto_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(NULL)
 
@@ -51,7 +51,7 @@ gtlv_gen_test_SOURCES = \
 	$(NULL)
 
 gtlv_gen_test_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(NULL)
 

tests/libosmo-gtlv/test_tliv/Makefile.am

@@ -35,7 +35,7 @@ gen__myproto_ies_auto_SOURCES = \
 	$(NULL)
 
 gen__myproto_ies_auto_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(NULL)
 
@@ -51,7 +51,7 @@ tliv_test_SOURCES = \
 	$(NULL)
 
 tliv_test_LDADD = \
-	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.a \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
 	$(LIBOSMOCORE_LIBS) \
 	$(NULL)
 

tests/libosmo-pfcp/Makefile.am

@@ -0,0 +1,32 @@
+AM_CPPFLAGS = \
+	$(all_includes) \
+	-I$(top_srcdir)/include \
+	-I$(top_builddir)/include \
+	$(NULL)
+
+AM_CFLAGS = \
+	-Wall \
+	$(LIBOSMOCORE_CFLAGS) \
+	$(NULL)
+
+noinst_PROGRAMS = \
+	pfcp_test \
+	$(NULL)
+
+EXTRA_DIST = \
+	pfcp_test.ok \
+	$(NULL)
+
+pfcp_test_SOURCES = \
+	pfcp_test.c \
+	$(NULL)
+
+pfcp_test_LDADD = \
+	$(top_builddir)/src/libosmo-pfcp/libosmo-pfcp.la \
+	$(top_builddir)/src/libosmo-gtlv/libosmo-gtlv.la \
+	$(LIBOSMOCORE_LIBS) \
+	$(NULL)
+
+.PHONY: update_exp
+update_exp:
+	$(builddir)/pfcp_test >$(srcdir)/pfcp_test.ok

tests/libosmo-pfcp/pfcp_test.c

@@ -0,0 +1,505 @@
+/*
+ * (C) 2021-2022 by sysmocom - s.f.m.c. GmbH <info@sysmocom.de>
+ * All Rights Reserved.
+ *
+ * Author: Neels Janosch Hofmeyr <nhofmeyr@sysmocom.de>
+ *
+ * SPDX-License-Identifier: GPL-2.0+
+ *
+ *  This program is free software; you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation; either version 2 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  You should have received a copy of the GNU General Public License
+ *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ *
+ */
+
+#include <stdio.h>
+#include <errno.h>
+
+#include <osmocom/core/application.h>
+#include <osmocom/core/utils.h>
+#include <osmocom/core/msgb.h>
+
+#include <osmocom/pfcp/pfcp_msg.h>
+
+void *ctx;
+
+static const struct osmo_sockaddr v4_ue = {
+	.u.sin = {
+		.sin_family = AF_INET,
+		.sin_addr = { 0x1700a8c0 },
+	}
+};
+
+static const struct osmo_sockaddr v4_gtp = {
+	.u.sin = {
+		.sin_family = AF_INET,
+		.sin_addr = { 0x0708090a },
+	}
+};
+
+static const struct osmo_pfcp_ie_f_teid f_teid_access_local = {
+	.choose_flag = true,
+	.choose = {
+		.ipv4_addr = true,
+	},
+};
+
+static const struct osmo_pfcp_ie_f_teid f_teid_created = {
+	.fixed = {
+		.teid = 1234,
+		.ip_addr = {
+			.v4_present = true,
+			.v4 = v4_gtp,
+		},
+	},
+};
+
+static const struct osmo_pfcp_ie_outer_header_creation ohc_access = {
+	.desc_bits = { 0x01 },
+	.teid_present = true,
+	.teid = 0xabcdef,
+	.ip_addr = {
+		.v4_present = true,
+		.v4 = v4_gtp,
+	},
+};
+
+static const struct osmo_pfcp_ie_apply_action aa_forw = {
+	.bits = { 0x02 },
+};
+
+static const struct osmo_pfcp_ie_f_seid f_seid = {
+	.seid = 0x1234567890abcdef,
+	.ip_addr = {
+		.v4_present = true,
+		.v4 = v4_gtp,
+	},
+};
+
+struct osmo_pfcp_msg tests[] = {
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_HEARTBEAT_REQ,
+			.sequence_nr = 1,
+		},
+		.ies.heartbeat_req = {
+			.recovery_time_stamp = 1234,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_HEARTBEAT_RESP,
+			.sequence_nr = 2,
+		},
+		.ies.heartbeat_resp = {
+			.recovery_time_stamp = 5678,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_ASSOC_SETUP_REQ,
+			.sequence_nr = 3,
+		},
+		.ies.assoc_setup_req = {
+			.node_id = {
+				.type = OSMO_PFCP_NODE_ID_T_IPV4,
+				.ip.u.sin = {
+					.sin_family = AF_INET,
+					.sin_addr = { 0x01020304 },
+				},
+			},
+			.recovery_time_stamp = 0x2b2b2b2b,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_ASSOC_SETUP_RESP,
+			.sequence_nr = 4,
+		},
+		.ies.assoc_setup_resp = {
+			.node_id = {
+				.type = OSMO_PFCP_NODE_ID_T_FQDN,
+				.fqdn = "example.com",
+			},
+			.cause = OSMO_PFCP_CAUSE_REQUEST_ACCEPTED,
+			.recovery_time_stamp = 0x2b2b2b2b,
+			.up_function_features_present = true,
+			.up_function_features.bits = { 1, 2 },
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_ASSOC_RELEASE_REQ,
+			.sequence_nr = 5,
+		},
+		.ies.assoc_release_req = {
+			.node_id = {
+				.type = OSMO_PFCP_NODE_ID_T_IPV6,
+				.ip.u.sin6 = {
+					.sin6_family = AF_INET6,
+					.sin6_addr = {{{ 1, 2, 3, 4 }}},
+				},
+			},
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_ASSOC_RELEASE_RESP,
+			.sequence_nr = 6,
+		},
+		.ies.assoc_release_resp = {
+			.node_id = {
+				.type = OSMO_PFCP_NODE_ID_T_IPV4,
+				.ip.u.sin = {
+					.sin_family = AF_INET,
+					.sin_addr = { 0x01020304 },
+				},
+			},
+			.cause = OSMO_PFCP_CAUSE_REQUEST_REJECTED,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_EST_REQ,
+			.sequence_nr = 7,
+			.seid_present = true,
+			.seid = 0,
+		},
+		.ies.session_est_req = {
+			.node_id = {
+				.type = OSMO_PFCP_NODE_ID_T_IPV4,
+				.ip.u.sin = {
+					.sin_family = AF_INET,
+					.sin_addr = { 0x0100007f },
+				},
+			},
+			.cp_f_seid_present = true,
+			.cp_f_seid = f_seid,
+			.create_pdr_count = 2,
+			.create_pdr = {
+				{
+					.pdr_id = 1,
+					.precedence = 255,
+					.pdi = {
+						.source_iface = OSMO_PFCP_SOURCE_IFACE_CORE,
+						.ue_ip_address_present = true,
+						.ue_ip_address = {
+							.ip_is_destination = true,
+							.ip_addr = {
+								.v4_present = true,
+								.v4 = v4_ue,
+							},
+						},
+					},
+					.far_id_present = true,
+					.far_id = 1,
+				},
+				{
+					.pdr_id = 2,
+					.precedence = 255,
+					.pdi = {
+						.source_iface = OSMO_PFCP_SOURCE_IFACE_ACCESS,
+						.local_f_teid_present = true,
+						.local_f_teid = f_teid_access_local,
+					},
+					.outer_header_removal_present = true,
+					.outer_header_removal = {
+						.desc = OSMO_PFCP_OUTER_HEADER_REMOVAL_GTP_U_UDP_IPV4,
+					},
+					.far_id_present = true,
+					.far_id = 2,
+				},
+			},
+			.create_far_count = 2,
+			.create_far = {
+				{
+					.far_id = 1,
+					.forw_params_present = true,
+					.forw_params = {
+						.destination_iface = OSMO_PFCP_DEST_IFACE_ACCESS,
+						.outer_header_creation_present = true,
+						.outer_header_creation = ohc_access,
+					},
+					.apply_action = aa_forw,
+				},
+				{
+					.far_id = 2,
+					.forw_params_present = true,
+					.forw_params = {
+						.destination_iface = OSMO_PFCP_DEST_IFACE_CORE,
+					},
+					.apply_action = aa_forw,
+				},
+			},
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_EST_RESP,
+			.sequence_nr = 8,
+			.seid_present = true,
+			.seid = 0x0123456789abcdef,
+		},
+		.ies.session_est_resp = {
+			.node_id = {
+				.type = OSMO_PFCP_NODE_ID_T_IPV4,
+				.ip.u.sin = {
+					.sin_family = AF_INET,
+					.sin_addr = { 0x0200007f },
+				},
+			},
+			.cause = OSMO_PFCP_CAUSE_REQUEST_ACCEPTED,
+			.up_f_seid_present = true,
+			.up_f_seid = f_seid,
+			.created_pdr_count = 2,
+			.created_pdr = {
+				{
+					.pdr_id = 1,
+				},
+				{
+					.pdr_id = 2,
+					.local_f_teid_present = true,
+					.local_f_teid = f_teid_created,
+				},
+			},
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_MOD_REQ,
+			.sequence_nr = 9,
+			.seid_present = true,
+			.seid = 0,
+		},
+		.ies.session_mod_req = {
+			.remove_pdr_count = 1,
+			.remove_pdr = {
+				{
+					.pdr_id = 1,
+				},
+			},
+			.remove_far_count = 1,
+			.remove_far = {
+				{
+					.far_id = 1,
+				},
+			},
+			.create_pdr_count = 1,
+			.create_pdr = {
+				{
+					.pdr_id = 3,
+					.precedence = 255,
+					.pdi = {
+						.source_iface = OSMO_PFCP_SOURCE_IFACE_ACCESS,
+						.local_f_teid_present = true,
+						.local_f_teid = f_teid_access_local,
+					},
+					.outer_header_removal_present = true,
+					.outer_header_removal = {
+						.desc = OSMO_PFCP_OUTER_HEADER_REMOVAL_GTP_U_UDP_IPV4,
+					},
+					.far_id_present = true,
+					.far_id = 3,
+				},
+			},
+			.create_far_count = 1,
+			.create_far = {
+				{
+					.far_id = 3,
+					.forw_params_present = true,
+					.forw_params = {
+						.destination_iface = OSMO_PFCP_DEST_IFACE_ACCESS,
+						.outer_header_creation_present = true,
+						.outer_header_creation = ohc_access,
+					},
+					.apply_action = aa_forw,
+				},
+			},
+			.upd_pdr_count = 1,
+			.upd_pdr = {
+				{
+					.pdr_id = 1,
+					.pdi = {
+						.source_iface = OSMO_PFCP_SOURCE_IFACE_ACCESS,
+						.local_f_teid_present = true,
+						.local_f_teid = f_teid_access_local,
+					},
+					.outer_header_removal_present = true,
+					.outer_header_removal = {
+						.desc = OSMO_PFCP_OUTER_HEADER_REMOVAL_GTP_U_UDP_IPV4,
+					},
+					.far_id_present = true,
+					.far_id = 1,
+				},
+			},
+			.upd_far_count = 1,
+			.upd_far = {
+				{
+					.far_id = 1,
+					.upd_forw_params_present = true,
+					.upd_forw_params = {
+						.destination_iface = OSMO_PFCP_DEST_IFACE_CORE,
+						.network_inst_present = true,
+						.network_inst = {
+							.str = "internet",
+						},
+					},
+					.apply_action = aa_forw,
+				},
+			},
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_MOD_RESP,
+			.sequence_nr = 10,
+			.seid_present = true,
+			.seid = 0x0123456789abcdef,
+		},
+		.ies.session_mod_resp = {
+			.cause = OSMO_PFCP_CAUSE_REQUEST_ACCEPTED,
+			.created_pdr_count = 1,
+			.created_pdr = {
+				{
+					.pdr_id = 3,
+					.local_f_teid_present = true,
+					.local_f_teid = f_teid_created,
+				},
+			},
+			.updated_pdr_count = 1,
+			.updated_pdr = {
+				{
+					.pdr_id = 1,
+					.local_f_teid_present = true,
+					.local_f_teid = f_teid_created,
+				},
+			},
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_MOD_RESP,
+			.sequence_nr = 11,
+			.seid_present = true,
+			.seid = 0x0123456789abcdef,
+		},
+		.ies.session_mod_resp = {
+			.cause = OSMO_PFCP_CAUSE_MANDATORY_IE_MISSING,
+			.offending_ie_present = true,
+			.offending_ie = OSMO_PFCP_IEI_APPLY_ACTION,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_DEL_REQ,
+			.sequence_nr = 12,
+			.seid_present = true,
+			.seid = 0x0123456789abcdef,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_DEL_RESP,
+			.sequence_nr = 13,
+			.seid_present = true,
+			.seid = 0x0123456789abcdef,
+		},
+		.ies.session_del_resp = {
+			.cause = OSMO_PFCP_CAUSE_NO_ESTABLISHED_PFCP_ASSOC,
+		},
+	},
+	{
+		.h = {
+			.version = 1,
+			.message_type = OSMO_PFCP_MSGT_SESSION_DEL_RESP,
+			.sequence_nr = 13,
+			.seid_present = true,
+			.seid = 0x0123456789abcdef,
+		},
+		.ies.session_del_resp = {
+			.cause = OSMO_PFCP_CAUSE_REQUEST_ACCEPTED,
+		},
+	},
+};
+
+void test_enc_dec()
+{
+	int i;
+	for (i = 0; i < ARRAY_SIZE(tests); i++) {
+		void *loop_ctx = talloc_named_const(ctx, 0, "loop");
+		int rc;
+		const struct osmo_pfcp_msg *orig = &tests[i];
+		struct osmo_pfcp_msg parsed = {};
+		struct msgb *msg;
+		struct osmo_gtlv_load tlv;
+
+		printf("\n=== start %s[%d]\n", __func__, i);
+		printf("encoding: %s\n", osmo_pfcp_message_type_str(orig->h.message_type));
+		printf("%s\n", osmo_pfcp_msg_to_str_c(loop_ctx, orig));
+		msg = msgb_alloc(1024, __func__),
+		rc = osmo_pfcp_msg_encode(msg, orig);
+		printf("osmo_pfcp_msg_encode() rc = %d\n", rc);
+		printf("%s.\n", osmo_hexdump(msg->data, msg->len));
+
+		rc = osmo_pfcp_msg_decode_header(&tlv, &parsed, msg);
+		printf("osmo_pfcp_msg_decode_header() rc = %d\n", rc);
+		if (rc != msgb_length(msg)) {
+			printf("ERROR: expected rc = %d\n", msgb_length(msg));
+			exit(1);
+		} else {
+			printf("rc == msgb_length()\n");
+		}
+
+		rc = osmo_pfcp_msg_decode_tlv(&parsed, &tlv);
+		printf("osmo_pfcp_msg_decode_tlv() rc = %d\n", rc);
+
+		if (strcmp(osmo_pfcp_msg_to_str_c(loop_ctx, orig),
+			   osmo_pfcp_msg_to_str_c(loop_ctx, &parsed))) {
+			printf(" ERROR: parsed != orig\n");
+			printf("   orig: %s\n",
+			       osmo_pfcp_msg_to_str_c(loop_ctx, orig));
+			printf(" parsed: %s\n",
+			       osmo_pfcp_msg_to_str_c(loop_ctx, &parsed));
+			exit(1);
+		} else {
+			printf("parsed == orig\n");
+		}
+
+		msgb_free(msg);
+		printf("=== end %s[%d]\n", __func__, i);
+		talloc_free(loop_ctx);
+	}
+}
+
+int main()
+{
+	ctx = talloc_named_const(NULL, 0, "pfcp_test");
+	msgb_talloc_ctx_init(ctx, 0);
+
+	test_enc_dec();
+
+	talloc_free(ctx);
+	return 0;
+}

tests/libosmo-pfcp/pfcp_test.ok

@@ -0,0 +1,154 @@
+
+=== start test_enc_dec[0]
+encoding: HEARTBEAT_REQ
+PFCPv1 HEARTBEAT_REQ hdr={seq=1} ies={ 'Recovery Time Stamp'=1234 }
+osmo_pfcp_msg_encode() rc = 0
+20 01 00 0c 00 00 01 00 00 60 00 04 00 00 04 d2 .
+osmo_pfcp_msg_decode_header() rc = 16
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[0]
+
+=== start test_enc_dec[1]
+encoding: HEARTBEAT_RESP
+PFCPv1 HEARTBEAT_RESP hdr={seq=2} ies={ 'Recovery Time Stamp'=5678 }
+osmo_pfcp_msg_encode() rc = 0
+20 02 00 0c 00 00 02 00 00 60 00 04 00 00 16 2e .
+osmo_pfcp_msg_decode_header() rc = 16
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[1]
+
+=== start test_enc_dec[2]
+encoding: ASSOC_SETUP_REQ
+PFCPv1 ASSOC_SETUP_REQ hdr={seq=3} ies={ 'Node ID'=v4:4.3.2.1 'Recovery Time Stamp'=724249387 }
+osmo_pfcp_msg_encode() rc = 0
+20 05 00 15 00 00 03 00 00 3c 00 05 00 04 03 02 01 00 60 00 04 2b 2b 2b 2b .
+osmo_pfcp_msg_decode_header() rc = 25
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[2]
+
+=== start test_enc_dec[3]
+encoding: ASSOC_SETUP_RESP
+PFCPv1 ASSOC_SETUP_RESP hdr={seq=4} ies={ 'Node ID'=fqdn:"example.com" 'Cause'=Request accepted (success) 'Recovery Time Stamp'=724249387 'UP Function Features'=( BUCP PDIU ) }
+osmo_pfcp_msg_encode() rc = 0
+20 06 00 2b 00 00 04 00 00 3c 00 0c 02 65 78 61 6d 70 6c 65 2e 63 6f 6d 00 13 00 01 01 00 60 00 04 2b 2b 2b 2b 00 2b 00 06 01 02 00 00 00 00 .
+osmo_pfcp_msg_decode_header() rc = 47
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[3]
+
+=== start test_enc_dec[4]
+encoding: ASSOC_RELEASE_REQ
+PFCPv1 ASSOC_RELEASE_REQ hdr={seq=5} ies={ 'Node ID'=v6:[102:304::] }
+osmo_pfcp_msg_encode() rc = 0
+20 09 00 19 00 00 05 00 00 3c 00 11 01 01 02 03 04 00 00 00 00 00 00 00 00 00 00 00 00 .
+osmo_pfcp_msg_decode_header() rc = 29
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[4]
+
+=== start test_enc_dec[5]
+encoding: ASSOC_RELEASE_RESP
+PFCPv1 ASSOC_RELEASE_RESP hdr={seq=6} ies={ 'Node ID'=v4:4.3.2.1 'Cause'=Request rejected (reason not specified) }
+osmo_pfcp_msg_encode() rc = 0
+20 0a 00 12 00 00 06 00 00 3c 00 05 00 04 03 02 01 00 13 00 01 40 .
+osmo_pfcp_msg_decode_header() rc = 22
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[5]
+
+=== start test_enc_dec[6]
+encoding: SESSION_EST_REQ
+PFCPv1 SESSION_EST_REQ hdr={seq=7 SEID=0x0} ies={ 'Node ID'=v4:127.0.0.1 'F-SEID'=0x1234567890abcdef,v4:10.9.8.7 'Create PDR'={ { 'PDR ID'=1 'Precedence'=255 'PDI'={ 'Source Interface'=Core 'UE IP Address'=,dst,v4:192.168.0.23 } 'FAR ID'=1 }, { 'PDR ID'=2 'Precedence'=255 'PDI'={ 'Source Interface'=Access 'F-TEID'=CHOOSE-v4 } 'Outer Header Removal'=GTP_U_UDP_IPV4 'FAR ID'=2 } } 'Create FAR'={ { 'FAR ID'=1 'Apply Action'=( FORW ) 'Forwarding Parameters'={ 'Destination Interface'=Access 'Outer Header Creation'=( GTP_U_UDP_IPV4 ),TEID:0xabcdef,v4:10.9.8.7 } }, { 'FAR ID'=2 'Apply Action'=( FORW ) 'Forwarding Parameters'={ 'Destination Interface'=Core } } } }
+osmo_pfcp_msg_encode() rc = 0
+21 32 00 c3 00 00 00 00 00 00 00 00 00 00 07 00 00 3c 00 05 00 7f 00 00 01 00 39 00 0d 02 12 34 56 78 90 ab cd ef 0a 09 08 07 00 01 00 28 00 38 00 02 00 01 00 1d 00 04 00 00 00 ff 00 02 00 0e 00 14 00 01 01 00 5d 00 05 06 c0 a8 00 17 00 6c 00 04 00 00 00 01 00 01 00 29 00 38 00 02 00 02 00 1d 00 04 00 00 00 ff 00 02 00 0a 00 14 00 01 00 00 15 00 01 05 00 5f 00 01 00 00 6c 00 04 00 00 00 02 00 03 00 25 00 6c 00 04 00 00 00 01 00 2c 00 02 02 00 00 04 00 13 00 2a 00 01 00 00 54 00 0a 01 00 00 ab cd ef 0a 09 08 07 00 03 00 17 00 6c 00 04 00 00 00 02 00 2c 00 02 02 00 00 04 00 05 00 2a 00 01 01 .
+osmo_pfcp_msg_decode_header() rc = 199
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[6]
+
+=== start test_enc_dec[7]
+encoding: SESSION_EST_RESP
+PFCPv1 SESSION_EST_RESP hdr={seq=8 SEID=0x123456789abcdef} ies={ 'Node ID'=v4:127.0.0.2 'Cause'=Request accepted (success) 'F-SEID'=0x1234567890abcdef,v4:10.9.8.7 'Created PDR'={ { 'PDR ID'=1 }, { 'PDR ID'=2 'F-TEID'=TEID-0x4d2,v4:10.9.8.7 } } }
+osmo_pfcp_msg_encode() rc = 0
+21 33 00 4c 01 23 45 67 89 ab cd ef 00 00 08 00 00 3c 00 05 00 7f 00 00 02 00 13 00 01 01 00 39 00 0d 02 12 34 56 78 90 ab cd ef 0a 09 08 07 00 08 00 06 00 38 00 02 00 01 00 08 00 13 00 38 00 02 00 02 00 15 00 09 01 00 00 04 d2 0a 09 08 07 .
+osmo_pfcp_msg_decode_header() rc = 80
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[7]
+
+=== start test_enc_dec[8]
+encoding: SESSION_MOD_REQ
+PFCPv1 SESSION_MOD_REQ hdr={seq=9 SEID=0x0} ies={ 'Remove PDR'={ { 'PDR ID'=1 } } 'Remove FAR'={ { 'FAR ID'=1 } } 'Create PDR'={ { 'PDR ID'=3 'Precedence'=255 'PDI'={ 'Source Interface'=Access 'F-TEID'=CHOOSE-v4 } 'Outer Header Removal'=GTP_U_UDP_IPV4 'FAR ID'=3 } } 'Create FAR'={ { 'FAR ID'=3 'Apply Action'=( FORW ) 'Forwarding Parameters'={ 'Destination Interface'=Access 'Outer Header Creation'=( GTP_U_UDP_IPV4 ),TEID:0xabcdef,v4:10.9.8.7 } } } 'Update PDR'={ { 'PDR ID'=1 'Outer Header Removal'=GTP_U_UDP_IPV4 'FAR ID'=1 } } 'Update FAR'={ { 'FAR ID'=1 'Update Forwarding Parameters'={ 'Network Instance'="internet" } } } }
+osmo_pfcp_msg_encode() rc = 0
+21 34 00 ab 00 00 00 00 00 00 00 00 00 00 09 00 00 0f 00 06 00 38 00 02 00 01 00 10 00 08 00 6c 00 04 00 00 00 01 00 01 00 29 00 38 00 02 00 03 00 1d 00 04 00 00 00 ff 00 02 00 0a 00 14 00 01 00 00 15 00 01 05 00 5f 00 01 00 00 6c 00 04 00 00 00 03 00 03 00 25 00 6c 00 04 00 00 00 03 00 2c 00 02 02 00 00 04 00 13 00 2a 00 01 00 00 54 00 0a 01 00 00 ab cd ef 0a 09 08 07 00 09 00 13 00 38 00 02 00 01 00 5f 00 01 00 00 6c 00 04 00 00 00 01 00 0a 00 18 00 6c 00 04 00 00 00 01 00 0b 00 0c 00 16 00 08 69 6e 74 65 72 6e 65 74 .
+osmo_pfcp_msg_decode_header() rc = 175
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[8]
+
+=== start test_enc_dec[9]
+encoding: SESSION_MOD_RESP
+PFCPv1 SESSION_MOD_RESP hdr={seq=10 SEID=0x123456789abcdef} ies={ 'Cause'=Request accepted (success) 'Created PDR'={ { 'PDR ID'=3 'F-TEID'=TEID-0x4d2,v4:10.9.8.7 } } 'Updated PDR'={ { 'PDR ID'=1 'F-TEID'=TEID-0x4d2,v4:10.9.8.7 } } }
+osmo_pfcp_msg_encode() rc = 0
+21 35 00 3f 01 23 45 67 89 ab cd ef 00 00 0a 00 00 13 00 01 01 00 08 00 13 00 38 00 02 00 03 00 15 00 09 01 00 00 04 d2 0a 09 08 07 01 00 00 13 00 38 00 02 00 01 00 15 00 09 01 00 00 04 d2 0a 09 08 07 .
+osmo_pfcp_msg_decode_header() rc = 67
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[9]
+
+=== start test_enc_dec[10]
+encoding: SESSION_MOD_RESP
+PFCPv1 SESSION_MOD_RESP hdr={seq=11 SEID=0x123456789abcdef} ies={ 'Cause'=Mandatory IE missing 'Offending IE'=Apply Action }
+osmo_pfcp_msg_encode() rc = 0
+21 35 00 17 01 23 45 67 89 ab cd ef 00 00 0b 00 00 13 00 01 42 00 28 00 02 00 2c .
+osmo_pfcp_msg_decode_header() rc = 27
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[10]
+
+=== start test_enc_dec[11]
+encoding: SESSION_DEL_REQ
+PFCPv1 SESSION_DEL_REQ hdr={seq=12 SEID=0x123456789abcdef} ies={ }
+osmo_pfcp_msg_encode() rc = 0
+21 36 00 0c 01 23 45 67 89 ab cd ef 00 00 0c 00 .
+osmo_pfcp_msg_decode_header() rc = 16
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[11]
+
+=== start test_enc_dec[12]
+encoding: SESSION_DEL_RESP
+PFCPv1 SESSION_DEL_RESP hdr={seq=13 SEID=0x123456789abcdef} ies={ 'Cause'=No established PFCP Association }
+osmo_pfcp_msg_encode() rc = 0
+21 37 00 11 01 23 45 67 89 ab cd ef 00 00 0d 00 00 13 00 01 48 .
+osmo_pfcp_msg_decode_header() rc = 21
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[12]
+
+=== start test_enc_dec[13]
+encoding: SESSION_DEL_RESP
+PFCPv1 SESSION_DEL_RESP hdr={seq=13 SEID=0x123456789abcdef} ies={ 'Cause'=Request accepted (success) }
+osmo_pfcp_msg_encode() rc = 0
+21 37 00 11 01 23 45 67 89 ab cd ef 00 00 0d 00 00 13 00 01 01 .
+osmo_pfcp_msg_decode_header() rc = 21
+rc == msgb_length()
+osmo_pfcp_msg_decode_tlv() rc = 0
+parsed == orig
+=== end test_enc_dec[13]

tests/testsuite.at

@@ -24,3 +24,9 @@ AT_KEYWORDS([tliv])
 cat $abs_srcdir/libosmo-gtlv/test_tliv/tliv_test.ok > expout
 AT_CHECK([$abs_top_builddir/tests/libosmo-gtlv/test_tliv/tliv_test], [], [expout], [ignore])
 AT_CLEANUP
+
+AT_SETUP([pfcp])
+AT_KEYWORDS([pfcp])
+cat $abs_srcdir/libosmo-pfcp/pfcp_test.ok > expout
+AT_CHECK([$abs_top_builddir/tests/libosmo-pfcp/pfcp_test], [], [expout], [ignore])
+AT_CLEANUP

tests/upf.vty

@@ -2,3 +2,34 @@ OsmoUPF> enable
 OsmoUPF# configure terminal
 OsmoUPF(config)# show running-config
 ...
+
+OsmoUPF(config)# pfcp
+OsmoUPF(config-pfcp)# list
+...
+  local-addr IP_ADDR
+OsmoUPF(config-pfcp)# local-addr?
+  local-addr  Set the local IP address to bind on for PFCP
+OsmoUPF(config-pfcp)# local-addr ?
+  IP_ADDR  IP address
+OsmoUPF(config-pfcp)# exit
+
+OsmoUPF(config)# gtp
+OsmoUPF(config-gtp)# list
+...
+  dev create DEVNAME [LISTEN_ADDR]
+  dev use DEVNAME
+  dev delete DEVNAME
+
+OsmoUPF(config-gtp)# dev?
+  dev  Configure the GTP device to use for encaps/decaps.
+OsmoUPF(config-gtp)# dev ?
+  create  create a new GTP device. Will listen on GTPv1 port 2152 and GTPv0 port 3386 on the specified interface, or on ANY if LISTEN_ADDR is omitted.
+  use     use an existing GTP device, e.g. created by 'gtp-link'
+  delete  Remove a GTP device from the configuration, and delete the device if it was created here.
+OsmoUPF(config-gtp)# dev create ?
+  DEVNAME  device name, e.g. 'apn0'
+OsmoUPF(config-gtp)# dev create foo ?
+  [LISTEN_ADDR]  IPv4 or IPv6 address to listen on, omit for any
+OsmoUPF(config-gtp)# dev delete ?
+  DEVNAME  device name, e.g. 'apn0'
+OsmoUPF(config-gtp)# exit