447 lines
19 KiB
C
447 lines
19 KiB
C
#include "saem.h"
|
|
#include "facilities.h"
|
|
#include "infrastructure.h"
|
|
#include "tpm.h"
|
|
|
|
#include <it2s-tender/time.h>
|
|
#include <it2s-tender/space.h>
|
|
#include <it2s-tender/recorder.h>
|
|
#include <it2s-tender/packet.h>
|
|
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
|
|
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
|
|
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
|
|
#include <it2s-asn/etsi-its-v1/tpm/EI1_TollingPaymentInfo.h>
|
|
#include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
|
|
#include <zmq.h>
|
|
|
|
|
|
SAEM_CODE_R saem_check(EI1_SAEM_t* saem, uint8_t* neighbour) {
|
|
|
|
int rv = 0;
|
|
|
|
bulletin_t* bulletin = &facilities.bulletin;
|
|
|
|
if (saem->header.messageID != EI1_messageID_saem) {
|
|
return SAEM_INVALID_HEADER_MESSAGE_ID;
|
|
}
|
|
|
|
if (saem->header.protocolVersion != 1) {
|
|
return SAEM_INVALID_HEADER_VERSION;
|
|
}
|
|
|
|
if (bulletin->to_consume_len >= MAX_ANNOUNCEMENTS_LEN - 1) {
|
|
return SAEM_MAX_ANNOUNCEMENTS_REACHED;
|
|
}
|
|
|
|
pthread_mutex_lock(&bulletin->lock);
|
|
|
|
if (saem->sam.body.serviceInfos) {
|
|
for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) {
|
|
EI1_ServiceInfo_t* si = saem->sam.body.serviceInfos->list.array[i];
|
|
|
|
uint16_t its_aid = si->serviceID;
|
|
|
|
bool new_announcement = false;
|
|
|
|
if (si->chOptions.extensions) {
|
|
|
|
for (int e = 0; e < si->chOptions.extensions->list.count; ++e) {
|
|
switch (si->chOptions.extensions->list.array[e]->present) {
|
|
case EI1_ServiceInfoExt_PR_providerServiceContext:
|
|
bulletin->to_consume[bulletin->to_consume_len]->info.context_len = si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size;
|
|
bulletin->to_consume[bulletin->to_consume_len]->info.context = malloc(si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size);
|
|
memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.context,
|
|
si->chOptions.extensions->list.array[e]->choice.providerServiceContext.buf,
|
|
si->chOptions.extensions->list.array[e]->choice.providerServiceContext.size
|
|
);
|
|
break;
|
|
|
|
case EI1_ServiceInfoExt_PR_applicationDataSAM:
|
|
bulletin->to_consume[bulletin->to_consume_len]->info.data_len = si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size;
|
|
bulletin->to_consume[bulletin->to_consume_len]->info.data = malloc(si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size);
|
|
memcpy(bulletin->to_consume[bulletin->to_consume_len]->info.data,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
|
|
);
|
|
|
|
switch (its_aid) {
|
|
case SAID_ETC:
|
|
if (facilities.station_type != 15 && facilities.tolling.infos.length < TOLLING_INFOS_MAX_LENGTH) {
|
|
EI1_TollingPaymentInfo_t* tpi = NULL;
|
|
asn_dec_rval_t dec = uper_decode_complete(
|
|
NULL,
|
|
&asn_DEF_EI1_TollingPaymentInfo,
|
|
(void**) &tpi,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.buf,
|
|
si->chOptions.extensions->list.array[e]->choice.applicationDataSAM.size
|
|
);
|
|
if (!dec.code) {
|
|
|
|
bool found = false;
|
|
for (int t = 0; t < bulletin->to_consume_len; ++t) {
|
|
if (((tolling_info_t*)bulletin->to_consume[t]->info.internal_p)->asn->id == tpi->id) {
|
|
found = true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
if (!found) {
|
|
facilities.tolling.infos.z[facilities.tolling.infos.length] = tolling_info_new(tpi);
|
|
bulletin->to_consume[bulletin->to_consume_len]->info.internal_p = facilities.tolling.infos.z[facilities.tolling.infos.length];
|
|
++facilities.tolling.infos.length;
|
|
new_announcement = true;
|
|
}
|
|
} else {
|
|
ASN_STRUCT_FREE(asn_DEF_EI1_TollingPaymentInfo, tpi);
|
|
}
|
|
}
|
|
break;
|
|
|
|
case SAID_EVCSN:
|
|
// TODO
|
|
break;
|
|
}
|
|
break;
|
|
|
|
case EI1_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 EI1_ServiceInfoExt_PR_servicePort:
|
|
bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
uint16_t ci_index = si->channelIndex;
|
|
|
|
// TODO channelInfos
|
|
if (saem->sam.body.channelInfos) {
|
|
if (saem->sam.body.channelInfos->list.count >= ci_index + 1) {
|
|
}
|
|
}
|
|
|
|
if (new_announcement && bulletin->to_consume_len < MAX_ANNOUNCEMENTS_LEN - 1) {
|
|
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 = itss_time_get();
|
|
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);
|
|
}
|
|
++bulletin->to_consume_len;
|
|
rv = SAEM_NEW;
|
|
}
|
|
|
|
}
|
|
}
|
|
|
|
pthread_mutex_unlock(&bulletin->lock);
|
|
|
|
return rv == SAEM_NEW ? SAEM_NEW : SAEM_OK;
|
|
}
|
|
|
|
void bulletin_init() {
|
|
|
|
bulletin_t* bulletin = &facilities.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));
|
|
}
|
|
|
|
}
|
|
|
|
/**
|
|
* @brief Make a SAEM.
|
|
* @param b_saem A large enough buffer.
|
|
* @param b_saem_len The encoded SAEM length.
|
|
* @return 0 on success, 1 otherwise.
|
|
*/
|
|
static int mk_saem(uint8_t* b_saem, uint32_t* b_saem_len) {
|
|
int rv = 0;
|
|
|
|
// Check tolling advertisements
|
|
if (!facilities.tolling.infos.length /* || facilites.evcsn.infos.length */) {
|
|
return 1;
|
|
}
|
|
|
|
asn_enc_rval_t enc;
|
|
|
|
EI1_SAEM_t* saem = calloc(1, sizeof(EI1_SAEM_t));
|
|
|
|
/* header */
|
|
saem->header.protocolVersion = 1;
|
|
saem->header.messageID = EI1_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(EI1_ServiceInfos_t));
|
|
saem->sam.body.serviceInfos->list.count = facilities.tolling.infos.length; // + facilities.evcsn.infos.length;
|
|
saem->sam.body.serviceInfos->list.size = facilities.tolling.infos.length * sizeof(void*); // + facilities.evcsn.infos.length * sizeof(void*);
|
|
saem->sam.body.serviceInfos->list.array = malloc(facilities.tolling.infos.length * sizeof(void*)); // + facilities.evcsn.infos.length * sizeof(void*);
|
|
|
|
uint8_t buf[1024];
|
|
int i;
|
|
for (i = 0; i < facilities.tolling.infos.length; ++i) {
|
|
saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(EI1_ServiceInfo_t));
|
|
saem->sam.body.serviceInfos->list.array[i]->serviceID = SAID_ETC;
|
|
|
|
saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions = calloc(1, sizeof(EI1_ServiceInfoExts_t));
|
|
|
|
EI1_ServiceInfoExts_t* exts = saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions;
|
|
|
|
switch (facilities.tolling.protocol.p) {
|
|
case TOLLING_PROTOCOL_GN_SPKI:
|
|
case TOLLING_PROTOCOL_GN_DPKI:
|
|
exts->list.count = 3;
|
|
exts->list.size = 3 * sizeof(void*);
|
|
exts->list.array = malloc(3 * sizeof(void*));
|
|
|
|
exts->list.array[0] = calloc(1, sizeof(EI1_ServiceInfoExt_t));
|
|
exts->list.array[0]->present = EI1_ServiceInfoExt_PR_providerServiceContext;
|
|
|
|
char ctx_s[] = "tolling:gn";
|
|
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(EI1_ServiceInfoExt_t));
|
|
exts->list.array[1]->present = EI1_ServiceInfoExt_PR_servicePort;
|
|
exts->list.array[1]->choice.servicePort = 7011;
|
|
|
|
exts->list.array[2] = calloc(1, sizeof(EI1_ServiceInfoExt_t));
|
|
exts->list.array[2]->present = EI1_ServiceInfoExt_PR_applicationDataSAM;
|
|
exts->list.array[2]->choice.applicationDataSAM.buf = malloc(1024);
|
|
|
|
enc = uper_encode_to_buffer(&asn_DEF_EI1_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[2]->choice.applicationDataSAM.buf, 1024);
|
|
if (enc.encoded == -1) {
|
|
log_error("[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:
|
|
case TOLLING_PROTOCOL_TLS_GN:
|
|
case TOLLING_PROTOCOL_TLS_SHS:
|
|
exts->list.count = 4;
|
|
exts->list.size = 4 * sizeof(void*);
|
|
exts->list.array = malloc(4 * sizeof(void*));
|
|
|
|
exts->list.array[0] = calloc(1, sizeof(EI1_ServiceInfoExt_t));
|
|
exts->list.array[0]->present = EI1_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(EI1_ServiceInfoExt_t));
|
|
exts->list.array[1]->present = EI1_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(EI1_ServiceInfoExt_t));
|
|
exts->list.array[2]->present = EI1_ServiceInfoExt_PR_servicePort;
|
|
exts->list.array[2]->choice.servicePort = 7011;
|
|
|
|
exts->list.array[3] = calloc(1, sizeof(EI1_ServiceInfoExt_t));
|
|
exts->list.array[3]->present = EI1_ServiceInfoExt_PR_applicationDataSAM;
|
|
exts->list.array[3]->choice.applicationDataSAM.buf = malloc(1024);
|
|
|
|
enc = uper_encode_to_buffer(&asn_DEF_EI1_TollingPaymentInfo, NULL, facilities.tolling.infos.z[i]->asn, exts->list.array[3]->choice.applicationDataSAM.buf, 1024);
|
|
if (enc.encoded == -1) {
|
|
log_error("[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;
|
|
}
|
|
}
|
|
|
|
for (int j = 0; j < 0/* + facilities.evcsn.infos.length */; ++j) {
|
|
saem->sam.body.serviceInfos->list.array[i+j] = calloc(1, sizeof(EI1_ServiceInfo_t));
|
|
saem->sam.body.serviceInfos->list.array[i+j]->serviceID = SAID_EVCSN;
|
|
// TODO
|
|
}
|
|
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
|
|
enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_EI1_SAEM, saem, b_saem, *b_saem_len);
|
|
if (enc.encoded == -1) {
|
|
log_error("[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_EI1_SAEM, saem);
|
|
|
|
return rv;
|
|
}
|
|
|
|
void *sa_service() {
|
|
|
|
pthread_mutex_init(&facilities.bulletin.lock, NULL);
|
|
|
|
EIS_NetworkingRequest_t* nr = calloc(1, sizeof(EIS_NetworkingRequest_t));
|
|
nr->present = EIS_NetworkingRequest_PR_packet;
|
|
EIS_NetworkingPacketRequest_t* npr = &nr->choice.packet;
|
|
|
|
npr->network.present = EIS_NetworkingPacketRequestNW_PR_gn;
|
|
npr->network.choice.gn.trafficClass = 2;
|
|
npr->network.choice.gn.destinationAddress.buf = malloc(6);
|
|
for (int i = 0; i < 6; ++i) {
|
|
npr->network.choice.gn.destinationAddress.buf[i] = 0xff;
|
|
}
|
|
npr->network.choice.gn.destinationAddress.size = 6;
|
|
npr->network.choice.gn.packetTransportType = EIS_PacketTransportType_shb;
|
|
npr->network.choice.gn.securityProfile.sign = true;
|
|
|
|
npr->transport.present = EIS_NetworkingPacketRequestTP_PR_btp;
|
|
npr->transport.choice.btp.btpType = EIS_BTPType_btpB;
|
|
npr->transport.choice.btp.destinationPort = EIS_Port_saem;
|
|
|
|
bulletin_t* bulletin = &facilities.bulletin;
|
|
|
|
npr->data.buf = malloc(512);
|
|
npr->data.size = 512;
|
|
|
|
int linger_ms = 0;
|
|
void* security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);
|
|
|
|
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(npr->data.buf, (uint32_t *) &npr->data.size);
|
|
if (!rv) {
|
|
|
|
npr->id = itss_id(npr->data.buf, npr->data.size);
|
|
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_EIS_NetworkingRequest, NULL, nr, tr_oer+1, 1023);
|
|
if (enc.encoded == -1) {
|
|
log_error("encoding TR for SAEM failed");
|
|
continue;
|
|
} else {
|
|
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(tr_oer, enc.encoded+1, ITSS_NETWORKING, npr->id, "NR.packet.btp"));
|
|
// Logging
|
|
if (facilities.logging.dbms) {
|
|
pthread_mutex_lock(&facilities.id.lock);
|
|
uint32_t station_id = facilities.id.station_id;
|
|
pthread_mutex_unlock(&facilities.id.lock);
|
|
itss_db_add(facilities.logging.dbms, station_id, npr->id, true, EI1_messageID_saem, NULL, npr->data.buf, npr->data.size);
|
|
}
|
|
if (facilities.logging.recorder) {
|
|
uint16_t buffer_len = 2048;
|
|
uint8_t buffer[buffer_len];
|
|
int e = itss_management_record_packet_sdu(
|
|
buffer,
|
|
buffer_len,
|
|
npr->data.buf,
|
|
npr->data.size,
|
|
npr->id,
|
|
itss_time_get(),
|
|
ITSS_FACILITIES,
|
|
true);
|
|
if (e != -1) {
|
|
itss_queue_send(facilities.tx_queue, itss_queue_packet_new(buffer, e, ITSS_MANAGEMENT, npr->id, "MReq.packet.set"));
|
|
}
|
|
}
|
|
}
|
|
}
|
|
mk_saem_n_sleep = 0;
|
|
}
|
|
++mk_saem_n_sleep;
|
|
|
|
int32_t lat, lon;
|
|
itss_space_lock();
|
|
itss_space_get();
|
|
lat = epv.space.data.latitude.value;
|
|
lon = epv.space.data.longitude.value;
|
|
itss_space_unlock();
|
|
|
|
uint64_t now = itss_time_get();
|
|
|
|
pthread_mutex_lock(&bulletin->lock);
|
|
for (int a = 0; a < bulletin->to_consume_len; ++a) {
|
|
|
|
// Tolling
|
|
if (facilities.tolling.enabled &&
|
|
bulletin->to_consume[a]->its_aid == 1 &&
|
|
(
|
|
now > bulletin->to_consume[a]->t_trigger + TOLLING_PAYMENT_MIN_PERIOD_MS ||
|
|
((facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_SPKI || facilities.tolling.protocol.p == TOLLING_PROTOCOL_GN_DPKI) && tpm_should_retransmit())
|
|
) &&
|
|
facilities.station_type != 15) {
|
|
|
|
tolling_info_t* info = (tolling_info_t*) bulletin->to_consume[a]->info.internal_p;
|
|
|
|
if (!tpm_is_inside_zone(info)) {
|
|
continue;
|
|
}
|
|
|
|
switch (facilities.tolling.protocol.p) {
|
|
case TOLLING_PROTOCOL_GN_SPKI:
|
|
case TOLLING_PROTOCOL_GN_DPKI:
|
|
tpm_pay(info, &security_socket, bulletin->to_consume[a]->certificate_id, NULL);
|
|
++bulletin->to_consume[a]->n_trigger;
|
|
bulletin->to_consume[a]->t_trigger = now;
|
|
break;
|
|
|
|
case TOLLING_PROTOCOL_TLS:
|
|
case TOLLING_PROTOCOL_TLS_GN:
|
|
case TOLLING_PROTOCOL_TLS_SHS:
|
|
tpm_pay(info, &security_socket, NULL, bulletin->to_consume[a]->endpoint.ipv6_addr);
|
|
++bulletin->to_consume[a]->n_trigger;
|
|
bulletin->to_consume[a]->t_trigger = now;
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
pthread_mutex_unlock(&bulletin->lock);
|
|
|
|
// Tolling management
|
|
if (facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS ||
|
|
facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_GN ||
|
|
facilities.tolling.protocol.p == TOLLING_PROTOCOL_TLS_SHS) {
|
|
tolling_tlsc_mgmt(facilities.tx_queue, &security_socket);
|
|
}
|
|
|
|
itss_usleep(sleep_ms*1000);
|
|
}
|
|
|
|
ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingRequest, nr);
|
|
|
|
itss_0close(security_socket);
|
|
|
|
|
|
return NULL;
|
|
}
|