diff --git a/src/cam.c b/src/cam.c index ee5ae9a..a4d1a32 100644 --- a/src/cam.c +++ b/src/cam.c @@ -157,7 +157,7 @@ static void path_history_update(pos_point_t* pn) { } } -static int mk_cam(uint8_t *cam_oer, uint32_t *cam_len) { +static int mk_cam(uint8_t *cam_oer, uint16_t *cam_len) { int rv = 0; int shm_fd, shm_valid = 0; it2s_obd_data* shared_message; @@ -824,7 +824,16 @@ void* ca_service() { bpr->gn.trafficClass = 2; - bpr->data.buf = malloc(512); + if (facilities.edm.enabled) { + bpr->destinationPortInfo = calloc(1, sizeof(OCTET_STRING_t)); + bpr->destinationPortInfo->size = 2; + bpr->destinationPortInfo->buf = malloc(2); + *(uint16_t*)bpr->destinationPortInfo->buf = 0xed; + } + + const int buf_len = 1024; + + bpr->data.buf = malloc(buf_len); // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs @@ -834,20 +843,25 @@ void* ca_service() { fi->present = FacilitiesIndication_PR_message; FacilitiesMessageIndication_t* fmi = &fi->choice.message; fmi->itsMessageType = ItsMessageType_cam; - fmi->data.buf = malloc(512); + fmi->data.buf = malloc(buf_len); - uint8_t tr_oer[1024]; - uint8_t fi_oer[1024]; + uint8_t tr_oer[buf_len]; + uint8_t fi_oer[buf_len]; tr_oer[0] = 4; // Facilities fi_oer[0] = 4; while (!facilities.exit) { usleep(1000*50); if (lightship_check() && facilities.lightship.active) { - rv = mk_cam(bpr->data.buf, (uint32_t *) &bpr->data.size); + rv = mk_cam(bpr->data.buf, (uint16_t*) &bpr->data.size); if (rv) { continue; } + + if (facilities.edm.enabled) { + edm_encap(bpr->data.buf, (uint16_t*) &bpr->data.size, buf_len, Port_cam); + } + memcpy(fmi->data.buf, bpr->data.buf, bpr->data.size); fmi->data.size = bpr->data.size; diff --git a/src/edm.c b/src/edm.c index a7a815e..a61fbdf 100644 --- a/src/edm.c +++ b/src/edm.c @@ -28,7 +28,7 @@ int edm_encap(uint8_t* msg, uint16_t* msg_len, uint16_t msg_buf_len, int its_msg } pthread_mutex_lock(&edm->lock); - itss_0send(edm->app_socket, b1, enc.encoded); + itss_0send(edm->app_socket, b1, enc.encoded+1); int rl = itss_0recv_rt(&edm->app_socket, b2, b_len, b1, enc.encoded+1, 1000); if (rl == -1) { rv = 1; diff --git a/src/requests.c b/src/requests.c index 47c0ad3..6f8d743 100644 --- a/src/requests.c +++ b/src/requests.c @@ -20,6 +20,28 @@ #include "facilities.h" #include "infrastructure.h" +static void fwd_to_apps(uint8_t* msg, uint16_t msg_len, int its_msg_type, uint32_t iid) { + FacilitiesIndication_t* fi = NULL; + + fi = calloc(1, sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t *fmi = &fi->choice.message; + + fmi->id = iid; + + fmi->itsMessageType = its_msg_type; + + fmi->data.size = msg_len; + fmi->data.buf = malloc(msg_len); + memcpy(fmi->data.buf, msg, msg_len); + + uint8_t buffer[ITSS_SDU_MAX_LEN]; + buffer[0] = 4; // Facilities + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1); + + itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, iid, "FI.message"); +} + int facilities_request_result_accepted(void *responder) { int rv = 0; @@ -767,10 +789,11 @@ int transport_indication_btp(BTPPacketIndication_t* bpi, void** security_socket) break; case 2043: + fwd_to_apps(bpi->data.buf, bpi->data.size, bpi->destinationPort, bpi->id); + fwd = false; if (facilities.coordination.active) { vcm_check(its_msg); } - fwd = true; break; case Port_poi: @@ -795,23 +818,7 @@ int transport_indication_btp(BTPPacketIndication_t* bpi, void** security_socket) // Forward to [applications] if (fwd) { - fi = calloc(1, sizeof(FacilitiesIndication_t)); - fi->present = FacilitiesIndication_PR_message; - FacilitiesMessageIndication_t *fmi = &fi->choice.message; - - fmi->id = bpi->id; - - fmi->itsMessageType = bpi->destinationPort; - - fmi->data.size = bpi->data.size; - fmi->data.buf = malloc(bpi->data.size); - memcpy(fmi->data.buf, bpi->data.buf, bpi->data.size); - - uint8_t buffer[ITSS_SDU_MAX_LEN]; - buffer[0] = 4; // Facilities - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, buffer + 1, ITSS_SDU_MAX_LEN - 1); - - itss_queue_send(facilities.tx_queue, buffer, enc.encoded + 1, ITSS_APPLICATIONS, bpi->id, "FI.message"); + fwd_to_apps(bpi->data.buf, bpi->data.size, bpi->destinationPort, bpi->id); } // Logging