From 0dded574858ae4b283f023b0c48e2fb92bfbfba9 Mon Sep 17 00:00:00 2001 From: emanuel Date: Tue, 19 Mar 2024 12:02:51 +0000 Subject: [PATCH] config, cam: Refactor version handling --- src/cam.c | 497 ++--------------------------------------------- src/cam.h | 3 +- src/config.c | 14 +- src/facilities.h | 9 +- src/requests.c | 45 +---- 5 files changed, 32 insertions(+), 536 deletions(-) diff --git a/src/cam.c b/src/cam.c index 4beba67..9d3c915 100644 --- a/src/cam.c +++ b/src/cam.c @@ -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; } diff --git a/src/cam.h b/src/cam.h index 366f022..441df94 100644 --- a/src/cam.h +++ b/src/cam.h @@ -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 diff --git a/src/config.c b/src/config.c index 3d839c1..90a0814 100644 --- a/src/config.c +++ b/src/config.c @@ -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; diff --git a/src/facilities.h b/src/facilities.h index 62c6a75..4772e9f 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -21,11 +21,6 @@ #include #include -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; diff --git a/src/requests.c b/src/requests.c index 7df53c5..264045d 100644 --- a/src/requests.c +++ b/src/requests.c @@ -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;