diff --git a/src/facilities.c b/src/facilities.c index 11ce159..0f3ee11 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -230,6 +230,7 @@ static int transport_indication(facilities_t *facilities, void* responder, void* case 2043: pcm_check(facilities, its_msg); + fwd = true; break; default: diff --git a/src/pcm.c b/src/pcm.c index 1eb6b8a..0be4147 100644 --- a/src/pcm.c +++ b/src/pcm.c @@ -7,6 +7,7 @@ #include #include #include +#include #include static int are_vehicles_intersecting( @@ -54,6 +55,7 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei PCM_t* pcm_rep = NULL; TransportRequest_t* tr = NULL; + FacilitiesIndication_t* fi = NULL; const ssize_t buf_len = 512; uint8_t buf[buf_len]; @@ -132,27 +134,19 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei 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 = rand() + 1; - 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(pcm_rep_len); memcpy(bpr->data.buf, buf, pcm_rep_len); bpr->data.size = pcm_rep_len; - buf[0] = 4; enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1); if (enc.encoded == -1) { @@ -163,6 +157,23 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei it2s_tender_queue_send(facilities->tx_queue, buf, 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 = 43; + 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); + buf[0] = 4; + enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1); + if (enc.encoded == -1) { + syslog_err("[facilities] [pc] TR PCM.reply encode failure (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message"); + if (facilities->logging.recorder) { uint16_t buffer_len = 2048; uint8_t buffer[buffer_len]; @@ -183,6 +194,7 @@ static int pcm_check_handle_request(facilities_t* facilities, PCM_t* pcm, mc_nei cleanup: ASN_STRUCT_FREE(asn_DEF_PCM, pcm_rep); ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); + ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); return rv; } @@ -209,6 +221,7 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, PCM_t* pcm_req = NULL; TransportRequest_t* tr = NULL; + FacilitiesIndication_t* fi = NULL; const ssize_t buf_len = 1024; uint8_t buf[buf_len]; @@ -321,27 +334,19 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, tr->present = TransportRequest_PR_packet; tr->choice.packet.present = TransportPacketRequest_PR_btp; BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp; - bpr->id = rand() + 1; - bpr->btpType = BTPType_btpB; - 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(pcm_req_len); memcpy(bpr->data.buf, buf, pcm_req_len); bpr->data.size = pcm_req_len; - buf[0] = 4; enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_TransportRequest, tr, buf+1, buf_len-1); if (enc.encoded == -1) { @@ -352,6 +357,23 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, it2s_tender_queue_send(facilities->tx_queue, buf, 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 = 43; + 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); + buf[0] = 4; + enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesIndication, fi, buf+1, buf_len-1); + if (enc.encoded == -1) { + syslog_err("[facilities] [pc] TR PCM.reply encode failure (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + + it2s_tender_queue_send(facilities->tx_queue, buf, enc.encoded+1, ITSS_APPLICATIONS, bpr->id, "FI.message"); + if (facilities->logging.recorder) { uint16_t buffer_len = 2048; uint8_t buffer[buffer_len]; @@ -372,6 +394,7 @@ static int pcm_check_intersection_detected(facilities_t* facilities, PCM_t* pcm, cleanup: ASN_STRUCT_FREE(asn_DEF_PCM, pcm_req); ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr); + ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi); return rv; }