diff --git a/src/config.c b/src/config.c index f8b32ff..444cc32 100644 --- a/src/config.c +++ b/src/config.c @@ -297,6 +297,7 @@ int facilities_config(void* facilities_s) { ++i; syslog_debug("[facilities] [config] loaded protection zone @ (%ld, %ld)", zone->protectedZoneLatitude, zone->protectedZoneLongitude); } else { + syslog_err("[facilities] [config] failure to decode protection zone '%s'", dir->d_name); ASN_STRUCT_FREE(asn_DEF_ProtectedCommunicationZone, zone); } @@ -305,6 +306,55 @@ 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 e503981..66be0d7 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -175,7 +175,7 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8 break; case Port_saem: - saem_check(facilities, facilities->press, its_msg); + saem_check(facilities, &facilities->press, its_msg); break; default: @@ -513,6 +513,11 @@ int main() { if(facilities.dissemination->active) 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); + } + uint8_t buffer[PACKET_MAX_LEN]; syslog_info("[facilities] listening"); uint8_t code; diff --git a/src/facilities.h b/src/facilities.h index 96a39b8..44956c5 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -27,6 +27,8 @@ typedef struct facilities { pthread_t infrastructure_service; pthread_t transmitting; pthread_t cp_service; + pthread_t sa_service; + // ZMQ struct { void* ctx; @@ -55,7 +57,7 @@ typedef struct facilities { dissemination_t* dissemination; // SA - press_t* press; + press_t press; int station_type; bool use_security; diff --git a/src/sa.c b/src/sa.c index aaa9bf2..f23dcda 100644 --- a/src/sa.c +++ b/src/sa.c @@ -1,7 +1,21 @@ #include "sa.h" #include "facilities.h" +#include "infrastructure.h" #include +#include +#include +#include + +#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__VA_ARGS__) +#define syslog_emerg(msg, ...) syslog(LOG_EMERG, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) +#define syslog_err(msg, ...) syslog(LOG_ERR, "%s:%d [" msg "]", __func__, __LINE__, ##__VA_ARGS__) + +#ifndef NDEBUG +#define syslog_debug(msg, ...) syslog(LOG_DEBUG, "%s:%d " msg "", __func__, __LINE__, ##__VA_ARGS__) +#else +#define syslog_debug(msg, ...) +#endif SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { @@ -39,22 +53,22 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { } int index = -1; - for (int a = 0; a < press->announcements_len; ++a) { + for (int a = 0; a < press->available_len; ++a) { /* Check existence through stationID and itsAid */ - if (saem->header.stationID == press->announcements[a]->station_id && - its_aid == press->announcements[a]->its_aid) { + if (saem->header.stationID == press->available[a]->station_id && + its_aid == press->available[a]->its_aid) { index = a; break; } } if (index == -1) { - if (press->announcements_len + 1 < MAX_ANNOUNCEMENTS_LEN) { - press->announcements[press->announcements_len]->its_aid = its_aid; - press->announcements[press->announcements_len]->station_id = saem->header.stationID; - press->announcements[press->announcements_len]->timestamp = it2s_tender_get_clock(&facilities->epv); - index = press->announcements_len; - ++press->announcements_len; + if (press->available_len + 1 < MAX_ANNOUNCEMENTS_LEN) { + press->available[press->available_len]->its_aid = its_aid; + press->available[press->available_len]->station_id = saem->header.stationID; + press->available[press->available_len]->timestamp = it2s_tender_get_clock(&facilities->epv); + index = press->available_len; + ++press->available_len; } } @@ -65,9 +79,13 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { Extension_34P0_t* sie = (Extension_34P0_t*) si->chOptions.extensions->list.array[o]; switch (sie->value.present) { case Extension_34P0__value_PR_TwoDLocation: - press->announcements[index]->position.latitude = sie->value.choice.TwoDLocation.latitude; + 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; @@ -93,15 +111,109 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { void press_init(press_t* press) { - press->announcements_len = 0; + press->available_len = 0; for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) { - press->announcements[i] = calloc(1, sizeof(announcement_t)); + press->available[i] = calloc(1, sizeof(announcement_t)); } } +int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) { + int rv = 0; + + SAEM_t* saem = calloc(1, sizeof(SAEM_t)); + + /* header */ + saem->header.protocolVersion = 2; + saem->header.messageID = ItsPduHeader__messageID_saem; + + pthread_mutex_lock(&facilities->id.lock); + saem->header.stationID = facilities->id.value; + pthread_mutex_unlock(&facilities->id.lock); + + /* sam */ + saem->sam.version = 0; + + saem->sam.body.serviceInfos = calloc(1, sizeof(ServiceInfos_t)); + saem->sam.body.serviceInfos->list.count = facilities->press.providing_len; + saem->sam.body.serviceInfos->list.size = facilities->press.providing_len * sizeof(void*); + saem->sam.body.serviceInfos->list.array = malloc(facilities->press.providing_len * sizeof(void*)); + + 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); + } + + asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len); + if (enc.encoded == -1) { + syslog_err("[facilities] [sa] failure to encode SAEM (%s)", enc.failed_type->name); + rv = 1; + goto cleanup; + } + *b_saem_len = (enc.encoded + 7) / 8; + +cleanup: + ASN_STRUCT_FREE(asn_DEF_SAEM, saem); + + return rv; +} + void *sa_service(void *fc) { facilities_t *facilities = (facilities_t*) fc; + BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); + + bdr->btpType = BTPType_btpB; + + bdr->gnDestinationAddress.buf = malloc(6); + for (int i = 0; i < 6; ++i) { + bdr->gnDestinationAddress.buf[i] = 0xff; + } + bdr->gnDestinationAddress.size = 6; + + bdr->gnPacketTransportType = PacketTransportType_shb; + + bdr->destinationPort = Port_saem; + + bdr->gnTrafficClass = 2; + + bdr->data.buf = malloc(512); + bdr->data.size = 512; + + if (facilities->use_security) { + bdr->gnSecurityProfile = malloc(sizeof(long)); + *bdr->gnSecurityProfile = 1; + } + + uint8_t bdr_oer[1024]; + bdr_oer[0] = 4; // Facilities + + int rv = 0; + + while (!facilities->exit) { + sleep(1); + + rv = mk_saem(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size); + if (rv) { + continue; + } + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 1023); + if (enc.encoded == -1) { + syslog_err("[facilities] encoding BTPDataRequest for SAEM failed"); + continue; + } + + queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); + + lightship_reset_timer(facilities->lightship, &facilities->epv); + } + + ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr); + + return NULL; } diff --git a/src/sa.h b/src/sa.h index 15eb919..85b8ab7 100644 --- a/src/sa.h +++ b/src/sa.h @@ -21,8 +21,12 @@ typedef struct announcement { } announcement_t; typedef struct press { - announcement_t* announcements[MAX_ANNOUNCEMENTS_LEN]; - uint16_t announcements_len; + announcement_t* available[MAX_ANNOUNCEMENTS_LEN]; + uint16_t available_len; + + ServiceInfo_t* providing[16]; + uint8_t providing_len; + } press_t; typedef enum SAEM_CODE { @@ -34,3 +38,5 @@ typedef enum SAEM_CODE { void press_init(press_t* press); SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem); + +void* sa_service(void* fc);