diff --git a/.vscode/settings.json b/.vscode/settings.json index 2030e35..93f6441 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -3,6 +3,16 @@ "packet.h": "c", "managementrequest.h": "c", "facilitiesindication.h": "c", - "constr_sequence.h": "c" + "constr_sequence.h": "c", + "btppacketindication.h": "c", + "transportrequest.h": "c", + "evcsnm.h": "c", + "mman.h": "c", + "space.h": "c", + "evcsnpdu.h": "c", + "asn_application.h": "c", + "itspduheader.h": "c", + "evchargingspotnotificationpoimessage.h": "c", + "vcm.h": "c" } } \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f11be2c..09d3cac 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,6 +10,7 @@ ADD_EXECUTABLE(it2s-itss-facilities saem.c tpm.c vcm.c + evcsnm.c ) TARGET_LINK_LIBRARIES(it2s-itss-facilities @@ -28,6 +29,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-saem -lit2s-asn-tpm -lit2s-asn-vcm + -lit2s-asn-evcsnm -lit2s-asn-verco -lit2s-tender -lit2s-obd diff --git a/src/config.c b/src/config.c index b9cbf14..98b5617 100644 --- a/src/config.c +++ b/src/config.c @@ -394,6 +394,9 @@ int facilities_config() { facilities.coordination.vcm_period_min = config->facilities.mcm.period_min; facilities.coordination.vcm_period_max = config->facilities.mcm.period_max; + // EVCSNM + facilities.evcsnm_args.activate = config->facilities.evcsnm.activate; + // Replay facilities.replay = config->networking.replay.activate; diff --git a/src/evcsnm.c b/src/evcsnm.c index f63981a..179c660 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -21,71 +21,19 @@ #include #include -#include - -#define EARTH_RADIUS 6369000.0 -#define RAD_PER_DEG M_PI / 180.0 - -const cid_ssp_bm_t CID_SSP_BM_MAP[] = { - {"CenDsrcTollingZone/ProtectedCommunicationZonesRSU", 0x8000}, - {"publicTransport/publicTransportContainer", 0x4000}, - {"specialTransport/specialTransportContainer", 0x2000}, - {"dangerousGoods/dangerousGoodsContainer", 0x1000}, - {"roadwork/roadWorksContainerBasic", 0x0800}, - {"rescue/rescueContainer", 0x0400}, - {"emergency/emergencyContainer", 0x0200}, - {"safetyCar/safetyCarContainer", 0x0100}, - {"closedLanes/RoadworksContainerBasic", 0x0080}, - {"requestForRightOfWay/EmergencyContainer: EmergencyPriority", 0x0040}, - {"requestForFreeCrossingAtATrafficLight/EmergencyContainer: EmergencyPriority", 0x0020}, - {"noPassing/SafetyCarContainer: TrafficRule", 0x0010}, - {"noPassingForTrucks/SafetyCarContainer: TrafficRule", 0x0008}, - {"speedLimit/SafetyCarContainer", 0x0004}, - {"reserved0", 0x0002}, - {"reserved1", 0x0001}, -}; - -static int permissions_check(int cid, uint8_t *permissions, uint8_t permissions_len) +static UTF8String_t *create_empty_utf8_string() { - /* evcsnm SSP scheme - * - * byte | description - * --------------------------------- - * 0 | SSP version control - * 1-2 | Service-specific parameter - * 3-30 | Reserved - */ - - if (permissions_len < 3) - { - log_debug("[ca] permissions length too small"); - return 0; - } - - uint16_t perms_int = *(uint16_t *)(permissions + 1); - - uint16_t perm_val = CID_SSP_BM_MAP[cid].bitmap_val; - perm_val = (perm_val >> 8) | (perm_val << 8); - - if ((perm_val & perms_int) == perm_val) - return 1; - else - return 0; + UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t)); + utf8_string->buf = calloc(1, 1); + utf8_string->size = 0; + utf8_string->buf[0] = '\0'; + return utf8_string; } static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) { int rv = 0; int shm_fd, shm_valid = 0; - it2s_obd_data *shared_message; - - lightship_t *lightship = &facilities.lightship; - - if (lightship->use_obd) - { - it2s_obd_data *shared_message = malloc(sizeof(it2s_obd_data)); - it2s_obd_read(shared_message); - } EvcsnPdu_t *evcsnm = calloc(1, sizeof(EvcsnPdu_t)); evcsnm->header.protocolVersion = 2; @@ -97,40 +45,46 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) uint64_t now = itss_time_get(); evcsnm->evcsn.poiHeader.poiType = 1; // set to "EV charging station POI ID = 1" - asn_ulong2INTEGER(&evcsnm->evcsn.poiHeader.timeStamp, now % 65536); + asn_ulong2INTEGER(&evcsnm->evcsn.poiHeader.timeStamp, now); evcsnm->evcsn.poiHeader.relayCapable = 0; - pthread_mutex_lock(&lightship->lock); - if (facilities.station_type != StationType_roadSideUnit) - { - itss_space_lock(); - itss_space_get(); - uint16_t lat_conf = epv.space.latitude_conf; - uint16_t lon_conf = epv.space.longitude_conf; + evcsnm->evcsn.evcsnData.totalNumberOfStations = 1; + evcsnm->evcsn.evcsnData.chargingStationsData.list.array = calloc(1, sizeof(void *)); + evcsnm->evcsn.evcsnData.chargingStationsData.list.count = 1; + evcsnm->evcsn.evcsnData.chargingStationsData.list.size = 1; + evcsnm->evcsn.evcsnData.chargingStationsData.list.array[0] = calloc(1, sizeof(struct ItsChargingStationData)); + struct ItsChargingStationData *cs0 = evcsnm->evcsn.evcsnData.chargingStationsData.list.array[0]; + cs0->chargingStationID = 0; + itss_space_lock(); + itss_space_get(); + cs0->chargingStationLocation.latitude = epv.space.latitude; + cs0->chargingStationLocation.longitude = epv.space.longitude; + cs0->chargingStationLocation.altitude.altitudeValue = epv.space.altitude; + cs0->chargingStationLocation.altitude.altitudeConfidence = epv.space.altitude_conf; + cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + cs0->chargingStationLocation.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorOrientation = SemiAxisLength_unavailable; + cs0->accessibility = *create_empty_utf8_string(); + cs0->pricing = *create_empty_utf8_string(); + cs0->openingDaysHours = *create_empty_utf8_string(); + cs0->chargingSpotsAvailable.list.array = calloc(1, sizeof(void *)); + cs0->chargingSpotsAvailable.list.count = 1; + cs0->chargingSpotsAvailable.list.size = 1; + cs0->chargingSpotsAvailable.list.array[0] = calloc(1, sizeof(struct ItsChargingSpotDataElements)); + struct ItsChargingSpotDataElements *cs_elem0 = cs0->chargingSpotsAvailable.list.array[0]; + cs_elem0->energyAvailability = *create_empty_utf8_string(); + cs_elem0->type.buf = calloc(1, sizeof(uint8_t)); + cs_elem0->type.size = 1; + cs_elem0->type.bits_unused = 24; + cs_elem0->type.buf[0] = 0; + cs_elem0->typeOfReceptacle.buf = calloc(1, sizeof(uint8_t)); + cs_elem0->typeOfReceptacle.size = 1; + cs_elem0->typeOfReceptacle.bits_unused = 24; - itss_space_unlock(epv); - // Reference position ellipse - //if (lat_conf > lon_conf) - //{ - // bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf; - // bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf; - // bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0; - //} - //else - //{ - // bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf; - // bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf; - // bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900; - //} - - - } - else - { - - } - - pthread_mutex_unlock(&lightship->lock); + itss_space_unlock(epv); + // if (facilities.station_type == StationType_roadSideUnit) + //{ + // } asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EvcsnPdu, NULL, evcsnm, evcsnm_oer, 512); if (enc.encoded == -1) @@ -141,429 +95,22 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) } *evcsnm_len = (enc.encoded + 7) / 8; - if (lightship->use_obd) - free(shared_message); - cleanup: ASN_STRUCT_FREE(asn_DEF_EvcsnPdu, evcsnm); return rv; } -int lightship_init() -{ - - lightship_t *lightship = &facilities.lightship; - - lightship->protected_zones.pz = calloc(256, sizeof(void *)); - pthread_mutex_init(&lightship->lock, NULL); - - int shm_fd; - shm_fd = shm_open("it2s-obd", O_RDONLY, 0666); - if (shm_fd == -1) - { - log_debug("obd shmem not found\n"); - lightship->use_obd = 0; - } - else - { - log_debug("obd shmem found\n"); - lightship->use_obd = 1; - close(shm_fd); - } - +int evcsnm_check(EvcsnPdu_t* evcsnm) { return 0; } -int lightship_check() +enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len) { int rv = 0; - lightship_t *lightship = &facilities.lightship; - - uint64_t now = itss_time_get(); - - pthread_mutex_lock(&lightship->lock); - - if (lightship->type == StationType_roadSideUnit) - { // RSU - if (lightship->is_vehicle_near && now > lightship->next_evcsnm_min) - { - rv = 1; - } - } - else - { // Vehicle - if (now > lightship->next_evcsnm_max) - { - rv = 1; - } - else if (now > lightship->next_evcsnm_min) - { - - itss_space_lock(); - itss_space_get(); - - // Check heading delta > 4ยบ - int diff = epv.space.heading - lightship->last_pos.heading; - if (abs(diff) > 40) - rv = 1; - - if (!rv) - { - // Check speed delta > 0.5 m/s - diff = epv.space.speed - lightship->last_pos.speed; - if (abs(diff) > 50) - rv = 1; - - if (!rv) - { - // Check position delta > 4 m - // TODO make an *accurate* distance calculator using GPS coords - int32_t avg_speed = (epv.space.speed + lightship->last_pos.speed) / 2 / 100; /* cm/s to m/s */ - uint64_t delta_time = (now - lightship->t_last_evcsnm) / 1000; /* ms to s */ - if (avg_speed * delta_time > 4) - rv = 1; - } - } - itss_space_unlock(epv); - } - - // Remove expired PZs - for (int i = 0; i < lightship->protected_zones.pz_len; ++i) - { - uint64_t expiry; - if (lightship->protected_zones.pz[i]->expiryTime) - { - asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, (unsigned long long *)&expiry); - if (now >= expiry) - { - ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]); - for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j) - { - lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j + 1]; - } - --lightship->protected_zones.pz_len; - } - } - } - } - - pthread_mutex_unlock(&lightship->lock); - return rv; } - -void lightship_reset_timer() -{ - - lightship_t *lightship = &facilities.lightship; - - uint64_t now = itss_time_get(); - - pthread_mutex_lock(&lightship->lock); - - if (lightship->type != StationType_roadSideUnit) - { // Vehicle - lightship->next_evcsnm_max = now + lightship->vehicle_gen_max; - lightship->next_evcsnm_min = now + lightship->vehicle_gen_min; - } - else - { // RSU - if (now > lightship->t_last_vehicle + lightship->rsu_vehicle_permanence) - { - lightship->is_vehicle_near = false; - } - lightship->next_evcsnm_min = now + lightship->rsu_gen_min; - } - - pthread_mutex_unlock(&lightship->lock); -} - -enum evcsnm_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, evcsnm_t *evcsnm, uint8_t *ssp, uint32_t ssp_len) -{ - int rv = 0; - lightship_t *lightship = &facilities.lightship; - - uint64_t now = itss_time_get(); - - // Check permissions - if (ssp) - { - if (evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.present == HighFrequencyContainer_PR_rsuContainerHighFrequency && - evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) - { - if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container); - return rv; - } - } - if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer) - { - switch (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->present) - { - case SpecialVehicleContainer_PR_NOTHING: - break; - case SpecialVehicleContainer_PR_publicTransportContainer: - if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container); - return rv; - } - break; - case SpecialVehicleContainer_PR_specialTransportContainer: - if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container); - return rv; - } - break; - case SpecialVehicleContainer_PR_dangerousGoodsContainer: - if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container); - return rv; - } - break; - case SpecialVehicleContainer_PR_roadWorksContainerBasic: - if (!permissions_check(CID_ROADWORK, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container); - return rv; - } - if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) - { - if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container); - return rv; - } - } - break; - case SpecialVehicleContainer_PR_rescueContainer: - if (!permissions_check(CID_RESCUE, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container); - return rv; - } - break; - case SpecialVehicleContainer_PR_emergencyContainer: - if (!permissions_check(CID_EMERGENCY, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container); - return rv; - } - if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority && - evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) - { - // TODO verify bitmap - uint8_t bm = *evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf; - if (bm & 0x02) - { - if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_RIGHT_OF_WAY].container); - return rv; - } - } - if (bm & 0x01) - { - if (!permissions_check(CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT].container); - return rv; - } - } - } - break; - case SpecialVehicleContainer_PR_safetyCarContainer: - if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container); - return rv; - } - if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) - { - switch (*evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) - { - case TrafficRule_noPassing: - if (!permissions_check(CID_NO_PASSING, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container); - return rv; - } - break; - case TrafficRule_noPassingForTrucks: - if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING_FOR_TRUCKS].container); - return rv; - } - break; - default: - break; - } - } - - if (evcsnm->evcsnm.evcsnmParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) - { - if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len)) - { - rv = evcsnm_BAD_PERMISSIONS; - log_debug("[ca] received evcsnm does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPEED_LIMIT].container); - return rv; - } - } - break; - } - } - } - - pthread_mutex_lock(&lightship->lock); - if (lightship->type == StationType_roadSideUnit) - { - // Send evcsnms if vehicles nearby - if (bpi->stationType != StationType_roadSideUnit && bpi->isNeighbour) - { - lightship->t_last_vehicle = now; - lightship->is_vehicle_near = true; - } - } - else - { - // Protected zones - if (evcsnm->evcsnm.evcsnmParameters.basicContainer.stationType == StationType_roadSideUnit && - evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) - { - ProtectedCommunicationZonesRSU_t *pzs = evcsnm->evcsnm.evcsnmParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; - if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255) - { - - bool new_pz = false; - for (int k = 0; k < pzs->list.count; ++k) - { - - bool found = false; - for (int j = 0; j < lightship->protected_zones.pz_len; ++j) - { - if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude && - lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) - { - found = true; - break; - } - } - if (found) - continue; - - new_pz = true; - - lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t)); - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude; - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude; - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType; - - if (pzs->list.array[k]->expiryTime) - { - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size; - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size); - memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size); - } - if (pzs->list.array[k]->protectedZoneID) - { - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8); - *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID; - } - if (pzs->list.array[k]->protectedZoneRadius) - { - lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8); - *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius; - } - ++lightship->protected_zones.pz_len; - } - - // Inform [management] - if (new_pz) - { - uint8_t b_oer[512]; - ManagementRequest_t *mreq = calloc(1, sizeof(ManagementRequest_t)); - mreq->present = ManagementRequest_PR_attributes; - mreq->choice.attributes.present = ManagementRequestAttributes_PR_set; - mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones)); - mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len; - mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void *); - mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void *)); - for (int p = 0; p < lightship->protected_zones.pz_len; ++p) - { - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512); - oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void **)&mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded); - } - - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512); - void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); - itss_0send(management_socket, b_oer, enc.encoded); - log_debug("[ca]-> sending MReq.attributes.set.protectedZones to ->[management]"); - uint8_t code; - itss_0recv_rt(&management_socket, &code, 1, b_oer, enc.encoded, 1000); - itss_0close(management_socket); - } - } - } - } - pthread_mutex_unlock(&lightship->lock); - - return rv; -} - -static int check_pz() -{ - - lightship_t *lightship = &facilities.lightship; - - bool is_inside = false; - - itss_space_lock(); - itss_space_get(); - double lat = epv.space.latitude / 10000000.0; - double lon = epv.space.longitude / 10000000.0; - itss_space_unlock(); - - pthread_mutex_lock(&lightship->lock); - - for (int i = 0; i < lightship->protected_zones.pz_len; ++i) - { - double d = itss_haversine(lat, lon, (double)lightship->protected_zones.pz[i]->protectedZoneLatitude / 10000000.0, (double)lightship->protected_zones.pz[i]->protectedZoneLongitude / 10000000.0); - - int pz_radius = 50; - if (lightship->protected_zones.pz[i]->protectedZoneRadius) - { - pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius; - } - if (d < pz_radius) - { - is_inside = true; - break; - } - } - - pthread_mutex_unlock(&lightship->lock); - return is_inside; -} - -void *ca_service() +void *evcsn_service() { int rv = 0; @@ -584,7 +131,7 @@ void *ca_service() bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->destinationPort = Port_evcsnm; + bpr->destinationPort = Port_cam; bpr->gn.trafficClass = 2; @@ -597,7 +144,7 @@ void *ca_service() FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t)); fi->present = FacilitiesIndication_PR_message; FacilitiesMessageIndication_t *fmi = &fi->choice.message; - fmi->itsMessageType = ItsMessageType_evcsnm; + fmi->itsMessageType = messageID_evcsn; fmi->data.buf = malloc(512); uint8_t tr_oer[1024]; @@ -606,9 +153,8 @@ void *ca_service() fi_oer[0] = 4; while (!facilities.exit) { - usleep(1000 * 50); - - if (lightship_check() && facilities.lightship.active) + usleep(1000 * 1000); + if (facilities.evcsnm_args.activate) { rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size); if (rv) @@ -618,11 +164,6 @@ void *ca_service() memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size); fmi->data.size = bpr->data.size; - // Check if inside PZ - bpr->gn.communicationProfile = 0; - if (facilities.station_type != 15 && check_pz()) - bpr->gn.communicationProfile = 1; - uint32_t id = itss_id(bpr->data.buf, bpr->data.size); bpr->id = id; fmi->id = id; @@ -645,15 +186,13 @@ void *ca_service() itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); - lightship_reset_timer(); - // 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, bpr->id, true, messageID_evcsnm, NULL, bpr->data.buf, bpr->data.size); + itss_db_add(facilities.logging.dbms, station_id, bpr->id, true, messageID_evcsn, NULL, bpr->data.buf, bpr->data.size); } if (facilities.logging.recorder) diff --git a/src/evcsnm.h b/src/evcsnm.h index 2777d31..0ccd2fc 100644 --- a/src/evcsnm.h +++ b/src/evcsnm.h @@ -1,12 +1,12 @@ -#ifndef FACILITIES_CAM_H -#define FACILITIES_CAM_H +#ifndef FACILITIES_EVCSNM_H +#define FACILITIES_EVCSNM_H #include #include #include #include -#include +#include #include #include @@ -14,117 +14,35 @@ #define POS_HISTORY_MAX_LEN 24 #define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1 -typedef enum CID_CAM { - CID_PROTECTED_ZONES, - CID_PUBLIC_TRANSPORT, - CID_SPECIAL_TRANSPORT, - CID_DANGEROUS_GOODS, - CID_ROADWORK, - CID_RESCUE, - CID_EMERGENCY, - CID_SAFETY_CAR, - CID_CLOSED_LANES, - CID_REQUEST_FOR_RIGHT_OF_WAY, - CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT, - CID_NO_PASSING, - CID_NO_PASSING_FOR_TRUCKS, - CID_SPEED_LIMIT, - CID_RESERVED0, - CID_RESERVED1, -} CID_CAM_e; - -typedef struct cid_ssp_bm { - const char* container; - const uint32_t bitmap_val; -} cid_ssp_bm_t; - -enum CAM_CHECK_R { - CAM_OK, - CAM_INVALID, - CAM_BAD_PERMISSIONS +enum EVCSNM_CHECK_R { + EVCSNM_OK, + EVCSNM_INVALID, + EVCSNM_BAD_PERMISSIONS }; -typedef struct pos_point { - uint64_t ts; - uint16_t heading; - int32_t lon; - int32_t lat; - int32_t alt; - uint16_t speed; -} pos_point_t; - -typedef struct lightship { - bool active; - - pthread_mutex_t lock; - - uint8_t type; - - pos_point_t last_pos; - uint64_t t_last_cam; - uint64_t next_cam_max; - uint64_t next_cam_min; - - uint64_t t_last_cam_lfc; - - pos_point_t concise_points[3]; - uint8_t concise_points_len; - bool concise_keep_start; - - pos_point_t* path_history[PATH_HISTORY_MAX_LEN]; - uint16_t path_history_len; - - - bool is_vehicle_near; - uint64_t t_last_vehicle; - - uint32_t vehicle_gen_min; - uint32_t vehicle_gen_max; - uint32_t rsu_gen_min; - uint32_t rsu_vehicle_permanence; - - bool use_obd; - - struct { - ProtectedCommunicationZone_t ** pz; - uint16_t pz_len; - } protected_zones; - -} lightship_t; - /* - * @brief Initializes the main CA struct (lightship) + * @brief Analyzes a received EVCSNM * - * @return Always zero + * @return A EVCSNM check code */ -int lightship_init(); - -/* - * @brief Checks if a CAM must be sent - * - * @return True if CAM must be sent, false otherwise - */ -int lightship_check(); - -/* - * @brief Resets the CAM sending timer - * - * @return Nothing - */ -void lightship_reset_timer(); - -/* - * @brief Analyzes a received CAM - * - * @return A CAM check code - */ -enum CAM_CHECK_R check_cam(BTPPacketIndication_t* bpi, CAM_t* cam,uint8_t* ssp, uint32_t ssp_len); +enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len); /* * @brief Main CA service * * @return NULL */ -void* ca_service(); +void* evcsn_service(); + +/** + * Analyses a VCM + * @param vcm The VCM to be analyzed + * @return 0 on success, other value otherwise + */ +int evcsnm_check(EvcsnPdu_t* evcsnm); + +typedef struct evcsnm_args { + bool activate; +} evcsnm_args_t; #endif diff --git a/src/facilities.c b/src/facilities.c index f16b367..ae6a197 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -9,6 +9,7 @@ #include "saem.h" #include "tpm.h" #include "vcm.h" +#include "evcsnm.h" #include #include @@ -45,22 +46,24 @@ facilities_t facilities = {0}; -static int transport_indication(void* responder, void** security_socket, uint8_t *msg, uint32_t msg_len) { +static int transport_indication(void *responder, void **security_socket, uint8_t *msg, uint32_t msg_len) +{ int rv = 0; uint8_t code = 0; bool handled_msg = false; FacilitiesIndication_t *fi = NULL; - SecurityRequest_t* sreq = NULL; - SecurityReply_t* srep = NULL; - TransportRequest_t* tr = NULL; + SecurityRequest_t *sreq = NULL; + SecurityReply_t *srep = NULL; + TransportRequest_t *tr = NULL; uint16_t buf_len = 2048; uint8_t buf[2048]; - TransportIndication_t* ti = calloc(1, sizeof(TransportIndication_t)); + TransportIndication_t *ti = calloc(1, sizeof(TransportIndication_t)); - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_TransportIndication, (void**) &ti, msg, msg_len); - if (dec.code) { + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_TransportIndication, (void **)&ti, msg, msg_len); + if (dec.code) + { log_error("<- invalid TI received"); rv = 1; code = 1; @@ -70,19 +73,20 @@ static int transport_indication(void* responder, void** security_socket, uint8_t itss_0send(responder, &code, 1); - switch (ti->present) { - case TransportIndication_PR_packet: - break; - case TransportIndication_PR_data: - transport_data_indication(&ti->choice.data, security_socket); - goto cleanup; - default: - log_debug("<- unrecognized TI.choice received"); - rv = 1; - goto cleanup; + switch (ti->present) + { + case TransportIndication_PR_packet: + break; + case TransportIndication_PR_data: + transport_data_indication(&ti->choice.data, security_socket); + goto cleanup; + default: + log_debug("<- unrecognized TI.choice received"); + rv = 1; + goto cleanup; } - TransportPacketIndication_t* tpi = &ti->choice.packet; // TODO + TransportPacketIndication_t *tpi = &ti->choice.packet; // TODO // bool fwd = false; @@ -92,328 +96,366 @@ static int transport_indication(void* responder, void** security_socket, uint8_t asn_TYPE_descriptor_t *its_msg_descriptor = NULL; void *its_msg = NULL; int its_msg_type = 0; - uint8_t* packet = NULL; + uint8_t *packet = NULL; uint16_t packet_len = 0; - switch (tpi->present) { - case TransportPacketIndication_PR_btp: - id = tpi->choice.btp.id; - log_debug("<- TI.packet.btp | id:%08x size:%dB", (uint32_t) id, msg_len); - // Parse message - switch (tpi->choice.btp.destinationPort) { - case Port_cam: - its_msg_descriptor = &asn_DEF_CAM; - its_msg = calloc(1, sizeof(CAM_t)); - its_msg_type = messageID_cam; - handled_msg = true; - break; - - case Port_denm: - its_msg_descriptor = &asn_DEF_DENM; - its_msg = calloc(1, sizeof(DENM_t)); - its_msg_type = messageID_denm; - handled_msg = true; - break; - - case Port_ivim: - its_msg_descriptor = &asn_DEF_IVIM; - its_msg = calloc(1, sizeof(IVIM_t)); - its_msg_type = messageID_ivim; - handled_msg = true; - break; - - case Port_cpm: - its_msg_descriptor = &asn_DEF_CPM; - its_msg = calloc(1, sizeof(CPM_t)); - its_msg_type = 14; - handled_msg = true; - break; - - case Port_saem: - its_msg_descriptor = &asn_DEF_SAEM; - its_msg = calloc(1, sizeof(SAEM_t)); - its_msg_type = messageID_saem; - handled_msg = true; - break; - - case 7011: /* tolling */ - its_msg_descriptor = &asn_DEF_TPM; - its_msg = calloc(1, sizeof(TPM_t)); - its_msg_type = 117; - handled_msg = true; - break; - - case 2043: /* maneuvers */ - its_msg_descriptor = &asn_DEF_VCM; - its_msg = calloc(1, sizeof(VCM_t)); - its_msg_type = 43; - handled_msg = true; - break; - - case 2044: /* VERCOe */ - its_msg_descriptor = &asn_DEF_VERCOe; - its_msg = calloc(1, sizeof(VERCOe_t)); - its_msg_type = 44; - handled_msg = true; - fwd = true; - break; - - default: - log_debug("messsage with unhandled BTP port received (%lld), ignoring", tpi->choice.btp.destinationPort); - goto cleanup; - } - - packet = tpi->choice.btp.data.buf; - packet_len = tpi->choice.btp.data.size; - - dec = uper_decode_complete(NULL, its_msg_descriptor, (void**) &its_msg, tpi->choice.btp.data.buf, tpi->choice.btp.data.size); - if (dec.code) { - log_debug("<- invalid %s received", its_msg_descriptor->name); - rv = 1; - goto cleanup; - } - - // Get permisisons - uint8_t* ssp = NULL; - uint16_t ssp_len; - if (tpi->choice.btp.gn.securityPermissions) { - ssp = tpi->choice.btp.gn.securityPermissions->ssp.buf; - ssp_len = tpi->choice.btp.gn.securityPermissions->ssp.size; - } - - // Get neighbour certificate ID - uint8_t* neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL; - - // Manage message - switch (tpi->choice.btp.destinationPort) { - case Port_cam: - switch (check_cam(&tpi->choice.btp, its_msg, ssp, ssp_len)) { - case CAM_OK: - fwd = true; - break; - case CAM_INVALID: - case CAM_BAD_PERMISSIONS: - default: - break; - } - break; - - case Port_denm: - ; -#ifdef DEBUG - uint8_t* xml_denm = malloc(32768); - asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_DENM, its_msg); - log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm); - free(xml_denm); -#endif - switch (event_manage(its_msg, &id, ssp, ssp_len)) { - case EVENT_NEW: - case EVENT_CANCELLATION: - case EVENT_NEGATION: - case EVENT_UPDATE: - case EVENT_NUMBER_EXCEEDED: - fwd = true; - break; - case EVENT_INVALID: - case EVENT_PASSED: - case EVENT_REPEATED: - case EVENT_BAD_PERMISSIONS: - break; - } - break; - - case Port_ivim: - switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len)) { - case SERVICE_NEW: - case SERVICE_CANCELLATION: - case SERVICE_NEGATION: - case SERVICE_UPDATE: - case SERVICE_NUMBER_EXCEEDED: - fwd = true; - break; - case SERVICE_INVALID: - case SERVICE_REPEATED: - case SERVICE_PASSED: - case SERVICE_BAD_PERMISSIONS: - default: - break; - } - break; - - case Port_saem: - switch (saem_check(its_msg, neighbour_cert)) { - case SAEM_NEW: - fwd = true; - break; - default: - break; - } - break; - - case 7011: - if (facilities.tolling.enabled) { - tpm_recv(its_msg, security_socket, neighbour_cert, NULL); - fwd = true; - } - break; - - case 2043: - if (facilities.coordination.active) { - vcm_check(its_msg); - } - fwd = true; - break; - - default: - break; - } + switch (tpi->present) + { + case TransportPacketIndication_PR_btp: + id = tpi->choice.btp.id; + log_debug("<- TI.packet.btp | id:%08x size:%dB", (uint32_t)id, msg_len); + // Parse message + switch (tpi->choice.btp.destinationPort) + { + case Port_cam: + its_msg_descriptor = &asn_DEF_CAM; + its_msg = calloc(1, sizeof(CAM_t)); + its_msg_type = messageID_cam; + handled_msg = true; break; - case TransportPacketIndication_PR_tcp: - id = tpi->choice.tcp.id; - packet = tpi->choice.tcp.data.buf; - packet_len = tpi->choice.tcp.data.size; - log_debug("<- TI.packet.tcp | id:%ld size:%dB", id, msg_len); - - sreq = calloc(1, sizeof(SecurityRequest_t)); - sreq->present = SecurityRequest_PR_tlsRecv; - sreq->choice.tlsRecv.data.size = tpi->choice.tcp.data.size; - sreq->choice.tlsRecv.data.buf = malloc(tpi->choice.tcp.data.size); - memcpy(sreq->choice.tlsRecv.data.buf, tpi->choice.tcp.data.buf, tpi->choice.tcp.data.size); - - pthread_mutex_lock(&facilities.tolling.lock); - tlsc_t* tlsc = tolling_tlsc_get(tpi->choice.tcp.sourceAddress->buf, 7011); - if (tlsc) { - id = tlsc->id; - } else { - tlsc = tolling_tlsc_new(tpi->choice.tcp.sourceAddress->buf, 7011); - id = tlsc->id; - } - ++tlsc->nmsg; - pthread_mutex_unlock(&facilities.tolling.lock); - sreq->choice.tlsSend.connId = id; - - uint8_t b_tx[2048], b_rx[2048]; - b_tx[0] = 4; - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, b_tx+1, 2047); - - log_debug("->[security] SecurityRequest.tlsRecv (%ldB)", enc.encoded+1); - itss_0send(*security_socket, b_tx, enc.encoded+1); - int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded+1, 1000); - log_debug("<-[security] SecurityReply.tlsRecv (%dB)", rl); - - if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, b_rx, rl).code) { - log_error("SecurityReply.tlsRecv decode failure"); - rv = 1; - goto cleanup; - } - - if (srep->returnCode == SecurityReplyReturnCode_rejected) { - log_error("SecurityReply.tlsRecv rejected"); - - SecurityRequest_t* sREQ = calloc(1, sizeof(SecurityRequest_t)); - sREQ->present = SecurityRequest_PR_tlsShutdown; - sREQ->choice.tlsShutdown.connId = id; - b_tx[0] = 4; - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sREQ, b_tx+1, 2047); - log_debug("->[security] SecurityRequest.tlsShutdown (%ldB)", enc.encoded+1); - itss_0send(*security_socket, b_tx, enc.encoded+1); - int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded+1, 1000); - log_debug("<-[security] SecurityReply.tlsShutdown (%dB)", rl); - - rv = 1; - goto cleanup; - } - - log_debug("[tolling] tls n-msg:%d state:%d", tlsc->nmsg, tlsc->state); - - // Forward to [transport] - if (srep->data->choice.tlsRecv.state != 1) { - tr = calloc(1, sizeof(TransportRequest_t)); - tr->present = TransportRequest_PR_packet; - tr->choice.packet.present = TransportPacketRequest_PR_tcp; - TCPPacketRequest_t* tpr = &tr->choice.packet.choice.tcp; - tpr->data.size = srep->data->choice.tlsRecv.data.size; - tpr->data.buf = malloc(srep->data->choice.tlsRecv.data.size); - memcpy(tpr->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size); - - tpr->sourcePort = tpi->choice.tcp.destinationPort; - tpr->destinationPort = tpi->choice.tcp.sourcePort; - - tpr->destinationAddress = calloc(1, sizeof(OCTET_STRING_t)); - tpr->destinationAddress->buf = malloc(16); - tpr->destinationAddress->size = 16; - memcpy(tpr->destinationAddress->buf, tpi->choice.tcp.sourceAddress->buf, 16); - tpr->destinationPort = 7011; - tpr->sourcePort = 7011; - - if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN || - (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS && tlsc->nmsg < 2)) { - tpr->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t)); - tpr->gn->packetTransportType = PacketTransportType_shb; - tpr->gn->destinationAddress.buf = calloc(1, 6); - tpr->gn->destinationAddress.size = 6; - } - - tpr->id = itss_id(tpr->data.buf, tpr->data.size); - - buf[0] = 4; - enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, buf_len-1); - if (enc.encoded == -1) { - log_error("TransportRequest encoding fail"); - rv = 1; - goto cleanup; - } - - itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, tpr->id, "TR.packet.tcp"); - } else { - - if (facilities.tolling.enabled && srep->data->choice.tlsRecv.data.size) { - dec = uper_decode_complete(NULL, &asn_DEF_TPM, (void**) &its_msg, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size); - if (dec.code) { - log_debug("<- invalid TPM received"); - rv = 1; - goto cleanup; - } - if (!dec.code) { - tpm_recv(its_msg, security_socket, NULL, tpi->choice.tcp.sourceAddress->buf); - - // Fwd to [applications] - fi = calloc(1, sizeof(FacilitiesIndication_t)); - fi->present = FacilitiesIndication_PR_message; - FacilitiesMessageIndication_t* fmi = &fi->choice.message; - - fmi->id = id; - - fmi->itsMessageType = 7011; - - fmi->data.size = srep->data->choice.tlsRecv.data.size; - fmi->data.buf = malloc(srep->data->choice.tlsRecv.data.size); - memcpy(fmi->data.buf, srep->data->choice.tlsRecv.data.buf,srep->data->choice.tlsRecv.data.size); - - uint8_t buffer[ITSS_SDU_MAX_LEN]; - buffer[0] = 4; // Facilities - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer+1, ITSS_SDU_MAX_LEN-1); - - itss_queue_send(facilities.tx_queue, buffer, enc.encoded+1, ITSS_APPLICATIONS, id, "FI.message"); - } - } - } + case Port_denm: + its_msg_descriptor = &asn_DEF_DENM; + its_msg = calloc(1, sizeof(DENM_t)); + its_msg_type = messageID_denm; + handled_msg = true; break; - case TransportPacketIndication_PR_udp: - id = tpi->choice.udp.id; - log_debug("<- TI.packet.udp | id:%ld size:%dB", id, msg_len); + + case Port_ivim: + its_msg_descriptor = &asn_DEF_IVIM; + its_msg = calloc(1, sizeof(IVIM_t)); + its_msg_type = messageID_ivim; + handled_msg = true; + break; + + case Port_cpm: + its_msg_descriptor = &asn_DEF_CPM; + its_msg = calloc(1, sizeof(CPM_t)); + its_msg_type = 14; + handled_msg = true; + break; + + case Port_saem: + its_msg_descriptor = &asn_DEF_SAEM; + its_msg = calloc(1, sizeof(SAEM_t)); + its_msg_type = messageID_saem; + handled_msg = true; + break; + + case 7011: /* tolling */ + its_msg_descriptor = &asn_DEF_TPM; + its_msg = calloc(1, sizeof(TPM_t)); + its_msg_type = 117; + handled_msg = true; + break; + + case 2043: /* maneuvers */ + its_msg_descriptor = &asn_DEF_VCM; + its_msg = calloc(1, sizeof(VCM_t)); + its_msg_type = 43; + handled_msg = true; + break; + + case 2044: /* VERCOe */ + its_msg_descriptor = &asn_DEF_VERCOe; + its_msg = calloc(1, sizeof(VERCOe_t)); + its_msg_type = 44; + handled_msg = true; + fwd = true; + break; + + case Port_poi: /* EVCSNM */ + its_msg_descriptor = &asn_DEF_EvcsnPdu; + its_msg = calloc(1, sizeof(EvcsnPdu_t)); + its_msg_type = messageID_evcsn; + handled_msg = true; + fwd = true; break; default: + log_debug("messsage with unhandled BTP port received (%lld), ignoring", tpi->choice.btp.destinationPort); + goto cleanup; + } + + packet = tpi->choice.btp.data.buf; + packet_len = tpi->choice.btp.data.size; + + dec = uper_decode_complete(NULL, its_msg_descriptor, (void **)&its_msg, tpi->choice.btp.data.buf, tpi->choice.btp.data.size); + if (dec.code) + { + log_debug("<- invalid %s received", its_msg_descriptor->name); + rv = 1; + goto cleanup; + } + + // Get permisisons + uint8_t *ssp = NULL; + uint16_t ssp_len; + if (tpi->choice.btp.gn.securityPermissions) + { + ssp = tpi->choice.btp.gn.securityPermissions->ssp.buf; + ssp_len = tpi->choice.btp.gn.securityPermissions->ssp.size; + } + + // Get neighbour certificate ID + uint8_t *neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL; + + // Manage message + switch (tpi->choice.btp.destinationPort) + { + case Port_cam: + switch (check_cam(&tpi->choice.btp, its_msg, ssp, ssp_len)) + { + case CAM_OK: + fwd = true; + break; + case CAM_INVALID: + case CAM_BAD_PERMISSIONS: + default: + break; + } break; + + case Port_denm:; +#ifdef DEBUG + uint8_t *xml_denm = malloc(32768); + asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_DENM, its_msg); + log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded, xml_denm); + free(xml_denm); +#endif + switch (event_manage(its_msg, &id, ssp, ssp_len)) + { + case EVENT_NEW: + case EVENT_CANCELLATION: + case EVENT_NEGATION: + case EVENT_UPDATE: + case EVENT_NUMBER_EXCEEDED: + fwd = true; + break; + case EVENT_INVALID: + case EVENT_PASSED: + case EVENT_REPEATED: + case EVENT_BAD_PERMISSIONS: + break; + } + break; + + case Port_ivim: + switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len)) + { + case SERVICE_NEW: + case SERVICE_CANCELLATION: + case SERVICE_NEGATION: + case SERVICE_UPDATE: + case SERVICE_NUMBER_EXCEEDED: + fwd = true; + break; + case SERVICE_INVALID: + case SERVICE_REPEATED: + case SERVICE_PASSED: + case SERVICE_BAD_PERMISSIONS: + default: + break; + } + break; + + case Port_saem: + switch (saem_check(its_msg, neighbour_cert)) + { + case SAEM_NEW: + fwd = true; + break; + default: + break; + } + break; + + case 7011: + if (facilities.tolling.enabled) + { + tpm_recv(its_msg, security_socket, neighbour_cert, NULL); + fwd = true; + } + break; + + case 2043: + if (facilities.coordination.active) + { + vcm_check(its_msg); + } + fwd = true; + break; + case Port_poi: + if (facilities.evcsnm_args.activate) + { + evcsnm_check(its_msg); + } + fwd = true; + break; + default: + break; + } + break; + + case TransportPacketIndication_PR_tcp: + id = tpi->choice.tcp.id; + packet = tpi->choice.tcp.data.buf; + packet_len = tpi->choice.tcp.data.size; + log_debug("<- TI.packet.tcp | id:%ld size:%dB", id, msg_len); + + sreq = calloc(1, sizeof(SecurityRequest_t)); + sreq->present = SecurityRequest_PR_tlsRecv; + sreq->choice.tlsRecv.data.size = tpi->choice.tcp.data.size; + sreq->choice.tlsRecv.data.buf = malloc(tpi->choice.tcp.data.size); + memcpy(sreq->choice.tlsRecv.data.buf, tpi->choice.tcp.data.buf, tpi->choice.tcp.data.size); + + pthread_mutex_lock(&facilities.tolling.lock); + tlsc_t *tlsc = tolling_tlsc_get(tpi->choice.tcp.sourceAddress->buf, 7011); + if (tlsc) + { + id = tlsc->id; + } + else + { + tlsc = tolling_tlsc_new(tpi->choice.tcp.sourceAddress->buf, 7011); + id = tlsc->id; + } + ++tlsc->nmsg; + pthread_mutex_unlock(&facilities.tolling.lock); + sreq->choice.tlsSend.connId = id; + + uint8_t b_tx[2048], b_rx[2048]; + b_tx[0] = 4; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, b_tx + 1, 2047); + + log_debug("->[security] SecurityRequest.tlsRecv (%ldB)", enc.encoded + 1); + itss_0send(*security_socket, b_tx, enc.encoded + 1); + int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000); + log_debug("<-[security] SecurityReply.tlsRecv (%dB)", rl); + + if (oer_decode(NULL, &asn_DEF_SecurityReply, (void **)&srep, b_rx, rl).code) + { + log_error("SecurityReply.tlsRecv decode failure"); + rv = 1; + goto cleanup; + } + + if (srep->returnCode == SecurityReplyReturnCode_rejected) + { + log_error("SecurityReply.tlsRecv rejected"); + + SecurityRequest_t *sREQ = calloc(1, sizeof(SecurityRequest_t)); + sREQ->present = SecurityRequest_PR_tlsShutdown; + sREQ->choice.tlsShutdown.connId = id; + b_tx[0] = 4; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sREQ, b_tx + 1, 2047); + log_debug("->[security] SecurityRequest.tlsShutdown (%ldB)", enc.encoded + 1); + itss_0send(*security_socket, b_tx, enc.encoded + 1); + int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000); + log_debug("<-[security] SecurityReply.tlsShutdown (%dB)", rl); + + rv = 1; + goto cleanup; + } + + log_debug("[tolling] tls n-msg:%d state:%d", tlsc->nmsg, tlsc->state); + + // Forward to [transport] + if (srep->data->choice.tlsRecv.state != 1) + { + tr = calloc(1, sizeof(TransportRequest_t)); + tr->present = TransportRequest_PR_packet; + tr->choice.packet.present = TransportPacketRequest_PR_tcp; + TCPPacketRequest_t *tpr = &tr->choice.packet.choice.tcp; + tpr->data.size = srep->data->choice.tlsRecv.data.size; + tpr->data.buf = malloc(srep->data->choice.tlsRecv.data.size); + memcpy(tpr->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size); + + tpr->sourcePort = tpi->choice.tcp.destinationPort; + tpr->destinationPort = tpi->choice.tcp.sourcePort; + + tpr->destinationAddress = calloc(1, sizeof(OCTET_STRING_t)); + tpr->destinationAddress->buf = malloc(16); + tpr->destinationAddress->size = 16; + memcpy(tpr->destinationAddress->buf, tpi->choice.tcp.sourceAddress->buf, 16); + tpr->destinationPort = 7011; + tpr->sourcePort = 7011; + + if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN || + (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS && tlsc->nmsg < 2)) + { + tpr->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t)); + tpr->gn->packetTransportType = PacketTransportType_shb; + tpr->gn->destinationAddress.buf = calloc(1, 6); + tpr->gn->destinationAddress.size = 6; + } + + tpr->id = itss_id(tpr->data.buf, tpr->data.size); + + buf[0] = 4; + enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf + 1, buf_len - 1); + if (enc.encoded == -1) + { + log_error("TransportRequest encoding fail"); + rv = 1; + goto cleanup; + } + + itss_queue_send(facilities.tx_queue, buf, enc.encoded + 1, ITSS_TRANSPORT, tpr->id, "TR.packet.tcp"); + } + else + { + + if (facilities.tolling.enabled && srep->data->choice.tlsRecv.data.size) + { + dec = uper_decode_complete(NULL, &asn_DEF_TPM, (void **)&its_msg, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size); + if (dec.code) + { + log_debug("<- invalid TPM received"); + rv = 1; + goto cleanup; + } + if (!dec.code) + { + tpm_recv(its_msg, security_socket, NULL, tpi->choice.tcp.sourceAddress->buf); + + // Fwd to [applications] + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; + + fmi->id = id; + + fmi->itsMessageType = 7011; + + fmi->data.size = srep->data->choice.tlsRecv.data.size; + fmi->data.buf = malloc(srep->data->choice.tlsRecv.data.size); + memcpy(fmi->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size); + + uint8_t buffer[ITSS_SDU_MAX_LEN]; + buffer[0] = 4; // Facilities + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1); + + itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); + } + } + } + break; + case TransportPacketIndication_PR_udp: + id = tpi->choice.udp.id; + log_debug("<- TI.packet.udp | id:%ld size:%dB", id, msg_len); + break; + + default: + break; } // Forward to [applications] - if (fwd) { + if (fwd) + { fi = calloc(1, sizeof(FacilitiesIndication_t)); fi->present = FacilitiesIndication_PR_message; - FacilitiesMessageIndication_t* fmi = &fi->choice.message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; fmi->id = id; @@ -425,35 +467,39 @@ static int transport_indication(void* responder, void** security_socket, uint8_t uint8_t buffer[ITSS_SDU_MAX_LEN]; buffer[0] = 4; // Facilities - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer+1, ITSS_SDU_MAX_LEN-1); + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1); - itss_queue_send(facilities.tx_queue, buffer, enc.encoded+1, ITSS_APPLICATIONS, id, "FI.message"); + itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); } - // Logging - if (facilities.logging.dbms) { + // 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, id, false, its_msg_type, NULL, packet, packet_len); } - if (facilities.logging.recorder) { + if (facilities.logging.recorder) + { int e = itss_management_record_packet_sdu( - buf, - buf_len, - tpi->choice.btp.data.buf, - tpi->choice.btp.data.size, - tpi->choice.btp.id, - itss_time_get(), - ITSS_FACILITIES, - false); - if (e != -1) { - itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, tpi->choice.btp.id, "MReq.packet.set"); + buf, + buf_len, + tpi->choice.btp.data.buf, + tpi->choice.btp.data.size, + tpi->choice.btp.id, + itss_time_get(), + ITSS_FACILITIES, + false); + if (e != -1) + { + itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, tpi->choice.btp.id, "MReq.packet.set"); } } cleanup: - if (handled_msg && tpi->choice.btp.destinationPort != Port_denm && tpi->choice.btp.destinationPort != Port_ivim) { + if (handled_msg && tpi->choice.btp.destinationPort != Port_denm && tpi->choice.btp.destinationPort != Port_ivim) + { ASN_STRUCT_FREE(*its_msg_descriptor, its_msg); } ASN_STRUCT_FREE(asn_DEF_TransportIndication, ti); @@ -465,55 +511,59 @@ cleanup: return rv; } -static int facilities_request(void* responder, uint8_t *msg, uint32_t msg_len) { +static int facilities_request(void *responder, uint8_t *msg, uint32_t msg_len) +{ int rv = 0; - FacilitiesRequest_t *fr = calloc(1, sizeof(FacilitiesRequest_t)); + FacilitiesRequest_t *fr = calloc(1, sizeof(FacilitiesRequest_t)); - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_FacilitiesRequest, (void**) &fr, msg, msg_len); - if (dec.code) { + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_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 FacilitiesRequest_PR_message: - rv = facilities_request_single_message(responder, &fr->choice.message); + switch (fr->present) + { + case FacilitiesRequest_PR_message: + rv = facilities_request_single_message(responder, &fr->choice.message); + break; + + case FacilitiesRequest_PR_data: + switch (fr->choice.data.present) + { + case FacilitiesDataRequest_PR_activeEpisodes: + rv = facilities_request_active_episodes(responder, fr); break; - case FacilitiesRequest_PR_data: - switch (fr->choice.data.present) { - case FacilitiesDataRequest_PR_activeEpisodes: - rv = facilities_request_active_episodes(responder, fr); - break; + case FacilitiesDataRequest_PR_attributeTypes: + rv = facilities_request_attribute_types(responder, fr); + break; - case FacilitiesDataRequest_PR_attributeTypes: - rv = facilities_request_attribute_types(responder, fr); - break; + case FacilitiesDataRequest_PR_loadedProtectionZones: + rv = facilities_request_loaded_protected_zones(responder, fr); + break; - case FacilitiesDataRequest_PR_loadedProtectionZones: - rv = facilities_request_loaded_protected_zones(responder, fr); - break; - - case 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; - } + case FacilitiesDataRequest_PR_chainInfoSet: + rv = facilities_request_chaininfo_set(responder, &fr->choice.data.choice.chainInfoSet); break; default: - log_error("<- unrecognized FR type received (%d)", fr->present); + 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: @@ -522,26 +572,28 @@ cleanup: return rv; } - -static int security_indication(void* responder_secured, uint8_t *msg, uint32_t msg_len) { +static int security_indication(void *responder_secured, uint8_t *msg, uint32_t msg_len) +{ int rv = 0; - SecurityIndication_t* si = calloc(1, sizeof(SecurityIndication_t)); - SecurityResponse_t* sr = calloc(1, sizeof(SecurityResponse_t)); + SecurityIndication_t *si = calloc(1, sizeof(SecurityIndication_t)); + SecurityResponse_t *sr = calloc(1, sizeof(SecurityResponse_t)); uint8_t buffer[64]; asn_enc_rval_t enc; - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityIndication, (void**) &si, msg, msg_len); - if (dec.code) { + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_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) { + if (facilities.id.change.stage == ID_CHANGE_BLOCKED) + { pthread_mutex_unlock(&facilities.id.change.lock); log_debug("identity change is currently blocked"); rv = 1; @@ -549,8 +601,9 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m } if (facilities.id.change.stage == ID_CHANGE_PREPARE && - si->choice.idChangeEvent.command != SecurityIdChangeEventType_commit && - si->choice.idChangeEvent.command != SecurityIdChangeEventType_abort) { + si->choice.idChangeEvent.command != SecurityIdChangeEventType_commit && + si->choice.idChangeEvent.command != 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; @@ -559,67 +612,69 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m bool id_changed = false; - switch (si->choice.idChangeEvent.command) { - case SecurityIdChangeEventType_prepare: + switch (si->choice.idChangeEvent.command) + { + case SecurityIdChangeEventType_prepare: - facilities.id.change.stage = ID_CHANGE_PREPARE; + facilities.id.change.stage = ID_CHANGE_PREPARE; - pthread_mutex_lock(&facilities.id.lock); - pthread_mutex_lock(&facilities.lightship.lock); - break; + pthread_mutex_lock(&facilities.id.lock); + pthread_mutex_lock(&facilities.lightship.lock); + break; - case SecurityIdChangeEventType_commit: - ; - facilities.id.change.stage = ID_CHANGE_COMMIT; + case 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]); + // 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 SecurityId_PR_stationId: + facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId; + break; + case SecurityId_PR_ipv6Address: + memcpy(facilities.id.ipv6_addr, si->choice.idChangeEvent.ids.list.array[i]->choice.ipv6Address.buf, 16); + break; + default: + break; } - 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; + facilities.id.change.stage = ID_CHANGE_INACTIVE; - pthread_mutex_unlock(&facilities.lightship.lock); + pthread_mutex_unlock(&facilities.id.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 SecurityId_PR_stationId: - facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId; - break; - case SecurityId_PR_ipv6Address: - memcpy(facilities.id.ipv6_addr, si->choice.idChangeEvent.ids.list.array[i]->choice.ipv6Address.buf, 16); - break; - default: - break; - } - } + id_changed = true; - facilities.id.change.stage = ID_CHANGE_INACTIVE; + break; - pthread_mutex_unlock(&facilities.id.lock); + case SecurityIdChangeEventType_abort: + pthread_mutex_unlock(&facilities.id.lock); + pthread_mutex_unlock(&facilities.lightship.lock); - id_changed = true; - - break; - - case 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; + 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 = SecurityResponse_PR_idChangeEvent; @@ -630,7 +685,8 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m pthread_mutex_unlock(&facilities.id.change.lock); cleanup: - if (rv) { + if (rv) + { sr->present = SecurityResponse_PR_idChangeEvent; sr->choice.idChangeEvent.returnCode = 1; enc = oer_encode_to_buffer(&asn_DEF_SecurityResponse, NULL, sr, buffer, 64); @@ -644,15 +700,17 @@ cleanup: return rv; } -static int networking_indication(void* responder, uint8_t* msg, uint32_t msg_len) { +static int networking_indication(void *responder, uint8_t *msg, uint32_t msg_len) +{ int rv = 0; uint8_t code = 0; - NetworkingIndication_t* ni = calloc(1, sizeof(NetworkingIndication_t)); + NetworkingIndication_t *ni = calloc(1, sizeof(NetworkingIndication_t)); - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_NetworkingIndication, (void**) &ni, msg, msg_len); - if (dec.code) { + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_NetworkingIndication, (void **)&ni, msg, msg_len); + if (dec.code) + { rv = 1; code = 1; itss_0send(responder, &code, 1); @@ -660,12 +718,14 @@ static int networking_indication(void* responder, uint8_t* msg, uint32_t msg_len } itss_0send(responder, &code, 1); - if (ni->present != NetworkingIndication_PR_data) { + if (ni->present != NetworkingIndication_PR_data) + { goto cleanup; } - if (ni->choice.data.mobileNeighbour && - *ni->choice.data.mobileNeighbour) { + if (ni->choice.data.mobileNeighbour && + *ni->choice.data.mobileNeighbour) + { pthread_mutex_lock(&facilities.lightship.lock); facilities.lightship.t_last_vehicle = itss_time_get(); @@ -673,23 +733,23 @@ static int networking_indication(void* responder, uint8_t* msg, uint32_t msg_len pthread_mutex_unlock(&facilities.lightship.lock); } - cleanup: ASN_STRUCT_FREE(asn_DEF_NetworkingIndication, ni); return rv; } - -static int management_indication(void* responder, uint8_t* msg, uint32_t msg_len) { +static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len) +{ int rv = 0; uint8_t code = 0; - ManagementIndication_t* mi = calloc(1, sizeof(ManagementIndication_t)); + ManagementIndication_t *mi = calloc(1, sizeof(ManagementIndication_t)); - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_ManagementIndication, (void**) &mi, msg, msg_len); - if (dec.code) { + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_ManagementIndication, (void **)&mi, msg, msg_len); + if (dec.code) + { rv = 1; code = 1; itss_0send(responder, &code, 1); @@ -697,7 +757,8 @@ static int management_indication(void* responder, uint8_t* msg, uint32_t msg_len } itss_0send(responder, &code, 1); - if (mi->present == ManagementIndication_PR_attributes) { + if (mi->present == ManagementIndication_PR_attributes) + { itss_space_lock(); epv.space.latitude = mi->choice.attributes.coordinates.latitude; epv.space.latitude_conf = mi->choice.attributes.coordinates.latitudeConfidence; @@ -713,18 +774,20 @@ static int management_indication(void* responder, uint8_t* msg, uint32_t msg_len itss_space_unlock(); itss_trajectory_lock(); - if (mi->choice.attributes.trajectory) { + if (mi->choice.attributes.trajectory) + { epv.trajectory.len = mi->choice.attributes.trajectory->list.count; - for (int i = 0; i < mi->choice.attributes.trajectory->list.count; ++i) { + for (int i = 0; i < mi->choice.attributes.trajectory->list.count; ++i) + { epv.trajectory.path[i].latitude = mi->choice.attributes.trajectory->list.array[i]->latitude; epv.trajectory.path[i].longitude = mi->choice.attributes.trajectory->list.array[i]->longitude; - asn_INTEGER2ulong(&mi->choice.attributes.trajectory->list.array[i]->timestamp, (unsigned long long*) &epv.trajectory.path[i].timestamp); + asn_INTEGER2ulong(&mi->choice.attributes.trajectory->list.array[i]->timestamp, (unsigned long long *)&epv.trajectory.path[i].timestamp); } } itss_trajectory_unlock(); itss_time_lock(); - asn_INTEGER2ulong(&mi->choice.attributes.clock, (unsigned long long*) &epv.time.clock); + asn_INTEGER2ulong(&mi->choice.attributes.clock, (unsigned long long *)&epv.time.clock); itss_time_unlock(); } @@ -734,25 +797,28 @@ cleanup: return rv; } - -void* tx() { +void *tx() +{ int rv = 0; - itss_queue_t* queue = facilities.tx_queue; + itss_queue_t *queue = facilities.tx_queue; uint8_t code; - void* applications_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ); - void* transport_socket = itss_0connect(facilities.zmq.transport_address, ZMQ_REQ); - void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); + void *applications_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ); + void *transport_socket = itss_0connect(facilities.zmq.transport_address, ZMQ_REQ); + void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); - itss_queue_t* stream = itss_queue_new(); + itss_queue_t *stream = itss_queue_new(); - while (!facilities.exit) { + while (!facilities.exit) + { pthread_mutex_lock(&queue->lock); - while (!queue->len && !facilities.exit) { + while (!queue->len && !facilities.exit) + { pthread_cond_wait(&queue->trigger, &queue->lock); } - for (int i = 0; i < queue->len; ++i) { + for (int i = 0; i < queue->len; ++i) + { memcpy(stream->packet[i], queue->packet[i], queue->packet_len[i]); stream->packet_len[i] = queue->packet_len[i]; stream->destination[i] = queue->destination[i]; @@ -764,36 +830,41 @@ void* tx() { queue->len = 0; pthread_mutex_unlock(&queue->lock); - for (int i = 0; i < stream->len; ++i) { - switch (stream->destination[i]) { - case ITSS_TRANSPORT: - log_debug("-> %s ->[transport] | id:%08x size:%dB", - stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); - itss_0send(transport_socket, stream->packet[i], stream->packet_len[i]); - rv = itss_0recv_rt(&transport_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000); - if (rv == -1) { - log_error("-> %s ->[transport] | id:%08x size:%dB ", - stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); - } - break; - case ITSS_APPLICATIONS: - log_debug("-> %s ->[applications] | id:%08x size:%dB", - stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); - itss_0send(applications_socket, stream->packet[i], stream->packet_len[i]); - rv = itss_0recv_rt(&applications_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000); - if (rv == -1) { - log_error("-> %s ->[applications] | id:%08x size:%dB ", - stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); - } - break; - case ITSS_MANAGEMENT: - itss_0send(management_socket, stream->packet[i], stream->packet_len[i]); - rv = itss_0recv_rt(&management_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000); - if (rv == -1) { - log_error("-> %s ->[management] | id:%08x size:%dB ", - stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); - } - break; + for (int i = 0; i < stream->len; ++i) + { + switch (stream->destination[i]) + { + case ITSS_TRANSPORT: + log_debug("-> %s ->[transport] | id:%08x size:%dB", + stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]); + itss_0send(transport_socket, stream->packet[i], stream->packet_len[i]); + rv = itss_0recv_rt(&transport_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000); + if (rv == -1) + { + log_error("-> %s ->[transport] | id:%08x size:%dB ", + stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]); + } + break; + case ITSS_APPLICATIONS: + log_debug("-> %s ->[applications] | id:%08x size:%dB", + stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]); + itss_0send(applications_socket, stream->packet[i], stream->packet_len[i]); + rv = itss_0recv_rt(&applications_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000); + if (rv == -1) + { + log_error("-> %s ->[applications] | id:%08x size:%dB ", + stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]); + } + break; + case ITSS_MANAGEMENT: + itss_0send(management_socket, stream->packet[i], stream->packet_len[i]); + rv = itss_0recv_rt(&management_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000); + if (rv == -1) + { + log_error("-> %s ->[management] | id:%08x size:%dB ", + stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]); + } + break; } } } @@ -805,29 +876,31 @@ void* tx() { return NULL; } -static void sigh(int signum) { +static void sigh(int signum) +{ facilities.exit = true; uint8_t code = 0; - void* socket = itss_0connect(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); + void *socket = itss_0connect(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); itss_0send(socket, &code, sizeof(code)); itss_0close(socket); } -int main() { +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; + void *security_socket = NULL; - if (facilities_config()) { + if (facilities_config()) + { goto cleanup; } @@ -845,8 +918,8 @@ int main() { // Infrastructure pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL); - // CP - if (facilities.dissemination.active) + // CP + if (facilities.dissemination.active) pthread_create(&facilities.cp_service, NULL, cp_service, NULL); // SA @@ -856,11 +929,16 @@ int main() { tolling_init(facilities.station_type); // VC - if (facilities.coordination.active) { + if (facilities.coordination.active) + { coordination_init(); pthread_create(&facilities.vc_service, NULL, vc_service, NULL); } + // EVCSN + if (facilities.evcsnm_args.activate) + pthread_create(&facilities.evcsn_service, NULL, evcsn_service, NULL); + security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); uint8_t buffer[ITSS_SDU_MAX_LEN]; @@ -870,77 +948,89 @@ int main() { int32_t rl; - while (!facilities.exit) { + 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) { + 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 */ + switch (buffer[0]) + { /* source */ - case ITSS_INTERNAL: - break; + case ITSS_INTERNAL: + break; - case ITSS_NETWORKING: - networking_indication(facilities.zmq.responders[i].socket, buffer+1, rl); - break; + case ITSS_NETWORKING: + networking_indication(facilities.zmq.responders[i].socket, buffer + 1, rl); + break; + + case ITSS_TRANSPORT: + 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) + { + transport_indication(facilities.zmq.responders[i].socket, &security_socket, buffer + 1, rl); - case ITSS_TRANSPORT: - 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; - } + facilities.id.change.stage = ID_CHANGE_INACTIVE; pthread_mutex_unlock(&facilities.id.change.lock); - - if (!in_idchange) { - transport_indication(facilities.zmq.responders[i].socket, &security_socket, buffer+1, rl); - - 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); - - 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); - break; - - case ITSS_SECURITY: - security_indication(facilities.zmq.responders[i].socket, buffer+1, rl); - break; - - default: - log_debug("<- unrecognized service"); + } + else + { code = 1; itss_0send(facilities.zmq.responders[i].socket, &code, 1); - continue; + } + 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); + + 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); + break; + + case ITSS_SECURITY: + security_indication(facilities.zmq.responders[i].socket, buffer + 1, rl); + break; + + default: + log_debug("<- unrecognized service"); + code = 1; + itss_0send(facilities.zmq.responders[i].socket, &code, 1); + continue; } } } @@ -948,6 +1038,7 @@ int main() { // Exit cleanup: + pthread_join(facilities.evcsn_service, NULL); pthread_join(facilities.ca_service, NULL); pthread_join(facilities.den_service, NULL); pthread_join(facilities.infrastructure_service, NULL); @@ -960,7 +1051,8 @@ cleanup: pthread_join(facilities.vc_service, NULL); itss_0close(security_socket); - for (int i = 0; i < facilities.zmq.n_responders; ++i) { + for (int i = 0; i < facilities.zmq.n_responders; ++i) + { itss_0close(facilities.zmq.responders[i].socket); } itss_0destroy(); diff --git a/src/facilities.h b/src/facilities.h index a1efd02..c4b7123 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -12,6 +12,7 @@ #include "saem.h" #include "tpm.h" #include "vcm.h" +#include "evcsnm.h" #include #include @@ -34,6 +35,7 @@ typedef struct facilities { pthread_t cp_service; pthread_t sa_service; pthread_t vc_service; + pthread_t evcsn_service; // ZMQ struct { @@ -70,6 +72,9 @@ typedef struct facilities { // PC coordination_t coordination; + // EVCSN + evcsnm_args_t evcsnm_args; + // Logging struct { bool recorder;