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",
"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"
}
}

2
src/CMakeLists.txt
View File

@ -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

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_max = config->facilities.mcm.period_max;
// EVCSNM
facilities.evcsnm_args.activate = config->facilities.evcsnm.activate;
// Replay
facilities.replay = config->networking.replay.activate;

553
src/evcsnm.c
View File

@ -21,71 +21,19 @@
#include <it2s-tender/recorder.h>
#include <it2s-tender/packet.h>
#include <it2s-obd.h>
#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)
{
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();
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
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)
// if (facilities.station_type == StationType_roadSideUnit)
//{
// 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)
@ -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)

126
src/evcsnm.h
View File

@ -1,12 +1,12 @@
#ifndef FACILITIES_CAM_H
#define FACILITIES_CAM_H
#ifndef FACILITIES_EVCSNM_H
#define FACILITIES_EVCSNM_H
#include <stdint.h>
#include <stdlib.h>
#include <pthread.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-tender/epv.h>
@ -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

266
src/facilities.c
View File

@ -9,6 +9,7 @@
#include "saem.h"
#include "tpm.h"
#include "vcm.h"
#include "evcsnm.h"
#include <it2s-asn/itss-networking/NetworkingIndication.h>
#include <it2s-asn/itss-transport/TransportRequest.h>
@ -45,7 +46,8 @@
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;
@ -60,7 +62,8 @@ 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;
@ -70,7 +73,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
itss_0send(responder, &code, 1);
switch (ti->present) {
switch (ti->present)
{
case TransportIndication_PR_packet:
break;
case TransportIndication_PR_data:
@ -95,12 +99,14 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
uint8_t *packet = NULL;
uint16_t packet_len = 0;
switch (tpi->present) {
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) {
switch (tpi->choice.btp.destinationPort)
{
case Port_cam:
its_msg_descriptor = &asn_DEF_CAM;
its_msg = calloc(1, sizeof(CAM_t));
@ -158,6 +164,14 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
fwd = true;
break;
case Port_poi: /* EVCSNM */
its_msg_descriptor = &asn_DEF_EvcsnPdu;
its_msg = calloc(1, sizeof(EvcsnPdu_t));
its_msg_type = messageID_evcsn;
handled_msg = true;
fwd = true;
break;
default:
log_debug("messsage with unhandled BTP port received (%lld), ignoring", tpi->choice.btp.destinationPort);
goto cleanup;
@ -167,7 +181,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
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) {
if (dec.code)
{
log_debug("<- invalid %s received", its_msg_descriptor->name);
rv = 1;
goto cleanup;
@ -176,7 +191,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
// Get permisisons
uint8_t *ssp = NULL;
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_len = tpi->choice.btp.gn.securityPermissions->ssp.size;
}
@ -185,9 +201,11 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
uint8_t *neighbour_cert = tpi->choice.btp.gn.securityNeighbour ? tpi->choice.btp.gn.securityNeighbour->buf : NULL;
// Manage message
switch (tpi->choice.btp.destinationPort) {
switch (tpi->choice.btp.destinationPort)
{
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:
fwd = true;
break;
@ -198,15 +216,15 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
}
break;
case Port_denm:
;
case Port_denm:;
#ifdef DEBUG
uint8_t *xml_denm = malloc(32768);
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)) {
switch (event_manage(its_msg, &id, ssp, ssp_len))
{
case EVENT_NEW:
case EVENT_CANCELLATION:
case EVENT_NEGATION:
@ -223,7 +241,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
break;
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_CANCELLATION:
case SERVICE_NEGATION:
@ -241,7 +260,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
break;
case Port_saem:
switch (saem_check(its_msg, neighbour_cert)) {
switch (saem_check(its_msg, neighbour_cert))
{
case SAEM_NEW:
fwd = true;
break;
@ -251,19 +271,27 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
break;
case 7011:
if (facilities.tolling.enabled) {
if (facilities.tolling.enabled)
{
tpm_recv(its_msg, security_socket, neighbour_cert, NULL);
fwd = true;
}
break;
case 2043:
if (facilities.coordination.active) {
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;
}
@ -283,9 +311,12 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
pthread_mutex_lock(&facilities.tolling.lock);
tlsc_t *tlsc = tolling_tlsc_get(tpi->choice.tcp.sourceAddress->buf, 7011);
if (tlsc) {
if (tlsc)
{
id = tlsc->id;
} else {
}
else
{
tlsc = tolling_tlsc_new(tpi->choice.tcp.sourceAddress->buf, 7011);
id = tlsc->id;
}
@ -302,13 +333,15 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
int32_t rl = itss_0recv_rt(security_socket, b_rx, 2048, b_tx, enc.encoded + 1, 1000);
log_debug("<-[security] SecurityReply.tlsRecv (%dB)", rl);
if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, b_rx, rl).code) {
if (oer_decode(NULL, &asn_DEF_SecurityReply, (void **)&srep, b_rx, rl).code)
{
log_error("SecurityReply.tlsRecv decode failure");
rv = 1;
goto cleanup;
}
if (srep->returnCode == SecurityReplyReturnCode_rejected) {
if (srep->returnCode == SecurityReplyReturnCode_rejected)
{
log_error("SecurityReply.tlsRecv rejected");
SecurityRequest_t *sREQ = calloc(1, sizeof(SecurityRequest_t));
@ -328,7 +361,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
log_debug("[tolling] tls n-msg:%d state:%d", tlsc->nmsg, tlsc->state);
// Forward to [transport]
if (srep->data->choice.tlsRecv.state != 1) {
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;
@ -348,7 +382,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
tpr->sourcePort = 7011;
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->packetTransportType = PacketTransportType_shb;
tpr->gn->destinationAddress.buf = calloc(1, 6);
@ -359,23 +394,29 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
buf[0] = 4;
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");
rv = 1;
goto cleanup;
}
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) {
if (dec.code)
{
log_debug("<- invalid TPM received");
rv = 1;
goto cleanup;
}
if (!dec.code) {
if (!dec.code)
{
tpm_recv(its_msg, security_socket, NULL, tpi->choice.tcp.sourceAddress->buf);
// Fwd to [applications]
@ -410,7 +451,8 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
}
// Forward to [applications]
if (fwd) {
if (fwd)
{
fi = calloc(1, sizeof(FacilitiesIndication_t));
fi->present = FacilitiesIndication_PR_message;
FacilitiesMessageIndication_t *fmi = &fi->choice.message;
@ -431,13 +473,15 @@ static int transport_indication(void* responder, void** security_socket, uint8_t
}
// 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,
@ -447,13 +491,15 @@ 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);
@ -465,26 +511,30 @@ 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) {
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) {
switch (fr->choice.data.present)
{
case FacilitiesDataRequest_PR_activeEpisodes:
rv = facilities_request_active_episodes(responder, fr);
break;
@ -522,8 +572,8 @@ 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));
@ -534,14 +584,16 @@ 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;
@ -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 &&
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;
@ -559,7 +612,8 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
bool id_changed = false;
switch (si->choice.idChangeEvent.command) {
switch (si->choice.idChangeEvent.command)
{
case SecurityIdChangeEventType_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);
break;
case SecurityIdChangeEventType_commit:
;
case SecurityIdChangeEventType_commit:;
facilities.id.change.stage = ID_CHANGE_COMMIT;
// 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]);
}
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);
// Change Station ID
for (int i = 0; i < si->choice.idChangeEvent.ids.list.count; ++i) {
switch (si->choice.idChangeEvent.ids.list.array[i]->present) {
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;
@ -619,7 +675,6 @@ static int security_indication(void* responder_secured, uint8_t *msg, uint32_t m
log_error("[networking]<- unhandled idChangeEvent command type");
rv = 1;
goto cleanup;
}
sr->present = SecurityResponse_PR_idChangeEvent;
@ -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,7 +700,8 @@ 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;
@ -652,7 +709,8 @@ 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);
@ -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) {
*ni->choice.data.mobileNeighbour)
{
pthread_mutex_lock(&facilities.lightship.lock);
facilities.lightship.t_last_vehicle = itss_time_get();
@ -673,15 +733,14 @@ 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;
@ -689,7 +748,8 @@ 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);
@ -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,9 +774,11 @@ 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);
@ -734,8 +797,8 @@ cleanup:
return rv;
}
void* tx() {
void *tx()
{
int rv = 0;
itss_queue_t *queue = facilities.tx_queue;
@ -747,12 +810,15 @@ 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];
@ -764,14 +830,17 @@ void* tx() {
queue->len = 0;
pthread_mutex_unlock(&queue->lock);
for (int i = 0; i < stream->len; ++i) {
switch (stream->destination[i]) {
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) {
if (rv == -1)
{
log_error("-> %s ->[transport] | id:%08x size:%dB <TIMEOUT>",
stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
}
@ -781,7 +850,8 @@ void* tx() {
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) {
if (rv == -1)
{
log_error("-> %s ->[applications] | id:%08x size:%dB <TIMEOUT>",
stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
}
@ -789,7 +859,8 @@ void* tx() {
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) {
if (rv == -1)
{
log_error("-> %s ->[management] | id:%08x size:%dB <TIMEOUT>",
stream->info_msg[i], (uint32_t)stream->id[i], stream->packet_len[i]);
}
@ -805,7 +876,8 @@ 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);
@ -813,12 +885,12 @@ static void sigh(int signum) {
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();
@ -827,7 +899,8 @@ int main() {
bulletin_init();
void *security_socket = NULL;
if (facilities_config()) {
if (facilities_config())
{
goto cleanup;
}
@ -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,13 +948,17 @@ 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;
@ -888,19 +970,23 @@ int main() {
case ITSS_TRANSPORT:
in_idchange = true;
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;
facilities.id.change.stage = ID_CHANGE_BLOCKED;
}
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);
pthread_mutex_lock(&facilities.id.change.lock);
facilities.id.change.stage = ID_CHANGE_INACTIVE;
pthread_mutex_unlock(&facilities.id.change.lock);
} else {
}
else
{
code = 1;
itss_0send(facilities.zmq.responders[i].socket, &code, 1);
}
@ -909,19 +995,23 @@ int main() {
case ITSS_APPLICATIONS:
in_idchange = true;
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;
facilities.id.change.stage = ID_CHANGE_BLOCKED;
}
pthread_mutex_unlock(&facilities.id.change.lock);
if (!in_idchange) {
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 {
}
else
{
code = 1;
itss_0send(facilities.zmq.responders[i].socket, &code, 1);
}
@ -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();

5
src/facilities.h
View File

@ -12,6 +12,7 @@
#include "saem.h"
#include "tpm.h"
#include "vcm.h"
#include "evcsnm.h"
#include <it2s-tender/epv.h>
#include <it2s-tender/database.h>
@ -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;