Clawdia
221 lines
7.5 KiB
C
221 lines
7.5 KiB
C
/**
|
|
* Block 2: C-ITS Parser für ESP32-C5 C-ITS Sniffer
|
|
*
|
|
* Ziel: 802.11p / ITS-G5 Pakete empfangen, WAVE-Header parsen,
|
|
* C-ITS BSI (Basic Security Element) Header extrahieren
|
|
* und Nachrichtentyp identifizieren.
|
|
*
|
|
* Paket-Struktur (ITS-G5 / IEEE 1609.3 WSM):
|
|
* [802.11 MAC Header ~14-36B] [LLC/SNAP 8B] [WSM Header 6B] [C-ITS Payload]
|
|
*
|
|
* WSM Header (IEEE 1609.3):
|
|
* Bytes 0-2: PDU Length (24-bit)
|
|
* Bytes 3-8: Sender MAC (6 Bytes)
|
|
* Bytes 9-12: Timestamp (32-bit)
|
|
* Byte 13: Priority (0-255)
|
|
* Byte 14: PDU Type (WSM)
|
|
*
|
|
* C-ITS BSI (ETSI TS 103 097 / EN 15754):
|
|
* Byte 0: Message ID (msgId)
|
|
* Byte 1: Message Count (msgCnt)
|
|
* Byte 2: Section Number (sectionNum)
|
|
* Byte 3: Part ID (partId)
|
|
* Bytes 4-11: Station ID (8 bytes)
|
|
* Bytes 12+: DERSignedCertificate / Payload
|
|
*/
|
|
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
#include <stdlib.h>
|
|
#include <stdint.h>
|
|
#include <stdbool.h>
|
|
|
|
#include "sdkconfig.h"
|
|
#include "esp_err.h"
|
|
#include "esp_log.h"
|
|
#include "esp_wifi.h"
|
|
#include "freertos/FreeRTOS.h"
|
|
#include "freertos/task.h"
|
|
|
|
#include "block3_csv_writer.h"
|
|
|
|
static const char *TAG = "CITS_PARSER";
|
|
|
|
/* ---------- WAVE Short Message Header ---------- */
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t pdu_length[3]; // PDU Length (little-endian, 24-bit)
|
|
uint8_t sender_id[6]; // Sender MAC
|
|
uint32_t timestamp; // Timestamp
|
|
uint8_t priority; // Priority (0=lowest, 255=highest)
|
|
uint8_t pdu_type; // WSM PDU Type
|
|
} wave_header_t;
|
|
|
|
/* ---------- C-ITS BSI Header (ETSI TS 103 097) ---------- */
|
|
typedef struct __attribute__((packed)) {
|
|
uint8_t msg_id; // Message ID
|
|
uint8_t msg_cnt; // Message Count
|
|
uint8_t section_num; // Section Number
|
|
uint8_t part_id; // Part ID
|
|
uint8_t station_id[8]; // Station ID
|
|
} cits_bsi_header_t;
|
|
|
|
/* ---------- Message IDs nach ETSI TS 103 097 ---------- */
|
|
#define CITS_MSGID_DECEVENT 0x01
|
|
#define CITS_MSGID_DSECN 0x02
|
|
#define CITS_MSGID_DMM 0x03
|
|
#define CITS_MSGID_MAP 0x04
|
|
#define CITS_MSGID_MAPDATA 0x05
|
|
#define CITS_MSGID_MAPSELDATA 0x06
|
|
#define CITS_MSGID_RSM 0x07
|
|
#define CITS_MSGID_CAM 0x08
|
|
#define CITS_MSGID_DENM 0x09
|
|
#define CITS_MSGID_SPA 0x0A
|
|
#define CITS_MSGID_MAM 0x0B
|
|
#define CITS_MSGID_VIT 0x0C
|
|
#define CITS_MSGID_VSL 0x0D
|
|
#define CITS_MSGID_CSP 0x0E
|
|
#define CITS_MSGID_CAMsub 0x10
|
|
#define CITS_MSGID_DENMsub 0x11
|
|
#define CITS_MSGID_DCC 0x12
|
|
#define CITS_MSGID_SSM 0x13
|
|
#define CITS_MSGID_IVIM 0x14
|
|
#define CITS_MSGID_MAC 0x15
|
|
#define CITS_MSGID_CLM 0x16
|
|
#define CITS_MSGID_CLMrev 0x17
|
|
#define CITS_MSGID_DDM 0x18
|
|
|
|
static const char *msg_id_str(uint8_t msg_id)
|
|
{
|
|
switch (msg_id) {
|
|
case CITS_MSGID_DECEVENT: return "DECEVENT";
|
|
case CITS_MSGID_DSECN: return "DSECN";
|
|
case CITS_MSGID_DMM: return "DMM";
|
|
case CITS_MSGID_MAP: return "MAP";
|
|
case CITS_MSGID_MAPDATA: return "MAPDATA";
|
|
case CITS_MSGID_MAPSELDATA:return "MAPSELDATA";
|
|
case CITS_MSGID_RSM: return "RSM";
|
|
case CITS_MSGID_CAM: return "CAM";
|
|
case CITS_MSGID_DENM: return "DENM";
|
|
case CITS_MSGID_SPA: return "SPA";
|
|
case CITS_MSGID_MAM: return "MAM";
|
|
case CITS_MSGID_VIT: return "VIT";
|
|
case CITS_MSGID_VSL: return "VSL";
|
|
case CITS_MSGID_CSP: return "CSP";
|
|
case CITS_MSGID_CAMsub: return "CAMsub";
|
|
case CITS_MSGID_DENMsub: return "DENMsub";
|
|
case CITS_MSGID_DCC: return "DCC";
|
|
case CITS_MSGID_SSM: return "SSM";
|
|
case CITS_MSGID_IVIM: return "IVIM";
|
|
case CITS_MSGID_MAC: return "MAC";
|
|
case CITS_MSGID_CLM: return "CLM";
|
|
case CITS_MSGID_CLMrev: return "CLMrev";
|
|
case CITS_MSGID_DDM: return "DDM";
|
|
default: return "UNKWN";
|
|
}
|
|
}
|
|
|
|
/* ---------- MAC als hex-String ---------- */
|
|
static void mac_to_str(const uint8_t *mac, char *buf, size_t buflen)
|
|
{
|
|
snprintf(buf, buflen, "%02X:%02X:%02X:%02X:%02X:%02X",
|
|
mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);
|
|
}
|
|
|
|
/* ---------- PDU Length (24-bit little-endian) ---------- */
|
|
static uint32_t pdu_length_bytes(const uint8_t *pdu_length_bytes)
|
|
{
|
|
return pdu_length_bytes[0] | ((uint32_t)pdu_length_bytes[1] << 8)
|
|
| ((uint32_t)pdu_length_bytes[2] << 16);
|
|
}
|
|
|
|
/* ---------- Paket verarbeiten ---------- */
|
|
static uint32_t g_packet_count = 0;
|
|
|
|
void cits_process_packet(const uint8_t *data, uint16_t data_len,
|
|
int8_t rssi, uint32_t channel_mhz,
|
|
uint32_t timestamp_sec, uint32_t timestamp_usec)
|
|
{
|
|
if (!data || data_len < 30) {
|
|
// Nicht genug Daten für WSM Header
|
|
return;
|
|
}
|
|
|
|
g_packet_count++;
|
|
|
|
// WSM Header ab offset 12 (14 B MAC + 8 B LLC/SNAP = 22; WSM startet bei 12 im WSM-Offset-Modus)
|
|
// Wir lesen WSM Header direkt ab data[12]
|
|
if (data_len < 12 + sizeof(wave_header_t)) {
|
|
return;
|
|
}
|
|
|
|
wave_header_t wave;
|
|
memcpy(&wave, data + 12, sizeof(wave_header_t));
|
|
|
|
uint32_t pdu_len = pdu_length_bytes(wave.pdu_length);
|
|
char mac_str[18];
|
|
mac_to_str(wave.sender_id, mac_str, sizeof(mac_str));
|
|
|
|
// Prüfen ob C-ITS BSI Header im Payload ist
|
|
uint8_t msg_id = 0;
|
|
uint8_t msg_cnt = 0;
|
|
uint8_t sec_num = 0;
|
|
uint8_t part_id = 0;
|
|
bool has_bsi = false;
|
|
|
|
if (data_len >= 12 + sizeof(wave_header_t) + sizeof(cits_bsi_header_t)) {
|
|
const uint8_t *bsi_ptr = data + 12 + sizeof(wave_header_t);
|
|
msg_id = bsi_ptr[0];
|
|
msg_cnt = bsi_ptr[1];
|
|
sec_num = bsi_ptr[2];
|
|
part_id = bsi_ptr[3];
|
|
has_bsi = true;
|
|
}
|
|
|
|
// Log alle empfangenen Pakete
|
|
ESP_LOGI(TAG, "[PKT#%lu] len=%u rssi=%d ch=%u | WAVE PDU=%u MAC=%s pri=%u type=0x%02X",
|
|
(unsigned long)g_packet_count, data_len, rssi, channel_mhz,
|
|
pdu_len, mac_str, wave.priority, wave.pdu_type);
|
|
|
|
// Wenn C-ITS BSI gefunden → Detail-Log
|
|
if (has_bsi) {
|
|
char sta_str[24];
|
|
snprintf(sta_str, sizeof(sta_str), "%02X:%02X:%02X:%02X:%02X:%02X:%02X:%02X",
|
|
data[12 + sizeof(wave_header_t) + 4],
|
|
data[12 + sizeof(wave_header_t) + 5],
|
|
data[12 + sizeof(wave_header_t) + 6],
|
|
data[12 + sizeof(wave_header_t) + 7],
|
|
data[12 + sizeof(wave_header_t) + 8],
|
|
data[12 + sizeof(wave_header_t) + 9],
|
|
data[12 + sizeof(wave_header_t) + 10],
|
|
data[12 + sizeof(wave_header_t) + 11]);
|
|
|
|
ESP_LOGI(TAG, " >>> C-ITS %s (msg=0x%02X cnt=%u sec=%u part=%u STA=%s)",
|
|
msg_id_str(msg_id), msg_id, msg_cnt, sec_num, part_id, sta_str);
|
|
}
|
|
|
|
// In CSV schreiben
|
|
cits_message_t csv_msg;
|
|
csv_msg.timestamp_sec = timestamp_sec;
|
|
csv_msg.timestamp_usec = timestamp_usec;
|
|
csv_msg.length = data_len;
|
|
memcpy(csv_msg.sender_id, wave.sender_id, 6);
|
|
csv_msg.message_type = has_bsi ? msg_id : wave.pdu_type;
|
|
csv_msg.channel = channel_mhz;
|
|
csv_msg.rssi = rssi;
|
|
csv_store_cits_message(&csv_msg);
|
|
}
|
|
|
|
/* ---------- Init / Deinit ---------- */
|
|
esp_err_t cits_parser_init(void)
|
|
{
|
|
g_packet_count = 0;
|
|
ESP_LOGI(TAG, "C-ITS Parser initialisiert");
|
|
return ESP_OK;
|
|
}
|
|
|
|
void cits_parser_deinit(void)
|
|
{
|
|
ESP_LOGI(TAG, "C-ITS Parser beendet. Total Pakete: %lu",
|
|
(unsigned long)g_packet_count);
|
|
}
|