From 62ae51845a3ff9a63e04da6125c21822f0517eb3 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 21 Nov 2022 19:38:46 +0000 Subject: [PATCH] VCM: reject VCM after checking it is for ego --- src/vcm.c | 338 +++++++++++++++++++++++++++--------------------------- 1 file changed, 170 insertions(+), 168 deletions(-) diff --git a/src/vcm.c b/src/vcm.c index 222ed20..9dd6b3f 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -46,6 +46,167 @@ static int do_paths_intersect( return 0; } +static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) { + + coordination_t* coordination = &facilities.coordination; + + pthread_mutex_lock(&facilities.id.lock); + uint32_t my_station_id = facilities.id.station_id; + pthread_mutex_unlock(&facilities.id.lock); + + int32_t lat, lon; + itss_space_lock(); + itss_space_get(); + lat = epv.space.latitude; + lon = epv.space.longitude; + itss_space_unlock(); + + uint64_t now = itss_time_get(); + + itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ + ssize_t trajectoryA_len = 0; + + TransportRequest_t* tr = NULL; + FacilitiesIndication_t* fi = NULL; + VCM_t* vcm_rep = calloc(1, sizeof(VCM_t)); + + int rv = 0; + const uint16_t buf_len = 2048; + uint8_t buf1[buf_len]; + + vcm_rep->header.messageID = 43; + vcm_rep->header.protocolVersion = 1; + vcm_rep->header.stationID = my_station_id; + + vcm_rep->vcm.currentPosition.latitude = lat; + vcm_rep->vcm.currentPosition.longitude = lon; + asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now); + + if (coordination->chain.id) { + vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t)); + vcm_rep->vcm.chain->id = coordination->chain.id; + vcm_rep->vcm.chain->link.buf = malloc(32); + vcm_rep->vcm.chain->link.size = 32; + memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 32); + vcm_rep->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t)); + vcm_rep->vcm.chain->area->buf = malloc(4); + vcm_rep->vcm.chain->area->size = 4; + } + + vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; + ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle; + + // Vehicle DimensionsDimensions + mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length; + mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0; + mvc_rep->vehicleWidth = facilities.vehicle.width; + + itss_trajectory_lock(); + trajectoryA_len = epv.trajectory.len; + memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); + itss_trajectory_unlock(); + + trajectoryA[0].latitude = lat; + trajectoryA[0].longitude = lon; + trajectoryA[0].timestamp = now; + ++trajectoryA_len; + + // Planned trajectory + mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1; + mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*); + mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*)); + for (int i = 0; i < trajectoryA_len - 1; ++i) { + mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); + mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; + mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; + mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp; + } + + mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); + mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply; + mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0; + mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = 0; + mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = NULL; + mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID; + mvc_rep->negotiation->choice.reply.nonce = vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request.nonce; + + asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf1, buf_len); + if (enc.encoded == -1) { + log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + ssize_t vcm_rep_len = (enc.encoded + 7) / 8; + + tr = calloc(1, sizeof(TransportRequest_t)); + tr->present = TransportRequest_PR_packet; + tr->choice.packet.present = TransportPacketRequest_PR_btp; + BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp; + bpr->btpType = BTPType_btpB; + bpr->id = itss_id(buf1, vcm_rep_len); + bpr->gn.destinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i) { + bpr->gn.destinationAddress.buf[i] = 0xff; + } + bpr->gn.destinationAddress.size = 6; + bpr->gn.packetTransportType = PacketTransportType_shb; + bpr->destinationPort = 2043; + bpr->gn.trafficClass = 2; + bpr->data.buf = malloc(vcm_rep_len); + memcpy(bpr->data.buf, buf1, vcm_rep_len); + bpr->data.size = vcm_rep_len; + buf1[0] = 4; + enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf1+1, buf_len-1); + if (enc.encoded == -1) { + log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); + + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + fi->choice.message.id = bpr->id; + fi->choice.message.itsMessageType = 2043; + fi->choice.message.data.size = bpr->data.size; + fi->choice.message.data.buf = malloc(bpr->data.size); + memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size); + buf1[0] = 4; + enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf1+1, buf_len-1); + if (enc.encoded == -1) { + log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message"); + + coordination->t_last_send_vcm = now; + + if (facilities.logging.recorder) { + uint16_t buffer_len = 2048; + uint8_t buffer[buffer_len]; + int e = itss_management_record_packet_sdu( + buffer, + buffer_len, + bpr->data.buf, + bpr->data.size, + bpr->id, + itss_time_get(), + ITSS_FACILITIES, + true); + if (e != -1) { + itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set"); + } + } + +cleanup: + ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep); + ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); + ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); +} + static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { int rv = 0; @@ -72,6 +233,14 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { return 0; } + uint64_t now = itss_time_get(); + + if (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */ + log_debug("[vc] rejecting VCM from %d - currently in maneuver", vcm->header.stationID); + vcm_reject(vcm, neighbour); + return 1; + } + const ssize_t buf_len = 512; uint8_t buf1[buf_len], buf2[buf_len]; @@ -136,12 +305,10 @@ static int vcm_check_handle_request(VCM_t* vcm, mc_neighbour_s* neighbour) { itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ ssize_t trajectoryA_len = 0; - uint64_t now = itss_time_get(); neighbour->proposed = true; neighbour->t_proposal = now; - vcm_rep = calloc(1, sizeof(VCM_t)); vcm_rep->header.messageID = 43; @@ -667,166 +834,6 @@ static void intersection_check(VCM_t* vcm, mc_neighbour_s* neighbour) { } } -static void vcm_reject(VCM_t* vcm, mc_neighbour_s* neighbour) { - - coordination_t* coordination = &facilities.coordination; - - pthread_mutex_lock(&facilities.id.lock); - uint32_t my_station_id = facilities.id.station_id; - pthread_mutex_unlock(&facilities.id.lock); - - int32_t lat, lon; - itss_space_lock(); - itss_space_get(); - lat = epv.space.latitude; - lon = epv.space.longitude; - itss_space_unlock(); - - uint64_t now = itss_time_get(); - - itss_st_t trajectoryA[TRAJECTORY_MAX_LEN+1]; /* ego trajectory */ - ssize_t trajectoryA_len = 0; - - TransportRequest_t* tr = NULL; - FacilitiesIndication_t* fi = NULL; - VCM_t* vcm_rep = calloc(1, sizeof(VCM_t)); - - int rv = 0; - const uint16_t buf_len = 2048; - uint8_t buf1[buf_len]; - - vcm_rep->header.messageID = 43; - vcm_rep->header.protocolVersion = 1; - vcm_rep->header.stationID = my_station_id; - - vcm_rep->vcm.currentPosition.latitude = lat; - vcm_rep->vcm.currentPosition.longitude = lon; - asn_ulong2INTEGER(&vcm_rep->vcm.currentPosition.timestamp, now); - - if (coordination->chain.id) { - vcm_rep->vcm.chain = calloc(1, sizeof(ChainInformation_t)); - vcm_rep->vcm.chain->id = coordination->chain.id; - vcm_rep->vcm.chain->link.buf = malloc(32); - vcm_rep->vcm.chain->link.size = 32; - memcpy(vcm_rep->vcm.chain->link.buf, coordination->chain.link, 32); - vcm_rep->vcm.chain->area = calloc(1, sizeof(OCTET_STRING_t)); - vcm_rep->vcm.chain->area->buf = malloc(4); - vcm_rep->vcm.chain->area->size = 4; - } - - vcm_rep->vcm.maneuverContainer.present = ManeuverContainer_PR_vehicle; - ManeuverVehicleContainer_t* mvc_rep = &vcm_rep->vcm.maneuverContainer.choice.vehicle; - - // Vehicle DimensionsDimensions - mvc_rep->vehicleLength.vehicleLengthValue= facilities.vehicle.length; - mvc_rep->vehicleLength.vehicleLengthConfidenceIndication = 0; - mvc_rep->vehicleWidth = facilities.vehicle.width; - - itss_trajectory_lock(); - trajectoryA_len = epv.trajectory.len; - memcpy(trajectoryA + 1, epv.trajectory.path, trajectoryA_len * sizeof(itss_st_t)); - itss_trajectory_unlock(); - - trajectoryA[0].latitude = lat; - trajectoryA[0].longitude = lon; - trajectoryA[0].timestamp = now; - ++trajectoryA_len; - - // Planned trajectory - mvc_rep->plannedTrajectory.list.count = trajectoryA_len - 1; - mvc_rep->plannedTrajectory.list.size = (trajectoryA_len - 1) * sizeof(void*); - mvc_rep->plannedTrajectory.list.array = malloc((trajectoryA_len - 1) * sizeof(void*)); - for (int i = 0; i < trajectoryA_len - 1; ++i) { - mvc_rep->plannedTrajectory.list.array[i] = calloc(1, sizeof(STPoint_t)); - mvc_rep->plannedTrajectory.list.array[i]->deltaLatitude = trajectoryA[i+1].latitude - trajectoryA[i].latitude; - mvc_rep->plannedTrajectory.list.array[i]->deltaLongitude = trajectoryA[i+1].longitude - trajectoryA[i].longitude; - mvc_rep->plannedTrajectory.list.array[i]->deltaTime = trajectoryA[i+1].timestamp - trajectoryA[i].timestamp; - } - - mvc_rep->negotiation = calloc(1, sizeof(CoordinationNegotiation_t)); - mvc_rep->negotiation->present = CoordinationNegotiation_PR_reply; - mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.count = 0; - mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.size = 0; - mvc_rep->negotiation->choice.reply.acceptedTrajectoriesIds.list.array = NULL; - mvc_rep->negotiation->choice.reply.requesterId = vcm->header.stationID; - mvc_rep->negotiation->choice.reply.nonce = vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.request.nonce; - - asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_VCM, NULL, vcm_rep, buf1, buf_len); - if (enc.encoded == -1) { - log_error("[vc] VCM.reply encode failure (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } - ssize_t vcm_rep_len = (enc.encoded + 7) / 8; - - tr = calloc(1, sizeof(TransportRequest_t)); - tr->present = TransportRequest_PR_packet; - tr->choice.packet.present = TransportPacketRequest_PR_btp; - BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp; - bpr->btpType = BTPType_btpB; - bpr->id = itss_id(buf1, vcm_rep_len); - bpr->gn.destinationAddress.buf = malloc(6); - for (int i = 0; i < 6; ++i) { - bpr->gn.destinationAddress.buf[i] = 0xff; - } - bpr->gn.destinationAddress.size = 6; - bpr->gn.packetTransportType = PacketTransportType_shb; - bpr->destinationPort = 2043; - bpr->gn.trafficClass = 2; - bpr->data.buf = malloc(vcm_rep_len); - memcpy(bpr->data.buf, buf1, vcm_rep_len); - bpr->data.size = vcm_rep_len; - buf1[0] = 4; - enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf1+1, buf_len-1); - if (enc.encoded == -1) { - log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } - - itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_TRANSPORT, bpr->id, "TR.packet.btp"); - - fi = calloc(1, sizeof(FacilitiesIndication_t)); - fi->present = FacilitiesIndication_PR_message; - fi->choice.message.id = bpr->id; - fi->choice.message.itsMessageType = 2043; - fi->choice.message.data.size = bpr->data.size; - fi->choice.message.data.buf = malloc(bpr->data.size); - memcpy(fi->choice.message.data.buf, bpr->data.buf, bpr->data.size); - buf1[0] = 4; - enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf1+1, buf_len-1); - if (enc.encoded == -1) { - log_error("[vc] TR VCM.reply encode failure (%s)", enc.failed_type->name); - rv = 1; - goto cleanup; - } - - itss_queue_send(facilities.tx_queue, buf1, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message"); - - coordination->t_last_send_vcm = now; - - if (facilities.logging.recorder) { - uint16_t buffer_len = 2048; - uint8_t buffer[buffer_len]; - int e = itss_management_record_packet_sdu( - buffer, - buffer_len, - bpr->data.buf, - bpr->data.size, - bpr->id, - itss_time_get(), - ITSS_FACILITIES, - true); - if (e != -1) { - itss_queue_send(facilities.tx_queue, buffer, e, ITSS_MANAGEMENT, bpr->id, "MReq.packet.set"); - } - } - -cleanup: - ASN_STRUCT_FREE(asn_DEF_VCM, vcm_rep); - ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); - ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); -} int vcm_check(VCM_t* vcm) { coordination_t* coordination = &facilities.coordination; @@ -869,12 +876,7 @@ int vcm_check(VCM_t* vcm) { if (mvc->negotiation) { switch (mvc->negotiation->present) { case CoordinationNegotiation_PR_request: - if (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */ - log_debug("[vc] rejecting VCM from %d - currently in maneuver", vcm->header.stationID); - vcm_reject(vcm, neighbour); - } else { - vcm_check_handle_request(vcm, neighbour); - } + vcm_check_handle_request(vcm, neighbour); break; case CoordinationNegotiation_PR_reply: