From 299013b59e3de03c5c8039d71a649b566219200b Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 21 Nov 2022 16:36:57 +0000 Subject: [PATCH] VCM: reject if in maneuver --- src/vcm.c | 172 +++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 164 insertions(+), 8 deletions(-) diff --git a/src/vcm.c b/src/vcm.c index 5f8c580..053ea6e 100644 --- a/src/vcm.c +++ b/src/vcm.c @@ -276,6 +276,12 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) { int rv = 0; coordination_t* coordination = &facilities.coordination; + + if (!coordination->session.req) { + log_debug("[coordination] unknown reply context - session expired or another ITS-S rejected"); + return 1; + } + CoordinationReply_t* reply = &vcm->vcm.maneuverContainer.choice.vehicle.negotiation->choice.reply; itss_time_lock(); @@ -283,6 +289,19 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) { itss_time_unlock(); if (neighbour->intersecting) { + + if (reply->acceptedTrajectoriesIds.list.count == 0) { + log_debug("[coordination] maneuver %lld rejected by %d", reply->requesterId, vcm->header.stationID); + ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req); + coordination->session.req = NULL; + coordination->session.ts = itss_time_get(); + memset(coordination->session.affs, 0, sizeof(coordination->session.affs)); + memset(coordination->session.n_affs_neighs, 0, sizeof(coordination->session.n_affs_neighs)); + coordination->session.n_affs_trjs = 0; + neighbour->intersecting = false; + neighbour->proposed = false; + return 1; + } for (int t = 0; t < reply->acceptedTrajectoriesIds.list.count; ++t) { int tid = *reply->acceptedTrajectoriesIds.list.array[t]; if (tid > MC_TRAJECTORIES_N_MAX - 1) { @@ -322,14 +341,13 @@ static int vcm_check_handle_reply(VCM_t* vcm, mc_neighbour_s* neighbour) { fin, now_us-neighbour->t_iid); + neighbour->intersecting = false; + neighbour->proposed = false; } else { log_info("[vc] received VCM.reply is response to another ITS-S VCM.request"); } - neighbour->intersecting = false; - neighbour->proposed = false; - return rv; } @@ -364,7 +382,6 @@ static int intersection_detected(VCM_t* vcm, mc_neighbour_s* neighbour) { 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; @@ -632,6 +649,143 @@ 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(); + + 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; + + 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; @@ -673,7 +827,11 @@ int vcm_check(VCM_t* vcm) { if (mvc->negotiation) { switch (mvc->negotiation->present) { case CoordinationNegotiation_PR_request: - vcm_check_handle_request(vcm, neighbour); + if (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) { /* in maneuver */ + vcm_reject(vcm, neighbour); + } else { + vcm_check_handle_request(vcm, neighbour); + } break; case CoordinationNegotiation_PR_reply: @@ -688,9 +846,7 @@ int vcm_check(VCM_t* vcm) { (coordination->session.req && now < coordination->session.ts + MC_RESOLUTION_TIMEOUT) || /* issued request */ now < neighbour->t_proposal + MC_RESOLUTION_TIMEOUT /* issued reply */ ) { /* in maneuver */ - log_debug("[coordination] ignoring VCM - currently in maneuver"); - // TODO dont ignore-> reject - + log_debug("[coordination] ignoring VCM from %d - currently in maneuver", vcm->header.stationID); } else { /* not in maneuver OR timeout reached */ if (coordination->session.req) { /* clear previous request if exists */ ASN_STRUCT_FREE(asn_DEF_VCM, coordination->session.req);