diff --git a/src/cam.c b/src/cam.c index da773d7..b8a992a 100644 --- a/src/cam.c +++ b/src/cam.c @@ -3,6 +3,7 @@ #include #include +#include #include #include @@ -85,7 +86,7 @@ static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_ } -static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) { +static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len, uint8_t *fi_oer, uint32_t *fi_len) { int rv = 0; CAM_t *cam = calloc(1, sizeof(CAM_t)); @@ -289,6 +290,15 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) } *cam_len = (enc.encoded + 7) / 8; + asn_enc_rval_t retval_enc_fi = uper_encode_to_buffer(&asn_DEF_CAM, NULL, cam, fi_oer, 512); + if (retval_enc_fi.encoded == -1) { + syslog_err("[facilities] [ca] failed encoding CAM (%s)", retval_enc_fi.failed_type->name); + rv = 1; + goto cleanup; + } + + *fi_len = ((retval_enc_fi.encoded + 7) / 8); + cleanup: ASN_STRUCT_FREE(asn_DEF_CAM, cam); @@ -660,13 +670,24 @@ void *ca_service(void *fc) { *bpr->gn.securityProfile = 1; } + + // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs + + FacilitiesIndication_t* fi = calloc(1,sizeof(FacilitiesIndication_t)); + fi->present = FacilitiesIndication_PR_message; + FacilitiesMessageIndication_t* fmi = &fi->choice.message; + fmi->itsMessageType = ItsMessageType_cam; + fmi->data.buf = malloc(512); + uint8_t tr_oer[1024]; + uint8_t fi_oer[1024]; tr_oer[0] = 4; // Facilities + fi_oer[0] = 4; while (!facilities->exit) { usleep(1000*50); if (lightship_check(facilities->lightship, &facilities->epv) && facilities->lightship->active) { - rv = mk_cam(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size); + rv = mk_cam(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size, fmi->data.buf, (uint32_t *) &fmi->data.size); if (rv) { continue; } @@ -681,8 +702,19 @@ void *ca_service(void *fc) { continue; } + asn_enc_rval_t enc_fdi = oer_encode_to_buffer(&asn_DEF_FacilitiesIndication, NULL, fi, fi_oer+1, 1023); + if(enc_fdi.encoded == -1){ + syslog_err("[facilities] encoding FI for cam failed"); + continue; + } + + queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3); pthread_cond_signal(&facilities->tx_queue->trigger); + + + queue_add(facilities->tx_queue, fi_oer, enc_fdi.encoded+1, 5); + pthread_cond_signal(&facilities->tx_queue->trigger); lightship_reset_timer(facilities->lightship, &facilities->epv); }