Faciliites requests code isolation
This commit is contained in:
emanuel
parent
cf8b0dc5fa
commit
c569b2c725
|
|
@ -4,6 +4,7 @@ ADD_EXECUTABLE(it2s-itss-facilities
|
|||
cam.c
|
||||
denm.c
|
||||
infrastructure.c
|
||||
requests.c
|
||||
facilities.c
|
||||
)
|
||||
|
||||
|
|
|
|||
364
src/facilities.c
364
src/facilities.c
|
|
@ -2,8 +2,8 @@
|
|||
#include "cam.h"
|
||||
#include "config.h"
|
||||
#include "denm.h"
|
||||
#include "requests.h"
|
||||
|
||||
#include <camv2/xer_encoder.h>
|
||||
#include <itss-transport/BTPDataRequest.h>
|
||||
#include <itss-transport/BTPDataIndication.h>
|
||||
#include <itss-facilities/FacilitiesDataIndication.h>
|
||||
|
|
@ -142,391 +142,39 @@ cleanup:
|
|||
|
||||
static int facilities_request(facilities_t *facilities, void* responder, uint8_t *msg, uint32_t msg_len) {
|
||||
int rv = 0;
|
||||
int code = 0;
|
||||
|
||||
uint8_t *fdres_oer = NULL;
|
||||
asn_enc_rval_t enc;
|
||||
|
||||
void *its_msg = NULL;
|
||||
asn_TYPE_descriptor_t *its_msg_def = NULL;
|
||||
|
||||
bool managed_msg = false;
|
||||
|
||||
FacilitiesDataRequest_t *fdreq = calloc(1, sizeof(FacilitiesDataRequest_t));
|
||||
FacilitiesDataResult_t *fdres = calloc(1, sizeof(FacilitiesDataResult_t));
|
||||
BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));
|
||||
|
||||
asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_FacilitiesDataRequest, (void**) &fdreq, msg, msg_len);
|
||||
if (dec.code) {
|
||||
syslog_err("[facilities]<- invalid FDRequest received");
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
code = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
switch (fdreq->present) {
|
||||
case FacilitiesDataRequest_PR_singleMessage:
|
||||
;
|
||||
bool fwd = true;
|
||||
|
||||
uint64_t transmission_duration = 0;
|
||||
uint32_t transmission_interval = 0;
|
||||
uint64_t transmission_start = 0;
|
||||
|
||||
switch (fdreq->choice.singleMessage.itssMessageType) {
|
||||
case ItssMessageType_cam:
|
||||
its_msg_def = &asn_DEF_CAM;
|
||||
its_msg = calloc(1, sizeof(CAM_t));
|
||||
bdr->destinationPort = Port_cam;
|
||||
bdr->gnPacketTransportType = PacketTransportType_shb;
|
||||
bdr->gnTrafficClass = 1;
|
||||
|
||||
rv = facilities_request_single_message(facilities, responder, fdreq);
|
||||
break;
|
||||
case ItssMessageType_denm:
|
||||
its_msg_def = &asn_DEF_DENM;
|
||||
its_msg = calloc(1, sizeof(DENM_t));
|
||||
bdr->destinationPort = Port_denm;
|
||||
bdr->gnPacketTransportType = PacketTransportType_gbc;
|
||||
bdr->gnTrafficClass = 2;
|
||||
|
||||
break;
|
||||
case ItssMessageType_ivim:
|
||||
its_msg_def = &asn_DEF_IVIM;
|
||||
its_msg = calloc(1, sizeof(IVIM_t));
|
||||
bdr->destinationPort = Port_ivim;
|
||||
bdr->gnPacketTransportType = PacketTransportType_shb;
|
||||
bdr->gnTrafficClass = 1;
|
||||
|
||||
break;
|
||||
default:
|
||||
syslog_err("[facilities] unrecognized FDRequest message type (%ld)", fdreq->choice.singleMessage.itssMessageType);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
dec = uper_decode_complete(NULL, its_msg_def, (void**) &its_msg, fdreq->choice.singleMessage.data.buf, fdreq->choice.singleMessage.data.size);
|
||||
if (dec.code) {
|
||||
syslog_debug("[facilities] invalid FDRequest %s received", its_msg_def->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
int64_t id = -1;
|
||||
bool is_update = false;
|
||||
|
||||
if (fdreq->choice.singleMessage.itssMessageType == ItssMessageType_denm) {
|
||||
managed_msg = true;
|
||||
|
||||
uint8_t event_type = event_manage(facilities->den, its_msg, &id, NULL);
|
||||
// Do not free its_msg! event_manage takes care of the msg
|
||||
// id will get set to another val if EVENT NEW or UPDATE or CANCELLATION or NEGATION
|
||||
if (event_type != EVENT_NEW &&
|
||||
event_type != EVENT_UPDATE &&
|
||||
event_type != EVENT_CANCELLATION &&
|
||||
event_type != EVENT_NEGATION) {
|
||||
|
||||
fwd = false;
|
||||
}
|
||||
|
||||
if (event_type == EVENT_UPDATE ||
|
||||
event_type == EVENT_CANCELLATION ||
|
||||
event_type == EVENT_NEGATION) {
|
||||
|
||||
is_update = true;
|
||||
}
|
||||
|
||||
if (fwd) {
|
||||
// set stationID
|
||||
pthread_mutex_lock(&facilities->lock);
|
||||
((DENM_t*)its_msg)->header.stationID = facilities->station_id;
|
||||
pthread_mutex_unlock(&facilities->lock);
|
||||
|
||||
// Set only one trace
|
||||
if (facilities->station_type != 15) {
|
||||
pthread_mutex_lock(&facilities->lightship->lock);
|
||||
if (facilities->lightship->pos_history_len > 0) {
|
||||
|
||||
if (!((DENM_t*)its_msg)->denm.location) {
|
||||
((DENM_t*)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t));
|
||||
}
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.count = 1;
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.size = 1 * sizeof(void*);
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.array = malloc(1 * sizeof(void*));
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(PathHistory_t));
|
||||
PathHistory_t* ph = ((DENM_t*)its_msg)->denm.location->traces.list.array[0];
|
||||
|
||||
pos_vector_t** pos_history = facilities->lightship->pos_history;
|
||||
uint16_t pos_history_len = facilities->lightship->pos_history_len;
|
||||
|
||||
ph->list.array = malloc((pos_history_len-1) * sizeof(void*));
|
||||
ph->list.count = pos_history_len-1;
|
||||
ph->list.size = (pos_history_len-1) * sizeof(void*);
|
||||
|
||||
for (int i = 0; i < pos_history_len-1; ++i) {
|
||||
ph->list.array[i] = calloc(1,sizeof(PathPoint_t));
|
||||
|
||||
if (pos_history[i+1]->alt != AltitudeValue_unavailable && pos_history[i]->alt != AltitudeValue_unavailable) {
|
||||
ph->list.array[i]->pathPosition.deltaAltitude = pos_history[i+1]->alt - pos_history[i]->alt;
|
||||
} else {
|
||||
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
|
||||
}
|
||||
|
||||
if (pos_history[i+1]->lat != Latitude_unavailable && pos_history[i]->lat != Latitude_unavailable) {
|
||||
ph->list.array[i]->pathPosition.deltaLatitude = pos_history[i+1]->lat - pos_history[i]->lat;
|
||||
} else {
|
||||
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
|
||||
}
|
||||
|
||||
if (pos_history[i+1]->lon != Longitude_unavailable && pos_history[i]->lon != Longitude_unavailable) {
|
||||
ph->list.array[i]->pathPosition.deltaLongitude = pos_history[i+1]->lon - pos_history[i]->lon;
|
||||
} else {
|
||||
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
|
||||
}
|
||||
|
||||
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
|
||||
*ph->list.array[i]->pathDeltaTime = (pos_history[i]->ts - pos_history[i+1]->ts)/10;
|
||||
}
|
||||
}
|
||||
pthread_mutex_unlock(&facilities->lightship->lock);
|
||||
}
|
||||
|
||||
|
||||
// get, set retransmission, duration
|
||||
if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) {
|
||||
transmission_interval = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.transmissionInterval );
|
||||
|
||||
if ( ((DENM_t*)its_msg)->denm.management.validityDuration ) {
|
||||
transmission_duration = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.validityDuration ) * 1000;
|
||||
} else {
|
||||
transmission_duration = facilities->den->default_event_duration * 1000;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (fdreq->choice.singleMessage.itssMessageType == ItssMessageType_ivim) {
|
||||
managed_msg = true;
|
||||
|
||||
uint8_t service_type = service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &id, NULL);
|
||||
|
||||
if (service_type != SERVICE_NEW &&
|
||||
service_type != SERVICE_UPDATE &&
|
||||
service_type != SERVICE_CANCELLATION &&
|
||||
service_type != SERVICE_NEGATION) {
|
||||
|
||||
fwd = false;
|
||||
}
|
||||
|
||||
if (service_type == SERVICE_UPDATE ||
|
||||
service_type == SERVICE_CANCELLATION ||
|
||||
service_type == SERVICE_NEGATION) {
|
||||
|
||||
is_update = true;
|
||||
}
|
||||
|
||||
if (fwd) {
|
||||
uint64_t valid_to, valid_from;
|
||||
|
||||
if (!((IVIM_t*) its_msg)->ivi.mandatory.validFrom) {
|
||||
struct timespec systemtime;
|
||||
clock_gettime(CLOCK_REALTIME, &systemtime);
|
||||
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
|
||||
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
|
||||
valid_from = now;
|
||||
} else {
|
||||
asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validFrom, &valid_from);
|
||||
}
|
||||
|
||||
if (!((IVIM_t*) its_msg)->ivi.mandatory.validTo) {
|
||||
valid_to = valid_from + facilities->infrastructure->default_service_duration;
|
||||
} else {
|
||||
asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validTo, &valid_to);
|
||||
}
|
||||
|
||||
transmission_start = valid_from;
|
||||
transmission_interval = facilities->infrastructure->replay_interval;
|
||||
transmission_duration = valid_to - valid_from;
|
||||
}
|
||||
}
|
||||
|
||||
if (!facilities->replay) {
|
||||
transmission_interval = 0;
|
||||
transmission_duration = 0;
|
||||
}
|
||||
|
||||
// Respond to [applications]
|
||||
fdres->code = FacilitiesResultCode_accepted;
|
||||
fdres_oer = malloc(16);
|
||||
enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 16);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding FDResult (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
// Forward message to [transport]
|
||||
if (fwd) {
|
||||
if (id != -1) {
|
||||
bdr->id = id;
|
||||
} else {
|
||||
bdr->id = 0;
|
||||
}
|
||||
|
||||
bdr->btpType = BTPType_btpB;
|
||||
|
||||
bdr->data.buf = malloc(2048);
|
||||
enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, bdr->data.buf, 2048);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding ITS message into UPER (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
bdr->data.size = (enc.encoded + 7) / 8;
|
||||
|
||||
bdr->gnDestinationAddress.buf = malloc(6);
|
||||
for (int i = 0; i < 6; ++i) bdr->gnDestinationAddress.buf[i] = 0xff;
|
||||
bdr->gnDestinationAddress.size = 6;
|
||||
|
||||
if (transmission_start) {
|
||||
bdr->gnRepetitionStart = malloc(sizeof(long));
|
||||
*bdr->gnRepetitionStart = transmission_start;
|
||||
}
|
||||
|
||||
if (transmission_interval) {
|
||||
bdr->gnRepetitionInterval = malloc(sizeof(long));
|
||||
*bdr->gnRepetitionInterval = transmission_interval;
|
||||
}
|
||||
|
||||
if (transmission_duration) {
|
||||
bdr->gnMaximumRepetitionTime = malloc(sizeof(long));
|
||||
*bdr->gnMaximumRepetitionTime = transmission_duration;
|
||||
}
|
||||
|
||||
if (is_update) {
|
||||
bdr->gnIsUpdate = malloc(sizeof(long));
|
||||
*bdr->gnIsUpdate = 1;
|
||||
}
|
||||
|
||||
if (facilities->use_security) {
|
||||
bdr->gnSecurityProfile = malloc(sizeof(long));
|
||||
*bdr->gnSecurityProfile = 1;
|
||||
}
|
||||
|
||||
uint8_t bdr_oer[2048];
|
||||
bdr_oer[0] = 4; // [facilities] service id
|
||||
enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer + 1, 2047);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding BDR (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
|
||||
pthread_cond_signal(&facilities->tx_queue->trigger);
|
||||
|
||||
}
|
||||
|
||||
break;
|
||||
case FacilitiesDataRequest_PR_activeEvents:
|
||||
pthread_mutex_lock(&facilities->den->lock);
|
||||
|
||||
fdres->code = FacilitiesResultCode_accepted;
|
||||
fdres->result = calloc(1, sizeof(FacilitiesResult_t));
|
||||
fdres->result->present = FacilitiesResult_PR_events;
|
||||
|
||||
uint16_t nae = facilities->den->n_active_events;
|
||||
|
||||
switch (fdreq->choice.activeEvents) {
|
||||
case EventType_active:
|
||||
fdres->result->choice.events.list.count = nae;
|
||||
fdres->result->choice.events.list.size = nae * sizeof(DENM_t *);
|
||||
fdres->result->choice.events.list.array = malloc(nae * sizeof(DENM_t *));
|
||||
for (int i = 0, j = 0; j < nae; ++i) {
|
||||
if (facilities->den->events[i]->state == EVENT_ACTIVE) {
|
||||
fdres->result->choice.events.list.array[j] = calloc(1, sizeof(ItssMessage_t));
|
||||
fdres->result->choice.events.list.array[j]->itssMessageType = ItssMessageType_denm;
|
||||
fdres->result->choice.events.list.array[j]->data.buf = malloc(2048);
|
||||
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities->den->events[i]->denm, fdres->result->choice.events.list.array[j]->data.buf, 2048);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding DENM for FDResult (%s)", enc.failed_type->name);
|
||||
continue;
|
||||
}
|
||||
fdres->result->choice.events.list.array[j]->data.size = (enc.encoded + 7) / 8;
|
||||
++j;
|
||||
}
|
||||
}
|
||||
rv = facilities_request_active_events(facilities, responder, fdreq);
|
||||
break;
|
||||
default:
|
||||
syslog_err("[facilities] unrecognized FDR event type (%ld)", fdreq->choice.activeEvents);
|
||||
pthread_mutex_unlock(&facilities->den->lock);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
fdres_oer = malloc(16384);
|
||||
enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 16384);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding FDResult (%s)", enc.failed_type->name);
|
||||
pthread_mutex_unlock(&facilities->den->lock);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
pthread_mutex_unlock(&facilities->den->lock);
|
||||
break;
|
||||
case FacilitiesDataRequest_PR_attributeTypes:
|
||||
fdres->code = FacilitiesResultCode_accepted;
|
||||
int nra = fdreq->choice.attributeTypes.list.count;
|
||||
fdres->result = calloc(1, sizeof(FacilitiesResult_t));
|
||||
fdres->result->present = FacilitiesResult_PR_attributes;
|
||||
fdres->result->choice.attributes.list.count = nra;
|
||||
fdres->result->choice.attributes.list.size = sizeof(void*) * nra;
|
||||
fdres->result->choice.attributes.list.array = malloc(sizeof(void*) * nra);
|
||||
for (int j = 0; j < nra; ++j) {
|
||||
switch (*fdreq->choice.attributeTypes.list.array[j]) {
|
||||
case FacilitiesAttributeType_stationId:
|
||||
fdres->result->choice.attributes.list.array[j] = calloc(1, sizeof(FacilitiesAttribute_t) );
|
||||
fdres->result->choice.attributes.list.array[j]->data.size = 8;
|
||||
fdres->result->choice.attributes.list.array[j]->data.buf = malloc(8);
|
||||
pthread_mutex_lock(&facilities->lock);
|
||||
*((uint64_t*) fdres->result->choice.attributes.list.array[j]->data.buf) = facilities->station_id;
|
||||
pthread_mutex_unlock(&facilities->lock);
|
||||
rv = facilities_request_attribute_types(facilities, responder, fdreq);
|
||||
break;
|
||||
default:
|
||||
syslog_debug("[facilities] unrecognized FDR attribute type request");
|
||||
goto cleanup;
|
||||
}
|
||||
}
|
||||
fdres_oer = malloc(256);
|
||||
enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 256);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding FDResult (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
break;
|
||||
default:
|
||||
syslog_err("[facilities] unrecognized FDR type received (%d)", fdreq->present);
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
cleanup:
|
||||
if (rv) {
|
||||
fdres->code = FacilitiesResultCode_rejected;
|
||||
uint8_t fdres_oer[32];
|
||||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 32);
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
}
|
||||
free(fdres_oer);
|
||||
if (its_msg_def && !managed_msg) ASN_STRUCT_FREE(*its_msg_def, its_msg);
|
||||
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataResult, fdres);
|
||||
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataRequest, fdreq);
|
||||
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,428 @@
|
|||
#include "facilities.h"
|
||||
#include "cam.h"
|
||||
#include "denm.h"
|
||||
#include "infrastructure.h"
|
||||
#include "requests.h"
|
||||
|
||||
#include <itss-facilities/FacilitiesDataResult.h>
|
||||
#include <itss-transport/BTPDataRequest.h>
|
||||
|
||||
#include <zmq.h>
|
||||
#include <syslog.h>
|
||||
|
||||
#define syslog_info(msg, ...) syslog(LOG_INFO, msg, ##__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
|
||||
|
||||
int facilities_request_result_accepted(void* responder) {
|
||||
int rv = 0;
|
||||
|
||||
FacilitiesDataResult_t *fdres = calloc(1, sizeof(FacilitiesDataResult_t));
|
||||
fdres->code = FacilitiesResultCode_accepted;
|
||||
uint8_t fdres_oer[32];
|
||||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 32);
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataResult, fdres);
|
||||
return rv;
|
||||
}
|
||||
|
||||
int facilities_request_result_rejected(void* responder) {
|
||||
int rv = 0;
|
||||
|
||||
FacilitiesDataResult_t *fdres = calloc(1, sizeof(FacilitiesDataResult_t));
|
||||
fdres->code = FacilitiesResultCode_rejected;
|
||||
uint8_t fdres_oer[32];
|
||||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 32);
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataResult, fdres);
|
||||
return rv;
|
||||
}
|
||||
|
||||
int facilities_request_single_message(facilities_t* facilities, void* responder, FacilitiesDataRequest_t* fdreq) {
|
||||
int rv = 0;
|
||||
|
||||
BTPDataRequest_t* bdr = calloc(1, sizeof(BTPDataRequest_t));
|
||||
|
||||
void *its_msg = NULL;
|
||||
asn_TYPE_descriptor_t *its_msg_def = NULL;
|
||||
|
||||
bool fwd = true;
|
||||
|
||||
uint64_t transmission_duration = 0;
|
||||
uint32_t transmission_interval = 0;
|
||||
uint64_t transmission_start = 0;
|
||||
|
||||
switch (fdreq->choice.singleMessage.itssMessageType) {
|
||||
case ItssMessageType_cam:
|
||||
its_msg_def = &asn_DEF_CAM;
|
||||
its_msg = calloc(1, sizeof(CAM_t));
|
||||
bdr->destinationPort = Port_cam;
|
||||
bdr->gnPacketTransportType = PacketTransportType_shb;
|
||||
bdr->gnTrafficClass = 1;
|
||||
|
||||
break;
|
||||
case ItssMessageType_denm:
|
||||
its_msg_def = &asn_DEF_DENM;
|
||||
its_msg = calloc(1, sizeof(DENM_t));
|
||||
bdr->destinationPort = Port_denm;
|
||||
bdr->gnPacketTransportType = PacketTransportType_gbc;
|
||||
bdr->gnTrafficClass = 2;
|
||||
|
||||
break;
|
||||
case ItssMessageType_ivim:
|
||||
its_msg_def = &asn_DEF_IVIM;
|
||||
its_msg = calloc(1, sizeof(IVIM_t));
|
||||
bdr->destinationPort = Port_ivim;
|
||||
bdr->gnPacketTransportType = PacketTransportType_shb;
|
||||
bdr->gnTrafficClass = 1;
|
||||
|
||||
break;
|
||||
default:
|
||||
syslog_err("[facilities] unrecognized FDRequest message type (%ld)", fdreq->choice.singleMessage.itssMessageType);
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void**) &its_msg, fdreq->choice.singleMessage.data.buf, fdreq->choice.singleMessage.data.size);
|
||||
if (dec.code) {
|
||||
syslog_debug("[facilities] invalid FDRequest %s received", its_msg_def->name);
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
int64_t id = -1;
|
||||
bool is_update = false;
|
||||
|
||||
int managed_msg = false;
|
||||
if (fdreq->choice.singleMessage.itssMessageType == ItssMessageType_denm) {
|
||||
managed_msg = true;
|
||||
|
||||
uint8_t event_type = event_manage(facilities->den, its_msg, &id, NULL);
|
||||
// Do not free its_msg! event_manage takes care of the msg
|
||||
// id will get set to another val if EVENT NEW or UPDATE or CANCELLATION or NEGATION
|
||||
if (event_type != EVENT_NEW &&
|
||||
event_type != EVENT_UPDATE &&
|
||||
event_type != EVENT_CANCELLATION &&
|
||||
event_type != EVENT_NEGATION) {
|
||||
|
||||
fwd = false;
|
||||
}
|
||||
|
||||
if (event_type == EVENT_UPDATE ||
|
||||
event_type == EVENT_CANCELLATION ||
|
||||
event_type == EVENT_NEGATION) {
|
||||
|
||||
is_update = true;
|
||||
}
|
||||
|
||||
if (fwd) {
|
||||
// set stationID
|
||||
pthread_mutex_lock(&facilities->lock);
|
||||
((DENM_t*)its_msg)->header.stationID = facilities->station_id;
|
||||
pthread_mutex_unlock(&facilities->lock);
|
||||
|
||||
// Set only one trace
|
||||
if (facilities->station_type != 15) {
|
||||
pthread_mutex_lock(&facilities->lightship->lock);
|
||||
if (facilities->lightship->pos_history_len > 0) {
|
||||
|
||||
if (!((DENM_t*)its_msg)->denm.location) {
|
||||
((DENM_t*)its_msg)->denm.location = calloc(1, sizeof(LocationContainer_t));
|
||||
}
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.count = 1;
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.size = 1 * sizeof(void*);
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.array = malloc(1 * sizeof(void*));
|
||||
((DENM_t*)its_msg)->denm.location->traces.list.array[0] = calloc(1, sizeof(PathHistory_t));
|
||||
PathHistory_t* ph = ((DENM_t*)its_msg)->denm.location->traces.list.array[0];
|
||||
|
||||
pos_vector_t** pos_history = facilities->lightship->pos_history;
|
||||
uint16_t pos_history_len = facilities->lightship->pos_history_len;
|
||||
|
||||
ph->list.array = malloc((pos_history_len-1) * sizeof(void*));
|
||||
ph->list.count = pos_history_len-1;
|
||||
ph->list.size = (pos_history_len-1) * sizeof(void*);
|
||||
|
||||
for (int i = 0; i < pos_history_len-1; ++i) {
|
||||
ph->list.array[i] = calloc(1,sizeof(PathPoint_t));
|
||||
|
||||
if (pos_history[i+1]->alt != AltitudeValue_unavailable && pos_history[i]->alt != AltitudeValue_unavailable) {
|
||||
ph->list.array[i]->pathPosition.deltaAltitude = pos_history[i+1]->alt - pos_history[i]->alt;
|
||||
} else {
|
||||
ph->list.array[i]->pathPosition.deltaAltitude = DeltaAltitude_unavailable;
|
||||
}
|
||||
|
||||
if (pos_history[i+1]->lat != Latitude_unavailable && pos_history[i]->lat != Latitude_unavailable) {
|
||||
ph->list.array[i]->pathPosition.deltaLatitude = pos_history[i+1]->lat - pos_history[i]->lat;
|
||||
} else {
|
||||
ph->list.array[i]->pathPosition.deltaLatitude = DeltaLatitude_unavailable;
|
||||
}
|
||||
|
||||
if (pos_history[i+1]->lon != Longitude_unavailable && pos_history[i]->lon != Longitude_unavailable) {
|
||||
ph->list.array[i]->pathPosition.deltaLongitude = pos_history[i+1]->lon - pos_history[i]->lon;
|
||||
} else {
|
||||
ph->list.array[i]->pathPosition.deltaLongitude = DeltaLongitude_unavailable;
|
||||
}
|
||||
|
||||
ph->list.array[i]->pathDeltaTime = calloc(1,sizeof(PathDeltaTime_t));
|
||||
*ph->list.array[i]->pathDeltaTime = (pos_history[i]->ts - pos_history[i+1]->ts)/10;
|
||||
}
|
||||
}
|
||||
pthread_mutex_unlock(&facilities->lightship->lock);
|
||||
}
|
||||
|
||||
|
||||
// get, set retransmission, duration
|
||||
if ( ((DENM_t*)its_msg)->denm.management.transmissionInterval ) {
|
||||
transmission_interval = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.transmissionInterval );
|
||||
|
||||
if ( ((DENM_t*)its_msg)->denm.management.validityDuration ) {
|
||||
transmission_duration = *( (uint32_t*) ((DENM_t*)its_msg)->denm.management.validityDuration ) * 1000;
|
||||
} else {
|
||||
transmission_duration = facilities->den->default_event_duration * 1000;
|
||||
}
|
||||
}
|
||||
}
|
||||
} else if (fdreq->choice.singleMessage.itssMessageType == ItssMessageType_ivim) {
|
||||
managed_msg = true;
|
||||
|
||||
uint8_t service_type = service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &id, NULL);
|
||||
|
||||
if (service_type != SERVICE_NEW &&
|
||||
service_type != SERVICE_UPDATE &&
|
||||
service_type != SERVICE_CANCELLATION &&
|
||||
service_type != SERVICE_NEGATION) {
|
||||
|
||||
fwd = false;
|
||||
}
|
||||
|
||||
if (service_type == SERVICE_UPDATE ||
|
||||
service_type == SERVICE_CANCELLATION ||
|
||||
service_type == SERVICE_NEGATION) {
|
||||
|
||||
is_update = true;
|
||||
}
|
||||
|
||||
if (fwd) {
|
||||
uint64_t valid_to, valid_from;
|
||||
|
||||
if (!((IVIM_t*) its_msg)->ivi.mandatory.validFrom) {
|
||||
struct timespec systemtime;
|
||||
clock_gettime(CLOCK_REALTIME, &systemtime);
|
||||
long now = (long) (systemtime.tv_sec * 1000 + systemtime.tv_nsec / 1E6);
|
||||
now = now - 1072915200000; // Convert EPOCH to 2004/01/01 00:00:000
|
||||
valid_from = now;
|
||||
} else {
|
||||
asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validFrom, &valid_from);
|
||||
}
|
||||
|
||||
if (!((IVIM_t*) its_msg)->ivi.mandatory.validTo) {
|
||||
valid_to = valid_from + facilities->infrastructure->default_service_duration;
|
||||
} else {
|
||||
asn_INTEGER2ulong((INTEGER_t*) ((IVIM_t*) its_msg)->ivi.mandatory.validTo, &valid_to);
|
||||
}
|
||||
|
||||
transmission_start = valid_from;
|
||||
transmission_interval = facilities->infrastructure->replay_interval;
|
||||
transmission_duration = valid_to - valid_from;
|
||||
}
|
||||
}
|
||||
|
||||
if (!facilities->replay) {
|
||||
transmission_interval = 0;
|
||||
transmission_duration = 0;
|
||||
}
|
||||
|
||||
// Respond to [applications]
|
||||
facilities_request_result_accepted(responder);
|
||||
|
||||
// Forward message to [transport]
|
||||
if (fwd) {
|
||||
if (id != -1) {
|
||||
bdr->id = id;
|
||||
} else {
|
||||
bdr->id = 0;
|
||||
}
|
||||
|
||||
bdr->btpType = BTPType_btpB;
|
||||
|
||||
bdr->data.buf = malloc(2048);
|
||||
asn_enc_rval_t enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, bdr->data.buf, 2048);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding ITS message into UPER (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
bdr->data.size = (enc.encoded + 7) / 8;
|
||||
|
||||
bdr->gnDestinationAddress.buf = malloc(6);
|
||||
for (int i = 0; i < 6; ++i) bdr->gnDestinationAddress.buf[i] = 0xff;
|
||||
bdr->gnDestinationAddress.size = 6;
|
||||
|
||||
if (transmission_start) {
|
||||
bdr->gnRepetitionStart = malloc(sizeof(long));
|
||||
*bdr->gnRepetitionStart = transmission_start;
|
||||
}
|
||||
|
||||
if (transmission_interval) {
|
||||
bdr->gnRepetitionInterval = malloc(sizeof(long));
|
||||
*bdr->gnRepetitionInterval = transmission_interval;
|
||||
}
|
||||
|
||||
if (transmission_duration) {
|
||||
bdr->gnMaximumRepetitionTime = malloc(sizeof(long));
|
||||
*bdr->gnMaximumRepetitionTime = transmission_duration;
|
||||
}
|
||||
|
||||
if (is_update) {
|
||||
bdr->gnIsUpdate = malloc(sizeof(long));
|
||||
*bdr->gnIsUpdate = 1;
|
||||
}
|
||||
|
||||
if (facilities->use_security) {
|
||||
bdr->gnSecurityProfile = malloc(sizeof(long));
|
||||
*bdr->gnSecurityProfile = 1;
|
||||
}
|
||||
|
||||
uint8_t bdr_oer[2048];
|
||||
bdr_oer[0] = 4; // [facilities] service id
|
||||
enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer + 1, 2047);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding BDR (%s)", enc.failed_type->name);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
|
||||
pthread_cond_signal(&facilities->tx_queue->trigger);
|
||||
|
||||
}
|
||||
|
||||
cleanup:
|
||||
if (its_msg_def && !managed_msg) ASN_STRUCT_FREE(*its_msg_def, its_msg);
|
||||
ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int facilities_request_active_events(facilities_t* facilities, void* responder, FacilitiesDataRequest_t* fdreq) {
|
||||
int rv = 0;
|
||||
|
||||
FacilitiesDataResult_t* fdres = calloc(1, sizeof(FacilitiesDataResult_t));
|
||||
uint8_t* fdres_oer = NULL;
|
||||
|
||||
pthread_mutex_lock(&facilities->den->lock);
|
||||
|
||||
fdres->code = FacilitiesResultCode_accepted;
|
||||
fdres->result = calloc(1, sizeof(FacilitiesResult_t));
|
||||
fdres->result->present = FacilitiesResult_PR_events;
|
||||
|
||||
uint16_t nae = facilities->den->n_active_events;
|
||||
|
||||
switch (fdreq->choice.activeEvents) {
|
||||
case EventType_active:
|
||||
fdres->result->choice.events.list.count = nae;
|
||||
fdres->result->choice.events.list.size = nae * sizeof(DENM_t *);
|
||||
fdres->result->choice.events.list.array = malloc(nae * sizeof(DENM_t *));
|
||||
for (int i = 0, j = 0; j < nae; ++i) {
|
||||
if (facilities->den->events[i]->state == EVENT_ACTIVE) {
|
||||
fdres->result->choice.events.list.array[j] = calloc(1, sizeof(ItssMessage_t));
|
||||
fdres->result->choice.events.list.array[j]->itssMessageType = ItssMessageType_denm;
|
||||
fdres->result->choice.events.list.array[j]->data.buf = malloc(2048);
|
||||
asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities->den->events[i]->denm, fdres->result->choice.events.list.array[j]->data.buf, 2048);
|
||||
if (enc.encoded == -1) { /* encoding shouldn't fail as all saved DENMs are structurally valid */
|
||||
syslog_err("[facilities] failed encoding DENM for FDResult (%s)", enc.failed_type->name);
|
||||
fdres->result->choice.events.list.array[j]->data.size = 0;
|
||||
continue;
|
||||
}
|
||||
fdres->result->choice.events.list.array[j]->data.size = (enc.encoded + 7) / 8;
|
||||
++j;
|
||||
}
|
||||
}
|
||||
break;
|
||||
default:
|
||||
syslog_err("[facilities] unrecognized FDR event type (%ld)", fdreq->choice.activeEvents);
|
||||
pthread_mutex_unlock(&facilities->den->lock);
|
||||
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
fdres_oer = malloc(32768);
|
||||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 32768);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding FDResult (%s)", enc.failed_type->name);
|
||||
pthread_mutex_unlock(&facilities->den->lock);
|
||||
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
pthread_mutex_unlock(&facilities->den->lock);
|
||||
|
||||
cleanup:
|
||||
free(fdres_oer);
|
||||
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataResult, fdres);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
||||
int facilities_request_attribute_types(facilities_t* facilities, void* responder, FacilitiesDataRequest_t* fdreq) {
|
||||
int rv = 0;
|
||||
|
||||
FacilitiesDataResult_t* fdres = calloc(1, sizeof(FacilitiesDataResult_t));
|
||||
|
||||
fdres->code = FacilitiesResultCode_accepted;
|
||||
int nra = fdreq->choice.attributeTypes.list.count;
|
||||
fdres->result = calloc(1, sizeof(FacilitiesResult_t));
|
||||
fdres->result->present = FacilitiesResult_PR_attributes;
|
||||
fdres->result->choice.attributes.list.count = nra;
|
||||
fdres->result->choice.attributes.list.size = sizeof(void*) * nra;
|
||||
fdres->result->choice.attributes.list.array = malloc(sizeof(void*) * nra);
|
||||
for (int j = 0; j < nra; ++j) {
|
||||
switch (*fdreq->choice.attributeTypes.list.array[j]) {
|
||||
case FacilitiesAttributeType_stationId:
|
||||
fdres->result->choice.attributes.list.array[j] = calloc(1, sizeof(FacilitiesAttribute_t) );
|
||||
fdres->result->choice.attributes.list.array[j]->data.size = 8;
|
||||
fdres->result->choice.attributes.list.array[j]->data.buf = malloc(8);
|
||||
pthread_mutex_lock(&facilities->lock);
|
||||
*((uint64_t*) fdres->result->choice.attributes.list.array[j]->data.buf) = facilities->station_id;
|
||||
pthread_mutex_unlock(&facilities->lock);
|
||||
break;
|
||||
|
||||
default:
|
||||
syslog_debug("[facilities] unrecognized FDR attribute type request");
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
}
|
||||
uint8_t fdres_oer[256];
|
||||
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 256);
|
||||
if (enc.encoded == -1) {
|
||||
syslog_err("[facilities] failed encoding FDResult (%s)", enc.failed_type->name);
|
||||
facilities_request_result_rejected(responder);
|
||||
rv = 1;
|
||||
goto cleanup;
|
||||
}
|
||||
|
||||
zmq_send(responder, fdres_oer, enc.encoded, 0);
|
||||
|
||||
cleanup:
|
||||
ASN_STRUCT_FREE(asn_DEF_FacilitiesDataResult, fdres);
|
||||
|
||||
return rv;
|
||||
}
|
||||
|
|
@ -0,0 +1,14 @@
|
|||
#ifndef FACILITIES_REQUESTS
|
||||
#define FACILITIES_REQUESTS
|
||||
|
||||
#include "facilities.h"
|
||||
#include <itss-facilities/FacilitiesDataRequest.h>
|
||||
|
||||
int facilities_request_result_accepted(void* responder);
|
||||
int facilities_request_result_rejected(void* responder);
|
||||
|
||||
int facilities_request_single_message(facilities_t* facilities, void* responder, FacilitiesDataRequest_t* fdreq);
|
||||
int facilities_request_active_events(facilities_t* facilities, void* responder, FacilitiesDataRequest_t* fdreq);
|
||||
int facilities_request_attribute_types(facilities_t* facilities, void* responder, FacilitiesDataRequest_t* fdreq);
|
||||
|
||||
#endif
|
||||
Loading…
Reference in New Issue