449 lines
20 KiB
C
449 lines
20 KiB
C
#include "saem.h"
|
|
#include "facilities.h"
|
|
#include "infrastructure.h"
|
|
#include "tpm.h"
|
|
|
|
#include <it2s-tender/time.h>
|
|
#include <it2s-tender/geodesy.h>
|
|
#include <it2s-tender/syslog.h>
|
|
#include <it2s-tender/space.h>
|
|
#include <itss-transport/TransportRequest.h>
|
|
#include <itss-security/SecurityRequest.h>
|
|
#include <itss-security/SecurityReply.h>
|
|
#include <saem/asn_application.h>
|
|
#include <saem/per_decoder.h>
|
|
#include <saem/per_encoder.h>
|
|
#include <tpm/TollingPaymentInfo.h>
|
|
#include <saem/SAEM.h>
|
|
|
|
|
|
SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem, uint8_t* neighbour) {
|
|
|
|
facilities_t* facilities = (facilities_t*) fc;
|
|
|
|
int rv = 0;
|
|
|
|
if (saem->header.messageID != messageID_saem) {
|
|
return SAEM_INVALID_HEADER_MESSAGE_ID;
|
|
}
|
|
|
|
if (saem->header.protocolVersion != 1) {
|
|
return SAEM_INVALID_HEADER_VERSION;
|
|
}
|
|
|
|
pthread_mutex_lock(&bulletin->lock);
|
|
|
|
if (saem->sam.body.serviceInfos) {
|
|
for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) {
|
|
ServiceInfo_t* si = saem->sam.body.serviceInfos->list.array[i];
|
|
|
|
uint16_t its_aid = si->serviceID;
|
|
|
|
int index = -1;
|
|
|
|
for (int a = 0; a < bulletin->to_consume_len; ++a) {
|
|
// TODO allow different services for same itsAid and stationID
|
|
|
|
/* Check existence through stationID and itsAid */
|
|
if (saem->header.stationID == bulletin->to_consume[a]->station_id &&
|
|
its_aid == bulletin->to_consume[a]->its_aid) {
|
|
index = a;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (index == -1) {
|
|
if (bulletin->to_consume_len + 1 < MAX_ANNOUNCEMENTS_LEN) {
|
|
bulletin->to_consume[bulletin->to_consume_len]->its_aid = its_aid;
|
|
bulletin->to_consume[bulletin->to_consume_len]->station_id = saem->header.stationID;
|
|
bulletin->to_consume[bulletin->to_consume_len]->timestamp = it2s_tender_get_clock(&facilities->epv);
|
|
|
|
if (si->chOptions.extensions) {
|
|
for (int e = 0; e < si->chOptions.extensions->list.count; ++e) {
|
|
switch (si->chOptions.extensions->list.array[e]->present) {
|
|
case ServiceInfoExt_PR_providerServiceContext:
|
|
bulletin->to_consume[bulletin->to_provide_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size;
|
|
bulletin->to_consume[bulletin->to_provide_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size);
|
|
memcpy(bulletin->to_consume[bulletin->to_provide_len]->info.context,
|
|
si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf,
|
|
si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size
|
|
);
|
|
break;
|
|
|
|
case ServiceInfoExt_PR_applicationDataSAM:
|
|
bulletin->to_consume[bulletin->to_provide_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size;
|
|
bulletin->to_consume[bulletin->to_provide_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size);
|
|
memcpy(bulletin->to_consume[bulletin->to_provide_len]->info.data,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
|
|
);
|
|
|
|
if (facilities->station_type != 15 && facilities->tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) {
|
|
asn_dec_rval_t dec = uper_decode_complete(
|
|
NULL,
|
|
&asn_DEF_TollingPaymentInfo,
|
|
(void**) &facilities->tolling.infos.z[facilities->tolling.infos.length],
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
|
|
);
|
|
syslog_err("dec.code:: %d", dec.code);
|
|
if (!dec.code) {
|
|
bulletin->to_consume[bulletin->to_provide_len]->info.internal_p = facilities->tolling.infos.z[facilities->tolling.infos.length];
|
|
++facilities->tolling.infos.length;
|
|
} else {
|
|
ASN_STRUCT_FREE(asn_DEF_TollingPaymentInfo, facilities->tolling.infos.z[facilities->tolling.infos.length]);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case ServiceInfoExt_PR_addressIPv6:
|
|
memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16);
|
|
break;
|
|
|
|
case ServiceInfoExt_PR_servicePort:
|
|
bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
if (neighbour) {
|
|
bulletin->to_consume[bulletin->to_consume_len]->certificate_id = malloc(8);
|
|
memcpy(bulletin->to_consume[bulletin->to_consume_len]->certificate_id, neighbour, 8);
|
|
}
|
|
|
|
index = bulletin->to_consume_len;
|
|
++bulletin->to_consume_len;
|
|
}
|
|
}
|
|
|
|
uint16_t ci_index = si->channelIndex;
|
|
|
|
// TODO channelInfos
|
|
if (saem->sam.body.channelInfos) {
|
|
if (saem->sam.body.channelInfos->list.count >= ci_index + 1) {
|
|
}
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
pthread_mutex_unlock(&bulletin->lock);
|
|
|
|
return rv;
|
|
}
|
|
|
|
void bulletin_init(bulletin_t* bulletin) {
|
|
|
|
pthread_mutex_init(&bulletin->lock, NULL);
|
|
|
|
bulletin->to_consume_len = 0;
|
|
for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) {
|
|
bulletin->to_consume[i] = calloc(1, sizeof(announcement_t));
|
|
}
|
|
|
|
}
|
|
|
|
int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
|
|
int rv = 0;
|
|
|
|
// Check tolling advertisements
|
|
if (!facilities->tolling.infos.length) {
|
|
return 1;
|
|
}
|
|
|
|
asn_enc_rval_t enc;
|
|
|
|
SAEM_t* saem = calloc(1, sizeof(SAEM_t));
|
|
|
|
/* header */
|
|
saem->header.protocolVersion = 1;
|
|
saem->header.messageID = messageID_saem;
|
|
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
saem->header.stationID = facilities->id.station_id;
|
|
|
|
/* sam */
|
|
saem->sam.version = 0;
|
|
|
|
saem->sam.body.serviceInfos = calloc(1, sizeof(ServiceInfos_t));
|
|
saem->sam.body.serviceInfos->list.count = facilities->bulletin.to_provide_len;
|
|
saem->sam.body.serviceInfos->list.size = facilities->bulletin.to_provide_len * sizeof(void*);
|
|
saem->sam.body.serviceInfos->list.array = malloc(facilities->bulletin.to_provide_len * sizeof(void*));
|
|
|
|
uint8_t buf[1024];
|
|
for (int i = 0; i < facilities->bulletin.to_provide_len; ++i) {
|
|
saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t));
|
|
saem->sam.body.serviceInfos->list.array[i]->serviceID = facilities->bulletin.to_provide[i]->its_aid;
|
|
|
|
saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t));
|
|
|
|
ServiceInfoExts_t* exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions;
|
|
|
|
switch (facilities->tolling.protocol) {
|
|
case TOLLING_PROTOCOL_SIMPLE:
|
|
exts->list.count = 3;
|
|
exts->list.size = 3 * sizeof(void*);
|
|
exts->list.array = malloc(3 * sizeof(void*));
|
|
|
|
exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext;
|
|
|
|
char ctx_s[] = "tolling:simple";
|
|
exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_s);
|
|
exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_s));
|
|
memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_s, strlen(ctx_s));
|
|
|
|
exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[1]->present = ServiceInfoExt_PR_servicePort;
|
|
exts->list.array[1]->choice.servicePort = 7011;
|
|
|
|
exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[2]->present = ServiceInfoExt_PR_applicationDataSAM;
|
|
exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024);
|
|
|
|
enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities->tolling.infos.z[0], exts->list.array[2]->choice.applicationDataSAM.buf, 1024);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
|
|
exts->list.array[2]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8;
|
|
break;
|
|
|
|
case TOLLING_PROTOCOL_TLS:
|
|
exts->list.count = 4;
|
|
exts->list.size = 4 * sizeof(void*);
|
|
exts->list.array = malloc(4 * sizeof(void*));
|
|
|
|
exts->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[0]->present = ServiceInfoExt_PR_providerServiceContext;
|
|
char ctx_t[] = "tolling:tls";
|
|
exts->list.array[0]->choice.providerServiceContext.size = strlen(ctx_t);
|
|
exts->list.array[0]->choice.providerServiceContext.buf = malloc(strlen(ctx_t));
|
|
memcpy(exts->list.array[0]->choice.providerServiceContext.buf, ctx_t, strlen(ctx_t));
|
|
|
|
exts->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[1]->present = ServiceInfoExt_PR_addressIPv6;
|
|
exts->list.array[1]->choice.addressIPv6.size = 16;
|
|
exts->list.array[1]->choice.addressIPv6.buf = malloc(16);
|
|
memcpy(exts->list.array[1]->choice.addressIPv6.buf, facilities->id.ipv6_addr, 16);
|
|
|
|
exts->list.array[2] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[2]->present = ServiceInfoExt_PR_servicePort;
|
|
exts->list.array[2]->choice.servicePort = 7011;
|
|
|
|
exts->list.array[3] = calloc(1, sizeof(ServiceInfoExt_t));
|
|
exts->list.array[3]->present = ServiceInfoExt_PR_applicationDataSAM;
|
|
exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024);
|
|
|
|
enc = uper_encode_to_buffer(&asn_DEF_TollingPaymentInfo, NULL, facilities->tolling.infos.z[0], exts->list.array[3]->choice.applicationDataSAM.buf, 1024);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [sa] failure to encode TollingPaymentInfo (%s)", enc.failed_type->name);
|
|
rv = 1;
|
|
goto cleanup;
|
|
}
|
|
|
|
exts->list.array[3]->choice.applicationDataSAM.size = (enc.encoded + 7) / 8;
|
|
break;
|
|
}
|
|
}
|
|
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
|
|
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;
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_SAEM, saem);
|
|
|
|
return rv;
|
|
}
|
|
|
|
void *sa_service(void *fc) {
|
|
facilities_t *facilities = (facilities_t*) fc;
|
|
|
|
pthread_mutex_init(&facilities->bulletin.lock, NULL);
|
|
|
|
TransportRequest_t *tr = calloc(1, sizeof(TransportRequest_t));
|
|
tr->present = TransportRequest_PR_packet;
|
|
TransportPacketRequest_t* tpr = &tr->choice.packet;
|
|
|
|
tpr->present = TransportPacketRequest_PR_btp;
|
|
BTPPacketRequest_t *bpr = &tpr->choice.btp;
|
|
|
|
bulletin_t* bulletin = &facilities->bulletin;
|
|
|
|
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.packetTransportType = PacketTransportType_shb;
|
|
|
|
bpr->destinationPort = Port_saem;
|
|
|
|
bpr->gn.trafficClass = 2;
|
|
|
|
bpr->data.buf = malloc(512);
|
|
bpr->data.size = 512;
|
|
|
|
bpr->gn.securityProfile.sign = true;
|
|
|
|
void* security_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ);
|
|
zmq_connect(security_socket, facilities->zmq.security_address);
|
|
|
|
uint8_t tr_oer[1024];
|
|
tr_oer[0] = 4; // Facilities
|
|
|
|
int rv = 0;
|
|
|
|
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) {
|
|
rv = mk_saem(facilities, bpr->data.buf, (uint32_t *) &bpr->data.size);
|
|
if (!rv) {
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer+1, 1023);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] encoding TR for SAEM failed");
|
|
continue;
|
|
} else {
|
|
queue_add(facilities->tx_queue, tr_oer, enc.encoded+1, 3);
|
|
pthread_cond_signal(&facilities->tx_queue->trigger);
|
|
}
|
|
}
|
|
mk_saem_n_sleep = 0;
|
|
}
|
|
++mk_saem_n_sleep;
|
|
|
|
int32_t lat, lon;
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
lat = facilities->epv.space.latitude;
|
|
lon = facilities->epv.space.longitude;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
pthread_mutex_lock(&bulletin->lock);
|
|
for (int a = 0; a < bulletin->to_consume_len; ++a) {
|
|
/* do some checks, e.g.
|
|
* is advertised service close by?
|
|
* do we want to enjoy the advertised service? */
|
|
|
|
// Tolling
|
|
if (facilities->tolling.enabled &&
|
|
bulletin->to_consume[a]->its_aid == 0 &&
|
|
!bulletin->to_consume[a]->n_trigger &&
|
|
facilities->station_type != 15) {
|
|
|
|
if (!it2s_tender_is_inside_rectangle(
|
|
lat/1e07,
|
|
lon/1e07,
|
|
((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.latitude/1e07,
|
|
((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.longitude/1e07,
|
|
((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.a/10.0,
|
|
((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.b/10.0,
|
|
((TollingPaymentInfo_t*)bulletin->to_consume[a]->info.internal_p)->zone.angle/10.0,
|
|
DCM_HAVERSINE
|
|
)) {
|
|
continue;
|
|
}
|
|
|
|
switch (facilities->tolling.protocol) {
|
|
case TOLLING_PROTOCOL_SIMPLE:
|
|
tpm_pay(facilities, bulletin->to_consume[a]->certificate_id);
|
|
++bulletin->to_consume[a]->n_trigger;
|
|
break;
|
|
|
|
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);
|
|
|
|
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;
|
|
}
|
|
}
|
|
}
|
|
pthread_mutex_unlock(&bulletin->lock);
|
|
|
|
usleep(sleep_ms*1000);
|
|
}
|
|
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
|
|
|
|
return NULL;
|
|
}
|