From 05e920c68886762e1a21ead154d5866a3a0b630f Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Mon, 20 Feb 2023 17:18:25 +0000 Subject: [PATCH 01/31] created the skeleton for the EVCSNM --- .vscode/settings.json | 8 + src/evcsnm.c | 683 ++++++++++++++++++++++++++++++++++++++++++ src/evcsnm.h | 130 ++++++++ 3 files changed, 821 insertions(+) create mode 100644 .vscode/settings.json create mode 100644 src/evcsnm.c create mode 100644 src/evcsnm.h diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..2030e35 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,8 @@ +{ + "files.associations": { + "packet.h": "c", + "managementrequest.h": "c", + "facilitiesindication.h": "c", + "constr_sequence.h": "c" + } +} \ No newline at end of file diff --git a/src/evcsnm.c b/src/evcsnm.c new file mode 100644 index 0000000..f63981a --- /dev/null +++ b/src/evcsnm.c @@ -0,0 +1,683 @@ +#include "evcsnm.h" +#include "facilities.h" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#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) +{ + /* 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; +} + +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; + 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 % 65536); + 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; + + 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); + + 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; + + 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); + } + + return 0; +} + +int lightship_check() +{ + 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() +{ + 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_evcsnm; + + 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_evcsnm; + 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 * 50); + + if (lightship_check() && facilities.lightship.active) + { + 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; + + // 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; + + 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"); + + 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); + } + + 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/evcsnm.h b/src/evcsnm.h new file mode 100644 index 0000000..2777d31 --- /dev/null +++ b/src/evcsnm.h @@ -0,0 +1,130 @@ +#ifndef FACILITIES_CAM_H +#define FACILITIES_CAM_H + +#include +#include +#include +#include + +#include +#include + +#include + +#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 +}; + +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) + * + * @return Always zero + */ +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); + +/* + * @brief Main CA service + * + * @return NULL + */ +void* ca_service(); + +#endif From 5f37056d2f2fa033bdc7e81121bf6e5f2ac4b226 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 21 Feb 2023 17:27:30 +0000 Subject: [PATCH 02/31] more evcsnm related changes --- .vscode/settings.json | 12 +- src/CMakeLists.txt | 2 + src/config.c | 3 + src/evcsnm.c | 563 ++----------------- src/evcsnm.h | 126 +---- src/facilities.c | 1212 ++++++++++++++++++++++------------------- src/facilities.h | 5 + 7 files changed, 746 insertions(+), 1177 deletions(-) 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; From 0f306312ff0091e7f87df85ec300fb3a723cc787 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 21 Feb 2023 18:11:28 +0000 Subject: [PATCH 03/31] in the process... --- src/evcsnm.c | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 179c660..cf0f05b 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -25,8 +25,7 @@ static UTF8String_t *create_empty_utf8_string() { UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t)); utf8_string->buf = calloc(1, 1); - utf8_string->size = 0; - utf8_string->buf[0] = '\0'; + utf8_string->size = 1; return utf8_string; } @@ -51,7 +50,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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.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; @@ -63,24 +62,23 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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->chargingStationLocation.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_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.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_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.bits_unused = 0; 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; - + cs_elem0->typeOfReceptacle.bits_unused = 0; itss_space_unlock(epv); // if (facilities.station_type == StationType_roadSideUnit) //{ @@ -100,7 +98,8 @@ cleanup: return rv; } -int evcsnm_check(EvcsnPdu_t* evcsnm) { +int evcsnm_check(EvcsnPdu_t *evcsnm) +{ return 0; } From 39623c38e223453891c63c719f43ee3dd2871e8b Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 21 Feb 2023 18:15:48 +0000 Subject: [PATCH 04/31] evcsnm working --- src/evcsnm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index cf0f05b..9becb79 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -143,7 +143,7 @@ void *evcsn_service() FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t)); fi->present = FacilitiesIndication_PR_message; FacilitiesMessageIndication_t *fmi = &fi->choice.message; - fmi->itsMessageType = messageID_evcsn; + fmi->itsMessageType = ItsMessageType_poi; fmi->data.buf = malloc(512); uint8_t tr_oer[1024]; From bb921cd3bd620460b544825f7e7cdade89c06bb4 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 21 Feb 2023 18:16:37 +0000 Subject: [PATCH 05/31] fixed the port --- src/evcsnm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 9becb79..362fe39 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -130,7 +130,7 @@ void *evcsn_service() bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->destinationPort = Port_cam; + bpr->destinationPort = Port_poi; bpr->gn.trafficClass = 2; From 948780526b430ee73378b46b78faad435632d10e Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Mon, 27 Feb 2023 17:08:51 +0000 Subject: [PATCH 06/31] switch from 0x00 to 0xff to make the compilers work --- src/evcsnm.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 362fe39..8dc2457 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -75,10 +75,11 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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] = 0; + cs_elem0->type.buf[0] = 0xff; 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] = 0xff; itss_space_unlock(epv); // if (facilities.station_type == StationType_roadSideUnit) //{ From dfbc7bf8662ef361845349b93592b3098b7d26f7 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Wed, 1 Mar 2023 16:01:52 +0000 Subject: [PATCH 07/31] added EVCSInfo to SAEMs --- .vscode/settings.json | 5 +- src/config.c | 556 ++++++++++++++++++++++++++++-------------- src/evcsnm.c | 15 ++ src/evcsnm.h | 26 +- src/saem.c | 504 +++++++++++++++++++++++--------------- 5 files changed, 716 insertions(+), 390 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index 93f6441..e252684 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -13,6 +13,9 @@ "asn_application.h": "c", "itspduheader.h": "c", "evchargingspotnotificationpoimessage.h": "c", - "vcm.h": "c" + "vcm.h": "c", + "constr_sequence_of.h": "c", + "asn_internal.h": "c", + "bit_string.h": "c" } } \ No newline at end of file diff --git a/src/config.c b/src/config.c index 98b5617..c2eeef1 100644 --- a/src/config.c +++ b/src/config.c @@ -13,6 +13,7 @@ #include #include #include +#include #include #include @@ -22,23 +23,29 @@ #include #include -static int fetch_target_address(char** addresses, uint16_t addresses_len) { +static int fetch_target_address(char **addresses, uint16_t addresses_len) +{ int index = -1; - for (int i = 0; i < addresses_len; ++i) { - char* addr = addresses[i]; + for (int i = 0; i < addresses_len; ++i) + { + char *addr = addresses[i]; // If TCP, ignore receiver addresses, e.g. tcp://*:[port] - if (!memcmp(addr, "tcp", 3)) { + if (!memcmp(addr, "tcp", 3)) + { bool found_astk = false; - for (int j = 0; j < strlen(addr); ++j) { - if (addr[j] == '*') { + for (int j = 0; j < strlen(addr); ++j) + { + if (addr[j] == '*') + { found_astk = true; break; } } - if (found_astk) continue; + if (found_astk) + continue; } index = i; @@ -48,24 +55,32 @@ static int fetch_target_address(char** addresses, uint16_t addresses_len) { return index; } -int facilities_config() { +int facilities_config() +{ int rv = 0; - it2s_config_t* config = calloc(1, sizeof(it2s_config_t)); + it2s_config_t *config = calloc(1, sizeof(it2s_config_t)); rv = it2s_config_read("/etc/it2s/itss.toml", config); - if (rv) { + if (rv) + { log_error("[config] read config failed"); goto cleanup; } // Logging - status messages bool use_syslog = false, use_std = false, use_file = false; - for (int i = 0; i < config->general.logging.status_len; ++i) { - if (!strcmp(config->general.logging.status[i], "syslog")) { + for (int i = 0; i < config->general.logging.status_len; ++i) + { + if (!strcmp(config->general.logging.status[i], "syslog")) + { use_syslog = true; - } else if (!strcmp(config->general.logging.status[i], "std")) { + } + else if (!strcmp(config->general.logging.status[i], "std")) + { use_std = true; - } else if (!strcmp(config->general.logging.status[i], "file")) { + } + else if (!strcmp(config->general.logging.status[i], "file")) + { use_file = true; } } @@ -74,58 +89,67 @@ int facilities_config() { itss_0init(&facilities.exit); struct timeval time; - gettimeofday(&time,NULL); + gettimeofday(&time, NULL); srand((time.tv_sec * 1000) + (time.tv_usec / 1000)); facilities.zmq.responders = calloc(config->facilities.zmq.addresses_len + 1, sizeof(zmq_pollitem_t)); facilities.zmq.n_responders = 1; // Internal socket - void* socket = itss_0bind(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); + void *socket = itss_0bind(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); facilities.zmq.responders[0].socket = socket; facilities.zmq.responders[0].events = ZMQ_POLLIN; - for (int i = 0; i < config->facilities.zmq.addresses_len; ++i) { - char* addr = config->facilities.zmq.addresses[i]; + for (int i = 0; i < config->facilities.zmq.addresses_len; ++i) + { + char *addr = config->facilities.zmq.addresses[i]; // IPC - if (!memcmp(addr, "ipc", 3)) { - - // Create dir + if (!memcmp(addr, "ipc", 3)) + { + + // Create dir int lp = 0; - for (int j = 0; j < strlen(addr); ++j) { - if (addr[j] == '/') lp = j; + for (int j = 0; j < strlen(addr); ++j) + { + if (addr[j] == '/') + lp = j; } char dir[256]; - memcpy(dir, addr+6, lp-6); - dir[lp-6] = 0; + memcpy(dir, addr + 6, lp - 6); + dir[lp - 6] = 0; struct stat st = {0}; - if (stat(dir, &st) == -1) { + if (stat(dir, &st) == -1) + { mkdir(dir, 0777); } // Bind - void* socket = itss_0bind(addr, ZMQ_REP); + void *socket = itss_0bind(addr, ZMQ_REP); facilities.zmq.responders[facilities.zmq.n_responders].socket = socket; facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN; ++facilities.zmq.n_responders; + } + else if (!memcmp(addr, "tcp", 3)) + { - } else if (!memcmp(addr, "tcp", 3)) { - - bool found_astk = false; - for (int j = 0; j < strlen(addr); ++j) { - if (addr[j] == '*') { - found_astk = true; + bool found_astk = false; + for (int j = 0; j < strlen(addr); ++j) + { + if (addr[j] == '*') + { + found_astk = true; break; } } - if (found_astk) { + if (found_astk) + { // Bind - void* socket = itss_0bind(addr, ZMQ_REP); + void *socket = itss_0bind(addr, ZMQ_REP); facilities.zmq.responders[facilities.zmq.n_responders].socket = socket; facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN; @@ -133,7 +157,8 @@ int facilities_config() { } } } - if (facilities.zmq.n_responders <= 1) { + if (facilities.zmq.n_responders <= 1) + { log_info("[config] a valid address to listen to was not found, exiting now"); rv = 1; goto cleanup; @@ -141,10 +166,13 @@ int facilities_config() { // Fetch [transport] address int index = fetch_target_address(config->transport.zmq.addresses, config->transport.zmq.addresses_len); - if (index != -1) { - facilities.zmq.transport_address = malloc(strlen(config->transport.zmq.addresses[index])+1); + if (index != -1) + { + facilities.zmq.transport_address = malloc(strlen(config->transport.zmq.addresses[index]) + 1); strcpy(facilities.zmq.transport_address, config->transport.zmq.addresses[index]); - } else { + } + else + { log_error("[config] a valid address for [transport] was not found"); rv = 1; goto cleanup; @@ -152,10 +180,13 @@ int facilities_config() { // Fetch [applications] address index = fetch_target_address(config->applications.zmq.addresses, config->applications.zmq.addresses_len); - if (index != -1) { - facilities.zmq.applications_address = malloc(strlen(config->applications.zmq.addresses[index])+1); + if (index != -1) + { + facilities.zmq.applications_address = malloc(strlen(config->applications.zmq.addresses[index]) + 1); strcpy(facilities.zmq.applications_address, config->applications.zmq.addresses[index]); - } else { + } + else + { log_error("[config] a valid address for [applications] was not found"); rv = 1; goto cleanup; @@ -163,10 +194,13 @@ int facilities_config() { // Fetch [security] address index = fetch_target_address(config->security.zmq.addresses, config->security.zmq.addresses_len); - if (index != -1) { - facilities.zmq.security_address = malloc(strlen(config->security.zmq.addresses[index])+1); + if (index != -1) + { + facilities.zmq.security_address = malloc(strlen(config->security.zmq.addresses[index]) + 1); strcpy(facilities.zmq.security_address, config->security.zmq.addresses[index]); - } else { + } + else + { log_error("[config] a valid address for [security] was not found"); rv = 1; goto cleanup; @@ -174,10 +208,13 @@ int facilities_config() { // Fetch [management] address index = fetch_target_address(config->management.zmq.addresses, config->management.zmq.addresses_len); - if (index != -1) { - facilities.zmq.management_address = malloc(strlen(config->management.zmq.addresses[index])+1); + if (index != -1) + { + facilities.zmq.management_address = malloc(strlen(config->management.zmq.addresses[index]) + 1); strcpy(facilities.zmq.management_address, config->management.zmq.addresses[index]); - } else { + } + else + { log_error("[config] a valid address for [management] was not found"); rv = 1; goto cleanup; @@ -185,58 +222,89 @@ int facilities_config() { // Values // General - if (!strcmp("obu", config->general.itss_type)) { + if (!strcmp("obu", config->general.itss_type)) + { facilities.station_type = 5; - } else if (!strcmp("rsu", config->general.itss_type)) { + } + else if (!strcmp("rsu", config->general.itss_type)) + { facilities.station_type = 15; - } else if (!strcmp("unknown", config->general.itss_type)) { + } + else if (!strcmp("unknown", config->general.itss_type)) + { facilities.station_type = 0; - } else if (!strcmp("pedestrian", config->general.itss_type)) { + } + else if (!strcmp("pedestrian", config->general.itss_type)) + { facilities.station_type = 1; - } else if (!strcmp("cyclist", config->general.itss_type)) { + } + else if (!strcmp("cyclist", config->general.itss_type)) + { facilities.station_type = 2; - } else if (!strcmp("moped", config->general.itss_type)) { + } + else if (!strcmp("moped", config->general.itss_type)) + { facilities.station_type = 3; - } else if (!strcmp("motorcycle", config->general.itss_type)) { + } + else if (!strcmp("motorcycle", config->general.itss_type)) + { facilities.station_type = 4; - } else if (!strcmp("passengerCar", config->general.itss_type)) { + } + else if (!strcmp("passengerCar", config->general.itss_type)) + { facilities.station_type = 5; - } else if (!strcmp("bus", config->general.itss_type)) { + } + else if (!strcmp("bus", config->general.itss_type)) + { facilities.station_type = 6; - } else if (!strcmp("lightTruck", config->general.itss_type)) { + } + else if (!strcmp("lightTruck", config->general.itss_type)) + { facilities.station_type = 7; - } else if (!strcmp("heavyTruck", config->general.itss_type)) { + } + else if (!strcmp("heavyTruck", config->general.itss_type)) + { facilities.station_type = 8; - } else if (!strcmp("trailer", config->general.itss_type)) { + } + else if (!strcmp("trailer", config->general.itss_type)) + { facilities.station_type = 9; - } else if (!strcmp("specialVehicles", config->general.itss_type)) { + } + else if (!strcmp("specialVehicles", config->general.itss_type)) + { facilities.station_type = 10; - } else if (!strcmp("tram", config->general.itss_type)) { + } + else if (!strcmp("tram", config->general.itss_type)) + { facilities.station_type = 11; - } else if (!strcmp("roadSideUnit", config->general.itss_type)) { + } + else if (!strcmp("roadSideUnit", config->general.itss_type)) + { facilities.station_type = 15; - } else { + } + else + { log_error("[config] unrecognized ITS-S type, running as OBU"); facilities.station_type = 5; } - facilities.use_security = config->security.use_security; pthread_mutex_init(&facilities.id.lock, NULL); pthread_mutex_init(&facilities.id.change.lock, NULL); facilities.id.change.random = config->security.identity.random; - if (facilities.id.change.random) { + if (facilities.id.change.random) + { // Ask [security] for station id - - SecurityRequest_t* sREQ = calloc(1, sizeof(SecurityRequest_t)); - SecurityReply_t* sREP = NULL; + + SecurityRequest_t *sREQ = calloc(1, sizeof(SecurityRequest_t)); + SecurityReply_t *sREP = NULL; sREQ->present = SecurityRequest_PR_ids; sREQ->choice.ids.list.count = 2; - sREQ->choice.ids.list.size = 2*sizeof(void*); - sREQ->choice.ids.list.array = malloc(2*sizeof(void*)); + sREQ->choice.ids.list.size = 2 * sizeof(void *); + sREQ->choice.ids.list.array = malloc(2 * sizeof(void *)); sREQ->choice.ids.list.array[0] = calloc(1, sizeof(SecurityIdType_t)); *sREQ->choice.ids.list.array[0] = SecurityIdType_stationId; sREQ->choice.ids.list.array[1] = calloc(1, sizeof(SecurityIdType_t)); @@ -245,51 +313,56 @@ int facilities_config() { uint8_t b_tx[256], b_rx[256]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sREQ, b_tx, 256); - void* ss = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); + void *ss = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); itss_0send(ss, b_tx, enc.encoded); int rl = itss_0recv_rt(&ss, b_rx, 256, b_tx, 256, -1); itss_0close(ss); - if (rl == -1) { + if (rl == -1) + { rv = 1; goto cleanup; } - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &sREP, b_rx, 256); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void **)&sREP, b_rx, 256); - if (sREP->returnCode == SecurityReplyReturnCode_rejected) { + if (sREP->returnCode == SecurityReplyReturnCode_rejected) + { // TODO handle it goto cleanup; } - for (int i = 0; i < sREP->data->choice.ids.list.count; ++i) { - switch (sREP->data->choice.ids.list.array[i]->present) { - case SecurityId_PR_stationId: - facilities.id.station_id = sREP->data->choice.ids.list.array[i]->choice.stationId; - break; - case SecurityId_PR_ipv6Address: - memcpy(facilities.id.ipv6_addr, sREP->data->choice.ids.list.array[i]->choice.ipv6Address.buf, 16); - break; - default: - break; + for (int i = 0; i < sREP->data->choice.ids.list.count; ++i) + { + switch (sREP->data->choice.ids.list.array[i]->present) + { + case SecurityId_PR_stationId: + facilities.id.station_id = sREP->data->choice.ids.list.array[i]->choice.stationId; + break; + case SecurityId_PR_ipv6Address: + memcpy(facilities.id.ipv6_addr, sREP->data->choice.ids.list.array[i]->choice.ipv6Address.buf, 16); + break; + default: + break; } } - + ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sREQ); ASN_STRUCT_FREE(asn_DEF_SecurityReply, sREP); - - } else { + } + else + { facilities.id.station_id = config->security.identity.station_id; uint8_t src_mac[6]; unsigned int tmp_uint[6]; sscanf(config->security.identity.mac_address, "%02x:%02x:%02x:%02x:%02x:%02x", - &tmp_uint[0], - &tmp_uint[1], - &tmp_uint[2], - &tmp_uint[3], - &tmp_uint[4], - &tmp_uint[5]); + &tmp_uint[0], + &tmp_uint[1], + &tmp_uint[2], + &tmp_uint[3], + &tmp_uint[4], + &tmp_uint[5]); src_mac[0] = (uint8_t)tmp_uint[0]; src_mac[1] = (uint8_t)tmp_uint[1]; @@ -299,19 +372,18 @@ int facilities_config() { src_mac[5] = (uint8_t)tmp_uint[5]; memset(facilities.id.ipv6_addr, 0, 8); - memcpy(facilities.id.ipv6_addr+8, src_mac, 3); + memcpy(facilities.id.ipv6_addr + 8, src_mac, 3); facilities.id.ipv6_addr[11] = 0xff; facilities.id.ipv6_addr[12] = 0xfe; - memcpy(facilities.id.ipv6_addr+13, src_mac+3, 3); + memcpy(facilities.id.ipv6_addr + 13, src_mac + 3, 3); facilities.id.ipv6_addr[8] ^= 0x02; } - // DENM facilities.den.n_max_events = config->facilities.denm.nmax_active_events; // CAM - facilities.lightship.active = config->facilities.cam.activate; + facilities.lightship.active = config->facilities.cam.activate; facilities.lightship.vehicle_gen_min = config->facilities.cam.obu_period_min; facilities.lightship.vehicle_gen_max = config->facilities.cam.obu_period_max; facilities.lightship.rsu_gen_min = config->facilities.cam.rsu_period_min; @@ -329,27 +401,36 @@ int facilities_config() { facilities.dissemination.radar_rotation = config->applications.its_center.radar_rotation; facilities.dissemination.tmc_connect = config->facilities.cpm.tmc_connected; facilities.dissemination.T_AddSensorInformation = 1000; - int oa_start = (360-facilities.dissemination.radar_rotation) * 10 - 500; - int oa_end = (360-facilities.dissemination.radar_rotation) * 10 + 500; + int oa_start = (360 - facilities.dissemination.radar_rotation) * 10 - 500; + int oa_end = (360 - facilities.dissemination.radar_rotation) * 10 + 500; facilities.dissemination.opening_angle_start = (oa_start + 3600) % 3600; facilities.dissemination.opening_angle_end = (oa_end + 3600) % 3600; - facilities.dissemination.int_radar = malloc(strlen(config->facilities.cpm.radar_interface)+1); - strcpy(facilities.dissemination.int_radar,config->facilities.cpm.radar_interface); - facilities.dissemination.ip_radar = malloc(strlen(config->facilities.cpm.radar_ip)+1); - strcpy(facilities.dissemination.ip_radar,config->facilities.cpm.radar_ip); + facilities.dissemination.int_radar = malloc(strlen(config->facilities.cpm.radar_interface) + 1); + strcpy(facilities.dissemination.int_radar, config->facilities.cpm.radar_interface); + facilities.dissemination.ip_radar = malloc(strlen(config->facilities.cpm.radar_ip) + 1); + strcpy(facilities.dissemination.ip_radar, config->facilities.cpm.radar_ip); // TPM facilities.tolling.enabled = config->facilities.tpm.activate; - if (!strcmp("simple", config->facilities.tpm.protocol)) { + if (!strcmp("simple", config->facilities.tpm.protocol)) + { facilities.tolling.protocol.p = TOLLING_PROTOCOL_SIMPLE; - } else if (!strcmp("tls", config->facilities.tpm.protocol)) { + } + else if (!strcmp("tls", config->facilities.tpm.protocol)) + { facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS; - } else if (!strcmp("tls-gn", config->facilities.tpm.protocol)) { + } + else if (!strcmp("tls-gn", config->facilities.tpm.protocol)) + { facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_GN; - } else if (!strcmp("tls-shs", config->facilities.tpm.protocol)) { + } + else if (!strcmp("tls-shs", config->facilities.tpm.protocol)) + { facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_SHS; - } else { + } + else + { log_error("[config] unrecognized tolling protocol, defaulting to 'simple'"); facilities.tolling.protocol.p = TOLLING_PROTOCOL_SIMPLE; } @@ -357,37 +438,58 @@ int facilities_config() { // MCM facilities.coordination.active = config->facilities.mcm.activate; - if (!strcmp("vcm-RR", config->facilities.mcm.protocol)) { + if (!strcmp("vcm-RR", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; facilities.coordination.chain.enabled = true; - } else if (!strcmp("vcm-RR1C", config->facilities.mcm.protocol)) { + } + else if (!strcmp("vcm-RR1C", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = true; - } else if (!strcmp("vcm-RR1Cc", config->facilities.mcm.protocol)) { + } + else if (!strcmp("vcm-RR1Cc", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = true; - } else if (!strcmp("vcm-RRAC", config->facilities.mcm.protocol)) { + } + else if (!strcmp("vcm-RRAC", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = true; - } else if (!strcmp("vcm-RRACc", config->facilities.mcm.protocol)) { + } + else if (!strcmp("vcm-RRACc", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = true; - } else if (!strcmp("evcm-RR-PBFT", config->facilities.mcm.protocol)) { + } + else if (!strcmp("evcm-RR-PBFT", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; facilities.coordination.chain.enabled = false; - } else if (!strcmp("evcm-RR1C-PBFT", config->facilities.mcm.protocol)) { + } + else if (!strcmp("evcm-RR1C-PBFT", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = false; - } else if (!strcmp("evcm-RR1Cc-PBFT", config->facilities.mcm.protocol)) { + } + else if (!strcmp("evcm-RR1Cc-PBFT", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = false; - } else if (!strcmp("evcm-RRAC-PBFT", config->facilities.mcm.protocol)) { + } + else if (!strcmp("evcm-RRAC-PBFT", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = false; - } else if (!strcmp("evcm-RRACc-PBFT", config->facilities.mcm.protocol)) { + } + else if (!strcmp("evcm-RRACc-PBFT", config->facilities.mcm.protocol)) + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = false; - } else { + } + else + { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; facilities.coordination.chain.enabled = true; } @@ -396,12 +498,65 @@ int facilities_config() { // EVCSNM facilities.evcsnm_args.activate = config->facilities.evcsnm.activate; + if (facilities.station_type == 15) + { + int i = 0; + DIR *d = opendir(config->facilities.evcsnm.evcsi_path); + struct dirent *dir; + char file[256]; + char ti_xml[2048]; + if (d) + { + while ((dir = readdir(d)) != NULL && i < EVCSNM_INFOS_MAX_LENGTH) + { + if (dir->d_name[0] == '.') + continue; + sprintf(file, "%s/%s", config->facilities.evcsnm.evcsi_path, dir->d_name); + FILE *fp = fopen(file, "r"); + if (!fp) + continue; + fseek(fp, 0, SEEK_END); + uint16_t size = ftell(fp); + fseek(fp, 0, SEEK_SET); + if (!size) + { + fclose(fp); + continue; + } + if (!fread(ti_xml, 1, size, fp)) + { + fclose(fp); + continue; + } + fclose(fp); + + EVCSInfo_t *ti = calloc(1, sizeof(EVCSInfo_t)); + + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_EVCSInfo, (void **)&ti, ti_xml, size); + if (!dec.code) + { + facilities.evcsnm_args.infos.z[i] = calloc(1, sizeof(evcs_info_t)); + facilities.evcsnm_args.infos.z[i]->asn = ti; + ++facilities.evcsnm_args.infos.length; + ++i; + log_debug("[config] loaded evcs info | id:%lld", ti->id); + } + else + { + log_error("[config] failure to decode evcs info '%s'", dir->d_name); + ASN_STRUCT_FREE(asn_DEF_EVCSInfo, ti); + } + } + closedir(d); + } + } // Replay facilities.replay = config->networking.replay.activate; // PZ - if (facilities.station_type == 15) { + if (facilities.station_type == 15) + { int i = 0; @@ -409,45 +564,54 @@ int facilities_config() { struct dirent *dir; char file[256]; char pz_xml[2048]; - if (d) { - while ((dir = readdir(d)) != NULL && i < 16) { - if (dir->d_name[0] == '.') continue; + if (d) + { + while ((dir = readdir(d)) != NULL && i < 16) + { + if (dir->d_name[0] == '.') + continue; sprintf(file, "%s/%s", config->facilities.protected_zones.path, dir->d_name); FILE *fp = fopen(file, "r"); - if (!fp) continue; + if (!fp) + continue; fseek(fp, 0, SEEK_END); uint16_t size = ftell(fp); fseek(fp, 0, SEEK_SET); - if (!size) { + if (!size) + { fclose(fp); continue; } - if (!fread(pz_xml, 1, size, fp)) { + if (!fread(pz_xml, 1, size, fp)) + { fclose(fp); continue; } fclose(fp); - + ProtectedCommunicationZone_t *zone = calloc(1, sizeof(ProtectedCommunicationZone_t)); - asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &zone, pz_xml, size); - if (!dec.code) { + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void **)&zone, pz_xml, size); + if (!dec.code) + { facilities.lightship.protected_zones.pz[i] = zone; ++facilities.lightship.protected_zones.pz_len; ++i; log_debug("[config] loaded protection zone @ (%lld, %lld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude); - } else { + } + else + { log_error("[config] failure to decode protection zone '%s'", dir->d_name); ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, zone); } - } closedir(d); } } // TZ - if (facilities.station_type == 15) { + if (facilities.station_type == 15) + { int i = 0; @@ -455,42 +619,50 @@ int facilities_config() { struct dirent *dir; char file[256]; char ti_xml[2048]; - if (d) { - while ((dir = readdir(d)) != NULL && i < TOLLING_INFOS_MAX_LENGTH) { - if (dir->d_name[0] == '.') continue; + if (d) + { + while ((dir = readdir(d)) != NULL && i < TOLLING_INFOS_MAX_LENGTH) + { + if (dir->d_name[0] == '.') + continue; sprintf(file, "%s/%s", config->facilities.tpm.tis_path, dir->d_name); FILE *fp = fopen(file, "r"); - if (!fp) continue; + if (!fp) + continue; fseek(fp, 0, SEEK_END); uint16_t size = ftell(fp); fseek(fp, 0, SEEK_SET); - if (!size) { + if (!size) + { fclose(fp); continue; } - if (!fread(ti_xml, 1, size, fp)) { + if (!fread(ti_xml, 1, size, fp)) + { fclose(fp); continue; } fclose(fp); - + TollingPaymentInfo_t *ti = calloc(1, sizeof(TollingPaymentInfo_t)); - asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_TollingPaymentInfo, (void**) &ti, ti_xml, size); - if (!dec.code) { + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_TollingPaymentInfo, (void **)&ti, ti_xml, size); + if (!dec.code) + { facilities.tolling.infos.z[i] = calloc(1, sizeof(tolling_info_t)); facilities.tolling.infos.z[i]->asn = ti; ++facilities.tolling.infos.length; ++i; - log_debug("[config] loaded tolling info | id:%lld type:%s", - ti->id, - ti->tollType==TollType_entry ? "entry": ti->tollType==TollType_exit ? "exit": "single" - ); - } else { + log_debug("[config] loaded tolling info | id:%lld type:%s", + ti->id, + ti->tollType == TollType_entry ? "entry" : ti->tollType == TollType_exit ? "exit" + : "single"); + } + else + { log_error("[config] failure to decode tolling info '%s'", dir->d_name); ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, ti); } - } closedir(d); } @@ -498,7 +670,7 @@ int facilities_config() { itss_epv_init(); - ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t)); + ManagementRequest_t *mreq = calloc(1, sizeof(ManagementRequest_t)); mreq->present = ManagementRequest_PR_attributes; mreq->choice.attributes.present = ManagementRequestAttributes_PR_get; mreq->choice.attributes.choice.get.coordinates = 1; @@ -511,33 +683,35 @@ int facilities_config() { mreq->choice.attributes.choice.get.clock = 1; mreq->choice.attributes.choice.get.clockOffset = 1; mreq->choice.attributes.choice.get.trajectory = config->facilities.mcm.activate; - void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); + void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); uint8_t b_tx[256], b_rx[256]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_tx, 256); itss_0send(management_socket, b_tx, enc.encoded); int rl = itss_0recv_rt(&management_socket, b_rx, 256, b_tx, 256, -1); itss_0close(management_socket); - if (rl == -1) { + if (rl == -1) + { rv = 1; goto cleanup; } - ManagementReply_t* mrep = calloc(1, sizeof(ManagementReply_t)); - oer_decode(NULL, &asn_DEF_ManagementReply, (void**) &mrep, b_rx, 256); + ManagementReply_t *mrep = calloc(1, sizeof(ManagementReply_t)); + oer_decode(NULL, &asn_DEF_ManagementReply, (void **)&mrep, b_rx, 256); long lat, lon, alt, alt_conf; if (mrep->returnCode == ManagementReplyReturnCode_accepted && - mrep->data && - mrep->data->choice.attributes.coordinates && - mrep->data->choice.attributes.altitude && - mrep->data->choice.attributes.heading && - mrep->data->choice.attributes.speed && - mrep->data->choice.attributes.acceleration && - mrep->data->choice.attributes.clockType && - mrep->data->choice.attributes.clock && - mrep->data->choice.attributes.clockOffset && - mrep->data->choice.attributes.gpsType && - (!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) { + mrep->data && + mrep->data->choice.attributes.coordinates && + mrep->data->choice.attributes.altitude && + mrep->data->choice.attributes.heading && + mrep->data->choice.attributes.speed && + mrep->data->choice.attributes.acceleration && + mrep->data->choice.attributes.clockType && + mrep->data->choice.attributes.clock && + mrep->data->choice.attributes.clockOffset && + mrep->data->choice.attributes.gpsType && + (!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) + { epv.space.latitude = mrep->data->choice.attributes.coordinates->latitude; epv.space.latitude_conf = mrep->data->choice.attributes.coordinates->latitudeConfidence; epv.space.longitude = mrep->data->choice.attributes.coordinates->longitude; @@ -553,18 +727,22 @@ int facilities_config() { epv.space.type = *mrep->data->choice.attributes.gpsType; epv.time.type = *mrep->data->choice.attributes.clockType; - asn_INTEGER2ulong(mrep->data->choice.attributes.clock, (unsigned long long*) &epv.time.clock); - asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset,(unsigned long long*) &epv.time.offset); + asn_INTEGER2ulong(mrep->data->choice.attributes.clock, (unsigned long long *)&epv.time.clock); + asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset, (unsigned long long *)&epv.time.offset); - if (config->facilities.mcm.activate) { + if (config->facilities.mcm.activate) + { epv.trajectory.len = mrep->data->choice.attributes.trajectory->list.count; - for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) { + for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) + { epv.trajectory.path[i].latitude = mrep->data->choice.attributes.trajectory->list.array[i]->latitude; epv.trajectory.path[i].longitude = mrep->data->choice.attributes.trajectory->list.array[i]->longitude; - asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, (unsigned long long*) &epv.trajectory.path[i].timestamp); + asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, (unsigned long long *)&epv.trajectory.path[i].timestamp); } } - } else { + } + else + { log_error("rejected MR attribute request"); rv = 1; goto cleanup; @@ -572,11 +750,23 @@ int facilities_config() { ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq); ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep); - if (config->facilities.saem.activate) { // TODO handle various services + if (config->facilities.saem.activate) + { // TODO handle various services facilities.bulletin.to_provide_len = 1; facilities.bulletin.to_provide[0] = calloc(1, sizeof(announcement_t)); - facilities.bulletin.to_provide[0]->endpoint.port = 7010 + config->facilities.saem.service_to_advertise; facilities.bulletin.to_provide[0]->its_aid = config->facilities.saem.service_to_advertise; + switch (config->facilities.saem.service_to_advertise) + { + case SAID_ETC: + facilities.bulletin.to_provide[0]->endpoint.port = 7011; + break; + case SAID_EVCSN: + facilities.bulletin.to_provide[0]->endpoint.port = Port_poi; + break; + default: + log_error("unknown service to advertise! [config.c:facilities.saem.service_to_advertise]"); + break; + } } facilities.vehicle.length = config->facilities.vehicle.length; @@ -585,18 +775,19 @@ int facilities_config() { // Logging facilities.logging.recorder = config->facilities.logging.management; - if (config->general.logging.enabled && config->facilities.logging.dbms) { + if (config->general.logging.enabled && config->facilities.logging.dbms) + { facilities.logging.dbms = calloc(1, sizeof(itss_db_t)); if (itss_db_init( - facilities.logging.dbms, - config->general.logging.database, - config->general.logging.table_style, - ITSS_FACILITIES, - config->general.logging.host, - config->general.logging.port, - config->general.logging.username, - config->general.logging.password - )) { + facilities.logging.dbms, + config->general.logging.database, + config->general.logging.table_style, + ITSS_FACILITIES, + config->general.logging.host, + config->general.logging.port, + config->general.logging.username, + config->general.logging.password)) + { log_error("failed to initialize the database -> turning off logging"); free(facilities.logging.dbms); facilities.logging.dbms = NULL; @@ -608,4 +799,3 @@ cleanup: return rv; } - diff --git a/src/evcsnm.c b/src/evcsnm.c index 8dc2457..955c0ec 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -220,3 +220,18 @@ void *evcsn_service() return NULL; } + + +evcs_info_t* evcs_info_new(EVCSInfo_t* tpi) { + evcs_info_t* ti = calloc(1, sizeof(evcs_info_t)); + + ti->timestamp = itss_time_get(); + ti->asn = tpi; + + return ti; +} + +void evcs_info_free(evcs_info_t* ti) { + ASN_STRUCT_FREE(asn_DEF_EVCSInfo, ti->asn); + free(ti); +} diff --git a/src/evcsnm.h b/src/evcsnm.h index 0ccd2fc..4cc799c 100644 --- a/src/evcsnm.h +++ b/src/evcsnm.h @@ -7,14 +7,16 @@ #include #include +#include #include #include +#define EVCSNM_INFOS_MAX_LENGTH 16 //MAX number of charging spots #define POS_HISTORY_MAX_LEN 24 -#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1 -enum EVCSNM_CHECK_R { +enum EVCSNM_CHECK_R +{ EVCSNM_OK, EVCSNM_INVALID, EVCSNM_BAD_PERMISSIONS @@ -32,17 +34,31 @@ enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, * * @return NULL */ -void* evcsn_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); +int evcsnm_check(EvcsnPdu_t *evcsnm); -typedef struct evcsnm_args { +typedef struct evcs_info { + uint64_t timestamp; + EVCSInfo_t* asn; +} evcs_info_t; + +typedef struct evcsnm_args +{ bool activate; + struct + { + evcs_info_t *z[EVCSNM_INFOS_MAX_LENGTH]; + uint8_t length; + } infos; } evcsnm_args_t; +evcs_info_t* evcs_info_new(EVCSInfo_t* tpi); +void evcs_info_free(evcs_info_t* ti); + #endif diff --git a/src/saem.c b/src/saem.c index 6933fdd..7b33c7d 100644 --- a/src/saem.c +++ b/src/saem.c @@ -12,108 +12,156 @@ #include #include #include +#include #include #include - -SAEM_CODE_R saem_check(SAEM_t* saem, uint8_t* neighbour) { +SAEM_CODE_R saem_check(SAEM_t *saem, uint8_t *neighbour) +{ int rv = 0; - bulletin_t* bulletin = &facilities.bulletin; + bulletin_t *bulletin = &facilities.bulletin; - if (saem->header.messageID != messageID_saem) { + if (saem->header.messageID != messageID_saem) + { return SAEM_INVALID_HEADER_MESSAGE_ID; } - if (saem->header.protocolVersion != 1) { + if (saem->header.protocolVersion != 1) + { return SAEM_INVALID_HEADER_VERSION; } - if (bulletin->to_consume_len >= MAX_ANNOUNCEMENTS_LEN - 1) { + if (bulletin->to_consume_len >= MAX_ANNOUNCEMENTS_LEN - 1) + { return SAEM_MAX_ANNOUNCEMENTS_REACHED; } pthread_mutex_lock(&bulletin->lock); - if (saem->sam.body.serviceInfos) { - for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) { - ServiceInfo_t* si = saem->sam.body.serviceInfos->list.array[i]; + if (saem->sam.body.serviceInfos) + { + for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) + { + ServiceInfo_t *si = saem->sam.body.serviceInfos->list.array[i]; uint16_t its_aid = si->serviceID; bool new_announcement = false; - if (si->chOptions.extensions) { + if (si->chOptions.extensions) + { - for (int e = 0; e < si->chOptions.extensions->list.count; ++e) { - switch (si->chOptions.extensions->list.array[e]->present) { - case ServiceInfoExt_PR_providerServiceContext: - bulletin->to_consume[bulletin->to_consume_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size; - bulletin->to_consume[bulletin->to_consume_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size); - memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.context, - si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf, - si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size - ); - break; + for (int e = 0; e < si->chOptions.extensions->list.count; ++e) + { + switch (si->chOptions.extensions->list.array[e]->present) + { + case ServiceInfoExt_PR_providerServiceContext: + bulletin->to_consume[bulletin->to_consume_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size; + bulletin->to_consume[bulletin->to_consume_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size); + memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.context, + si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf, + si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size); + break; - case ServiceInfoExt_PR_applicationDataSAM: - bulletin->to_consume[bulletin->to_consume_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size; - bulletin->to_consume[bulletin->to_consume_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); - memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.data, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size - ); + case ServiceInfoExt_PR_applicationDataSAM: + bulletin->to_consume[bulletin->to_consume_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size; + bulletin->to_consume[bulletin->to_consume_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); + memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.data, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); - switch (its_aid) { - case SAID_ETC: - if (facilities.station_type != 15 && facilities.tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) { - TollingPaymentInfo_t* tpi = NULL; - asn_dec_rval_t dec = uper_decode_complete( - NULL, - &asn_DEF_TollingPaymentInfo, - (void**) &tpi, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size - ); - if (!dec.code) { + switch (its_aid) + { + case SAID_ETC: + if (facilities.station_type != 15 && facilities.tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) + { + TollingPaymentInfo_t *tpi = NULL; + asn_dec_rval_t dec = uper_decode_complete( + NULL, + &asn_DEF_TollingPaymentInfo, + (void **)&tpi, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); + if (!dec.code) + { - bool found = false; - for (int t = 0; t < bulletin->to_consume_len; ++t) { - if (((tolling_info_t*)bulletin->to_consume[t]->info.internal_p)->asn->id == tpi->id) { - found = true; - break; - } - } - - if (!found) { - facilities.tolling.infos.z[facilities.tolling.infos.length] = tolling_info_new(tpi); - bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.tolling.infos.z[facilities.tolling.infos.length]; - ++facilities.tolling.infos.length; - new_announcement = true; - } - } else { - ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, tpi); + bool found = false; + for (int t = 0; t < bulletin->to_consume_len; ++t) + { + if (((tolling_info_t *)bulletin->to_consume[t]->info.internal_p)->asn->id == tpi->id) + { + found = true; + break; } } - break; - case SAID_EVCSN: - // TODO - break; + if (!found) + { + facilities.tolling.infos.z[facilities.tolling.infos.length] = tolling_info_new(tpi); + bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.tolling.infos.z[facilities.tolling.infos.length]; + ++facilities.tolling.infos.length; + new_announcement = true; + } + } + else + { + ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, tpi); + } } break; - case ServiceInfoExt_PR_addressIPv6: - memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16); - break; + case SAID_EVCSN: + if (facilities.station_type != 15 && facilities.evcsnm_args.infos.length < EVCSNM_INFOS_MAX_LENGTH) + { + EVCSInfo_t *evcsi = NULL; + asn_dec_rval_t dec = uper_decode_complete( + NULL, + &asn_DEF_EVCSInfo, + (void **)&evcsi, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); + if (!dec.code) + { - case ServiceInfoExt_PR_servicePort: - bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort; - break; + bool found = false; + for (int t = 0; t < bulletin->to_consume_len; ++t) + { + if (((evcs_info_t *)bulletin->to_consume[t]->info.internal_p)->asn->id == evcsi->id) + { + found = true; + break; + } + } - default: + if (!found) + { + facilities.evcsnm_args.infos.z[facilities.evcsnm_args.infos.length] = evcs_info_new(evcsi); + bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.evcsnm_args.infos.z[facilities.evcsnm_args.infos.length]; + ++facilities.evcsnm_args.infos.length; + new_announcement = true; + } + } + else + { + ASN_STRUCT_FREE(asn_DEF_EVCSInfo, evcsi); + } + } break; + } + break; + + case ServiceInfoExt_PR_addressIPv6: + memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16); + break; + + case ServiceInfoExt_PR_servicePort: + bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort; + break; + + default: + break; } } } @@ -121,23 +169,26 @@ SAEM_CODE_R saem_check(SAEM_t* saem, uint8_t* neighbour) { uint16_t ci_index = si->channelIndex; // TODO channelInfos - if (saem->sam.body.channelInfos) { - if (saem->sam.body.channelInfos->list.count >= ci_index + 1) { + if (saem->sam.body.channelInfos) + { + if (saem->sam.body.channelInfos->list.count >= ci_index + 1) + { } } - if (new_announcement && bulletin->to_consume_len < MAX_ANNOUNCEMENTS_LEN - 1) { + if (new_announcement && bulletin->to_consume_len < MAX_ANNOUNCEMENTS_LEN - 1) + { bulletin->to_consume[bulletin->to_consume_len]->its_aid = its_aid; bulletin->to_consume[bulletin->to_consume_len]->station_id = saem->header.stationID; bulletin->to_consume[bulletin->to_consume_len]->timestamp = itss_time_get(); - if (neighbour) { + if (neighbour) + { bulletin->to_consume[bulletin->to_consume_len]->certificate_id = malloc(8); memcpy(bulletin->to_consume[bulletin->to_consume_len]->certificate_id, neighbour, 8); } ++bulletin->to_consume_len; rv = SAEM_NEW; } - } } @@ -146,17 +197,18 @@ SAEM_CODE_R saem_check(SAEM_t* saem, uint8_t* neighbour) { return rv == SAEM_NEW ? SAEM_NEW : SAEM_OK; } -void bulletin_init() { +void bulletin_init() +{ - bulletin_t* bulletin = &facilities.bulletin; + bulletin_t *bulletin = &facilities.bulletin; pthread_mutex_init(&bulletin->lock, NULL); bulletin->to_consume_len = 0; - for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) { + for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) + { bulletin->to_consume[i] = calloc(1, sizeof(announcement_t)); } - } /** @@ -165,17 +217,18 @@ void bulletin_init() { * @param b_saem_len The encoded SAEM length. * @return 0 on success, 1 otherwise. */ -static int mk_saem(uint8_t* b_saem, uint32_t* b_saem_len) { +static int mk_saem(uint8_t *b_saem, uint32_t *b_saem_len) +{ int rv = 0; - // Check tolling advertisements - if (!facilities.tolling.infos.length /* || facilites.evcsn.infos.length */) { + if (!facilities.tolling.infos.length && !facilities.evcsnm_args.infos.length) + { return 1; } asn_enc_rval_t enc; - SAEM_t* saem = calloc(1, sizeof(SAEM_t)); + SAEM_t *saem = calloc(1, sizeof(SAEM_t)); /* header */ saem->header.protocolVersion = 1; @@ -188,102 +241,138 @@ static int mk_saem(uint8_t* b_saem, uint32_t* b_saem_len) { saem->sam.version = 0; saem->sam.body.serviceInfos = calloc(1, sizeof(ServiceInfos_t)); - saem->sam.body.serviceInfos->list.count = facilities.tolling.infos.length; // + facilities.evcsn.infos.length; - saem->sam.body.serviceInfos->list.size = facilities.tolling.infos.length * sizeof(void*); // + facilities.evcsn.infos.length * sizeof(void*); - saem->sam.body.serviceInfos->list.array = malloc(facilities.tolling.infos.length * sizeof(void*)); // + facilities.evcsn.infos.length * sizeof(void*); + saem->sam.body.serviceInfos->list.count = facilities.tolling.infos.length + facilities.evcsnm_args.infos.length; + saem->sam.body.serviceInfos->list.size = (facilities.tolling.infos.length + facilities.evcsnm_args.infos.length) * sizeof(void *); + saem->sam.body.serviceInfos->list.array = malloc((facilities.tolling.infos.length + facilities.evcsnm_args.infos.length) * sizeof(void *)); uint8_t buf[1024]; int i; - for (i = 0; i < facilities.tolling.infos.length; ++i) { + for (i = 0; i < facilities.tolling.infos.length; ++i) + { saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t)); saem->sam.body.serviceInfos->list.array[i]->serviceID = SAID_ETC; saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t)); - ServiceInfoExts_t* exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions; + ServiceInfoExts_t *exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions; - switch (facilities.tolling.protocol.p) { - case TOLLING_PROTOCOL_SIMPLE: - exts->list.count = 3; - exts->list.size = 3 * sizeof(void*); - exts->list.array = malloc(3 * sizeof(void*)); + switch (facilities.tolling.protocol.p) + { + case TOLLING_PROTOCOL_SIMPLE: + exts->list.count = 3; + exts->list.size = 3 * sizeof(void *); + exts->list.array = malloc(3 * sizeof(void *)); - exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; - - char ctx_s[] = "tolling:simple"; - exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s); - exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s)); - memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s)); + exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; - exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[1]->present = ServiceInfoExt_PR_servicePort; - exts->list.array[1]->choice.servicePort = 7011; + char ctx_s[] = "tolling:simple"; + exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s); + exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s)); + memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s)); - exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM; - exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024); + exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[1]->present = ServiceInfoExt_PR_servicePort; + exts->list.array[1]->choice.servicePort = 7011; - enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024); - if (enc.encoded == -1) { - log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } + exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM; + exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024); - exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; - break; + enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024); + if (enc.encoded == -1) + { + log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } - case TOLLING_PROTOCOL_TLS: - case TOLLING_PROTOCOL_TLS_GN: - case TOLLING_PROTOCOL_TLS_SHS: - exts->list.count = 4; - exts->list.size = 4 * sizeof(void*); - exts->list.array = malloc(4 * sizeof(void*)); + exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; + break; - exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; - char ctx_t[] = "tolling:tls"; - exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_t); - exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_t)); - memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_t, strlen(ctx_t)); + case TOLLING_PROTOCOL_TLS: + case TOLLING_PROTOCOL_TLS_GN: + case TOLLING_PROTOCOL_TLS_SHS: + exts->list.count = 4; + exts->list.size = 4 * sizeof(void *); + exts->list.array = malloc(4 * sizeof(void *)); - exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[1]->present = ServiceInfoExt_PR_addressIPv6; - exts->list.array[1]->choice.addressIPv6.size = 16; - exts->list.array[1]->choice.addressIPv6.buf = malloc(16); - memcpy(exts->list.array[1]->choice.addressIPv6.buf, facilities.id.ipv6_addr, 16); + exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; + char ctx_t[] = "tolling:tls"; + exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_t); + exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_t)); + memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_t, strlen(ctx_t)); - exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[2]->present = ServiceInfoExt_PR_servicePort; - exts->list.array[2]->choice.servicePort = 7011; + exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[1]->present = ServiceInfoExt_PR_addressIPv6; + exts->list.array[1]->choice.addressIPv6.size = 16; + exts->list.array[1]->choice.addressIPv6.buf = malloc(16); + memcpy(exts->list.array[1]->choice.addressIPv6.buf, facilities.id.ipv6_addr, 16); - exts->list.array[3] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM; - exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024); + exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[2]->present = ServiceInfoExt_PR_servicePort; + exts->list.array[2]->choice.servicePort = 7011; - enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024); - if (enc.encoded == -1) { - log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } + exts->list.array[3] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM; + exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024); - exts->list.array[3]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; - break; + enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024); + if (enc.encoded == -1) + { + log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + exts->list.array[3]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; + break; } } - for (int j = 0; j < 0/* + facilities.evcsn.infos.length */; ++j) { - saem->sam.body.serviceInfos->list.array[i+j] = calloc(1, sizeof(ServiceInfo_t)); - saem->sam.body.serviceInfos->list.array[i+j]->serviceID = SAID_EVCSN; - // TODO + for (int j = 0; j < facilities.evcsnm_args.infos.length; ++j) + { + saem->sam.body.serviceInfos->list.array[i + j] = calloc(1, sizeof(ServiceInfo_t)); + saem->sam.body.serviceInfos->list.array[i + j]->serviceID = SAID_EVCSN; + saem->sam.body.serviceInfos->list.array[i + j]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t)); + + ServiceInfoExts_t *exts = saem->sam.body.serviceInfos->list.array[i + j]->chOptions.extensions; + exts->list.count = 3; + exts->list.size = 3 * sizeof(void *); + exts->list.array = malloc(3 * sizeof(void *)); + + exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; + + char ctx_s[] = "evcsnm"; + exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s); + exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s)); + memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s)); + + exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[1]->present = ServiceInfoExt_PR_servicePort; + exts->list.array[1]->choice.servicePort = Port_poi; + + exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM; + exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024); + + enc = uper_encode_to_buffer(&asn_DEF_EVCSInfo, NULL, facilities.evcsnm_args.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024); + if (enc.encoded == -1) + { + log_error("[sa] failure to encode EVCSInfo (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; } pthread_mutex_unlock(&facilities.id.lock); enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len); - if (enc.encoded == -1) { + if (enc.encoded == -1) + { log_error("[sa] failure to encode SAEM (%s)", enc.failed_type->name); rv = 1; goto cleanup; @@ -292,27 +381,29 @@ static int mk_saem(uint8_t* b_saem, uint32_t* b_saem_len) { cleanup: ASN_STRUCT_FREE(asn_DEF_SAEM, saem); - + return rv; } -void *sa_service() { +void *sa_service() +{ pthread_mutex_init(&facilities.bulletin.lock, NULL); 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; - bulletin_t* bulletin = &facilities.bulletin; + bulletin_t *bulletin = &facilities.bulletin; bpr->btpType = BTPType_btpB; bpr->gn.destinationAddress.buf = malloc(6); - for (int i = 0; i < 6; ++i) { + for (int i = 0; i < 6; ++i) + { bpr->gn.destinationAddress.buf[i] = 0xff; } bpr->gn.destinationAddress.size = 6; @@ -329,7 +420,7 @@ void *sa_service() { bpr->gn.securityProfile.sign = true; int linger_ms = 0; - void* security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); + void *security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); uint8_t tr_oer[1024]; tr_oer[0] = 4; // Facilities @@ -339,41 +430,50 @@ void *sa_service() { int sleep_ms = 100; int mk_saem_n_sleep = 0; - while (!facilities.exit) { + while (!facilities.exit) + { - if (bulletin->to_provide_len && sleep_ms*mk_saem_n_sleep >= 1000) { - rv = mk_saem(bpr->data.buf, (uint32_t *) &bpr->data.size); - if (!rv) { + if (bulletin->to_provide_len && sleep_ms * mk_saem_n_sleep >= 1000) + { + rv = mk_saem(bpr->data.buf, (uint32_t *)&bpr->data.size); + if (!rv) + { bpr->id = itss_id(bpr->data.buf, bpr->data.size); - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023); - if (enc.encoded == -1) { + 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 SAEM failed"); continue; - } else { - itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); - // Logging - if (facilities.logging.dbms) { + } + else + { + itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded + 1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); + // 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_saem, NULL, bpr->data.buf, bpr->data.size); } - if (facilities.logging.recorder) { + 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, - bpr->id, - itss_time_get(), - ITSS_FACILITIES, - true); - if (e != -1) { - itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set"); + buffer, + buffer_len, + bpr->data.buf, + bpr->data.size, + bpr->id, + itss_time_get(), + ITSS_FACILITIES, + true); + if (e != -1) + { + itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set"); } } } @@ -392,40 +492,42 @@ void *sa_service() { uint64_t now = itss_time_get(); pthread_mutex_lock(&bulletin->lock); - for (int a = 0; a < bulletin->to_consume_len; ++a) { + for (int a = 0; a < bulletin->to_consume_len; ++a) + { // Tolling - if (facilities.tolling.enabled && - bulletin->to_consume[a]->its_aid == 1 && - ( - now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS || - (facilities.tolling.protocol.p == TOLLING_PROTOCOL_SIMPLE && tpm_should_retransmit()) - ) && - facilities.station_type != 15) { + if (facilities.tolling.enabled && + bulletin->to_consume[a]->its_aid == 1 && + (now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS || + (facilities.tolling.protocol.p == TOLLING_PROTOCOL_SIMPLE && tpm_should_retransmit())) && + facilities.station_type != 15) + { - tolling_info_t* info = (tolling_info_t*) bulletin->to_consume[a]->info.internal_p; + tolling_info_t *info = (tolling_info_t *)bulletin->to_consume[a]->info.internal_p; - if (!tpm_is_inside_zone(info)) { + if (!tpm_is_inside_zone(info)) + { continue; } - switch (facilities.tolling.protocol.p) { - case TOLLING_PROTOCOL_SIMPLE: - tpm_pay(info, &security_socket, bulletin->to_consume[a]->certificate_id, NULL); - ++bulletin->to_consume[a]->n_trigger; - bulletin->to_consume[a]->t_trigger = now; - break; + switch (facilities.tolling.protocol.p) + { + case TOLLING_PROTOCOL_SIMPLE: + tpm_pay(info, &security_socket, bulletin->to_consume[a]->certificate_id, NULL); + ++bulletin->to_consume[a]->n_trigger; + bulletin->to_consume[a]->t_trigger = now; + break; - case TOLLING_PROTOCOL_TLS: - case TOLLING_PROTOCOL_TLS_GN: - case TOLLING_PROTOCOL_TLS_SHS: - tpm_pay(info, &security_socket, NULL, bulletin->to_consume[a]->endpoint.ipv6_addr); - ++bulletin->to_consume[a]->n_trigger; - bulletin->to_consume[a]->t_trigger = now; - break; + case TOLLING_PROTOCOL_TLS: + case TOLLING_PROTOCOL_TLS_GN: + case TOLLING_PROTOCOL_TLS_SHS: + tpm_pay(info, &security_socket, NULL, bulletin->to_consume[a]->endpoint.ipv6_addr); + ++bulletin->to_consume[a]->n_trigger; + bulletin->to_consume[a]->t_trigger = now; + break; - default: - break; + default: + break; } } } @@ -434,17 +536,17 @@ void *sa_service() { // Tolling management if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS || facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN || - facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS) { + facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS) + { tolling_tlsc_mgmt(facilities.tx_queue, &security_socket); } - usleep(sleep_ms*1000); + usleep(sleep_ms * 1000); } ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); itss_0close(security_socket); - return NULL; } From 544741d0d37c66f5861f8b2c3444d429782006c0 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 7 Mar 2023 11:53:03 +0000 Subject: [PATCH 08/31] reverted the last commit --- .vscode/settings.json | 5 +- src/config.c | 556 ++++++++++++++---------------------------- src/evcsnm.c | 15 -- src/evcsnm.h | 26 +- src/saem.c | 504 +++++++++++++++----------------------- 5 files changed, 390 insertions(+), 716 deletions(-) diff --git a/.vscode/settings.json b/.vscode/settings.json index e252684..93f6441 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -13,9 +13,6 @@ "asn_application.h": "c", "itspduheader.h": "c", "evchargingspotnotificationpoimessage.h": "c", - "vcm.h": "c", - "constr_sequence_of.h": "c", - "asn_internal.h": "c", - "bit_string.h": "c" + "vcm.h": "c" } } \ No newline at end of file diff --git a/src/config.c b/src/config.c index c2eeef1..98b5617 100644 --- a/src/config.c +++ b/src/config.c @@ -13,7 +13,6 @@ #include #include #include -#include #include #include @@ -23,29 +22,23 @@ #include #include -static int fetch_target_address(char **addresses, uint16_t addresses_len) -{ +static int fetch_target_address(char** addresses, uint16_t addresses_len) { int index = -1; - for (int i = 0; i < addresses_len; ++i) - { - char *addr = addresses[i]; + for (int i = 0; i < addresses_len; ++i) { + char* addr = addresses[i]; // If TCP, ignore receiver addresses, e.g. tcp://*:[port] - if (!memcmp(addr, "tcp", 3)) - { + if (!memcmp(addr, "tcp", 3)) { bool found_astk = false; - for (int j = 0; j < strlen(addr); ++j) - { - if (addr[j] == '*') - { + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '*') { found_astk = true; break; } } - if (found_astk) - continue; + if (found_astk) continue; } index = i; @@ -55,32 +48,24 @@ static int fetch_target_address(char **addresses, uint16_t addresses_len) return index; } -int facilities_config() -{ +int facilities_config() { int rv = 0; - it2s_config_t *config = calloc(1, sizeof(it2s_config_t)); + it2s_config_t* config = calloc(1, sizeof(it2s_config_t)); rv = it2s_config_read("/etc/it2s/itss.toml", config); - if (rv) - { + if (rv) { log_error("[config] read config failed"); goto cleanup; } // Logging - status messages bool use_syslog = false, use_std = false, use_file = false; - for (int i = 0; i < config->general.logging.status_len; ++i) - { - if (!strcmp(config->general.logging.status[i], "syslog")) - { + for (int i = 0; i < config->general.logging.status_len; ++i) { + if (!strcmp(config->general.logging.status[i], "syslog")) { use_syslog = true; - } - else if (!strcmp(config->general.logging.status[i], "std")) - { + } else if (!strcmp(config->general.logging.status[i], "std")) { use_std = true; - } - else if (!strcmp(config->general.logging.status[i], "file")) - { + } else if (!strcmp(config->general.logging.status[i], "file")) { use_file = true; } } @@ -89,67 +74,58 @@ int facilities_config() itss_0init(&facilities.exit); struct timeval time; - gettimeofday(&time, NULL); + gettimeofday(&time,NULL); srand((time.tv_sec * 1000) + (time.tv_usec / 1000)); facilities.zmq.responders = calloc(config->facilities.zmq.addresses_len + 1, sizeof(zmq_pollitem_t)); facilities.zmq.n_responders = 1; // Internal socket - void *socket = itss_0bind(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); + void* socket = itss_0bind(ZMQ_INTERNAL_ADDR, ZMQ_PAIR); facilities.zmq.responders[0].socket = socket; facilities.zmq.responders[0].events = ZMQ_POLLIN; - for (int i = 0; i < config->facilities.zmq.addresses_len; ++i) - { - char *addr = config->facilities.zmq.addresses[i]; + for (int i = 0; i < config->facilities.zmq.addresses_len; ++i) { + char* addr = config->facilities.zmq.addresses[i]; // IPC - if (!memcmp(addr, "ipc", 3)) - { - - // Create dir + if (!memcmp(addr, "ipc", 3)) { + + // Create dir int lp = 0; - for (int j = 0; j < strlen(addr); ++j) - { - if (addr[j] == '/') - lp = j; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '/') lp = j; } char dir[256]; - memcpy(dir, addr + 6, lp - 6); - dir[lp - 6] = 0; + memcpy(dir, addr+6, lp-6); + dir[lp-6] = 0; struct stat st = {0}; - if (stat(dir, &st) == -1) - { + if (stat(dir, &st) == -1) { mkdir(dir, 0777); } // Bind - void *socket = itss_0bind(addr, ZMQ_REP); + void* socket = itss_0bind(addr, ZMQ_REP); facilities.zmq.responders[facilities.zmq.n_responders].socket = socket; facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN; ++facilities.zmq.n_responders; - } - else if (!memcmp(addr, "tcp", 3)) - { - bool found_astk = false; - for (int j = 0; j < strlen(addr); ++j) - { - if (addr[j] == '*') - { - found_astk = true; + } else if (!memcmp(addr, "tcp", 3)) { + + bool found_astk = false; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '*') { + found_astk = true; break; } } - if (found_astk) - { + if (found_astk) { // Bind - void *socket = itss_0bind(addr, ZMQ_REP); + void* socket = itss_0bind(addr, ZMQ_REP); facilities.zmq.responders[facilities.zmq.n_responders].socket = socket; facilities.zmq.responders[facilities.zmq.n_responders].events = ZMQ_POLLIN; @@ -157,8 +133,7 @@ int facilities_config() } } } - if (facilities.zmq.n_responders <= 1) - { + if (facilities.zmq.n_responders <= 1) { log_info("[config] a valid address to listen to was not found, exiting now"); rv = 1; goto cleanup; @@ -166,13 +141,10 @@ int facilities_config() // Fetch [transport] address int index = fetch_target_address(config->transport.zmq.addresses, config->transport.zmq.addresses_len); - if (index != -1) - { - facilities.zmq.transport_address = malloc(strlen(config->transport.zmq.addresses[index]) + 1); + if (index != -1) { + facilities.zmq.transport_address = malloc(strlen(config->transport.zmq.addresses[index])+1); strcpy(facilities.zmq.transport_address, config->transport.zmq.addresses[index]); - } - else - { + } else { log_error("[config] a valid address for [transport] was not found"); rv = 1; goto cleanup; @@ -180,13 +152,10 @@ int facilities_config() // Fetch [applications] address index = fetch_target_address(config->applications.zmq.addresses, config->applications.zmq.addresses_len); - if (index != -1) - { - facilities.zmq.applications_address = malloc(strlen(config->applications.zmq.addresses[index]) + 1); + if (index != -1) { + facilities.zmq.applications_address = malloc(strlen(config->applications.zmq.addresses[index])+1); strcpy(facilities.zmq.applications_address, config->applications.zmq.addresses[index]); - } - else - { + } else { log_error("[config] a valid address for [applications] was not found"); rv = 1; goto cleanup; @@ -194,13 +163,10 @@ int facilities_config() // Fetch [security] address index = fetch_target_address(config->security.zmq.addresses, config->security.zmq.addresses_len); - if (index != -1) - { - facilities.zmq.security_address = malloc(strlen(config->security.zmq.addresses[index]) + 1); + if (index != -1) { + facilities.zmq.security_address = malloc(strlen(config->security.zmq.addresses[index])+1); strcpy(facilities.zmq.security_address, config->security.zmq.addresses[index]); - } - else - { + } else { log_error("[config] a valid address for [security] was not found"); rv = 1; goto cleanup; @@ -208,13 +174,10 @@ int facilities_config() // Fetch [management] address index = fetch_target_address(config->management.zmq.addresses, config->management.zmq.addresses_len); - if (index != -1) - { - facilities.zmq.management_address = malloc(strlen(config->management.zmq.addresses[index]) + 1); + if (index != -1) { + facilities.zmq.management_address = malloc(strlen(config->management.zmq.addresses[index])+1); strcpy(facilities.zmq.management_address, config->management.zmq.addresses[index]); - } - else - { + } else { log_error("[config] a valid address for [management] was not found"); rv = 1; goto cleanup; @@ -222,89 +185,58 @@ int facilities_config() // Values // General - if (!strcmp("obu", config->general.itss_type)) - { + if (!strcmp("obu", config->general.itss_type)) { facilities.station_type = 5; - } - else if (!strcmp("rsu", config->general.itss_type)) - { + } else if (!strcmp("rsu", config->general.itss_type)) { facilities.station_type = 15; - } - else if (!strcmp("unknown", config->general.itss_type)) - { + } else if (!strcmp("unknown", config->general.itss_type)) { facilities.station_type = 0; - } - else if (!strcmp("pedestrian", config->general.itss_type)) - { + } else if (!strcmp("pedestrian", config->general.itss_type)) { facilities.station_type = 1; - } - else if (!strcmp("cyclist", config->general.itss_type)) - { + } else if (!strcmp("cyclist", config->general.itss_type)) { facilities.station_type = 2; - } - else if (!strcmp("moped", config->general.itss_type)) - { + } else if (!strcmp("moped", config->general.itss_type)) { facilities.station_type = 3; - } - else if (!strcmp("motorcycle", config->general.itss_type)) - { + } else if (!strcmp("motorcycle", config->general.itss_type)) { facilities.station_type = 4; - } - else if (!strcmp("passengerCar", config->general.itss_type)) - { + } else if (!strcmp("passengerCar", config->general.itss_type)) { facilities.station_type = 5; - } - else if (!strcmp("bus", config->general.itss_type)) - { + } else if (!strcmp("bus", config->general.itss_type)) { facilities.station_type = 6; - } - else if (!strcmp("lightTruck", config->general.itss_type)) - { + } else if (!strcmp("lightTruck", config->general.itss_type)) { facilities.station_type = 7; - } - else if (!strcmp("heavyTruck", config->general.itss_type)) - { + } else if (!strcmp("heavyTruck", config->general.itss_type)) { facilities.station_type = 8; - } - else if (!strcmp("trailer", config->general.itss_type)) - { + } else if (!strcmp("trailer", config->general.itss_type)) { facilities.station_type = 9; - } - else if (!strcmp("specialVehicles", config->general.itss_type)) - { + } else if (!strcmp("specialVehicles", config->general.itss_type)) { facilities.station_type = 10; - } - else if (!strcmp("tram", config->general.itss_type)) - { + } else if (!strcmp("tram", config->general.itss_type)) { facilities.station_type = 11; - } - else if (!strcmp("roadSideUnit", config->general.itss_type)) - { + } else if (!strcmp("roadSideUnit", config->general.itss_type)) { facilities.station_type = 15; - } - else - { + } else { log_error("[config] unrecognized ITS-S type, running as OBU"); facilities.station_type = 5; } + facilities.use_security = config->security.use_security; pthread_mutex_init(&facilities.id.lock, NULL); pthread_mutex_init(&facilities.id.change.lock, NULL); facilities.id.change.random = config->security.identity.random; - if (facilities.id.change.random) - { + if (facilities.id.change.random) { // Ask [security] for station id - - SecurityRequest_t *sREQ = calloc(1, sizeof(SecurityRequest_t)); - SecurityReply_t *sREP = NULL; + + SecurityRequest_t* sREQ = calloc(1, sizeof(SecurityRequest_t)); + SecurityReply_t* sREP = NULL; sREQ->present = SecurityRequest_PR_ids; sREQ->choice.ids.list.count = 2; - sREQ->choice.ids.list.size = 2 * sizeof(void *); - sREQ->choice.ids.list.array = malloc(2 * sizeof(void *)); + sREQ->choice.ids.list.size = 2*sizeof(void*); + sREQ->choice.ids.list.array = malloc(2*sizeof(void*)); sREQ->choice.ids.list.array[0] = calloc(1, sizeof(SecurityIdType_t)); *sREQ->choice.ids.list.array[0] = SecurityIdType_stationId; sREQ->choice.ids.list.array[1] = calloc(1, sizeof(SecurityIdType_t)); @@ -313,56 +245,51 @@ int facilities_config() uint8_t b_tx[256], b_rx[256]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sREQ, b_tx, 256); - void *ss = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); + void* ss = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); itss_0send(ss, b_tx, enc.encoded); int rl = itss_0recv_rt(&ss, b_rx, 256, b_tx, 256, -1); itss_0close(ss); - if (rl == -1) - { + if (rl == -1) { rv = 1; goto cleanup; } - asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void **)&sREP, b_rx, 256); + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &sREP, b_rx, 256); - if (sREP->returnCode == SecurityReplyReturnCode_rejected) - { + if (sREP->returnCode == SecurityReplyReturnCode_rejected) { // TODO handle it goto cleanup; } - for (int i = 0; i < sREP->data->choice.ids.list.count; ++i) - { - switch (sREP->data->choice.ids.list.array[i]->present) - { - case SecurityId_PR_stationId: - facilities.id.station_id = sREP->data->choice.ids.list.array[i]->choice.stationId; - break; - case SecurityId_PR_ipv6Address: - memcpy(facilities.id.ipv6_addr, sREP->data->choice.ids.list.array[i]->choice.ipv6Address.buf, 16); - break; - default: - break; + for (int i = 0; i < sREP->data->choice.ids.list.count; ++i) { + switch (sREP->data->choice.ids.list.array[i]->present) { + case SecurityId_PR_stationId: + facilities.id.station_id = sREP->data->choice.ids.list.array[i]->choice.stationId; + break; + case SecurityId_PR_ipv6Address: + memcpy(facilities.id.ipv6_addr, sREP->data->choice.ids.list.array[i]->choice.ipv6Address.buf, 16); + break; + default: + break; } } - + ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sREQ); ASN_STRUCT_FREE(asn_DEF_SecurityReply, sREP); - } - else - { + + } else { facilities.id.station_id = config->security.identity.station_id; uint8_t src_mac[6]; unsigned int tmp_uint[6]; sscanf(config->security.identity.mac_address, "%02x:%02x:%02x:%02x:%02x:%02x", - &tmp_uint[0], - &tmp_uint[1], - &tmp_uint[2], - &tmp_uint[3], - &tmp_uint[4], - &tmp_uint[5]); + &tmp_uint[0], + &tmp_uint[1], + &tmp_uint[2], + &tmp_uint[3], + &tmp_uint[4], + &tmp_uint[5]); src_mac[0] = (uint8_t)tmp_uint[0]; src_mac[1] = (uint8_t)tmp_uint[1]; @@ -372,18 +299,19 @@ int facilities_config() src_mac[5] = (uint8_t)tmp_uint[5]; memset(facilities.id.ipv6_addr, 0, 8); - memcpy(facilities.id.ipv6_addr + 8, src_mac, 3); + memcpy(facilities.id.ipv6_addr+8, src_mac, 3); facilities.id.ipv6_addr[11] = 0xff; facilities.id.ipv6_addr[12] = 0xfe; - memcpy(facilities.id.ipv6_addr + 13, src_mac + 3, 3); + memcpy(facilities.id.ipv6_addr+13, src_mac+3, 3); facilities.id.ipv6_addr[8] ^= 0x02; } + // DENM facilities.den.n_max_events = config->facilities.denm.nmax_active_events; // CAM - facilities.lightship.active = config->facilities.cam.activate; + facilities.lightship.active = config->facilities.cam.activate; facilities.lightship.vehicle_gen_min = config->facilities.cam.obu_period_min; facilities.lightship.vehicle_gen_max = config->facilities.cam.obu_period_max; facilities.lightship.rsu_gen_min = config->facilities.cam.rsu_period_min; @@ -401,36 +329,27 @@ int facilities_config() facilities.dissemination.radar_rotation = config->applications.its_center.radar_rotation; facilities.dissemination.tmc_connect = config->facilities.cpm.tmc_connected; facilities.dissemination.T_AddSensorInformation = 1000; - int oa_start = (360 - facilities.dissemination.radar_rotation) * 10 - 500; - int oa_end = (360 - facilities.dissemination.radar_rotation) * 10 + 500; + int oa_start = (360-facilities.dissemination.radar_rotation) * 10 - 500; + int oa_end = (360-facilities.dissemination.radar_rotation) * 10 + 500; facilities.dissemination.opening_angle_start = (oa_start + 3600) % 3600; facilities.dissemination.opening_angle_end = (oa_end + 3600) % 3600; - facilities.dissemination.int_radar = malloc(strlen(config->facilities.cpm.radar_interface) + 1); - strcpy(facilities.dissemination.int_radar, config->facilities.cpm.radar_interface); - facilities.dissemination.ip_radar = malloc(strlen(config->facilities.cpm.radar_ip) + 1); - strcpy(facilities.dissemination.ip_radar, config->facilities.cpm.radar_ip); + facilities.dissemination.int_radar = malloc(strlen(config->facilities.cpm.radar_interface)+1); + strcpy(facilities.dissemination.int_radar,config->facilities.cpm.radar_interface); + facilities.dissemination.ip_radar = malloc(strlen(config->facilities.cpm.radar_ip)+1); + strcpy(facilities.dissemination.ip_radar,config->facilities.cpm.radar_ip); // TPM facilities.tolling.enabled = config->facilities.tpm.activate; - if (!strcmp("simple", config->facilities.tpm.protocol)) - { + if (!strcmp("simple", config->facilities.tpm.protocol)) { facilities.tolling.protocol.p = TOLLING_PROTOCOL_SIMPLE; - } - else if (!strcmp("tls", config->facilities.tpm.protocol)) - { + } else if (!strcmp("tls", config->facilities.tpm.protocol)) { facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS; - } - else if (!strcmp("tls-gn", config->facilities.tpm.protocol)) - { + } else if (!strcmp("tls-gn", config->facilities.tpm.protocol)) { facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_GN; - } - else if (!strcmp("tls-shs", config->facilities.tpm.protocol)) - { + } else if (!strcmp("tls-shs", config->facilities.tpm.protocol)) { facilities.tolling.protocol.p = TOLLING_PROTOCOL_TLS_SHS; - } - else - { + } else { log_error("[config] unrecognized tolling protocol, defaulting to 'simple'"); facilities.tolling.protocol.p = TOLLING_PROTOCOL_SIMPLE; } @@ -438,58 +357,37 @@ int facilities_config() // MCM facilities.coordination.active = config->facilities.mcm.activate; - if (!strcmp("vcm-RR", config->facilities.mcm.protocol)) - { + if (!strcmp("vcm-RR", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; facilities.coordination.chain.enabled = true; - } - else if (!strcmp("vcm-RR1C", config->facilities.mcm.protocol)) - { + } else if (!strcmp("vcm-RR1C", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = true; - } - else if (!strcmp("vcm-RR1Cc", config->facilities.mcm.protocol)) - { + } else if (!strcmp("vcm-RR1Cc", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = true; - } - else if (!strcmp("vcm-RRAC", config->facilities.mcm.protocol)) - { + } else if (!strcmp("vcm-RRAC", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = true; - } - else if (!strcmp("vcm-RRACc", config->facilities.mcm.protocol)) - { + } else if (!strcmp("vcm-RRACc", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = true; - } - else if (!strcmp("evcm-RR-PBFT", config->facilities.mcm.protocol)) - { + } else if (!strcmp("evcm-RR-PBFT", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; facilities.coordination.chain.enabled = false; - } - else if (!strcmp("evcm-RR1C-PBFT", config->facilities.mcm.protocol)) - { + } else if (!strcmp("evcm-RR1C-PBFT", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = false; - } - else if (!strcmp("evcm-RR1Cc-PBFT", config->facilities.mcm.protocol)) - { + } else if (!strcmp("evcm-RR1Cc-PBFT", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR1C; facilities.coordination.chain.enabled = false; - } - else if (!strcmp("evcm-RRAC-PBFT", config->facilities.mcm.protocol)) - { + } else if (!strcmp("evcm-RRAC-PBFT", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = false; - } - else if (!strcmp("evcm-RRACc-PBFT", config->facilities.mcm.protocol)) - { + } else if (!strcmp("evcm-RRACc-PBFT", config->facilities.mcm.protocol)) { facilities.coordination.protocol = MC_PROTOCOL_VCM_RRAC; facilities.coordination.chain.enabled = false; - } - else - { + } else { facilities.coordination.protocol = MC_PROTOCOL_VCM_RR; facilities.coordination.chain.enabled = true; } @@ -498,65 +396,12 @@ int facilities_config() // EVCSNM facilities.evcsnm_args.activate = config->facilities.evcsnm.activate; - if (facilities.station_type == 15) - { - int i = 0; - DIR *d = opendir(config->facilities.evcsnm.evcsi_path); - struct dirent *dir; - char file[256]; - char ti_xml[2048]; - if (d) - { - while ((dir = readdir(d)) != NULL && i < EVCSNM_INFOS_MAX_LENGTH) - { - if (dir->d_name[0] == '.') - continue; - sprintf(file, "%s/%s", config->facilities.evcsnm.evcsi_path, dir->d_name); - FILE *fp = fopen(file, "r"); - if (!fp) - continue; - fseek(fp, 0, SEEK_END); - uint16_t size = ftell(fp); - fseek(fp, 0, SEEK_SET); - if (!size) - { - fclose(fp); - continue; - } - if (!fread(ti_xml, 1, size, fp)) - { - fclose(fp); - continue; - } - fclose(fp); - - EVCSInfo_t *ti = calloc(1, sizeof(EVCSInfo_t)); - - asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_EVCSInfo, (void **)&ti, ti_xml, size); - if (!dec.code) - { - facilities.evcsnm_args.infos.z[i] = calloc(1, sizeof(evcs_info_t)); - facilities.evcsnm_args.infos.z[i]->asn = ti; - ++facilities.evcsnm_args.infos.length; - ++i; - log_debug("[config] loaded evcs info | id:%lld", ti->id); - } - else - { - log_error("[config] failure to decode evcs info '%s'", dir->d_name); - ASN_STRUCT_FREE(asn_DEF_EVCSInfo, ti); - } - } - closedir(d); - } - } // Replay facilities.replay = config->networking.replay.activate; // PZ - if (facilities.station_type == 15) - { + if (facilities.station_type == 15) { int i = 0; @@ -564,54 +409,45 @@ int facilities_config() struct dirent *dir; char file[256]; char pz_xml[2048]; - if (d) - { - while ((dir = readdir(d)) != NULL && i < 16) - { - if (dir->d_name[0] == '.') - continue; + if (d) { + while ((dir = readdir(d)) != NULL && i < 16) { + if (dir->d_name[0] == '.') continue; sprintf(file, "%s/%s", config->facilities.protected_zones.path, dir->d_name); FILE *fp = fopen(file, "r"); - if (!fp) - continue; + if (!fp) continue; fseek(fp, 0, SEEK_END); uint16_t size = ftell(fp); fseek(fp, 0, SEEK_SET); - if (!size) - { + if (!size) { fclose(fp); continue; } - if (!fread(pz_xml, 1, size, fp)) - { + if (!fread(pz_xml, 1, size, fp)) { fclose(fp); continue; } fclose(fp); - + ProtectedCommunicationZone_t *zone = calloc(1, sizeof(ProtectedCommunicationZone_t)); - asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void **)&zone, pz_xml, size); - if (!dec.code) - { + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &zone, pz_xml, size); + if (!dec.code) { facilities.lightship.protected_zones.pz[i] = zone; ++facilities.lightship.protected_zones.pz_len; ++i; log_debug("[config] loaded protection zone @ (%lld, %lld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude); - } - else - { + } else { log_error("[config] failure to decode protection zone '%s'", dir->d_name); ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, zone); } + } closedir(d); } } // TZ - if (facilities.station_type == 15) - { + if (facilities.station_type == 15) { int i = 0; @@ -619,50 +455,42 @@ int facilities_config() struct dirent *dir; char file[256]; char ti_xml[2048]; - if (d) - { - while ((dir = readdir(d)) != NULL && i < TOLLING_INFOS_MAX_LENGTH) - { - if (dir->d_name[0] == '.') - continue; + if (d) { + while ((dir = readdir(d)) != NULL && i < TOLLING_INFOS_MAX_LENGTH) { + if (dir->d_name[0] == '.') continue; sprintf(file, "%s/%s", config->facilities.tpm.tis_path, dir->d_name); FILE *fp = fopen(file, "r"); - if (!fp) - continue; + if (!fp) continue; fseek(fp, 0, SEEK_END); uint16_t size = ftell(fp); fseek(fp, 0, SEEK_SET); - if (!size) - { + if (!size) { fclose(fp); continue; } - if (!fread(ti_xml, 1, size, fp)) - { + if (!fread(ti_xml, 1, size, fp)) { fclose(fp); continue; } fclose(fp); - + TollingPaymentInfo_t *ti = calloc(1, sizeof(TollingPaymentInfo_t)); - asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_TollingPaymentInfo, (void **)&ti, ti_xml, size); - if (!dec.code) - { + asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_TollingPaymentInfo, (void**) &ti, ti_xml, size); + if (!dec.code) { facilities.tolling.infos.z[i] = calloc(1, sizeof(tolling_info_t)); facilities.tolling.infos.z[i]->asn = ti; ++facilities.tolling.infos.length; ++i; - log_debug("[config] loaded tolling info | id:%lld type:%s", - ti->id, - ti->tollType == TollType_entry ? "entry" : ti->tollType == TollType_exit ? "exit" - : "single"); - } - else - { + log_debug("[config] loaded tolling info | id:%lld type:%s", + ti->id, + ti->tollType==TollType_entry ? "entry": ti->tollType==TollType_exit ? "exit": "single" + ); + } else { log_error("[config] failure to decode tolling info '%s'", dir->d_name); ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, ti); } + } closedir(d); } @@ -670,7 +498,7 @@ int facilities_config() itss_epv_init(); - ManagementRequest_t *mreq = calloc(1, sizeof(ManagementRequest_t)); + ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t)); mreq->present = ManagementRequest_PR_attributes; mreq->choice.attributes.present = ManagementRequestAttributes_PR_get; mreq->choice.attributes.choice.get.coordinates = 1; @@ -683,35 +511,33 @@ int facilities_config() mreq->choice.attributes.choice.get.clock = 1; mreq->choice.attributes.choice.get.clockOffset = 1; mreq->choice.attributes.choice.get.trajectory = config->facilities.mcm.activate; - void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); + void* management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ); uint8_t b_tx[256], b_rx[256]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_tx, 256); itss_0send(management_socket, b_tx, enc.encoded); int rl = itss_0recv_rt(&management_socket, b_rx, 256, b_tx, 256, -1); itss_0close(management_socket); - if (rl == -1) - { + if (rl == -1) { rv = 1; goto cleanup; } - ManagementReply_t *mrep = calloc(1, sizeof(ManagementReply_t)); - oer_decode(NULL, &asn_DEF_ManagementReply, (void **)&mrep, b_rx, 256); + ManagementReply_t* mrep = calloc(1, sizeof(ManagementReply_t)); + oer_decode(NULL, &asn_DEF_ManagementReply, (void**) &mrep, b_rx, 256); long lat, lon, alt, alt_conf; if (mrep->returnCode == ManagementReplyReturnCode_accepted && - mrep->data && - mrep->data->choice.attributes.coordinates && - mrep->data->choice.attributes.altitude && - mrep->data->choice.attributes.heading && - mrep->data->choice.attributes.speed && - mrep->data->choice.attributes.acceleration && - mrep->data->choice.attributes.clockType && - mrep->data->choice.attributes.clock && - mrep->data->choice.attributes.clockOffset && - mrep->data->choice.attributes.gpsType && - (!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) - { + mrep->data && + mrep->data->choice.attributes.coordinates && + mrep->data->choice.attributes.altitude && + mrep->data->choice.attributes.heading && + mrep->data->choice.attributes.speed && + mrep->data->choice.attributes.acceleration && + mrep->data->choice.attributes.clockType && + mrep->data->choice.attributes.clock && + mrep->data->choice.attributes.clockOffset && + mrep->data->choice.attributes.gpsType && + (!!mrep->data->choice.attributes.trajectory == mreq->choice.attributes.choice.get.trajectory)) { epv.space.latitude = mrep->data->choice.attributes.coordinates->latitude; epv.space.latitude_conf = mrep->data->choice.attributes.coordinates->latitudeConfidence; epv.space.longitude = mrep->data->choice.attributes.coordinates->longitude; @@ -727,22 +553,18 @@ int facilities_config() epv.space.type = *mrep->data->choice.attributes.gpsType; epv.time.type = *mrep->data->choice.attributes.clockType; - asn_INTEGER2ulong(mrep->data->choice.attributes.clock, (unsigned long long *)&epv.time.clock); - asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset, (unsigned long long *)&epv.time.offset); + asn_INTEGER2ulong(mrep->data->choice.attributes.clock, (unsigned long long*) &epv.time.clock); + asn_INTEGER2ulong(mrep->data->choice.attributes.clockOffset,(unsigned long long*) &epv.time.offset); - if (config->facilities.mcm.activate) - { + if (config->facilities.mcm.activate) { epv.trajectory.len = mrep->data->choice.attributes.trajectory->list.count; - for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) - { + for (int i = 0; i < mrep->data->choice.attributes.trajectory->list.count; ++i) { epv.trajectory.path[i].latitude = mrep->data->choice.attributes.trajectory->list.array[i]->latitude; epv.trajectory.path[i].longitude = mrep->data->choice.attributes.trajectory->list.array[i]->longitude; - asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, (unsigned long long *)&epv.trajectory.path[i].timestamp); + asn_INTEGER2ulong(&mrep->data->choice.attributes.trajectory->list.array[i]->timestamp, (unsigned long long*) &epv.trajectory.path[i].timestamp); } } - } - else - { + } else { log_error("rejected MR attribute request"); rv = 1; goto cleanup; @@ -750,23 +572,11 @@ int facilities_config() ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq); ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep); - if (config->facilities.saem.activate) - { // TODO handle various services + if (config->facilities.saem.activate) { // TODO handle various services facilities.bulletin.to_provide_len = 1; facilities.bulletin.to_provide[0] = calloc(1, sizeof(announcement_t)); + facilities.bulletin.to_provide[0]->endpoint.port = 7010 + config->facilities.saem.service_to_advertise; facilities.bulletin.to_provide[0]->its_aid = config->facilities.saem.service_to_advertise; - switch (config->facilities.saem.service_to_advertise) - { - case SAID_ETC: - facilities.bulletin.to_provide[0]->endpoint.port = 7011; - break; - case SAID_EVCSN: - facilities.bulletin.to_provide[0]->endpoint.port = Port_poi; - break; - default: - log_error("unknown service to advertise! [config.c:facilities.saem.service_to_advertise]"); - break; - } } facilities.vehicle.length = config->facilities.vehicle.length; @@ -775,19 +585,18 @@ int facilities_config() // Logging facilities.logging.recorder = config->facilities.logging.management; - if (config->general.logging.enabled && config->facilities.logging.dbms) - { + if (config->general.logging.enabled && config->facilities.logging.dbms) { facilities.logging.dbms = calloc(1, sizeof(itss_db_t)); if (itss_db_init( - facilities.logging.dbms, - config->general.logging.database, - config->general.logging.table_style, - ITSS_FACILITIES, - config->general.logging.host, - config->general.logging.port, - config->general.logging.username, - config->general.logging.password)) - { + facilities.logging.dbms, + config->general.logging.database, + config->general.logging.table_style, + ITSS_FACILITIES, + config->general.logging.host, + config->general.logging.port, + config->general.logging.username, + config->general.logging.password + )) { log_error("failed to initialize the database -> turning off logging"); free(facilities.logging.dbms); facilities.logging.dbms = NULL; @@ -799,3 +608,4 @@ cleanup: return rv; } + diff --git a/src/evcsnm.c b/src/evcsnm.c index 955c0ec..8dc2457 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -220,18 +220,3 @@ void *evcsn_service() return NULL; } - - -evcs_info_t* evcs_info_new(EVCSInfo_t* tpi) { - evcs_info_t* ti = calloc(1, sizeof(evcs_info_t)); - - ti->timestamp = itss_time_get(); - ti->asn = tpi; - - return ti; -} - -void evcs_info_free(evcs_info_t* ti) { - ASN_STRUCT_FREE(asn_DEF_EVCSInfo, ti->asn); - free(ti); -} diff --git a/src/evcsnm.h b/src/evcsnm.h index 4cc799c..0ccd2fc 100644 --- a/src/evcsnm.h +++ b/src/evcsnm.h @@ -7,16 +7,14 @@ #include #include -#include #include #include -#define EVCSNM_INFOS_MAX_LENGTH 16 //MAX number of charging spots #define POS_HISTORY_MAX_LEN 24 +#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1 -enum EVCSNM_CHECK_R -{ +enum EVCSNM_CHECK_R { EVCSNM_OK, EVCSNM_INVALID, EVCSNM_BAD_PERMISSIONS @@ -34,31 +32,17 @@ enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, * * @return NULL */ -void *evcsn_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); +int evcsnm_check(EvcsnPdu_t* evcsnm); -typedef struct evcs_info { - uint64_t timestamp; - EVCSInfo_t* asn; -} evcs_info_t; - -typedef struct evcsnm_args -{ +typedef struct evcsnm_args { bool activate; - struct - { - evcs_info_t *z[EVCSNM_INFOS_MAX_LENGTH]; - uint8_t length; - } infos; } evcsnm_args_t; -evcs_info_t* evcs_info_new(EVCSInfo_t* tpi); -void evcs_info_free(evcs_info_t* ti); - #endif diff --git a/src/saem.c b/src/saem.c index 7b33c7d..6933fdd 100644 --- a/src/saem.c +++ b/src/saem.c @@ -12,156 +12,108 @@ #include #include #include -#include #include #include -SAEM_CODE_R saem_check(SAEM_t *saem, uint8_t *neighbour) -{ + +SAEM_CODE_R saem_check(SAEM_t* saem, uint8_t* neighbour) { int rv = 0; - bulletin_t *bulletin = &facilities.bulletin; + bulletin_t* bulletin = &facilities.bulletin; - if (saem->header.messageID != messageID_saem) - { + if (saem->header.messageID != messageID_saem) { return SAEM_INVALID_HEADER_MESSAGE_ID; } - if (saem->header.protocolVersion != 1) - { + if (saem->header.protocolVersion != 1) { return SAEM_INVALID_HEADER_VERSION; } - if (bulletin->to_consume_len >= MAX_ANNOUNCEMENTS_LEN - 1) - { + if (bulletin->to_consume_len >= MAX_ANNOUNCEMENTS_LEN - 1) { return SAEM_MAX_ANNOUNCEMENTS_REACHED; } pthread_mutex_lock(&bulletin->lock); - if (saem->sam.body.serviceInfos) - { - for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) - { - ServiceInfo_t *si = saem->sam.body.serviceInfos->list.array[i]; + if (saem->sam.body.serviceInfos) { + for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) { + ServiceInfo_t* si = saem->sam.body.serviceInfos->list.array[i]; uint16_t its_aid = si->serviceID; bool new_announcement = false; - if (si->chOptions.extensions) - { + if (si->chOptions.extensions) { - for (int e = 0; e < si->chOptions.extensions->list.count; ++e) - { - switch (si->chOptions.extensions->list.array[e]->present) - { - case ServiceInfoExt_PR_providerServiceContext: - bulletin->to_consume[bulletin->to_consume_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size; - bulletin->to_consume[bulletin->to_consume_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size); - memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.context, - si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf, - si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size); - break; + for (int e = 0; e < si->chOptions.extensions->list.count; ++e) { + switch (si->chOptions.extensions->list.array[e]->present) { + case ServiceInfoExt_PR_providerServiceContext: + bulletin->to_consume[bulletin->to_consume_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size; + bulletin->to_consume[bulletin->to_consume_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size); + memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.context, + si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf, + si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size + ); + break; - case ServiceInfoExt_PR_applicationDataSAM: - bulletin->to_consume[bulletin->to_consume_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size; - bulletin->to_consume[bulletin->to_consume_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); - memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.data, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); + case ServiceInfoExt_PR_applicationDataSAM: + bulletin->to_consume[bulletin->to_consume_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size; + bulletin->to_consume[bulletin->to_consume_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); + memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.data, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size + ); - switch (its_aid) - { - case SAID_ETC: - if (facilities.station_type != 15 && facilities.tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) - { - TollingPaymentInfo_t *tpi = NULL; - asn_dec_rval_t dec = uper_decode_complete( - NULL, - &asn_DEF_TollingPaymentInfo, - (void **)&tpi, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); - if (!dec.code) - { + switch (its_aid) { + case SAID_ETC: + if (facilities.station_type != 15 && facilities.tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) { + TollingPaymentInfo_t* tpi = NULL; + asn_dec_rval_t dec = uper_decode_complete( + NULL, + &asn_DEF_TollingPaymentInfo, + (void**) &tpi, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, + si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size + ); + if (!dec.code) { - bool found = false; - for (int t = 0; t < bulletin->to_consume_len; ++t) - { - if (((tolling_info_t *)bulletin->to_consume[t]->info.internal_p)->asn->id == tpi->id) - { - found = true; - break; + bool found = false; + for (int t = 0; t < bulletin->to_consume_len; ++t) { + if (((tolling_info_t*)bulletin->to_consume[t]->info.internal_p)->asn->id == tpi->id) { + found = true; + break; + } + } + + if (!found) { + facilities.tolling.infos.z[facilities.tolling.infos.length] = tolling_info_new(tpi); + bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.tolling.infos.z[facilities.tolling.infos.length]; + ++facilities.tolling.infos.length; + new_announcement = true; + } + } else { + ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, tpi); } } + break; - if (!found) - { - facilities.tolling.infos.z[facilities.tolling.infos.length] = tolling_info_new(tpi); - bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.tolling.infos.z[facilities.tolling.infos.length]; - ++facilities.tolling.infos.length; - new_announcement = true; - } - } - else - { - ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, tpi); - } + case SAID_EVCSN: + // TODO + break; } break; - case SAID_EVCSN: - if (facilities.station_type != 15 && facilities.evcsnm_args.infos.length < EVCSNM_INFOS_MAX_LENGTH) - { - EVCSInfo_t *evcsi = NULL; - asn_dec_rval_t dec = uper_decode_complete( - NULL, - &asn_DEF_EVCSInfo, - (void **)&evcsi, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf, - si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size); - if (!dec.code) - { - - bool found = false; - for (int t = 0; t < bulletin->to_consume_len; ++t) - { - if (((evcs_info_t *)bulletin->to_consume[t]->info.internal_p)->asn->id == evcsi->id) - { - found = true; - break; - } - } - - if (!found) - { - facilities.evcsnm_args.infos.z[facilities.evcsnm_args.infos.length] = evcs_info_new(evcsi); - bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.evcsnm_args.infos.z[facilities.evcsnm_args.infos.length]; - ++facilities.evcsnm_args.infos.length; - new_announcement = true; - } - } - else - { - ASN_STRUCT_FREE(asn_DEF_EVCSInfo, evcsi); - } - } + case ServiceInfoExt_PR_addressIPv6: + memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16); break; - } - break; - case ServiceInfoExt_PR_addressIPv6: - memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16); - break; + case ServiceInfoExt_PR_servicePort: + bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort; + break; - case ServiceInfoExt_PR_servicePort: - bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort; - break; - - default: - break; + default: + break; } } } @@ -169,26 +121,23 @@ SAEM_CODE_R saem_check(SAEM_t *saem, uint8_t *neighbour) uint16_t ci_index = si->channelIndex; // TODO channelInfos - if (saem->sam.body.channelInfos) - { - if (saem->sam.body.channelInfos->list.count >= ci_index + 1) - { + if (saem->sam.body.channelInfos) { + if (saem->sam.body.channelInfos->list.count >= ci_index + 1) { } } - if (new_announcement && bulletin->to_consume_len < MAX_ANNOUNCEMENTS_LEN - 1) - { + if (new_announcement && bulletin->to_consume_len < MAX_ANNOUNCEMENTS_LEN - 1) { bulletin->to_consume[bulletin->to_consume_len]->its_aid = its_aid; bulletin->to_consume[bulletin->to_consume_len]->station_id = saem->header.stationID; bulletin->to_consume[bulletin->to_consume_len]->timestamp = itss_time_get(); - if (neighbour) - { + if (neighbour) { bulletin->to_consume[bulletin->to_consume_len]->certificate_id = malloc(8); memcpy(bulletin->to_consume[bulletin->to_consume_len]->certificate_id, neighbour, 8); } ++bulletin->to_consume_len; rv = SAEM_NEW; } + } } @@ -197,18 +146,17 @@ SAEM_CODE_R saem_check(SAEM_t *saem, uint8_t *neighbour) return rv == SAEM_NEW ? SAEM_NEW : SAEM_OK; } -void bulletin_init() -{ +void bulletin_init() { - bulletin_t *bulletin = &facilities.bulletin; + bulletin_t* bulletin = &facilities.bulletin; pthread_mutex_init(&bulletin->lock, NULL); bulletin->to_consume_len = 0; - for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) - { + for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) { bulletin->to_consume[i] = calloc(1, sizeof(announcement_t)); } + } /** @@ -217,18 +165,17 @@ void bulletin_init() * @param b_saem_len The encoded SAEM length. * @return 0 on success, 1 otherwise. */ -static int mk_saem(uint8_t *b_saem, uint32_t *b_saem_len) -{ +static int mk_saem(uint8_t* b_saem, uint32_t* b_saem_len) { int rv = 0; + // Check tolling advertisements - if (!facilities.tolling.infos.length && !facilities.evcsnm_args.infos.length) - { + if (!facilities.tolling.infos.length /* || facilites.evcsn.infos.length */) { return 1; } asn_enc_rval_t enc; - SAEM_t *saem = calloc(1, sizeof(SAEM_t)); + SAEM_t* saem = calloc(1, sizeof(SAEM_t)); /* header */ saem->header.protocolVersion = 1; @@ -241,138 +188,102 @@ static int mk_saem(uint8_t *b_saem, uint32_t *b_saem_len) saem->sam.version = 0; saem->sam.body.serviceInfos = calloc(1, sizeof(ServiceInfos_t)); - saem->sam.body.serviceInfos->list.count = facilities.tolling.infos.length + facilities.evcsnm_args.infos.length; - saem->sam.body.serviceInfos->list.size = (facilities.tolling.infos.length + facilities.evcsnm_args.infos.length) * sizeof(void *); - saem->sam.body.serviceInfos->list.array = malloc((facilities.tolling.infos.length + facilities.evcsnm_args.infos.length) * sizeof(void *)); + saem->sam.body.serviceInfos->list.count = facilities.tolling.infos.length; // + facilities.evcsn.infos.length; + saem->sam.body.serviceInfos->list.size = facilities.tolling.infos.length * sizeof(void*); // + facilities.evcsn.infos.length * sizeof(void*); + saem->sam.body.serviceInfos->list.array = malloc(facilities.tolling.infos.length * sizeof(void*)); // + facilities.evcsn.infos.length * sizeof(void*); uint8_t buf[1024]; int i; - for (i = 0; i < facilities.tolling.infos.length; ++i) - { + for (i = 0; i < facilities.tolling.infos.length; ++i) { saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t)); saem->sam.body.serviceInfos->list.array[i]->serviceID = SAID_ETC; saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t)); - ServiceInfoExts_t *exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions; + ServiceInfoExts_t* exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions; - switch (facilities.tolling.protocol.p) - { - case TOLLING_PROTOCOL_SIMPLE: - exts->list.count = 3; - exts->list.size = 3 * sizeof(void *); - exts->list.array = malloc(3 * sizeof(void *)); + switch (facilities.tolling.protocol.p) { + case TOLLING_PROTOCOL_SIMPLE: + exts->list.count = 3; + exts->list.size = 3 * sizeof(void*); + exts->list.array = malloc(3 * sizeof(void*)); - exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; + exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; + + char ctx_s[] = "tolling:simple"; + exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s); + exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s)); + memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s)); - char ctx_s[] = "tolling:simple"; - exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s); - exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s)); - memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s)); + exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[1]->present = ServiceInfoExt_PR_servicePort; + exts->list.array[1]->choice.servicePort = 7011; - exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[1]->present = ServiceInfoExt_PR_servicePort; - exts->list.array[1]->choice.servicePort = 7011; + exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM; + exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024); - exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM; - exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024); + enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024); + if (enc.encoded == -1) { + log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } - enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024); - if (enc.encoded == -1) - { - log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } + exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; + break; - exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; - break; + case TOLLING_PROTOCOL_TLS: + case TOLLING_PROTOCOL_TLS_GN: + case TOLLING_PROTOCOL_TLS_SHS: + exts->list.count = 4; + exts->list.size = 4 * sizeof(void*); + exts->list.array = malloc(4 * sizeof(void*)); - case TOLLING_PROTOCOL_TLS: - case TOLLING_PROTOCOL_TLS_GN: - case TOLLING_PROTOCOL_TLS_SHS: - exts->list.count = 4; - exts->list.size = 4 * sizeof(void *); - exts->list.array = malloc(4 * sizeof(void *)); + exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; + char ctx_t[] = "tolling:tls"; + exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_t); + exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_t)); + memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_t, strlen(ctx_t)); - exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; - char ctx_t[] = "tolling:tls"; - exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_t); - exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_t)); - memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_t, strlen(ctx_t)); + exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[1]->present = ServiceInfoExt_PR_addressIPv6; + exts->list.array[1]->choice.addressIPv6.size = 16; + exts->list.array[1]->choice.addressIPv6.buf = malloc(16); + memcpy(exts->list.array[1]->choice.addressIPv6.buf, facilities.id.ipv6_addr, 16); - exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[1]->present = ServiceInfoExt_PR_addressIPv6; - exts->list.array[1]->choice.addressIPv6.size = 16; - exts->list.array[1]->choice.addressIPv6.buf = malloc(16); - memcpy(exts->list.array[1]->choice.addressIPv6.buf, facilities.id.ipv6_addr, 16); + exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[2]->present = ServiceInfoExt_PR_servicePort; + exts->list.array[2]->choice.servicePort = 7011; - exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[2]->present = ServiceInfoExt_PR_servicePort; - exts->list.array[2]->choice.servicePort = 7011; + exts->list.array[3] = calloc(1, sizeof(ServiceInfoExt_t)); + exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM; + exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024); - exts->list.array[3] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM; - exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024); + enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024); + if (enc.encoded == -1) { + log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } - enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024); - if (enc.encoded == -1) - { - log_error("[sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } - - exts->list.array[3]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; - break; + exts->list.array[3]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; + break; } } - for (int j = 0; j < facilities.evcsnm_args.infos.length; ++j) - { - saem->sam.body.serviceInfos->list.array[i + j] = calloc(1, sizeof(ServiceInfo_t)); - saem->sam.body.serviceInfos->list.array[i + j]->serviceID = SAID_EVCSN; - saem->sam.body.serviceInfos->list.array[i + j]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t)); - - ServiceInfoExts_t *exts = saem->sam.body.serviceInfos->list.array[i + j]->chOptions.extensions; - exts->list.count = 3; - exts->list.size = 3 * sizeof(void *); - exts->list.array = malloc(3 * sizeof(void *)); - - exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext; - - char ctx_s[] = "evcsnm"; - exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s); - exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s)); - memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s)); - - exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[1]->present = ServiceInfoExt_PR_servicePort; - exts->list.array[1]->choice.servicePort = Port_poi; - - exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t)); - exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM; - exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024); - - enc = uper_encode_to_buffer(&asn_DEF_EVCSInfo, NULL, facilities.evcsnm_args.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024); - if (enc.encoded == -1) - { - log_error("[sa] failure to encode EVCSInfo (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } - exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8; + for (int j = 0; j < 0/* + facilities.evcsn.infos.length */; ++j) { + saem->sam.body.serviceInfos->list.array[i+j] = calloc(1, sizeof(ServiceInfo_t)); + saem->sam.body.serviceInfos->list.array[i+j]->serviceID = SAID_EVCSN; + // TODO } pthread_mutex_unlock(&facilities.id.lock); enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len); - if (enc.encoded == -1) - { + if (enc.encoded == -1) { log_error("[sa] failure to encode SAEM (%s)", enc.failed_type->name); rv = 1; goto cleanup; @@ -381,29 +292,27 @@ static int mk_saem(uint8_t *b_saem, uint32_t *b_saem_len) cleanup: ASN_STRUCT_FREE(asn_DEF_SAEM, saem); - + return rv; } -void *sa_service() -{ +void *sa_service() { pthread_mutex_init(&facilities.bulletin.lock, NULL); 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; - bulletin_t *bulletin = &facilities.bulletin; + bulletin_t* bulletin = &facilities.bulletin; bpr->btpType = BTPType_btpB; bpr->gn.destinationAddress.buf = malloc(6); - for (int i = 0; i < 6; ++i) - { + for (int i = 0; i < 6; ++i) { bpr->gn.destinationAddress.buf[i] = 0xff; } bpr->gn.destinationAddress.size = 6; @@ -420,7 +329,7 @@ void *sa_service() bpr->gn.securityProfile.sign = true; int linger_ms = 0; - void *security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); + void* security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); uint8_t tr_oer[1024]; tr_oer[0] = 4; // Facilities @@ -430,50 +339,41 @@ void *sa_service() int sleep_ms = 100; int mk_saem_n_sleep = 0; - while (!facilities.exit) - { + while (!facilities.exit) { - if (bulletin->to_provide_len && sleep_ms * mk_saem_n_sleep >= 1000) - { - rv = mk_saem(bpr->data.buf, (uint32_t *)&bpr->data.size); - if (!rv) - { + if (bulletin->to_provide_len && sleep_ms*mk_saem_n_sleep >= 1000) { + rv = mk_saem(bpr->data.buf, (uint32_t *) &bpr->data.size); + if (!rv) { bpr->id = itss_id(bpr->data.buf, bpr->data.size); - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer + 1, 1023); - if (enc.encoded == -1) - { + 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 SAEM failed"); continue; - } - else - { - itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded + 1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); - // Logging - if (facilities.logging.dbms) - { + } else { + itss_queue_send(facilities.tx_queue, tr_oer, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); + // 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_saem, NULL, bpr->data.buf, bpr->data.size); } - if (facilities.logging.recorder) - { + 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, - bpr->id, - itss_time_get(), - ITSS_FACILITIES, - true); - if (e != -1) - { - itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set"); + buffer, + buffer_len, + bpr->data.buf, + bpr->data.size, + bpr->id, + itss_time_get(), + ITSS_FACILITIES, + true); + if (e != -1) { + itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set"); } } } @@ -492,42 +392,40 @@ void *sa_service() uint64_t now = itss_time_get(); pthread_mutex_lock(&bulletin->lock); - for (int a = 0; a < bulletin->to_consume_len; ++a) - { + for (int a = 0; a < bulletin->to_consume_len; ++a) { // Tolling - if (facilities.tolling.enabled && - bulletin->to_consume[a]->its_aid == 1 && - (now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS || - (facilities.tolling.protocol.p == TOLLING_PROTOCOL_SIMPLE && tpm_should_retransmit())) && - facilities.station_type != 15) - { + if (facilities.tolling.enabled && + bulletin->to_consume[a]->its_aid == 1 && + ( + now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS || + (facilities.tolling.protocol.p == TOLLING_PROTOCOL_SIMPLE && tpm_should_retransmit()) + ) && + facilities.station_type != 15) { - tolling_info_t *info = (tolling_info_t *)bulletin->to_consume[a]->info.internal_p; + tolling_info_t* info = (tolling_info_t*) bulletin->to_consume[a]->info.internal_p; - if (!tpm_is_inside_zone(info)) - { + if (!tpm_is_inside_zone(info)) { continue; } - switch (facilities.tolling.protocol.p) - { - case TOLLING_PROTOCOL_SIMPLE: - tpm_pay(info, &security_socket, bulletin->to_consume[a]->certificate_id, NULL); - ++bulletin->to_consume[a]->n_trigger; - bulletin->to_consume[a]->t_trigger = now; - break; + switch (facilities.tolling.protocol.p) { + case TOLLING_PROTOCOL_SIMPLE: + tpm_pay(info, &security_socket, bulletin->to_consume[a]->certificate_id, NULL); + ++bulletin->to_consume[a]->n_trigger; + bulletin->to_consume[a]->t_trigger = now; + break; - case TOLLING_PROTOCOL_TLS: - case TOLLING_PROTOCOL_TLS_GN: - case TOLLING_PROTOCOL_TLS_SHS: - tpm_pay(info, &security_socket, NULL, bulletin->to_consume[a]->endpoint.ipv6_addr); - ++bulletin->to_consume[a]->n_trigger; - bulletin->to_consume[a]->t_trigger = now; - break; + case TOLLING_PROTOCOL_TLS: + case TOLLING_PROTOCOL_TLS_GN: + case TOLLING_PROTOCOL_TLS_SHS: + tpm_pay(info, &security_socket, NULL, bulletin->to_consume[a]->endpoint.ipv6_addr); + ++bulletin->to_consume[a]->n_trigger; + bulletin->to_consume[a]->t_trigger = now; + break; - default: - break; + default: + break; } } } @@ -536,17 +434,17 @@ void *sa_service() // Tolling management if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS || facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN || - facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS) - { + facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS) { tolling_tlsc_mgmt(facilities.tx_queue, &security_socket); } - usleep(sleep_ms * 1000); + usleep(sleep_ms*1000); } ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); itss_0close(security_socket); + return NULL; } From 6d67577eae6010735b0ac12da5ce8b5a147a4c15 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 7 Mar 2023 12:21:57 +0000 Subject: [PATCH 09/31] added parkingPlacesData and the station is now at Ilhavo's market --- src/evcsnm.c | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 8dc2457..56b8999 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -56,8 +56,8 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs0->chargingStationID = 0; itss_space_lock(); itss_space_get(); - cs0->chargingStationLocation.latitude = epv.space.latitude; - cs0->chargingStationLocation.longitude = epv.space.longitude; + 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; @@ -80,6 +80,16 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs_elem0->typeOfReceptacle.size = 1; cs_elem0->typeOfReceptacle.bits_unused = 0; cs_elem0->typeOfReceptacle.buf[0] = 0xff; + cs_elem0->parkingPlacesData->list.array = calloc(4, sizeof(struct ParkingPlacesData *)); + cs_elem0->parkingPlacesData->list.count = 4; + cs_elem0->parkingPlacesData->list.size = sizeof(struct ParkingPlacesData *) * 4; + for(int parckingPlaceIndex = 0; parckingPlaceIndex < 4; parckingPlaceIndex++) + { + cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex] = calloc(1, sizeof(struct SpotAvailability)); + struct SpotAvailability *spot = cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex]; + spot->blocking = 0; + spot->maxWaitingTimeMinutes = 0; + } itss_space_unlock(epv); // if (facilities.station_type == StationType_roadSideUnit) //{ From 78172e9f5bf112665a0f725df8448e2ed6cc4ebc Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 7 Mar 2023 12:45:05 +0000 Subject: [PATCH 10/31] fixed mem allocs --- src/evcsnm.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 56b8999..9ea83e2 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -80,9 +80,10 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs_elem0->typeOfReceptacle.size = 1; cs_elem0->typeOfReceptacle.bits_unused = 0; cs_elem0->typeOfReceptacle.buf[0] = 0xff; - cs_elem0->parkingPlacesData->list.array = calloc(4, sizeof(struct ParkingPlacesData *)); + cs_elem0->parkingPlacesData = calloc(1, sizeof(struct ParkingPlacesData)); + cs_elem0->parkingPlacesData->list.array = calloc(4, sizeof(void *)); cs_elem0->parkingPlacesData->list.count = 4; - cs_elem0->parkingPlacesData->list.size = sizeof(struct ParkingPlacesData *) * 4; + cs_elem0->parkingPlacesData->list.size = sizeof(void *) * 4; for(int parckingPlaceIndex = 0; parckingPlaceIndex < 4; parckingPlaceIndex++) { cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex] = calloc(1, sizeof(struct SpotAvailability)); From d7a9fb7b83f207b5bcd7f78239f77acd223d0b65 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 7 Mar 2023 19:18:32 +0000 Subject: [PATCH 11/31] changed blocking to true --- src/evcsnm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 9ea83e2..1a79b64 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -88,7 +88,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) { cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex] = calloc(1, sizeof(struct SpotAvailability)); struct SpotAvailability *spot = cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex]; - spot->blocking = 0; + spot->blocking = 1; spot->maxWaitingTimeMinutes = 0; } itss_space_unlock(epv); From 1580d634ec4b7fbc543e989ca606bd36e2a904fe Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Wed, 8 Mar 2023 15:18:37 +0000 Subject: [PATCH 12/31] only 2 parking spaces available --- src/evcsnm.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 1a79b64..46b2a14 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -84,12 +84,21 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs_elem0->parkingPlacesData->list.array = calloc(4, sizeof(void *)); cs_elem0->parkingPlacesData->list.count = 4; cs_elem0->parkingPlacesData->list.size = sizeof(void *) * 4; - for(int parckingPlaceIndex = 0; parckingPlaceIndex < 4; parckingPlaceIndex++) + // As requested onnly 2 parking places are available + for (int parckingPlaceIndex = 0; parckingPlaceIndex < 4; parckingPlaceIndex++) { cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex] = calloc(1, sizeof(struct SpotAvailability)); struct SpotAvailability *spot = cs_elem0->parkingPlacesData->list.array[parckingPlaceIndex]; - spot->blocking = 1; - spot->maxWaitingTimeMinutes = 0; + if (parckingPlaceIndex < 2) + { + spot->maxWaitingTimeMinutes = 0; + spot->blocking = 1; + } + else + { + spot->maxWaitingTimeMinutes = 10; + spot->blocking = 0; + } } itss_space_unlock(epv); // if (facilities.station_type == StationType_roadSideUnit) From f999521721e8b1ee6d3f8fd43d1fab4258aefee8 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Thu, 9 Mar 2023 16:05:31 +0000 Subject: [PATCH 13/31] removed the extra 2 spaces --- src/evcsnm.c | 20 ++++++-------------- 1 file changed, 6 insertions(+), 14 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 46b2a14..4152075 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -81,24 +81,16 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs_elem0->typeOfReceptacle.bits_unused = 0; cs_elem0->typeOfReceptacle.buf[0] = 0xff; cs_elem0->parkingPlacesData = calloc(1, sizeof(struct ParkingPlacesData)); - cs_elem0->parkingPlacesData->list.array = calloc(4, sizeof(void *)); - cs_elem0->parkingPlacesData->list.count = 4; - cs_elem0->parkingPlacesData->list.size = sizeof(void *) * 4; + 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 < 4; parckingPlaceIndex++) + 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]; - if (parckingPlaceIndex < 2) - { - spot->maxWaitingTimeMinutes = 0; - spot->blocking = 1; - } - else - { - spot->maxWaitingTimeMinutes = 10; - spot->blocking = 0; - } + spot->maxWaitingTimeMinutes = 0; + spot->blocking = 1; } itss_space_unlock(epv); // if (facilities.station_type == StationType_roadSideUnit) From 6920e5bc69bd222aef29b3a438ee0c0ed41eb497 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Thu, 9 Mar 2023 16:54:42 +0000 Subject: [PATCH 14/31] eddited the ilhavo charging spot type and type of receptacle --- src/evcsnm.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 4152075..383bfc6 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -75,11 +75,11 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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] = 0xff; + 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] = 0xff; + 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; From d98d059defa007e1522512d9e3f16fa88d49530c Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Sat, 11 Mar 2023 13:30:42 +0000 Subject: [PATCH 15/31] set relayCapable to true, tried to fix the emptystring creation and added bookingContactInfo --- src/evcsnm.c | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 383bfc6..a108c4d 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -21,11 +21,22 @@ #include #include +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 + 1, sizeof(uint8_t)); + utf8_string->size = length + 1; + memcpy(utf8_string->buf, string, length); + utf8_string->buf[length] = '\0'; + return utf8_string; +} + + static UTF8String_t *create_empty_utf8_string() { UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t)); utf8_string->buf = calloc(1, 1); utf8_string->size = 1; + utf8_string->buf[0] = '\0'; return utf8_string; } @@ -45,7 +56,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) evcsnm->evcsn.poiHeader.poiType = 1; // set to "EV charging station POI ID = 1" asn_ulong2INTEGER(&evcsnm->evcsn.poiHeader.timeStamp, now); - evcsnm->evcsn.poiHeader.relayCapable = 0; + evcsnm->evcsn.poiHeader.relayCapable = 1; evcsnm->evcsn.evcsnData.totalNumberOfStations = 1; evcsnm->evcsn.evcsnData.chargingStationsData.list.array = calloc(1, sizeof(void *)); @@ -66,6 +77,8 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs0->accessibility = *create_empty_utf8_string(); cs0->pricing = *create_empty_utf8_string(); cs0->openingDaysHours = *create_empty_utf8_string(); + const 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; From b5f772324ee7f988e786c1decaa3b8363f7435fc Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Sat, 11 Mar 2023 13:55:08 +0000 Subject: [PATCH 16/31] fixed missing type --- src/evcsnm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index a108c4d..89347a8 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -77,7 +77,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs0->accessibility = *create_empty_utf8_string(); cs0->pricing = *create_empty_utf8_string(); cs0->openingDaysHours = *create_empty_utf8_string(); - const booking_url = "ccam.av.it.pt"; + 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; From 16b70f92da286d3caaa1c7c6458aa43c46d62abf Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Sat, 11 Mar 2023 16:51:38 +0000 Subject: [PATCH 17/31] another attempt at fixing empty strings --- src/evcsnm.c | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 89347a8..fae3283 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -23,10 +23,9 @@ 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 + 1, sizeof(uint8_t)); - utf8_string->size = length + 1; + utf8_string->buf = calloc(length, sizeof(uint8_t)); + utf8_string->size = length; memcpy(utf8_string->buf, string, length); - utf8_string->buf[length] = '\0'; return utf8_string; } @@ -34,9 +33,7 @@ static UTF8String_t *create_utf8_from_string(const char* string, size_t length) static UTF8String_t *create_empty_utf8_string() { UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t)); - utf8_string->buf = calloc(1, 1); - utf8_string->size = 1; - utf8_string->buf[0] = '\0'; + utf8_string->size = 0; return utf8_string; } From dd462510489d981e1dd429ca656ed2e126ae4363 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 14 Mar 2023 15:10:10 +0000 Subject: [PATCH 18/31] modified the mandatory fields with the ilhavo spot info --- src/evcsnm.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index fae3283..58804c3 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -71,9 +71,9 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; cs0->chargingStationLocation.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; cs0->chargingStationLocation.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; - cs0->accessibility = *create_empty_utf8_string(); - cs0->pricing = *create_empty_utf8_string(); - cs0->openingDaysHours = *create_empty_utf8_string(); + cs0->accessibility = *create_utf8_from_string("Free Access", 11); + cs0->pricing = *create_utf8_from_string(".15 €/kWh", 9); + cs0->openingDaysHours = *create_utf8_from_string("Always", 6); 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 *)); @@ -81,7 +81,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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_empty_utf8_string(); + cs_elem0->energyAvailability = *create_utf8_from_string("Max: 30kW", 9); cs_elem0->type.buf = calloc(1, sizeof(uint8_t)); cs_elem0->type.size = 1; cs_elem0->type.bits_unused = 0; From 8d4a4a0419b31cb989571b6a29985aac8679f52c Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 14 Mar 2023 15:17:46 +0000 Subject: [PATCH 19/31] added \0 to try to control the utf8strings --- src/evcsnm.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 58804c3..56d06af 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -23,9 +23,10 @@ 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); + utf8_string->buf = calloc(length+1, sizeof(uint8_t)); + utf8_string->size = length+1; + memcpy(utf8_string->buf, string, length+1); + utf8_string->buf[length] = '\0'; return utf8_string; } From 44423580636426922490c1354163de99babbd591 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 14 Mar 2023 17:50:54 +0000 Subject: [PATCH 20/31] tmp fix test --- src/evcsnm.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 56d06af..713b68d 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -72,9 +72,9 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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", 11); - cs0->pricing = *create_utf8_from_string(".15 €/kWh", 9); - cs0->openingDaysHours = *create_utf8_from_string("Always", 6); + 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 *)); @@ -82,7 +82,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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", 9); + 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; From b31988f1b289402c2f589c0bfd10c4e29fde50af Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Tue, 14 Mar 2023 17:53:33 +0000 Subject: [PATCH 21/31] final utf8String translation --- src/evcsnm.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/evcsnm.c b/src/evcsnm.c index 713b68d..e68d86f 100644 --- a/src/evcsnm.c +++ b/src/evcsnm.c @@ -23,10 +23,9 @@ 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+1, sizeof(uint8_t)); - utf8_string->size = length+1; - memcpy(utf8_string->buf, string, length+1); - utf8_string->buf[length] = '\0'; + utf8_string->buf = calloc(length, sizeof(uint8_t)); + utf8_string->size = length; + memcpy(utf8_string->buf, string, length); return utf8_string; } From 119593d8e1ae8b4530d3cac7ef5998f3ff53abc6 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 31 Mar 2023 14:59:27 +0100 Subject: [PATCH 22/31] generalized the EVMessages and added evrsrm --- .vscode/settings.json | 7 ++++++- src/CMakeLists.txt | 3 ++- src/config.c | 2 +- src/{evcsnm.c => evm.c} | 22 ++++++++++++++++------ src/{evcsnm.h => evm.h} | 24 ++++++++++++------------ src/facilities.c | 21 ++++++++++++++++++--- src/facilities.h | 4 ++-- src/requests.c | 9 +++++++++ 8 files changed, 66 insertions(+), 26 deletions(-) rename src/{evcsnm.c => evm.c} (96%) rename src/{evcsnm.h => evm.h} (54%) diff --git a/.vscode/settings.json b/.vscode/settings.json index 93f6441..47993ba 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -13,6 +13,11 @@ "asn_application.h": "c", "itspduheader.h": "c", "evchargingspotnotificationpoimessage.h": "c", - "vcm.h": "c" + "vcm.h": "c", + "cam.h": "c", + "facilities.h": "c", + "indications.h": "c", + "time.h": "c", + "stdint.h": "c" } } \ No newline at end of file diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 09d3cac..ca34548 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -10,7 +10,7 @@ ADD_EXECUTABLE(it2s-itss-facilities saem.c tpm.c vcm.c - evcsnm.c + evm.c ) TARGET_LINK_LIBRARIES(it2s-itss-facilities @@ -30,6 +30,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -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 98b5617..edf0365 100644 --- a/src/config.c +++ b/src/config.c @@ -395,7 +395,7 @@ int facilities_config() { facilities.coordination.vcm_period_max = config->facilities.mcm.period_max; // EVCSNM - facilities.evcsnm_args.activate = config->facilities.evcsnm.activate; + facilities.evm_args.activate = config->facilities.evcsnm.activate; // Replay facilities.replay = config->networking.replay.activate; diff --git a/src/evcsnm.c b/src/evm.c similarity index 96% rename from src/evcsnm.c rename to src/evm.c index e68d86f..6e0887a 100644 --- a/src/evcsnm.c +++ b/src/evm.c @@ -1,4 +1,4 @@ -#include "evcsnm.h" +#include "evm.h" #include "facilities.h" #include @@ -126,12 +126,22 @@ int evcsnm_check(EvcsnPdu_t *evcsnm) return 0; } -enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len) -{ - int rv = 0; - return rv; +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; @@ -176,7 +186,7 @@ void *evcsn_service() while (!facilities.exit) { usleep(1000 * 1000); - if (facilities.evcsnm_args.activate) + if (facilities.evm_args.activate) { rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size); if (rv) diff --git a/src/evcsnm.h b/src/evm.h similarity index 54% rename from src/evcsnm.h rename to src/evm.h index 0ccd2fc..4e70f9f 100644 --- a/src/evcsnm.h +++ b/src/evm.h @@ -1,5 +1,5 @@ -#ifndef FACILITIES_EVCSNM_H -#define FACILITIES_EVCSNM_H +#ifndef FACILITIES_EVM_H +#define FACILITIES_EVM_H #include #include @@ -7,17 +7,15 @@ #include #include +#include #include #include -#define POS_HISTORY_MAX_LEN 24 -#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1 - -enum EVCSNM_CHECK_R { - EVCSNM_OK, - EVCSNM_INVALID, - EVCSNM_BAD_PERMISSIONS +enum EVM_CHECK_R { + EVM_OK, + EVM_INVALID, + EVM_BAD_PERMISSIONS }; /* @@ -25,7 +23,8 @@ enum EVCSNM_CHECK_R { * * @return A EVCSNM check code */ -enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len); +enum EVM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len); +enum EVM_CHECK_R check_evrsrm(BTPPacketIndication_t *bpi, EV_RSR_t *evrsrm, uint8_t *ssp, uint32_t ssp_len); /* * @brief Main CA service @@ -40,9 +39,10 @@ void* evcsn_service(); * @return 0 on success, other value otherwise */ int evcsnm_check(EvcsnPdu_t* evcsnm); +int evrsrm_check(EV_RSR_t *evrsrm); -typedef struct evcsnm_args { +typedef struct evm_args { bool activate; -} evcsnm_args_t; +} evm_args_t; #endif diff --git a/src/facilities.c b/src/facilities.c index ae6a197..d0a7d77 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -9,7 +9,7 @@ #include "saem.h" #include "tpm.h" #include "vcm.h" -#include "evcsnm.h" +#include "evm.h" #include #include @@ -171,6 +171,14 @@ static int transport_indication(void *responder, void **security_socket, uint8_t 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); @@ -286,12 +294,19 @@ static int transport_indication(void *responder, void **security_socket, uint8_t fwd = true; break; case Port_poi: - if (facilities.evcsnm_args.activate) + 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; default: break; } @@ -936,7 +951,7 @@ int main() } // EVCSN - if (facilities.evcsnm_args.activate) + if (facilities.evm_args.activate) pthread_create(&facilities.evcsn_service, NULL, evcsn_service, NULL); security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ); diff --git a/src/facilities.h b/src/facilities.h index c4b7123..2722045 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -12,7 +12,7 @@ #include "saem.h" #include "tpm.h" #include "vcm.h" -#include "evcsnm.h" +#include "evm.h" #include #include @@ -73,7 +73,7 @@ typedef struct facilities { coordination_t coordination; // EVCSN - evcsnm_args_t evcsnm_args; + evm_args_t evm_args; // Logging struct { diff --git a/src/requests.c b/src/requests.c index 97e0fe7..c5fc7c3 100644 --- a/src/requests.c +++ b/src/requests.c @@ -4,6 +4,7 @@ #include "infrastructure.h" #include "requests.h" #include "cpm.h" +#include "evm.h" #include #include @@ -108,6 +109,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); From a7faad26fb6053452946ea991d0fa30f1bc72ebf Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 31 Mar 2023 15:04:14 +0100 Subject: [PATCH 23/31] removed.vscode trash --- .vscode/settings.json | 23 ----------------------- 1 file changed, 23 deletions(-) delete mode 100644 .vscode/settings.json diff --git a/.vscode/settings.json b/.vscode/settings.json deleted file mode 100644 index 47993ba..0000000 --- a/.vscode/settings.json +++ /dev/null @@ -1,23 +0,0 @@ -{ - "files.associations": { - "packet.h": "c", - "managementrequest.h": "c", - "facilitiesindication.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", - "cam.h": "c", - "facilities.h": "c", - "indications.h": "c", - "time.h": "c", - "stdint.h": "c" - } -} \ No newline at end of file From 0c76b48ef624c9a6179ecc529951c29181c08534 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Mon, 3 Apr 2023 19:03:32 +0100 Subject: [PATCH 24/31] fixed documentation --- src/evm.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/evm.h b/src/evm.h index 4e70f9f..89e6f88 100644 --- a/src/evm.h +++ b/src/evm.h @@ -34,8 +34,8 @@ enum EVM_CHECK_R check_evrsrm(BTPPacketIndication_t *bpi, EV_RSR_t *evrsrm, uint void* evcsn_service(); /** - * Analyses a VCM - * @param vcm The VCM to be analyzed + * Analyses a evm + * @param evm The evm to be analyzed * @return 0 on success, other value otherwise */ int evcsnm_check(EvcsnPdu_t* evcsnm); From a8a1509175ecdf3b95f3cebe5db4009cccda4c0e Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Wed, 5 Apr 2023 16:01:49 +0100 Subject: [PATCH 25/31] added managed_msg for evrsrm --- src/requests.c | 554 ++++++++++++++++++++++++++++--------------------- 1 file changed, 317 insertions(+), 237 deletions(-) diff --git a/src/requests.c b/src/requests.c index c5fc7c3..a012275 100644 --- a/src/requests.c +++ b/src/requests.c @@ -19,7 +19,8 @@ #include #include -int facilities_request_result_accepted(void* responder) { +int facilities_request_result_accepted(void *responder) +{ int rv = 0; FacilitiesReply_t *fr = calloc(1, sizeof(FacilitiesReply_t)); @@ -33,7 +34,8 @@ 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)); @@ -47,15 +49,15 @@ 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; @@ -66,66 +68,68 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ uint32_t transmission_interval = 0; uint64_t transmission_start = 0; - switch (frm->itsMessageType) { - case ItsMessageType_cam: - its_msg_def = &asn_DEF_CAM; - its_msg = calloc(1, sizeof(CAM_t)); - bpr->destinationPort = Port_cam; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 1; + switch (frm->itsMessageType) + { + case ItsMessageType_cam: + its_msg_def = &asn_DEF_CAM; + its_msg = calloc(1, sizeof(CAM_t)); + bpr->destinationPort = Port_cam; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 1; - break; - case ItsMessageType_denm: - its_msg_def = &asn_DEF_DENM; - its_msg = calloc(1, sizeof(DENM_t)); - bpr->destinationPort = Port_denm; - bpr->gn.packetTransportType = PacketTransportType_gbc; - bpr->gn.trafficClass = 2; + break; + case ItsMessageType_denm: + its_msg_def = &asn_DEF_DENM; + its_msg = calloc(1, sizeof(DENM_t)); + bpr->destinationPort = Port_denm; + bpr->gn.packetTransportType = PacketTransportType_gbc; + bpr->gn.trafficClass = 2; - break; - case ItsMessageType_ivim: - its_msg_def = &asn_DEF_IVIM; - its_msg = calloc(1, sizeof(IVIM_t)); - bpr->destinationPort = Port_ivim; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 1; + break; + case ItsMessageType_ivim: + its_msg_def = &asn_DEF_IVIM; + its_msg = calloc(1, sizeof(IVIM_t)); + bpr->destinationPort = Port_ivim; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 1; - break; + break; - case ItsMessageType_cpm: - its_msg_def = &asn_DEF_CPM; - its_msg = calloc(1, sizeof(CPM_t)); - bpr->destinationPort = Port_cpm; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 2; + case ItsMessageType_cpm: + its_msg_def = &asn_DEF_CPM; + its_msg = calloc(1, sizeof(CPM_t)); + bpr->destinationPort = Port_cpm; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 2; - break; + break; - case 2044: //VERCOe - its_msg_def = &asn_DEF_VERCOe; - its_msg = calloc(1, sizeof(VERCOe_t)); - bpr->destinationPort = 2044; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 2; - break; + case 2044: // VERCOe + its_msg_def = &asn_DEF_VERCOe; + its_msg = calloc(1, sizeof(VERCOe_t)); + bpr->destinationPort = 2044; + bpr->gn.packetTransportType = PacketTransportType_shb; + 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; + 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); - rv = 1; - goto cleanup; + default: + log_error("unrecognized FR message type (%lld)", frm->itsMessageType); + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; } - asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void**) &its_msg, frm->data.buf, frm->data.size); - if (dec.code) { + 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_debug("invalid FR %s received", its_msg_def->name); facilities_request_result_rejected(responder); rv = 1; @@ -133,161 +137,204 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ } uint64_t id = 0; - if (!frm->id) { + if (!frm->id) + { id = itss_id(frm->data.buf, frm->data.size); - } else if (*frm->id == 0) { + } + else if (*frm->id == 0) + { id = itss_id(frm->data.buf, frm->data.size); } bool is_update = false; int managed_msg = false; - if (frm->itsMessageType == ItsMessageType_denm) { + 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) { + 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) { + if (facilities.station_type != 15) + { pthread_mutex_lock(&facilities.lightship.lock); - if (facilities.lightship.path_history_len > 0) { + 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) { + if (path_history[0]->alt != AltitudeValue_unavailable && epv.space.altitude != AltitudeValue_unavailable) + { ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.altitude; - } else { + } + else + { ph->list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; } - if (path_history[0]->lat != Latitude_unavailable && epv.space.latitude != Latitude_unavailable) { + if (path_history[0]->lat != Latitude_unavailable && epv.space.latitude != Latitude_unavailable) + { ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.latitude; - } else { + } + else + { ph->list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; } - 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; - } else { + 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; + } + 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)); + for (int i = 1; i < path_history_len; ++i) + { + 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; - } else { + 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; - } else { + 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; - } else { + 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; - } else { + 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; } } } - } else if (frm->itsMessageType == ItsMessageType_ivim) { + } + 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) { + 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); + } + else + { + 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); + } + else + { + asn_INTEGER2ulong((INTEGER_t *)((IVIM_t *)its_msg)->ivi.mandatory.validTo, (unsigned long long *)&valid_to); } transmission_start = valid_from; @@ -295,8 +342,11 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ transmission_duration = valid_to - valid_from; } } - - if (!facilities.replay) { + else if (frm->itsMessageType == ItsMessageType_evrsr) { + managed_msg = true; + } + if (!facilities.replay) + { transmission_interval = 0; transmission_duration = 0; } @@ -305,14 +355,16 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ facilities_request_result_accepted(responder); // Forward message to [transport] - if (fwd) { + if (fwd) + { bpr->btpType = BTPType_btpB; bpr->id = id; bpr->data.buf = malloc(2048); asn_enc_rval_t enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, bpr->data.buf, 2048); - if (enc.encoded == -1) { + if (enc.encoded == -1) + { log_error("failed encoding ITS message into UPER (%s)", enc.failed_type->name); rv = 1; goto cleanup; @@ -320,25 +372,30 @@ 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) { + if (transmission_start) + { bpr->gn.repetitionStart = malloc(sizeof(long)); *bpr->gn.repetitionStart = transmission_start; } - if (transmission_interval) { + if (transmission_interval) + { bpr->gn.repetitionInterval = malloc(sizeof(long)); *bpr->gn.repetitionInterval = transmission_interval; } - if (transmission_duration) { + if (transmission_duration) + { bpr->gn.maximumRepetitionTime = malloc(sizeof(long)); *bpr->gn.maximumRepetitionTime = transmission_duration; } - if (is_update) { + if (is_update) + { bpr->gn.isUpdate = malloc(sizeof(long)); *bpr->gn.isUpdate = 1; } @@ -348,45 +405,50 @@ int facilities_request_single_message(void* responder, FacilitiesMessageRequest_ uint8_t tr_oer[2048]; 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) { + if (enc.encoded == -1) + { log_error("failed encoding TR (%s)", enc.failed_type->name); 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, tr_oer, enc.encoded + 1, ITSS_TRANSPORT, id, "TR.packet.btp"); } // Logging - if (facilities.logging.recorder) { + if (facilities.logging.recorder) + { 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); - if (e != -1) { - itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"); + 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"); } } 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; @@ -399,14 +461,16 @@ int facilities_request_active_episodes(void* responder, FacilitiesRequest_t* fre uint16_t na = 0; - for (int e = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) { - switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) { - case EpisodeType_denm: - na += nae; - break; - case EpisodeType_ivim: - na += nas; - break; + for (int e = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) + { + switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) + { + case EpisodeType_denm: + na += nae; + break; + case EpisodeType_ivim: + na += nas; + break; } } @@ -414,52 +478,60 @@ int facilities_request_active_episodes(void* responder, FacilitiesRequest_t* fre frep->choice.data.choice.episodes.list.size = na * sizeof(void *); frep->choice.data.choice.episodes.list.array = malloc(na * sizeof(void *)); - for (int e = 0, j = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) { - switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) { - case EpisodeType_denm: - for (int i = 0, n = 0; n < nae; ++i) { - if (facilities.den.events[i]->enabled && facilities.den.events[i]->state == EVENT_ACTIVE) { - frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); - frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_denm; - frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities.den.events[i]->denm, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); - if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */ - log_error("failed encoding DENM for FDResult (%s)", enc.failed_type->name); - frep->choice.data.choice.episodes.list.array[j]->data.size = 0; - continue; - } - frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; - ++j; - ++n; + for (int e = 0, j = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) + { + switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) + { + case EpisodeType_denm: + for (int i = 0, n = 0; n < nae; ++i) + { + if (facilities.den.events[i]->enabled && facilities.den.events[i]->state == EVENT_ACTIVE) + { + frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); + frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_denm; + frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities.den.events[i]->denm, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); + if (enc.encoded == -1) + { /* encoding shouldn't fail as all saved DENMs are structurally valid */ + log_error("failed encoding DENM for FDResult (%s)", enc.failed_type->name); + frep->choice.data.choice.episodes.list.array[j]->data.size = 0; + continue; } + frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; + ++j; + ++n; } - break; - case EpisodeType_ivim: - for (int i = 0, n = 0; n < nas; ++i) { - if (facilities.infrastructure.services[i]->enabled && facilities.infrastructure.services[i]->state == SERVICE_ACTIVE) { - frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); - frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_ivim; - frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_IVIM, NULL, facilities.infrastructure.services[i]->ivim, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); - if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */ - log_error("failed encoding IVIM for FDResult (%s)", enc.failed_type->name); - frep->choice.data.choice.episodes.list.array[j]->data.size = 0; - continue; - } - frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; - ++j; - ++n; + } + break; + case EpisodeType_ivim: + for (int i = 0, n = 0; n < nas; ++i) + { + if (facilities.infrastructure.services[i]->enabled && facilities.infrastructure.services[i]->state == SERVICE_ACTIVE) + { + frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); + frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_ivim; + frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_IVIM, NULL, facilities.infrastructure.services[i]->ivim, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); + if (enc.encoded == -1) + { /* encoding shouldn't fail as all saved DENMs are structurally valid */ + log_error("failed encoding IVIM for FDResult (%s)", enc.failed_type->name); + frep->choice.data.choice.episodes.list.array[j]->data.size = 0; + continue; } + frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; + ++j; + ++n; } - break; - default: - log_error("unrecognized FR event type (%lld)", *freq->choice.data.choice.activeEpisodes.list.array[e]); - pthread_mutex_unlock(&facilities.den.lock); - pthread_mutex_unlock(&facilities.infrastructure.lock); + } + break; + default: + log_error("unrecognized FR event type (%lld)", *freq->choice.data.choice.activeEpisodes.list.array[e]); + pthread_mutex_unlock(&facilities.den.lock); + pthread_mutex_unlock(&facilities.infrastructure.lock); - facilities_request_result_rejected(responder); - rv = 1; - goto cleanup; + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; } } @@ -468,7 +540,8 @@ int facilities_request_active_episodes(void* responder, FacilitiesRequest_t* fre frep_oer = malloc(32768); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 32768); - if (enc.encoded == -1) { + if (enc.encoded == -1) + { log_error("failed encoding FReply (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); @@ -485,38 +558,42 @@ 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); - 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]->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; - pthread_mutex_unlock(&facilities.id.lock); - break; + 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]->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; + pthread_mutex_unlock(&facilities.id.lock); + break; - default: - log_debug("unrecognized FR attribute type request"); - facilities_request_result_rejected(responder); - rv = 1; - goto cleanup; + default: + log_debug("unrecognized FR attribute type request"); + facilities_request_result_rejected(responder); + 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) { + if (enc.encoded == -1) + { log_error("failed encoding FReply (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); rv = 1; @@ -531,11 +608,11 @@ 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; @@ -543,21 +620,23 @@ 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) { + 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); uint8_t frep_oer[1024]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 1024); - if (enc.encoded == -1) { + if (enc.encoded == -1) + { log_error("failed encoding FDResult (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); rv = 1; @@ -572,30 +651,32 @@ 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) { + 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; uint8_t frep_oer[1024]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 1024); - if (enc.encoded == -1) { + if (enc.encoded == -1) + { log_error("failed encoding FDResult (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); rv = 1; @@ -609,4 +690,3 @@ cleanup: return rv; } - From 84d1417d98daa5956cbc9dc435fdfa46ae49c081 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 14 Apr 2023 19:56:48 +0100 Subject: [PATCH 26/31] added a basic response to the pre reservation request --- .vscode/settings.json | 11 + src/evm.c | 229 ++++++-- src/evm.h | 18 + src/facilities.c | 1185 +++++++++++++++++++---------------------- src/requests.c | 451 +++++++--------- 5 files changed, 931 insertions(+), 963 deletions(-) create mode 100644 .vscode/settings.json 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/evm.c b/src/evm.c index 6e0887a..fd60e25 100644 --- a/src/evm.c +++ b/src/evm.c @@ -1,27 +1,26 @@ #include "evm.h" -#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 -static UTF8String_t *create_utf8_from_string(const char* string, size_t length) { +#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; @@ -29,16 +28,13 @@ static UTF8String_t *create_utf8_from_string(const char* string, size_t length) return utf8_string; } - -static UTF8String_t *create_empty_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) -{ +static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) { int rv = 0; int shm_fd, shm_valid = 0; @@ -51,7 +47,7 @@ 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" + 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; @@ -74,8 +70,8 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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)); + 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; @@ -95,8 +91,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) 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++) - { + 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; @@ -108,8 +103,7 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len) // } asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EvcsnPdu, NULL, evcsnm, evcsnm_oer, 512); - if (enc.encoded == -1) - { + if (enc.encoded == -1) { log_error("[ca] failed encoding evcsnm (%s)", enc.failed_type->name); rv = 1; goto cleanup; @@ -121,29 +115,155 @@ cleanup: return rv; } -int evcsnm_check(EvcsnPdu_t *evcsnm) -{ +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_cam; + + 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) { + return 1; + } + int evrsr_response; + if (evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage) + evrsr_response = evrsrm_pre_reservation_response(evrsr_request, 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 cam 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(EV_RSR_t *evrsr_request, uint8_t *evrsrm_oer, uint32_t *evrsrm_len) { + if (!evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage) + return 1; + + 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; + + pre_reservation_id++; + + 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); + return 1; + } + *evrsrm_len = (enc.encoded + 7) / 8; return 0; } - -int evrsrm_check(EV_RSR_t *evrsrm) -{ +int evcsnm_check(EvcsnPdu_t *evcsnm) { return 0; } -enum EVM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len) -{ +int evrsrm_check(EV_RSR_t *evrsrm) { return 0; } -enum EVM_CHECK_R check_evrsrm(BTPPacketIndication_t *bpi, EV_RSR_t *evrsrm, uint8_t *ssp, uint32_t ssp_len) -{ +enum EVM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len) { return 0; } -void *evcsn_service() -{ +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)); @@ -155,8 +275,7 @@ void *evcsn_service() bpr->btpType = BTPType_btpB; bpr->gn.destinationAddress.buf = malloc(6); - for (int i = 0; i < 6; ++i) - { + for (int i = 0; i < 6; ++i) { bpr->gn.destinationAddress.buf[i] = 0xff; } bpr->gn.destinationAddress.size = 6; @@ -181,16 +300,13 @@ void *evcsn_service() uint8_t tr_oer[1024]; uint8_t fi_oer[1024]; - tr_oer[0] = 4; // Facilities + tr_oer[0] = 4; // Facilities fi_oer[0] = 4; - while (!facilities.exit) - { + while (!facilities.exit) { usleep(1000 * 1000); - if (facilities.evm_args.activate) - { + if (facilities.evm_args.activate) { rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size); - if (rv) - { + if (rv) { continue; } memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size); @@ -201,15 +317,13 @@ void *evcsn_service() 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) - { + 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) - { + if (enc_fdi.encoded == -1) { log_error("encoding FI for evcsnm failed"); continue; } @@ -219,16 +333,14 @@ void *evcsn_service() itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); // Logging - if (facilities.logging.dbms) - { + 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) - { + if (facilities.logging.recorder) { uint16_t buffer_len = 2048; uint8_t buffer[buffer_len]; int e = itss_management_record_packet_sdu( @@ -240,8 +352,7 @@ void *evcsn_service() itss_time_get(), ITSS_FACILITIES, true); - if (e != -1) - { + if (e != -1) { itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"); } } diff --git a/src/evm.h b/src/evm.h index 89e6f88..414988a 100644 --- a/src/evm.h +++ b/src/evm.h @@ -33,6 +33,24 @@ enum EVM_CHECK_R check_evrsrm(BTPPacketIndication_t *bpi, EV_RSR_t *evrsrm, uint */ 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 evrsr_request The reservation request + * + * @return The reservation response or NULL if the request isn't a Pre reservation request + */ + +static int evrsrm_pre_reservation_response(EV_RSR_t *evrsr_request, uint8_t *evrsrm_oer, uint32_t *evrsrm_len); + /** * Analyses a evm * @param evm The evm to be analyzed diff --git a/src/facilities.c b/src/facilities.c index d0a7d77..a2855ba 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -1,53 +1,50 @@ #include "facilities.h" -#include "cam.h" -#include "config.h" -#include "denm.h" -#include "infrastructure.h" -#include "indications.h" -#include "requests.h" -#include "cpm.h" -#include "saem.h" -#include "tpm.h" -#include "vcm.h" -#include "evm.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 #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 "cpm.h" +#include "denm.h" +#include "evm.h" +#include "indications.h" +#include "infrastructure.h" +#include "requests.h" +#include "saem.h" +#include "tpm.h" +#include "vcm.h" facilities_t facilities = {0}; -static int 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; @@ -62,8 +59,7 @@ static int transport_indication(void *responder, void **security_socket, uint8_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) - { + if (dec.code) { log_error("<- invalid TI received"); rv = 1; code = 1; @@ -73,21 +69,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; @@ -99,375 +94,350 @@ static int transport_indication(void *responder, void **security_socket, uint8_t 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; + 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_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_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_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 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 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 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 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; - - 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; + 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; - } + 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; - 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.evm_args.activate) - { - evcsnm_check(its_msg); - } - fwd = true; - break; - - case Port_evrsr: - if (facilities.evm_args.activate) - { - evrsrm_check(its_msg); - } - fwd = true; - 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; + default: + log_debug("messsage with unhandled BTP port received (%lld), ignoring", tpi->choice.btp.destinationPort); + goto cleanup; } - tpr->id = itss_id(tpr->data.buf, tpr->data.size); + packet = tpi->choice.btp.data.buf; + packet_len = tpi->choice.btp.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"); + 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; } - itss_queue_send(facilities.tx_queue, buf, enc.encoded + 1, ITSS_TRANSPORT, tpr->id, "TR.packet.tcp"); - } - else - { + // 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; + } - 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"); + // 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.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; + } + 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; } - 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; + 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); - fmi->id = id; + // Fwd to [applications] + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; - fmi->itsMessageType = 7011; + fmi->id = id; - 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->itsMessageType = 7011; - 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); + 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); - itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message"); + 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; + 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; + 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; @@ -481,22 +451,20 @@ 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 + 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"); } // Logging - if (facilities.logging.dbms) - { + 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, @@ -506,15 +474,13 @@ static int transport_indication(void *responder, void **security_socket, uint8_t itss_time_get(), ITSS_FACILITIES, false); - if (e != -1) - { + 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); @@ -526,59 +492,55 @@ 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)); asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_FacilitiesRequest, (void **)&fr, msg, msg_len); - if (dec.code) - { + 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); - break; - - case FacilitiesRequest_PR_data: - switch (fr->choice.data.present) - { - case FacilitiesDataRequest_PR_activeEpisodes: - rv = facilities_request_active_episodes(responder, fr); + switch (fr->present) { + case FacilitiesRequest_PR_message: + rv = facilities_request_single_message(responder, &fr->choice.message); break; - case FacilitiesDataRequest_PR_attributeTypes: - rv = facilities_request_attribute_types(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_loadedProtectionZones: - rv = facilities_request_loaded_protected_zones(responder, fr); - break; + case FacilitiesDataRequest_PR_attributeTypes: + rv = facilities_request_attribute_types(responder, fr); + break; - case FacilitiesDataRequest_PR_chainInfoSet: - rv = facilities_request_chaininfo_set(responder, &fr->choice.data.choice.chainInfoSet); + 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; + } break; default: - log_error("<- unrecognized FDR type received (%d)", fr->choice.data.present); + log_error("<- unrecognized FR type received (%d)", fr->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: @@ -587,8 +549,7 @@ 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)); @@ -599,16 +560,14 @@ static int security_indication(void *responder_secured, uint8_t *msg, uint32_t m asn_enc_rval_t enc; asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityIndication, (void **)&si, msg, msg_len); - if (dec.code) - { + 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; @@ -617,8 +576,7 @@ 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_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; @@ -627,69 +585,65 @@ 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]); - } - 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; + // 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.id.change.stage = ID_CHANGE_INACTIVE; + 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.id.lock); + pthread_mutex_unlock(&facilities.lightship.lock); - id_changed = true; + // 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; + } + } - break; + facilities.id.change.stage = ID_CHANGE_INACTIVE; - case SecurityIdChangeEventType_abort: - pthread_mutex_unlock(&facilities.id.lock); - pthread_mutex_unlock(&facilities.lightship.lock); + pthread_mutex_unlock(&facilities.id.lock); - facilities.id.change.stage = ID_CHANGE_INACTIVE; - break; + id_changed = true; - default: - pthread_mutex_unlock(&facilities.id.change.lock); - log_error("[networking]<- unhandled idChangeEvent command type"); - rv = 1; - goto cleanup; + 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; } sr->present = SecurityResponse_PR_idChangeEvent; @@ -700,8 +654,7 @@ 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); @@ -715,8 +668,7 @@ 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; @@ -724,8 +676,7 @@ static int networking_indication(void *responder, uint8_t *msg, uint32_t msg_len 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) - { + if (dec.code) { rv = 1; code = 1; itss_0send(responder, &code, 1); @@ -733,15 +684,12 @@ 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) - { - + *ni->choice.data.mobileNeighbour) { pthread_mutex_lock(&facilities.lightship.lock); facilities.lightship.t_last_vehicle = itss_time_get(); facilities.lightship.is_vehicle_near = true; @@ -754,8 +702,7 @@ cleanup: 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; @@ -763,8 +710,7 @@ static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len 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) - { + if (dec.code) { rv = 1; code = 1; itss_0send(responder, &code, 1); @@ -772,8 +718,7 @@ 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; @@ -789,11 +734,9 @@ 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); @@ -812,8 +755,7 @@ cleanup: return rv; } -void *tx() -{ +void *tx() { int rv = 0; itss_queue_t *queue = facilities.tx_queue; @@ -825,15 +767,12 @@ void *tx() 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]; @@ -845,41 +784,36 @@ 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 ", + 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]); - } - 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 ", + 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]); - } - 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; + 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; } } } @@ -891,8 +825,7 @@ 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); @@ -900,8 +833,7 @@ static void sigh(int signum) itss_0close(socket); } -int main() -{ +int main() { signal(SIGTERM, sigh); signal(SIGINT, sigh); signal(SIGKILL, sigh); @@ -914,8 +846,7 @@ int main() bulletin_init(); void *security_socket = NULL; - if (facilities_config()) - { + if (facilities_config()) { goto cleanup; } @@ -944,8 +875,7 @@ 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); } @@ -963,89 +893,77 @@ 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_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_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); - facilities.id.change.stage = ID_CHANGE_INACTIVE; + 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); - } - else - { + + 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"); 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"); - code = 1; - itss_0send(facilities.zmq.responders[i].socket, &code, 1); - continue; + continue; } } } @@ -1066,8 +984,7 @@ 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/requests.c b/src/requests.c index a012275..e3ff66a 100644 --- a/src/requests.c +++ b/src/requests.c @@ -1,26 +1,23 @@ -#include "facilities.h" -#include "cam.h" -#include "denm.h" -#include "infrastructure.h" #include "requests.h" -#include "cpm.h" -#include "evm.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)); @@ -34,8 +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)); @@ -49,8 +45,7 @@ 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)); @@ -68,68 +63,66 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ uint32_t transmission_interval = 0; uint64_t transmission_start = 0; - switch (frm->itsMessageType) - { - case ItsMessageType_cam: - its_msg_def = &asn_DEF_CAM; - its_msg = calloc(1, sizeof(CAM_t)); - bpr->destinationPort = Port_cam; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 1; + switch (frm->itsMessageType) { + case ItsMessageType_cam: + its_msg_def = &asn_DEF_CAM; + its_msg = calloc(1, sizeof(CAM_t)); + bpr->destinationPort = Port_cam; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 1; - break; - case ItsMessageType_denm: - its_msg_def = &asn_DEF_DENM; - its_msg = calloc(1, sizeof(DENM_t)); - bpr->destinationPort = Port_denm; - bpr->gn.packetTransportType = PacketTransportType_gbc; - bpr->gn.trafficClass = 2; + break; + case ItsMessageType_denm: + its_msg_def = &asn_DEF_DENM; + its_msg = calloc(1, sizeof(DENM_t)); + bpr->destinationPort = Port_denm; + bpr->gn.packetTransportType = PacketTransportType_gbc; + bpr->gn.trafficClass = 2; - break; - case ItsMessageType_ivim: - its_msg_def = &asn_DEF_IVIM; - its_msg = calloc(1, sizeof(IVIM_t)); - bpr->destinationPort = Port_ivim; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 1; + break; + case ItsMessageType_ivim: + its_msg_def = &asn_DEF_IVIM; + its_msg = calloc(1, sizeof(IVIM_t)); + bpr->destinationPort = Port_ivim; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 1; - break; + break; - case ItsMessageType_cpm: - its_msg_def = &asn_DEF_CPM; - its_msg = calloc(1, sizeof(CPM_t)); - bpr->destinationPort = Port_cpm; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 2; + case ItsMessageType_cpm: + its_msg_def = &asn_DEF_CPM; + its_msg = calloc(1, sizeof(CPM_t)); + bpr->destinationPort = Port_cpm; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->gn.trafficClass = 2; - break; + break; - case 2044: // VERCOe - its_msg_def = &asn_DEF_VERCOe; - its_msg = calloc(1, sizeof(VERCOe_t)); - bpr->destinationPort = 2044; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->gn.trafficClass = 2; - break; + case 2044: // VERCOe + its_msg_def = &asn_DEF_VERCOe; + its_msg = calloc(1, sizeof(VERCOe_t)); + bpr->destinationPort = 2044; + bpr->gn.packetTransportType = PacketTransportType_shb; + 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; + 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); - rv = 1; - goto cleanup; + default: + log_error("unrecognized FR message type (%lld)", frm->itsMessageType); + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; } asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void **)&its_msg, frm->data.buf, frm->data.size); - if (dec.code) - { + if (dec.code) { log_debug("invalid FR %s received", its_msg_def->name); facilities_request_result_rejected(responder); rv = 1; @@ -137,20 +130,16 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ } uint64_t id = 0; - if (!frm->id) - { + if (!frm->id) { id = itss_id(frm->data.buf, frm->data.size); - } - else if (*frm->id == 0) - { + } else if (*frm->id == 0) { id = itss_id(frm->data.buf, frm->data.size); } bool is_update = false; int managed_msg = false; - if (frm->itsMessageType == ItsMessageType_denm) - { + if (frm->itsMessageType == ItsMessageType_denm) { managed_msg = true; uint8_t event_type = event_manage(its_msg, &id, NULL, 0); @@ -159,36 +148,27 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ if (event_type != EVENT_NEW && event_type != EVENT_UPDATE && event_type != EVENT_CANCELLATION && - event_type != EVENT_NEGATION) - { - + event_type != EVENT_NEGATION) { fwd = false; } if (event_type == EVENT_UPDATE || event_type == EVENT_CANCELLATION || - event_type == EVENT_NEGATION) - { - + event_type == EVENT_NEGATION) { is_update = true; } - if (fwd) - { + if (fwd) { // set stationID pthread_mutex_lock(&facilities.id.lock); ((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) - { + 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) - { + 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)); } ((DENM_t *)its_msg)->denm.location->traces.list.count = 1; @@ -207,30 +187,21 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ ph->list.array[0] = calloc(1, sizeof(PathPoint_t)); itss_space_lock(); - if (path_history[0]->alt != AltitudeValue_unavailable && epv.space.altitude != AltitudeValue_unavailable) - { + if (path_history[0]->alt != AltitudeValue_unavailable && epv.space.altitude != AltitudeValue_unavailable) { ph->list.array[0]->pathPosition.deltaAltitude = path_history[0]->alt - epv.space.altitude; - } - else - { + } else { ph->list.array[0]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; } - if (path_history[0]->lat != Latitude_unavailable && epv.space.latitude != Latitude_unavailable) - { + if (path_history[0]->lat != Latitude_unavailable && epv.space.latitude != Latitude_unavailable) { ph->list.array[0]->pathPosition.deltaLatitude = path_history[0]->lat - epv.space.latitude; - } - else - { + } else { ph->list.array[0]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; } - if (path_history[0]->lon != Longitude_unavailable && epv.space.longitude != Longitude_unavailable) - { + 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; - } - else - { + } else { ph->list.array[0]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; } itss_space_unlock(); @@ -238,34 +209,24 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ 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) - { + for (int i = 1; i < path_history_len; ++i) { ph->list.array[i] = calloc(1, sizeof(PathPoint_t)); - if (path_history[i]->alt != AltitudeValue_unavailable && path_history[i - 1]->alt != AltitudeValue_unavailable) - { + 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 - { + } else { ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable; } - if (path_history[i]->lat != Latitude_unavailable && path_history[i - 1]->lat != Latitude_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; - } - else - { + } else { ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable; } - if (path_history[i]->lon != Longitude_unavailable && path_history[i - 1]->lon != Longitude_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; - } - else - { + } else { ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable; } @@ -277,23 +238,17 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ } // get, set retransmission, duration - if (((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) - { + if (((DENM_t *)its_msg)->denm.management.validityDuration) { transmission_duration = *((uint32_t *)((DENM_t *)its_msg)->denm.management.validityDuration) * 1000; - } - else - { + } else { transmission_duration = 600 * 1000; } } } - } - else if (frm->itsMessageType == ItsMessageType_ivim) - { + } else if (frm->itsMessageType == ItsMessageType_ivim) { managed_msg = true; uint8_t service_type = service_eval(SERVICE_IVI, its_msg, &id, NULL, 0); @@ -301,39 +256,28 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ if (service_type != SERVICE_NEW && service_type != SERVICE_UPDATE && service_type != SERVICE_CANCELLATION && - service_type != SERVICE_NEGATION) - { - + service_type != SERVICE_NEGATION) { fwd = false; } if (service_type == SERVICE_UPDATE || service_type == SERVICE_CANCELLATION || - service_type == SERVICE_NEGATION) - { - + service_type == SERVICE_NEGATION) { is_update = true; } - if (fwd) - { + 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 - { + } else { 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 - { + } else { asn_INTEGER2ulong((INTEGER_t *)((IVIM_t *)its_msg)->ivi.mandatory.validTo, (unsigned long long *)&valid_to); } @@ -341,12 +285,10 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ transmission_interval = facilities.infrastructure.replay_interval; transmission_duration = valid_to - valid_from; } - } - else if (frm->itsMessageType == ItsMessageType_evrsr) { + } else if (frm->itsMessageType == ItsMessageType_evrsr) { managed_msg = true; } - if (!facilities.replay) - { + if (!facilities.replay) { transmission_interval = 0; transmission_duration = 0; } @@ -355,16 +297,14 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ facilities_request_result_accepted(responder); // Forward message to [transport] - if (fwd) - { + if (fwd) { bpr->btpType = BTPType_btpB; bpr->id = id; bpr->data.buf = malloc(2048); asn_enc_rval_t enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, bpr->data.buf, 2048); - if (enc.encoded == -1) - { + if (enc.encoded == -1) { log_error("failed encoding ITS message into UPER (%s)", enc.failed_type->name); rv = 1; goto cleanup; @@ -376,26 +316,22 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ bpr->gn.destinationAddress.buf[i] = 0xff; bpr->gn.destinationAddress.size = 6; - if (transmission_start) - { + if (transmission_start) { bpr->gn.repetitionStart = malloc(sizeof(long)); *bpr->gn.repetitionStart = transmission_start; } - if (transmission_interval) - { + if (transmission_interval) { bpr->gn.repetitionInterval = malloc(sizeof(long)); *bpr->gn.repetitionInterval = transmission_interval; } - if (transmission_duration) - { + if (transmission_duration) { bpr->gn.maximumRepetitionTime = malloc(sizeof(long)); *bpr->gn.maximumRepetitionTime = transmission_duration; } - if (is_update) - { + if (is_update) { bpr->gn.isUpdate = malloc(sizeof(long)); *bpr->gn.isUpdate = 1; } @@ -403,10 +339,9 @@ 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) - { + if (enc.encoded == -1) { log_error("failed encoding TR (%s)", enc.failed_type->name); rv = 1; goto cleanup; @@ -416,8 +351,7 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ } // Logging - if (facilities.logging.recorder) - { + if (facilities.logging.recorder) { uint16_t buffer_len = 2048; uint8_t buffer[buffer_len]; int e = itss_management_record_packet_sdu( @@ -429,8 +363,7 @@ int facilities_request_single_message(void *responder, FacilitiesMessageRequest_ itss_time_get(), ITSS_FACILITIES, true); - if (e != -1) - { + if (e != -1) { itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, id, "MReq.packet.set"); } } @@ -443,8 +376,7 @@ cleanup: 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)); @@ -461,16 +393,14 @@ int facilities_request_active_episodes(void *responder, FacilitiesRequest_t *fre uint16_t na = 0; - for (int e = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) - { - switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) - { - case EpisodeType_denm: - na += nae; - break; - case EpisodeType_ivim: - na += nas; - break; + for (int e = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) { + switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) { + case EpisodeType_denm: + na += nae; + break; + case EpisodeType_ivim: + na += nas; + break; } } @@ -478,60 +408,52 @@ int facilities_request_active_episodes(void *responder, FacilitiesRequest_t *fre frep->choice.data.choice.episodes.list.size = na * sizeof(void *); frep->choice.data.choice.episodes.list.array = malloc(na * sizeof(void *)); - for (int e = 0, j = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) - { - switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) - { - case EpisodeType_denm: - for (int i = 0, n = 0; n < nae; ++i) - { - if (facilities.den.events[i]->enabled && facilities.den.events[i]->state == EVENT_ACTIVE) - { - frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); - frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_denm; - frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities.den.events[i]->denm, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); - if (enc.encoded == -1) - { /* encoding shouldn't fail as all saved DENMs are structurally valid */ - log_error("failed encoding DENM for FDResult (%s)", enc.failed_type->name); - frep->choice.data.choice.episodes.list.array[j]->data.size = 0; - continue; + for (int e = 0, j = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) { + switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) { + case EpisodeType_denm: + for (int i = 0, n = 0; n < nae; ++i) { + if (facilities.den.events[i]->enabled && facilities.den.events[i]->state == EVENT_ACTIVE) { + frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); + frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_denm; + frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities.den.events[i]->denm, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); + if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */ + log_error("failed encoding DENM for FDResult (%s)", enc.failed_type->name); + frep->choice.data.choice.episodes.list.array[j]->data.size = 0; + continue; + } + frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; + ++j; + ++n; } - frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; - ++j; - ++n; } - } - break; - case EpisodeType_ivim: - for (int i = 0, n = 0; n < nas; ++i) - { - if (facilities.infrastructure.services[i]->enabled && facilities.infrastructure.services[i]->state == SERVICE_ACTIVE) - { - frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); - frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_ivim; - frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_IVIM, NULL, facilities.infrastructure.services[i]->ivim, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); - if (enc.encoded == -1) - { /* encoding shouldn't fail as all saved DENMs are structurally valid */ - log_error("failed encoding IVIM for FDResult (%s)", enc.failed_type->name); - frep->choice.data.choice.episodes.list.array[j]->data.size = 0; - continue; + break; + case EpisodeType_ivim: + for (int i = 0, n = 0; n < nas; ++i) { + if (facilities.infrastructure.services[i]->enabled && facilities.infrastructure.services[i]->state == SERVICE_ACTIVE) { + frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t)); + frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_ivim; + frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048); + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_IVIM, NULL, facilities.infrastructure.services[i]->ivim, frep->choice.data.choice.episodes.list.array[j]->data.buf, 2048); + if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */ + log_error("failed encoding IVIM for FDResult (%s)", enc.failed_type->name); + frep->choice.data.choice.episodes.list.array[j]->data.size = 0; + continue; + } + frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; + ++j; + ++n; } - frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8; - ++j; - ++n; } - } - break; - default: - log_error("unrecognized FR event type (%lld)", *freq->choice.data.choice.activeEpisodes.list.array[e]); - pthread_mutex_unlock(&facilities.den.lock); - pthread_mutex_unlock(&facilities.infrastructure.lock); + break; + default: + log_error("unrecognized FR event type (%lld)", *freq->choice.data.choice.activeEpisodes.list.array[e]); + pthread_mutex_unlock(&facilities.den.lock); + pthread_mutex_unlock(&facilities.infrastructure.lock); - facilities_request_result_rejected(responder); - rv = 1; - goto cleanup; + facilities_request_result_rejected(responder); + rv = 1; + goto cleanup; } } @@ -540,8 +462,7 @@ int facilities_request_active_episodes(void *responder, FacilitiesRequest_t *fre frep_oer = malloc(32768); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 32768); - if (enc.encoded == -1) - { + if (enc.encoded == -1) { log_error("failed encoding FReply (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); @@ -558,8 +479,7 @@ 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)); @@ -570,30 +490,27 @@ int facilities_request_attribute_types(void *responder, FacilitiesRequest_t *fre 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); - 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]->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; - pthread_mutex_unlock(&facilities.id.lock); - break; + 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]->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; + pthread_mutex_unlock(&facilities.id.lock); + break; - default: - log_debug("unrecognized FR attribute type request"); - facilities_request_result_rejected(responder); - rv = 1; - goto cleanup; + default: + log_debug("unrecognized FR attribute type request"); + facilities_request_result_rejected(responder); + 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) - { + if (enc.encoded == -1) { log_error("failed encoding FReply (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); rv = 1; @@ -608,8 +525,7 @@ 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)); @@ -624,8 +540,7 @@ int facilities_request_loaded_protected_zones(void *responder, FacilitiesRequest 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) - { + 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); @@ -635,8 +550,7 @@ int facilities_request_loaded_protected_zones(void *responder, FacilitiesRequest uint8_t frep_oer[1024]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 1024); - if (enc.encoded == -1) - { + if (enc.encoded == -1) { log_error("failed encoding FDResult (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); rv = 1; @@ -651,14 +565,12 @@ 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) - { + for (int i = 0; i < cis->linkSet.list.count; ++i) { memcpy( facilities.coordination.chain.links[i], cis->linkSet.list.array[i]->buf, @@ -675,8 +587,7 @@ int facilities_request_chaininfo_set(void *responder, ChainInformationSet_t *cis uint8_t frep_oer[1024]; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 1024); - if (enc.encoded == -1) - { + if (enc.encoded == -1) { log_error("failed encoding FDResult (%s)", enc.failed_type->name); facilities_request_result_rejected(responder); rv = 1; From aa3e5d1b6f10d6fa2e19acd622a2f23d9254a369 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 14 Apr 2023 20:15:58 +0100 Subject: [PATCH 27/31] small fixes to evm.c --- src/evm.c | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/src/evm.c b/src/evm.c index fd60e25..613cd9a 100644 --- a/src/evm.c +++ b/src/evm.c @@ -157,7 +157,8 @@ int evrsrm_recv(EV_RSR_t *evrsr_request) { fi_oer[0] = 4; if (!evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage || !evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_reservationRequestMessage) { - return 1; + rv = 1; + goto cleanup; } int evrsr_response; if (evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage) @@ -235,15 +236,13 @@ static int evrsrm_pre_reservation_response(EV_RSR_t *evrsr_request, uint8_t *evr response.supportedPaymentTypes.buf[0] = 0x00; response.preReservationExpirationTime = 0; - - pre_reservation_id++; - 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); return 1; } *evrsrm_len = (enc.encoded + 7) / 8; + pre_reservation_id++; return 0; } From f7c787ba1e12c70df1587c0fc16763cf364123db Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 14 Apr 2023 20:17:28 +0100 Subject: [PATCH 28/31] removed unnecessary pass of the request to an internal funtion --- src/evm.c | 7 ++----- src/evm.h | 2 +- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/src/evm.c b/src/evm.c index 613cd9a..17b013b 100644 --- a/src/evm.c +++ b/src/evm.c @@ -162,7 +162,7 @@ int evrsrm_recv(EV_RSR_t *evrsr_request) { } int evrsr_response; if (evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage) - evrsr_response = evrsrm_pre_reservation_response(evrsr_request, bpr->data.buf, (uint32_t *)&bpr->data.size); + 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) { @@ -211,10 +211,7 @@ cleanup: return rv; } -static int evrsrm_pre_reservation_response(EV_RSR_t *evrsr_request, uint8_t *evrsrm_oer, uint32_t *evrsrm_len) { - if (!evrsr_request->messageBody.present == EV_RSR_MessageBody_PR_preReservationRequestMessage) - return 1; - +static int evrsrm_pre_reservation_response(uint8_t *evrsrm_oer, uint32_t *evrsrm_len) { EV_RSR_t *evrsr_response = calloc(1, sizeof(EV_RSR_t)); evrsr_response->header.protocolVersion = 1; evrsr_response->header.messageID = 1; diff --git a/src/evm.h b/src/evm.h index 414988a..3c43ac3 100644 --- a/src/evm.h +++ b/src/evm.h @@ -49,7 +49,7 @@ int evrsrm_recv(EV_RSR_t *evrsr_request); * @return The reservation response or NULL if the request isn't a Pre reservation request */ -static int evrsrm_pre_reservation_response(EV_RSR_t *evrsr_request, uint8_t *evrsrm_oer, uint32_t *evrsrm_len); +static int evrsrm_pre_reservation_response(uint8_t *evrsrm_oer, uint32_t *evrsrm_len); /** * Analyses a evm From a2a45cf1dac85e8667eef4fcd1fb6842ddd31210 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 14 Apr 2023 20:19:44 +0100 Subject: [PATCH 29/31] proper documentation of the evm functions --- src/evm.h | 21 +++++++++++++++++---- 1 file changed, 17 insertions(+), 4 deletions(-) diff --git a/src/evm.h b/src/evm.h index 3c43ac3..c58dac3 100644 --- a/src/evm.h +++ b/src/evm.h @@ -21,9 +21,15 @@ enum EVM_CHECK_R { /* * @brief Analyzes a received EVCSNM * - * @return A EVCSNM check code + * @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); /* @@ -44,7 +50,8 @@ int evrsrm_recv(EV_RSR_t *evrsr_request); /** * @brief Creates a pre reservation response. - * @param evrsr_request The reservation request + * @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 */ @@ -52,11 +59,17 @@ int evrsrm_recv(EV_RSR_t *evrsr_request); static int evrsrm_pre_reservation_response(uint8_t *evrsrm_oer, uint32_t *evrsrm_len); /** - * Analyses a evm - * @param evm The evm to be analyzed + * 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 { From 2d5b779ae419b82ec8f21b0176272a5fe74766ab Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Fri, 14 Apr 2023 20:23:14 +0100 Subject: [PATCH 30/31] fixed wrong port on evm --- src/evm.c | 4 ++-- src/facilities.c | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/src/evm.c b/src/evm.c index 17b013b..3b15db4 100644 --- a/src/evm.c +++ b/src/evm.c @@ -135,7 +135,7 @@ int evrsrm_recv(EV_RSR_t *evrsr_request) { bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->destinationPort = Port_cam; + bpr->destinationPort = Port_evrsr; bpr->gn.trafficClass = 2; @@ -198,7 +198,7 @@ int evrsrm_recv(EV_RSR_t *evrsr_request) { 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 cam failed"); + log_error("[ev] encoding FI for evrsrm failed"); rv = 1; goto cleanup; } diff --git a/src/facilities.c b/src/facilities.c index a2855ba..5196d48 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -290,7 +290,6 @@ static int transport_indication(void *responder, void **security_socket, uint8_t fwd = true; EV_RSR_t *evrsr_request = (EV_RSR_t *)its_msg; evrsrm_recv(evrsr_request); - } default: break; From 9dab57bc2af19d884729066495d3b085054e7325 Mon Sep 17 00:00:00 2001 From: gilteixeira Date: Sat, 15 Apr 2023 16:31:01 +0100 Subject: [PATCH 31/31] currectly freed the memory for the evrsrm --- src/evm.c | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/evm.c b/src/evm.c index 3b15db4..882c24a 100644 --- a/src/evm.c +++ b/src/evm.c @@ -205,13 +205,14 @@ int evrsrm_recv(EV_RSR_t *evrsr_request) { 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; @@ -236,11 +237,14 @@ static int evrsrm_pre_reservation_response(uint8_t *evrsrm_oer, uint32_t *evrsrm 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); - return 1; + rv = 1; + goto cleanup; } *evrsrm_len = (enc.encoded + 7) / 8; pre_reservation_id++; - return 0; +cleanup: + ASN_STRUCT_FREE(asn_DEF_EV_RSR, evrsr_response); + return rv; } int evcsnm_check(EvcsnPdu_t *evcsnm) {