diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 52be034..e01840d 100644 --- a/src/CMakeLists.txt +++ b/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 ) diff --git a/src/config.c b/src/config.c index 0eaca92..972eaae 100644 --- a/src/config.c +++ b/src/config.c @@ -1,5 +1,6 @@ #include "config.h" #include "facilities.h" +#include "tpm.h" #include #include @@ -16,16 +17,6 @@ #include #include -#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; } diff --git a/src/facilities.c b/src/facilities.c index 2eefdda..eab2587 100644 --- a/src/facilities.c +++ b/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; diff --git a/src/facilities.h b/src/facilities.h index ad37adf..6f8d814 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -11,8 +11,10 @@ #include "queue.h" #include "cpm.h" #include "sa.h" +#include "tpm.h" #include +#include 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; diff --git a/src/queue.c b/src/queue.c index 8d16cce..6094c6e 100644 --- a/src/queue.c +++ b/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); +} + diff --git a/src/queue.h b/src/queue.h index 4fd05b1..0d8d101 100644 --- a/src/queue.h +++ b/src/queue.h @@ -5,7 +5,7 @@ #include #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); diff --git a/src/sa.c b/src/sa.c index a33d5d1..1c742ae 100644 --- a/src/sa.c +++ b/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); diff --git a/src/tpm.c b/src/tpm.c new file mode 100644 index 0000000..3536af0 --- /dev/null +++ b/src/tpm.c @@ -0,0 +1,198 @@ +#include "tpm.h" +#include "facilities.h" + +#include +#include +#include + +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; +} diff --git a/src/tpm.h b/src/tpm.h new file mode 100644 index 0000000..71acc23 --- /dev/null +++ b/src/tpm.h @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +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);