it2s-itss-facilities/src/requests.c

#include "facilities.h"
#include "cam.h"
#include "denm.h"
#include "infrastructure.h"
#include "requests.h"
#include "cpm.h"

#include <itss-facilities/FacilitiesReply.h>
#include <itss-transport/TransportRequest.h>

#include <zmq.h>
#include <syslog.h>
#include <cpm/CPM.h>

#include <it2s-tender/time.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;

    FacilitiesReply_t *fr = calloc(1, sizeof(FacilitiesReply_t));
    fr->present = FacilitiesReply_PR_message;
    fr->choice.message.returnCode = 0;
    uint8_t fr_oer[32];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, fr, fr_oer, 32);
    zmq_send(responder, fr_oer, enc.encoded, 0);

    ASN_STRUCT_FREE(asn_DEF_FacilitiesReply, fr);
    return rv;
}

int facilities_request_result_rejected(void* responder) {
    int rv = 0;

    FacilitiesReply_t *fr = calloc(1, sizeof(FacilitiesReply_t));
    fr->present = FacilitiesReply_PR_message;
    fr->choice.message.returnCode = 1;
    uint8_t fr_oer[32];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, fr, fr_oer, 32);
    zmq_send(responder, fr_oer, enc.encoded, 0);

    ASN_STRUCT_FREE(asn_DEF_FacilitiesReply, fr);
    return rv;
}

int facilities_request_single_message(facilities_t* facilities, void* responder, FacilitiesRequest_t* fr) {
    int rv = 0;

    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;

    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 (fr->choice.message.itsMessageType) {
        case ItsMessageType_cam:
            its_msg_def = &asn_DEF_CAM;
            its_msg = calloc(1, sizeof(CAM_t));
            bpr->destinationPort = Port_cam;
            bpr->gn.packetTransportType = PacketTransportType_shb;
            bpr->gn.trafficClass = 1;

            break;
        case ItsMessageType_denm:
            its_msg_def = &asn_DEF_DENM;
            its_msg = calloc(1, sizeof(DENM_t));
            bpr->destinationPort = Port_denm;
            bpr->gn.packetTransportType = PacketTransportType_gbc;
            bpr->gn.trafficClass = 2;

            break;
        case ItsMessageType_ivim:
            its_msg_def = &asn_DEF_IVIM;
            its_msg = calloc(1, sizeof(IVIM_t));
            bpr->destinationPort = Port_ivim;
            bpr->gn.packetTransportType = PacketTransportType_shb;
            bpr->gn.trafficClass = 1;

            break;

         case ItsMessageType_cpm:
            its_msg_def = &asn_DEF_CPM;
            its_msg = calloc(1, sizeof(CPM_t));
            bpr->destinationPort = Port_cpm;
            bpr->gn.packetTransportType = PacketTransportType_shb;
            bpr->gn.trafficClass = 2;

            break;
        default:
            syslog_err("[facilities] unrecognized FR message type (%ld)", fr->choice.message.itsMessageType);
            facilities_request_result_rejected(responder);
            rv = 1;
            goto cleanup;
    }

    asn_dec_rval_t dec = uper_decode_complete(NULL, its_msg_def, (void**) &its_msg, fr->choice.message.data.buf, fr->choice.message.data.size);
    if (dec.code) {
        syslog_debug("[facilities] invalid FR %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 (fr->choice.message.itsMessageType == ItsMessageType_denm) {
        managed_msg = true;

        uint8_t event_type = event_manage(facilities->den, its_msg, &facilities->epv, &id, NULL, 0); 
        // 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->id.lock);
            ((DENM_t*)its_msg)->header.stationID = facilities->id.station_id;
            pthread_mutex_unlock(&facilities->id.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 = 600 * 1000;
                }
            }
        }
    } else if (fr->choice.message.itsMessageType == ItsMessageType_ivim) {
        managed_msg = true;

        uint8_t service_type = service_eval(facilities->infrastructure, SERVICE_IVI, its_msg, &facilities->epv, &id, NULL, 0); 

        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) {
                valid_from = it2s_tender_get_clock(&facilities->epv);;
            } 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) {
            bpr->id = id;
        } else {
            bpr->id = 0;
        }

        bpr->btpType = BTPType_btpB;

        bpr->data.buf = malloc(2048);
        asn_enc_rval_t enc = uper_encode_to_buffer(its_msg_def, NULL, its_msg, bpr->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;
        }
        bpr->data.size = (enc.encoded + 7) / 8;

        bpr->gn.destinationAddress.buf = malloc(6);
        for (int i = 0; i < 6; ++i) bpr->gn.destinationAddress.buf[i] = 0xff;
        bpr->gn.destinationAddress.size = 6;

        if (transmission_start) {
            bpr->gn.repetitionStart = malloc(sizeof(long));
            *bpr->gn.repetitionStart = transmission_start;
        }

        if (transmission_interval) {
            bpr->gn.repetitionInterval = malloc(sizeof(long));
            *bpr->gn.repetitionInterval = transmission_interval;
        }

        if (transmission_duration) {
            bpr->gn.maximumRepetitionTime = malloc(sizeof(long));
            *bpr->gn.maximumRepetitionTime = transmission_duration;
        }

        if (is_update) {
            bpr->gn.isUpdate = malloc(sizeof(long));
            *bpr->gn.isUpdate = 1;
        }

        bpr->gn.securityProfile.sign = true;

        uint8_t tr_oer[2048];
        tr_oer[0] = 4; // [facilities] service id
        enc = oer_encode_to_buffer(&asn_DEF_TransportRequest, NULL, tr, tr_oer + 1, 2047);
        if (enc.encoded == -1) {
            syslog_err("[facilities] failed encoding TR (%s)", enc.failed_type->name);
            rv = 1;
            goto cleanup;
        }

        queue_add(facilities->tx_queue, tr_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_TransportRequest, tr);

    return rv;
}

int facilities_request_active_episodes(facilities_t* facilities, void* responder, FacilitiesRequest_t* freq) {
    int rv = 0;

    FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t));
    uint8_t* frep_oer = NULL;

    frep->present = FacilitiesReply_PR_data;
    frep->choice.data.present = FacilitiesDataReply_PR_episodes;

    pthread_mutex_lock(&facilities->den->lock);
    pthread_mutex_lock(&facilities->infrastructure->lock);

    uint16_t nae = facilities->den->n_active_events;
    uint16_t nas = facilities->infrastructure->n_active_services;

    uint16_t na = 0;

    for (int e = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) {
        switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) {
            case EpisodeType_denm:
                na += nae;
                break;
            case EpisodeType_ivim:
                na += nas;
                break;
        }
    }

    frep->choice.data.choice.episodes.list.count = na;
    frep->choice.data.choice.episodes.list.size = na * sizeof(void *);
    frep->choice.data.choice.episodes.list.array = malloc(na * sizeof(void *));

    for (int e = 0, j = 0; e < freq->choice.data.choice.activeEpisodes.list.count; ++e) {
        switch (*freq->choice.data.choice.activeEpisodes.list.array[e]) {
            case EpisodeType_denm:
                for (int i = 0, n = 0; n < nae; ++i) {
                    if (facilities->den->events[i]->enabled && facilities->den->events[i]->state == EVENT_ACTIVE) {
                        frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t));
                        frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_denm;
                        frep->choice.data.choice.episodes.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, frep->choice.data.choice.episodes.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);
                            frep->choice.data.choice.episodes.list.array[j]->data.size = 0;
                            continue;
                        }
                        frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8;
                        ++j;
                        ++n;
                    }
                }
                break;
            case EpisodeType_ivim:
                for (int i = 0, n = 0; n < nas; ++i) {
                    if (facilities->infrastructure->services[i]->enabled && facilities->infrastructure->services[i]->state == SERVICE_ACTIVE) {
                        frep->choice.data.choice.episodes.list.array[j] = calloc(1, sizeof(ItsMessage_t));
                        frep->choice.data.choice.episodes.list.array[j]->itsMessageType = ItsMessageType_ivim;
                        frep->choice.data.choice.episodes.list.array[j]->data.buf = malloc(2048);
                        asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_IVIM, NULL, facilities->infrastructure->services[i]->ivim, frep->choice.data.choice.episodes.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 IVIM for FDResult (%s)", enc.failed_type->name);
                            frep->choice.data.choice.episodes.list.array[j]->data.size = 0;
                            continue;
                        }
                        frep->choice.data.choice.episodes.list.array[j]->data.size = (enc.encoded + 7) / 8;
                        ++j;
                        ++n;
                    }
                }
                break;
            default:
                syslog_err("[facilities] unrecognized FR event type (%ld)", *freq->choice.data.choice.activeEpisodes.list.array[e]);
                pthread_mutex_unlock(&facilities->den->lock);
                pthread_mutex_unlock(&facilities->infrastructure->lock);

                facilities_request_result_rejected(responder);
                rv = 1;
                goto cleanup;
        }
    }

    pthread_mutex_unlock(&facilities->den->lock);
    pthread_mutex_unlock(&facilities->infrastructure->lock);

    frep_oer = malloc(32768);
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 32768);
    if (enc.encoded == -1) {
        syslog_err("[facilities] failed encoding FReply (%s)", enc.failed_type->name);

        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    zmq_send(responder, frep_oer, enc.encoded, 0);

cleanup:
    free(frep_oer);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesReply, frep);

    return rv;
}

int facilities_request_attribute_types(facilities_t* facilities, void* responder, FacilitiesRequest_t* freq) {
    int rv = 0;
    
    FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t));
    frep->present = FacilitiesReply_PR_data;

    int nra = freq->choice.data.choice.attributeTypes.list.count;
    frep->choice.data.present = FacilitiesDataReply_PR_attributes;
    frep->choice.data.choice.attributes.list.count = nra;
    frep->choice.data.choice.attributes.list.size = sizeof(void*) * nra;
    frep->choice.data.choice.attributes.list.array = malloc(sizeof(void*) * nra);
    for (int j = 0; j < nra; ++j) {
        switch (*freq->choice.data.choice.attributeTypes.list.array[j]) {
            case FacilitiesAttributeType_stationId:
                frep->choice.data.choice.attributes.list.array[j] = calloc(1, sizeof(FacilitiesAttribute_t) );
                frep->choice.data.choice.attributes.list.array[j]->data.size = 8;
                frep->choice.data.choice.attributes.list.array[j]->data.buf = malloc(8);
                pthread_mutex_lock(&facilities->id.lock);
                *((uint64_t*) frep->choice.data.choice.attributes.list.array[j]->data.buf) = facilities->id.station_id;
                pthread_mutex_unlock(&facilities->id.lock);
                break;

            default:
                syslog_debug("[facilities] unrecognized FR attribute type request");
                facilities_request_result_rejected(responder);
                rv = 1;
                goto cleanup;
        }
    }   
    uint8_t frep_oer[256];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 256);
    if (enc.encoded == -1) {
        syslog_err("[facilities] failed encoding FReply (%s)", enc.failed_type->name);
        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    zmq_send(responder, frep_oer, enc.encoded, 0);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_FacilitiesReply, frep);

    return rv;
}


int facilities_request_loaded_protected_zones(facilities_t* facilities, void* responder, FacilitiesRequest_t* freq) {
    int rv = 0;
    
    FacilitiesReply_t* frep = calloc(1, sizeof(FacilitiesReply_t));

    frep->present = FacilitiesReply_PR_data;
    frep->choice.data.present = FacilitiesDataReply_PR_protectedCommunicationZones;
    
    pthread_mutex_lock(&facilities->lightship->lock);

    frep->choice.data.choice.protectedCommunicationZones.list.count = facilities->lightship->protected_zones.pz_len;
    frep->choice.data.choice.protectedCommunicationZones.list.size = facilities->lightship->protected_zones.pz_len * sizeof(void*);
    frep->choice.data.choice.protectedCommunicationZones.list.array = malloc(facilities->lightship->protected_zones.pz_len * sizeof(void*));

    uint8_t buf[256];
    for (int z = 0; z < facilities->lightship->protected_zones.pz_len; ++z) {
        frep->choice.data.choice.protectedCommunicationZones.list.array[z] = calloc(1, sizeof(ProtectedCommunicationZone_t));
        asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_ProtectedCommunicationZone, NULL, facilities->lightship->protected_zones.pz[z], buf, 256);
        uper_decode_complete(NULL, &asn_DEF_ProtectedCommunicationZone, (void**) &frep->choice.data.choice.protectedCommunicationZones.list.array[z], buf, (enc.encoded+7) / 8);
    }

    pthread_mutex_unlock(&facilities->lightship->lock);

    uint8_t frep_oer[1024];
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesReply, NULL, frep, frep_oer, 1024);
    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, frep_oer, enc.encoded, 0);

cleanup:
    ASN_STRUCT_FREE(asn_DEF_FacilitiesReply, frep);

    return rv;
}