it2s-itss-facilities/backup/src/facilities.c

#include "facilities.h"

#include <it2s-asn/etsi-its-v1/cam/EI1_CAM.h>
#include <it2s-asn/etsi-its-v1/cpm/EI1_CPM.h>
#include <it2s-asn/etsi-its-v1/denm/EI1_DENM.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesReply.h>
#include <it2s-asn/etsi-its-sdu/itss-facilities/EIS_FacilitiesRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-management/EIS_ManagementRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityReply.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityRequest.h>
#include <it2s-asn/etsi-its-sdu/itss-security/EIS_SecurityResponse.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingIndication.h>
#include <it2s-asn/etsi-its-sdu/itss-networking/EIS_NetworkingRequest.h>
#include <it2s-asn/etsi-its-v1/ivim/EI1_IVIM.h>
#include <it2s-asn/etsi-its-v1/saem/EI1_SAEM.h>
#include <it2s-tender/packet.h>
#include <it2s-tender/recorder.h>
#include <it2s-tender/space.h>
#include <it2s-tender/time.h>
#include <signal.h>
#include <stdbool.h>
#include <sys/stat.h>
#include <sys/types.h>
#include <unistd.h>
#include <zmq.h>

#include "cam.h"
#include "config.h"
#include "cpm.h"
#include "denm.h"
#include "evm.h"
#include "indications.h"
#include "infrastructure.h"
#include "requests.h"
#include "saem.h"
#include "tpm.h"
#include "vcm.h"

facilities_t facilities = {0};

static int networking_indication(void *responder, void **security_socket, uint8_t *msg, uint32_t msg_len) {
    int rv = 0;
    uint8_t code = 0;
    bool stored = false;

    EIS_NetworkingIndication_t *ni = calloc(1, sizeof(EIS_NetworkingIndication_t));

    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_NetworkingIndication, (void **)&ni, msg, msg_len);
    if (dec.code) {
        log_error("<- invalid TI received");
        rv = 1;
        code = 1;
        itss_0send(responder, &code, 1);
        goto cleanup;
    }

    itss_0send(responder, &code, 1);

    switch (ni->present) {
        case EIS_NetworkingIndication_PR_packet:
            networking_packet_indication(&ni->choice.packet, security_socket);
            break;
        case EIS_NetworkingIndication_PR_data:
            networking_data_indication(&ni->choice.data, security_socket);
            goto cleanup;
        default:
            log_debug("<- unrecognized NI.choice received");
            rv = 1;
            goto cleanup;
    }

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_NetworkingIndication, ni);

    return rv;
}

static int facilities_request(void *responder, uint8_t *msg, uint32_t msg_len) {
    int rv = 0;

    EIS_FacilitiesRequest_t *fr = calloc(1, sizeof(EIS_FacilitiesRequest_t));

    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_FacilitiesRequest, (void **)&fr, msg, msg_len);
    if (dec.code) {
        log_error("<- invalid FR received");
        facilities_request_result_rejected(responder);
        rv = 1;
        goto cleanup;
    }

    switch (fr->present) {
        case EIS_FacilitiesRequest_PR_message:
            rv = facilities_request_single_message(responder, &fr->choice.message);
            break;

        case EIS_FacilitiesRequest_PR_data:
            switch (fr->choice.data.present) {
                case EIS_FacilitiesDataRequest_PR_activeEpisodes:
                    rv = facilities_request_active_episodes(responder, fr);
                    break;

                case EIS_FacilitiesDataRequest_PR_attributeTypes:
                    rv = facilities_request_attribute_types(responder, fr);
                    break;

                case EIS_FacilitiesDataRequest_PR_loadedProtectionZones:
                    rv = facilities_request_loaded_protected_zones(responder, fr);
                    break;

                case EIS_FacilitiesDataRequest_PR_chainInfoSet:
                    rv = facilities_request_chaininfo_set(responder, &fr->choice.data.choice.chainInfoSet);
                    break;

                default:
                    log_error("<- unrecognized FDR type received (%d)", fr->choice.data.present);
                    facilities_request_result_rejected(responder);
                    rv = 1;
                    goto cleanup;
            }
            break;

        default:
            log_error("<- unrecognized FR type received (%d)", fr->present);
            facilities_request_result_rejected(responder);
            rv = 1;
            goto cleanup;
    }

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_FacilitiesRequest, fr);

    return rv;
}

static int security_indication(void *responder_secured, uint8_t *msg, uint32_t msg_len) {
    int rv = 0;

    EIS_SecurityIndication_t *si = calloc(1, sizeof(EIS_SecurityIndication_t));
    EIS_SecurityResponse_t *sr = calloc(1, sizeof(EIS_SecurityResponse_t));

    uint8_t buffer[64];

    asn_enc_rval_t enc;

    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_SecurityIndication, (void **)&si, msg, msg_len);
    if (dec.code) {
        log_error("<- invalid SIndication received");
        rv = 1;
        goto cleanup;
    }

    pthread_mutex_lock(&facilities.id.change.lock);
    if (facilities.id.change.stage == ID_CHANGE_BLOCKED) {
        pthread_mutex_unlock(&facilities.id.change.lock);
        log_debug("identity change is currently blocked");
        rv = 1;
        goto cleanup;
    }

    if (facilities.id.change.stage == ID_CHANGE_PREPARE &&
        si->choice.idChangeEvent.command != EIS_SecurityIdChangeEventType_commit &&
        si->choice.idChangeEvent.command != EIS_SecurityIdChangeEventType_abort) {
        pthread_mutex_unlock(&facilities.id.change.lock);
        log_debug("current identity change state is prepare, but received identity change command is not commit nor abort");
        rv = 1;
        goto cleanup;
    }

    bool id_changed = false;

    switch (si->choice.idChangeEvent.command) {
        case EIS_SecurityIdChangeEventType_prepare:

            facilities.id.change.stage = ID_CHANGE_PREPARE;

            pthread_mutex_lock(&facilities.id.lock);
            pthread_mutex_lock(&facilities.lightship.lock);
            break;

        case EIS_SecurityIdChangeEventType_commit:;
            facilities.id.change.stage = ID_CHANGE_COMMIT;

            // Reset lightship
            for (int i = 0; i < facilities.lightship.path_history_len; ++i) {
                free(facilities.lightship.path_history[i]);
            }
            facilities.lightship.path_history_len = 0;

            facilities.lightship.t_last_cam = 0;
            facilities.lightship.t_last_cam_lfc = 0;
            facilities.lightship.next_cam_max = 0;
            facilities.lightship.next_cam_min = 0;

            pthread_mutex_unlock(&facilities.lightship.lock);

            // Change Station ID
            for (int i = 0; i < si->choice.idChangeEvent.ids.list.count; ++i) {
                switch (si->choice.idChangeEvent.ids.list.array[i]->present) {
                    case EIS_SecurityId_PR_stationId:
                        facilities.id.station_id = si->choice.idChangeEvent.ids.list.array[i]->choice.stationId;
                        break;
                    case EIS_SecurityId_PR_ipv6Address:
                        memcpy(facilities.id.ipv6_addr, si->choice.idChangeEvent.ids.list.array[i]->choice.ipv6Address.buf, 16);
                        break;
                    default:
                        break;
                }
            }

            facilities.id.change.stage = ID_CHANGE_INACTIVE;

            pthread_mutex_unlock(&facilities.id.lock);

            id_changed = true;

            break;

        case EIS_SecurityIdChangeEventType_abort:
            pthread_mutex_unlock(&facilities.id.lock);
            pthread_mutex_unlock(&facilities.lightship.lock);

            facilities.id.change.stage = ID_CHANGE_INACTIVE;
            break;

        default:
            pthread_mutex_unlock(&facilities.id.change.lock);
            log_error("[networking]<- unhandled idChangeEvent command type");
            rv = 1;
            goto cleanup;
    }

    sr->present = EIS_SecurityResponse_PR_idChangeEvent;
    sr->choice.idChangeEvent.returnCode = 0;
    enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityResponse, NULL, sr, buffer, 64);
    itss_0send(responder_secured, buffer, enc.encoded);

    pthread_mutex_unlock(&facilities.id.change.lock);

cleanup:
    if (rv) {
        sr->present = EIS_SecurityResponse_PR_idChangeEvent;
        sr->choice.idChangeEvent.returnCode = 1;
        enc = oer_encode_to_buffer(&asn_DEF_EIS_SecurityResponse, NULL, sr, buffer, 64);
        itss_0send(responder_secured, buffer, enc.encoded);
    }

    pthread_mutex_unlock(&facilities.id.change.lock);
    ASN_STRUCT_FREE(asn_DEF_EIS_SecurityResponse, sr);
    ASN_STRUCT_FREE(asn_DEF_EIS_SecurityIndication, si);

    return rv;
}


static int management_indication(void *responder, uint8_t *msg, uint32_t msg_len) {
    int rv = 0;

    uint8_t code = 0;

    EIS_ManagementIndication_t *mi = calloc(1, sizeof(EIS_ManagementIndication_t));

    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_EIS_ManagementIndication, (void **)&mi, msg, msg_len);
    if (dec.code) {
        rv = 1;
        code = 1;
        itss_0send(responder, &code, 1);
        goto cleanup;
    }
    itss_0send(responder, &code, 1);

    if (mi->present == EIS_ManagementIndication_PR_attributes) {
        itss_time_lock();
        asn_INTEGER2ulong(&mi->choice.attributes.clock, &epv.time.clock);
        itss_time_unlock();
    }

cleanup:
    ASN_STRUCT_FREE(asn_DEF_EIS_ManagementIndication, mi);

    return rv;
}

void *tx(void *arg) {
    int rv = 0;

    itss_queue_t *queue = facilities.tx_queue;

    uint8_t code;
    void *applications_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ);
    void *networking_socket = itss_0connect(facilities.zmq.networking_address, ZMQ_REQ);
    void *management_socket = itss_0connect(facilities.zmq.management_address, ZMQ_REQ);

    itss_queue_t *stream = itss_queue_new();

    while (!facilities.exit) {
        pthread_mutex_lock(&queue->lock);
        while (!queue->len && !facilities.exit) {
            pthread_cond_wait(&queue->trigger, &queue->lock);
        }

        itss_queue_packet_t* qp;
        while ((qp = itss_queue_pop(queue))) {
            itss_queue_add(stream, qp);
        };
        pthread_mutex_unlock(&queue->lock);

        while ((qp = itss_queue_pop(stream))) {
            switch (qp->destination) {
                case ITSS_NETWORKING:
                    log_debug("-> %s ->[networking] | id:%08x size:%dB",
                              qp->info_msg, (uint32_t)qp->id, qp->data_len);
                    itss_0send(networking_socket, qp->data, qp->data_len);
                    rv = itss_0recv_rt(&networking_socket, &code, 1, qp->data, qp->data_len, 1000);
                    if (rv == -1) {
                        log_error("-> %s ->[networking] | id:%08x size:%dB <TIMEOUT>",
                                  qp->info_msg, (uint32_t)qp->id, qp->data_len);
                    }
                    break;
                case ITSS_APPLICATIONS:
                    log_debug("-> %s ->[applications] | id:%08x size:%dB",
                              qp->info_msg, (uint32_t)qp->id, qp->data_len);
                    itss_0send(applications_socket, qp->data, qp->data_len);
                    rv = itss_0recv_rt(&applications_socket, &code, 1, qp->data, qp->data_len, 1000);
                    if (rv == -1) {
                        log_error("-> %s ->[applications] | id:%08x size:%dB <TIMEOUT>",
                                  qp->info_msg, (uint32_t)qp->id, qp->data_len);
                    }
                    break;
                case ITSS_MANAGEMENT:
                    itss_0send(management_socket, qp->data, qp->data_len);
                    rv = itss_0recv_rt(&management_socket, &code, 1, qp->data, qp->data_len, 1000);
                    if (rv == -1) {
                        log_error("-> %s ->[management] | id:%08x size:%dB <TIMEOUT>",
                                  qp->info_msg, (uint32_t)qp->id, qp->data_len);
                    }
                    break;
            }
            itss_queue_packet_free(qp);
        }
    }

    itss_0close(networking_socket);
    itss_0close(management_socket);
    itss_0close(applications_socket);

    return NULL;
}

static void sigh(int signum) {
    facilities.exit = true;
    uint8_t code = 0;
    void *socket = itss_0connect(ZMQ_INTERNAL_ADDR, ZMQ_PAIR);
    itss_0send(socket, &code, sizeof(code));
    itss_0close(socket);
}

int main() {
    signal(SIGTERM, sigh);
    signal(SIGINT, sigh);
    signal(SIGKILL, sigh);

    facilities.tx_queue = itss_queue_new();
    lightship_init();
    den_init();
    infrastructure_init();
    dissemination_init();
    bulletin_init();
    void *security_socket = NULL;

    if (facilities_config()) {
        goto cleanup;
    }

    facilities.lightship.type = facilities.station_type;

    // Tx
    pthread_create(&facilities.transmitting, NULL, tx, NULL);

    // CA
    pthread_create(&facilities.ca_service, NULL, ca_service, NULL);

    // DEN
    pthread_create(&facilities.den_service, NULL, den_service, NULL);

    // Infrastructure
    pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, NULL);

    // CP
    if (facilities.dissemination.active)
        pthread_create(&facilities.cp_service, NULL, cp_service, NULL);

    // SA
    pthread_create(&facilities.sa_service, NULL, sa_service, NULL);

    // Tolling
    tolling_init(facilities.station_type);

    // VC
    if (facilities.coordination.active) {
        coordination_init();
        pthread_create(&facilities.vc_service, NULL, vc_service, NULL);
    }

    // EVCSN
    if (facilities.evm_args.activate)
        pthread_create(&facilities.evcsn_service, NULL, evcsn_service, NULL);

    // EDM
    if (facilities.edm.enabled) {
        edm_init();
    }

    // MCM
    if (facilities.mcm_coord.active){
        mcm_coord_init();
        pthread_create(&facilities.mcm_service, NULL, mc_service, NULL);
    }
    
    facilities.apps_socket = itss_0connect(facilities.zmq.applications_address, ZMQ_REQ);
    security_socket = itss_0connect(facilities.zmq.security_address, ZMQ_REQ);

    uint8_t buffer[ITSS_SDU_MAX_LEN];
    log_info("listening");
    uint8_t code;
    bool in_idchange;

    int32_t rl;

    while (!facilities.exit) {
        itss_0poll(facilities.zmq.responders, facilities.zmq.n_responders, -1);

        for (int i = 0; i < facilities.zmq.n_responders; ++i) {
            if (facilities.zmq.responders[i].revents) {
                rl = itss_0recv(facilities.zmq.responders[i].socket, buffer, ITSS_SDU_MAX_LEN);
                switch (buffer[0]) { /* source */

                    case ITSS_INTERNAL:
                        break;

                    case ITSS_NETWORKING:
                        in_idchange = true;
                        pthread_mutex_lock(&facilities.id.change.lock);
                        if (facilities.id.change.stage == ID_CHANGE_INACTIVE) {
                            in_idchange = false;
                            facilities.id.change.stage = ID_CHANGE_BLOCKED;
                        }
                        pthread_mutex_unlock(&facilities.id.change.lock);

                        if (!in_idchange) {
                            networking_indication(facilities.zmq.responders[i].socket, &security_socket, buffer + 1, rl-1);

                            pthread_mutex_lock(&facilities.id.change.lock);
                            facilities.id.change.stage = ID_CHANGE_INACTIVE;
                            pthread_mutex_unlock(&facilities.id.change.lock);
                        } else {
                            code = 1;
                            itss_0send(facilities.zmq.responders[i].socket, &code, 1);
                        }
                        break;

                    case ITSS_APPLICATIONS:
                        in_idchange = true;
                        pthread_mutex_lock(&facilities.id.change.lock);
                        if (facilities.id.change.stage == ID_CHANGE_INACTIVE) {
                            in_idchange = false;
                            facilities.id.change.stage = ID_CHANGE_BLOCKED;
                        }
                        pthread_mutex_unlock(&facilities.id.change.lock);

                        if (!in_idchange) {
                            facilities_request(facilities.zmq.responders[i].socket, buffer + 1, rl-1);

                            pthread_mutex_lock(&facilities.id.change.lock);
                            facilities.id.change.stage = ID_CHANGE_INACTIVE;
                            pthread_mutex_unlock(&facilities.id.change.lock);
                        } else {
                            code = 1;
                            itss_0send(facilities.zmq.responders[i].socket, &code, 1);
                        }

                        break;

                    case ITSS_MANAGEMENT:
                        management_indication(facilities.zmq.responders[i].socket, buffer + 1, rl-1);
                        break;

                    case ITSS_SECURITY:
                        security_indication(facilities.zmq.responders[i].socket, buffer + 1, rl-1);
                        break;

                    case ITSS_STATUS:
                        code = 0;
                        itss_0send(facilities.zmq.responders[i].socket, &code, 1);
                        break;

                    default:
                        log_debug("<- unrecognized service");
                        code = 1;
                        itss_0send(facilities.zmq.responders[i].socket, &code, 1);
                        continue;
                }
            }
        }
    }

    // Exit
cleanup:
    if (facilities.evm_args.activate)
        pthread_join(facilities.evcsn_service, NULL);
    pthread_join(facilities.ca_service, NULL);
    pthread_join(facilities.den_service, NULL);
    pthread_join(facilities.infrastructure_service, NULL);
    if (facilities.dissemination.active)
        pthread_join(facilities.cp_service, NULL);
    pthread_join(facilities.sa_service, NULL);
    itss_queue_trigger(facilities.tx_queue);
    pthread_join(facilities.transmitting, NULL);
    if (facilities.coordination.active)
        pthread_join(facilities.vc_service, NULL);
    if (facilities.edm.enabled) {
        itss_0close(facilities.edm.app_socket);
    }
    if (facilities.edm.enabled) {
        itss_0close(facilities.edm.app_socket);
    }
    itss_0close(facilities.apps_socket);

    itss_0close(security_socket);
    for (int i = 0; i < facilities.zmq.n_responders; ++i) {
        itss_0close(facilities.zmq.responders[i].socket);
    }
    itss_0destroy();

    log_info("exiting");
    log_close();

    return 0;
}