Simple tolling efforts

src/CMakeLists.txt

@@ -8,6 +8,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
     facilities.c
     cpm.c
     sa.c
+    tpm.c
 )
 
 TARGET_LINK_LIBRARIES(it2s-itss-facilities
@@ -23,6 +24,7 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities
     -lit2s-asn-denmv2
     -lit2s-asn-cpm
     -lit2s-asn-saem
+    -lit2s-asn-tpm
     -lit2s-tender
     -lm
 )

src/config.c

@@ -1,5 +1,6 @@
 #include "config.h"
 #include "facilities.h"
+#include "tpm.h"
 
 #include <syslog.h>
 #include <string.h>
@@ -16,16 +17,6 @@
 #include <itss-security/SecurityRequest.h>
 #include <itss-security/SecurityReply.h>
 
-#define syslog_info(msg, ...) syslog(LOG_INFO, "%s:%d [" msg "]", __func__, __LINE__, ##__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
-
 static int fetch_target_address(char** addresses, uint16_t addresses_len) {
     int index = -1;
 
@@ -310,6 +301,17 @@ int facilities_config(void* facilities_s) {
     facilities->dissemination->ip_radar  = malloc(strlen(config->facilities.cpm.radar_ip)+1);
     strcpy(facilities->dissemination->ip_radar,config->facilities.cpm.radar_ip);
 
+    // TPM
+    facilities->tolling.enabled = config->facilities.tpm.activate;
+    if (!strcmp("simple", config->facilities.tpm.protocol)) {
+        facilities->tolling.protocol = TOLLING_PROTOCOL_SIMPLE;
+    } else if (!strcmp("tls", config->facilities.tpm.protocol)) {
+        facilities->tolling.protocol = TOLLING_PROTOCOL_TLS;
+    } else {
+        syslog_err("[facilities] [config] unrecognized tolling protocol, defaulting to 'simple'");
+        facilities->tolling.protocol = TOLLING_PROTOCOL_SIMPLE;
+    }
+
 
     // Replay
     facilities->replay = config->networking.replay.activate;
@@ -423,7 +425,7 @@ int facilities_config(void* facilities_s) {
     if (config->facilities.saem.activate) {
         facilities->bulletin.to_provide_len = 1;
         facilities->bulletin.to_provide[0] = calloc(1, sizeof(announcement_t));
-        facilities->bulletin.to_provide[0]->endpoint.port = 7777;
+        facilities->bulletin.to_provide[0]->endpoint.port = 7011;
         facilities->bulletin.to_provide[0]->its_aid = config->facilities.saem.service_to_advertise;
     }
 

src/facilities.c

@@ -250,8 +250,8 @@ static int transport_indication(facilities_t *facilities, void* responder, void*
             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->destinationPort = 7011;
+            tpr->sourcePort = 7011;
 
             tpr->gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
             tpr->gn->packetTransportType = PacketTransportType_shb;

src/facilities.h

@@ -11,8 +11,10 @@
 #include "queue.h"
 #include "cpm.h"
 #include "sa.h"
+#include "tpm.h"
 
 #include <it2s-tender/epv.h>
+#include <it2s-tender/syslog.h>
 
 enum ID_CHANGE_STAGE {
     ID_CHANGE_INACTIVE,
@@ -53,12 +55,15 @@ typedef struct facilities {
     // Infrastructure
     infrastructure_t* infrastructure;
 
-    //CPM 
+    // CPM 
     dissemination_t* dissemination;
 
     // SA
     bulletin_t bulletin;
 
+    // TP
+    tolling_s tolling;
+
     int station_type;
     bool use_security;
     bool replay;

src/queue.c

@@ -38,3 +38,29 @@ int queue_add(queue_t* queue, uint8_t *packet, uint16_t packet_len, uint8_t dest
 
     return rv;
 }
+
+int queue_send(queue_t* queue, uint8_t *packet, uint16_t packet_len, uint8_t destination) {
+    int rv = 0;
+
+    pthread_mutex_lock(&queue->lock);
+
+    if (queue->len < QUEUE_MAX_LEN) {
+        queue->packet_len[queue->len] = packet_len;
+        memcpy(queue->packet[queue->len], packet, packet_len);
+        queue->destination[queue->len] = destination;
+        ++queue->len;
+    } else {
+        rv = 1;
+    }
+
+    pthread_mutex_unlock(&queue->lock);
+
+    queue_trigger(queue);
+
+    return rv;
+}
+
+void queue_trigger(queue_t* queue) {
+    pthread_cond_signal(&queue->trigger);
+}
+

src/queue.h

@@ -5,7 +5,7 @@
 #include <stdint.h>
 
 #define QUEUE_MAX_LEN 32
-#define PACKET_MAX_LEN 8192
+#define PACKET_MAX_LEN 32768
 
 typedef struct queue {
     uint8_t **packet;
@@ -18,3 +18,5 @@ typedef struct queue {
 
 queue_t* queue_init();
 int queue_add(queue_t* queue, uint8_t *packet, uint16_t packet_len, uint8_t destination);
+int queue_send(queue_t* queue, uint8_t *packet, uint16_t packet_len, uint8_t destination);
+void queue_trigger(queue_t* queue);

src/sa.c

@@ -148,7 +148,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[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;
+        saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1]->choice.servicePort = 7011;
     }
 
     pthread_mutex_unlock(&facilities->id.lock);
@@ -239,71 +239,82 @@ void *sa_service(void *fc) {
              * is advertised service close by?
              * do we want to enjoy the advertised service? */
     
-            if (bulletin->to_consume[a]->its_aid == 0 && !bulletin->to_consume[a]->n_trigger) {
+            // Tolling
+            if (facilities->tolling.active && 
+                bulletin->to_consume[a]->its_aid == 0 && 
+                !bulletin->to_consume[a]->n_trigger) {
 
-                SecurityRequest_t* sreq = calloc(1, sizeof(SecurityRequest_t));
-                sreq->present = SecurityRequest_PR_tlsSend;
-                sreq->choice.tlsSend.data.buf = malloc(7);
-                sreq->choice.tlsSend.data.size = 7;
-                char hello[] = "Hello!";
-                memcpy(sreq->choice.tlsSend.data.buf, hello, 7);
+                switch (facilities->tolling.protocol) {
+                    case TOLLING_PROTOCOL_SIMPLE:;
+                        tpm_pay(facilities);
+                        ++bulletin->to_consume[a]->n_trigger;
+                        break;
 
-                uint8_t buffer[1024];
-                buffer[0] = 4;
-                asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buffer+1, 1023);
-                ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
-                
-                syslog_debug("[facilities] [sa]-> SecurityRequest.tlsSend ->[security]");
-                zmq_send(security_socket, buffer, enc.encoded, 0);
-                zmq_recv(security_socket, buffer, 1024, 0);
-                syslog_debug("[facilities] [sa]<- SecurityReply <-[security]");
+                    case TOLLING_PROTOCOL_TLS:;
+                        SecurityRequest_t* sreq = calloc(1, sizeof(SecurityRequest_t));
+                        sreq->present = SecurityRequest_PR_tlsSend;
+                        sreq->choice.tlsSend.data.buf = malloc(7);
+                        sreq->choice.tlsSend.data.size = 7;
+                        char hello[] = "Hello!";
+                        memcpy(sreq->choice.tlsSend.data.buf, hello, 7);
 
-                SecurityReply_t* srep = NULL;
-                asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buffer, 1024);
-                
-                if (dec.code || srep->returnCode != SecurityReplyReturnCode_accepted) {
-                    ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
-                    continue; 
+                        uint8_t buffer[1024];
+                        buffer[0] = 4;
+                        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_SecurityRequest, NULL, sreq, buffer+1, 1023);
+                        ASN_STRUCT_FREE(asn_DEF_SecurityRequest, sreq);
+                        
+                        syslog_debug("[facilities] [sa]-> SecurityRequest.tlsSend ->[security]");
+                        zmq_send(security_socket, buffer, enc.encoded, 0);
+                        zmq_recv(security_socket, buffer, 1024, 0);
+                        syslog_debug("[facilities] [sa]<- SecurityReply <-[security]");
+
+                        SecurityReply_t* srep = NULL;
+                        asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_SecurityReply, (void**) &srep, buffer, 1024);
+                        
+                        if (dec.code || srep->returnCode != SecurityReplyReturnCode_accepted) {
+                            ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
+                            continue; 
+                        }
+
+                        TransportRequest_t* tr_etc = calloc(1, sizeof(TransportRequest_t));
+                        tr_etc->present = TransportRequest_PR_packet;
+                        TransportPacketRequest_t* tpr_etc = &tr_etc->choice.packet;
+                        tpr_etc->present = TransportPacketRequest_PR_tcp;
+
+                        tpr_etc->choice.tcp.destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
+                        tpr_etc->choice.tcp.destinationAddress->buf = malloc(16);
+                        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 = 7011;
+
+                        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);
+
+                        ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
+
+                        tpr_etc->choice.tcp.gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
+                        tpr_etc->choice.tcp.gn->packetTransportType = PacketTransportType_shb;
+                        tpr_etc->choice.tcp.gn->destinationAddress.buf = calloc(1, 6);
+                        tpr_etc->choice.tcp.gn->destinationAddress.size = 6;
+
+                        ++bulletin->to_consume[a]->n_trigger;
+
+                        enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr_etc, tr_oer+1, 1023);
+                        if (enc.encoded == -1) {
+                            syslog_err("[facilities] encoding TR for ETC-Req failed");
+                            continue;
+                        } else {
+                            queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
+                            pthread_cond_signal(&facilities->tx_queue->trigger);
+                        }
+
+                        ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr_etc);
+                        break;
                 }
-
-                
-                TransportRequest_t* tr_etc = calloc(1, sizeof(TransportRequest_t));
-                tr_etc->present = TransportRequest_PR_packet;
-                TransportPacketRequest_t* tpr_etc = &tr_etc->choice.packet;
-                tpr_etc->present = TransportPacketRequest_PR_tcp;
-
-                tpr_etc->choice.tcp.destinationAddress = calloc(1, sizeof(OCTET_STRING_t));
-                tpr_etc->choice.tcp.destinationAddress->buf = malloc(16);
-                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);
-
-                ASN_STRUCT_FREE(asn_DEF_SecurityReply, srep);
-
-                tpr_etc->choice.tcp.gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t));
-                tpr_etc->choice.tcp.gn->packetTransportType = PacketTransportType_shb;
-                tpr_etc->choice.tcp.gn->destinationAddress.buf = calloc(1, 6);
-                tpr_etc->choice.tcp.gn->destinationAddress.size = 6;
-
-                ++bulletin->to_consume[a]->n_trigger;
-
-                enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr_etc, tr_oer+1, 1023);
-                if (enc.encoded == -1) {
-                    syslog_err("[facilities] encoding TR for ETC-Req failed");
-                    continue;
-                } else {
-                    queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
-                    pthread_cond_signal(&facilities->tx_queue->trigger);
-                }
-
-                ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr_etc);
             }
         }
         pthread_mutex_unlock(&bulletin->lock);

src/tpm.c

@@ -0,0 +1,198 @@
+#include "tpm.h"
+#include "facilities.h"
+
+#include <itss-transport/TransportRequest.h>
+#include <stdint.h>
+#include <string.h>
+
+int tpm_pay(void* fc) {
+    int rv = 0;
+
+    facilities_t* facilities = (facilities_t*) fc;
+    tolling_s* tolling = (tolling_s*) &facilities->tolling;
+
+    TransportRequest_t* tr = NULL;
+    TollingPaymentMessage_t* tpm = NULL;
+
+    const size_t buf_len = 2048;
+    uint8_t buf[buf_len];
+
+    if (!tolling->enabled) {
+        syslog_debug("[facilities] [tolling] tolling is disabled");
+        goto cleanup;
+    }
+
+    // TPM
+    tpm = calloc(1, sizeof(TollingPaymentMessage_t));
+
+    asn_ulong2INTEGER(&tpm->timestamp, it2s_tender_get_clock(&facilities->epv));
+
+    tolling->active = true;
+
+    tpm->tollingFlow.present = TollingFlow_PR_request;
+    tpm->tollingFlow.choice.request.clientId = rand();
+
+    tolling->nonce = rand();
+    tpm->tollingFlow.choice.request.transactionNonce = tolling->nonce;
+
+
+    // encode TPM 
+    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, &tpm, buf, buf_len);
+    if (enc.encoded == -1) {
+        syslog_err("[facilities] [tolling] error encoding TPM.request (%s)", enc.failed_type->name);
+        rv = 1;
+        goto cleanup;
+    }
+    size_t tpm_uper_len = (enc.encoded + 7) / 8;
+
+    // [transport] request (TR)
+    tr = calloc(1, sizeof(TransportRequest_t));
+    tr->present = TransportRequest_PR_packet;
+    tr->choice.packet.present = TransportPacketRequest_PR_btp;
+    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;
+
+    bpr->data.size = tpm_uper_len;
+    bpr->data.buf = malloc(tpm_uper_len);
+    memcpy(bpr->data.buf, buf, tpm_uper_len);
+
+    bpr->destinationPort = 7011;
+    bpr->btpType = BTPType_btpB;
+    bpr->gn.destinationAddress.buf = malloc(6);
+    for (int i = 0; i < 6; ++i) {
+        bpr->gn.destinationAddress.buf[i] = 0xff;
+    }
+    bpr->gn.destinationAddress.size = 6;
+    bpr->gn.trafficClass = 2;
+    bpr->gn.packetTransportType = PacketTransportType_shb;
+
+    // encode TR
+    buf[0] = 4;
+    enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, buf_len-1);
+    if (enc.encoded == -1) {
+        syslog_err("[facilities] [tolling] error encoding TR TPM.request (%s)", enc.failed_type->name);
+        rv = 1;
+        goto cleanup;
+    }
+
+    queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
+
+cleanup:
+    ASN_STRUCT_FREE(asn_DEF_TollingPaymentMessage, tpm);
+    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
+
+    return rv;
+}
+
+static void rsu_handle_recv(facilities_t* facilities, TollRequest_t* req) {
+
+    const size_t buf_len = 2048;
+    uint8_t buf[buf_len];
+
+    TransportRequest_t* tr = NULL;
+    TollingPaymentMessage_t* tpm = NULL;
+
+    // TPM
+    tpm = calloc(1, sizeof(TollingPaymentMessage_t));
+    asn_ulong2INTEGER(&tpm->timestamp, it2s_tender_get_clock(&facilities->epv));
+
+
+    // encode TPM 
+    asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentMessage, NULL, &tpm, buf, buf_len);
+    if (enc.encoded == -1) {
+        syslog_err("[facilities] [tolling] error encoding TPM.request (%s)", enc.failed_type->name);
+        goto cleanup;
+    }
+    size_t tpm_uper_len = (enc.encoded + 7) / 8;
+
+    // [transport] request (TR)
+    tr = calloc(1, sizeof(TransportRequest_t));
+    tr->present = TransportRequest_PR_packet;
+    tr->choice.packet.present = TransportPacketRequest_PR_btp;
+    BTPPacketRequest_t* bpr = &tr->choice.packet.choice.btp;
+
+    bpr->data.size = tpm_uper_len;
+    bpr->data.buf = malloc(tpm_uper_len);
+    memcpy(bpr->data.buf, buf, tpm_uper_len);
+
+    bpr->destinationPort = 7011;
+    bpr->btpType = BTPType_btpB;
+    bpr->gn.destinationAddress.buf = malloc(6);
+    for (int i = 0; i < 6; ++i) {
+        bpr->gn.destinationAddress.buf[i] = 0xff;
+    }
+    bpr->gn.destinationAddress.size = 6;
+    bpr->gn.trafficClass = 2;
+    bpr->gn.packetTransportType = PacketTransportType_shb;
+
+    // encode TR
+    buf[0] = 4;
+    enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, buf+1, buf_len-1);
+    if (enc.encoded == -1) {
+        syslog_err("[facilities] [tolling] error encoding TR TPM.request (%s)", enc.failed_type->name);
+        goto cleanup;
+    }
+
+    queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
+
+cleanup:
+    ASN_STRUCT_FREE(asn_DEF_TollingPaymentMessage, tpm);
+    ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
+
+    // TODO check clientId
+    // TODO dlt: check balance
+    // TODO dlt: check transaction
+}
+
+static void veh_handle_recv(tolling_s* tolling, TollReply_t* rep) {
+
+    if (rep->clientId != tolling->client_id) {
+        syslog_debug("[facilities] [tolling]<- received TPM.reply clientId different from ego");
+        return;
+    }
+
+    if (rep->transactionNonce != tolling->nonce) {
+        syslog_err("[facilities] [tolling]<- received TPM.reply nonce different from sent request");
+        return;
+    }
+
+    
+
+    tolling->active = false;
+}
+
+int tpm_recv(void* fc, TollingPaymentMessage_t* tpm_rx) {
+    int rv = 0;
+
+    facilities_t* facilities = (facilities_t*) fc;
+    tolling_s* tolling = (tolling_s*) &facilities->tolling;
+
+    if (!tolling->enabled) {
+        syslog_debug("[facilities] [tolling] tolling is disabled");
+        goto cleanup;
+    }
+
+    switch (tpm_rx->tollingFlow.present) {
+        case TollingFlow_PR_request:
+            if (facilities->station_type != 15) {
+                syslog_debug("[facilities] [tolling] received TPM.request, ignoring");
+                goto cleanup;
+            }
+            rsu_handle_recv(facilities, &tpm_rx->tollingFlow.choice.request);
+            break;
+
+        case TollingFlow_PR_reply:
+            if (facilities->station_type == 15) {
+                syslog_debug("[facilities] [tolling] received TPM.reply, ignoring");
+                goto cleanup;
+            }
+            veh_handle_recv(tolling, &tpm_rx->tollingFlow.choice.reply);
+            break;
+
+        default:
+            break;
+    }
+
+cleanup:
+
+    return rv;
+}

src/tpm.h

@@ -0,0 +1,24 @@
+#pragma once
+
+#include <tpm/TollingPaymentMessage.h>
+#include <it2s-tender/time.h>
+#include <stdbool.h>
+
+typedef enum TOLLING_PROTOCOL {
+    TOLLING_PROTOCOL_SIMPLE,
+    TOLLING_PROTOCOL_TLS
+} TOLLING_PROTOCOL_e;
+
+typedef struct tolling {
+    bool enabled;
+    TOLLING_PROTOCOL_e protocol;
+
+    // Vehicles
+    bool active;
+    uint64_t nonce;
+    uint64_t client_id;
+
+} tolling_s;
+
+int tpm_pay(void* fc);
+int tpm_recv(void* fc, TollingPaymentMessage_t* tpm_rx);