MicrOBU/obu-firmware/main/gn_unwrap.c

#include "gn_unwrap.h"
#include <string.h>

// Mirrors dot11p.c / geonet.c's constants and layout, in reverse. Keep these two files in sync
// if either the TX-side frame shape or these constants change.
#define GN_ETHERTYPE                (0x8947)
#define LLC_SNAP_HEADER_LEN         (8)
#define IEEE80211_HEADER_LEN        (24)  // non-QoS Data
#define IEEE80211_QOS_CTRL_LEN      (2)   // extra field QoS Data frames add
#define IEEE80211_FC_TYPE_DATA      (2)
#define IEEE80211_FC_QOS_SUBTYPE_BIT (0x08)

#define GN_BASIC_HEADER_LEN         (4)
#define GN_COMMON_HEADER_LEN        (8)
#define GN_SHB_EXT_HEADER_LEN       (24) // Source Long Position Vector, geonet.c's SHB shape
#define BTP_B_HEADER_LEN            (4)

#define GN_HEADER_TYPE_TSB          (5)  // Topologically-Scoped Broadcast
#define GN_HEADER_SUBTYPE_SINGLE_HOP (0)

#define BTP_DEST_PORT_CAM           (2001) // ETSI TS 103 248

static const uint8_t s_llc_snap_prefix[6] = {0xAA, 0xAA, 0x03, 0x00, 0x00, 0x00};

bool gn_unwrap_cam(const uint8_t *frame, int frame_len,
                    const uint8_t **out_cam, int *out_cam_len)
{
    if (!frame || frame_len < IEEE80211_HEADER_LEN) {
        return false;
    }

    uint8_t fc0 = frame[0];
    uint8_t fc1 = frame[1];
    uint8_t type = (fc0 >> 2) & 0x03;
    uint8_t subtype = (fc0 >> 4) & 0x0F;
    bool to_ds = fc1 & 0x01;
    bool from_ds = fc1 & 0x02;

    // Only plain broadcast Data frames, no WDS - matches what dot11p_build_frame ever produces
    // (and what real ITS-G5 hardware sends).
    if (type != IEEE80211_FC_TYPE_DATA || (to_ds && from_ds)) {
        return false;
    }

    int offset = IEEE80211_HEADER_LEN;
    if (subtype & IEEE80211_FC_QOS_SUBTYPE_BIT) {
        offset += IEEE80211_QOS_CTRL_LEN;
    }

    if (frame_len < offset + LLC_SNAP_HEADER_LEN) {
        return false;
    }
    if (memcmp(frame + offset, s_llc_snap_prefix, sizeof(s_llc_snap_prefix)) != 0) {
        return false;
    }
    uint16_t ethertype = ((uint16_t)frame[offset + 6] << 8) | frame[offset + 7];
    if (ethertype != GN_ETHERTYPE) {
        return false;
    }
    offset += LLC_SNAP_HEADER_LEN;

    // ---- GN Basic Header (4 bytes) ---- nothing here we need to validate for our purposes;
    // just skip it. (version/NextHeader in byte0, lifetime in byte2, RHL in byte3.)
    if (frame_len < offset + GN_BASIC_HEADER_LEN) {
        return false;
    }
    offset += GN_BASIC_HEADER_LEN;

    // ---- GN Common Header (8 bytes) ----
    if (frame_len < offset + GN_COMMON_HEADER_LEN) {
        return false;
    }
    uint8_t next_header = (frame[offset + 0] >> 4) & 0x0F;
    uint8_t header_type = (frame[offset + 1] >> 4) & 0x0F;
    uint8_t header_subtype = frame[offset + 1] & 0x0F;
    if (next_header != 2 /* BTP-B */) {
        return false;
    }
    if (header_type != GN_HEADER_TYPE_TSB || header_subtype != GN_HEADER_SUBTYPE_SINGLE_HOP) {
        // Not a single-hop-broadcast frame - e.g. GeoBroadcast (DENM-style dissemination) or
        // something this project doesn't transmit/expect. Not an error, just not for us yet -
        // see gn_unwrap.h's note on scope.
        return false;
    }
    offset += GN_COMMON_HEADER_LEN;

    // ---- SHB extended header (24 bytes) ---- skip straight past it, we don't need the
    // sender's claimed position/speed/heading here (the CAM payload has its own, more precise
    // versions of those same fields).
    if (frame_len < offset + GN_SHB_EXT_HEADER_LEN) {
        return false;
    }
    offset += GN_SHB_EXT_HEADER_LEN;

    // ---- BTP-B header (4 bytes) ----
    if (frame_len < offset + BTP_B_HEADER_LEN) {
        return false;
    }
    uint16_t dest_port = ((uint16_t)frame[offset + 0] << 8) | frame[offset + 1];
    if (dest_port != BTP_DEST_PORT_CAM) {
        return false; // e.g. DENM (2002) - not decoded by this project yet
    }
    offset += BTP_B_HEADER_LEN;

    // ---- Whatever's left is the CAM UPER payload ----
    int cam_len = frame_len - offset;
    if (cam_len <= 0) {
        return false;
    }

    *out_cam = frame + offset;
    *out_cam_len = cam_len;
    return true;
}