From 34ae8db68b3a9a66170dbf67b8ed6f623b7057df Mon Sep 17 00:00:00 2001
From: Benjamin Valentin <benjamin.valentin@ml-pa.com>
Date: Sun, 19 Jun 2022 23:08:20 +0200
Subject: [PATCH] tools/zep_dispatch: add function to get payload from ZEP
 frame

---
 dist/tools/zep_dispatch/zep_parser.c | 33 ++++++++++++++++++++++++++++
 dist/tools/zep_dispatch/zep_parser.h | 11 ++++++++++
 2 files changed, 44 insertions(+)

diff --git a/dist/tools/zep_dispatch/zep_parser.c b/dist/tools/zep_dispatch/zep_parser.c
index f769c33e11..65402bd48b 100644
--- a/dist/tools/zep_dispatch/zep_parser.c
+++ b/dist/tools/zep_dispatch/zep_parser.c
@@ -13,6 +13,39 @@
 
 #define SOCKET_ZEP_V2_TYPE_HELLO   (255)
 
+const void *zep_get_payload(const void *buffer, size_t *len)
+{
+    const void *payload;
+    const zep_v2_data_hdr_t *zep = buffer;
+
+    if (*len == 0) {
+        return NULL;
+    }
+
+    if ((zep->hdr.preamble[0] != 'E') || (zep->hdr.preamble[1] != 'X')) {
+        return NULL;
+    }
+
+    if (zep->hdr.version != 2) {
+        return NULL;
+    }
+
+    switch (zep->type) {
+    case ZEP_V2_TYPE_DATA:
+        payload = (zep_v2_data_hdr_t *)zep + 1;
+        break;
+    case ZEP_V2_TYPE_ACK:
+        payload = (zep_v2_ack_hdr_t *)zep + 1;
+        break;
+    default:
+        return NULL;
+    }
+
+    *len = zep->length - IEEE802154_FCS_LEN;
+
+    return payload;
+}
+
 bool zep_parse_mac(const void *buffer, size_t len, void *out, uint8_t *out_len)
 {
     const void *payload;
diff --git a/dist/tools/zep_dispatch/zep_parser.h b/dist/tools/zep_dispatch/zep_parser.h
index 1a9c0c0ba3..fa2f7c8dc7 100644
--- a/dist/tools/zep_dispatch/zep_parser.h
+++ b/dist/tools/zep_dispatch/zep_parser.h
@@ -15,6 +15,17 @@
 extern "C" {
 #endif
 
+/**
+ * @brief   Get the payload of a ZEP frame
+ *
+ * @param[in]  buffer   ZEP frame
+ * @param[in, out] len  size of buffer, will contain size of payload
+ *
+ * @return pointer to payload on success
+ *         NULL if no payload was found
+ */
+const void *zep_get_payload(const void *buffer, size_t *len);
+
 /**
  * @brief   Parse l2 source address of a ZEP frame
  *