Add VLAN utils

2024-06-05 11:41:46 +08:00
parent a1e693a735
commit bb469ca1ed
9 changed files with 173 additions and 26 deletions
--- a/src/packet/vlan_utils.h
+++ b/src/packet/vlan_utils.h
@@ -0,0 +1,95 @@
+#ifndef _VLAN_UTILS_H
+#define _VLAN_UTILS_H
+
+#ifdef __cplusplus
+extern "C"
+{
+#endif
+
+#include <stdio.h>
+#include <string.h>
+#include <arpa/inet.h>
+#include "eth_utils.h"
+
+/*
+ * VLAN Header Format
+ *
+ *   0                   1                   2                   3
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  | Pri |I|    VLAN ID            |           Ethertype           |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+struct vlan_hdr
+{
+    uint16_t vlan_tci;
+    uint16_t vlan_ethertype;
+};
+
+/******************************************************************************
+ * get
+ ******************************************************************************/
+
+static inline uint8_t vlan_hdr_get_priority(const struct vlan_hdr *hdr)
+{
+    return (ntohs(hdr->vlan_tci) & 0xe000) >> 13;
+}
+
+static inline uint8_t vlan_hdr_get_dei(const struct vlan_hdr *hdr)
+{
+    return (ntohs(hdr->vlan_tci) & 0x1000) >> 12;
+}
+
+static inline uint16_t vlan_hdr_get_vid(const struct vlan_hdr *hdr)
+{
+    return ntohs(hdr->vlan_tci) & 0x0fff;
+}
+
+static inline uint16_t vlan_hdr_get_ethertype(const struct vlan_hdr *hdr)
+{
+    return ntohs(hdr->vlan_ethertype);
+}
+
+/******************************************************************************
+ * set
+ ******************************************************************************/
+
+static inline void vlan_hdr_set_priority(struct vlan_hdr *hdr, uint8_t priority)
+{
+    hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0xe000) | (priority << 13));
+}
+
+static inline void vlan_hdr_set_dei(struct vlan_hdr *hdr, uint8_t dei)
+{
+    hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x1000) | (dei << 12));
+}
+
+static inline void vlan_hdr_set_vid(struct vlan_hdr *hdr, uint16_t vid)
+{
+    hdr->vlan_tci = htons((ntohs(hdr->vlan_tci) & ~0x0fff) | vid);
+}
+
+static inline void vlan_hdr_set_ethertype(struct vlan_hdr *hdr, uint16_t ethertype)
+{
+    hdr->vlan_ethertype = htons(ethertype);
+}
+
+/******************************************************************************
+ * print
+ ******************************************************************************/
+
+static inline int vlan_hdr_to_str(const struct vlan_hdr *hdr, char *buf, size_t size)
+{
+    memset(buf, 0, size);
+    uint16_t proto = vlan_hdr_get_ethertype(hdr);
+    return snprintf(buf, size, "VLAN: priority=%u dei=%u id=%u ethertype=%s",
+                    vlan_hdr_get_priority(hdr), vlan_hdr_get_dei(hdr),
+                    vlan_hdr_get_vid(hdr), eth_proto_to_str(proto));
+}
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif