From 866eb427f5ee1880b334c5b8f6f938dc5739dc68 Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Thu, 2 Sep 2021 16:40:55 +0100
Subject: [PATCH] Load own IPv6 addr on fixed id

---
 src/config.c | 24 ++++++++++++++++++++++++
 src/sa.c     |  6 ++++++
 2 files changed, 30 insertions(+)

diff --git a/src/config.c b/src/config.c
index 8714426..b28c9d9 100644
--- a/src/config.c
+++ b/src/config.c
@@ -256,6 +256,30 @@ int facilities_config(void* facilities_s) {
 
     } else {
         facilities->id.station_id = config->security.identity.station_id;
+
+        uint8_t src_mac[6];
+        unsigned int tmp_uint[6];
+
+        sscanf(config->security.identity.mac_address, "%02x:%02x:%02x:%02x:%02x:%02x",
+                &tmp_uint[0],
+                &tmp_uint[1],
+                &tmp_uint[2],
+                &tmp_uint[3],
+                &tmp_uint[4],
+                &tmp_uint[5]);
+
+        src_mac[0] = (uint8_t)tmp_uint[0];
+        src_mac[1] = (uint8_t)tmp_uint[1];
+        src_mac[2] = (uint8_t)tmp_uint[2];
+        src_mac[3] = (uint8_t)tmp_uint[3];
+        src_mac[4] = (uint8_t)tmp_uint[4];
+        src_mac[5] = (uint8_t)tmp_uint[5];
+
+        memset(facilities->id.ipv6_addr, 0, 8);
+        memcpy(facilities->id.ipv6_addr+8, src_mac, 3);
+        facilities->id.ipv6_addr[11] = 0xff;
+        facilities->id.ipv6_addr[12] = 0xfe;
+        memcpy(facilities->id.ipv6_addr+13, src_mac+3, 3);
     }
 
     // DENM
diff --git a/src/sa.c b/src/sa.c
index 79bec8f..d5abbaf 100644
--- a/src/sa.c
+++ b/src/sa.c
@@ -145,6 +145,7 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
         saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0]->choice.addressIPv6.buf = malloc(16);
         memcpy(saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0]->choice.addressIPv6.buf, facilities->id.ipv6_addr, 16);
 
+
         saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t));
         saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1]->present = ServiceInfoExt_PR_servicePort;
         saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1]->choice.servicePort = 7777;
@@ -213,6 +214,7 @@ void *sa_service(void *fc) {
     int sleep_ms = 100;
     int mk_saem_n_sleep = 0;
 
+
     while (!facilities->exit) {
 
         if (bulletin->to_provide_len && sleep_ms*mk_saem_n_sleep >= 1000) {
@@ -274,6 +276,10 @@ void *sa_service(void *fc) {
                 tpr_etc->choice.tcp.destinationAddress->size = 16;
                 memcpy(tpr_etc->choice.tcp.destinationAddress->buf, bulletin->to_consume[a]->endpoint.ipv6_addr, 16);
 
+
+                tpr_etc->choice.tcp.destinationPort = bulletin->to_consume[a]->endpoint.port;
+                tpr_etc->choice.tcp.sourcePort = 7777;
+
                 tpr_etc->choice.tcp.data.buf = malloc(srep->data->choice.tlsSend.data.size);
                 tpr_etc->choice.tcp.data.size = srep->data->choice.tlsSend.data.size;
                 memcpy(tpr_etc->choice.tcp.data.buf, srep->data->choice.tlsSend.data.buf, srep->data->choice.tlsSend.data.size);