cpmv2

backup/src/CMakeLists.txt

@@ -0,0 +1,51 @@
+ADD_EXECUTABLE(it2s-itss-facilities
+    config.c
+    cam.c
+    denm.c
+    infrastructure.c
+    requests.c
+    indications.c
+    facilities.c
+    cpm.c
+    saem.c
+    tpm.c
+    vcm.c
+    evm.c
+    edm.c
+    mcm.c
+)
+
+TARGET_LINK_LIBRARIES(it2s-itss-facilities
+    -lit2s-asn-etsi-its-sdu-itss-facilities
+    -lit2s-asn-etsi-its-sdu-itss-security
+    -lit2s-asn-etsi-its-sdu-itss-management
+    -lit2s-asn-etsi-its-sdu-itss-networking
+    -lit2s-asn-etsi-its-sdu-cdd-1.3.1
+    -lzmq
+    -lit2s_gnss
+    -lpthread
+    -lit2s-config-etsi-its
+    -lit2s-asn-etsi-its-v1-cdd-1.3.1
+    -lit2s-asn-etsi-its-v1-cam
+    -lit2s-asn-etsi-its-v1-ivim
+    -lit2s-asn-etsi-its-v1-denm
+    -lit2s-asn-etsi-its-v1-cpm
+    -lit2s-asn-etsi-its-v1-saem
+    -lit2s-asn-etsi-its-v1-tpm
+    -lit2s-asn-etsi-its-v1-vcm
+    -lit2s-asn-etsi-its-v1-evcsnm
+    -lit2s-asn-etsi-its-v1-evrsrm
+    -lit2s-asn-etsi-its-v2-cdd-2.2.1
+    -lit2s-asn-etsi-its-v2-cam
+    -lit2s-asn-etsi-its-v2-denm
+    -lit2s-asn-etsi-its-v2-mcm
+    -lit2s-tender
+    -lm
+    -lrt
+)
+
+INSTALL(
+    TARGETS
+    it2s-itss-facilities
+    RUNTIME DESTINATION bin
+)

backup/src/config.c

@@ -0,0 +1,590 @@
+#include "config.h"
+#include "facilities.h"
+#include "saem.h"
+#include "tpm.h"
+#include "vcm.h"
+#include "mcm.h"
+
+#include <string.h>
+#include <stdlib.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <it2s-config/t2c-itss.h>
+#include <it2s-config/t2c-etsi-its.h>
+#include <dirent.h>
+#include <sys/time.h>
+#include <it2s-asn/etsi-its-sdu/cdd-1.3.1/EIS_ProtectedCommunicationZone.h>
+#include <it2s-asn/etsi-its-v1/tpm/EI1_TollingPaymentInfo.h>
+#include <zmq.h>
+
+#include <it2s-tender/packet.h>
+
+#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
+#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementReply.h>
+#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
+#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
+
+static int fetch_target_address(char** addresses, uint16_t addresses_len) {
+    int index = -1;
+
+    for (int i = 0; i < addresses_len; ++i) {
+        char* addr = addresses[i];
+
+        // If TCP, ignore receiver addresses, e.g. tcp://*:[port]
+        if (!memcmp(addr, "tcp", 3)) {
+            bool found_astk = false;
+            for (int j = 0; j < strlen(addr); ++j) {
+                if (addr[j] == '*') {
+                    found_astk = true;
+                    break;
+                }
+            }
+
+            if (found_astk) continue;
+        }
+
+        index = i;
+        break;
+    }
+
+    return index;
+}
+
+int facilities_config() {
+    int rv = 0;
+
+    t2c_itss_t* itss_cfg  = NULL;
+    t2c_etsi_its_t* etsi_its_cfg  = NULL;
+
+    rv = t2c_itss_read("/etc/it2s/itss.toml", &itss_cfg);
+    if (rv) {
+        syslog_error("[config] read ITS-S config failed");
+        goto cleanup;
+    }
+    rv = t2c_etsi_its_read("/etc/it2s/etsi-its.toml", &etsi_its_cfg);
+    if (rv) {
+        syslog_error("[config] read ETSI ITS config failed");
+        goto cleanup;
+    }
+
+    // Logging - status messages
+    bool use_syslog = false, use_std = false, use_file = false;
+    for (int i = 0; i < itss_cfg->general.logging.status_len; ++i) {
+        if (!strcmp(itss_cfg->general.logging.status[i], "syslog")) {
+            use_syslog = true;
+        } else if (!strcmp(itss_cfg->general.logging.status[i], "std")) {
+            use_std = true;
+        } else if (!strcmp(itss_cfg->general.logging.status[i], "file")) {
+            use_file = true;
+        }
+    }
+    log_init(use_syslog, use_std, use_file, "facilities", NULL);
+
+    itss_0init(&facilities.exit);
+
+    struct timeval time;
+    gettimeofday(&time,NULL);
+    srand((time.tv_sec * 1000) + (time.tv_usec / 1000));
+
+    facilities.zmq.responders = calloc(etsi_its_cfg->facilities.zmq.addresses_len + 1, sizeof(zmq_pollitem_t));
+    facilities.zmq.n_responders = 1;
+
+    // Internal socket
+    void* socket = itss_0bind(ZMQ_INTERNAL_ADDR, ZMQ_PAIR);
+    facilities.zmq.responders[0].socket = socket;
+    facilities.zmq.responders[0].events = ZMQ_POLLIN;
+
+    for (int i = 0; i < etsi_its_cfg->facilities.zmq.addresses_len; ++i) {
+        char* addr = etsi_its_cfg->facilities.zmq.addresses[i];
+
+        // IPC
+        if (!memcmp(addr, "ipc", 3)) {
+          
+            // Create dir 
+            int lp = 0;
+            for (int j = 0; j < strlen(addr); ++j) {
+                if (addr[j] == '/') lp = j;
+            }
+
+            char dir[256];
+            memcpy(dir, addr+6, lp-6);
+            dir[lp-6] = 0;
+
+            struct stat st = {0};
+            if (stat(dir, &st) == -1) {
+                mkdir(dir, 0777);
+            }
+
+            // Bind
+            void* socket = itss_0bind(addr, ZMQ_REP);
+
+            facilities.zmq.responders[facilities.zmq.n_responders].socket = socket;
+            facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN;
+            ++facilities.zmq.n_responders;
+
+        } else if (!memcmp(addr, "tcp", 3)) {
+            
+            bool found_astk = false; 
+            for (int j = 0; j < strlen(addr); ++j) {
+                if (addr[j] == '*') {
+                    found_astk = true; 
+                    break;
+                }
+            }
+
+            if (found_astk) {
+                // Bind
+                void* socket = itss_0bind(addr, ZMQ_REP);
+
+                facilities.zmq.responders[facilities.zmq.n_responders].socket = socket;
+                facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN;
+                ++facilities.zmq.n_responders;
+            }
+        }
+    }
+    if (facilities.zmq.n_responders <= 1) {
+        log_info("[config] a valid address to listen to was not found, exiting now");
+        rv = 1;
+        goto cleanup;
+    }
+
+    // Fetch [networking] address
+    int index = fetch_target_address(etsi_its_cfg->networking.zmq.addresses, etsi_its_cfg->networking.zmq.addresses_len);
+    if (index != -1) {
+        facilities.zmq.networking_address = malloc(strlen(etsi_its_cfg->networking.zmq.addresses[index])+1);
+        strcpy(facilities.zmq.networking_address, etsi_its_cfg->networking.zmq.addresses[index]);
+    } else {
+        log_error("[config] a valid address for [networking] was not found");
+        rv = 1;
+        goto cleanup;
+    }
+
+
+    // Fetch [applications] address
+    index = fetch_target_address(itss_cfg->applications.zmq.addresses, itss_cfg->applications.zmq.addresses_len);
+    if (index != -1) {
+        facilities.zmq.applications_address = malloc(strlen(itss_cfg->applications.zmq.addresses[index])+1);
+        strcpy(facilities.zmq.applications_address, itss_cfg->applications.zmq.addresses[index]);
+    } else {
+        log_error("[config] a valid address for [applications] was not found");
+        rv = 1;
+        goto cleanup;
+    }
+
+    // Fetch [security] address
+    index = fetch_target_address(itss_cfg->security.zmq.addresses, itss_cfg->security.zmq.addresses_len);
+    if (index != -1) {
+        facilities.zmq.security_address = malloc(strlen(itss_cfg->security.zmq.addresses[index])+1);
+        strcpy(facilities.zmq.security_address, itss_cfg->security.zmq.addresses[index]);
+    } else {
+        log_error("[config] a valid address for [security] was not found");
+        rv = 1;
+        goto cleanup;
+    }
+
+    // Fetch [management] address
+    index = fetch_target_address(etsi_its_cfg->management.zmq.addresses, etsi_its_cfg->management.zmq.addresses_len);
+    if (index != -1) {
+        facilities.zmq.management_address = malloc(strlen(etsi_its_cfg->management.zmq.addresses[index])+1);
+        strcpy(facilities.zmq.management_address, etsi_its_cfg->management.zmq.addresses[index]);
+    } else {
+        log_error("[config] a valid address for [management] was not found");
+        rv = 1;
+        goto cleanup;
+    }
+
+    // Values
+    // General
+    if (!strcmp("obu", itss_cfg->general.itss_type)) {
+        facilities.station_type = 5;
+    } else if (!strcmp("rsu", itss_cfg->general.itss_type)) {
+        facilities.station_type = 15;
+    } else if (!strcmp("unknown", itss_cfg->general.itss_type)) {
+        facilities.station_type = 0;
+    } else if (!strcmp("pedestrian", itss_cfg->general.itss_type)) {
+        facilities.station_type = 1;
+    } else if (!strcmp("cyclist", itss_cfg->general.itss_type)) {
+        facilities.station_type = 2;
+    } else if (!strcmp("moped", itss_cfg->general.itss_type)) {
+        facilities.station_type = 3;
+    } else if (!strcmp("motorcycle", itss_cfg->general.itss_type)) {
+        facilities.station_type = 4;
+    } else if (!strcmp("passengerCar", itss_cfg->general.itss_type)) {
+        facilities.station_type = 5;
+    } else if (!strcmp("bus", itss_cfg->general.itss_type)) {
+        facilities.station_type = 6;
+    } else if (!strcmp("lightTruck", itss_cfg->general.itss_type)) {
+        facilities.station_type = 7;
+    } else if (!strcmp("heavyTruck", itss_cfg->general.itss_type)) {
+        facilities.station_type = 8;
+    } else if (!strcmp("trailer", itss_cfg->general.itss_type)) {
+        facilities.station_type = 9;
+    } else if (!strcmp("specialVehicles", itss_cfg->general.itss_type)) {
+        facilities.station_type = 10;
+    } else if (!strcmp("tram", itss_cfg->general.itss_type)) {
+        facilities.station_type = 11;
+    } else if (!strcmp("roadSideUnit", itss_cfg->general.itss_type)) {
+        facilities.station_type = 15;
+    } else {
+        log_error("[config] unrecognized ITS-S type, running as OBU");
+        facilities.station_type = 5;
+    }
+
+
+    facilities.use_security = itss_cfg->security.use_security;
+
+    pthread_mutex_init(&facilities.id.lock, NULL);
+    pthread_mutex_init(&facilities.id.change.lock, NULL);
+
+    facilities.id.change.random = itss_cfg->security.identity.random;
+    if (facilities.id.change.random) {
+        // Ask [security] for station id
+        
+        EIS_SecurityRequest_t* sREQ = calloc(1, sizeof(EIS_SecurityRequest_t));
+        EIS_SecurityReply_t* sREP = NULL;
+
+        sREQ->present = EIS_SecurityRequest_PR_ids;
+        sREQ->choice.ids.list.count = 2;
+        sREQ->choice.ids.list.size = 2*sizeof(void*);
+        sREQ->choice.ids.list.array = malloc(2*sizeof(void*));
+        sREQ->choice.ids.list.array[0] = calloc(1, sizeof(EIS_SecurityIdType_t));
+        *sREQ->choice.ids.list.array[0] = EIS_SecurityIdType_stationId;
+        sREQ->choice.ids.list.array[1] = calloc(1, sizeof(EIS_SecurityIdType_t));
+        *sREQ->choice.ids.list.array[1] = EIS_SecurityIdType_ipv6Address;
+
+        uint8_t b_tx[256], b_rx[256];
+        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityRequest, NULL, sREQ, b_tx, 256);
+
+        void* ss = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);
+        itss_0send(ss, b_tx, enc.encoded);
+        int rl = itss_0recv_rt(&ss, b_rx, 256, b_tx, 256, -1);
+        itss_0close(ss);
+        if (rl == -1) {
+            rv = 1;
+            goto cleanup;
+        }
+
+        asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_SecurityReply, (void**) &sREP, b_rx, 256);
+
+        if (sREP->returnCode == EIS_SecurityReplyReturnCode_rejected) {
+            // TODO handle it
+            goto cleanup;
+        }
+
+        for (int i = 0; i < sREP->data->choice.ids.list.count; ++i) {
+            switch (sREP->data->choice.ids.list.array[i]->present) {
+                case EIS_SecurityId_PR_stationId:
+                    facilities.id.station_id = sREP->data->choice.ids.list.array[i]->choice.stationId;
+                    break;
+                case EIS_SecurityId_PR_ipv6Address:
+                    memcpy(facilities.id.ipv6_addr, sREP->data->choice.ids.list.array[i]->choice.ipv6Address.buf, 16);
+                    break;
+                default:
+                    break;
+            }
+        }
+        
+        ASN_STRUCT_FREE(asn_DEF_EIS_SecurityRequest, sREQ);
+        ASN_STRUCT_FREE(asn_DEF_EIS_SecurityReply, sREP);
+
+    } else {
+        facilities.id.station_id = itss_cfg->security.identity.station_id;
+
+        uint8_t src_mac[6];
+        unsigned int tmp_uint[6];
+
+        sscanf(itss_cfg->security.identity.mac_address, "%02x:%02x:%02x:%02x:%02x:%02x",
+                &tmp_uint[0],
+                &tmp_uint[1],
+                &tmp_uint[2],
+                &tmp_uint[3],
+                &tmp_uint[4],
+                &tmp_uint[5]);
+
+        src_mac[0] = (uint8_t)tmp_uint[0];
+        src_mac[1] = (uint8_t)tmp_uint[1];
+        src_mac[2] = (uint8_t)tmp_uint[2];
+        src_mac[3] = (uint8_t)tmp_uint[3];
+        src_mac[4] = (uint8_t)tmp_uint[4];
+        src_mac[5] = (uint8_t)tmp_uint[5];
+
+        memset(facilities.id.ipv6_addr, 0, 8);
+        memcpy(facilities.id.ipv6_addr+8, src_mac, 3);
+        facilities.id.ipv6_addr[11] = 0xff;
+        facilities.id.ipv6_addr[12] = 0xfe;
+        memcpy(facilities.id.ipv6_addr+13, src_mac+3, 3);
+        facilities.id.ipv6_addr[8] ^= 0x02;
+    }
+
+    // Message version
+    if (!strcmp("v1", etsi_its_cfg->facilities.mver.default_version)) {
+        log_error("[config] only default message version 2 is supported");
+        return -1;
+    } else if (!strcmp("v2", etsi_its_cfg->facilities.mver.default_version)) {
+        facilities.pver.defaultv = 2;
+    } else {
+        log_warn("[config] unrecognized default messages version :: using version 2");
+        facilities.pver.defaultv = 2;
+    }
+
+    // DENM
+    facilities.den.n_max_events = etsi_its_cfg->facilities.denm.nmax_active_events;
+
+    // CAM
+    facilities.lightship.active = etsi_its_cfg->facilities.cam.activate; 
+    facilities.lightship.vehicle_gen_min = etsi_its_cfg->facilities.cam.obu_period_min;
+    facilities.lightship.vehicle_gen_max = etsi_its_cfg->facilities.cam.obu_period_max;
+    facilities.lightship.rsu_gen_min = etsi_its_cfg->facilities.cam.rsu_period_min;
+    facilities.lightship.rsu_vehicle_permanence = etsi_its_cfg->facilities.cam.rsu_vehicle_permanence;
+
+    // IVIM
+    facilities.infrastructure.n_max_services = etsi_its_cfg->facilities.ivim.nmax_active_services;
+    facilities.infrastructure.replay_interval = etsi_its_cfg->facilities.ivim.replay_interval;
+    facilities.infrastructure.default_service_duration = etsi_its_cfg->facilities.ivim.default_service_duration * 60000;
+
+    // CPM
+    facilities.dissemination.active = etsi_its_cfg->facilities.cpm.activate;
+    facilities.dissemination.T_GenCpmMin = etsi_its_cfg->facilities.cpm.rsu_obu_period_min;
+    facilities.dissemination.T_GenCpmMax = etsi_its_cfg->facilities.cpm.rsu_obu_period_max;
+    // Rotation not needed anymore?
+    //facilities.dissemination.radar_rotation = itss_cfg->radar.radar_rotation;
+    facilities.dissemination.tmc_connect = etsi_its_cfg->facilities.cpm.tmc_connected;
+    facilities.dissemination.T_AddSensorInformation = 1000;
+    int oa_start = (360-facilities.dissemination.radar_rotation) * 10 - 500;
+    int oa_end = (360-facilities.dissemination.radar_rotation) * 10 + 500;
+    facilities.dissemination.opening_angle_start = (oa_start + 3600) % 3600;
+    facilities.dissemination.opening_angle_end = (oa_end + 3600) % 3600;
+
+    facilities.dissemination.int_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_interface)+1);
+    strcpy(facilities.dissemination.int_radar,etsi_its_cfg->facilities.cpm.radar_interface);
+    facilities.dissemination.ip_radar  = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_ip)+1);
+    strcpy(facilities.dissemination.ip_radar,etsi_its_cfg->facilities.cpm.radar_ip);
+
+    // TPM
+    facilities.tolling.enabled = etsi_its_cfg->facilities.tpm.activate;
+    if (!strcmp("gn-spki", etsi_its_cfg->facilities.tpm.protocol)) {
+        facilities.tolling.protocol.p = TOLLING_PROTOCOL_GN_SPKI;
+    } else if (!strcmp("gn-dpki", etsi_its_cfg->facilities.tpm.protocol)) {
+        facilities.tolling.protocol.p = TOLLING_PROTOCOL_GN_DPKI;
+    } else if (!strcmp("tls", etsi_its_cfg->facilities.tpm.protocol)) {
+        facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS;
+    } else if (!strcmp("tls-gn", etsi_its_cfg->facilities.tpm.protocol)) {
+        facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_GN;
+    } else if (!strcmp("tls-shs", etsi_its_cfg->facilities.tpm.protocol)) {
+        facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_SHS;
+    } else {
+        log_error("[config] unrecognized tolling protocol, defaulting to 'gn-spki'");
+        facilities.tolling.protocol.p = TOLLING_PROTOCOL_GN_SPKI;
+    }
+    facilities.tolling.station.obu.client_id = etsi_its_cfg->facilities.tpm.client_id;
+
+    // MCM
+    if (!strcmp("mcm", etsi_its_cfg->facilities.mcm.protocol)){
+        facilities.mcm_coord.protocol = MC_PROTOCOL_MCM; 
+        facilities.mcm_coord.active = etsi_its_cfg->facilities.mcm.activate;
+        facilities.coordination.active = false;
+    }
+    else if (!strcmp("vcm-RR", etsi_its_cfg->facilities.mcm.protocol)){
+        facilities.coordination.protocol = MC_PROTOCOL_VCM_RR;
+        facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate;
+        facilities.mcm_coord.active = false;
+    }
+    else if (!strcmp("vcm-RR1C", etsi_its_cfg->facilities.mcm.protocol)){
+        facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C;
+        facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate;
+        facilities.mcm_coord.active = false;
+    }
+    else if (!strcmp("vcm-RRAC", etsi_its_cfg->facilities.mcm.protocol)){
+        facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC;
+        facilities.coordination.active = etsi_its_cfg->facilities.mcm.activate;
+        facilities.mcm_coord.active = false;
+    }
+    else {
+        log_error("[config] unrecognized mcm protocol");
+        facilities.coordination.active = false;
+        facilities.mcm_coord.active = false;
+    }
+
+    // EVCSNM
+    facilities.evm_args.activate = etsi_its_cfg->facilities.evcsnm.activate;
+
+    // Replay
+    facilities.replay = etsi_its_cfg->networking.replay.activate;
+
+    // Forward unknown packets (in-stack)
+    facilities.upf = etsi_its_cfg->general.unknown_packet_forwarding;
+
+    // PZ
+    if (facilities.station_type == 15) {
+
+        int i = 0;
+
+        DIR *d = opendir(etsi_its_cfg->facilities.protected_zones.path);
+        struct dirent *dir;
+        char file[256];
+        char pz_xml[2048];
+        if (d) {
+            while ((dir = readdir(d)) != NULL && i < 16) {
+                if (dir->d_name[0] == '.') continue;
+                sprintf(file, "%s/%s", etsi_its_cfg->facilities.protected_zones.path, dir->d_name);
+                FILE *fp = fopen(file, "r");
+                if (!fp) continue;
+                fseek(fp, 0, SEEK_END);
+                uint16_t size = ftell(fp);
+                fseek(fp, 0, SEEK_SET);
+                if (!size) {
+                    fclose(fp);
+                    continue;
+                }
+                if (!fread(pz_xml, 1, size, fp)) {
+                    fclose(fp);
+                    continue;
+                }
+                fclose(fp);
+               
+                EIS_ProtectedCommunicationZone_t *zone = calloc(1, sizeof(EIS_ProtectedCommunicationZone_t));
+
+                asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_EIS_ProtectedCommunicationZone, (void**) &zone, pz_xml, size);
+                if (!dec.code) {
+                    facilities.lightship.protected_zones.pz[i] = (EI1_ProtectedCommunicationZone_t*)zone;
+                    ++facilities.lightship.protected_zones.pz_len;
+                    ++i;
+                    log_debug("[config] loaded protection zone @ (%lld, %lld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude);
+                } else {
+                    log_error("[config] failure to decode protection zone '%s'", dir->d_name);
+                    ASN_STRUCT_FREE(asn_DEF_EIS_ProtectedCommunicationZone, zone);
+                }
+
+            }
+            closedir(d);
+        }
+    }
+
+    // TZ
+    if (facilities.station_type == 15) {
+
+        int i = 0;
+
+        DIR *d = opendir(etsi_its_cfg->facilities.tpm.tis_path);
+        struct dirent *dir;
+        char file[256];
+        char ti_xml[2048];
+        if (d) {
+            while ((dir = readdir(d)) != NULL && i < TOLLING_INFOS_MAX_LENGTH) {
+                if (dir->d_name[0] == '.') continue;
+                sprintf(file, "%s/%s", etsi_its_cfg->facilities.tpm.tis_path, dir->d_name);
+                FILE *fp = fopen(file, "r");
+                if (!fp) continue;
+                fseek(fp, 0, SEEK_END);
+                uint16_t size = ftell(fp);
+                fseek(fp, 0, SEEK_SET);
+                if (!size) {
+                    fclose(fp);
+                    continue;
+                }
+                if (!fread(ti_xml, 1, size, fp)) {
+                    fclose(fp);
+                    continue;
+                }
+                fclose(fp);
+               
+                EI1_TollingPaymentInfo_t *ti = calloc(1, sizeof(EI1_TollingPaymentInfo_t));
+
+                asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_EI1_TollingPaymentInfo, (void**) &ti, ti_xml, size);
+                if (!dec.code) {
+                    facilities.tolling.infos.z[i] = calloc(1, sizeof(tolling_info_t));
+                    facilities.tolling.infos.z[i]->asn = ti;
+                    ++facilities.tolling.infos.length;
+                    ++i;
+                    log_debug("[config] loaded tolling info | id:%lld type:%s", 
+                            ti->id,
+                            ti->tollType==EI1_TollType_entry ? "entry": ti->tollType==EI1_TollType_exit ? "exit": "single"
+                            );
+                } else {
+                    log_error("[config] failure to decode tolling info '%s'", dir->d_name);
+                    ASN_STRUCT_FREE(asn_DEF_EI1_TollingPaymentInfo, ti);
+                }
+
+            }
+            closedir(d);
+        }
+    }
+
+
+    facilities.edm.enabled = itss_cfg->applications.extensions.enabled;
+
+    EIS_ManagementRequest_t* mreq = calloc(1, sizeof(EIS_ManagementRequest_t));
+    mreq->present = EIS_ManagementRequest_PR_attributes;
+    mreq->choice.attributes.present = EIS_ManagementRequestAttributes_PR_get;
+    mreq->choice.attributes.choice.get.clockType = 1;
+    mreq->choice.attributes.choice.get.clockOffset = 1;
+    void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
+    uint8_t b_tx[256], b_rx[256];
+    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_ManagementRequest, NULL, mreq, b_tx, 256);
+
+    itss_0send(management_socket, b_tx, enc.encoded);
+    int rl = itss_0recv_rt(&management_socket, b_rx, 256, b_tx, 256, -1);
+    itss_0close(management_socket);
+    if (rl == -1) {
+        rv = 1;
+        goto cleanup;
+    }
+
+    EIS_ManagementReply_t* mrep = calloc(1, sizeof(EIS_ManagementReply_t));
+    oer_decode(NULL, &asn_DEF_EIS_ManagementReply, (void**) &mrep, b_rx, 256);
+    long lat, lon, alt, alt_conf;
+    if (mrep->returnCode == EIS_ManagementReplyReturnCode_accepted &&
+            mrep->data &&
+            mrep->data->choice.attributes.clockType &&
+            mrep->data->choice.attributes.clockOffset) {
+
+        itss_epv_init(*mrep->data->choice.attributes.clockType, TIME_MILLISECONDS, itss_cfg->time.simulated.source);
+
+        asn_INTEGER2uint64(mrep->data->choice.attributes.clockOffset, &epv.time.offset);
+    } else {
+        log_error("rejected MR attribute request");
+        rv = 1;
+        goto cleanup;
+    }
+    ASN_STRUCT_FREE(asn_DEF_EIS_ManagementRequest, mreq);
+    ASN_STRUCT_FREE(asn_DEF_EIS_ManagementReply, mrep);
+
+
+    if (etsi_its_cfg->facilities.saem.activate) { // TODO handle various services
+        facilities.bulletin.to_provide_len = 1;
+        facilities.bulletin.to_provide[0] = calloc(1, sizeof(announcement_t));
+        facilities.bulletin.to_provide[0]->endpoint.port = 7010 + etsi_its_cfg->facilities.saem.service_to_advertise;
+        facilities.bulletin.to_provide[0]->its_aid = etsi_its_cfg->facilities.saem.service_to_advertise;
+    }
+
+    facilities.vehicle.length = itss_cfg->vehicle.length;
+    facilities.vehicle.width = itss_cfg->vehicle.width;
+    facilities.vehicle.role = itss_cfg->vehicle.role;
+
+    // Logging
+    facilities.logging.recorder = etsi_its_cfg->facilities.logging.management;
+    if (itss_cfg->general.logging.enabled && etsi_its_cfg->facilities.logging.dbms) {
+        facilities.logging.dbms = calloc(1, sizeof(itss_db_t));
+        if (itss_db_init(
+                    facilities.logging.dbms, 
+                    itss_cfg->general.logging.database,
+                    itss_cfg->general.logging.table_style, 
+                    ITSS_FACILITIES,
+                    itss_cfg->general.logging.host,
+                    itss_cfg->general.logging.port,
+                    itss_cfg->general.logging.username,
+                    itss_cfg->general.logging.password
+                    )) {
+            log_error("failed to initialize the database -> turning off logging");
+            free(facilities.logging.dbms);
+            facilities.logging.dbms = NULL;
+        }
+    }
+
+cleanup:
+    t2c_itss_free(itss_cfg);
+    t2c_etsi_its_free(etsi_its_cfg);
+
+    return rv;
+}
+

backup/src/cpm.c

@@ -0,0 +1,832 @@
+#include "cpm.h"
+#include "facilities.h"
+
+#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
+#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
+#include <math.h>
+#include <signal.h>
+#include <unistd.h>
+#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
+
+#include <arpa/inet.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <signal.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+
+#include <it2s-tender/time.h>
+#include <it2s-tender/space.h>
+#include <it2s-tender/recorder.h>
+#include <it2s-tender/packet.h>
+
+#define PI 3.141592654
+
+/* Variables */
+
+float roadRotationSin;
+float roadRotationCos;
+
+S_ETHERNET_CONNECTION_T s_socket;
+S_INTERFACE_CONNECTION_T raw_socket;
+S_OBJECT_CONTROL_T s_objectControl;
+S_OBJECTS_T as_objects[NOF_OBJECTS];
+
+int radar_ready(){
+    // Create temporary ifr struct and socket to 
+    // check if the radar interface is running i.e if the
+    // radar has booted up
+
+    struct ifreq ifr;
+    memset(&ifr, 0, sizeof(ifr));
+
+    int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
+
+    strncpy(ifr.ifr_name, facilities.dissemination.int_radar, sizeof(ifr.ifr_name));
+    if(ioctl(sock, SIOCGIFFLAGS, &ifr) <0)
+        log_error(" IOCTL failed, could not retrieve radar interface flags");
+
+    close(sock);
+
+    return (ifr.ifr_flags & IFF_UP) && (ifr.ifr_flags & IFF_RUNNING);
+}
+
+
+
+bool radar_connection(char* radar_port){
+    if(radar_ready() == 1){
+        if(facilities.dissemination.tmc_connect == false){
+
+            // Create TCP socket
+            s_socket.i32_socket = socket(AF_INET, SOCK_STREAM, 0);
+
+            if(s_socket.i32_socket < 0){
+                log_error("Initializing socket failed ...");
+                return false;
+            }
+
+            // Bind it to server address and port
+            bzero(&s_socket.s_server, sizeof(s_socket.s_server));
+            s_socket.s_server.sin_family = AF_INET;
+            s_socket.s_server.sin_addr.s_addr = inet_addr(facilities.dissemination.ip_radar);
+            s_socket.s_server.sin_port = htons(atoi(radar_port));
+
+            if(bind(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server,sizeof(s_socket.s_server)) < 0){
+                log_error("Binding socket to address error ...");
+                return false;
+            }
+
+            // Listening to the socket (Waiting for incoming connection)
+            unsigned int len = sizeof(s_socket.s_client);
+
+            if(listen(s_socket.i32_socket,1)<0){
+                log_error("Waiting for incoming requests failed...");
+                return false;
+            }
+
+            if((s_socket.i32_client = accept(s_socket.i32_socket, (struct sockaddr*)&s_socket.s_server, &len)) < 0){
+                log_error("Client disconnected...");
+                return false;
+            }
+
+            log_debug("Radar connected");
+
+        }
+
+        // Create RAW socket
+
+        raw_socket.raw_fd = socket(AF_PACKET, SOCK_RAW, htons(0x0800));
+
+        if(raw_socket.raw_fd < 0){
+            log_error("Failed to initializing RAW socket ...");
+            return false;
+        }
+
+        // Get interface index
+
+        bzero(&raw_socket.sll, sizeof(raw_socket.sll));
+        bzero(&raw_socket.ifr, sizeof(raw_socket.ifr));
+
+        strncpy((char *)raw_socket.ifr.ifr_name, facilities.dissemination.int_radar, IFNAMSIZ);
+        if((ioctl(raw_socket.raw_fd, SIOCGIFINDEX, &raw_socket.ifr)) == -1){
+            log_error("Error getting interface index");
+            return false;
+        }
+
+        raw_socket.sll.sll_family = AF_PACKET;
+        raw_socket.sll.sll_ifindex = raw_socket.ifr.ifr_ifindex;
+        raw_socket.sll.sll_protocol = htons(0x0800);
+
+        // Bind it to the interface
+
+        if(bind(raw_socket.raw_fd, (struct sockaddr *)&raw_socket.sll, sizeof(raw_socket.sll))<0){
+            log_error("Error binding RAW socket ...");
+            return false;
+        }
+
+
+        return true;
+    }else{
+        return false;
+    }
+
+}
+
+dissemination_t* dissemination_init(){
+    /* Mutex init and dissemination memory allocation */
+    dissemination_t* dissemination = (dissemination_t*) calloc(1, sizeof(dissemination_t));
+    pthread_mutex_init(&dissemination->lock, NULL);
+
+    return dissemination;
+}
+
+
+int dissemination_check(int f) {
+    int rv = 0;
+
+    dissemination_t* dissemination = &facilities.dissemination;
+
+    uint64_t now = itss_time_get(); 
+
+    pthread_mutex_lock(&dissemination->lock);  // mutex is used to lock shared resources
+
+    /* If f = 0 indicates that it is to check the Sensor Information Container timer
+     * If f = 1 inidcates that it is to check the CPM generation */
+
+    if(f == 0){
+        if(now >= dissemination->next_AddSensorInformation){
+            rv = 1;
+        }else{
+            rv = 0;
+        }
+    }else{
+        if (now >= dissemination->next_cpm_min){
+            rv = 1; 
+        }else if(now >= dissemination->next_cpm_max){
+            rv = 0;
+        }   
+    }
+
+    pthread_mutex_unlock(&dissemination->lock);
+
+    return rv;
+}
+
+
+void dissemination_reset_timer(int f){
+
+    dissemination_t* dissemination = &facilities.dissemination;
+    uint64_t now = itss_time_get(); 
+
+    /* Both cases for RSU and OBU */
+
+    /* If f = 0 indicates that the reset corresponds to the timer of the Sensor Information Container Inclusion	
+       If f = 1 indicates that the reset corresponds to the timer of the CPM generation */
+
+    pthread_mutex_lock(&dissemination->lock);
+
+    if(f == 0){
+        dissemination->next_AddSensorInformation = now + dissemination->T_AddSensorInformation;
+    }else{
+        dissemination->next_cpm_min = now + dissemination->T_GenCpmMin;
+        dissemination->next_cpm_max = now + dissemination->T_GenCpmMax;	
+    }
+
+    pthread_mutex_unlock(&dissemination->lock);
+}
+
+
+void parse_can_data_tm(u_int32_t u32_can_id, int i32_can_len, u_int8_t* au8_can_data) {
+    u_int8_t u8_pvrMessagePart = 0;
+    u_int8_t tmp = 0;
+    //static long last = 0;
+    S_PVR_T s_pvr;
+    S_SENSOR_CONTROL_T s_sensorControl;
+
+    switch (u32_can_id)  // Interpret the different types of CAN messages
+    {
+        case 0x785:
+            s_pvr.u8_numberOfCountedObjects = 0;
+            s_pvr.u32_UnixTime = 0;
+            s_pvr.u16_Milliseconds = 0;
+            s_pvr.u8_SensorNetworkID = 0;
+
+            u8_pvrMessagePart = (au8_can_data[0] & 0x1);
+            if (u8_pvrMessagePart == 0) {
+                s_pvr.u8_numberOfCountedObjects = (au8_can_data[0] & 0xFE) >> 1;
+                s_pvr.u32_UnixTime |= au8_can_data[4] << 24;
+                s_pvr.u32_UnixTime |= au8_can_data[3] << 16;
+                s_pvr.u32_UnixTime |= au8_can_data[2] << 8;
+                s_pvr.u32_UnixTime |= au8_can_data[1];
+
+                s_pvr.u16_Milliseconds |= (au8_can_data[6] & 0x3) << 8;
+                s_pvr.u16_Milliseconds |= au8_can_data[5];
+
+                s_pvr.u8_SensorNetworkID = (au8_can_data[6] & 0xC) >> 2;
+
+                // printf("Unix Time: %u, Ms: %u\n", s_pvr.u32_UnixTime, s_pvr.u16_Milliseconds);
+
+                long current = s_pvr.u32_UnixTime * 1000 + s_pvr.u16_Milliseconds;
+
+                //printf("Unix Time Epoch Fixed: %ld\n", (long)(current - 1072915200000));
+                //printf("Diff: %ld\n", current - last);
+                //last = current;
+            } else if (u8_pvrMessagePart == 1) {
+                s_pvr.u8_ObjectNumber = 0;
+                s_pvr.u8_ObjectID = 0;
+                s_pvr.i16_speed = 0;
+                s_pvr.u8_class = 0;
+                s_pvr.u8_mLineNumber = 0;
+                s_pvr.u8_laneNumber = 0;
+
+                s_pvr.u8_ObjectNumber = (au8_can_data[0] & 0xFE) >> 1;
+
+                s_pvr.u8_ObjectID = au8_can_data[1];
+
+                s_pvr.i16_speed |= (au8_can_data[3] & 0x7) << 8;
+                s_pvr.i16_speed |= au8_can_data[2];
+                s_pvr.i16_speed -= 1024;  // Speed Offset
+
+                s_pvr.u8_class = (au8_can_data[3] & 0x38) >> 3;
+
+                s_pvr.u8_mLineNumber = (au8_can_data[3] & 0xC0) >> 6;
+
+                s_pvr.u8_laneNumber = (au8_can_data[4] & 0x7) >> 3;
+            }
+            break;
+        case 0x500:
+            memset(&s_sensorControl, 0, sizeof(s_sensorControl));
+            s_sensorControl.u8_sensorStatus = au8_can_data[0];
+            s_sensorControl.u8_InterfaceMode = (au8_can_data[1] & 0xF);
+            s_sensorControl.u8_networkID = (au8_can_data[1] & 0xF0) >> 4;
+            s_sensorControl.u8_diagnose = au8_can_data[2];
+            s_sensorControl.u32_time |= au8_can_data[7] << 24;
+            s_sensorControl.u32_time |= au8_can_data[6] << 16;
+            s_sensorControl.u32_time |= au8_can_data[5] << 8;
+            s_sensorControl.u32_time |= au8_can_data[4];
+            break;
+        case 0x501:
+            memset(&s_objectControl, 0, sizeof(s_objectControl));
+            s_objectControl.u8_numberOfObjects = au8_can_data[0];
+            s_objectControl.u8_numberOfMessages = au8_can_data[1];
+            s_objectControl.u8_cycleDuration = au8_can_data[2];
+            s_objectControl.u8_objectData0Format = au8_can_data[3] & 0xF;
+            s_objectControl.u8_objectData1Format = (au8_can_data[3] & 0xF0) >> 4;
+            s_objectControl.u32_cycleCount |= au8_can_data[7] << 24;
+            s_objectControl.u32_cycleCount |= au8_can_data[6] << 16;
+            s_objectControl.u32_cycleCount |= au8_can_data[5] << 8;
+            s_objectControl.u32_cycleCount |= au8_can_data[4];
+            break;
+    }
+
+    if ((u32_can_id >= 0x502) && (u32_can_id <= 0x57F)) {
+        u_int16_t u16_objectIndex = (u_int16_t)u32_can_id - 0x502;
+        as_objects[u16_objectIndex].u8_modeSignal = au8_can_data[0] & 0x1;
+        switch (s_objectControl.u8_objectData0Format) {
+            /* data without updated flag */
+            case 3: 
+            case 5: /* with 7 classes */
+                as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
+                as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
+                as_objects[u16_objectIndex].f_xPoint -= 4096;
+                as_objects[u16_objectIndex].f_xPoint *= 0.128;
+
+                as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
+                as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
+                as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
+                as_objects[u16_objectIndex].f_yPoint -= 4096;
+                as_objects[u16_objectIndex].f_yPoint *= 0.128;
+
+                as_objects[u16_objectIndex].f_xSpeed = ((au8_can_data[4] & 0x3F) << 5);
+                as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
+                as_objects[u16_objectIndex].f_xSpeed -= 1024;
+                as_objects[u16_objectIndex].f_xSpeed *= 0.1;
+
+                as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
+                as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
+                as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
+                as_objects[u16_objectIndex].f_ySpeed -= 1024;
+                as_objects[u16_objectIndex].f_ySpeed *= 0.1;
+
+                as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2;
+
+                as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
+                break;
+
+            /* data with updated flag */
+            case 4:
+            case 6: /* with 7 classes */
+                as_objects[u16_objectIndex].f_xPoint = (au8_can_data[1] & 0x3F) << 7;
+                as_objects[u16_objectIndex].f_xPoint += au8_can_data[0] >> 1;
+                as_objects[u16_objectIndex].f_xPoint -= 4096;
+                as_objects[u16_objectIndex].f_xPoint *= 0.128;
+
+                as_objects[u16_objectIndex].f_yPoint = (au8_can_data[3] & 0x7) << 10;
+                as_objects[u16_objectIndex].f_yPoint += au8_can_data[2] << 2;
+                as_objects[u16_objectIndex].f_yPoint += au8_can_data[1] >> 6;
+                as_objects[u16_objectIndex].f_yPoint -= 4096;
+                as_objects[u16_objectIndex].f_yPoint *= 0.128;
+
+                as_objects[u16_objectIndex].f_xSpeed = (au8_can_data[4] << 5);
+                as_objects[u16_objectIndex].f_xSpeed += (au8_can_data[3] >> 3);
+                as_objects[u16_objectIndex].f_xSpeed -= 1024;
+                as_objects[u16_objectIndex].f_xSpeed *= 0.1;
+
+                as_objects[u16_objectIndex].f_ySpeed = (au8_can_data[6] & 0x1) << 10;
+                as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[5] << 2);
+                as_objects[u16_objectIndex].f_ySpeed += (au8_can_data[4] >> 6);
+                as_objects[u16_objectIndex].f_ySpeed -= 1024;
+                as_objects[u16_objectIndex].f_ySpeed *= 0.1;
+
+                as_objects[u16_objectIndex].f_objectLength = (au8_can_data[6] >> 1) * 0.2;
+
+                as_objects[u16_objectIndex].u8_objectID = au8_can_data[7] & 0x7F;
+                as_objects[u16_objectIndex].u8_updateFlag = au8_can_data[7] >> 7;
+                break;
+
+            default:
+                log_warn("[cp] unhandled object data0 format");
+        }
+    }
+}
+
+
+
+
+void parse_input(u_int8_t* u8_input_buffer, int i32_len) {
+    enum state_t s_state = IDLE;
+    u_int8_t u8_input = 0;
+    int i32_i;
+
+    int i32_can_len_counter = 0, i32_can_len = 0, i32_can_id = 0, i32_stop_search = 0;
+    unsigned char au8_can_data[16] = {0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
+    int i32_xor = 0;
+
+    for (i32_i = 0; i32_i < i32_len; i32_i++) {
+        u8_input = u8_input_buffer[i32_i];
+        switch (i32_stop_search) {
+            case 0:
+                if (u8_input == 0xea) {
+                    i32_stop_search = 1;
+                }
+                break;
+            case 1:
+                if (u8_input == 0xeb) {
+                    i32_stop_search = 2;
+                } else {
+                    i32_stop_search = 0;
+                }
+                break;
+            case 2:
+                if (u8_input == 0xec) {
+                    i32_stop_search = 3;
+                } else {
+                    i32_stop_search = 0;
+                }
+                break;
+            case 3:
+                if (u8_input == 0xed) {
+                    s_state = IDLE;
+                }
+                i32_stop_search = 0;
+                break;
+        }
+
+        switch (s_state) {
+            case IDLE:
+                if (u8_input == 0xca) {
+                    s_state = START_1;
+                }
+                break;
+            case START_1:
+                if (u8_input == 0xcb) {
+                    s_state = START_2;
+                }
+                break;
+            case START_2:
+                if (u8_input == 0xcc) {
+                    s_state = START_3;
+                }
+                break;
+            case START_3:
+                if (u8_input == 0xcd) {
+                    s_state = START_4;
+                }
+                break;
+            case START_4:
+                s_state = CAN_ID_H;
+                break;
+            case CAN_ID_H:
+                s_state = CAN_ID_L;
+                break;
+            case CAN_ID_L:
+                s_state = CAN_LEN;
+                break;
+            case CAN_LEN:
+                s_state = CAN_PAYLOAD;
+                break;
+            case CAN_PAYLOAD:
+                if (i32_can_len_counter >= i32_can_len) {
+                    s_state = CAN_ID_H;
+                }
+                break;
+            default:
+                printf("Something probably went wrong, this code is likely unreachable");
+        }
+
+        switch (s_state) {
+            case START_1:
+                i32_xor = 0;
+                break;
+            case CAN_ID_H:
+                i32_can_id = 0;
+                i32_can_id |= u8_input << 8;
+                i32_xor ^= u8_input;
+                break;
+            case CAN_ID_L:
+                i32_can_id |= u8_input;
+                i32_xor ^= u8_input;
+                break;
+            case CAN_LEN:
+                i32_can_len = u8_input;
+                i32_can_len_counter = 0;
+                i32_xor ^= u8_input;
+                break;
+            case CAN_PAYLOAD:
+                if ((i32_can_len_counter < 16) && (i32_can_len_counter <= i32_can_len)) {
+                    //if (i32_can_len - i32_can_len_counter - 1 > 15)
+                    //  printf("%d", i32_can_len - i32_can_len_counter - 1);
+                    if (i32_can_len - i32_can_len_counter - 1 < 16)
+                        au8_can_data[i32_can_len - i32_can_len_counter - 1] = u8_input;
+                }
+                i32_can_len_counter++;
+                if (i32_can_len_counter >= i32_can_len) {
+                    parse_can_data_tm(i32_can_id, i32_can_len, au8_can_data);
+                }
+                i32_xor ^= u8_input;
+                break;
+            default:;
+        }
+    }
+}
+
+
+long rotate_x(long x, long y) {
+    return (long)(x * roadRotationCos - y * roadRotationSin);
+}
+
+long rotate_y(long x, long y) {
+    return (long)(x * roadRotationSin + y * roadRotationCos);
+}
+
+static void set_values(int i, int j, uint64_t timestamp, EI1_CPM_t* cpm_tx, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]){
+    /* Fill CPM */
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j] = calloc(1, sizeof(EI1_PerceivedObject_t));
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectID = (long)as_objects[i].u8_objectID;
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->timeOfMeasurement = 0;  //Sem informaçao do radar
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectConfidence = 95;
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.value = rotate_x(
+            (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100);
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xDistance.confidence = 102;
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.value = rotate_y(
+            (long)as_objects[i].f_xPoint * 100, (long)as_objects[i].f_yPoint * 100);
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->yDistance.confidence = 102;
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.value = rotate_x(
+            (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100);
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->xSpeed.confidence = 40;
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.value = rotate_y(
+            (long)as_objects[i].f_xSpeed * 100, (long)as_objects[i].f_ySpeed * 100);
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->ySpeed.confidence = 40;
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->objectRefPoint = EI1_ObjectRefPoint_bottomMid;
+
+    /* Detected Object Class*/
+    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->classification = calloc(1, sizeof(EI1_ObjectClassDescription_t));
+    EI1_ObjectClassDescription_t *class = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array[j]->classification;
+
+    class->list.size = 1 * sizeof(void*);
+    class->list.array = malloc(1* sizeof(void*));
+    class->list.count = 1;
+
+    class->list.array[0] = calloc(1, sizeof(EI1_ObjectClass_t));
+    class->list.array[0]->confidence = 0;
+    int obj_len = (int)(as_objects[i].f_objectLength*10);
+
+    if (obj_len == 10) {
+        class->list.array[0]->Class.present = EI1_class_PR_person;
+        class->list.array[0]->Class.choice.person.type  = 1; // pedestrian
+        class->list.array[0]->Class.choice.person.confidence  = 0;
+    } else if (obj_len == 16) {
+        class->list.array[0]->Class.present = EI1_class_PR_person;
+        class->list.array[0]->Class.choice.person.type  = 3; // cyclist
+        class->list.array[0]->Class.choice.person.confidence  = 0;
+    } else if (obj_len == 26) {
+        class->list.array[0]->Class.present = EI1_class_PR_vehicle;
+        class->list.array[0]->Class.choice.vehicle.type  = 2; // motorcycle
+        class->list.array[0]->Class.choice.vehicle.confidence  = 0;
+    } else if (obj_len >= 46 && obj_len <= 54) {
+        class->list.array[0]->Class.present = EI1_class_PR_vehicle;
+        class->list.array[0]->Class.choice.vehicle.type  = 3; // passenger car
+        class->list.array[0]->Class.choice.vehicle.confidence  = 0;
+    } else if (obj_len >= 56 && obj_len <= 88) {
+        class->list.array[0]->Class.present = EI1_class_PR_vehicle;
+        class->list.array[0]->Class.choice.vehicle.type  = 5; // light truck
+        class->list.array[0]->Class.choice.vehicle.confidence  = 0;
+    } else if (obj_len >= 90) {
+        class->list.array[0]->Class.present = EI1_class_PR_vehicle;
+        class->list.array[0]->Class.choice.vehicle.type  = 6; // heavy truck
+        class->list.array[0]->Class.choice.vehicle.confidence  = 0;
+    } else {
+        class->list.array[0]->Class.present = EI1_class_PR_other;
+        class->list.array[0]->Class.choice.other.type  = 0; // unknown
+        class->list.array[0]->Class.choice.other.confidence  = 0;
+    }
+
+    /* Fill History values */
+    valid_array[as_objects[i].u8_objectID] = 1;					// Comparation Array
+    history_list[as_objects[i].u8_objectID][0] = (long)as_objects[i].f_xPoint * 100;	// xPoint (Distance)					     
+    history_list[as_objects[i].u8_objectID][1] = (long)as_objects[i].f_yPoint * 100;	// yPoint (Distance)
+    history_list[as_objects[i].u8_objectID][2] = (long)as_objects[i].f_xSpeed * 100;	// xSpeed (Speed)
+    history_list[as_objects[i].u8_objectID][3] = (long)as_objects[i].f_ySpeed * 100;	// ySpeed (Speed)
+    history_timestamp[as_objects[i].u8_objectID] = timestamp;				// Time stamp of detected object
+}
+
+
+static int mk_cpm(uint8_t *bdr_oer, uint32_t *bdr_len, uint8_t *fdi_oer, uint32_t *fdi_len, long history_list[NOF_OBJECTS][4], int valid_array[], uint64_t history_timestamp[]) {
+
+    /* Variables */
+    EI1_CPM_t* cpm_tx = calloc(1, sizeof(EI1_CPM_t));
+
+    long euclidian_dist, abs_speed, abs_speed_hist, angle, angle_hist, angle_diff;
+    int j = 0, rv = 0;
+    int temp[NOF_OBJECTS];
+
+
+    cpm_tx->header.protocolVersion = PROTOCOL_VERSION;
+    cpm_tx->header.messageID = MESSAGE_ID;
+    pthread_mutex_lock(&facilities.id.lock);
+    cpm_tx->header.stationID = facilities.id.station_id; 
+    pthread_mutex_unlock(&facilities.id.lock);
+
+    uint64_t generationDeltaTime = itss_time_get() % 65536;  // generationDeltaTime = TimestampIts mod 65 536
+
+    int32_t lat, lon, alt, alt_conf;
+    itss_space_lock();
+    itss_space_get();
+    lat = epv.space.data.latitude.value; 
+    lon = epv.space.data.longitude.value; 
+    alt = epv.space.data.altitude.value; 
+    alt_conf = epv.space.data.altitude.confidence; 
+    itss_space_unlock();
+
+    cpm_tx->cpm.generationDeltaTime = generationDeltaTime;
+    cpm_tx->cpm.cpmParameters.managementContainer.stationType = EI1_StationType_roadSideUnit;
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.latitude = lat; 
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.longitude = lon;
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorConfidence = 100; // TODO
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMinorConfidence = 100; // TODO
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.positionConfidenceEllipse.semiMajorOrientation = EI1_HeadingValue_wgs84North; // TODO
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeValue = alt;
+    cpm_tx->cpm.cpmParameters.managementContainer.referencePosition.altitude.altitudeConfidence = alt_conf;
+
+    if(dissemination_check(0) == 1){  /* Sensor Information Container Inclusion Management */
+
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer = calloc(1, sizeof(EI1_SensorInformationContainer_t));
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.count = 1;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.size = 1;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array = calloc(1, sizeof(EI1_SensorInformation_t));
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0] = calloc(1, sizeof(EI1_SensorInformation_t));
+
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->sensorID = 0;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->type = EI1_SensorType_radar;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.present = EI1_DetectionArea_PR_stationarySensorRadial;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.range = 3400;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleStart = facilities.dissemination.opening_angle_start;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.stationaryHorizontalOpeningAngleEnd = facilities.dissemination.opening_angle_end;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart = calloc(1, sizeof(EI1_CartesianAngleValue_t));
+        (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleStart) = 1730;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd = calloc(1, sizeof(EI1_CartesianAngleValue_t));
+        (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.verticalOpeningAngleEnd) = 1890;
+        cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight = calloc(1, sizeof(EI1_SensorHeight_t));
+        (*cpm_tx->cpm.cpmParameters.sensorInformationContainer->list.array[0]->detectionArea.choice.stationarySensorRadial.sensorHeight) = 600;
+        dissemination_reset_timer(0);
+    }
+
+    if (s_objectControl.u8_numberOfObjects > 0) {
+	    cpm_tx->cpm.cpmParameters.perceivedObjectContainer = calloc(1, sizeof(EI1_PerceivedObjectContainer_t));
+	    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.array = calloc(s_objectControl.u8_numberOfObjects,sizeof(EI1_PerceivedObject_t*));
+
+	    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = s_objectControl.u8_numberOfObjects; 
+
+	    memcpy(temp, valid_array, NOF_OBJECTS * sizeof(int)); // NOF_OBJECTS * sizeof(int) = size of valid_array
+	    memset(valid_array, 0, NOF_OBJECTS * sizeof(int));
+
+	    for(int i = 0; i < cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;i++){
+		if(temp[as_objects[i].u8_objectID] == 0 ){ // The object is going to be added without comparison (It is a new object) (valid_array[id] = 0)
+		    set_values(i,j,generationDeltaTime,cpm_tx,history_list,valid_array,history_timestamp);
+		    j++;
+
+		}else{ // The object is going to be compared (It was included in previous CPMs) (valid_array[id] = 1)
+
+		    // Getting the euclidian distance value from the object detected and the same object in the last cpm (xcurrent - xhistory)^2 + (ycurrent - yhistory)^2
+		    euclidian_dist = sqrt( pow(((long)as_objects[i].f_xPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][0]), 2) + pow(((long)as_objects[i].f_yPoint * 100) - (history_list[(long)as_objects[i].u8_objectID][1]) ,2) );
+
+		    // Getting the absolute speed value from the object detected and the same object in the last cpm (sqrt(x^2 + y^2))
+		    abs_speed = sqrt( pow( ((long)as_objects[i].f_xSpeed * 100),2) + pow( ( (long)as_objects[i].f_ySpeed * 100),2) ); 
+		    abs_speed_hist = sqrt( pow( history_list[(long)as_objects[i].u8_objectID][2] ,2) + pow( history_list[(long)as_objects[i].u8_objectID][3],2) ); // sqrt(xSpeed^2 + ySpeed^2)
+
+		    // Getting the angle from the velocity vector detected and the same object in the last cpm 
+		    angle = (long)((180 / PI) * atan2( (long)as_objects[i].f_ySpeed * 100 , (long)as_objects[i].f_xSpeed * 100 ));
+		    angle_hist = (long)((180 / PI) * atan2( history_list[(long)as_objects[i].u8_objectID][3] , history_list[(long)as_objects[i].u8_objectID][2]) ); // tan(yspeed / xspeed)
+		    angle_diff = ((angle - angle_hist) + 180) % 360 - 180;
+
+		    // Requirements to include the object in the CPM (> 4 m or > 0.5 m/s or > 4º or > T_GenCpmMax)
+
+		    if(abs(euclidian_dist) > 400 || abs(abs_speed - abs_speed_hist) > 50 || abs(angle_diff) > 4 || abs(generationDeltaTime - history_timestamp[i]) >= facilities.dissemination.T_GenCpmMax){
+			set_values(i,j,generationDeltaTime,cpm_tx, history_list, valid_array,history_timestamp);
+			j++;
+
+		    }else{ //The object is not included but is valid for comparison in the upcoming CPMs
+			valid_array[(long)as_objects[i].u8_objectID] = 1;
+		    }	
+		}
+	    }
+
+
+	    cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count;	// Object perceived by the Radar (Does not have to match the objects included in the CPM)
+	    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.count = j;							// The number of objects that were included in the CPM
+	    cpm_tx->cpm.cpmParameters.perceivedObjectContainer->list.size = j;
+    }
+    cpm_tx->cpm.cpmParameters.numberOfPerceivedObjects = j;
+
+
+    /******* Encode CPMs to FDI and BDR ********/
+
+    //BDR
+    memset(bdr_oer, 0, 1500);
+    asn_enc_rval_t retval_enc_bdr = uper_encode_to_buffer(&asn_DEF_EI1_CPM, NULL, cpm_tx, bdr_oer, 1500);
+    if (retval_enc_bdr.encoded == -1) {
+        log_error("[cp] failed encoding CPM (%s)", retval_enc_bdr.failed_type->name);
+        rv = 1;
+        goto cleanup;
+    }
+
+    *bdr_len = ((retval_enc_bdr.encoded + 7) / 8);
+
+    //FDI
+    memset(fdi_oer, 0, 1500);
+    asn_enc_rval_t retval_enc_fdi = uper_encode_to_buffer(&asn_DEF_EI1_CPM, NULL, cpm_tx, fdi_oer, 1500);
+    if (retval_enc_fdi.encoded == -1) {
+        log_error("[cp] failed encoding CPM (%s)", retval_enc_fdi.failed_type->name);
+        rv = 1;
+        goto cleanup;
+    }
+
+    *fdi_len = ((retval_enc_fdi.encoded + 7) / 8);
+
+cleanup:
+    ASN_STRUCT_FREE(asn_DEF_EI1_CPM, cpm_tx);
+
+    return rv;
+}
+
+void *cp_service(void *arg){  
+    /* Variables */
+    int i32_recv_bytes;
+    u_int8_t au8_readBuffer[READ_BUFFER_SIZE];
+    u_int8_t au8_readTcp[READ_BUFFER_SIZE];
+    bool is_radar_connected;
+    long history_list[NOF_OBJECTS][4];
+    int valid_array[NOF_OBJECTS];
+    uint64_t history_timestamp[NOF_OBJECTS];
+
+    memset(history_list, 0, sizeof(history_list));
+    memset(valid_array, 0, sizeof(valid_array));
+    memset(history_timestamp, 0, sizeof(history_timestamp));
+
+    uint8_t tr_oer[INDICATION_BUFFER_SIZE];
+    uint8_t fi_oer[INDICATION_BUFFER_SIZE];
+    tr_oer[0] = ITSS_FACILITIES;		//Facilities
+    fi_oer[0] = ITSS_FACILITIES;
+
+    EIS_NetworkingRequest_t* nr = calloc(1, sizeof(EIS_NetworkingRequest_t));
+    nr->present = EIS_NetworkingRequest_PR_packet;
+    EIS_NetworkingPacketRequest_t* npr = &nr->choice.packet;
+
+    npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn;
+    npr->network.choice.gn.trafficClass = 2;
+    npr->network.choice.gn.destinationAddress.buf = malloc(6);
+    for (int i = 0; i < 6; ++i) {
+        npr->network.choice.gn.destinationAddress.buf[i] = 0xff;
+    }
+    npr->network.choice.gn.destinationAddress.size = 6;
+    npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
+    npr->network.choice.gn.securityProfile.sign = true;
+
+    npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp;
+    npr->transport.choice.btp.btpType = EIS_BTPType_btpB;
+    npr->transport.choice.btp.destinationPort = EIS_Port_cpm;
+
+    EIS_FacilitiesIndication_t* fi = calloc(1, sizeof(EIS_FacilitiesIndication_t));
+    fi->present = EIS_FacilitiesIndication_PR_message;
+    EIS_FacilitiesMessageIndication_t* fmi = &fi->choice.message;
+
+    roadRotationSin = sin(((facilities.dissemination.radar_rotation + 90.0) * PI) / 180);
+    roadRotationCos = cos(((facilities.dissemination.radar_rotation + 90.0) * PI) / 180);
+
+
+    npr->data.buf = malloc(1500);                   	//CPM Data to be sent to the Networking layer
+
+    /*--- Fill mandatory Facilities Message Indication parameters ---*/
+    fmi->itsMessageType = EIS_ItsMessageType_cpm;
+    fmi->data.buf = malloc(1500);
+
+    /* Creating sockets and waiting for radar to connect*/	
+    is_radar_connected = radar_connection(RADAR_PORT);	
+
+    while(!facilities.exit){
+        itss_usleep(1000*50);
+
+        /* If the Radar is not connected to TMC, a TCP socket is needed to fool the Radar */
+        /* To maintain the connection the content must be read */ 		
+        if(facilities.dissemination.tmc_connect == false)
+            i32_recv_bytes = recv(s_socket.i32_client, &au8_readTcp, READ_BUFFER_SIZE, 0);
+
+        /* Reads from the radar */
+        i32_recv_bytes = recv(raw_socket.raw_fd, &au8_readBuffer, READ_BUFFER_SIZE, 0);
+
+        if (dissemination_check(1)) {
+            if(is_radar_connected){
+                /* Information parsing from radar */
+                parse_input(au8_readBuffer,i32_recv_bytes);																				
+
+                /* CPM build and encoding to BDR and FDI */ 						
+                if(mk_cpm(npr->data.buf, (uint32_t *) &npr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size, history_list, valid_array, history_timestamp) == 1)
+                    continue;				
+    
+                uint32_t id = itss_id(npr->data.buf, npr->data.size);
+                npr->id = id;
+                fmi->id = id;
+
+                /* Encode NetworkingRequest */
+                asn_enc_rval_t enc_tdr = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, tr_oer+1, INDICATION_BUFFER_SIZE-1);
+                if(enc_tdr.encoded == -1){
+                    log_error("encoding TR for cpm failed");
+                    continue;
+                }
+
+                /* Encode FacilitiesIndication */ 
+                asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_EIS_FacilitiesIndication, NULL, fi, fi_oer+1, INDICATION_BUFFER_SIZE-1);
+                if(enc_fdi.encoded == -1){
+                    log_error("encoding FI for cpm failed");
+                    continue;
+                }
+
+                /* Create thread to send packet to the Networking Layer (=3) */
+                itss_queue_send(facilities.tx_queue, itss_queue_packet_new(tr_oer, enc_tdr.encoded+1, ITSS_NETWORKING, id, "NR.packet.btp"));
+
+                /* Create thread to send packet to the Applications Layer (=5) */
+                itss_queue_send(facilities.tx_queue, itss_queue_packet_new(fi_oer, enc_fdi.encoded+1, ITSS_APPLICATIONS, id, "FI.message"));
+
+                /*Reset Timer for dissemination control */ 
+                dissemination_reset_timer(1);
+
+                // Logging 
+                if (facilities.logging.dbms) {
+                    pthread_mutex_lock(&facilities.id.lock);
+                    uint32_t station_id = facilities.id.station_id;
+                    pthread_mutex_unlock(&facilities.id.lock);
+                    itss_db_add(facilities.logging.dbms, station_id, npr->id, true, 14, NULL, npr->data.buf, npr->data.size);
+                }
+                if (facilities.logging.recorder) {
+                    uint16_t buffer_len = INDICATION_BUFFER_SIZE;
+                    uint8_t buffer[buffer_len];
+                    int e = itss_management_record_packet_sdu(
+                            buffer,
+                            buffer_len,
+                            npr->data.buf,
+                            npr->data.size,
+                            npr->id,
+                            itss_time_get(),
+                            ITSS_FACILITIES,
+                            true);
+                    if (e != -1) {
+                        itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buffer, e, ITSS_MANAGEMENT, npr->id, "MReq.packet.set"));  
+                    }
+                }
+
+            }else{
+                is_radar_connected = radar_connection(RADAR_PORT);
+            }	
+        }
+    }
+
+    ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);
+    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
+
+    /* Close sockets */
+    if(facilities.dissemination.tmc_connect)
+        shutdown(s_socket.i32_socket,2);
+
+    shutdown(raw_socket.raw_fd,2);
+    return NULL; 
+}

backup/src/cpm.h

@@ -0,0 +1,187 @@
+#ifndef FACILITIES_CPM_H
+#define FACILITIES_CPM_H
+
+
+#include <arpa/inet.h>
+#include <stdbool.h>
+
+#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
+#include <termios.h>
+#include <unistd.h>
+#include <time.h>
+#include <net/if.h>
+#include <linux/if_packet.h>
+
+#define READ_BUFFER_SIZE 1024     // Buffer for Serial and Ethernet read function (Used in cp_service)
+#define NOF_OBJECTS 127  	  // Number of Object UMRR-0C
+#define MESSAGE_ID 14
+#define PROTOCOL_VERSION 1
+#define RADAR_PORT "55555"	  // Destination port from the radar
+
+
+/* Structures */
+
+typedef struct
+{
+  int i32_socket;  // Socket descriptor server
+  int i32_client;  // Socket descriptor UMRR-0C
+  int i32_port;    // Listen on Port
+  bool b_moxa;
+  struct sockaddr_in s_server;
+  struct sockaddr_in s_client;
+} S_ETHERNET_CONNECTION_T;
+
+typedef struct
+{
+  int raw_fd;
+  struct sockaddr_ll sll;
+  struct ifreq ifr;
+} S_INTERFACE_CONNECTION_T;
+
+typedef struct
+{
+  u_int8_t u8_numberOfCountedObjects;
+  u_int32_t u32_UnixTime;
+  u_int16_t u16_Milliseconds;
+  u_int8_t u8_SensorNetworkID;
+  u_int8_t u8_ObjectNumber;
+  u_int8_t u8_ObjectID;
+  int16_t i16_speed;
+  u_int8_t u8_class;
+  u_int8_t u8_mLineNumber;
+  u_int16_t u8_laneNumber;
+} S_PVR_T;
+
+typedef struct
+{
+  u_int8_t u8_sensorStatus;
+  u_int8_t u8_InterfaceMode;
+  u_int8_t u8_networkID;
+  u_int8_t u8_diagnose;
+  u_int32_t u32_time;
+} S_SENSOR_CONTROL_T;
+
+typedef struct
+{
+  u_int8_t u8_numberOfObjects;
+  u_int8_t u8_numberOfMessages;
+  u_int8_t u8_cycleDuration;
+  u_int8_t u8_objectData0Format;
+  u_int8_t u8_objectData1Format;
+  u_int32_t u32_cycleCount;
+} S_OBJECT_CONTROL_T;
+
+typedef struct
+{
+  u_int8_t u8_modeSignal;
+  float f_xPoint;
+  float f_yPoint;
+  float f_xSpeed;
+  float f_ySpeed;
+  float f_objectLength;
+  u_int8_t u8_objectID;
+  u_int8_t u8_updateFlag;
+} S_OBJECTS_T;
+
+
+enum state_t {
+  IDLE,
+  START_1,
+  START_2,
+  START_3,
+  START_4,
+  START_PATTERN,
+  PROTOCOLVERSION,
+  HEADERLENGTH,
+  PAYLOADLENGTH_H,
+  PAYLOADLENGTH_L,
+  APPPROTOCOLTYPE,
+  FLAG_1_H,
+  FLAG_1_L,
+  FLAG_2_H,
+  FLAG_2_L,
+  CRC16HEADER_H,
+  CRC16HEADER_L,
+  PAYLOAD_DATA,
+  CAN_ID_H,
+  CAN_ID_L,
+  CAN_LEN,
+  CAN_PAYLOAD,
+  PAYLOAD_CRC16_H,
+  PAYLOAD_CRC16_L
+};
+
+typedef struct
+{
+
+   bool active;
+   bool tmc_connect;
+   char* int_radar;
+   char* ip_radar;
+
+   pthread_mutex_t lock;
+
+   uint32_t type;
+
+   uint64_t next_cpm_max;
+   uint64_t next_cpm_min;
+   
+
+   uint64_t T_GenCpmMin;
+   uint64_t T_GenCpmMax;
+
+   uint64_t T_AddSensorInformation;
+   uint64_t next_AddSensorInformation;
+
+
+/* Position of the radar (Value from toml)  */
+   
+   int64_t radar_rotation;
+   int16_t opening_angle_start;
+   int16_t opening_angle_end;
+ 
+} dissemination_t;
+
+/* Prototype Functions */
+
+/* 
+Summary : Waits for client to connect (Radar)
+*/
+
+bool waitingIncomingConnection(void);
+
+
+
+/* 
+Summary : Creation of socket and binding to the radar add and port
+	param [in] char* radar_ip: Radar IP
+	param [in] char* radar_port: Radar PORT 
+*/
+
+bool initEthernetConnection(const char* radar_ip,const char* radar_port);
+
+
+
+/* 
+Summary : Initialization for dissemination control. (Memory allocation and mutex init
+*/
+
+dissemination_t* dissemination_init();
+
+
+/* 
+Summary : Creation of socket and binding to the radar add and port
+	param [in] char* radar_ip: Radar IP
+	param [in] char* radar_port: Radar PORT 
+*/
+
+bool initEthernetConnection(const char* radar_ip,const char* radar_port);
+
+/*
+Summary: Read from Radar socket (s_socket.i32_socket) call the function to interpret data 
+ param[in] void	
+*/
+
+void* cp_service(void *arg);
+
+#endif

backup/src/facilities.c

@@ -0,0 +1,542 @@
+#include "facilities.h"
+
+#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
+#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
+#include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h>
+#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
+#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
+#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesRequest.h>
+#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementIndication.h>
+#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
+#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
+#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityIndication.h>
+#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
+#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
+#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityResponse.h>
+#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
+#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
+#include <it2s-asn/etsi-its-v1/ivim/EI1_IVIM.h>
+#include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
+#include <it2s-tender/packet.h>
+#include <it2s-tender/recorder.h>
+#include <it2s-tender/space.h>
+#include <it2s-tender/time.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <zmq.h>
+
+#include "cam.h"
+#include "config.h"
+#include "cpm.h"
+#include "denm.h"
+#include "evm.h"
+#include "indications.h"
+#include "infrastructure.h"
+#include "requests.h"
+#include "saem.h"
+#include "tpm.h"
+#include "vcm.h"
+
+facilities_t facilities = {0};
+
+static int networking_indication(void *responder, void **security_socket, uint8_t *msg, uint32_t msg_len) {
+    int rv = 0;
+    uint8_t code = 0;
+    bool stored = false;
+
+    EIS_NetworkingIndication_t *ni = calloc(1, sizeof(EIS_NetworkingIndication_t));
+
+    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_NetworkingIndication, (void **)&ni, msg, msg_len);
+    if (dec.code) {
+        log_error("<- invalid TI received");
+        rv = 1;
+        code = 1;
+        itss_0send(responder, &code, 1);
+        goto cleanup;
+    }
+
+    itss_0send(responder, &code, 1);
+
+    switch (ni->present) {
+        case EIS_NetworkingIndication_PR_packet:
+            networking_packet_indication(&ni->choice.packet, security_socket);
+            break;
+        case EIS_NetworkingIndication_PR_data:
+            networking_data_indication(&ni->choice.data, security_socket);
+            goto cleanup;
+        default:
+            log_debug("<- unrecognized NI.choice received");
+            rv = 1;
+            goto cleanup;
+    }
+
+cleanup:
+    ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingIndication, ni);
+
+    return rv;
+}
+
+static int facilities_request(void *responder, uint8_t *msg, uint32_t msg_len) {
+    int rv = 0;
+
+    EIS_FacilitiesRequest_t *fr = calloc(1, sizeof(EIS_FacilitiesRequest_t));
+
+    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_FacilitiesRequest, (void **)&fr, msg, msg_len);
+    if (dec.code) {
+        log_error("<- invalid FR received");
+        facilities_request_result_rejected(responder);
+        rv = 1;
+        goto cleanup;
+    }
+
+    switch (fr->present) {
+        case EIS_FacilitiesRequest_PR_message:
+            rv = facilities_request_single_message(responder, &fr->choice.message);
+            break;
+
+        case EIS_FacilitiesRequest_PR_data:
+            switch (fr->choice.data.present) {
+                case EIS_FacilitiesDataRequest_PR_activeEpisodes:
+                    rv = facilities_request_active_episodes(responder, fr);
+                    break;
+
+                case EIS_FacilitiesDataRequest_PR_attributeTypes:
+                    rv = facilities_request_attribute_types(responder, fr);
+                    break;
+
+                case EIS_FacilitiesDataRequest_PR_loadedProtectionZones:
+                    rv = facilities_request_loaded_protected_zones(responder, fr);
+                    break;
+
+                case EIS_FacilitiesDataRequest_PR_chainInfoSet:
+                    rv = facilities_request_chaininfo_set(responder, &fr->choice.data.choice.chainInfoSet);
+                    break;
+
+                default:
+                    log_error("<- unrecognized FDR type received (%d)", fr->choice.data.present);
+                    facilities_request_result_rejected(responder);
+                    rv = 1;
+                    goto cleanup;
+            }
+            break;
+
+        default:
+            log_error("<- unrecognized FR type received (%d)", fr->present);
+            facilities_request_result_rejected(responder);
+            rv = 1;
+            goto cleanup;
+    }
+
+cleanup:
+    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesRequest, fr);
+
+    return rv;
+}
+
+static int security_indication(void *responder_secured, uint8_t *msg, uint32_t msg_len) {
+    int rv = 0;
+
+    EIS_SecurityIndication_t *si = calloc(1, sizeof(EIS_SecurityIndication_t));
+    EIS_SecurityResponse_t *sr = calloc(1, sizeof(EIS_SecurityResponse_t));
+
+    uint8_t buffer[64];
+
+    asn_enc_rval_t enc;
+
+    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_SecurityIndication, (void **)&si, msg, msg_len);
+    if (dec.code) {
+        log_error("<- invalid SIndication received");
+        rv = 1;
+        goto cleanup;
+    }
+
+    pthread_mutex_lock(&facilities.id.change.lock);
+    if (facilities.id.change.stage == ID_CHANGE_BLOCKED) {
+        pthread_mutex_unlock(&facilities.id.change.lock);
+        log_debug("identity change is currently blocked");
+        rv = 1;
+        goto cleanup;
+    }
+
+    if (facilities.id.change.stage == ID_CHANGE_PREPARE &&
+        si->choice.idChangeEvent.command != EIS_SecurityIdChangeEventType_commit &&
+        si->choice.idChangeEvent.command != EIS_SecurityIdChangeEventType_abort) {
+        pthread_mutex_unlock(&facilities.id.change.lock);
+        log_debug("current identity change state is prepare, but received identity change command is not commit nor abort");
+        rv = 1;
+        goto cleanup;
+    }
+
+    bool id_changed = false;
+
+    switch (si->choice.idChangeEvent.command) {
+        case EIS_SecurityIdChangeEventType_prepare:
+
+            facilities.id.change.stage = ID_CHANGE_PREPARE;
+
+            pthread_mutex_lock(&facilities.id.lock);
+            pthread_mutex_lock(&facilities.lightship.lock);
+            break;
+
+        case EIS_SecurityIdChangeEventType_commit:;
+            facilities.id.change.stage = ID_CHANGE_COMMIT;
+
+            // Reset lightship
+            for (int i = 0; i < facilities.lightship.path_history_len; ++i) {
+                free(facilities.lightship.path_history[i]);
+            }
+            facilities.lightship.path_history_len = 0;
+
+            facilities.lightship.t_last_cam = 0;
+            facilities.lightship.t_last_cam_lfc = 0;
+            facilities.lightship.next_cam_max = 0;
+            facilities.lightship.next_cam_min = 0;
+
+            pthread_mutex_unlock(&facilities.lightship.lock);
+
+            // Change Station ID
+            for (int i = 0; i < si->choice.idChangeEvent.ids.list.count; ++i) {
+                switch (si->choice.idChangeEvent.ids.list.array[i]->present) {
+                    case EIS_SecurityId_PR_stationId:
+                        facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId;
+                        break;
+                    case EIS_SecurityId_PR_ipv6Address:
+                        memcpy(facilities.id.ipv6_addr, si->choice.idChangeEvent.ids.list.array[i]->choice.ipv6Address.buf, 16);
+                        break;
+                    default:
+                        break;
+                }
+            }
+
+            facilities.id.change.stage = ID_CHANGE_INACTIVE;
+
+            pthread_mutex_unlock(&facilities.id.lock);
+
+            id_changed = true;
+
+            break;
+
+        case EIS_SecurityIdChangeEventType_abort:
+            pthread_mutex_unlock(&facilities.id.lock);
+            pthread_mutex_unlock(&facilities.lightship.lock);
+
+            facilities.id.change.stage = ID_CHANGE_INACTIVE;
+            break;
+
+        default:
+            pthread_mutex_unlock(&facilities.id.change.lock);
+            log_error("[networking]<- unhandled idChangeEvent command type");
+            rv = 1;
+            goto cleanup;
+    }
+
+    sr->present = EIS_SecurityResponse_PR_idChangeEvent;
+    sr->choice.idChangeEvent.returnCode = 0;
+    enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityResponse, NULL, sr, buffer, 64);
+    itss_0send(responder_secured, buffer, enc.encoded);
+
+    pthread_mutex_unlock(&facilities.id.change.lock);
+
+cleanup:
+    if (rv) {
+        sr->present = EIS_SecurityResponse_PR_idChangeEvent;
+        sr->choice.idChangeEvent.returnCode = 1;
+        enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityResponse, NULL, sr, buffer, 64);
+        itss_0send(responder_secured, buffer, enc.encoded);
+    }
+
+    pthread_mutex_unlock(&facilities.id.change.lock);
+    ASN_STRUCT_FREE(asn_DEF_EIS_SecurityResponse, sr);
+    ASN_STRUCT_FREE(asn_DEF_EIS_SecurityIndication, si);
+
+    return rv;
+}
+
+
+static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len) {
+    int rv = 0;
+
+    uint8_t code = 0;
+
+    EIS_ManagementIndication_t *mi = calloc(1, sizeof(EIS_ManagementIndication_t));
+
+    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_ManagementIndication, (void **)&mi, msg, msg_len);
+    if (dec.code) {
+        rv = 1;
+        code = 1;
+        itss_0send(responder, &code, 1);
+        goto cleanup;
+    }
+    itss_0send(responder, &code, 1);
+
+    if (mi->present == EIS_ManagementIndication_PR_attributes) {
+        itss_time_lock();
+        asn_INTEGER2ulong(&mi->choice.attributes.clock, &epv.time.clock);
+        itss_time_unlock();
+    }
+
+cleanup:
+    ASN_STRUCT_FREE(asn_DEF_EIS_ManagementIndication, mi);
+
+    return rv;
+}
+
+void *tx(void *arg) {
+    int rv = 0;
+
+    itss_queue_t *queue = facilities.tx_queue;
+
+    uint8_t code;
+    void *applications_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ);
+    void *networking_socket = itss_0connect(facilities.zmq.networking_address, ZMQ_REQ);
+    void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);
+
+    itss_queue_t *stream = itss_queue_new();
+
+    while (!facilities.exit) {
+        pthread_mutex_lock(&queue->lock);
+        while (!queue->len && !facilities.exit) {
+            pthread_cond_wait(&queue->trigger, &queue->lock);
+        }
+
+        itss_queue_packet_t* qp;
+        while ((qp = itss_queue_pop(queue))) {
+            itss_queue_add(stream, qp);
+        };
+        pthread_mutex_unlock(&queue->lock);
+
+        while ((qp = itss_queue_pop(stream))) {
+            switch (qp->destination) {
+                case ITSS_NETWORKING:
+                    log_debug("-> %s ->[networking] | id:%08x size:%dB",
+                              qp->info_msg, (uint32_t)qp->id, qp->data_len);
+                    itss_0send(networking_socket, qp->data, qp->data_len);
+                    rv = itss_0recv_rt(&networking_socket, &code, 1, qp->data, qp->data_len, 1000);
+                    if (rv == -1) {
+                        log_error("-> %s ->[networking] | id:%08x size:%dB <TIMEOUT>",
+                                  qp->info_msg, (uint32_t)qp->id, qp->data_len);
+                    }
+                    break;
+                case ITSS_APPLICATIONS:
+                    log_debug("-> %s ->[applications] | id:%08x size:%dB",
+                              qp->info_msg, (uint32_t)qp->id, qp->data_len);
+                    itss_0send(applications_socket, qp->data, qp->data_len);
+                    rv = itss_0recv_rt(&applications_socket, &code, 1, qp->data, qp->data_len, 1000);
+                    if (rv == -1) {
+                        log_error("-> %s ->[applications] | id:%08x size:%dB <TIMEOUT>",
+                                  qp->info_msg, (uint32_t)qp->id, qp->data_len);
+                    }
+                    break;
+                case ITSS_MANAGEMENT:
+                    itss_0send(management_socket, qp->data, qp->data_len);
+                    rv = itss_0recv_rt(&management_socket, &code, 1, qp->data, qp->data_len, 1000);
+                    if (rv == -1) {
+                        log_error("-> %s ->[management] | id:%08x size:%dB <TIMEOUT>",
+                                  qp->info_msg, (uint32_t)qp->id, qp->data_len);
+                    }
+                    break;
+            }
+            itss_queue_packet_free(qp);
+        }
+    }
+
+    itss_0close(networking_socket);
+    itss_0close(management_socket);
+    itss_0close(applications_socket);
+
+    return NULL;
+}
+
+static void sigh(int signum) {
+    facilities.exit = true;
+    uint8_t code = 0;
+    void *socket = itss_0connect(ZMQ_INTERNAL_ADDR, ZMQ_PAIR);
+    itss_0send(socket, &code, sizeof(code));
+    itss_0close(socket);
+}
+
+int main() {
+    signal(SIGTERM, sigh);
+    signal(SIGINT, sigh);
+    signal(SIGKILL, sigh);
+
+    facilities.tx_queue = itss_queue_new();
+    lightship_init();
+    den_init();
+    infrastructure_init();
+    dissemination_init();
+    bulletin_init();
+    void *security_socket = NULL;
+
+    if (facilities_config()) {
+        goto cleanup;
+    }
+
+    facilities.lightship.type = facilities.station_type;
+
+    // Tx
+    pthread_create(&facilities.transmitting, NULL, tx, NULL);
+
+    // CA
+    pthread_create(&facilities.ca_service, NULL, ca_service, NULL);
+
+    // DEN
+    pthread_create(&facilities.den_service, NULL, den_service, NULL);
+
+    // Infrastructure
+    pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL);
+
+    // CP
+    if (facilities.dissemination.active)
+        pthread_create(&facilities.cp_service, NULL, cp_service, NULL);
+
+    // SA
+    pthread_create(&facilities.sa_service, NULL, sa_service, NULL);
+
+    // Tolling
+    tolling_init(facilities.station_type);
+
+    // VC
+    if (facilities.coordination.active) {
+        coordination_init();
+        pthread_create(&facilities.vc_service, NULL, vc_service, NULL);
+    }
+
+    // EVCSN
+    if (facilities.evm_args.activate)
+        pthread_create(&facilities.evcsn_service, NULL, evcsn_service, NULL);
+
+    // EDM
+    if (facilities.edm.enabled) {
+        edm_init();
+    }
+
+    // MCM
+    if (facilities.mcm_coord.active){
+        mcm_coord_init();
+        pthread_create(&facilities.mcm_service, NULL, mc_service, NULL);
+    }
+    
+    facilities.apps_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ);
+    security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);
+
+    uint8_t buffer[ITSS_SDU_MAX_LEN];
+    log_info("listening");
+    uint8_t code;
+    bool in_idchange;
+
+    int32_t rl;
+
+    while (!facilities.exit) {
+        itss_0poll(facilities.zmq.responders, facilities.zmq.n_responders, -1);
+
+        for (int i = 0; i < facilities.zmq.n_responders; ++i) {
+            if (facilities.zmq.responders[i].revents) {
+                rl = itss_0recv(facilities.zmq.responders[i].socket, buffer, ITSS_SDU_MAX_LEN);
+                switch (buffer[0]) { /* source */
+
+                    case ITSS_INTERNAL:
+                        break;
+
+                    case ITSS_NETWORKING:
+                        in_idchange = true;
+                        pthread_mutex_lock(&facilities.id.change.lock);
+                        if (facilities.id.change.stage == ID_CHANGE_INACTIVE) {
+                            in_idchange = false;
+                            facilities.id.change.stage = ID_CHANGE_BLOCKED;
+                        }
+                        pthread_mutex_unlock(&facilities.id.change.lock);
+
+                        if (!in_idchange) {
+                            networking_indication(facilities.zmq.responders[i].socket, &security_socket, buffer + 1, rl-1);
+
+                            pthread_mutex_lock(&facilities.id.change.lock);
+                            facilities.id.change.stage = ID_CHANGE_INACTIVE;
+                            pthread_mutex_unlock(&facilities.id.change.lock);
+                        } else {
+                            code = 1;
+                            itss_0send(facilities.zmq.responders[i].socket, &code, 1);
+                        }
+                        break;
+
+                    case ITSS_APPLICATIONS:
+                        in_idchange = true;
+                        pthread_mutex_lock(&facilities.id.change.lock);
+                        if (facilities.id.change.stage == ID_CHANGE_INACTIVE) {
+                            in_idchange = false;
+                            facilities.id.change.stage = ID_CHANGE_BLOCKED;
+                        }
+                        pthread_mutex_unlock(&facilities.id.change.lock);
+
+                        if (!in_idchange) {
+                            facilities_request(facilities.zmq.responders[i].socket, buffer + 1, rl-1);
+
+                            pthread_mutex_lock(&facilities.id.change.lock);
+                            facilities.id.change.stage = ID_CHANGE_INACTIVE;
+                            pthread_mutex_unlock(&facilities.id.change.lock);
+                        } else {
+                            code = 1;
+                            itss_0send(facilities.zmq.responders[i].socket, &code, 1);
+                        }
+
+                        break;
+
+                    case ITSS_MANAGEMENT:
+                        management_indication(facilities.zmq.responders[i].socket, buffer + 1, rl-1);
+                        break;
+
+                    case ITSS_SECURITY:
+                        security_indication(facilities.zmq.responders[i].socket, buffer + 1, rl-1);
+                        break;
+
+                    case ITSS_STATUS:
+                        code = 0;
+                        itss_0send(facilities.zmq.responders[i].socket, &code, 1);
+                        break;
+
+                    default:
+                        log_debug("<- unrecognized service");
+                        code = 1;
+                        itss_0send(facilities.zmq.responders[i].socket, &code, 1);
+                        continue;
+                }
+            }
+        }
+    }
+
+    // Exit
+cleanup:
+    if (facilities.evm_args.activate)
+        pthread_join(facilities.evcsn_service, NULL);
+    pthread_join(facilities.ca_service, NULL);
+    pthread_join(facilities.den_service, NULL);
+    pthread_join(facilities.infrastructure_service, NULL);
+    if (facilities.dissemination.active)
+        pthread_join(facilities.cp_service, NULL);
+    pthread_join(facilities.sa_service, NULL);
+    itss_queue_trigger(facilities.tx_queue);
+    pthread_join(facilities.transmitting, NULL);
+    if (facilities.coordination.active)
+        pthread_join(facilities.vc_service, NULL);
+    if (facilities.edm.enabled) {
+        itss_0close(facilities.edm.app_socket);
+    }
+    if (facilities.edm.enabled) {
+        itss_0close(facilities.edm.app_socket);
+    }
+    itss_0close(facilities.apps_socket);
+
+    itss_0close(security_socket);
+    for (int i = 0; i < facilities.zmq.n_responders; ++i) {
+        itss_0close(facilities.zmq.responders[i].socket);
+    }
+    itss_0destroy();
+
+    log_info("exiting");
+    log_close();
+
+    return 0;
+}

backup/src/facilities.h

@@ -0,0 +1,132 @@
+#ifndef FACILITIES_H
+#define FACILITIES_H
+
+#include <pthread.h>
+#include <stdbool.h>
+#include <zmq.h>
+
+#include "cam.h"
+#include "denm.h"
+#include "infrastructure.h"
+#include "cpm.h"
+#include "saem.h"
+#include "tpm.h"
+#include "vcm.h"
+#include "evm.h"
+#include "edm.h"
+#include "mcm.h"
+
+#include <it2s-tender/epv.h>
+#include <it2s-tender/database.h>
+#include <it2s-tender/log.h>
+#include <it2s-tender/constants.h>
+#include <it2s-tender/queue.h>
+
+/*
+ * IT2S-GN defines a maximum packet size equal to 2360 bytes.
+ * For indications to the networking layer, a maximum of 2360 bytes for data + 512 bytes for other fields is set.
+ */
+#define INDICATION_BUFFER_SIZE 2560
+
+enum ID_CHANGE_STAGE {
+    ID_CHANGE_INACTIVE,
+    ID_CHANGE_BLOCKED,
+    ID_CHANGE_PREPARE,
+    ID_CHANGE_COMMIT
+};
+
+typedef struct facilities {
+    pthread_t ca_service;
+    pthread_t den_service;
+    pthread_t infrastructure_service;
+    pthread_t transmitting;
+    pthread_t cp_service;
+    pthread_t sa_service;
+    pthread_t vc_service;
+    pthread_t evcsn_service;
+    pthread_t mcm_service;
+
+    // ZMQ
+    struct {
+        zmq_pollitem_t* responders;
+        uint16_t n_responders;
+
+        char* networking_address;
+        char* applications_address;
+        char* security_address;
+        char* management_address;
+    } zmq;
+
+    // Transmitter
+    itss_queue_t* tx_queue;
+    void* apps_socket; /* alternative to tx queue, only used in rx/main thread */
+
+    struct {
+        uint8_t defaultv;
+        bool handle_all;
+    } pver;
+
+    // CA
+    lightship_t lightship;
+
+    // DEN
+    den_t den;
+
+    // Infrastructure
+    infrastructure_t infrastructure;
+
+    // CPM 
+    dissemination_t dissemination;
+
+    // SA
+    bulletin_t bulletin;
+
+    // TP
+    tolling_t tolling;
+
+    // VC
+    coordination_t coordination;
+
+    // EVCSN
+    evm_args_t evm_args;
+
+    // EDM
+    edm_t edm;
+
+    // MCM
+    mcm_coord_t mcm_coord;
+
+    // Logging
+    struct {
+        bool recorder;
+        itss_db_t* dbms;
+    } logging;
+
+    int station_type;
+    bool use_security;
+    bool replay;
+    bool upf; 
+
+    struct {
+        uint16_t width; 
+        uint16_t length;
+        uint8_t role;
+    } vehicle;
+
+    struct {
+        pthread_mutex_t lock;
+        uint32_t station_id;
+        uint8_t ipv6_addr[16];
+        struct {
+            pthread_mutex_t lock;
+            bool random;
+            int stage;
+        } change;
+    } id; 
+
+    bool exit;
+} facilities_t;
+
+extern facilities_t facilities;
+
+#endif

src/CMakeLists.txt

@@ -23,13 +23,13 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
     -lit2s-asn-etsi-its-sdu-cdd-1.3.1
     -lzmq
     -lit2s_gnss
+    -lit2s-radar
     -lpthread
     -lit2s-config-etsi-its
     -lit2s-asn-etsi-its-v1-cdd-1.3.1
     -lit2s-asn-etsi-its-v1-cam
     -lit2s-asn-etsi-its-v1-ivim
     -lit2s-asn-etsi-its-v1-denm
-    -lit2s-asn-etsi-its-v1-cpm
     -lit2s-asn-etsi-its-v1-saem
     -lit2s-asn-etsi-its-v1-tpm
     -lit2s-asn-etsi-its-v1-vcm
@@ -39,6 +39,8 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
     -lit2s-asn-etsi-its-v2-cam
     -lit2s-asn-etsi-its-v2-denm
     -lit2s-asn-etsi-its-v2-mcm
+    -lit2s-asn-etsi-its-v2-cpm
+    -lit2s-asn-etsi-its-v2-ldm
     -lit2s-tender
     -lm
     -lrt

src/config.c

@@ -19,6 +19,10 @@
 
 #include <it2s-tender/packet.h>
 
+#include <it2s-radar/constants.h>
+#include <it2s-radar/t2c-radar.h>
+#include <it2s-radar/umrr0c.h>
+
 #include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
 #include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementReply.h>
 #include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
@@ -55,6 +59,7 @@ int facilities_config() {
 
     t2c_itss_t* itss_cfg  = NULL;
     t2c_etsi_its_t* etsi_its_cfg  = NULL;
+    t2c_radar_t* radar_cfg = NULL;
 
     rv = t2c_itss_read("/etc/it2s/itss.toml", &itss_cfg);
     if (rv) {
@@ -66,6 +71,11 @@ int facilities_config() {
         syslog_error("[config] read ETSI ITS config failed");
         goto cleanup;
     }
+    rv = t2c_radar_read("/etc/it2s/radar.toml", &radar_cfg);
+    if (rv) {
+        syslog_error("[config] read radar config failed");
+        radar_cfg = NULL;  // goto cleanup; // radar is optional
+    }
 
     // Logging - status messages
     bool use_syslog = false, use_std = false, use_file = false;
@@ -343,22 +353,53 @@ int facilities_config() {
     facilities.infrastructure.default_service_duration = etsi_its_cfg->facilities.ivim.default_service_duration * 60000;
 
     // CPM
-    facilities.dissemination.active = etsi_its_cfg->facilities.cpm.activate;
-    facilities.dissemination.T_GenCpmMin = etsi_its_cfg->facilities.cpm.rsu_obu_period_min;
-    facilities.dissemination.T_GenCpmMax = etsi_its_cfg->facilities.cpm.rsu_obu_period_max;
-    // Rotation not needed anymore?
-    //facilities.dissemination.radar_rotation = itss_cfg->radar.radar_rotation;
-    facilities.dissemination.tmc_connect = etsi_its_cfg->facilities.cpm.tmc_connected;
-    facilities.dissemination.T_AddSensorInformation = 1000;
-    int oa_start = (360-facilities.dissemination.radar_rotation) * 10 - 500;
-    int oa_end = (360-facilities.dissemination.radar_rotation) * 10 + 500;
-    facilities.dissemination.opening_angle_start = (oa_start + 3600) % 3600;
-    facilities.dissemination.opening_angle_end = (oa_end + 3600) % 3600;
+    facilities.cp.active = etsi_its_cfg->facilities.cpm.activate;
+    facilities.cp.T_GenCpm = etsi_its_cfg->facilities.cpm.t_gen_cpm;
 
-    facilities.dissemination.int_radar = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_interface)+1);
-    strcpy(facilities.dissemination.int_radar,etsi_its_cfg->facilities.cpm.radar_interface);
-    facilities.dissemination.ip_radar  = malloc(strlen(etsi_its_cfg->facilities.cpm.radar_ip)+1);
-    strcpy(facilities.dissemination.ip_radar,etsi_its_cfg->facilities.cpm.radar_ip);
+    // CP - LDM connection
+    facilities.cp.ldm.active = etsi_its_cfg->facilities.ldm.activate;
+    facilities.cp.ldm.registered = false;
+    index = fetch_target_address(etsi_its_cfg->facilities.ldm.zmq.addresses, etsi_its_cfg->facilities.ldm.zmq.addresses_len);
+    if (index != -1) {
+        facilities.cp.ldm.address = malloc(strlen(etsi_its_cfg->facilities.ldm.zmq.addresses[index]) + 1);
+        strcpy(facilities.cp.ldm.address, etsi_its_cfg->facilities.ldm.zmq.addresses[index]);
+    } else {
+        log_error("[config] a valid address for [ldm] was not found");
+        rv = 1;
+        goto cleanup;
+    }
+
+    // CPM - Register UMRR0C radar
+    if (radar_cfg && radar_cfg->umrr0c.activate) {
+        EI2_SensorInformation_t *si = calloc(1, sizeof(EI2_SensorInformation_t));
+
+        si->sensorId = UMRR0CId;
+        si->sensorType = EI2_SensorType_radar;
+        si->perceptionRegionShape = calloc(1, sizeof(EI2_Shape_t));
+        si->perceptionRegionShape->present = EI2_Shape_PR_radial;
+        si->perceptionRegionShape->choice.radial.range = 3400;
+        int hoa = (360 - radar_cfg->umrr0c.rotation) * 10 - 500;
+        si->perceptionRegionShape->choice.radial.horizontalOpeningAngleStart = (hoa + 3600) % 3600;
+        hoa = (360 - radar_cfg->umrr0c.rotation) * 10 + 500;
+        si->perceptionRegionShape->choice.radial.horizontalOpeningAngleEnd = (hoa + 3600) % 3600;
+        si->perceptionRegionShape->choice.radial.verticalOpeningAngleStart = calloc(1, sizeof(EI2_CartesianAngleValue_t));
+        *(si->perceptionRegionShape->choice.radial.verticalOpeningAngleStart) = 1730;
+        si->perceptionRegionShape->choice.radial.verticalOpeningAngleEnd = calloc(1, sizeof(EI2_CartesianAngleValue_t));
+        *(si->perceptionRegionShape->choice.radial.verticalOpeningAngleEnd) = 1890;
+        si->perceptionRegionShape->choice.radial.shapeReferencePoint = calloc(1, sizeof(EI2_CartesianPosition3d_t));
+        si->perceptionRegionShape->choice.radial.shapeReferencePoint->xCoordinate = 0;
+        si->perceptionRegionShape->choice.radial.shapeReferencePoint->yCoordinate = 0;
+        si->perceptionRegionShape->choice.radial.shapeReferencePoint->zCoordinate = calloc(1, sizeof(EI2_CartesianCoordinate_t));
+        *(si->perceptionRegionShape->choice.radial.shapeReferencePoint->zCoordinate) = radar_cfg->umrr0c.height;
+
+        if (facilities.cp.si_count >= MAX_SENSORS) {
+            log_error("failed to register UMRR0-C");
+            ASN_STRUCT_FREE(asn_DEF_EI2_SensorInformation, si);
+        } else {
+            facilities.cp.si_arr[facilities.cp.si_count] = si;
+            facilities.cp.si_count++;
+        }
+    }
 
     // TPM
     facilities.tolling.enabled = etsi_its_cfg->facilities.tpm.activate;
@@ -584,6 +625,7 @@ int facilities_config() {
 cleanup:
     t2c_itss_free(itss_cfg);
     t2c_etsi_its_free(etsi_its_cfg);
+    t2c_radar_free(radar_cfg);
 
     return rv;
 }

src/cpm.h

@@ -1,187 +1,210 @@
+/**
+ * @file cpmv2.h
+ * @author Diogo Jesus (diogopjesus@ua.pt)
+ * @brief Collective Perception Service implementation.
+ */
+
 #ifndef FACILITIES_CPM_H
 #define FACILITIES_CPM_H
 
-
-#include <arpa/inet.h>
+#include <it2s-asn/etsi-its-v2/cpm/EI2_CollectivePerceptionMessage.h>
+#include <it2s-radar/it2s-radar.h>
+#include <pthread.h>
 #include <stdbool.h>
+#include <stdint.h>
 
-#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
-#include <termios.h>
-#include <unistd.h>
-#include <time.h>
-#include <net/if.h>
-#include <linux/if_packet.h>
+#define MAX_SENSORS \
+    128  ///< Maximum number of sensors allowed to exist inside the sensing system. (128 is the standard defined value for the maximum
+         ///< number of sensors in a CPM)
+#define MAX_PERCEPTION_REGIONS \
+    256  ///< Maximum number of perception regions allowed to be retrived from the LDM. (8 is standard defined value for the maximum number
+         ///< of perception regions in a CPM)
+#define MAX_PERCEIVED_OBJECTS \
+    1024  ///< Maximum number of perceived objects allowed to be retrieved from the LDM. (arbitrary value to help with memory allocation).
 
-#define READ_BUFFER_SIZE 1024     // Buffer for Serial and Ethernet read function (Used in cp_service)
-#define NOF_OBJECTS 127  	  // Number of Object UMRR-0C
-#define MESSAGE_ID 14
-#define PROTOCOL_VERSION 1
-#define RADAR_PORT "55555"	  // Destination port from the radar
+#define TOTAL_PO_IDS (1 << 16)           ///< Total number of possible object identifiers.
+#define UNUSED_PO_ID (TOTAL_PO_IDS - 1)  ///< Value used to indicate an unused object identifier (last identifier).
+#define TOTAL_SI_IDS (1 << 8)            ///< Total number of possible sensor identifiers.
+#define UNUSED_SI_ID (TOTAL_SI_IDS - 1)  ///< Value used to indicate an unused sensor identifier (last identifier).
 
+/**
+ * @brief CPS configuration struct.
+ *
+ * Variable names match the standard ts_103324v020101.
+ * For more information regarding each field, please refer to Annex F of the standard.
+ *
+ * Parameter names follow the standard (Annex F).
+ * Type matching:
+ *   `Time(ms)` to `uint64_t`,
+ *   `Number` to `uint8_t`,
+ *   `Identifier` to `uint8_t`,
+ *   `Ratio` to `double`.
+ *   `Distance(m)` to `double`.
+ *   `Speed(m/s)` to `double`.
+ *   `Orientation(degrees)` to `double`.
+ *   `Time(s)` to `uint64_t`.
+ *
+ * @note In the standard the `UnusedSensorIdRetentionPeriod` and `UnusedObjectIdRetentionPeriod` are defined
+ * in seconds (`Time(s)`). However, to maintain consistency, this values will be treated as `Time(ms)` instead.
+ */
+typedef struct cp_config {
+    uint64_t T_GenCpmMin;             ///< Lower limit for the time T_GenCpm elapsed between two consecutive CPM generation events.
+    uint64_t T_GenCpmMax;             ///< Upper limit for the time T_GenCpm elapsed between two consecutive CPM generation events.
+    uint64_t T_AddSensorInformation;  ///< Time after which the SensorInformationContainer is usually included again into a CPM
 
-/* Structures */
+    uint8_t MaxPerceptionRegions;   ///< Maximum number of perception regions included into a CPM to constrain the message size
+    uint8_t ObjectInclusionConfig;  ///< CPS configuration parameter that indicates the object inclusion management to be employed
+                                    ///< by the disemminating ITS-S.
 
-typedef struct
-{
-  int i32_socket;  // Socket descriptor server
-  int i32_client;  // Socket descriptor UMRR-0C
-  int i32_port;    // Listen on Port
-  bool b_moxa;
-  struct sockaddr_in s_server;
-  struct sockaddr_in s_client;
-} S_ETHERNET_CONNECTION_T;
+    double ObjectPerceptionQualityThreshold;  ///< Threshold of the objectPerceptionQuality defining the lower limit for objects
+                                              ///< to be included into a CPM.
+    double minPositionChangeThreshold;        ///< Minimum Euclidean distance between the current estimated position of the reference
+                                              ///< point of a perceived object and the estimated position of the reference point lastly
+                                              ///< included in a CPM
+    double minGroundSpeedChangeThreshold;     ///< Minimum difference between the current estimated ground speed of the reference
+                                              ///< point of a perceived object and the estimated ground speed of the reference point
+                                              ///< lastly included in a CPM
+    double minGroundVelocityOrientationChangeThreshold;  ///< Minimum difference between the current estimated ground velocity
+                                                         ///< orientation of the reference point of a perceived object and the
+                                                         ///< estimated ground velocity orientation of the reference point lastly
+                                                         ///< included in a CPM
 
-typedef struct
-{
-  int raw_fd;
-  struct sockaddr_ll sll;
-  struct ifreq ifr;
-} S_INTERFACE_CONNECTION_T;
+    uint8_t MessageAssemblyConfig;  ///< CPS configuration parameter that indicates the CPM assembly mechanism to be employed by
+                                    ///< the disemminating ITS-S
 
-typedef struct
-{
-  u_int8_t u8_numberOfCountedObjects;
-  u_int32_t u32_UnixTime;
-  u_int16_t u16_Milliseconds;
-  u_int8_t u8_SensorNetworkID;
-  u_int8_t u8_ObjectNumber;
-  u_int8_t u8_ObjectID;
-  int16_t i16_speed;
-  u_int8_t u8_class;
-  u_int8_t u8_mLineNumber;
-  u_int16_t u8_laneNumber;
-} S_PVR_T;
+    double minPositionChangePriorityThreshold;     ///< Lower limit for which the Euclidean distance between the current estimated
+                                                   ///< position of the reference point of the object and the estimated position of
+                                                   ///< the reference point of this object lastly included in a CPM starts to
+                                                   ///< contribute to the object utility function
+    double maxPositionChangePriorityThreshold;     ///< Upper limit for which the Euclidean distance between the current estimated
+                                                   ///< position of the reference point of the object and the estimated position of
+                                                   ///< the reference point of this object lastly included in a CPM starts to
+                                                   ///< contribute to the object utility function
+    double minGroundSpeedChangePriorityThreshold;  ///< Lower limit for which the difference between the current estimated ground
+                                                   ///< speed of the reference point of the object and the estimated ground speed
+                                                   ///< of the reference point of this object lastly included in a CPM starts to
+                                                   ///< contribute to the object utility function
+    double maxGroundSpeedChangePriorityThreshold;  ///< Upper limit for which the difference between the current estimated ground
+                                                   ///< speed of the reference point of the object and the estimated ground speed
+                                                   ///< of the reference point of this object lastly included in a CPM starts to
+                                                   ///< contribute to the object utility function
+    double minGroundVelocityOrientationChangePriorityThreshold;  ///< Lower limit for which the difference between the current
+                                                                 ///< estimated ground velocity orientation of the reference point
+                                                                 ///< of the object and the estimated ground velocity orientation
+                                                                 ///< of the reference point of this object lastly included in a
+                                                                 ///< CPM starts to contribute to the object utility function
+    double maxGroundVelocityOrientationChangePriorityThreshold;  ///< Upper limit for which the difference between the current
+                                                                 ///< estimated ground velocity orientation of the reference point
+                                                                 ///< of the object and the estimated ground velocity orientation
+                                                                 ///< of the reference point of this object lastly included in a
+                                                                 ///< CPM starts to contribute to the object utility function
+    uint64_t minLastInclusionTimePriorityThreshold;              ///< Lower limit for which the time elapsed since the last inclusion of the
+                                                                 ///< object into a CPM starts to contribute to the object utility function
+    uint64_t maxLastInclusionTimePriorityThreshold;              ///< Upper limit for which the time elapsed since the last inclusion of the
+                                                                 ///< object into a CPM starts to contribute to the object utility function
 
-typedef struct
-{
-  u_int8_t u8_sensorStatus;
-  u_int8_t u8_InterfaceMode;
-  u_int8_t u8_networkID;
-  u_int8_t u8_diagnose;
-  u_int32_t u32_time;
-} S_SENSOR_CONTROL_T;
+    uint64_t UnusedSensorIdRetentionPeriod;  ///< Minimum time to ellapse before an ID used to identify a sensor can be reused
+                                             ///< after it has last been used
+    uint64_t UnusedObjectIdRetentionPeriod;  ///< Minimum time to ellapse before an ID used to identify an object can be reused
+                                             ///< after it has last been used
+} cp_config_t;
 
-typedef struct
-{
-  u_int8_t u8_numberOfObjects;
-  u_int8_t u8_numberOfMessages;
-  u_int8_t u8_cycleDuration;
-  u_int8_t u8_objectData0Format;
-  u_int8_t u8_objectData1Format;
-  u_int32_t u32_cycleCount;
-} S_OBJECT_CONTROL_T;
+/**
+ * @brief CP Perceived Object History struct.
+ *
+ * Contains data related to the inclusion management of objects into a CPM.
+ */
+typedef struct cp_history {
+    uint64_t* inclusion;  ///< Timestamp of the last inclusion of an object into a CPM. (milliseconds)
+    struct {
+        double latitude;   // degrees
+        double longitude;  // degrees
+    }* position;           ///< Position of an object at the time of the last inclusion into a CPM. (degrees, degrees)
+    double* speed;         ///< Speed of an object at the time of the last inclusion into a CPM. (metes per second)
+    double* vel_ori;       ///< Velocity orientation of an object at the time of the last inclusion into a CPM. (degrees)
+} cp_history_t;
 
-typedef struct
-{
-  u_int8_t u8_modeSignal;
-  float f_xPoint;
-  float f_yPoint;
-  float f_xSpeed;
-  float f_ySpeed;
-  float f_objectLength;
-  u_int8_t u8_objectID;
-  u_int8_t u8_updateFlag;
-} S_OBJECTS_T;
+/**
+ * @brief Bi-directional mapping of LDM object IDs to CPS object IDs.
+ * @note There is the assumption that the sensor identifier (outside the CPS) does not change.
+ */
+typedef struct cp_obj_id {
+    uint16_t* ldm2cps;  ///< Maps LDM object IDs to CPS object IDs.
+    uint16_t* cps2ldm;  ///< Maps CPS object IDs to LDM object IDs.
 
+    uint16_t* age;        ///< Age associated to an object with a CPS object ID.
+    uint64_t* timestamp;  ///< Timestamp associated to an object with a CPS object ID.
+} cp_obj_id_t;
 
-enum state_t {
-  IDLE,
-  START_1,
-  START_2,
-  START_3,
-  START_4,
-  START_PATTERN,
-  PROTOCOLVERSION,
-  HEADERLENGTH,
-  PAYLOADLENGTH_H,
-  PAYLOADLENGTH_L,
-  APPPROTOCOLTYPE,
-  FLAG_1_H,
-  FLAG_1_L,
-  FLAG_2_H,
-  FLAG_2_L,
-  CRC16HEADER_H,
-  CRC16HEADER_L,
-  PAYLOAD_DATA,
-  CAN_ID_H,
-  CAN_ID_L,
-  CAN_LEN,
-  CAN_PAYLOAD,
-  PAYLOAD_CRC16_H,
-  PAYLOAD_CRC16_L
-};
+/**
+ * @brief Bi-directional mapping of sensor IDs (ouside the CPS) to CPS sensor IDs.
+ */
+typedef struct cp_sensor_id {
+    uint8_t* sen2cps;  ///< Maps sensor IDs (outside CPS) to CPS sensor IDs.
+    uint8_t* cps2sen;  ///< Maps CPS sensor IDs to sensor IDs (outside CPS).
 
-typedef struct
-{
+    uint64_t* timestamp;  ///< Timestamp associated to a sensor with a CPS sensor ID.
+} cp_sensor_id_t;
 
-   bool active;
-   bool tmc_connect;
-   char* int_radar;
-   char* ip_radar;
+/**
+ * @brief Main CPS struct.
+ */
+typedef struct cp {
+    bool active;  ///< Flag to signal the CPS is active.
 
-   pthread_mutex_t lock;
+    pthread_mutex_t lock;  ///< Mutex to protect the CP struct.
 
-   uint32_t type;
+    uint64_t T_GenCpm;  ///< CPM generation period.
 
-   uint64_t next_cpm_max;
-   uint64_t next_cpm_min;
-   
+    uint64_t lastCpmTimestamp;  ///< Timestamp of the last CPM sent.
+    uint64_t lastSicTimestamp;  ///< Timestamp of the last SIC sent.
 
-   uint64_t T_GenCpmMin;
-   uint64_t T_GenCpmMax;
+    cp_config_t config;    ///< CPS configuration.
+    cp_history_t history;  ///< CPS history.
 
-   uint64_t T_AddSensorInformation;
-   uint64_t next_AddSensorInformation;
+    EI2_SensorInformation_t** si_arr;  ///< Array of sensor information.
+    size_t si_count;                   ///< Number of sensor information in the sensor information array.
 
+    EI2_PerceptionRegion_t** pr_arr;  ///< Array of perception regions.
+    size_t pr_count;                  ///< Number of perception regions in the perception region array.
 
-/* Position of the radar (Value from toml)  */
-   
-   int64_t radar_rotation;
-   int16_t opening_angle_start;
-   int16_t opening_angle_end;
- 
-} dissemination_t;
+    EI2_PerceivedObject_t** po_arr;  ///< Array of perceived objects.
+    size_t po_count;                 ///< Number of perceived objects in the perceived object array.
 
-/* Prototype Functions */
+    size_t po_total;  ///< Total number of perceived objects in the LDM (as per standard)
 
-/* 
-Summary : Waits for client to connect (Radar)
-*/
+    cp_sensor_id_t sen_id;  ///< Sensor ID mapping.
+    cp_obj_id_t obj_id;     ///< Object ID mapping.
 
-bool waitingIncomingConnection(void);
+    struct {
+        bool active;
+        bool registered;
+        char* address;
+    } ldm;
+} cp_t;
 
+/**
+ * @brief Initialize the main CP struct.
+ *
+ * @return Always zero.
+ */
+int cp_init();
 
+/**
+ * @brief Check if a CPM should be sent.
+ *
+ * Also checks if the CPS should reset the ID mappings after a pseudonym change (reset_id flag).
+ *
+ * @arg now Current time in milliseconds.
+ * @return true if a CPM should be sent, false otherwise.
+ */
+int cp_check(uint64_t now);
 
-/* 
-Summary : Creation of socket and binding to the radar add and port
-	param [in] char* radar_ip: Radar IP
-	param [in] char* radar_port: Radar PORT 
-*/
+/**
+ * @brief Main CP service.
+ *
+ * @return NULL
+ */
+void* cp_service();
 
-bool initEthernetConnection(const char* radar_ip,const char* radar_port);
-
-
-
-/* 
-Summary : Initialization for dissemination control. (Memory allocation and mutex init
-*/
-
-dissemination_t* dissemination_init();
-
-
-/* 
-Summary : Creation of socket and binding to the radar add and port
-	param [in] char* radar_ip: Radar IP
-	param [in] char* radar_port: Radar PORT 
-*/
-
-bool initEthernetConnection(const char* radar_ip,const char* radar_port);
-
-/*
-Summary: Read from Radar socket (s_socket.i32_socket) call the function to interpret data 
- param[in] void	
-*/
-
-void* cp_service(void *arg);
-
-#endif
+#endif  // FACILITIES_CPM_H

src/facilities.c

@@ -1,7 +1,6 @@
 #include "facilities.h"
 
 #include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
-#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
 #include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h>
 #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
 #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
@@ -17,6 +16,7 @@
 #include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
 #include <it2s-asn/etsi-its-v1/ivim/EI1_IVIM.h>
 #include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
+#include <it2s-asn/etsi-its-v2/cpm/EI2_CollectivePerceptionMessage.h>
 #include <it2s-tender/packet.h>
 #include <it2s-tender/recorder.h>
 #include <it2s-tender/space.h>
@@ -367,7 +367,7 @@ int main() {
     lightship_init();
     den_init();
     infrastructure_init();
-    dissemination_init();
+    cp_init();
     bulletin_init();
     void *security_socket = NULL;
 
@@ -390,8 +390,7 @@ int main() {
     pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL);
 
     // CP
-    if (facilities.dissemination.active)
-        pthread_create(&facilities.cp_service, NULL, cp_service, NULL);
+    pthread_create(&facilities.cp_service, NULL, cp_service, NULL);
 
     // SA
     pthread_create(&facilities.sa_service, NULL, sa_service, NULL);
@@ -514,8 +513,7 @@ cleanup:
     pthread_join(facilities.ca_service, NULL);
     pthread_join(facilities.den_service, NULL);
     pthread_join(facilities.infrastructure_service, NULL);
-    if (facilities.dissemination.active)
-        pthread_join(facilities.cp_service, NULL);
+    pthread_join(facilities.cp_service, NULL);
     pthread_join(facilities.sa_service, NULL);
     itss_queue_trigger(facilities.tx_queue);
     pthread_join(facilities.transmitting, NULL);

src/facilities.h

@@ -76,7 +76,7 @@ typedef struct facilities {
     infrastructure_t infrastructure;
 
     // CPM 
-    dissemination_t dissemination;
+    cp_t cp;
 
     // SA
     bulletin_t bulletin;

src/mcm.c

@@ -175,7 +175,7 @@ void *mc_service(void *arg){
     mcm_coord_t *mcm_coord = (mcm_coord_t *)&facilities.mcm_coord;
 
     while (!facilities.exit){
-        sleep(1);
+        itss_sleep(1);
         
         pthread_mutex_lock(&mcm_coord->lock);
         rv = mk_mcm();

src/requests.c

@@ -1,6 +1,6 @@
 #include "requests.h"
 
-#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
+#include <it2s-asn/etsi-its-v2/cpm/EI2_CollectivePerceptionMessage.h>
 #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
 #include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
 #include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
@@ -125,8 +125,8 @@ int facilities_request_single_message(void *responder, EIS_FacilitiesMessageRequ
             break;
 
         case EIS_ItsMessageType_cpm:
-            its_msg_def = &asn_DEF_EI1_CPM;
-            its_msg = calloc(1, sizeof(EI1_CPM_t));
+            its_msg_def = &asn_DEF_EI2_CollectivePerceptionMessage;
+            its_msg = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t));
             npr->transport.choice.btp.destinationPort = EIS_Port_cpm;
             npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
             npr->network.choice.gn.trafficClass = 2;
@@ -710,8 +710,8 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
         break;
 
     case EIS_Port_cpm:
-        its_msg_descriptor = &asn_DEF_EI1_CPM;
-        its_msg = calloc(1, sizeof(EI1_CPM_t));
+        its_msg_descriptor = &asn_DEF_EI2_CollectivePerceptionMessage;
+        its_msg = calloc(1, sizeof(EI2_CollectivePerceptionMessage_t));
         its_msg_type = 14;
         break;