stream: Add IPA send function/IPA-mode read to srv

Also: Adapt the example to work with the new changes.

Related OS#5753, OS#5751

Change-Id: I61e1fe59166c46595efe8c1f32b8f2607cb6c529

3 files changed, 89 insertions, 8 deletions

diff --git a/examples/ipa-stream-server.c b/examples/ipa-stream-server.c
index df5c26c..3146e63 100644
--- a/examples/ipa-stream-server.c
+++ b/examples/ipa-stream-server.c
@@ -49,15 +49,9 @@ void sighandler(int foo)
 
 int read_cb(struct osmo_stream_srv *conn, struct msgb *msg)
 {
-	LOGP(DSTREAMTEST, LOGL_DEBUG, "received message from stream (len=%d)\n", msgb_length(msg));
+	LOGP(DSTREAMTEST, LOGL_DEBUG, "received message from stream (payload len=%d)\n", msgb_length(msg));
 
-	if (osmo_ipa_process_msg(msg) < 0) {
-		LOGP(DSTREAMTEST, LOGL_ERROR, "Bad IPA message\n");
-		msgb_free(msg);
-		return 0;
-	}
-
-	osmo_stream_srv_send(conn, msg);
+	osmo_stream_srv_send_ipa(conn, IPAC_PROTO_UNSPECIFIED, IPAC_PROTO_UNSPECIFIED, msg);
 	return 0;
 }
 
diff --git a/include/osmocom/netif/stream.h b/include/osmocom/netif/stream.h
index fa76d96..16940dc 100644
--- a/include/osmocom/netif/stream.h
+++ b/include/osmocom/netif/stream.h
@@ -4,6 +4,7 @@
 #include <stdint.h>
 
 #include <osmocom/core/msgb.h>
+#include <osmocom/gsm/protocol/ipaccess.h>
 
 /*! \addtogroup stream
  *  @{
@@ -29,6 +30,8 @@ enum osmo_stream_proto {
 	_NUM_OSMO_STREAM_PROTOS
 };
 
+#define IPAC_PROTO_UNSPECIFIED -1
+
 /*! \brief Shortcut for unsetting the stream protocol (gets rid of segmentation pertaining to stream protocol) */
 #define osmo_stream_srv_link_unset_stream_proto(struct_osmo_stream_srv_link_ptr)\
 	osmo_stream_srv_link_set_stream_proto(struct_osmo_stream_srv_link_ptr, OSMO_STREAM_UNSPECIFIED)
@@ -68,6 +71,8 @@ void osmo_stream_srv_set_flush_and_destroy(struct osmo_stream_srv *conn);
 void osmo_stream_srv_set_data(struct osmo_stream_srv *conn, void *data);
 
 void osmo_stream_srv_send(struct osmo_stream_srv *conn, struct msgb *msg);
+void osmo_stream_srv_send_ipa(struct osmo_stream_srv *conn, int ipaccess_proto,
+                              enum ipaccess_proto_ext pe, struct msgb *msg);
 int osmo_stream_srv_recv(struct osmo_stream_srv *conn, struct msgb *msg);
 
 void osmo_stream_srv_clear_tx_queue(struct osmo_stream_srv *conn);
diff --git a/src/stream.c b/src/stream.c
index e773466..4e57e03 100644
--- a/src/stream.c
+++ b/src/stream.c
@@ -44,6 +44,7 @@
 #include <osmocom/core/talloc.h>
 #include <osmocom/core/socket.h>
 
+#include <osmocom/netif/ipa.h>
 #include <osmocom/netif/stream.h>
 
 #include "config.h"
@@ -637,6 +638,45 @@ static void handle_connecting(struct osmo_stream_cli *cli, int res)
 		cli->connect_cb(cli);
 }
 
+#define LOG_CLI_OR_SRV(cli, level, fmt, args...) do {\
+	if (cli != NULL)\
+		LOGSCLI(cli, level, fmt, ## args);\
+	else\
+		LOGP(DLINP, level, fmt, ## args);\
+} while (0)
+
+/* Check and remove headers, in case of p == IPAC_PROTO_OSMO also the IPA extension header.
+ * Returns a negative number on error, otherwise the number of header octets removed */
+static inline int ipa_check_pull_headers(struct msgb *msg, struct osmo_stream_cli *cli)
+{
+	int ret;
+	size_t octets_removed = 0;
+	msg->l1h = msg->data;
+	struct ipa_head *ih = (struct ipa_head *)msg->data;
+	if ((ret = osmo_ipa_process_msg(msg)) < 0) {
+		LOG_CLI_OR_SRV(cli, LOGL_ERROR, "Error processing IPA message\n");
+		return ret;
+	}
+	msgb_pull(msg, sizeof(struct ipa_head));
+	octets_removed += sizeof(struct ipa_head);
+	if (ih->proto != IPAC_PROTO_OSMO) /* No extensions expected */
+		return octets_removed;
+	msg->l2h = msg->data;
+	msgb_pull(msg, sizeof(struct ipa_head_ext));
+	octets_removed += sizeof(struct ipa_head_ext);
+	return octets_removed;
+}
+
+/* Push IPA headers; if we have IPAC_PROTO_OSMO this also takes care of the
+ * extension header */
+static inline void ipa_push_headers(enum ipaccess_proto p, enum ipaccess_proto_ext pe,
+				    struct msgb *msg)
+{
+	if (p == IPAC_PROTO_OSMO)
+		ipa_prepend_header_ext(msg, pe);
+	osmo_ipa_msg_push_header(msg, p);
+}
+
 static void stream_cli_iofd_read_cb(struct osmo_io_fd *iofd, int res, struct msgb *msg)
 {
 	struct osmo_stream_cli *cli  = osmo_iofd_get_data(iofd);
@@ -1428,6 +1468,8 @@ static void stream_srv_iofd_read_cb(struct osmo_io_fd *iofd, int res, struct msg
 		if (osmo_iofd_txqueue_len(iofd) == 0)
 			osmo_stream_srv_destroy(conn);
 	} else if (conn->iofd_read_cb) {
+		if (conn->srv->stream_proto == OSMO_STREAM_IPAC)
+			ipa_check_pull_headers(msg, NULL);
 		conn->iofd_read_cb(conn, msg); // TODO: Handle return value?
 	}
 
@@ -1892,6 +1934,46 @@ void osmo_stream_srv_destroy(struct osmo_stream_srv *conn)
 	talloc_free(conn);
 }
 
+/* msgb_l1(msg) is expected to be set */
+static inline enum ipaccess_proto msg_get_ipa_proto(struct msgb *msg)
+{
+	struct ipa_head *ih = msgb_l1(msg);
+	OSMO_ASSERT(ih);
+	return ih->proto;
+}
+
+/* msgb->l1h is expected to be set, msgb->l2h is expected to be set if
+ * we have IPAC_PROTO_OSMO specified in the header.
+ * Returns the protocol extension (enum ipaccess_proto) or -ENOPROTOOPT if
+ * we don't have IPAC_PROTO_OSMO specified in the IPA header */
+static inline int msg_get_ipa_proto_ext(struct msgb *msg)
+{
+	if (msg_get_ipa_proto(msg) != IPAC_PROTO_OSMO)
+		return -ENOPROTOOPT;
+	struct ipa_head_ext *ihe = msgb_l2(msg);
+	OSMO_ASSERT(ihe);
+	return ihe->proto;
+}
+
+/*! \brief Enqueue IPA data to be sent via an Osmocom stream server
+ *  \param[in] conn Stream Server through which we want to send
+ *  \param[in] p   Protocol transported by IPA. When set to IPAC_PROTO_UNSPECIFIED, the protocol will be
+ *		   read from the msgb structure's l1 and possibly l2 headers.
+ *  \param[in] pe  Ignored, unless p == IPAC_PROTO_OSMO, in which case this specifies the
+ *		 Osmocom protocol extension
+ *  \param[in] msg Message buffer to enqueue in transmit queue */
+void osmo_stream_srv_send_ipa(struct osmo_stream_srv *conn, int ipaccess_proto,
+			      enum ipaccess_proto_ext pe, struct msgb *msg)
+{
+	OSMO_ASSERT(msg);
+	if (ipaccess_proto == IPAC_PROTO_UNSPECIFIED) {
+		ipaccess_proto = msg_get_ipa_proto(msg);
+		pe = msg_get_ipa_proto_ext(msg);
+	}
+	ipa_push_headers(ipaccess_proto, pe, msg);
+	osmo_stream_srv_send(conn, msg);
+}
+
 /*! \brief Enqueue data to be sent via an Osmocom stream server
  *  \param[in] conn Stream Server through which we want to send
  *  \param[in] msg Message buffer to enqueue in transmit queue */