From 4e0bcaeda1bbc28ae5f7532081cf5c6a3ccc41bc Mon Sep 17 00:00:00 2001
From: emanuel <vieira.e@ua.pt>
Date: Thu, 25 Nov 2021 16:27:21 +0000
Subject: [PATCH] tcp replies

---
 src/cam.c        |  2 --
 src/facilities.c | 62 +++++++++++++++++++++++++++++++++++++++++++-----
 src/sa.c         |  1 +
 3 files changed, 57 insertions(+), 8 deletions(-)

diff --git a/src/cam.c b/src/cam.c
index d154132..84212b8 100644
--- a/src/cam.c
+++ b/src/cam.c
@@ -661,7 +661,6 @@ void *ca_service(void *fc) {
         *bpr->gn.securityProfile = 1;
     }
 
-
     // Fill header for FacilitiesIndication and FacilitiesMessageIndication structs
 
     FacilitiesIndication_t* fi = calloc(1,sizeof(FacilitiesIndication_t));
@@ -701,7 +700,6 @@ void *ca_service(void *fc) {
                 continue;
             }
 
-
             queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
             pthread_cond_signal(&facilities->tx_queue->trigger);
 
diff --git a/src/facilities.c b/src/facilities.c
index 94f0360..2eefdda 100644
--- a/src/facilities.c
+++ b/src/facilities.c
@@ -51,6 +51,10 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
     FacilitiesIndication_t *fi = NULL;
     SecurityRequest_t* sreq = NULL;
     SecurityReply_t* srep = NULL;
+    TransportRequest_t* tr = NULL;
+
+    uint16_t buf_len = 2048;
+    uint8_t buf[2048];
 
     TransportIndication_t* ti = calloc(1, sizeof(TransportIndication_t));
 
@@ -202,24 +206,69 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
             sreq->choice.tlsRecv.data.buf = malloc(tpi->choice.tcp.data.size);
             memcpy(sreq->choice.tlsRecv.data.buf, tpi->choice.tcp.data.buf, tpi->choice.tcp.data.size);
 
-            uint8_t b_sdu[1024];
+            uint8_t b_sdu[2048];
             b_sdu[0] = 4;
-            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, b_sdu+1, 1023);
+            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, b_sdu+1, 2047);
 
-            syslog_debug("[facilities]-> SecurityRequest.tlsRecv ->[security]");
+            syslog_debug("[facilities]->[security] SecurityRequest.tlsRecv (%ldB)", enc.encoded+1);
             zmq_send(security_socket, b_sdu, enc.encoded+1, 0);
             int32_t rl = zmq_recv(security_socket, b_sdu, enc.encoded, 0);
-            syslog_debug("[facilities]<- SecurityReply.tlsRecv <-[security]");
+            syslog_debug("[facilities]<-[security] SecurityReply.tlsRecv (%ldB)", enc.encoded);
 
-            asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, b_sdu, rl);
-            printf("recv %d\n", rl); fflush(stdout);
+            if (oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, b_sdu, rl).code) {
+                syslog_err("[facilities] SecurityReply.tlsRecv decode failure");
+                rv = 1;
+                goto cleanup;
+            }
 
+            if (srep->returnCode == SecurityReplyReturnCode_rejected) {
+                syslog_err("[facilities] SecurityReply.tlsRecv rejected");
+                rv = 1;
+                goto cleanup;
+            }
+
+            printf("n=%ld\n",srep->data->choice.tlsRecv.data.size);
             for (int m = 0; m < srep->data->choice.tlsRecv.data.size; ++m) {
                 printf("%02x", srep->data->choice.tlsRecv.data.buf[m]);
             }
             printf("\n");
             fflush(stdout);
 
+            // Forward to [transport]
+            tr = calloc(1, sizeof(TransportRequest_t));
+            tr->present = TransportRequest_PR_packet;
+            tr->choice.packet.present = TransportPacketRequest_PR_tcp;
+            TCPPacketRequest_t* tpr = &tr->choice.packet.choice.tcp;
+            tpr->data.size = srep->data->choice.tlsRecv.data.size;
+            tpr->data.buf = malloc(srep->data->choice.tlsRecv.data.size);
+            memcpy(tpr->data.buf, srep->data->choice.tlsRecv.data.buf, srep->data->choice.tlsRecv.data.size);
+
+            tpr->sourcePort = tpi->choice.tcp.destinationPort;
+            tpr->destinationPort = tpi->choice.tcp.sourcePort;
+
+            tpr->destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
+            tpr->destinationAddress->buf = malloc(16);
+            tpr->destinationAddress->size = 16;
+            memcpy(tpr->destinationAddress->buf, tpi->choice.tcp.sourceAddress->buf, 16);
+            tpr->destinationPort = 7777;
+            tpr->sourcePort = 7777;
+
+            tpr->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
+            tpr->gn->packetTransportType = PacketTransportType_shb;
+            tpr->gn->destinationAddress.buf = calloc(1, 6);
+            tpr->gn->destinationAddress.size = 6;
+
+            buf[0] = 4;
+            enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, buf_len-1);
+            if (enc.encoded == -1) {
+                syslog_err("TransportRequest encoding fail");
+                rv = 1;
+                goto cleanup;
+            }
+
+            queue_add(facilities->tx_queue, buf, enc.encoded+1, 3);
+            pthread_cond_signal(&facilities->tx_queue->trigger);
+
             break;
         case TransportPacketIndication_PR_udp:
             break;
@@ -256,6 +305,7 @@ cleanup:
     ASN_STRUCT_FREE(asn_DEF_FacilitiesIndication, fi);
     ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
     ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
+    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
 
     return rv;
 }
diff --git a/src/sa.c b/src/sa.c
index d5abbaf..a33d5d1 100644
--- a/src/sa.c
+++ b/src/sa.c
@@ -265,6 +265,7 @@ void *sa_service(void *fc) {
                     ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
                     continue; 
                 }
+
                 
                 TransportRequest_t* tr_etc = calloc(1, sizeof(TransportRequest_t));
                 tr_etc->present = TransportRequest_PR_packet;