TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

more evcsnm related changes

This commit is contained in:
gilteixeira 2023-02-21 17:27:30 +00:00
parent 05e920c688
commit 5f37056d2f
7 changed files with 746 additions and 1177 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
.vscode/settings.json vendored
View File

@ -3,6 +3,16 @@
"packet.h": "c", "packet.h": "c",
"managementrequest.h": "c", "managementrequest.h": "c",
"facilitiesindication.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"
} }
} }

2
src/CMakeLists.txt
View File

@ -10,6 +10,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
saem.c saem.c
tpm.c tpm.c
vcm.c vcm.c
evcsnm.c
) )
TARGET_LINK_LIBRARIES(it2s-itss-facilities TARGET_LINK_LIBRARIES(it2s-itss-facilities
@ -28,6 +29,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-saem -lit2s-asn-saem
-lit2s-asn-tpm -lit2s-asn-tpm
-lit2s-asn-vcm -lit2s-asn-vcm
-lit2s-asn-evcsnm
-lit2s-asn-verco -lit2s-asn-verco
-lit2s-tender -lit2s-tender
-lit2s-obd -lit2s-obd

3
src/config.c
View File

@ -394,6 +394,9 @@ int facilities_config() {
facilities.coordination.vcm_period_min = config->facilities.mcm.period_min; facilities.coordination.vcm_period_min = config->facilities.mcm.period_min;
facilities.coordination.vcm_period_max = config->facilities.mcm.period_max; facilities.coordination.vcm_period_max = config->facilities.mcm.period_max;
// EVCSNM
facilities.evcsnm_args.activate = config->facilities.evcsnm.activate;
// Replay // Replay
facilities.replay = config->networking.replay.activate; facilities.replay = config->networking.replay.activate;

555
src/evcsnm.c
View File

@ -21,71 +21,19 @@
#include <it2s-tender/recorder.h> #include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h> #include <it2s-tender/packet.h>
#include <it2s-obd.h> static UTF8String_t *create_empty_utf8_string()
#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 UTF8String_t *utf8_string = calloc(1, sizeof(UTF8String_t));
* utf8_string->buf = calloc(1, 1);
* byte | description utf8_string->size = 0;
* --------------------------------- utf8_string->buf[0] = '\0';
* 0 | SSP version control return utf8_string;
* 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) static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len)
{ {
int rv = 0; int rv = 0;
int shm_fd, shm_valid = 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)); EvcsnPdu_t *evcsnm = calloc(1, sizeof(EvcsnPdu_t));
evcsnm->header.protocolVersion = 2; 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(); 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 % 65536); asn_ulong2INTEGER(&evcsnm->evcsn.poiHeader.timeStamp, now);
evcsnm->evcsn.poiHeader.relayCapable = 0; evcsnm->evcsn.poiHeader.relayCapable = 0;
pthread_mutex_lock(&lightship->lock);
if (facilities.station_type != StationType_roadSideUnit) 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_lock();
itss_space_get(); itss_space_get();
uint16_t lat_conf = epv.space.latitude_conf; cs0->chargingStationLocation.latitude = epv.space.latitude;
uint16_t lon_conf = epv.space.longitude_conf; 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); itss_space_unlock(epv);
// Reference position ellipse // if (facilities.station_type == StationType_roadSideUnit)
//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); 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)
@ -141,429 +95,22 @@ static int mk_evcsnm(uint8_t *evcsnm_oer, uint32_t *evcsnm_len)
} }
*evcsnm_len = (enc.encoded + 7) / 8; *evcsnm_len = (enc.encoded + 7) / 8;
if (lightship->use_obd)
free(shared_message);
cleanup: cleanup:
ASN_STRUCT_FREE(asn_DEF_EvcsnPdu, evcsnm); ASN_STRUCT_FREE(asn_DEF_EvcsnPdu, evcsnm);
return rv; return rv;
} }
int lightship_init() int evcsnm_check(EvcsnPdu_t* evcsnm) {
{
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; 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; 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; return rv;
} }
void *evcsn_service()
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; int rv = 0;
@ -584,7 +131,7 @@ void *ca_service()
bpr->gn.packetTransportType = PacketTransportType_shb; bpr->gn.packetTransportType = PacketTransportType_shb;
bpr->destinationPort = Port_evcsnm; bpr->destinationPort = Port_cam;
bpr->gn.trafficClass = 2; bpr->gn.trafficClass = 2;
@ -597,7 +144,7 @@ void *ca_service()
FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t)); FacilitiesIndication_t *fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message; fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t *fmi = &fi->choice.message; FacilitiesMessageIndication_t *fmi = &fi->choice.message;
fmi->itsMessageType = ItsMessageType_evcsnm; fmi->itsMessageType = messageID_evcsn;
fmi->data.buf = malloc(512); fmi->data.buf = malloc(512);
uint8_t tr_oer[1024]; uint8_t tr_oer[1024];
@ -606,9 +153,8 @@ void *ca_service()
fi_oer[0] = 4; fi_oer[0] = 4;
while (!facilities.exit) while (!facilities.exit)
{ {
usleep(1000 * 50); usleep(1000 * 1000);
if (facilities.evcsnm_args.activate)
if (lightship_check() && facilities.lightship.active)
{ {
rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size); rv = mk_evcsnm(bpr->data.buf, (uint32_t *)&bpr->data.size);
if (rv) if (rv)
@ -618,11 +164,6 @@ void *ca_service()
memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size); memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size);
fmi->data.size = 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); uint32_t id = itss_id(bpr->data.buf, bpr->data.size);
bpr->id = id; bpr->id = id;
fmi->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"); itss_queue_send(facilities.tx_queue, fi_oer, enc_fdi.encoded + 1, ITSS_APPLICATIONS, id, "FI.message");
lightship_reset_timer();
// Logging // Logging
if (facilities.logging.dbms) if (facilities.logging.dbms)
{ {
pthread_mutex_lock(&facilities.id.lock); pthread_mutex_lock(&facilities.id.lock);
uint32_t station_id = facilities.id.station_id; uint32_t station_id = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock); 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) if (facilities.logging.recorder)

126
src/evcsnm.h
View File

@ -1,12 +1,12 @@
#ifndef FACILITIES_CAM_H #ifndef FACILITIES_EVCSNM_H
#define FACILITIES_CAM_H #define FACILITIES_EVCSNM_H
#include <stdint.h> #include <stdint.h>
#include <stdlib.h> #include <stdlib.h>
#include <pthread.h> #include <pthread.h>
#include <stdbool.h> #include <stdbool.h>
#include <it2s-asn/camv2/CAM.h> #include <it2s-asn/evcsnm/EvcsnPdu.h>
#include <it2s-asn/itss-transport/BTPPacketIndication.h> #include <it2s-asn/itss-transport/BTPPacketIndication.h>
#include <it2s-tender/epv.h> #include <it2s-tender/epv.h>
@ -14,117 +14,35 @@
#define POS_HISTORY_MAX_LEN 24 #define POS_HISTORY_MAX_LEN 24
#define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1 #define PATH_HISTORY_MAX_LEN POS_HISTORY_MAX_LEN-1
typedef enum CID_CAM { enum EVCSNM_CHECK_R {
CID_PROTECTED_ZONES, EVCSNM_OK,
CID_PUBLIC_TRANSPORT, EVCSNM_INVALID,
CID_SPECIAL_TRANSPORT, EVCSNM_BAD_PERMISSIONS
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) * @brief Analyzes a received EVCSNM
* *
* @return Always zero * @return A EVCSNM check code
*/ */
int lightship_init(); enum EVCSNM_CHECK_R check_evcsnm(BTPPacketIndication_t *bpi, EvcsnPdu_t *evcsnm, uint8_t *ssp, uint32_t ssp_len);
/*
* @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 * @brief Main CA service
* *
* @return NULL * @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 #endif

388
src/facilities.c
View File

@ -9,6 +9,7 @@
#include "saem.h" #include "saem.h"
#include "tpm.h" #include "tpm.h"
#include "vcm.h" #include "vcm.h"
#include "evcsnm.h"
#include <it2s-asn/itss-networking/NetworkingIndication.h> #include <it2s-asn/itss-networking/NetworkingIndication.h>
#include <it2s-asn/itss-transport/TransportRequest.h> #include <it2s-asn/itss-transport/TransportRequest.h>
@ -45,22 +46,24 @@
facilities_t facilities = {0}; 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; int rv = 0;
uint8_t code = 0; uint8_t code = 0;
bool handled_msg = false; bool handled_msg = false;
FacilitiesIndication_t *fi = NULL; FacilitiesIndication_t *fi = NULL;
SecurityRequest_t* sreq = NULL; SecurityRequest_t *sreq = NULL;
SecurityReply_t* srep = NULL; SecurityReply_t *srep = NULL;
TransportRequest_t* tr = NULL; TransportRequest_t *tr = NULL;
uint16_t buf_len = 2048; uint16_t buf_len = 2048;
uint8_t buf[2048]; uint8_t buf[2048];
TransportIndication_t* ti = calloc(1, sizeof(TransportIndication_t)); TransportIndication_t *ti = calloc(1, sizeof(TransportIndication_t));
asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_TransportIndication, (void**) &ti, msg, msg_len); asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_TransportIndication, (void **)&ti, msg, msg_len);
if (dec.code) { if (dec.code)
{
log_error("<- invalid TI received"); log_error("<- invalid TI received");
rv = 1; rv = 1;
code = 1; code = 1;
@ -70,7 +73,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
itss_0send(responder, &code, 1); itss_0send(responder, &code, 1);
switch (ti->present) { switch (ti->present)
{
case TransportIndication_PR_packet: case TransportIndication_PR_packet:
break; break;
case TransportIndication_PR_data: case TransportIndication_PR_data:
@ -82,7 +86,7 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
goto cleanup; goto cleanup;
} }
TransportPacketIndication_t* tpi = &ti->choice.packet; // TODO TransportPacketIndication_t *tpi = &ti->choice.packet; // TODO
// //
bool fwd = false; bool fwd = false;
@ -92,15 +96,17 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
asn_TYPE_descriptor_t *its_msg_descriptor = NULL; asn_TYPE_descriptor_t *its_msg_descriptor = NULL;
void *its_msg = NULL; void *its_msg = NULL;
int its_msg_type = 0; int its_msg_type = 0;
uint8_t* packet = NULL; uint8_t *packet = NULL;
uint16_t packet_len = 0; uint16_t packet_len = 0;
switch (tpi->present) { switch (tpi->present)
{
case TransportPacketIndication_PR_btp: case TransportPacketIndication_PR_btp:
id = tpi->choice.btp.id; id = tpi->choice.btp.id;
log_debug("<- TI.packet.btp | id:%08x size:%dB", (uint32_t) id, msg_len); log_debug("<- TI.packet.btp | id:%08x size:%dB", (uint32_t)id, msg_len);
// Parse message // Parse message
switch (tpi->choice.btp.destinationPort) { switch (tpi->choice.btp.destinationPort)
{
case Port_cam: case Port_cam:
its_msg_descriptor = &asn_DEF_CAM; its_msg_descriptor = &asn_DEF_CAM;
its_msg = calloc(1, sizeof(CAM_t)); its_msg = calloc(1, sizeof(CAM_t));
@ -158,6 +164,14 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
fwd = true; fwd = true;
break; 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: default:
log_debug("messsage with unhandled BTP port received (%lld), ignoring", tpi->choice.btp.destinationPort); log_debug("messsage with unhandled BTP port received (%lld), ignoring", tpi->choice.btp.destinationPort);
goto cleanup; goto cleanup;
@ -166,28 +180,32 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
packet = tpi->choice.btp.data.buf; packet = tpi->choice.btp.data.buf;
packet_len = tpi->choice.btp.data.size; packet_len = tpi->choice.btp.data.size;
dec = uper_decode_complete(NULL, its_msg_descriptor, (void**) &its_msg, tpi->choice.btp.data.buf, tpi->choice.btp.data.size); dec = uper_decode_complete(NULL, its_msg_descriptor, (void **)&its_msg, tpi->choice.btp.data.buf, tpi->choice.btp.data.size);
if (dec.code) { if (dec.code)
{
log_debug("<- invalid %s received", its_msg_descriptor->name); log_debug("<- invalid %s received", its_msg_descriptor->name);
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
// Get permisisons // Get permisisons
uint8_t* ssp = NULL; uint8_t *ssp = NULL;
uint16_t ssp_len; uint16_t ssp_len;
if (tpi->choice.btp.gn.securityPermissions) { if (tpi->choice.btp.gn.securityPermissions)
{
ssp = tpi->choice.btp.gn.securityPermissions->ssp.buf; ssp = tpi->choice.btp.gn.securityPermissions->ssp.buf;
ssp_len = tpi->choice.btp.gn.securityPermissions->ssp.size; ssp_len = tpi->choice.btp.gn.securityPermissions->ssp.size;
} }
// Get neighbour certificate ID // Get neighbour certificate ID
uint8_t* neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL; uint8_t *neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL;
// Manage message // Manage message
switch (tpi->choice.btp.destinationPort) { switch (tpi->choice.btp.destinationPort)
{
case Port_cam: case Port_cam:
switch (check_cam(&tpi->choice.btp, its_msg, ssp, ssp_len)) { switch (check_cam(&tpi->choice.btp, its_msg, ssp, ssp_len))
{
case CAM_OK: case CAM_OK:
fwd = true; fwd = true;
break; break;
@ -198,15 +216,15 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
} }
break; break;
case Port_denm: case Port_denm:;
;
#ifdef DEBUG #ifdef DEBUG
uint8_t* xml_denm = malloc(32768); uint8_t *xml_denm = malloc(32768);
asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_DENM, its_msg); asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &asn_DEF_DENM, its_msg);
log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm); log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded, xml_denm);
free(xml_denm); free(xml_denm);
#endif #endif
switch (event_manage(its_msg, &id, ssp, ssp_len)) { switch (event_manage(its_msg, &id, ssp, ssp_len))
{
case EVENT_NEW: case EVENT_NEW:
case EVENT_CANCELLATION: case EVENT_CANCELLATION:
case EVENT_NEGATION: case EVENT_NEGATION:
@ -223,7 +241,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
break; break;
case Port_ivim: case Port_ivim:
switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len)) { switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len))
{
case SERVICE_NEW: case SERVICE_NEW:
case SERVICE_CANCELLATION: case SERVICE_CANCELLATION:
case SERVICE_NEGATION: case SERVICE_NEGATION:
@ -241,7 +260,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
break; break;
case Port_saem: case Port_saem:
switch (saem_check(its_msg, neighbour_cert)) { switch (saem_check(its_msg, neighbour_cert))
{
case SAEM_NEW: case SAEM_NEW:
fwd = true; fwd = true;
break; break;
@ -251,19 +271,27 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
break; break;
case 7011: case 7011:
if (facilities.tolling.enabled) { if (facilities.tolling.enabled)
{
tpm_recv(its_msg, security_socket, neighbour_cert, NULL); tpm_recv(its_msg, security_socket, neighbour_cert, NULL);
fwd = true; fwd = true;
} }
break; break;
case 2043: case 2043:
if (facilities.coordination.active) { if (facilities.coordination.active)
{
vcm_check(its_msg); vcm_check(its_msg);
} }
fwd = true; fwd = true;
break; break;
case Port_poi:
if (facilities.evcsnm_args.activate)
{
evcsnm_check(its_msg);
}
fwd = true;
break;
default: default:
break; break;
} }
@ -282,10 +310,13 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
memcpy(sreq->choice.tlsRecv.data.buf, tpi->choice.tcp.data.buf, tpi->choice.tcp.data.size); memcpy(sreq->choice.tlsRecv.data.buf, tpi->choice.tcp.data.buf, tpi->choice.tcp.data.size);
pthread_mutex_lock(&facilities.tolling.lock); pthread_mutex_lock(&facilities.tolling.lock);
tlsc_t* tlsc = tolling_tlsc_get(tpi->choice.tcp.sourceAddress->buf, 7011); tlsc_t *tlsc = tolling_tlsc_get(tpi->choice.tcp.sourceAddress->buf, 7011);
if (tlsc) { if (tlsc)
{
id = tlsc->id; id = tlsc->id;
} else { }
else
{
tlsc = tolling_tlsc_new(tpi->choice.tcp.sourceAddress->buf, 7011); tlsc = tolling_tlsc_new(tpi->choice.tcp.sourceAddress->buf, 7011);
id = tlsc->id; id = tlsc->id;
} }
@ -295,30 +326,32 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
uint8_t b_tx[2048], b_rx[2048]; uint8_t b_tx[2048], b_rx[2048];
b_tx[0] = 4; b_tx[0] = 4;
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, b_tx+1, 2047); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, b_tx + 1, 2047);
log_debug("->[security] SecurityRequest.tlsRecv (%ldB)", enc.encoded+1); log_debug("->[security] SecurityRequest.tlsRecv (%ldB)", enc.encoded + 1);
itss_0send(*security_socket, b_tx, 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); int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000);
log_debug("<-[security] SecurityReply.tlsRecv (%dB)", rl); log_debug("<-[security] SecurityReply.tlsRecv (%dB)", rl);
if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, b_rx, rl).code) { if (oer_decode(NULL, &asn_DEF_SecurityReply, (void **)&srep, b_rx, rl).code)
{
log_error("SecurityReply.tlsRecv decode failure"); log_error("SecurityReply.tlsRecv decode failure");
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
if (srep->returnCode == SecurityReplyReturnCode_rejected) { if (srep->returnCode == SecurityReplyReturnCode_rejected)
{
log_error("SecurityReply.tlsRecv rejected"); log_error("SecurityReply.tlsRecv rejected");
SecurityRequest_t* sREQ = calloc(1, sizeof(SecurityRequest_t)); SecurityRequest_t *sREQ = calloc(1, sizeof(SecurityRequest_t));
sREQ->present = SecurityRequest_PR_tlsShutdown; sREQ->present = SecurityRequest_PR_tlsShutdown;
sREQ->choice.tlsShutdown.connId = id; sREQ->choice.tlsShutdown.connId = id;
b_tx[0] = 4; b_tx[0] = 4;
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sREQ, b_tx+1, 2047); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sREQ, b_tx + 1, 2047);
log_debug("->[security] SecurityRequest.tlsShutdown (%ldB)", enc.encoded+1); log_debug("->[security] SecurityRequest.tlsShutdown (%ldB)", enc.encoded + 1);
itss_0send(*security_socket, b_tx, 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); int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000);
log_debug("<-[security] SecurityReply.tlsShutdown (%dB)", rl); log_debug("<-[security] SecurityReply.tlsShutdown (%dB)", rl);
rv = 1; rv = 1;
@ -328,11 +361,12 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
log_debug("[tolling] tls n-msg:%d state:%d", tlsc->nmsg, tlsc->state); log_debug("[tolling] tls n-msg:%d state:%d", tlsc->nmsg, tlsc->state);
// Forward to [transport] // Forward to [transport]
if (srep->data->choice.tlsRecv.state != 1) { if (srep->data->choice.tlsRecv.state != 1)
{
tr = calloc(1, sizeof(TransportRequest_t)); tr = calloc(1, sizeof(TransportRequest_t));
tr->present = TransportRequest_PR_packet; tr->present = TransportRequest_PR_packet;
tr->choice.packet.present = TransportPacketRequest_PR_tcp; tr->choice.packet.present = TransportPacketRequest_PR_tcp;
TCPPacketRequest_t* tpr = &tr->choice.packet.choice.tcp; TCPPacketRequest_t *tpr = &tr->choice.packet.choice.tcp;
tpr->data.size = srep->data->choice.tlsRecv.data.size; tpr->data.size = srep->data->choice.tlsRecv.data.size;
tpr->data.buf = malloc(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); memcpy(tpr->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);
@ -348,7 +382,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
tpr->sourcePort = 7011; tpr->sourcePort = 7011;
if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN || if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN ||
(facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS && tlsc->nmsg < 2)) { (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS && tlsc->nmsg < 2))
{
tpr->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t)); tpr->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
tpr->gn->packetTransportType = PacketTransportType_shb; tpr->gn->packetTransportType = PacketTransportType_shb;
tpr->gn->destinationAddress.buf = calloc(1, 6); tpr->gn->destinationAddress.buf = calloc(1, 6);
@ -358,30 +393,36 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
tpr->id = itss_id(tpr->data.buf, tpr->data.size); tpr->id = itss_id(tpr->data.buf, tpr->data.size);
buf[0] = 4; buf[0] = 4;
enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, buf_len-1); enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf + 1, buf_len - 1);
if (enc.encoded == -1) { if (enc.encoded == -1)
{
log_error("TransportRequest encoding fail"); log_error("TransportRequest encoding fail");
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
itss_queue_send(facilities.tx_queue, buf, enc.encoded+1, ITSS_TRANSPORT, tpr->id, "TR.packet.tcp"); itss_queue_send(facilities.tx_queue, buf, enc.encoded + 1, ITSS_TRANSPORT, tpr->id, "TR.packet.tcp");
} else { }
else
{
if (facilities.tolling.enabled && srep->data->choice.tlsRecv.data.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) { 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"); log_debug("<- invalid TPM received");
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
if (!dec.code) { if (!dec.code)
{
tpm_recv(its_msg, security_socket, NULL, tpi->choice.tcp.sourceAddress->buf); tpm_recv(its_msg, security_socket, NULL, tpi->choice.tcp.sourceAddress->buf);
// Fwd to [applications] // Fwd to [applications]
fi = calloc(1, sizeof(FacilitiesIndication_t)); fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message; fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t* fmi = &fi->choice.message; FacilitiesMessageIndication_t *fmi = &fi->choice.message;
fmi->id = id; fmi->id = id;
@ -389,13 +430,13 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
fmi->data.size = srep->data->choice.tlsRecv.data.size; fmi->data.size = srep->data->choice.tlsRecv.data.size;
fmi->data.buf = malloc(srep->data->choice.tlsRecv.data.size); 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); memcpy(fmi->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);
uint8_t buffer[ITSS_SDU_MAX_LEN]; 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); 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");
} }
} }
} }
@ -410,10 +451,11 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
} }
// Forward to [applications] // Forward to [applications]
if (fwd) { if (fwd)
{
fi = calloc(1, sizeof(FacilitiesIndication_t)); fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message; fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t* fmi = &fi->choice.message; FacilitiesMessageIndication_t *fmi = &fi->choice.message;
fmi->id = id; fmi->id = id;
@ -425,19 +467,21 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
uint8_t buffer[ITSS_SDU_MAX_LEN]; 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); asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1);
itss_queue_send(facilities.tx_queue, buffer, enc.encoded+1, ITSS_APPLICATIONS, id, "FI.message"); itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, id, "FI.message");
} }
// Logging // Logging
if (facilities.logging.dbms) { if (facilities.logging.dbms)
{
pthread_mutex_lock(&facilities.id.lock); pthread_mutex_lock(&facilities.id.lock);
uint32_t station_id = facilities.id.station_id; uint32_t station_id = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock); pthread_mutex_unlock(&facilities.id.lock);
itss_db_add(facilities.logging.dbms, station_id, id, false, its_msg_type, NULL, packet, packet_len); 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( int e = itss_management_record_packet_sdu(
buf, buf,
buf_len, buf_len,
@ -447,13 +491,15 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
itss_time_get(), itss_time_get(),
ITSS_FACILITIES, ITSS_FACILITIES,
false); false);
if (e != -1) { if (e != -1)
{
itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, tpi->choice.btp.id, "MReq.packet.set"); itss_queue_send(facilities.tx_queue, buf, e, ITSS_MANAGEMENT, tpi->choice.btp.id, "MReq.packet.set");
} }
} }
cleanup: 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(*its_msg_descriptor, its_msg);
} }
ASN_STRUCT_FREE(asn_DEF_TransportIndication, ti); ASN_STRUCT_FREE(asn_DEF_TransportIndication, ti);
@ -465,26 +511,30 @@ cleanup:
return rv; 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; int rv = 0;
FacilitiesRequest_t *fr = calloc(1, sizeof(FacilitiesRequest_t)); FacilitiesRequest_t *fr = calloc(1, sizeof(FacilitiesRequest_t));
asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_FacilitiesRequest, (void**) &fr, msg, msg_len); asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_FacilitiesRequest, (void **)&fr, msg, msg_len);
if (dec.code) { if (dec.code)
{
log_error("<- invalid FR received"); log_error("<- invalid FR received");
facilities_request_result_rejected(responder); facilities_request_result_rejected(responder);
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
switch (fr->present) { switch (fr->present)
{
case FacilitiesRequest_PR_message: case FacilitiesRequest_PR_message:
rv = facilities_request_single_message(responder, &fr->choice.message); rv = facilities_request_single_message(responder, &fr->choice.message);
break; break;
case FacilitiesRequest_PR_data: case FacilitiesRequest_PR_data:
switch (fr->choice.data.present) { switch (fr->choice.data.present)
{
case FacilitiesDataRequest_PR_activeEpisodes: case FacilitiesDataRequest_PR_activeEpisodes:
rv = facilities_request_active_episodes(responder, fr); rv = facilities_request_active_episodes(responder, fr);
break; break;
@ -522,26 +572,28 @@ cleanup:
return rv; 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; int rv = 0;
SecurityIndication_t* si = calloc(1, sizeof(SecurityIndication_t)); SecurityIndication_t *si = calloc(1, sizeof(SecurityIndication_t));
SecurityResponse_t* sr = calloc(1, sizeof(SecurityResponse_t)); SecurityResponse_t *sr = calloc(1, sizeof(SecurityResponse_t));
uint8_t buffer[64]; uint8_t buffer[64];
asn_enc_rval_t enc; asn_enc_rval_t enc;
asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityIndication, (void**) &si, msg, msg_len); asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityIndication, (void **)&si, msg, msg_len);
if (dec.code) { if (dec.code)
{
log_error("<- invalid SIndication received"); log_error("<- invalid SIndication received");
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
pthread_mutex_lock(&facilities.id.change.lock); 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); pthread_mutex_unlock(&facilities.id.change.lock);
log_debug("identity change is currently blocked"); log_debug("identity change is currently blocked");
rv = 1; rv = 1;
@ -550,7 +602,8 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
if (facilities.id.change.stage == ID_CHANGE_PREPARE && if (facilities.id.change.stage == ID_CHANGE_PREPARE &&
si->choice.idChangeEvent.command != SecurityIdChangeEventType_commit && 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); 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"); log_debug("current identity change state is prepare, but received identity change command is not commit nor abort");
rv = 1; rv = 1;
@ -559,7 +612,8 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
bool id_changed = false; bool id_changed = false;
switch (si->choice.idChangeEvent.command) { switch (si->choice.idChangeEvent.command)
{
case SecurityIdChangeEventType_prepare: case SecurityIdChangeEventType_prepare:
facilities.id.change.stage = ID_CHANGE_PREPARE; facilities.id.change.stage = ID_CHANGE_PREPARE;
@ -568,12 +622,12 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
pthread_mutex_lock(&facilities.lightship.lock); pthread_mutex_lock(&facilities.lightship.lock);
break; break;
case SecurityIdChangeEventType_commit: case SecurityIdChangeEventType_commit:;
;
facilities.id.change.stage = ID_CHANGE_COMMIT; facilities.id.change.stage = ID_CHANGE_COMMIT;
// Reset lightship // Reset lightship
for (int i = 0; i < facilities.lightship.path_history_len; ++i) { for (int i = 0; i < facilities.lightship.path_history_len; ++i)
{
free(facilities.lightship.path_history[i]); free(facilities.lightship.path_history[i]);
} }
facilities.lightship.path_history_len = 0; facilities.lightship.path_history_len = 0;
@ -586,8 +640,10 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
pthread_mutex_unlock(&facilities.lightship.lock); pthread_mutex_unlock(&facilities.lightship.lock);
// Change Station ID // Change Station ID
for (int i = 0; i < si->choice.idChangeEvent.ids.list.count; ++i) { for (int i = 0; i < si->choice.idChangeEvent.ids.list.count; ++i)
switch (si->choice.idChangeEvent.ids.list.array[i]->present) { {
switch (si->choice.idChangeEvent.ids.list.array[i]->present)
{
case SecurityId_PR_stationId: case SecurityId_PR_stationId:
facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId; facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId;
break; break;
@ -619,7 +675,6 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
log_error("[networking]<- unhandled idChangeEvent command type"); log_error("[networking]<- unhandled idChangeEvent command type");
rv = 1; rv = 1;
goto cleanup; goto cleanup;
} }
sr->present = SecurityResponse_PR_idChangeEvent; 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); pthread_mutex_unlock(&facilities.id.change.lock);
cleanup: cleanup:
if (rv) { if (rv)
{
sr->present = SecurityResponse_PR_idChangeEvent; sr->present = SecurityResponse_PR_idChangeEvent;
sr->choice.idChangeEvent.returnCode = 1; sr->choice.idChangeEvent.returnCode = 1;
enc = oer_encode_to_buffer(&asn_DEF_SecurityResponse, NULL, sr, buffer, 64); enc = oer_encode_to_buffer(&asn_DEF_SecurityResponse, NULL, sr, buffer, 64);
@ -644,15 +700,17 @@ cleanup:
return rv; 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; int rv = 0;
uint8_t code = 0; uint8_t code = 0;
NetworkingIndication_t* ni = calloc(1, sizeof(NetworkingIndication_t)); NetworkingIndication_t *ni = calloc(1, sizeof(NetworkingIndication_t));
asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_NetworkingIndication, (void**) &ni, msg, msg_len); asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_NetworkingIndication, (void **)&ni, msg, msg_len);
if (dec.code) { if (dec.code)
{
rv = 1; rv = 1;
code = 1; code = 1;
itss_0send(responder, &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); itss_0send(responder, &code, 1);
if (ni->present != NetworkingIndication_PR_data) { if (ni->present != NetworkingIndication_PR_data)
{
goto cleanup; goto cleanup;
} }
if (ni->choice.data.mobileNeighbour && if (ni->choice.data.mobileNeighbour &&
*ni->choice.data.mobileNeighbour) { *ni->choice.data.mobileNeighbour)
{
pthread_mutex_lock(&facilities.lightship.lock); pthread_mutex_lock(&facilities.lightship.lock);
facilities.lightship.t_last_vehicle = itss_time_get(); 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); pthread_mutex_unlock(&facilities.lightship.lock);
} }
cleanup: cleanup:
ASN_STRUCT_FREE(asn_DEF_NetworkingIndication, ni); ASN_STRUCT_FREE(asn_DEF_NetworkingIndication, ni);
return rv; 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; int rv = 0;
uint8_t code = 0; uint8_t code = 0;
ManagementIndication_t* mi = calloc(1, sizeof(ManagementIndication_t)); ManagementIndication_t *mi = calloc(1, sizeof(ManagementIndication_t));
asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_ManagementIndication, (void**) &mi, msg, msg_len); asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_ManagementIndication, (void **)&mi, msg, msg_len);
if (dec.code) { if (dec.code)
{
rv = 1; rv = 1;
code = 1; code = 1;
itss_0send(responder, &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); itss_0send(responder, &code, 1);
if (mi->present == ManagementIndication_PR_attributes) { if (mi->present == ManagementIndication_PR_attributes)
{
itss_space_lock(); itss_space_lock();
epv.space.latitude = mi->choice.attributes.coordinates.latitude; epv.space.latitude = mi->choice.attributes.coordinates.latitude;
epv.space.latitude_conf = mi->choice.attributes.coordinates.latitudeConfidence; 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_space_unlock();
itss_trajectory_lock(); itss_trajectory_lock();
if (mi->choice.attributes.trajectory) { if (mi->choice.attributes.trajectory)
{
epv.trajectory.len = mi->choice.attributes.trajectory->list.count; 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].latitude = mi->choice.attributes.trajectory->list.array[i]->latitude;
epv.trajectory.path[i].longitude = mi->choice.attributes.trajectory->list.array[i]->longitude; 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_trajectory_unlock();
itss_time_lock(); 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(); itss_time_unlock();
} }
@ -734,25 +797,28 @@ cleanup:
return rv; return rv;
} }
void *tx()
void* tx() { {
int rv = 0; int rv = 0;
itss_queue_t* queue = facilities.tx_queue; itss_queue_t *queue = facilities.tx_queue;
uint8_t code; uint8_t code;
void* applications_socket = itss_0connect(facilities.zmq.applications_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 *transport_socket = itss_0connect(facilities.zmq.transport_address, ZMQ_REQ);
void* management_socket = itss_0connect(facilities.zmq.management_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); pthread_mutex_lock(&queue->lock);
while (!queue->len && !facilities.exit) { while (!queue->len && !facilities.exit)
{
pthread_cond_wait(&queue->trigger, &queue->lock); 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]); memcpy(stream->packet[i], queue->packet[i], queue->packet_len[i]);
stream->packet_len[i] = queue->packet_len[i]; stream->packet_len[i] = queue->packet_len[i];
stream->destination[i] = queue->destination[i]; stream->destination[i] = queue->destination[i];
@ -764,34 +830,39 @@ void* tx() {
queue->len = 0; queue->len = 0;
pthread_mutex_unlock(&queue->lock); pthread_mutex_unlock(&queue->lock);
for (int i = 0; i < stream->len; ++i) { for (int i = 0; i < stream->len; ++i)
switch (stream->destination[i]) { {
switch (stream->destination[i])
{
case ITSS_TRANSPORT: case ITSS_TRANSPORT:
log_debug("-> %s ->[transport] | id:%08x size:%dB", log_debug("-> %s ->[transport] | id:%08x size:%dB",
stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
itss_0send(transport_socket, stream->packet[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); rv = itss_0recv_rt(&transport_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000);
if (rv == -1) { if (rv == -1)
{
log_error("-> %s ->[transport] | id:%08x size:%dB <TIMEOUT>", log_error("-> %s ->[transport] | id:%08x size:%dB <TIMEOUT>",
stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
} }
break; break;
case ITSS_APPLICATIONS: case ITSS_APPLICATIONS:
log_debug("-> %s ->[applications] | id:%08x size:%dB", log_debug("-> %s ->[applications] | id:%08x size:%dB",
stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
itss_0send(applications_socket, stream->packet[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); rv = itss_0recv_rt(&applications_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000);
if (rv == -1) { if (rv == -1)
{
log_error("-> %s ->[applications] | id:%08x size:%dB <TIMEOUT>", log_error("-> %s ->[applications] | id:%08x size:%dB <TIMEOUT>",
stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
} }
break; break;
case ITSS_MANAGEMENT: case ITSS_MANAGEMENT:
itss_0send(management_socket, stream->packet[i], stream->packet_len[i]); 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); rv = itss_0recv_rt(&management_socket, &code, 1, stream->packet[i], stream->packet_len[i], 1000);
if (rv == -1) { if (rv == -1)
{
log_error("-> %s ->[management] | id:%08x size:%dB <TIMEOUT>", log_error("-> %s ->[management] | id:%08x size:%dB <TIMEOUT>",
stream->info_msg[i], (uint32_t) stream->id[i], stream->packet_len[i]); stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
} }
break; break;
} }
@ -805,29 +876,31 @@ void* tx() {
return NULL; return NULL;
} }
static void sigh(int signum) { static void sigh(int signum)
{
facilities.exit = true; facilities.exit = true;
uint8_t code = 0; 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_0send(socket, &code, sizeof(code));
itss_0close(socket); itss_0close(socket);
} }
int main() { int main()
{
signal(SIGTERM, sigh); signal(SIGTERM, sigh);
signal(SIGINT, sigh); signal(SIGINT, sigh);
signal(SIGKILL, sigh); signal(SIGKILL, sigh);
facilities.tx_queue = itss_queue_new(); facilities.tx_queue = itss_queue_new();
lightship_init(); lightship_init();
den_init(); den_init();
infrastructure_init(); infrastructure_init();
dissemination_init(); dissemination_init();
bulletin_init(); bulletin_init();
void* security_socket = NULL; void *security_socket = NULL;
if (facilities_config()) { if (facilities_config())
{
goto cleanup; goto cleanup;
} }
@ -856,11 +929,16 @@ int main() {
tolling_init(facilities.station_type); tolling_init(facilities.station_type);
// VC // VC
if (facilities.coordination.active) { if (facilities.coordination.active)
{
coordination_init(); coordination_init();
pthread_create(&facilities.vc_service, NULL, vc_service, NULL); 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); security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);
uint8_t buffer[ITSS_SDU_MAX_LEN]; uint8_t buffer[ITSS_SDU_MAX_LEN];
@ -870,37 +948,45 @@ int main() {
int32_t rl; int32_t rl;
while (!facilities.exit) { while (!facilities.exit)
{
itss_0poll(facilities.zmq.responders, facilities.zmq.n_responders, -1); itss_0poll(facilities.zmq.responders, facilities.zmq.n_responders, -1);
for (int i = 0; i < facilities.zmq.n_responders; ++i) { for (int i = 0; i < facilities.zmq.n_responders; ++i)
if (facilities.zmq.responders[i].revents) { {
if (facilities.zmq.responders[i].revents)
{
rl = itss_0recv(facilities.zmq.responders[i].socket, buffer, ITSS_SDU_MAX_LEN); rl = itss_0recv(facilities.zmq.responders[i].socket, buffer, ITSS_SDU_MAX_LEN);
switch (buffer[0]) { /* source */ switch (buffer[0])
{ /* source */
case ITSS_INTERNAL: case ITSS_INTERNAL:
break; break;
case ITSS_NETWORKING: case ITSS_NETWORKING:
networking_indication(facilities.zmq.responders[i].socket, buffer+1, rl); networking_indication(facilities.zmq.responders[i].socket, buffer + 1, rl);
break; break;
case ITSS_TRANSPORT: case ITSS_TRANSPORT:
in_idchange = true; in_idchange = true;
pthread_mutex_lock(&facilities.id.change.lock); pthread_mutex_lock(&facilities.id.change.lock);
if (facilities.id.change.stage == ID_CHANGE_INACTIVE) { if (facilities.id.change.stage == ID_CHANGE_INACTIVE)
{
in_idchange = false; in_idchange = false;
facilities.id.change.stage = ID_CHANGE_BLOCKED; facilities.id.change.stage = ID_CHANGE_BLOCKED;
} }
pthread_mutex_unlock(&facilities.id.change.lock); pthread_mutex_unlock(&facilities.id.change.lock);
if (!in_idchange) { if (!in_idchange)
transport_indication(facilities.zmq.responders[i].socket, &security_socket, buffer+1, rl); {
transport_indication(facilities.zmq.responders[i].socket, &security_socket, buffer + 1, rl);
pthread_mutex_lock(&facilities.id.change.lock); pthread_mutex_lock(&facilities.id.change.lock);
facilities.id.change.stage = ID_CHANGE_INACTIVE; facilities.id.change.stage = ID_CHANGE_INACTIVE;
pthread_mutex_unlock(&facilities.id.change.lock); pthread_mutex_unlock(&facilities.id.change.lock);
} else { }
else
{
code = 1; code = 1;
itss_0send(facilities.zmq.responders[i].socket, &code, 1); itss_0send(facilities.zmq.responders[i].socket, &code, 1);
} }
@ -909,19 +995,23 @@ int main() {
case ITSS_APPLICATIONS: case ITSS_APPLICATIONS:
in_idchange = true; in_idchange = true;
pthread_mutex_lock(&facilities.id.change.lock); pthread_mutex_lock(&facilities.id.change.lock);
if (facilities.id.change.stage == ID_CHANGE_INACTIVE) { if (facilities.id.change.stage == ID_CHANGE_INACTIVE)
{
in_idchange = false; in_idchange = false;
facilities.id.change.stage = ID_CHANGE_BLOCKED; facilities.id.change.stage = ID_CHANGE_BLOCKED;
} }
pthread_mutex_unlock(&facilities.id.change.lock); pthread_mutex_unlock(&facilities.id.change.lock);
if (!in_idchange) { if (!in_idchange)
facilities_request(facilities.zmq.responders[i].socket, buffer+1, rl); {
facilities_request(facilities.zmq.responders[i].socket, buffer + 1, rl);
pthread_mutex_lock(&facilities.id.change.lock); pthread_mutex_lock(&facilities.id.change.lock);
facilities.id.change.stage = ID_CHANGE_INACTIVE; facilities.id.change.stage = ID_CHANGE_INACTIVE;
pthread_mutex_unlock(&facilities.id.change.lock); pthread_mutex_unlock(&facilities.id.change.lock);
} else { }
else
{
code = 1; code = 1;
itss_0send(facilities.zmq.responders[i].socket, &code, 1); itss_0send(facilities.zmq.responders[i].socket, &code, 1);
} }
@ -929,11 +1019,11 @@ int main() {
break; break;
case ITSS_MANAGEMENT: case ITSS_MANAGEMENT:
management_indication(facilities.zmq.responders[i].socket, buffer+1, rl); management_indication(facilities.zmq.responders[i].socket, buffer + 1, rl);
break; break;
case ITSS_SECURITY: case ITSS_SECURITY:
security_indication(facilities.zmq.responders[i].socket, buffer+1, rl); security_indication(facilities.zmq.responders[i].socket, buffer + 1, rl);
break; break;
default: default:
@ -948,6 +1038,7 @@ int main() {
// Exit // Exit
cleanup: cleanup:
pthread_join(facilities.evcsn_service, NULL);
pthread_join(facilities.ca_service, NULL); pthread_join(facilities.ca_service, NULL);
pthread_join(facilities.den_service, NULL); pthread_join(facilities.den_service, NULL);
pthread_join(facilities.infrastructure_service, NULL); pthread_join(facilities.infrastructure_service, NULL);
@ -960,7 +1051,8 @@ cleanup:
pthread_join(facilities.vc_service, NULL); pthread_join(facilities.vc_service, NULL);
itss_0close(security_socket); 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_0close(facilities.zmq.responders[i].socket);
} }
itss_0destroy(); itss_0destroy();

5
src/facilities.h
View File

@ -12,6 +12,7 @@
#include "saem.h" #include "saem.h"
#include "tpm.h" #include "tpm.h"
#include "vcm.h" #include "vcm.h"
#include "evcsnm.h"
#include <it2s-tender/epv.h> #include <it2s-tender/epv.h>
#include <it2s-tender/database.h> #include <it2s-tender/database.h>
@ -34,6 +35,7 @@ typedef struct facilities {
pthread_t cp_service; pthread_t cp_service;
pthread_t sa_service; pthread_t sa_service;
pthread_t vc_service; pthread_t vc_service;
pthread_t evcsn_service;
// ZMQ // ZMQ
struct { struct {
@ -70,6 +72,9 @@ typedef struct facilities {
// PC // PC
coordination_t coordination; coordination_t coordination;
// EVCSN
evcsnm_args_t evcsnm_args;
// Logging // Logging
struct { struct {
bool recorder; bool recorder;