wisp/cits-sniffer
2
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 2 Wiki Activity

build: fix USB-JTAG API, PRI macros, linker symbols for ESP-IDF 5.5

Browse Source
This commit is contained in:
Clawdia
2026-05-20 06:36:35 +02:00
parent 76d50165c4
commit 5248ff7b82
15 changed files with 1976 additions and 2273 deletions
Download Patch File Download Diff File Expand all files Collapse all files
main/block2_cits_parser.c
+163 -167
View File
@@ -1,13 +1,27 @@
/**
* Block 2: C-ITS Parser für ESP32-C5 C-ITS Sniffer
*
* Ziel: C-ITS DATENSATZ parsen und extrahieren
*
* WAVE/DSRC Paket Struktur (IEEE 1609.3):
* - MAC Header: 14 Bytes
* - LLC/SNAP: 8 Bytes
* - WAVE Short Message (WSM): 6 Bytes Header
* - C-ITS DATENSATZ (Payload)
*
* 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>
@@ -15,8 +29,6 @@
#include <stdlib.h>
#include <stdint.h>
#include <stdbool.h>
#include <time.h>
#include <sys/time.h>
#include "sdkconfig.h"
#include "esp_err.h"
@@ -25,200 +37,184 @@
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "block3_csv_writer.h"
static const char *TAG = "CITS_PARSER";
// WAVE Short Message Header Struktur
/* ---------- WAVE Short Message Header ---------- */
typedef struct __attribute__((packed)) {
uint8_t pduLength[3]; // PDU Länge (3 Bytes)
uint8_t senderId[6]; // Sender MAC Adresse
uint32_t timestamp; // Zeitstempel
uint8_t priority; // Priorität
uint8_t pduType; // PDU Typ
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 Message Type (BSI Format)
/* ---------- C-ITS BSI Header (ETSI TS 103 097) ---------- */
typedef struct __attribute__((packed)) {
uint8_t msgId; // Message ID
uint8_t msgCnt; // Message Count
uint8_t sectionNum; // Section Number
uint8_t partId; // Part ID
uint8_t reserved[8]; // Reserviert
uint8_t payload[]; // Payload
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;
// C-ITS Message Types (ETSI TS 103 097)
#define CITS_MSGID_DECEVENT 0x01 // Dezisionsunterstützung
#define CITS_MSGID_DSECN 0x02 // Dekstruktive Umgebung
#define CITS_MSGID_DMM 0x03 // Dynamisches Map-Matching
#define CITS_MSGID_MAP 0x04 // Kartendaten
#define CITS_MSGID_MAPDATA 0x05 // Kartendaten
#define CITS_MSGID_MAPSELDATA 0x06 // Kartendaten Auswahl
#define CITS_MSGID_RSM 0x07 // Road Safety Message
#define CITS_MSGID_CAM 0x08 // Cooperative Awareness Message
#define CITS_MSGID_DENM 0x09 // Decentralized Environmental Notification
#define CITS_MSGID_SPA 0x0A // Signal Phase and Timing
#define CITS_MSGID_MAM 0x0B // Map Alignment Message
#define CITS_MSGID_VIT 0x0C // Vehicle Information Message
#define CITS_MSGID_VSL 0x0D // Variable Speed Limit
#define CITS_MSGID_SPA 0x0E // Signal Phase and Timing
#define CITS_MSGID_CSP 0x0F // Cross-Signal Phase
/* ---------- 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
/**
* MAC Adresse als String formatieren
*/
static void format_mac(const uint8_t *mac, char *mac_str, size_t size)
static const char *msg_id_str(uint8_t msg_id)
{
snprintf(mac_str, size, "%02X:%02X:%02X:%02X:%02X:%02X",
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]);
}
/**
* WAVE Header parsen
*/
static esp_err_t parse_wave_header(const uint8_t *packet, wave_header_t *wave_hdr)
/* ---------- PDU Length (24-bit little-endian) ---------- */
static uint32_t pdu_length_bytes(const uint8_t *pdu_length_bytes)
{
if (packet == NULL || wave_hdr == NULL) {
return ESP_ERR_INVALID_ARG;
}
memcpy(wave_hdr->pduLength, packet, 3);
memcpy(wave_hdr->senderId, packet + 3, 6);
memcpy(&wave_hdr->timestamp, packet + 9, 4);
wave_hdr->priority = packet[13];
wave_hdr->pduType = packet[14];
return ESP_OK;
return pdu_length_bytes[0] | ((uint32_t)pdu_length_bytes[1] << 8)
| ((uint32_t)pdu_length_bytes[2] << 16);
}
/**
* C-ITS BSI Header parsen
*/
static esp_err_t parse_cits_bsi(const uint8_t *packet, cits_bsi_header_t *bsi_hdr)
{
if (packet == NULL || bsi_hdr == NULL) {
return ESP_ERR_INVALID_ARG;
}
bsi_hdr->msgId = packet[0];
bsi_hdr->msgCnt = packet[1];
bsi_hdr->sectionNum = packet[2];
bsi_hdr->partId = packet[3];
memcpy(bsi_hdr->reserved, packet + 4, 8);
return ESP_OK;
}
/* ---------- Paket verarbeiten ---------- */
static uint32_t g_packet_count = 0;
/**
* Nachrichtstyp identifizieren
*/
static const char* get_message_type(uint8_t msgId)
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)
{
switch (msgId) {
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";
default: return "UNKNOWN";
}
}
/**
* C-ITS Nachricht verarbeiten
*/
static uint32_t packet_count = 0;
static void process_cits_packet(const uint8_t *packet, uint32_t length,
uint32_t timestamp_sec, uint32_t timestamp_usec)
{
if (packet == NULL || length < 20) {
if (!data || data_len < 30) {
// Nicht genug Daten für WSM Header
return;
}
wave_header_t wave_hdr;
esp_err_t ret = parse_wave_header(packet, &wave_hdr);
if (ret != ESP_OK) {
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];
format_mac(wave_hdr.senderId, mac_str, sizeof(mac_str));
packet_count++;
// Wichtige C-ITS Message Types sofort loggen
uint8_t msgId = 0;
if (length >= 23) {
msgId = packet[15]; // BSI Message ID
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;
}
const char* msg_type = get_message_type(msgId);
ESP_LOGI(TAG, "[PACKET #%lu] C-ITS %s Nachricht empfangen!",
(unsigned long)packet_count, msg_type);
ESP_LOGI(TAG, " MAC: %s | PDU: %lu B | Kanal: %u MHz",
mac_str,
(unsigned long)(wave_hdr.pduLength[0] | wave_hdr.pduLength[1] << 8 | wave_hdr.pduLength[2] << 16),
CITS_CHANNEL_0_MHZ);
ESP_LOGI(TAG, " Zeit: %u.%06u | Priorität: %d",
timestamp_sec, timestamp_usec, wave_hdr.priority);
if (length >= 23) {
cits_bsi_header_t bsi_hdr;
if (parse_cits_bsi(packet + 15, &bsi_hdr) == ESP_OK) {
ESP_LOGI(TAG, " BSI: MsgID=%d | Count=%d | Section=%d | Part=%d",
bsi_hdr.msgId, bsi_hdr.msgCnt, bsi_hdr.sectionNum, bsi_hdr.partId);
}
// 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);
}
// Speichere für CSV
// In CSV schreiben
cits_message_t csv_msg;
csv_msg.timestamp_sec = timestamp_sec;
csv_msg.timestamp_usec = timestamp_usec;
csv_msg.length = length;
memcpy(csv_msg.sender_id, wave_hdr.senderId, 6);
csv_msg.message_type = msgId;
csv_msg.channel = CITS_CHANNEL_0_MHZ;
csv_msg.rssi = 0; // Wird vom WiFi Layer bereitgestellt
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);
}
/**
* C-ITS Parser initialisieren
*/
/* ---------- Init / Deinit ---------- */
esp_err_t cits_parser_init(void)
{
g_packet_count = 0;
ESP_LOGI(TAG, "C-ITS Parser initialisiert");
return ESP_OK;
}
/**
* C-ITS Parser beenden
*/
void cits_parser_deinit(void)
{
ESP_LOGI(TAG, "C-ITS Parser beendet");
ESP_LOGI(TAG, "C-ITS Parser beendet. Total Pakete: %lu",
(unsigned long)g_packet_count);
}
/**
* Hauptverarbeitungsfunktion
* Diese wird von der sniffer_task aufgerufen
*/
void cits_process_packet(sniffer_packet_info_t *packet_info)
{
if (packet_info == NULL || packet_info->payload == NULL) {
return;
}
process_cits_packet(packet_info->payload, packet_info->length,
packet_info->seconds, packet_info->microseconds);
}