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

config, cam: Refactor version handling

This commit is contained in:
emanuel 2024-03-19 12:02:51 +00:00
parent 117ed93e31
commit 0dded57485
5 changed files with 32 additions and 536 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

497
src/cam.c
View File

@ -157,251 +157,7 @@ static void path_history_update(pos_point_t* pn) {
}
}
static int mk_cam_v1(uint8_t *cam_oer, uint16_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
lightship_t* lightship = &facilities.lightship;
EI1_CAM_t *cam = calloc(1, sizeof(EI1_CAM_t));
cam->header.protocolVersion = 2;
cam->header.messageID = EI1_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;
EI1_BasicContainer_t* bc = &cam->cam.camParameters.basicContainer;
uint64_t now = itss_time_get();
pthread_mutex_lock(&lightship->lock);
if (facilities.station_type != EI1_StationType_roadSideUnit) {
cam->cam.generationDeltaTime = now % 65536;
itss_space_lock();
itss_space_get();
bc->referencePosition.altitude.altitudeValue = epv.space.data.altitude.value;
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.data.latitude.value;
bc->referencePosition.longitude = epv.space.data.longitude.value;
uint16_t lat_conf = epv.space.data.latitude.confidence;
uint16_t lon_conf = epv.space.data.longitude.confidence;
cam->cam.camParameters.highFrequencyContainer.present = EI1_HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
EI1_BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
// Set speed
bvc_hf->speed.speedValue = epv.space.data.speed.value;
bvc_hf->speed.speedConfidence = epv.space.data.speed.confidence;
// Set heading
bvc_hf->heading.headingValue = epv.space.data.heading.value;
bvc_hf->heading.headingConfidence = epv.space.data.heading.confidence;
itss_space_unlock(epv);
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;
}
bvc_hf->vehicleWidth = facilities.vehicle.width;
bvc_hf->vehicleLength.vehicleLengthValue = facilities.vehicle.length;
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = EI1_VehicleLengthConfidenceIndication_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = EI1_LongitudinalAccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = EI1_AccelerationConfidence_unavailable;
bvc_hf->driveDirection = EI1_DriveDirection_unavailable;
bvc_hf->curvature.curvatureValue = EI1_CurvatureValue_unavailable;
bvc_hf->curvature.curvatureConfidence = EI1_CurvatureConfidence_unavailable;
bvc_hf->yawRate.yawRateValue = EI1_YawRateValue_unavailable;
bvc_hf->yawRate.yawRateConfidence = EI1_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.longitudinalAccelerationValue = long_a;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = EI1_AccelerationConfidence_unavailable;
} else {
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = EI1_LongitudinalAccelerationValue_unavailable;
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = EI1_AccelerationConfidence_unavailable;
}
// Low frequency container
if (now > lightship->t_last_cam_lfc + 500) {
cam->cam.camParameters.lowFrequencyContainer = calloc(1, sizeof(EI1_LowFrequencyContainer_t));
cam->cam.camParameters.lowFrequencyContainer->present = EI1_LowFrequencyContainer_PR_basicVehicleContainerLowFrequency;
EI1_BasicVehicleContainerLowFrequency_t* bvc_lf = &cam->cam.camParameters.lowFrequencyContainer->choice.basicVehicleContainerLowFrequency;
EI1_PathHistory_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(EI1_PathPoint_t));
if (lightship->path_history[0]->alt != EI1_AltitudeValue_unavailable && pn.alt != EI1_AltitudeValue_unavailable) {
ph->list.array[0]->pathPosition.deltaAltitude = lightship->path_history[0]->alt - pn.alt;
} else {
ph->list.array[0]->pathPosition.deltaAltitude = EI1_DeltaAltitude_unavailable;
}
if (lightship->path_history[0]->lat != EI1_Latitude_unavailable && pn.lat != EI1_Latitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLatitude = lightship->path_history[0]->lat - pn.lat;
} else {
ph->list.array[0]->pathPosition.deltaLatitude = EI1_DeltaLatitude_unavailable;
}
if (lightship->path_history[0]->lon != EI1_Longitude_unavailable && pn.lon != EI1_Longitude_unavailable) {
ph->list.array[0]->pathPosition.deltaLongitude = lightship->path_history[0]->lon - pn.lon;
} else {
ph->list.array[0]->pathPosition.deltaLongitude = EI1_DeltaLongitude_unavailable;
}
ph->list.array[0]->pathDeltaTime = calloc(1,sizeof(EI1_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(EI1_PathPoint_t));
if (lightship->path_history[i]->alt != EI1_AltitudeValue_unavailable && lightship->path_history[i-1]->alt != EI1_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 = EI1_DeltaAltitude_unavailable;
}
if (lightship->path_history[i]->lat != EI1_Latitude_unavailable && lightship->path_history[i-1]->lat != EI1_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 = EI1_DeltaLatitude_unavailable;
}
if (lightship->path_history[i]->lon != EI1_Longitude_unavailable && lightship->path_history[i-1]->lon != EI1_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 = EI1_DeltaLongitude_unavailable;
}
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(EI1_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.data.altitude.value;
bc->referencePosition.altitude.altitudeConfidence = epv.space.data.altitude.confidence;
// Set GPS coordinates
bc->referencePosition.latitude = epv.space.data.latitude.value;
bc->referencePosition.longitude = epv.space.data.longitude.value;
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = EI1_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = EI1_SemiAxisLength_unavailable;
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = EI1_HeadingValue_unavailable;
itss_space_unlock();
cam->cam.camParameters.highFrequencyContainer.present = EI1_HighFrequencyContainer_PR_rsuContainerHighFrequency;
if (lightship->protected_zones.pz_len > 0) {
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(EI1_ProtectedCommunicationZonesRSU_t));
EI1_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(EI1_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_EI1_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_EI1_CAM, cam);
return rv;
}
static int mk_cam_v2(uint8_t *cam_oer, uint16_t *cam_len) {
static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) {
int rv = 0;
int shm_fd, shm_valid = 0;
@ -743,235 +499,27 @@ void lightship_reset_timer() {
pthread_mutex_unlock(&lightship->lock);
}
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(EIS_BTPPacketIndication_t *bpi, void* cam, uint8_t* ssp, uint32_t ssp_len) {
int rv = 0;
lightship_t* lightship = &facilities.lightship;
uint64_t now = itss_time_get();
/* CAM 1.4.1 and 2.1.1 are the same */
EI2_CAM_t* cam_c = cam;
// Check permissions
if (ssp) {
if (cam->cam.camParameters.highFrequencyContainer.present == EI1_HighFrequencyContainer_PR_rsuContainerHighFrequency &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
if (cam_c->cam.camParameters.highFrequencyContainer.present == EI2_HighFrequencyContainer_PR_rsuContainerHighFrequency &&
cam_c->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 EI1_SpecialVehicleContainer_PR_NOTHING:
break;
case EI1_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 EI1_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 EI1_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 EI1_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 EI1_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 EI1_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 EI1_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 EI1_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 EI1_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 == EI1_StationType_roadSideUnit) {
// Send CAMs if vehicles nearby
if (bpi->stationType != EI1_StationType_roadSideUnit && bpi->isNeighbour) {
lightship->t_last_vehicle = now;
lightship->is_vehicle_near = true;
}
} else {
// Protected zones
if (cam->cam.camParameters.basicContainer.stationType == EI1_StationType_roadSideUnit &&
cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
EI1_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(EI1_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_copy(&asn_DEF_EIS_ProtectedCommunicationZone,
(void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p],
lightship->protected_zones.pz[p]
);
}
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;
}
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) {
if (cam_c->cam.camParameters.specialVehicleContainer) {
switch (cam_c->cam.camParameters.specialVehicleContainer->present) {
case EI2_SpecialVehicleContainer_PR_NOTHING:
break;
case EI2_SpecialVehicleContainer_PR_publicTransportContainer:
@ -1001,7 +549,7 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
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 (cam_c->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);
@ -1022,10 +570,10 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
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) {
if (cam_c->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority &&
cam_c->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf) {
// TODO verify bitmap
uint8_t bm = *cam->cam.camParameters.specialVehicleContainer->choice.emergencyContainer.emergencyPriority->buf;
uint8_t bm = *cam_c->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;
@ -1048,8 +596,8 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
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) {
if (cam_c->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
switch (*cam_c->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.trafficRule) {
case EI2_TrafficRule_noPassing:
if (!permissions_check(CID_NO_PASSING, ssp, ssp_len)) {
rv = CAM_BAD_PERMISSIONS;
@ -1069,7 +617,7 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
}
}
if (cam->cam.camParameters.specialVehicleContainer->choice.safetyCarContainer.speedLimit) {
if (cam_c->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);
@ -1090,9 +638,9 @@ enum CAM_CHECK_R check_cam_v2(EIS_BTPPacketIndication_t *bpi, EI2_CAM_t* cam, ui
}
} 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 (cam_c->cam.camParameters.basicContainer.stationType == EI2_StationType_roadSideUnit &&
cam_c->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) {
EI2_ProtectedCommunicationZonesRSU_t *pzs = cam_c->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU;
if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255) {
bool new_pz = false;
@ -1242,12 +790,7 @@ void* ca_service() {
itss_usleep(50*1000);
if (lightship_check() && facilities.lightship.active) {
switch (facilities.mver.defaultv) {
case MVER_1:
case MVER_2:
rv = mk_cam_v2(npr->data.buf, (uint16_t*) &npr->data.size);
break;
}
rv = mk_cam(npr->data.buf, (uint16_t*) &npr->data.size);
if (rv) {
continue;
}

3
src/cam.h
View File

@ -117,8 +117,7 @@ void lightship_reset_timer();
*
* @return A CAM check code
*/
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);
enum CAM_CHECK_R check_cam(EIS_BTPPacketIndication_t* bpi, void* cam, uint8_t* ssp, uint32_t ssp_len);
/*
* @brief Main CA service

14
src/config.c
View File

@ -316,15 +316,15 @@ int facilities_config() {
}
// 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;
if (!strcmp("v1", etsi_its_cfg->facilities.default_message_version)) {
log_error("[config] only default message version 2 is supported");
return -1;
} else if (!strcmp("v2", etsi_its_cfg->facilities.default_message_version)) {
facilities.pver.defaultv = 2;
} else {
log_warn("[config] unrecognized default messages version :: using version 1");
facilities.mver.defaultv = MVER_1;
log_warn("[config] unrecognized default messages version :: using version 2");
facilities.pver.defaultv = 2;
}
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;

9
src/facilities.h
View File

@ -21,11 +21,6 @@
#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,
@ -59,9 +54,9 @@ typedef struct facilities {
void* apps_socket; /* alternative to tx queue, only used in rx/main thread */
struct {
enum MVER defaultv;
uint8_t defaultv;
bool handle_all;
} mver;
} pver;
// CA
lightship_t lightship;

45
src/requests.c
View File

@ -52,34 +52,6 @@ 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;
}
}
static int handled_mver(uint8_t* msg_data) {
if (!facilities.mver.handle_all &&
facilities.mver.defaultv != msg_data[0]) {
return 0;
}
return 1;
}
int facilities_request_result_accepted(void *responder) {
int rv = 0;
@ -691,17 +663,13 @@ 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];
// No message
if (npi->data.size == 0) {
return 1;
}
// Is message version handled
if (!handled_mver(npi->data.buf)) {
return 1;
}
uint8_t pver = npi->data.buf[0];
// Parse message
switch (bpi->destinationPort) {
@ -821,16 +789,7 @@ static int networking_packet_indication_btp(EIS_NetworkingPacketIndication_t* np
// Manage message
switch (bpi->destinationPort) {
case EIS_Port_cam:
switch (pver2mver(pver)) {
case MVER_2:
rv = check_cam_v2(bpi, its_msg, ssp, ssp_len);
break;
default:
log_debug("<- not processing CAMv%d", pver);
goto cleanup;
}
rv = check_cam(bpi, its_msg, ssp, ssp_len);
switch (rv) {
case CAM_OK:
fwd = true;