diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..5ca74dd --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,11 @@ +{ + "files.associations": { + "prereservation-id.h": "c", + "constr_sequence.h": "c", + "array": "c", + "string_view": "c", + "initializer_list": "c", + "nativeenumerated.h": "c", + "asn_application.h": "c" + } +} \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index f11be2c..ca34548 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,6 +10,7 @@ ADD_EXECUTABLE(it2s-itss-facilities saem.c tpm.c vcm.c + evm.c ) TARGET_LINK_LIBRARIES(it2s-itss-facilities @@ -28,6 +29,8 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-saem -lit2s-asn-tpm -lit2s-asn-vcm + -lit2s-asn-evcsnm + -lit2s-asn-evrsrm -lit2s-asn-verco -lit2s-tender -lit2s-obd diff --git a/src/config.c b/src/config.c index b9cbf14..edf0365 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.evm_args.activate = config->facilities.evcsnm.activate; + // Replay facilities.replay = config->networking.replay.activate; diff --git a/src/evm.c b/src/evm.c new file mode 100644 index 0000000..882c24a --- /dev/null +++ b/src/evm.c @@ -0,0 +1,365 @@ +#include "evm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "facilities.h" + +static UTF8String_t *create_utf8_from_string(const char *string, size_t length) { + UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t)); + utf8_string->buf = calloc(length, sizeof(uint8_t)); + utf8_string->size = length; + memcpy(utf8_string->buf, string, length); + return utf8_string; +} + +static UTF8String_t *create_empty_utf8_string() { + UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t)); + utf8_string->size = 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; + + EvcsnPdu_t *evcsnm = calloc(1, sizeof(EvcsnPdu_t)); + evcsnm->header.protocolVersion = 2; + evcsnm->header.messageID = 1; + pthread_mutex_lock(&facilities.id.lock); + evcsnm->header.stationID = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + + 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); + evcsnm->evcsn.poiHeader.relayCapable = 1; + + 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 = sizeof(void *) * 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 = 405970830; + cs0->chargingStationLocation.longitude = -86628610; + 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 = HeadingValue_unavailable; + cs0->accessibility = *create_utf8_from_string("Free Access", strlen("Free Access")); + cs0->pricing = *create_utf8_from_string(".15 €/kWh", strlen(".15 €/kWh")); + cs0->openingDaysHours = *create_utf8_from_string("Always", strlen("Always")); + const char *booking_url = "ccam.av.it.pt"; + cs0->bookingContactInfo = create_utf8_from_string(booking_url, strlen(booking_url)); + cs0->chargingSpotsAvailable.list.array = calloc(1, sizeof(void *)); + cs0->chargingSpotsAvailable.list.count = 1; + cs0->chargingSpotsAvailable.list.size = sizeof(void *) * 1; + cs0->chargingSpotsAvailable.list.array[0] = calloc(1, sizeof(struct ItsChargingSpotDataElements)); + struct ItsChargingSpotDataElements *cs_elem0 = cs0->chargingSpotsAvailable.list.array[0]; + cs_elem0->energyAvailability = *create_utf8_from_string("Max: 30kW", strlen("Max: 30kW")); + cs_elem0->type.buf = calloc(1, sizeof(uint8_t)); + cs_elem0->type.size = 1; + cs_elem0->type.bits_unused = 0; + cs_elem0->type.buf[0] = 0x03; + cs_elem0->typeOfReceptacle.buf = calloc(1, sizeof(uint8_t)); + cs_elem0->typeOfReceptacle.size = 1; + cs_elem0->typeOfReceptacle.bits_unused = 0; + cs_elem0->typeOfReceptacle.buf[0] = 0x0D; + cs_elem0->parkingPlacesData = calloc(1, sizeof(struct ParkingPlacesData)); + cs_elem0->parkingPlacesData->list.array = calloc(2, sizeof(void *)); + cs_elem0->parkingPlacesData->list.count = 2; + cs_elem0->parkingPlacesData->list.size = sizeof(void *) * 2; + // As requested onnly 2 parking places are available + for (int parckingPlaceIndex = 0; parckingPlaceIndex < 2; parckingPlaceIndex++) { + cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex] = calloc(1, sizeof(struct SpotAvailability)); + struct SpotAvailability *spot = cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex]; + spot->maxWaitingTimeMinutes = 0; + spot->blocking = 1; + } + 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) { + log_error("[ca] failed encoding evcsnm (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + *evcsnm_len = (enc.encoded + 7) / 8; + +cleanup: + ASN_STRUCT_FREE(asn_DEF_EvcsnPdu, evcsnm); + return rv; +} + +static int pre_reservation_id = 0; + +int evrsrm_recv(EV_RSR_t *evrsr_request) { + int rv = 0; + TransportRequest_t *tr = calloc(1, sizeof(TransportRequest_t)); + tr->present = TransportRequest_PR_packet; + TransportPacketRequest_t *tpr = &tr->choice.packet; + tpr->present = TransportPacketRequest_PR_btp; + BTPPacketRequest_t *bpr = &tpr->choice.btp; + + bpr->btpType = BTPType_btpB; + + bpr->gn.destinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i) { + bpr->gn.destinationAddress.buf[i] = 0xff; + } + bpr->gn.destinationAddress.size = 6; + + bpr->gn.packetTransportType = PacketTransportType_shb; + + bpr->destinationPort = Port_evrsr; + + bpr->gn.trafficClass = 2; + + bpr->data.buf = malloc(512); + + // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs + + bpr->gn.securityProfile.sign = true; + + FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; + fmi->itsMessageType = ItsMessageType_evrsr; + fmi->data.buf = malloc(512); + + uint8_t tr_oer[1024]; + uint8_t fi_oer[1024]; + tr_oer[0] = 4; // Facilities + fi_oer[0] = 4; + if (!evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage || + !evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_reservationRequestMessage) { + rv = 1; + goto cleanup; + } + int evrsr_response; + if (evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage) + evrsr_response = evrsrm_pre_reservation_response(bpr->data.buf, (uint32_t *)&bpr->data.size); + // else if (evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_reservationRequestMessage) + // evrsr_response = evrsrm_reservation_response(evrsr_request); + if (evrsr_response != 0) { + rv = 1; + goto cleanup; + } + 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) bpr->gn.communicationProfile = 1; + + uint32_t id = itss_id(bpr->data.buf, bpr->data.size); + bpr->id = id; + fmi->id = id; + + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, tr_oer + 1, 1024); + if (enc.encoded == -1) { + log_error("[ca] failed encoding facilities indication (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + tr->choice.packet.choice.btp.data.size = (enc.encoded + 7) / 8; + + enc = uper_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer, 1024); + if (enc.encoded == -1) { + log_error("[ev] failed encoding transport request (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer + 1, 1023); + if (enc_fdi.encoded == -1) { + log_error("[ev] encoding FI for evrsrm failed"); + rv = 1; + goto cleanup; + } + itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded + 1, ITSS_TRANSPORT, id, "TR.packet.btp"); + + itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); + +cleanup: + log_debug("[ev] evrsrm_recv done"); + return rv; +} + +static int evrsrm_pre_reservation_response(uint8_t *evrsrm_oer, uint32_t *evrsrm_len) { + int rv = 0; + EV_RSR_t *evrsr_response = calloc(1, sizeof(EV_RSR_t)); + evrsr_response->header.protocolVersion = 1; + evrsr_response->header.messageID = 1; + pthread_mutex_lock(&facilities.id.lock); + evrsr_response->header.stationID = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + evrsr_response->messageBody.present = EV_RSR_MessageBody_PR_preReservationResponseMessage; + + PreReservationResponseMessage_t response = evrsr_response->messageBody.choice.preReservationResponseMessage; + char *pre_reservation_id = calloc(1, sizeof(char) * 10); + sprintf(pre_reservation_id, "%d", pre_reservation_id); + response.preReservation_ID = *create_utf8_from_string(pre_reservation_id, strlen(pre_reservation_id)); + + response.availabilityStatus = 0; + + response.supportedPaymentTypes.buf = calloc(1, sizeof(uint8_t)); + response.supportedPaymentTypes.bits_unused = 0; + response.supportedPaymentTypes.size = 1; + response.supportedPaymentTypes.buf[0] = 0x00; + + response.preReservationExpirationTime = 0; + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EV_RSR, NULL, evrsr_response, evrsrm_oer, 512); + if (enc.encoded == -1) { + log_error("[ca] failed encoding evrsrm (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + *evrsrm_len = (enc.encoded + 7) / 8; + pre_reservation_id++; +cleanup: + ASN_STRUCT_FREE(asn_DEF_EV_RSR, evrsr_response); + return rv; +} + +int evcsnm_check(EvcsnPdu_t *evcsnm) { + return 0; +} + +int evrsrm_check(EV_RSR_t *evrsrm) { + return 0; +} + +enum EVM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len) { + return 0; +} + +enum EVM_CHECK_R check_evrsrm(BTPPacketIndication_t *bpi, EV_RSR_t *evrsrm, uint8_t *ssp, uint32_t ssp_len) { + return 0; +} + +void *evcsn_service() { + int rv = 0; + + TransportRequest_t *tr = calloc(1, sizeof(TransportRequest_t)); + tr->present = TransportRequest_PR_packet; + TransportPacketRequest_t *tpr = &tr->choice.packet; + tpr->present = TransportPacketRequest_PR_btp; + BTPPacketRequest_t *bpr = &tpr->choice.btp; + + bpr->btpType = BTPType_btpB; + + bpr->gn.destinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i) { + bpr->gn.destinationAddress.buf[i] = 0xff; + } + bpr->gn.destinationAddress.size = 6; + + bpr->gn.packetTransportType = PacketTransportType_shb; + + bpr->destinationPort = Port_poi; + + bpr->gn.trafficClass = 2; + + bpr->data.buf = malloc(512); + + // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs + + bpr->gn.securityProfile.sign = true; + + FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; + fmi->itsMessageType = ItsMessageType_poi; + fmi->data.buf = malloc(512); + + uint8_t tr_oer[1024]; + uint8_t fi_oer[1024]; + tr_oer[0] = 4; // Facilities + fi_oer[0] = 4; + while (!facilities.exit) { + usleep(1000 * 1000); + if (facilities.evm_args.activate) { + rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size); + if (rv) { + continue; + } + memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size); + fmi->data.size = bpr->data.size; + + uint32_t id = itss_id(bpr->data.buf, bpr->data.size); + bpr->id = id; + fmi->id = id; + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer + 1, 1023); + if (enc.encoded == -1) { + log_error("encoding TR for evcsnm failed"); + continue; + } + + asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer + 1, 1023); + if (enc_fdi.encoded == -1) { + log_error("encoding FI for evcsnm failed"); + continue; + } + + itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded + 1, ITSS_TRANSPORT, id, "TR.packet.btp"); + + itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); + + // 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_evcsn, NULL, bpr->data.buf, bpr->data.size); + } + + if (facilities.logging.recorder) { + uint16_t buffer_len = 2048; + uint8_t buffer[buffer_len]; + int e = itss_management_record_packet_sdu( + buffer, + buffer_len, + bpr->data.buf, + bpr->data.size, + id, + itss_time_get(), + ITSS_FACILITIES, + true); + if (e != -1) { + itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"); + } + } + } + } + + ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); + + return NULL; +} diff --git a/src/evm.h b/src/evm.h new file mode 100644 index 0000000..c58dac3 --- /dev/null +++ b/src/evm.h @@ -0,0 +1,79 @@ +#ifndef FACILITIES_EVM_H +#define FACILITIES_EVM_H + +#include +#include +#include +#include + +#include +#include +#include + +#include + +enum EVM_CHECK_R { + EVM_OK, + EVM_INVALID, + EVM_BAD_PERMISSIONS +}; + +/* + * @brief Analyzes a received EVCSNM + * + * @return A EVM check code + */ +enum EVM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len); + +/* + * @brief Analyzes a received EVRSRM + * + * @return A EVM check code + */ +enum EVM_CHECK_R check_evrsrm(BTPPacketIndication_t *bpi, EV_RSR_t *evrsrm, uint8_t *ssp, uint32_t ssp_len); + +/* + * @brief Main CA service + * + * @return NULL + */ +void* evcsn_service(); + +/** + * @brief Creates a EVCSNM response and sends it + * @param evcsnm_request The EVCSNM request + * + * @return 0 on success, 1 otherwise + */ +int evrsrm_recv(EV_RSR_t *evrsr_request); + + +/** + * @brief Creates a pre reservation response. + * @param evrsrm_oer The encoded pre reservation response + * @param evrsrm_len The length of the encoded pre reservation response + * + * @return The reservation response or NULL if the request isn't a Pre reservation request + */ + +static int evrsrm_pre_reservation_response(uint8_t *evrsrm_oer, uint32_t *evrsrm_len); + +/** + * Analyses a evcsnm + * @param evcsnm The evcsnm to be analyzed + * @return 0 on success, other value otherwise + */ +int evcsnm_check(EvcsnPdu_t* evcsnm); + +/** + * Analyses a evrsrm + * @param evrsrm The evrsrm to be analyzed + * @return 0 on success, other value otherwise + */ +int evrsrm_check(EV_RSR_t *evrsrm); + +typedef struct evm_args { + bool activate; +} evm_args_t; + +#endif diff --git a/src/facilities.c b/src/facilities.c index f16b367..5196d48 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -1,65 +1,64 @@ #include "facilities.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + #include "cam.h" #include "config.h" -#include "denm.h" -#include "infrastructure.h" -#include "indications.h" -#include "requests.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" -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - 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); + 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; @@ -82,8 +81,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t goto cleanup; } - TransportPacketIndication_t* tpi = &ti->choice.packet; // TODO - // + TransportPacketIndication_t *tpi = &ti->choice.packet; // TODO + // bool fwd = false; @@ -92,13 +91,13 @@ 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); + log_debug("<- TI.packet.btp | id:%08x size:%dB", (uint32_t)id, msg_len); // Parse message switch (tpi->choice.btp.destinationPort) { case Port_cam: @@ -158,6 +157,22 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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; + + case Port_evrsr: /* EVRSRM */ + its_msg_descriptor = &asn_DEF_EV_RSR; + its_msg = calloc(1, sizeof(EV_RSR_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; @@ -166,7 +181,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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); + 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; @@ -174,7 +189,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t } // Get permisisons - uint8_t* ssp = NULL; + uint8_t *ssp = NULL; uint16_t ssp_len; if (tpi->choice.btp.gn.securityPermissions) { ssp = tpi->choice.btp.gn.securityPermissions->ssp.buf; @@ -182,7 +197,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t } // Get neighbour certificate ID - uint8_t* neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL; + uint8_t *neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL; // Manage message switch (tpi->choice.btp.destinationPort) { @@ -198,12 +213,11 @@ static int transport_indication(void* responder, void** security_socket, uint8_t } break; - case Port_denm: - ; + case Port_denm:; #ifdef DEBUG - uint8_t* xml_denm = malloc(32768); + 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); + 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)) { @@ -263,7 +277,20 @@ static int transport_indication(void* responder, void** security_socket, uint8_t } fwd = true; break; + case Port_poi: + if (facilities.evm_args.activate) { + evcsnm_check(its_msg); + fwd = true; + } + break; + case Port_evrsr: + if (facilities.evm_args.activate) { + evrsrm_check(its_msg); + fwd = true; + EV_RSR_t *evrsr_request = (EV_RSR_t *)its_msg; + evrsrm_recv(evrsr_request); + } default: break; } @@ -282,7 +309,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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); + tlsc_t *tlsc = tolling_tlsc_get(tpi->choice.tcp.sourceAddress->buf, 7011); if (tlsc) { id = tlsc->id; } else { @@ -295,14 +322,14 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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); + 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] 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) { + if (oer_decode(NULL, &asn_DEF_SecurityReply, (void **)&srep, b_rx, rl).code) { log_error("SecurityReply.tlsRecv decode failure"); rv = 1; goto cleanup; @@ -311,14 +338,14 @@ static int transport_indication(void* responder, void** security_socket, uint8_t if (srep->returnCode == SecurityReplyReturnCode_rejected) { log_error("SecurityReply.tlsRecv rejected"); - SecurityRequest_t* sREQ = calloc(1, sizeof(SecurityRequest_t)); + 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); + 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; @@ -332,7 +359,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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; + 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); @@ -358,18 +385,17 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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); + 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"); + 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); + 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; @@ -378,24 +404,24 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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; + // Fwd to [applications] + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; - fmi->id = id; + fmi->id = id; - fmi->itsMessageType = 7011; + 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); + 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); + 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"); + itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); } } } @@ -413,7 +439,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t 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; @@ -424,13 +450,13 @@ static int transport_indication(void* responder, void** security_socket, uint8_t memcpy(fmi->data.buf, tpi->choice.btp.data.buf, tpi->choice.btp.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); + 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"); + itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); } - // Logging + // Logging if (facilities.logging.dbms) { pthread_mutex_lock(&facilities.id.lock); uint32_t station_id = facilities.id.station_id; @@ -439,16 +465,16 @@ static int transport_indication(void* responder, void** security_socket, uint8_t } 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); + 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"); + itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, tpi->choice.btp.id, "MReq.packet.set"); } } @@ -465,12 +491,12 @@ 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); + 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); @@ -522,18 +548,17 @@ 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); + 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; @@ -549,8 +574,8 @@ 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; @@ -568,8 +593,7 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m pthread_mutex_lock(&facilities.lightship.lock); break; - case SecurityIdChangeEventType_commit: - ; + case SecurityIdChangeEventType_commit:; facilities.id.change.stage = ID_CHANGE_COMMIT; // Reset lightship @@ -619,7 +643,6 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m log_error("[networking]<- unhandled idChangeEvent command type"); rv = 1; goto cleanup; - } sr->present = SecurityResponse_PR_idChangeEvent; @@ -644,14 +667,14 @@ 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); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_NetworkingIndication, (void **)&ni, msg, msg_len); if (dec.code) { rv = 1; code = 1; @@ -664,31 +687,28 @@ static int networking_indication(void* responder, uint8_t* msg, uint32_t msg_len 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(); facilities.lightship.is_vehicle_near = true; 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); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_ManagementIndication, (void **)&mi, msg, msg_len); if (dec.code) { rv = 1; code = 1; @@ -718,13 +738,13 @@ static int management_indication(void* responder, uint8_t* msg, uint32_t msg_len 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,18 +754,17 @@ 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) { pthread_mutex_lock(&queue->lock); @@ -767,31 +786,31 @@ void* tx() { 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]); + 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]); + 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]); + 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]); + 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]); + log_error("-> %s ->[management] | id:%08x size:%dB ", + stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]); } break; } @@ -808,7 +827,7 @@ void* tx() { 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); } @@ -818,14 +837,13 @@ int main() { 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()) { goto cleanup; @@ -845,8 +863,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 @@ -861,6 +879,10 @@ int main() { pthread_create(&facilities.vc_service, NULL, vc_service, NULL); } + // EVCSN + if (facilities.evm_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]; @@ -882,7 +904,7 @@ int main() { break; case ITSS_NETWORKING: - networking_indication(facilities.zmq.responders[i].socket, buffer+1, rl); + networking_indication(facilities.zmq.responders[i].socket, buffer + 1, rl); break; case ITSS_TRANSPORT: @@ -891,17 +913,17 @@ int main() { 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); + 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; + code = 1; itss_0send(facilities.zmq.responders[i].socket, &code, 1); } break; @@ -912,28 +934,28 @@ int main() { 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); + 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; + 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); + management_indication(facilities.zmq.responders[i].socket, buffer + 1, rl); break; case ITSS_SECURITY: - security_indication(facilities.zmq.responders[i].socket, buffer+1, rl); + security_indication(facilities.zmq.responders[i].socket, buffer + 1, rl); break; default: @@ -948,6 +970,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); diff --git a/src/facilities.h b/src/facilities.h index a1efd02..2722045 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -12,6 +12,7 @@ #include "saem.h" #include "tpm.h" #include "vcm.h" +#include "evm.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 + evm_args_t evm_args; + // Logging struct { bool recorder; diff --git a/src/requests.c b/src/requests.c index f452775..7fe3ff0 100644 --- a/src/requests.c +++ b/src/requests.c @@ -1,24 +1,23 @@ -#include "facilities.h" -#include "cam.h" -#include "denm.h" -#include "infrastructure.h" #include "requests.h" -#include "cpm.h" +#include #include #include - -#include -#include #include - -#include -#include #include +#include #include #include +#include -int facilities_request_result_accepted(void* responder) { +#include "cam.h" +#include "cpm.h" +#include "denm.h" +#include "evm.h" +#include "facilities.h" +#include "infrastructure.h" + +int facilities_request_result_accepted(void *responder) { int rv = 0; FacilitiesReply_t *fr = calloc(1, sizeof(FacilitiesReply_t)); @@ -32,7 +31,7 @@ int facilities_request_result_accepted(void* responder) { return rv; } -int facilities_request_result_rejected(void* responder) { +int facilities_request_result_rejected(void *responder) { int rv = 0; FacilitiesReply_t *fr = calloc(1, sizeof(FacilitiesReply_t)); @@ -46,15 +45,14 @@ int facilities_request_result_rejected(void* responder) { return rv; } -int facilities_request_single_message(void* responder, FacilitiesMessageRequest_t* frm) { +int facilities_request_single_message(void *responder, FacilitiesMessageRequest_t *frm) { int rv = 0; - - TransportRequest_t* tr = calloc(1, sizeof(TransportRequest_t)); + TransportRequest_t *tr = calloc(1, sizeof(TransportRequest_t)); tr->present = TransportRequest_PR_packet; - TransportPacketRequest_t* tpr = &tr->choice.packet; + TransportPacketRequest_t *tpr = &tr->choice.packet; tpr->present = TransportPacketRequest_PR_btp; - BTPPacketRequest_t* bpr = &tpr->choice.btp; + BTPPacketRequest_t *bpr = &tpr->choice.btp; void *its_msg = NULL; asn_TYPE_descriptor_t *its_msg_def = NULL; @@ -100,7 +98,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ break; - case 2044: //VERCOe + case 2044: // VERCOe its_msg_def = &asn_DEF_VERCOe; its_msg = calloc(1, sizeof(VERCOe_t)); bpr->destinationPort = 2044; @@ -108,6 +106,14 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ bpr->gn.trafficClass = 2; break; + case ItsMessageType_evrsr: + its_msg_def = &asn_DEF_EV_RSR; + its_msg = calloc(1, sizeof(EV_RSR_t)); + bpr->destinationPort = Port_evrsr; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 1; + break; + default: log_error("unrecognized FR message type (%lld)", frm->itsMessageType); facilities_request_result_rejected(responder); @@ -115,7 +121,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ goto cleanup; } - asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void**) &its_msg, frm->data.buf, frm->data.size); + asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void **)&its_msg, frm->data.buf, frm->data.size); if (dec.code) { log_warn("invalid FR %s received", its_msg_def->name); facilities_request_result_rejected(responder); @@ -136,52 +142,49 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ if (frm->itsMessageType == ItsMessageType_denm) { managed_msg = true; - uint8_t event_type = event_manage(its_msg, &id, NULL, 0); + uint8_t event_type = event_manage(its_msg, &id, NULL, 0); // Do not free its_msg! event_manage takes care of the msg // id will get set to another val if EVENT NEW or UPDATE or CANCELLATION or NEGATION if (event_type != EVENT_NEW && - event_type != EVENT_UPDATE && - event_type != EVENT_CANCELLATION && - event_type != EVENT_NEGATION) { - + event_type != EVENT_UPDATE && + event_type != EVENT_CANCELLATION && + event_type != EVENT_NEGATION) { fwd = false; } if (event_type == EVENT_UPDATE || - event_type == EVENT_CANCELLATION || - event_type == EVENT_NEGATION) { - + event_type == EVENT_CANCELLATION || + event_type == EVENT_NEGATION) { is_update = true; } if (fwd) { // set stationID pthread_mutex_lock(&facilities.id.lock); - ((DENM_t*)its_msg)->header.stationID = facilities.id.station_id; + ((DENM_t *)its_msg)->header.stationID = facilities.id.station_id; pthread_mutex_unlock(&facilities.id.lock); // Set only one trace if (facilities.station_type != 15) { pthread_mutex_lock(&facilities.lightship.lock); if (facilities.lightship.path_history_len > 0) { - - if (!((DENM_t*)its_msg)->denm.location) { - ((DENM_t*)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t)); + if (!((DENM_t *)its_msg)->denm.location) { + ((DENM_t *)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t)); } - ((DENM_t*)its_msg)->denm.location->traces.list.count = 1; - ((DENM_t*)its_msg)->denm.location->traces.list.size = 1 * sizeof(void*); - ((DENM_t*)its_msg)->denm.location->traces.list.array = malloc(1 * sizeof(void*)); - ((DENM_t*)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(PathHistory_t)); - PathHistory_t* ph = ((DENM_t*)its_msg)->denm.location->traces.list.array[0]; + ((DENM_t *)its_msg)->denm.location->traces.list.count = 1; + ((DENM_t *)its_msg)->denm.location->traces.list.size = 1 * sizeof(void *); + ((DENM_t *)its_msg)->denm.location->traces.list.array = malloc(1 * sizeof(void *)); + ((DENM_t *)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(PathHistory_t)); + PathHistory_t *ph = ((DENM_t *)its_msg)->denm.location->traces.list.array[0]; - pos_point_t** path_history = facilities.lightship.path_history; + pos_point_t **path_history = facilities.lightship.path_history; uint16_t path_history_len = facilities.lightship.path_history_len; - ph->list.array = malloc((path_history_len) * sizeof(void*)); + ph->list.array = malloc((path_history_len) * sizeof(void *)); ph->list.count = path_history_len; - ph->list.size = (path_history_len) * sizeof(void*); + ph->list.size = (path_history_len) * sizeof(void *); - ph->list.array[0] = calloc(1,sizeof(PathPoint_t)); + ph->list.array[0] = calloc(1, sizeof(PathPoint_t)); itss_space_lock(); if (path_history[0]->alt != AltitudeValue_unavailable && epv.space.altitude != AltitudeValue_unavailable) { @@ -197,50 +200,49 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ } if (path_history[0]->lon != Longitude_unavailable && epv.space.longitude != Longitude_unavailable) { - ph->list.array[0]->pathPosition.deltaLongitude =path_history[0]->lon - epv.space.longitude; + ph->list.array[0]->pathPosition.deltaLongitude = path_history[0]->lon - epv.space.longitude; } else { ph->list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; } itss_space_unlock(); - ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t)); - *ph->list.array[0]->pathDeltaTime = (itss_time_get() - path_history[0]->ts)/10; + ph->list.array[0]->pathDeltaTime = calloc(1, sizeof(PathDeltaTime_t)); + *ph->list.array[0]->pathDeltaTime = (itss_time_get() - path_history[0]->ts) / 10; for (int i = 1; i < path_history_len; ++i) { - ph->list.array[i] = calloc(1,sizeof(PathPoint_t)); + ph->list.array[i] = calloc(1, sizeof(PathPoint_t)); - if (path_history[i]->alt != AltitudeValue_unavailable && path_history[i-1]->alt != AltitudeValue_unavailable) { - ph->list.array[i]->pathPosition.deltaAltitude = path_history[i]->alt - path_history[i-1]->alt; + if (path_history[i]->alt != AltitudeValue_unavailable && path_history[i - 1]->alt != AltitudeValue_unavailable) { + ph->list.array[i]->pathPosition.deltaAltitude = path_history[i]->alt - path_history[i - 1]->alt; } else { ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; } - if (path_history[i]->lat != Latitude_unavailable && path_history[i-1]->lat != Latitude_unavailable) { - ph->list.array[i]->pathPosition.deltaLatitude = path_history[i]->lat - path_history[i-1]->lat; + if (path_history[i]->lat != Latitude_unavailable && path_history[i - 1]->lat != Latitude_unavailable) { + ph->list.array[i]->pathPosition.deltaLatitude = path_history[i]->lat - path_history[i - 1]->lat; } else { ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; } - if (path_history[i]->lon != Longitude_unavailable && path_history[i-1]->lon != Longitude_unavailable) { - ph->list.array[i]->pathPosition.deltaLongitude = path_history[i]->lon - path_history[i-1]->lon; + if (path_history[i]->lon != Longitude_unavailable && path_history[i - 1]->lon != Longitude_unavailable) { + ph->list.array[i]->pathPosition.deltaLongitude = path_history[i]->lon - path_history[i - 1]->lon; } else { ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; } - ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t)); - *ph->list.array[i]->pathDeltaTime = (path_history[i-1]->ts - path_history[i]->ts)/10; - + ph->list.array[i]->pathDeltaTime = calloc(1, sizeof(PathDeltaTime_t)); + *ph->list.array[i]->pathDeltaTime = (path_history[i - 1]->ts - path_history[i]->ts) / 10; } } pthread_mutex_unlock(&facilities.lightship.lock); } // get, set retransmission, duration - if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) { - transmission_interval = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.transmissionInterval ); + if (((DENM_t *)its_msg)->denm.management.transmissionInterval) { + transmission_interval = *((uint32_t *)((DENM_t *)its_msg)->denm.management.transmissionInterval); - if ( ((DENM_t*)its_msg)->denm.management.validityDuration ) { - transmission_duration = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.validityDuration ) * 1000; + if (((DENM_t *)its_msg)->denm.management.validityDuration) { + transmission_duration = *((uint32_t *)((DENM_t *)its_msg)->denm.management.validityDuration) * 1000; } else { transmission_duration = 600 * 1000; } @@ -249,44 +251,43 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ } else if (frm->itsMessageType == ItsMessageType_ivim) { managed_msg = true; - uint8_t service_type = service_eval(SERVICE_IVI, its_msg, &id, NULL, 0); + uint8_t service_type = service_eval(SERVICE_IVI, its_msg, &id, NULL, 0); if (service_type != SERVICE_NEW && - service_type != SERVICE_UPDATE && - service_type != SERVICE_CANCELLATION && - service_type != SERVICE_NEGATION) { - + service_type != SERVICE_UPDATE && + service_type != SERVICE_CANCELLATION && + service_type != SERVICE_NEGATION) { fwd = false; } if (service_type == SERVICE_UPDATE || - service_type == SERVICE_CANCELLATION || - service_type == SERVICE_NEGATION) { - + service_type == SERVICE_CANCELLATION || + service_type == SERVICE_NEGATION) { is_update = true; } if (fwd) { uint64_t valid_to, valid_from; - if (!((IVIM_t*) its_msg)->ivi.mandatory.validFrom) { + if (!((IVIM_t *)its_msg)->ivi.mandatory.validFrom) { valid_from = itss_time_get(); } else { - asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validFrom, (unsigned long long*) &valid_from); + asn_INTEGER2ulong((INTEGER_t *)((IVIM_t *)its_msg)->ivi.mandatory.validFrom, (unsigned long long *)&valid_from); } - if (!((IVIM_t*) its_msg)->ivi.mandatory.validTo) { + if (!((IVIM_t *)its_msg)->ivi.mandatory.validTo) { valid_to = valid_from + facilities.infrastructure.default_service_duration; } else { - asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validTo, (unsigned long long*) &valid_to); + asn_INTEGER2ulong((INTEGER_t *)((IVIM_t *)its_msg)->ivi.mandatory.validTo, (unsigned long long *)&valid_to); } transmission_start = valid_from; transmission_interval = facilities.infrastructure.replay_interval; transmission_duration = valid_to - valid_from; } + } else if (frm->itsMessageType == ItsMessageType_evrsr) { + managed_msg = true; } - if (!facilities.replay) { transmission_interval = 0; transmission_duration = 0; @@ -311,7 +312,8 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ bpr->data.size = (enc.encoded + 7) / 8; bpr->gn.destinationAddress.buf = malloc(6); - for (int i = 0; i < 6; ++i) bpr->gn.destinationAddress.buf[i] = 0xff; + for (int i = 0; i < 6; ++i) + bpr->gn.destinationAddress.buf[i] = 0xff; bpr->gn.destinationAddress.size = 6; if (transmission_start) { @@ -337,7 +339,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ bpr->gn.securityProfile.sign = true; uint8_t tr_oer[2048]; - tr_oer[0] = 4; // [facilities] service id + tr_oer[0] = 4; // [facilities] service id enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer + 1, 2047); if (enc.encoded == -1) { log_error("failed encoding TR (%s)", enc.failed_type->name); @@ -345,7 +347,7 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ goto cleanup; } - itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded+1, ITSS_TRANSPORT, id, "TR.packet.btp"); + itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded + 1, ITSS_TRANSPORT, id, "TR.packet.btp"); } // Logging @@ -353,31 +355,32 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ uint16_t buffer_len = 2048; uint8_t buffer[buffer_len]; int e = itss_management_record_packet_sdu( - buffer, - buffer_len, - frm->data.buf, - frm->data.size, - id, - itss_time_get(), - ITSS_FACILITIES, - true); + buffer, + buffer_len, + frm->data.buf, + frm->data.size, + id, + itss_time_get(), + ITSS_FACILITIES, + true); if (e != -1) { - itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"); + itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"); } } cleanup: - if (its_msg_def && !managed_msg) ASN_STRUCT_FREE(*its_msg_def, its_msg); + if (its_msg_def && !managed_msg) + ASN_STRUCT_FREE(*its_msg_def, its_msg); ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); return rv; } -int facilities_request_active_episodes(void* responder, FacilitiesRequest_t* freq) { +int facilities_request_active_episodes(void *responder, FacilitiesRequest_t *freq) { int rv = 0; - FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t)); - uint8_t* frep_oer = NULL; + FacilitiesReply_t *frep = calloc(1, sizeof(FacilitiesReply_t)); + uint8_t *frep_oer = NULL; frep->present = FacilitiesReply_PR_data; frep->choice.data.present = FacilitiesDataReply_PR_episodes; @@ -476,25 +479,25 @@ cleanup: return rv; } -int facilities_request_attribute_types(void* responder, FacilitiesRequest_t* freq) { +int facilities_request_attribute_types(void *responder, FacilitiesRequest_t *freq) { int rv = 0; - FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t)); + FacilitiesReply_t *frep = calloc(1, sizeof(FacilitiesReply_t)); frep->present = FacilitiesReply_PR_data; int nra = freq->choice.data.choice.attributeTypes.list.count; frep->choice.data.present = FacilitiesDataReply_PR_attributes; frep->choice.data.choice.attributes.list.count = nra; - frep->choice.data.choice.attributes.list.size = sizeof(void*) * nra; - frep->choice.data.choice.attributes.list.array = malloc(sizeof(void*) * nra); + frep->choice.data.choice.attributes.list.size = sizeof(void *) * nra; + frep->choice.data.choice.attributes.list.array = malloc(sizeof(void *) * nra); for (int j = 0; j < nra; ++j) { switch (*freq->choice.data.choice.attributeTypes.list.array[j]) { case FacilitiesAttributeType_stationId: - frep->choice.data.choice.attributes.list.array[j] = calloc(1, sizeof(FacilitiesAttribute_t) ); + frep->choice.data.choice.attributes.list.array[j] = calloc(1, sizeof(FacilitiesAttribute_t)); frep->choice.data.choice.attributes.list.array[j]->data.size = 8; frep->choice.data.choice.attributes.list.array[j]->data.buf = malloc(8); pthread_mutex_lock(&facilities.id.lock); - *((uint64_t*) frep->choice.data.choice.attributes.list.array[j]->data.buf) = facilities.id.station_id; + *((uint64_t *)frep->choice.data.choice.attributes.list.array[j]->data.buf) = facilities.id.station_id; pthread_mutex_unlock(&facilities.id.lock); break; @@ -504,7 +507,7 @@ int facilities_request_attribute_types(void* responder, FacilitiesRequest_t* fre rv = 1; goto cleanup; } - } + } uint8_t frep_oer[256]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 256); if (enc.encoded == -1) { @@ -522,11 +525,10 @@ cleanup: return rv; } - -int facilities_request_loaded_protected_zones(void* responder, FacilitiesRequest_t* freq) { +int facilities_request_loaded_protected_zones(void *responder, FacilitiesRequest_t *freq) { int rv = 0; - FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t)); + FacilitiesReply_t *frep = calloc(1, sizeof(FacilitiesReply_t)); frep->present = FacilitiesReply_PR_data; frep->choice.data.present = FacilitiesDataReply_PR_protectedCommunicationZones; @@ -534,14 +536,14 @@ int facilities_request_loaded_protected_zones(void* responder, FacilitiesRequest pthread_mutex_lock(&facilities.lightship.lock); frep->choice.data.choice.protectedCommunicationZones.list.count = facilities.lightship.protected_zones.pz_len; - frep->choice.data.choice.protectedCommunicationZones.list.size = facilities.lightship.protected_zones.pz_len * sizeof(void*); - frep->choice.data.choice.protectedCommunicationZones.list.array = malloc(facilities.lightship.protected_zones.pz_len * sizeof(void*)); + frep->choice.data.choice.protectedCommunicationZones.list.size = facilities.lightship.protected_zones.pz_len * sizeof(void *); + frep->choice.data.choice.protectedCommunicationZones.list.array = malloc(facilities.lightship.protected_zones.pz_len * sizeof(void *)); uint8_t buf[256]; for (int z = 0; z < facilities.lightship.protected_zones.pz_len; ++z) { frep->choice.data.choice.protectedCommunicationZones.list.array[z] = calloc(1, sizeof(ProtectedCommunicationZone_t)); asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, facilities.lightship.protected_zones.pz[z], buf, 256); - uper_decode_complete(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &frep->choice.data.choice.protectedCommunicationZones.list.array[z], buf, (enc.encoded+7) / 8); + uper_decode_complete(NULL, &asn_DEF_ProtectedCommunicationZone, (void **)&frep->choice.data.choice.protectedCommunicationZones.list.array[z], buf, (enc.encoded + 7) / 8); } pthread_mutex_unlock(&facilities.lightship.lock); @@ -563,23 +565,22 @@ cleanup: return rv; } -int facilities_request_chaininfo_set(void* responder, ChainInformationSet_t* cis) { +int facilities_request_chaininfo_set(void *responder, ChainInformationSet_t *cis) { int rv = 0; pthread_mutex_lock(&facilities.coordination.lock); facilities.coordination.chain.id = cis->idSet; for (int i = 0; i < cis->linkSet.list.count; ++i) { memcpy( - facilities.coordination.chain.links[i], - cis->linkSet.list.array[i]->buf, - cis->linkSet.list.array[i]->size - ); + facilities.coordination.chain.links[i], + cis->linkSet.list.array[i]->buf, + cis->linkSet.list.array[i]->size); } facilities.coordination.chain.n_links = cis->linkSet.list.count; // TODO region pthread_mutex_unlock(&facilities.coordination.lock); - FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t)); + FacilitiesReply_t *frep = calloc(1, sizeof(FacilitiesReply_t)); frep->present = FacilitiesReply_PR_data; frep->choice.data.present = FacilitiesDataReply_PR_chainInfoSet; frep->choice.data.choice.chainInfoSet = FacilitiesResultCode_accepted; @@ -600,4 +601,3 @@ cleanup: return rv; } -