it2s-itss-facilities/src/facilities.c

#include "facilities.h"
#include "cam.h"
#include "denm.h"

#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <itss-facilities/FacilitiesDataIndication.h>
#include <itss-facilities/FacilitiesDataRequest.h>
#include <itss-facilities/FacilitiesDataResult.h>

#include <camv2/CAM.h>
#include <denmv2/DENM.h>
#include <ivim/IVIM.h>

#include <it2s-btp.h>

#include <zmq.h>
#include <unistd.h>
#include <syslog.h>
#include <stdbool.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

static int transport_indication(facilities_t *facilities, uint8_t *msg, uint32_t msg_len) {
    int rv = 0, code = 0;
    FacilitiesDataIndication_t *fdi = NULL;

    BTPDataIndication_t *bdi = calloc(1, sizeof(BTPDataIndication_t));  

    asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_BTPDataIndication, (void**) &bdi, msg, msg_len);
    if (dec.code) {
        syslog_err("[facilities] <- invalid bdi received");
        rv = 1;
        code = 1;
        zmq_send(facilities->responder, &code, 1, 0);
        goto cleanup;
    }

    zmq_send(facilities->responder, &code, 1, 0);
    syslog_debug("[facilities] <- BDI (%ldB)", bdi->data.size);

    // Parse message
    asn_TYPE_descriptor_t *its_msg_descriptor;
    void *its_msg;
    switch (bdi->destinationPort) {
        case Port_cam:
            its_msg_descriptor = &asn_DEF_CAM;
            its_msg = calloc(1, sizeof(CAM_t));
            break;
        case Port_denm:
            its_msg_descriptor = &asn_DEF_DENM;
            its_msg = calloc(1, sizeof(DENM_t));
            break;
        case Port_ivim:
            its_msg_descriptor = &asn_DEF_IVIM;
            its_msg = calloc(1, sizeof(IVIM_t));
            break;
        default:
            syslog_debug("[facilities] messsage with unhandled BTP port received, ignoring");
            goto cleanup;
    }

    dec = uper_decode_complete(NULL, its_msg_descriptor, (void**) &its_msg, bdi->data.buf, bdi->data.size);
    if (dec.code) {
        syslog_debug("[facilities] invalid %s received", its_msg_descriptor->name);
        rv = 1;
        goto cleanup;
    }

    // Manage message
    if (bdi->destinationPort == Port_denm) {
        #ifdef DEBUG
            uint8_t* xml_denm = malloc(4096);
            asn_enc_rval_t rve = xer_encode_to_buffer(xml_denm, 4096, 0x02, &asn_DEF_DENM, its_msg);
            syslog_debug("DENM XER %d: %.*s", (int)rve.encoded, (int)rve.encoded , xml_denm);
            free(xml_denm);
        #endif

        event_manage(facilities->den, its_msg);
    }

    // Forward to app
    fdi = calloc(1, sizeof(FacilitiesDataIndication_t));

    fdi->itssMessageType = bdi->destinationPort;

    fdi->data.size = bdi->data.size;
    fdi->data.buf = malloc(bdi->data.size);
    memcpy(fdi->data.buf, bdi->data.buf, bdi->data.size);

    uint8_t buffer[512];
    buffer[0] = 4; // Facilities
    asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataIndication, NULL, fdi, buffer+1, 511);
    syslog_debug("[facilities] -> [app] FDI (%ldB)", enc.encoded);
    zmq_send(facilities->app_socket, buffer, enc.encoded+1, 0);
    zmq_recv(facilities->app_socket, &code, 1, 0);

cleanup:
    if (bdi->destinationPort != Port_denm) ASN_STRUCT_FREE(*its_msg_descriptor, its_msg);
    ASN_STRUCT_FREE(asn_DEF_BTPDataIndication, bdi);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesDataIndication, fdi);

    return rv;
}

static int facilities_request(facilities_t *facilities, uint8_t *msg, uint32_t msg_len) {
    int rv = 0;
    int code = 0;

    uint8_t *fdres_oer = NULL;
   
    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 FDR received");
        rv = 1;
        code = 1;
        goto cleanup;
    }

    switch (fdreq->present) {
        case FacilitiesDataRequest_PR_singleMessage:
            ;
            bool fwd = true;
            switch (fdreq->choice.singleMessage.itssMessageType) {
                case ItssMessageType_denm:
                    ;
                    DENM_t *denm = calloc(1, sizeof(DENM_t));
                    dec = uper_decode_complete(NULL, &asn_DEF_DENM, (void**) &denm, fdreq->choice.singleMessage.data.buf, fdreq->choice.singleMessage.data.size);
                    if (dec.code) {
                        syslog_debug("[facilities] invalid FDRequest DENM received");
                        rv = 1;
                        ASN_STRUCT_FREE(asn_DEF_DENM, denm);
                        goto cleanup;
                    }
                    
                    bdr->destinationPort = Port_denm;

                    if (event_manage(facilities->den, denm)) {
                        fwd = false;
                    }

                    break;
                default:
                    syslog_err("[facilities] unrecognized FDRequest message type (%ld)", fdreq->choice.singleMessage.itssMessageType);
                    rv = 1;
                    goto cleanup;
            }

            // Forward message to [transport]
            if (fwd) {
                bdr->btpType = BTPType_btpB;
                bdr->data.buf = malloc(fdreq->choice.singleMessage.data.size);
                memcpy(bdr->data.buf, fdreq->choice.singleMessage.data.buf, fdreq->choice.singleMessage.data.size);
                bdr->data.size = fdreq->choice.singleMessage.data.size;
                bdr->destinationAddress.buf = malloc(6);
                for (int i = 0; i < 6; ++i) bdr->destinationAddress.buf[i] = 0xff;
                bdr->destinationAddress.size = 6;
                bdr->packetTransportType = PacketTransportType_gbc;
                bdr->trafficClass = 1;

                // Encode ITS message into OER
                uint8_t bdr_oer[384];
                bdr_oer[0] = 4; // [facilities] service id
                asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer + 1, 383);
                if (enc.encoded == -1) {
                    syslog_err("[facilities] failed encoding BDR (%s)", enc.failed_type->name);
                    rv = 1;
                    goto cleanup;
                }
                zmq_send(facilities->transport_socket, bdr_oer, enc.encoded+1, 0);
                zmq_recv(facilities->transport_socket, &code, 1, 0);
            }

            break;
        case FacilitiesDataRequest_PR_activeEvents:
            pthread_mutex_lock(&facilities->den->lock);

            fdres->code = ResultCode_accepted;
            fdres->events = calloc(1, sizeof(Events_t));

            uint16_t nae = facilities->den->no_active_events;

            switch (fdreq->choice.activeEvents) {
                case EventType_active:
                    fdres->events = calloc(1, sizeof(Events_t));
                    fdres->events->list.count = nae;
                    fdres->events->list.size = nae * sizeof(DENM_t *);
                    fdres->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->events->list.array[j] = calloc(1, sizeof(ItssMessage_t));
                            fdres->events->list.array[j]->itssMessageType = ItssMessageType_denm;
                            fdres->events->list.array[j]->data.buf = malloc(512);
                            asn_enc_rval_t enc = uper_encode_to_buffer(&asn_DEF_DENM, NULL, facilities->den->events[i]->denm, fdres->events->list.array[j]->data.buf, 512);
                            if (enc.encoded == -1) {
                                syslog_err("[facilities] failed encoding DENM for FDResult (%s)", enc.failed_type->name);
                                continue;
                            }
                            fdres->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);
                    rv = 1;
                    goto cleanup;
            }
            fdres_oer = malloc(4096);
            asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_FacilitiesDataResult, NULL, fdres, fdres_oer, 4096);
            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(facilities->responder, fdres_oer, enc.encoded, 0);

            pthread_mutex_unlock(&facilities->den->lock);
            break;
        default:
            syslog_err("[facilities] unrecognized FDR type received (%d)", fdreq->present);
            rv = 1;
            goto cleanup;
    }
    
    cleanup:
    if (rv) {
        fdres->code = ResultCode_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(facilities->responder, fdres_oer, enc.encoded, 0);
    }
    free(fdres_oer);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesDataResult, fdres);
    ASN_STRUCT_FREE(asn_DEF_FacilitiesDataRequest, fdreq);
    ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);

    return rv;
}

int main() {
    syslog_info("[facilities] starting");

    facilities_t facilities;
    facilities.exit = false;

    void *context = zmq_ctx_new();
    facilities.responder = zmq_socket(context, ZMQ_REP);
    int rc = zmq_bind(facilities.responder, "ipc:///tmp/itss/facilities");

    facilities.app_socket = zmq_socket(context, ZMQ_REQ);
    rc = zmq_connect(facilities.app_socket, "ipc:///tmp/itss/application");

    facilities.transport_socket = zmq_socket(context, ZMQ_REQ);
    rc = zmq_connect(facilities.transport_socket, "ipc:///tmp/itss/transport");

    facilities.den = calloc(1, sizeof(den_t));

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

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

    uint8_t buffer[512];
    uint8_t code;
    syslog_info("[facilities] listening");

    while (!facilities.exit) {
        memset(buffer, 0x00, 512);
        zmq_recv(facilities.responder, buffer, 512, 0);

        printf("NEW MESSAGE buffer[0] %d \n", buffer[0]);

        switch (buffer[0]) {
            case 3:
                code = transport_indication(&facilities, buffer+1, 511);
                break;
            case 5:
                code = facilities_request(&facilities, buffer+1, 511);
                break;
            default:
                syslog_debug("[facilities] unrecognized service");
                code = 1;
                zmq_send(facilities.responder, &code, 1, 0);
                continue;
        }
    }


    return 0;
}