From aace2a33db3c1a228250c05a7fc62d737a798c29 Mon Sep 17 00:00:00 2001 From: emanuel Date: Mon, 14 Jun 2021 17:41:16 +0100 Subject: [PATCH] Modded SAEMs --- src/cam.c | 120 ++++++++++++++++++++++++++++++----------------- src/cam.h | 8 ++-- src/config.c | 53 +-------------------- src/facilities.c | 4 +- src/sa.c | 31 ++---------- src/sa.h | 8 +--- 6 files changed, 89 insertions(+), 135 deletions(-) diff --git a/src/cam.c b/src/cam.c index 45b36e3..708b50e 100644 --- a/src/cam.c +++ b/src/cam.c @@ -4,6 +4,7 @@ #include #include #include +#include #include #include @@ -240,30 +241,30 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; - if (facilities->lightship->pz_len > 0) { + if (facilities->lightship->protected_zones.pz_len > 0) { cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU = calloc(1, sizeof(ProtectedCommunicationZonesRSU_t)); ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; - pzs->list.count = facilities->lightship->pz_len; - pzs->list.size = facilities->lightship->pz_len * sizeof(void*); - pzs->list.array = malloc(facilities->lightship->pz_len * sizeof(void*)); - for (int i = 0; i < facilities->lightship->pz_len; ++i) { + pzs->list.count = facilities->lightship->protected_zones.pz_len; + pzs->list.size = facilities->lightship->protected_zones.pz_len * sizeof(void*); + pzs->list.array = malloc(facilities->lightship->protected_zones.pz_len * sizeof(void*)); + for (int i = 0; i < facilities->lightship->protected_zones.pz_len; ++i) { pzs->list.array[i] = calloc(1, sizeof(ProtectedCommunicationZone_t)); - pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->pz[i]->protectedZoneLatitude; - pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->pz[i]->protectedZoneLongitude; - pzs->list.array[i]->protectedZoneType = facilities->lightship->pz[i]->protectedZoneType; + pzs->list.array[i]->protectedZoneLatitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLatitude; + pzs->list.array[i]->protectedZoneLongitude = facilities->lightship->protected_zones.pz[i]->protectedZoneLongitude; + pzs->list.array[i]->protectedZoneType = facilities->lightship->protected_zones.pz[i]->protectedZoneType; - if (facilities->lightship->pz[i]->expiryTime) { - pzs->list.array[i]->expiryTime->size = facilities->lightship->pz[i]->expiryTime->size; - pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->pz[i]->expiryTime->size); - memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->buf, facilities->lightship->pz[i]->expiryTime->size); + if (facilities->lightship->protected_zones.pz[i]->expiryTime) { + pzs->list.array[i]->expiryTime->size = facilities->lightship->protected_zones.pz[i]->expiryTime->size; + pzs->list.array[i]->expiryTime->buf = malloc(facilities->lightship->protected_zones.pz[i]->expiryTime->size); + memcpy(pzs->list.array[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->buf, facilities->lightship->protected_zones.pz[i]->expiryTime->size); } - if (facilities->lightship->pz[i]->protectedZoneID) { + if (facilities->lightship->protected_zones.pz[i]->protectedZoneID) { pzs->list.array[i]->protectedZoneID = malloc(8); - *pzs->list.array[i]->protectedZoneID = *facilities->lightship->pz[i]->protectedZoneID; + *pzs->list.array[i]->protectedZoneID = *facilities->lightship->protected_zones.pz[i]->protectedZoneID; } - if (facilities->lightship->pz[i]->protectedZoneRadius) { + if (facilities->lightship->protected_zones.pz[i]->protectedZoneRadius) { pzs->list.array[i]->protectedZoneRadius = malloc(8); - *pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->pz[i]->protectedZoneRadius; + *pzs->list.array[i]->protectedZoneRadius = *facilities->lightship->protected_zones.pz[i]->protectedZoneRadius; } } } @@ -290,7 +291,7 @@ cleanup: lightship_t* lightship_init() { lightship_t* lightship = (lightship_t*) calloc(1, sizeof(lightship_t)); - lightship->pz = calloc(256 , sizeof(void*)); + lightship->protected_zones.pz = calloc(256 , sizeof(void*)); pthread_mutex_init(&lightship->lock, NULL); lightship->pos_history = malloc(POS_HISTORY_MAX_LEN * sizeof(void*)); @@ -340,16 +341,16 @@ int lightship_check(lightship_t* lightship, it2s_tender_epv_t* epv) { // Remove expired PZs - for (int i = 0; i < lightship->pz_len; ++i) { + for (int i = 0; i < lightship->protected_zones.pz_len; ++i) { uint64_t expiry; - if (lightship->pz[i]->expiryTime) { - asn_INTEGER2ulong(lightship->pz[i]->expiryTime, &expiry); + if (lightship->protected_zones.pz[i]->expiryTime) { + asn_INTEGER2ulong(lightship->protected_zones.pz[i]->expiryTime, &expiry); if (now >= expiry) { - ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->pz[i]); - for (int j = i; j < lightship->pz_len - 1; ++j) { - lightship->pz[j] = lightship->pz[j+1]; + ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, lightship->protected_zones.pz[i]); + for (int j = i; j < lightship->protected_zones.pz_len - 1; ++j) { + lightship->protected_zones.pz[j] = lightship->protected_zones.pz[j+1]; } - --lightship->pz_len; + --lightship->protected_zones.pz_len; } } } @@ -381,6 +382,7 @@ void lightship_reset_timer(lightship_t* lightship, it2s_tender_epv_t* epv) { enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, it2s_tender_epv_t* epv, uint8_t* ssp, uint32_t ssp_len) { int rv = 0; + facilities_t* facilities = (facilities_t*) fc; lightship_t *lightship = ((facilities_t*) fc)->lightship; uint64_t now = it2s_tender_get_clock(epv); @@ -518,39 +520,69 @@ enum CAM_CHECK_R check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, it2s_ if (cam->cam.camParameters.basicContainer.stationType == StationType_roadSideUnit && cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU) { ProtectedCommunicationZonesRSU_t *pzs = cam->cam.camParameters.highFrequencyContainer.choice.rsuContainerHighFrequency.protectedCommunicationZonesRSU; - if (pzs->list.count > 0 && pzs->list.count + lightship->pz_len < 255) { + if (pzs->list.count > 0 && pzs->list.count + lightship->protected_zones.pz_len < 255) { + bool new_pz = false; for (int k = 0; k < pzs->list.count; ++k) { bool found = false; - for (int j = 0; j < lightship->pz_len; ++j) { - if (lightship->pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude && - lightship->pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) { + for (int j = 0; j < lightship->protected_zones.pz_len; ++j) { + if (lightship->protected_zones.pz[j]->protectedZoneLatitude == pzs->list.array[k]->protectedZoneLatitude && + lightship->protected_zones.pz[j]->protectedZoneLongitude == pzs->list.array[k]->protectedZoneLongitude) { found = true; break; } } if (found) continue; - lightship->pz[lightship->pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t)); - lightship->pz[lightship->pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude; - lightship->pz[lightship->pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude; - lightship->pz[lightship->pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType; + new_pz = true; + + lightship->protected_zones.pz[lightship->protected_zones.pz_len] = calloc(1, sizeof(ProtectedCommunicationZone_t)); + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLatitude = pzs->list.array[k]->protectedZoneLatitude; + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneLongitude = pzs->list.array[k]->protectedZoneLongitude; + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneType = pzs->list.array[k]->protectedZoneType; if (pzs->list.array[k]->expiryTime) { - lightship->pz[lightship->pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size; - lightship->pz[lightship->pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size); - memcpy(lightship->pz[lightship->pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size); + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->size = pzs->list.array[k]->expiryTime->size; + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf = malloc(pzs->list.array[k]->expiryTime->size); + memcpy(lightship->protected_zones.pz[lightship->protected_zones.pz_len]->expiryTime->buf, pzs->list.array[k]->expiryTime->buf, pzs->list.array[k]->expiryTime->size); } if (pzs->list.array[k]->protectedZoneID) { - lightship->pz[lightship->pz_len]->protectedZoneID = malloc(8); - *lightship->pz[lightship->pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID; + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = malloc(8); + *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneID = *pzs->list.array[k]->protectedZoneID; } if (pzs->list.array[k]->protectedZoneRadius) { - lightship->pz[lightship->pz_len]->protectedZoneRadius = malloc(8); - *lightship->pz[lightship->pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius; + lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = malloc(8); + *lightship->protected_zones.pz[lightship->protected_zones.pz_len]->protectedZoneRadius = *pzs->list.array[k]->protectedZoneRadius; } - ++lightship->pz_len; + ++lightship->protected_zones.pz_len; + } + + // Inform [management] + if (new_pz) { + uint8_t b_oer[512]; + ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t)); + mreq->present = ManagementRequest_PR_attributes; + mreq->choice.attributes.present = ManagementRequestAttributes_PR_set; + mreq->choice.attributes.choice.set.protectedZones = calloc(1, sizeof(struct protectedZones)); + mreq->choice.attributes.choice.set.protectedZones->list.count = lightship->protected_zones.pz_len; + mreq->choice.attributes.choice.set.protectedZones->list.size = lightship->protected_zones.pz_len * sizeof(void*); + mreq->choice.attributes.choice.set.protectedZones->list.array = calloc(lightship->protected_zones.pz_len, sizeof(void*)); + for (int p = 0; p < lightship->protected_zones.pz_len; ++p) { + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, lightship->protected_zones.pz[p], b_oer, 512); + oer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &mreq->choice.attributes.choice.set.protectedZones->list.array[p], b_oer, enc.encoded); + } + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, b_oer, 512); + void* management_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ); + int wait_ms = 1000; + zmq_setsockopt(management_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int)); + zmq_connect(management_socket, facilities->zmq.management_address); + zmq_send(management_socket, b_oer, enc.encoded, 0); + syslog_debug("[facilities] [ca]-> sending MReq.attributes.set.protectedZones to ->[management]"); + uint8_t code; + zmq_recv(management_socket, &code, 1, 0); + zmq_close(management_socket); } } } @@ -571,12 +603,12 @@ static int check_pz(lightship_t *lightship, it2s_tender_epv_t* epv) { pthread_mutex_lock(&lightship->lock); - for (int i = 0; i < lightship->pz_len; ++i) { - double d = haversine(lat, lon, (double) lightship->pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->pz[i]->protectedZoneLongitude/10000000.0); + for (int i = 0; i < lightship->protected_zones.pz_len; ++i) { + double d = haversine(lat, lon, (double) lightship->protected_zones.pz[i]->protectedZoneLatitude/10000000.0, (double) lightship->protected_zones.pz[i]->protectedZoneLongitude/10000000.0); int pz_radius = 50; - if (lightship->pz[i]->protectedZoneRadius) { - pz_radius = *lightship->pz[i]->protectedZoneRadius; + if (lightship->protected_zones.pz[i]->protectedZoneRadius) { + pz_radius = *lightship->protected_zones.pz[i]->protectedZoneRadius; } if (d < pz_radius) { is_inside = true; diff --git a/src/cam.h b/src/cam.h index f26e889..109a537 100644 --- a/src/cam.h +++ b/src/cam.h @@ -69,7 +69,6 @@ typedef struct lightship { pos_vector_t** pos_history; uint16_t pos_history_len; - bool is_vehicle_near; uint64_t last_vehicle; @@ -78,8 +77,11 @@ typedef struct lightship { uint32_t rsu_gen_min; uint32_t rsu_vehicle_permanence; - ProtectedCommunicationZone_t ** pz; - uint16_t pz_len; + struct { + ProtectedCommunicationZone_t ** pz; + uint16_t pz_len; + } protected_zones; + } lightship_t; lightship_t* lightship_init(); diff --git a/src/config.c b/src/config.c index 444cc32..2f9b3f9 100644 --- a/src/config.c +++ b/src/config.c @@ -292,8 +292,8 @@ int facilities_config(void* facilities_s) { asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &zone, pz_xml, size); if (!dec.code) { - facilities->lightship->pz[i] = zone; - ++facilities->lightship->pz_len; + facilities->lightship->protected_zones.pz[i] = zone; + ++facilities->lightship->protected_zones.pz_len; ++i; syslog_debug("[facilities] [config] loaded protection zone @ (%ld, %ld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude); } else { @@ -306,55 +306,6 @@ int facilities_config(void* facilities_s) { } } - // Announcements - if (facilities->station_type == 15) { - - int i = 0; - - //DIR *d = opendir(config->facilities.announcements.path); - DIR *d = opendir("/etc/it2s/announcements"); - struct dirent *dir; - char file[256]; - char si_xml[2048]; - if (d) { - while ((dir = readdir(d)) != NULL && i < 16) { - if (dir->d_name[0] == '.') continue; - //sprintf(file, "%s/%s", config->facilities.announcements.path, dir->d_name); - sprintf(file, "%s/%s", "/etc/it2s/announcements", dir->d_name); - FILE *fp = fopen(file, "r"); - if (!fp) continue; - fseek(fp, 0, SEEK_END); - uint16_t size = ftell(fp); - fseek(fp, 0, SEEK_SET); - if (!size) { - fclose(fp); - continue; - } - if (!fread(si_xml, 1, size, fp)) { - fclose(fp); - continue; - } - fclose(fp); - - ServiceInfo_t *si = calloc(1, sizeof(ServiceInfo_t)); - - asn_dec_rval_t dec = xer_decode(NULL, &asn_DEF_ServiceInfo, (void**) &si, si_xml, size); - if (!dec.code) { - facilities->press.providing[i] = si; - ++facilities->press.providing_len; - ++i; - syslog_debug("[facilities] [config] loaded service announcement '%s'", dir->d_name); - } else { - syslog_err("[facilities] [config] failure to decode service announcement '%s'", dir->d_name); - ASN_STRUCT_FREE(asn_DEF_ServiceInfo, si); - } - - } - closedir(d); - } - } - - pthread_mutex_init(&facilities->epv.space.lock, NULL); pthread_mutex_init(&facilities->epv.time.lock, NULL); facilities->epv.time.resolution = TIME_MILLISECONDS; diff --git a/src/facilities.c b/src/facilities.c index 66be0d7..eefb096 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -514,9 +514,7 @@ int main() { pthread_create(&facilities.cp_service, NULL, cp_service, (void*) &facilities); // SA - if (facilities.press.providing_len > 0) { - pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities); - } + //pthread_create(&facilities.sa_service, NULL, sa_service, (void*) &facilities); uint8_t buffer[PACKET_MAX_LEN]; syslog_info("[facilities] listening"); diff --git a/src/sa.c b/src/sa.c index f23dcda..0db3dbe 100644 --- a/src/sa.c +++ b/src/sa.c @@ -23,7 +23,7 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { int rv = 0; - if (saem->header.messageID != ItsPduHeader__messageID_saem) { + if (saem->header.messageID != messageID_saem) { return SAEM_INVALID_HEADER_MESSAGE_ID; } @@ -74,35 +74,12 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { uint16_t ci_index = si->channelIndex; - // TODO chOptions - for (int o = 0; o < si->chOptions.extensions->list.count; ++o) { - Extension_34P0_t* sie = (Extension_34P0_t*) si->chOptions.extensions->list.array[o]; - switch (sie->value.present) { - case Extension_34P0__value_PR_TwoDLocation: - press->available[index]->position.latitude = sie->value.choice.TwoDLocation.latitude; - press->available[index]->position.longitude = sie->value.choice.TwoDLocation.longitude; - break; - case Extension_34P0__value_PR_ThreeDLocation: - press->available[index]->position.latitude = sie->value.choice.ThreeDLocation.latitude; - press->available[index]->position.longitude = sie->value.choice.ThreeDLocation.longitude; - press->available[index]->position.altitude = sie->value.choice.ThreeDLocation.elevation; - break; - case Extension_34P0__value_PR_RepeatRate: - break; - case Extension_34P0__value_PR_AdvertiserIdentifier: - break; - default: - break; - } - } - // TODO channelInfos if (saem->sam.body.channelInfos) { if (saem->sam.body.channelInfos->list.count >= ci_index + 1) { } } - } } @@ -125,7 +102,7 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) { /* header */ saem->header.protocolVersion = 2; - saem->header.messageID = ItsPduHeader__messageID_saem; + saem->header.messageID = messageID_saem; pthread_mutex_lock(&facilities->id.lock); saem->header.stationID = facilities->id.value; @@ -142,8 +119,8 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) { uint8_t buf[1024]; for (int i = 0; i < facilities->press.providing_len; ++i) { saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t)); - asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ServiceInfo, NULL, facilities->press.providing[i], buf, 1024); - oer_decode(NULL, &asn_DEF_ServiceInfo, (void**) &saem->sam.body.serviceInfos->list.array[i], buf, enc.encoded); + saem->sam.body.serviceInfos->list.array[i]->serviceID.present = VarLengthNumber_PR_content; + saem->sam.body.serviceInfos->list.array[i]->serviceID.choice.content = facilities->press.providing[i]->its_aid; } asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len); diff --git a/src/sa.h b/src/sa.h index 85b8ab7..d36cbce 100644 --- a/src/sa.h +++ b/src/sa.h @@ -12,19 +12,13 @@ typedef struct announcement { uint64_t station_id; uint64_t timestamp; - - struct { - int32_t latitude; - int32_t longitude; - int16_t altitude; - } position; } announcement_t; typedef struct press { announcement_t* available[MAX_ANNOUNCEMENTS_LEN]; uint16_t available_len; - ServiceInfo_t* providing[16]; + announcement_t* providing[16]; uint8_t providing_len; } press_t;