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 BTP_B_HEADER_LEN            (4)

// Extended-header lengths per GeoNetworking header type - see gn_unwrap.h for why these exact
// numbers, and why they must not be assumed equal.
#define GN_SHB_EXT_HEADER_LEN       (28) // SO PV (24) + Reserved (4)
#define GN_GBC_EXT_HEADER_LEN       (44) // SN(2) + Rsvd(2) + SO PV(24) + area(12) + Rsvd(4)

// Offsets of the destination-area fields within the GBC extended header.
#define GBC_AREA_LAT_OFFSET         (28)
#define GBC_AREA_LON_OFFSET         (32)
#define GBC_AREA_DIST_A_OFFSET      (36)

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

#define GN_NEXT_HEADER_COMMON       (1)  // unsecured; 2 would be a secured packet
#define GN_COMMON_NEXT_HEADER_BTP_B (2)

#define BTP_DEST_PORT_CAM           (2001) // ETSI TS 103 248
#define BTP_DEST_PORT_DENM          (2002)
// NOTE the crossover: SPATEM is BTP port 2004 but ItsPduHeader messageID 4, while MAPEM is port
// 2003 and messageID 5. Port and messageID are NOT the same number - mixing them up routes every
// message to the wrong decoder on the phone.
#define BTP_DEST_PORT_SPATEM        (2004)

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

static int32_t be32(const uint8_t *p)
{
    return (int32_t)(((uint32_t)p[0] << 24) | ((uint32_t)p[1] << 16) |
                     ((uint32_t)p[2] << 8) | (uint32_t)p[3]);
}

static uint16_t be16(const uint8_t *p)
{
    return (uint16_t)(((uint16_t)p[0] << 8) | (uint16_t)p[1]);
}

bool gn_unwrap_its(const uint8_t *frame, int frame_len, gn_rx_t *out)
{
    if (!frame || !out || frame_len < IEEE80211_HEADER_LEN) {
        return false;
    }
    memset(out, 0, sizeof(*out));

    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. Both QoS Data (what real ITS-G5 hardware sends,
    // 26-byte header) and non-QoS Data (24-byte, what our own TX currently builds) are accepted.
    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;
    }
    if (be16(frame + offset + 6) != GN_ETHERTYPE) {
        return false;
    }
    offset += LLC_SNAP_HEADER_LEN;

    // ---- GN Basic Header (4 bytes) ----
    if (frame_len < offset + GN_BASIC_HEADER_LEN) {
        return false;
    }
    // NextHeader distinguishes an unsecured packet (1 = Common Header follows) from a secured one
    // (2 = a TS 103 097 SecuredMessage follows, with the Common Header buried inside it at a
    // variable offset). Checking this rather than blindly skipping means a secured packet is
    // rejected cleanly instead of having its security envelope misread as a Common Header.
    if ((frame[offset] & 0x0F) != GN_NEXT_HEADER_COMMON) {
        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 != GN_COMMON_NEXT_HEADER_BTP_B) {
        return false;
    }
    offset += GN_COMMON_HEADER_LEN;

    // ---- Extended header: length depends on the header type ----
    int ext_len;
    bool is_gbc = false;
    if (header_type == GN_HEADER_TYPE_TSB && header_subtype == GN_HEADER_SUBTYPE_SINGLE_HOP) {
        ext_len = GN_SHB_EXT_HEADER_LEN;
    } else if (header_type == GN_HEADER_TYPE_GBC) {
        // Subtype selects the area shape (0 circle, 1 rectangle, 2 ellipse). All three carry the
        // same field layout - DistanceB and Angle are simply unused for a circle - so the length
        // is the same and we don't need to branch on it.
        ext_len = GN_GBC_EXT_HEADER_LEN;
        is_gbc = true;
    } else {
        return false; // Beacon / GeoUnicast / GeoAnycast / multi-hop TSB - see header comment
    }
    if (frame_len < offset + ext_len) {
        return false;
    }
    if (is_gbc) {
        out->has_geo_area = true;
        out->geo_area_lat_tenmicrodeg = be32(frame + offset + GBC_AREA_LAT_OFFSET);
        out->geo_area_lon_tenmicrodeg = be32(frame + offset + GBC_AREA_LON_OFFSET);
        out->geo_area_distance_a_m = be16(frame + offset + GBC_AREA_DIST_A_OFFSET);
    }
    offset += ext_len;

    // ---- BTP-B header (4 bytes) ----
    if (frame_len < offset + BTP_B_HEADER_LEN) {
        return false;
    }
    uint16_t dest_port = be16(frame + offset);
    if (dest_port != BTP_DEST_PORT_CAM && dest_port != BTP_DEST_PORT_DENM &&
        dest_port != BTP_DEST_PORT_SPATEM) {
        return false;
    }
    offset += BTP_B_HEADER_LEN;

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

    out->btp_dest_port = dest_port;
    out->payload = frame + offset;
    out->payload_len = payload_len;
    return true;
}