diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index bdf19ef..f907f06 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -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 diff --git a/src/cam.c b/src/cam.c index 79250c6..051b702 100644 --- a/src/cam.c +++ b/src/cam.c @@ -5,6 +5,8 @@ #include #include #include +#include +#include #include #include @@ -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; } diff --git a/src/cam.h b/src/cam.h index 71b770d..366f022 100644 --- a/src/cam.h +++ b/src/cam.h @@ -7,6 +7,7 @@ #include #include +#include #include #include @@ -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 diff --git a/src/config.c b/src/config.c index 27ad1c4..51d87fb 100644 --- a/src/config.c +++ b/src/config.c @@ -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; diff --git a/src/facilities.h b/src/facilities.h index 43b05cc..62c6a75 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -21,6 +21,11 @@ #include #include +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; diff --git a/src/requests.c b/src/requests.c index 902f3f0..6d89e06 100644 --- a/src/requests.c +++ b/src/requests.c @@ -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; } }