SAEM service

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;

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;

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;

src/sa.c

@@ -1,7 +1,21 @@
 #include "sa.h"
 #include "facilities.h"
+#include "infrastructure.h"
 
 #include <it2s-tender/time.h>
+#include <itss-transport/BTPDataRequest.h>
+#include <saem/SAEM.h>
+#include <syslog.h>
+
+#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 && 
+                if (saem->header.stationID == press->available[a]->station_id && 
-                    its_aid == press->announcements[a]->its_aid) {
+                    its_aid == press->available[a]->its_aid) {
                     index = a;
                     break;
                 }
             }
 
             if (index == -1) {
-                if (press->announcements_len + 1 < MAX_ANNOUNCEMENTS_LEN) {
+                if (press->available_len + 1 < MAX_ANNOUNCEMENTS_LEN) {
-                    press->announcements[press->announcements_len]->its_aid = its_aid;
+                    press->available[press->available_len]->its_aid = its_aid;
-                    press->announcements[press->announcements_len]->station_id = saem->header.stationID;
+                    press->available[press->available_len]->station_id = saem->header.stationID;
-                    press->announcements[press->announcements_len]->timestamp = it2s_tender_get_clock(&facilities->epv);
+                    press->available[press->available_len]->timestamp = it2s_tender_get_clock(&facilities->epv);
-                    index = press->announcements_len;
+                    index = press->available_len;
-                    ++press->announcements_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;
 }

src/sa.h

@@ -21,8 +21,12 @@ typedef struct announcement {
 } announcement_t;
 
 typedef struct press {
-    announcement_t* announcements[MAX_ANNOUNCEMENTS_LEN];
+    announcement_t* available[MAX_ANNOUNCEMENTS_LEN];
-    uint16_t 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);