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

CAM v2 trials

This commit is contained in:
emanuel 2024-01-29 18:02:05 +00:00
parent 711c61f6ca
commit 4739dabc63
6 changed files with 681 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/CMakeLists.txt
View File

@ -33,6 +33,8 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
-lit2s-asn-etsi-its-v1-vcm
-lit2s-asn-etsi-its-v1-evcsnm
-lit2s-asn-etsi-its-v1-evrsrm
-lit2s-asn-etsi-its-v2-cdd-2.2.1
-lit2s-asn-etsi-its-v2-cam
-lit2s-tender
-lm
-lrt

469
src/cam.c
View File

@ -5,6 +5,8 @@
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
#include <it2s-asn/etsi-its-v2/cdd-2.2.1/EI2_StationType.h>
#include <it2s-asn/etsi-its-v2/cam/EI2_CAM.h>
#include <sys/mman.h>
#include <stdint.h>
@ -155,7 +157,7 @@ static void path_history_update(pos_point_t* pn) {
}
}
static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) {
static int mk_cam_v1(uint8_t *cam_oer, uint16_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
@ -399,6 +401,251 @@ cleanup:
return rv;
}
static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
lightship_t* lightship = &facilities.lightship;
EI2_CAM_t *cam = calloc(1, sizeof(EI2_CAM_t));
cam->header.protocolVersion = 2;
cam->header.messageId = EI2_MessageId_cam;
pthread_mutex_lock(&facilities.id.lock);
cam->header.stationId = facilities.id.station_id;
pthread_mutex_unlock(&facilities.id.lock);
cam->cam.camParameters.basicContainer.stationType = facilities.station_type;
EI2_BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
uint64_t now = itss_time_get();
pthread_mutex_lock(&lightship->lock);
if (facilities.station_type != EI2_StationType_roadSideUnit) {
cam->cam.generationDeltaTime = now % 65536;
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
uint16_t lat_conf = epv.space.latitude_conf;
uint16_t lon_conf = epv.space.longitude_conf;
cam->cam.camParameters.highFrequencyContainer.present = EI2_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
EI2_BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
// Set speed
bvc_hf->speed.speedValue = epv.space.speed;
bvc_hf->speed.speedConfidence = epv.space.speed_conf;
// Set heading
bvc_hf->heading.headingValue = epv.space.heading;
bvc_hf->heading.headingConfidence = epv.space.heading_conf;
itss_space_unlock(epv);
if (lat_conf > lon_conf) {
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = lon_conf;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = lat_conf;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = 0;
} else {
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = lon_conf;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = lat_conf;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = 900;
}
bvc_hf->vehicleWidth = facilities.vehicle.width;
bvc_hf->vehicleLength.vehicleLengthValue = facilities.vehicle.length;
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = EI2_VehicleLengthConfidenceIndication_unavailable;
bvc_hf->longitudinalAcceleration.value = EI2_AccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.confidence = EI2_AccelerationConfidence_unavailable;
bvc_hf->driveDirection = EI2_DriveDirection_unavailable;
bvc_hf->curvature.curvatureValue = EI2_CurvatureValue_unavailable;
bvc_hf->curvature.curvatureConfidence = EI2_CurvatureConfidence_unavailable;
bvc_hf->yawRate.yawRateValue = EI2_YawRateValue_unavailable;
bvc_hf->yawRate.yawRateConfidence = EI2_YawRateConfidence_unavailable;
/*
// Save current values
if (lightship->path_history_len == POS_HISTORY_MAX_LEN) {
free(lightship->path_history[POS_HISTORY_MAX_LEN-1]);
--lightship->path_history_len;
}
*/
/*
for (int i = lightship->path_history_len-1; i >= 0; --i) {
lightship->path_history[i+1] = lightship->path_history[i];
}
*/
pos_point_t pn;
pn.heading = bvc_hf->heading.headingValue;
pn.lat = bc->referencePosition.latitude;
pn.lon = bc->referencePosition.longitude;
pn.alt = bc->referencePosition.altitude.altitudeValue;
pn.speed = bvc_hf->speed.speedValue;
pn.ts = now;
path_history_update(&pn);
lightship->t_last_cam = now;
// Acceleration
if (lightship->last_pos.ts != 0) {
double delta_angle = ((int)pn.heading - lightship->last_pos.heading) / 10.0; /* 1º */
double delta_speed = (pn.speed - (lightship->last_pos.speed * cos(delta_angle * RAD_PER_DEG))) / 10.0; /* 0.1 m/s */
int16_t long_a = (int16_t) (delta_speed / (double) ((pn.ts - lightship->last_pos.ts) / 1000.0)); /* 0.1 m/s^2 */
if (long_a > 160) long_a = 160;
else if (long_a < -160) long_a = -160;
bvc_hf->longitudinalAcceleration.value = long_a;
bvc_hf->longitudinalAcceleration.confidence = EI2_AccelerationConfidence_unavailable;
} else {
bvc_hf->longitudinalAcceleration.value = EI2_AccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.confidence = EI2_AccelerationConfidence_unavailable;
}
// Low frequency container
if (now > lightship->t_last_cam_lfc + 500) {
cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(EI2_LowFrequencyContainer_t));
cam->cam.camParameters.lowFrequencyContainer->present = EI2_LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
EI2_BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency;
EI2_Path_t* ph = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency.pathHistory;
if (lightship->path_history_len != 0) {
ph->list.array = malloc((lightship->path_history_len) * sizeof(void*));
ph->list.count = lightship->path_history_len;
ph->list.size = (lightship->path_history_len) * sizeof(void*);
ph->list.array[0] = calloc(1,sizeof(EI2_PathPoint_t));
if (lightship->path_history[0]->alt != EI2_AltitudeValue_unavailable && pn.alt != EI2_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = lightship->path_history[0]->alt - pn.alt;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = EI2_DeltaAltitude_unavailable;
}
if (lightship->path_history[0]->lat != EI2_Latitude_unavailable && pn.lat != EI2_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = lightship->path_history[0]->lat - pn.lat;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = EI2_DeltaLatitude_unavailable;
}
if (lightship->path_history[0]->lon != EI2_Longitude_unavailable && pn.lon != EI2_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = lightship->path_history[0]->lon - pn.lon;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = EI2_DeltaLongitude_unavailable;
}
ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(EI2_PathDeltaTime_t));
*ph->list.array[0]->pathDeltaTime = (pn.ts - lightship->path_history[0]->ts)/10;
for (int i = 1; i < lightship->path_history_len; ++i) {
ph->list.array[i] = calloc(1,sizeof(EI2_PathPoint_t));
if (lightship->path_history[i]->alt != EI2_AltitudeValue_unavailable && lightship->path_history[i-1]->alt != EI2_AltitudeValue_unavailable) {
ph->list.array[i]->pathPosition.deltaAltitude = lightship->path_history[i]->alt - lightship->path_history[i-1]->alt;
} else {
ph->list.array[i]->pathPosition.deltaAltitude = EI2_DeltaAltitude_unavailable;
}
if (lightship->path_history[i]->lat != EI2_Latitude_unavailable && lightship->path_history[i-1]->lat != EI2_Latitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLatitude = lightship->path_history[i]->lat - lightship->path_history[i-1]->lat;
} else {
ph->list.array[i]->pathPosition.deltaLatitude = EI2_DeltaLatitude_unavailable;
}
if (lightship->path_history[i]->lon != EI2_Longitude_unavailable && lightship->path_history[i-1]->lon != EI2_Longitude_unavailable) {
ph->list.array[i]->pathPosition.deltaLongitude = lightship->path_history[i]->lon - lightship->path_history[i-1]->lon;
} else {
ph->list.array[i]->pathPosition.deltaLongitude = EI2_DeltaLongitude_unavailable;
}
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(EI2_PathDeltaTime_t));
*ph->list.array[i]->pathDeltaTime = (lightship->path_history[i-1]->ts - lightship->path_history[i]->ts)/10;
}
}
lightship->t_last_cam_lfc = now;
}
lightship->last_pos = pn;
} else {
cam->cam.generationDeltaTime = now % 65536;
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.altitude;
bc->referencePosition.altitude.altitudeConfidence = epv.space.altitude_conf;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.latitude;
bc->referencePosition.longitude = epv.space.longitude;
bc->referencePosition.positionConfidenceEllipse.semiMinorAxisLength = EI2_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisLength = EI2_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorAxisOrientation = EI2_HeadingValue_unavailable;
itss_space_unlock();
cam->cam.camParameters.highFrequencyContainer.present = EI2_HighFrequencyContainer_PR_rsuContainerHighFrequency;
if (lightship->protected_zones.pz_len > 0) {
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(EI2_ProtectedCommunicationZonesRSU_t));
EI2_ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
pzs->list.count = lightship->protected_zones.pz_len;
pzs->list.size = lightship->protected_zones.pz_len * sizeof(void*);
pzs->list.array = malloc(lightship->protected_zones.pz_len * sizeof(void*));
for (int i = 0; i < lightship->protected_zones.pz_len; ++i) {
pzs->list.array[i] = calloc(1, sizeof(EI2_ProtectedCommunicationZone_t));
pzs->list.array[i]->protectedZoneLatitude = lightship->protected_zones.pz[i]->protectedZoneLatitude;
pzs->list.array[i]->protectedZoneLongitude = lightship->protected_zones.pz[i]->protectedZoneLongitude;
pzs->list.array[i]->protectedZoneType = lightship->protected_zones.pz[i]->protectedZoneType;
if (lightship->protected_zones.pz[i]->expiryTime) {
pzs->list.array[i]->expiryTime->size = lightship->protected_zones.pz[i]->expiryTime->size;
pzs->list.array[i]->expiryTime->buf = malloc(lightship->protected_zones.pz[i]->expiryTime->size);
memcpy(pzs->list.array[i]->expiryTime->buf, lightship->protected_zones.pz[i]->expiryTime->buf, lightship->protected_zones.pz[i]->expiryTime->size);
}
if (lightship->protected_zones.pz[i]->protectedZoneID) {
pzs->list.array[i]->protectedZoneId = malloc(8);
*pzs->list.array[i]->protectedZoneId = *lightship->protected_zones.pz[i]->protectedZoneID;
}
if (lightship->protected_zones.pz[i]->protectedZoneRadius) {
pzs->list.array[i]->protectedZoneRadius = malloc(8);
*pzs->list.array[i]->protectedZoneRadius = *lightship->protected_zones.pz[i]->protectedZoneRadius;
}
}
}
}
pthread_mutex_unlock(&lightship->lock);
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_EI2_CAM, NULL, cam, cam_oer, 512);
if (enc.encoded == -1) {
log_error("[ca] failed encoding CAM (%s)", enc.failed_type->name);
rv = 1;
goto cleanup;
}
*cam_len = (enc.encoded + 7) / 8;
cleanup:
ASN_STRUCT_FREE(asn_DEF_EI2_CAM, cam);
return rv;
}
int lightship_init() {
@ -496,7 +743,7 @@ void lightship_reset_timer() {
pthread_mutex_unlock(&lightship->lock);
}
enum CAM_CHECK_R check_cam(EIS_BTPPacketIndication_t *bpi, EI1_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len) {
enum CAM_CHECK_R check_cam_v1(EIS_BTPPacketIndication_t *bpi, EI1_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
lightship_t* lightship = &facilities.lightship;
@ -705,6 +952,215 @@ enum CAM_CHECK_R check_cam(EIS_BTPPacketIndication_t *bpi, EI1_CAM_t* cam, uint8
return rv;
}
enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, 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 (cam->cam.camParameters.highFrequencyContainer.present == EI2_HighFrequencyContainer_PR_rsuContainerHighFrequency &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
if (!permissions_check(CID_PROTECTED_ZONES, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PROTECTED_ZONES].container);
return rv;
}
}
if (cam->cam.camParameters.specialVehicleContainer) {
switch (cam->cam.camParameters.specialVehicleContainer->present) {
case EI2_SpecialVehicleContainer_PR_NOTHING:
break;
case EI2_SpecialVehicleContainer_PR_publicTransportContainer:
if (!permissions_check(CID_PUBLIC_TRANSPORT, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_PUBLIC_TRANSPORT].container);
return rv;
}
break;
case EI2_SpecialVehicleContainer_PR_specialTransportContainer:
if (!permissions_check(CID_SPECIAL_TRANSPORT, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SPECIAL_TRANSPORT].container);
return rv;
}
break;
case EI2_SpecialVehicleContainer_PR_dangerousGoodsContainer:
if (!permissions_check(CID_DANGEROUS_GOODS, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_DANGEROUS_GOODS].container);
return rv;
}
break;
case EI2_SpecialVehicleContainer_PR_roadWorksContainerBasic:
if (!permissions_check(CID_ROADWORK, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_ROADWORK].container);
return rv;
}
if (cam->cam.camParameters.specialVehicleContainer->choice.roadWorksContainerBasic.closedLanes) {
if (!permissions_check(CID_CLOSED_LANES, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_CLOSED_LANES].container);
return rv;
}
}
break;
case EI2_SpecialVehicleContainer_PR_rescueContainer:
if (!permissions_check(CID_RESCUE, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_RESCUE].container);
return rv;
}
break;
case EI2_SpecialVehicleContainer_PR_emergencyContainer:
if (!permissions_check(CID_EMERGENCY, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_EMERGENCY].container);
return rv;
}
if (cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority &&
cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) {
// TODO verify bitmap
uint8_t bm = *cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf;
if (bm & 0x02) {
if (!permissions_check(CID_REQUEST_FOR_RIGHT_OF_WAY, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam 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 = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_REQUEST_FOR_FREE_CROSSING_AT_A_TRAFFIC_LIGHT].container);
return rv;
}
}
}
break;
case EI2_SpecialVehicleContainer_PR_safetyCarContainer:
if (!permissions_check(CID_SAFETY_CAR, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_SAFETY_CAR].container);
return rv;
}
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
switch (*cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
case EI2_TrafficRule_noPassing:
if (!permissions_check(CID_NO_PASSING, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING].container);
return rv;
}
break;
case EI2_TrafficRule_noPassingForTrucks:
if (!permissions_check(CID_NO_PASSING_FOR_TRUCKS, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam does not have permissions for '%s'", CID_SSP_BM_MAP[CID_NO_PASSING_FOR_TRUCKS].container);
return rv;
}
break;
default:
break;
}
}
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
if (!permissions_check(CID_SPEED_LIMIT, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
log_debug("[ca] received cam 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 == EI2_StationType_roadSideUnit) {
// Send CAMs if vehicles nearby
if (bpi->stationType != EI2_StationType_roadSideUnit && bpi->isNeighbour) {
lightship->t_last_vehicle = now;
lightship->is_vehicle_near = true;
}
} else {
// Protected zones
if (cam->cam.camParameters.basicContainer.stationType == EI2_StationType_roadSideUnit &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
EI2_ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.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(EI2_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];
EIS_ManagementRequest_t* mreq = calloc(1, sizeof(EIS_ManagementRequest_t));
mreq->present = EIS_ManagementRequest_PR_attributes;
mreq->choice.attributes.present = EIS_ManagementRequestAttributes_PR_set;
mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct EIS_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_EIS_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512);
oer_decode(NULL, &asn_DEF_EIS_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_EIS_ManagementRequest, NULL, mreq, b_oer, 512);
ASN_STRUCT_FREE(asn_DEF_EIS_ManagementRequest, mreq);
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;
@ -782,7 +1238,14 @@ void* ca_service() {
usleep(1000*50);
if (lightship_check() && facilities.lightship.active) {
rv = mk_cam(npr->data.buf, (uint16_t*) &npr->data.size);
switch (facilities.mver.defaultv) {
case MVER_1:
rv = mk_cam_v1(npr->data.buf, (uint16_t*) &npr->data.size);
break;
case MVER_2:
rv = mk_cam_v2(npr->data.buf, (uint16_t*) &npr->data.size);
break;
}
if (rv) {
continue;
}

4
src/cam.h
View File

@ -7,6 +7,7 @@
#include <stdbool.h>
#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
#include <it2s-asn/etsi-its-v2/cam/EI2_CAM.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
#include <it2s-tender/epv.h>
@ -116,7 +117,8 @@ void lightship_reset_timer();
*
* @return A CAM check code
*/
enum CAM_CHECK_R check_cam(EIS_BTPPacketIndication_t* bpi, EI1_CAM_t* cam,uint8_t* ssp, uint32_t ssp_len);
enum CAM_CHECK_R check_cam_v1(EIS_BTPPacketIndication_t* bpi, EI1_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len);
enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t* bpi, EI2_CAM_t* cam, uint8_t* ssp, uint32_t ssp_len);
/*
* @brief Main CA service

10
src/config.c
View File

@ -315,6 +315,16 @@ int facilities_config() {
facilities.id.ipv6_addr[8] ^= 0x02;
}
// Message version
if (!strcmp("v1", etsi_its_cfg->facilities.mver.default_version)) {
facilities.mver.defaultv = MVER_1;
} else if (!strcmp("v2", etsi_its_cfg->facilities.mver.default_version)) {
facilities.mver.defaultv = MVER_2;
} else {
log_warn("[config] unrecognized default messages version :: using version 1");
facilities.mver.defaultv = MVER_1;
}
facilities.mver.handle_all = etsi_its_cfg->facilities.mver.handle_all;
// DENM
facilities.den.n_max_events = etsi_its_cfg->facilities.denm.nmax_active_events;

10
src/facilities.h
View File

@ -21,6 +21,11 @@
#include <it2s-tender/constants.h>
#include <it2s-tender/queue.h>
typedef enum MVER {
MVER_1,
MVER_2
} MVER_e;
enum ID_CHANGE_STAGE {
ID_CHANGE_INACTIVE,
ID_CHANGE_BLOCKED,
@ -53,6 +58,11 @@ typedef struct facilities {
itss_queue_t* tx_queue;
void* apps_socket; /* alternative to tx queue, only used in rx/main thread */
struct {
enum MVER defaultv;
bool handle_all;
} mver;
// CA
lightship_t lightship;

336
src/requests.c
View File

@ -52,6 +52,26 @@ static void fwd_to_apps(uint8_t* msg, uint16_t msg_len, int its_msg_type, uint32
ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesIndication, fi);
}
static int pver2mver(uint8_t pver) {
switch (pver) {
case 1:
if (!facilities.mver.handle_all &&
facilities.mver.defaultv != MVER_1) {
return -1;
}
return MVER_1;
case 2:
if (!facilities.mver.handle_all &&
facilities.mver.defaultv != MVER_2) {
return -1;
}
return MVER_2;
default:
return -1;
}
}
int facilities_request_result_accepted(void *responder) {
int rv = 0;
@ -663,72 +683,82 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
uint8_t buf[buf_len];
uint64_t id = 0;
uint8_t pver = npi->data.buf[0];
// Parse message
switch (bpi->destinationPort) {
case EIS_Port_cam:
case EIS_Port_cam:
switch (pver) {
case 1:
its_msg_descriptor = &asn_DEF_EI1_CAM;
its_msg = calloc(1, sizeof(EI1_CAM_t));
its_msg_type = EI1_messageID_cam;
break;
case EIS_Port_denm:
its_msg_descriptor = &asn_DEF_EI1_DENM;
its_msg = calloc(1, sizeof(EI1_DENM_t));
its_msg_type = EI1_messageID_denm;
case 2:
its_msg_descriptor = &asn_DEF_EI2_CAM;
its_msg = calloc(1, sizeof(EI2_CAM_t));
its_msg_type = EI2_MessageId_cam;
break;
}
break;
case EIS_Port_ivim:
its_msg_descriptor = &asn_DEF_EI1_IVIM;
its_msg = calloc(1, sizeof(EI1_IVIM_t));
its_msg_type = EI1_messageID_ivim;
break;
case EIS_Port_denm:
its_msg_descriptor = &asn_DEF_EI1_DENM;
its_msg = calloc(1, sizeof(EI1_DENM_t));
its_msg_type = EI1_messageID_denm;
break;
case EIS_Port_cpm:
its_msg_descriptor = &asn_DEF_EI1_CPM;
its_msg = calloc(1, sizeof(EI1_CPM_t));
its_msg_type = 14;
break;
case EIS_Port_ivim:
its_msg_descriptor = &asn_DEF_EI1_IVIM;
its_msg = calloc(1, sizeof(EI1_IVIM_t));
its_msg_type = EI1_messageID_ivim;
break;
case EIS_Port_saem:
its_msg_descriptor = &asn_DEF_EI1_SAEM;
its_msg = calloc(1, sizeof(EI1_SAEM_t));
its_msg_type = EI1_messageID_saem;
break;
case EIS_Port_cpm:
its_msg_descriptor = &asn_DEF_EI1_CPM;
its_msg = calloc(1, sizeof(EI1_CPM_t));
its_msg_type = 14;
break;
case 7011: /* tolling */
its_msg_descriptor = &asn_DEF_EI1_TPM;
its_msg = calloc(1, sizeof(EI1_TPM_t));
its_msg_type = 117;
break;
case EIS_Port_saem:
its_msg_descriptor = &asn_DEF_EI1_SAEM;
its_msg = calloc(1, sizeof(EI1_SAEM_t));
its_msg_type = EI1_messageID_saem;
break;
case 2043: /* maneuvers */
its_msg_descriptor = &asn_DEF_EI1_VCM;
its_msg = calloc(1, sizeof(EI1_VCM_t));
its_msg_type = 43;
break;
case 7011: /* tolling */
its_msg_descriptor = &asn_DEF_EI1_TPM;
its_msg = calloc(1, sizeof(EI1_TPM_t));
its_msg_type = 117;
break;
case EIS_Port_poi: /* EVCSNM */
its_msg_descriptor = &asn_DEF_EI1_EvcsnPdu;
its_msg = calloc(1, sizeof(EI1_EvcsnPdu_t));
its_msg_type = EI1_messageID_evcsn;
break;
case 2043: /* maneuvers */
its_msg_descriptor = &asn_DEF_EI1_VCM;
its_msg = calloc(1, sizeof(EI1_VCM_t));
its_msg_type = 43;
break;
case EIS_Port_evrsr: /* EVRSRM */
its_msg_descriptor = &asn_DEF_EI1_EV_RSR;
its_msg = calloc(1, sizeof(EI1_EV_RSR_t));
its_msg_type = EI1_messageID_evcsn;
break;
case EIS_Port_poi: /* EVCSNM */
its_msg_descriptor = &asn_DEF_EI1_EvcsnPdu;
its_msg = calloc(1, sizeof(EI1_EvcsnPdu_t));
its_msg_type = EI1_messageID_evcsn;
break;
default:
if (!facilities.upf) {
log_debug("messsage with unhandled BTP port received (%lld), ignoring", bpi->destinationPort);
goto cleanup;
} else {
process_msg = false;
fwd = true;
its_msg_type = 7120;
}
case EIS_Port_evrsr: /* EVRSRM */
its_msg_descriptor = &asn_DEF_EI1_EV_RSR;
its_msg = calloc(1, sizeof(EI1_EV_RSR_t));
its_msg_type = EI1_messageID_evcsn;
break;
default:
if (!facilities.upf) {
log_debug("messsage with unhandled BTP port received (%lld), ignoring", bpi->destinationPort);
goto cleanup;
} else {
process_msg = false;
fwd = true;
its_msg_type = 7120;
}
}
if (process_msg) {
@ -769,109 +799,123 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
// Manage message
switch (bpi->destinationPort) {
case EIS_Port_cam:
switch (check_cam(bpi, its_msg, ssp, ssp_len)) {
case CAM_OK:
fwd = true;
break;
case CAM_INVALID:
case CAM_BAD_PERMISSIONS:
default:
break;
}
break;
case EIS_Port_cam:
case EIS_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_EI1_DENM, its_msg);
log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded, xml_denm);
free(xml_denm);
#endif
stored = true;
switch (event_manage(its_msg, &id, ssp, ssp_len)) {
case EVENT_NEW:
case EVENT_CANCELLATION:
case EVENT_NEGATION:
case EVENT_UPDATE:
case EVENT_NUMBER_EXCEEDED:
fwd = true;
break;
case EVENT_INVALID:
case EVENT_PASSED:
case EVENT_REPEATED:
case EVENT_BAD_PERMISSIONS:
break;
}
switch (pver2mver(pver)) {
case MVER_1:
rv = check_cam_v1(bpi, its_msg, ssp, ssp_len);
break;
case EIS_Port_ivim:
stored = true;
switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len)) {
case SERVICE_NEW:
case SERVICE_CANCELLATION:
case SERVICE_NEGATION:
case SERVICE_UPDATE:
case SERVICE_NUMBER_EXCEEDED:
fwd = true;
break;
case SERVICE_INVALID:
case SERVICE_REPEATED:
case SERVICE_PASSED:
case SERVICE_BAD_PERMISSIONS:
default:
break;
}
case MVER_2:
rv = check_cam_v2(bpi, its_msg, ssp, ssp_len);
break;
default:
log_debug("<- not processing CAMv%d", pver);
goto cleanup;
}
case EIS_Port_saem:
switch (saem_check(its_msg, neighbour_cert)) {
case SAEM_NEW:
fwd = true;
break;
default:
break;
}
switch (rv) {
case CAM_OK:
fwd = true;
break;
case 7011:
if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_SPKI) { /* do not wait for facilities process if spki */
fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
fwd = false;
} else {
fwd = true;
}
if (facilities.tolling.enabled) {
tpm_recv(its_msg, security_socket, neighbour_cert, NULL);
}
break;
case 2043:
fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
fwd = false;
if (facilities.coordination.active) {
vcm_check(its_msg);
}
break;
case EIS_Port_poi:
if (facilities.evm_args.activate) {
evcsnm_check(its_msg);
fwd = true;
}
break;
case EIS_Port_evrsr:
if (facilities.evm_args.activate) {
evrsrm_check(its_msg);
fwd = true;
EI1_EV_RSR_t *evrsr_request = (EI1_EV_RSR_t *)its_msg;
evrsrm_recv(evrsr_request);
}
break;
case CAM_INVALID:
case CAM_BAD_PERMISSIONS:
default:
break;
}
break;
case EIS_Port_denm:
#ifdef DEBUG
uint8_t *xml_denm = malloc(32768);
asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 32768, 0x02, &its_msg_def, its_msg);
log_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded, xml_denm);
free(xml_denm);
#endif
stored = true;
switch (event_manage(its_msg, &id, ssp, ssp_len)) {
case EVENT_NEW:
case EVENT_CANCELLATION:
case EVENT_NEGATION:
case EVENT_UPDATE:
case EVENT_NUMBER_EXCEEDED:
fwd = true;
break;
case EVENT_INVALID:
case EVENT_PASSED:
case EVENT_REPEATED:
case EVENT_BAD_PERMISSIONS:
break;
}
break;
case EIS_Port_ivim:
stored = true;
switch (service_eval(SERVICE_IVI, its_msg, &id, ssp, ssp_len)) {
case SERVICE_NEW:
case SERVICE_CANCELLATION:
case SERVICE_NEGATION:
case SERVICE_UPDATE:
case SERVICE_NUMBER_EXCEEDED:
fwd = true;
break;
case SERVICE_INVALID:
case SERVICE_REPEATED:
case SERVICE_PASSED:
case SERVICE_BAD_PERMISSIONS:
default:
break;
}
break;
case EIS_Port_saem:
switch (saem_check(its_msg, neighbour_cert)) {
case SAEM_NEW:
fwd = true;
break;
default:
break;
}
break;
case 7011:
if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_SPKI) { /* do not wait for facilities process if spki */
fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
fwd = false;
} else {
fwd = true;
}
if (facilities.tolling.enabled) {
tpm_recv(its_msg, security_socket, neighbour_cert, NULL);
}
break;
case 2043:
fwd_to_apps(npi->data.buf, npi->data.size, bpi->destinationPort, npi->id);
fwd = false;
if (facilities.coordination.active) {
vcm_check(its_msg);
}
break;
case EIS_Port_poi:
if (facilities.evm_args.activate) {
evcsnm_check(its_msg);
fwd = true;
}
break;
case EIS_Port_evrsr:
if (facilities.evm_args.activate) {
evrsrm_check(its_msg);
fwd = true;
EI1_EV_RSR_t *evrsr_request = (EI1_EV_RSR_t *)its_msg;
evrsrm_recv(evrsr_request);
}
break;
default:
break;
}
}