289 lines
11 KiB
C
289 lines
11 KiB
C
#include "tpm.h"
|
|
#include "facilities.h"
|
|
|
|
#include <it2s-tender/time.h>
|
|
#include <itss-transport/TransportRequest.h>
|
|
#include <it2s-tender/space.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;
|
|
|
|
tolling->tz = it2s_tender_get_clock(&facilities->epv) ;
|
|
|
|
TransportRequest_t* tr = NULL;
|
|
TPM_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;
|
|
}
|
|
|
|
tolling->active = true;
|
|
tolling->nonce = rand();
|
|
|
|
syslog_info("[facilities] [tolling] issuing toll payment > client: %ld | nonce: %ld", tolling->client_id, tolling->nonce);
|
|
|
|
// TPM
|
|
tpm = calloc(1, sizeof(TPM_t));
|
|
|
|
tpm->header.messageID = 117;
|
|
tpm->header.protocolVersion = 0;
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
tpm->header.stationID = facilities->id.station_id;
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
|
|
BasicContainer_t* bc = &tpm->tpm.camParameters.basicContainer;
|
|
tpm->tpm.camParameters.basicContainer.stationType = facilities->station_type;
|
|
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
|
|
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
|
|
|
|
// Set GPS coordinates
|
|
bc->referencePosition.latitude = facilities->epv.space.latitude;
|
|
bc->referencePosition.longitude = facilities->epv.space.longitude;
|
|
uint16_t lat_conf = facilities->epv.space.latitude_conf;
|
|
uint16_t lon_conf = facilities->epv.space.longitude_conf;
|
|
|
|
tpm->tpm.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency;
|
|
BasicVehicleContainerHighFrequency_t* bvc_hf = &tpm->tpm.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency;
|
|
|
|
// Set speed
|
|
bvc_hf->speed.speedValue = facilities->epv.space.speed;
|
|
bvc_hf->speed.speedConfidence = facilities->epv.space.speed_conf;
|
|
|
|
// Set heading
|
|
bvc_hf->heading.headingValue = facilities->epv.space.heading;
|
|
bvc_hf->heading.headingConfidence = facilities->epv.space.heading_conf;
|
|
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
if (lat_conf > lon_conf) {
|
|
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 0;
|
|
} else {
|
|
bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = lon_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = lat_conf;
|
|
bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = 900;
|
|
}
|
|
|
|
bvc_hf->vehicleWidth = facilities->vehicle.width;
|
|
bvc_hf->vehicleLength.vehicleLengthValue = facilities->vehicle.length;
|
|
bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable;
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationValue = LongitudinalAccelerationValue_unavailable;
|
|
bvc_hf->longitudinalAcceleration.longitudinalAccelerationConfidence = AccelerationConfidence_unavailable;
|
|
|
|
bvc_hf->driveDirection = DriveDirection_unavailable;
|
|
bvc_hf->curvature.curvatureValue = CurvatureValue_unavailable;
|
|
bvc_hf->curvature.curvatureConfidence = CurvatureConfidence_unavailable;
|
|
bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable;
|
|
bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable;
|
|
|
|
asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&facilities->epv));
|
|
|
|
tpm->tpm.tollingFlow.present = TollingFlow_PR_request;
|
|
tpm->tpm.tollingFlow.choice.request.clientId = tolling->client_id;
|
|
tpm->tpm.tollingFlow.choice.request.paymentMethod.present = TollPaymentMethod_PR_fiat;
|
|
tpm->tpm.tollingFlow.choice.request.paymentMethod.choice.fiat.fiatId = FiatId_eur;
|
|
tpm->tpm.tollingFlow.choice.request.transactionNonce = tolling->nonce;
|
|
|
|
// encode TPM
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_TPM, 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_TPM, tpm);
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
|
|
return rv;
|
|
}
|
|
|
|
static void rsu_handle_recv(facilities_t* facilities, TollRequest_t* req) {
|
|
|
|
syslog_info("[facilities] [tolling] received toll payment > client: %ld | nonce: %ld", req->clientId, req->transactionNonce);
|
|
|
|
const size_t buf_len = 2048;
|
|
uint8_t buf[buf_len];
|
|
|
|
TransportRequest_t* tr = NULL;
|
|
TPM_t* tpm = NULL;
|
|
|
|
// TPM
|
|
tpm = calloc(1, sizeof(TPM_t));
|
|
|
|
tpm->header.messageID = 117;
|
|
tpm->header.protocolVersion = 0;
|
|
pthread_mutex_lock(&facilities->id.lock);
|
|
tpm->header.stationID = facilities->id.station_id;
|
|
pthread_mutex_unlock(&facilities->id.lock);
|
|
|
|
asn_ulong2INTEGER(&tpm->tpm.timestamp, it2s_tender_get_clock(&facilities->epv));
|
|
|
|
BasicContainer_t* bc = &tpm->tpm.camParameters.basicContainer;
|
|
tpm->tpm.camParameters.basicContainer.stationType = facilities->station_type;
|
|
|
|
it2s_tender_lock_space(&facilities->epv);
|
|
it2s_tender_get_space(&facilities->epv);
|
|
bc->referencePosition.altitude.altitudeValue = facilities->epv.space.altitude;
|
|
bc->referencePosition.altitude.altitudeConfidence = facilities->epv.space.altitude_conf;
|
|
|
|
// Set GPS coordinates
|
|
bc->referencePosition.latitude = facilities->epv.space.latitude;
|
|
bc->referencePosition.longitude = facilities->epv.space.longitude;
|
|
uint16_t lat_conf = facilities->epv.space.latitude_conf;
|
|
uint16_t lon_conf = facilities->epv.space.longitude_conf;
|
|
|
|
tpm->tpm.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency;
|
|
it2s_tender_unlock_space(&facilities->epv);
|
|
|
|
tpm->tpm.tollingFlow.present = TollingFlow_PR_reply;
|
|
tpm->tpm.tollingFlow.choice.reply.clientId = req->clientId;
|
|
tpm->tpm.tollingFlow.choice.reply.transactionNonce = req->transactionNonce;
|
|
|
|
// TODO check clientId
|
|
// TODO dlt: check transaction
|
|
|
|
// encode TPM
|
|
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_TPM, NULL, tpm, buf, buf_len);
|
|
if (enc.encoded == -1) {
|
|
syslog_err("[facilities] [tolling] error encoding TPM.reply (%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.reply (%s)", enc.failed_type->name);
|
|
goto cleanup;
|
|
}
|
|
|
|
queue_send(facilities->tx_queue, buf, enc.encoded+1, 3);
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_TPM, tpm);
|
|
ASN_STRUCT_FREE(asn_DEF_TransportRequest, tr);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
syslog_info("[facilities] [tolling] received tolling payment reply > client: %ld | nonce: %ld | accepted: %s", tolling->client_id, tolling->nonce, rep->confirmationCode == TollConfirmationCode_accepted ? "yes" : "no");
|
|
|
|
tolling->active = false;
|
|
}
|
|
|
|
int tpm_recv(void* fc, TPM_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->tpm.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->tpm.tollingFlow.choice.request);
|
|
break;
|
|
|
|
case TollingFlow_PR_reply:
|
|
if (facilities->station_type == 15) {
|
|
syslog_debug("[facilities] [tolling] received TPM.reply, ignoring");
|
|
goto cleanup;
|
|
}
|
|
syslog_info("[facilities] [tolling] reply took %ld ms", it2s_tender_get_clock(&facilities->epv) - tolling->tz);
|
|
veh_handle_recv(tolling, &tpm_rx->tpm.tollingFlow.choice.reply);
|
|
break;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
|
|
cleanup:
|
|
|
|
return rv;
|
|
}
|