it2s-itss-facilities/src/sa.c

#include "sa.h"
#include "facilities.h"
#include "infrastructure.h"

#include <it2s-tender/time.h>
#include <itss-transport/BTPDataRequest.h>
#include <saem/SAEM.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

SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) {

    facilities_t* facilities = (facilities_t*) fc;

    int rv = 0;

    if (saem->header.messageID != ItsPduHeader__messageID_saem) {
        return SAEM_INVALID_HEADER_MESSAGE_ID;
    }

    if (saem->header.protocolVersion != 1) {
        return SAEM_INVALID_HEADER_VERSION;
    }


    if (saem->sam.body.serviceInfos) {
        for (int i = 0; i < saem->sam.body.serviceInfos->list.count; ++i) {
            ServiceInfo_t* si = saem->sam.body.serviceInfos->list.array[i];

            uint16_t its_aid = 0;
            switch (si->serviceID.present) {
                case VarLengthNumber_PR_content:
                    its_aid = si->serviceID.choice.content;
                    break;
                case VarLengthNumber_PR_extension:
                    // TODO
                    break;
                default:
                    break;
            }

            if (!its_aid) {
                continue;
            }

            int index = -1;
            for (int a = 0; a < press->available_len; ++a) {
                /* Check existence through stationID and itsAid */
                if (saem->header.stationID == press->available[a]->station_id && 
                    its_aid == press->available[a]->its_aid) {
                    index = a;
                    break;
                }
            }

            if (index == -1) {
                if (press->available_len + 1 < MAX_ANNOUNCEMENTS_LEN) {
                    press->available[press->available_len]->its_aid = its_aid;
                    press->available[press->available_len]->station_id = saem->header.stationID;
                    press->available[press->available_len]->timestamp = it2s_tender_get_clock(&facilities->epv);
                    index = press->available_len;
                    ++press->available_len;
                }
            }

            uint16_t ci_index = si->channelIndex;

            // TODO channelInfos
            if (saem->sam.body.channelInfos) {
                if (saem->sam.body.channelInfos->list.count >= ci_index + 1) {
                }
            }

        }
    }

    return rv;
}

void press_init(press_t* press) {

    press->available_len = 0;
    for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) {
        press->available[i] = calloc(1, sizeof(announcement_t));
            
    }
}

int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) {
    int rv = 0;

    SAEM_t* saem = calloc(1, sizeof(SAEM_t));

    /* header */
    saem->header.protocolVersion = 2;
    saem->header.messageID = ItsPduHeader__messageID_saem;

    pthread_mutex_lock(&facilities->id.lock);
    saem->header.stationID = facilities->id.value;
    pthread_mutex_unlock(&facilities->id.lock);

    /* sam */
    saem->sam.version = 0;

    saem->sam.body.serviceInfos = calloc(1, sizeof(ServiceInfos_t));
    saem->sam.body.serviceInfos->list.count = facilities->press.providing_len;
    saem->sam.body.serviceInfos->list.size = facilities->press.providing_len * sizeof(void*);
    saem->sam.body.serviceInfos->list.array = malloc(facilities->press.providing_len * sizeof(void*));

    uint8_t buf[1024];
    for (int i = 0; i < facilities->press.providing_len; ++i) {
        saem->sam.body.serviceInfos->list.array[i] = calloc(1, sizeof(ServiceInfo_t));
        saem->sam.body.serviceInfos->list.array[i]->serviceID.present = VarLengthNumber_PR_content;
        saem->sam.body.serviceInfos->list.array[i]->serviceID.choice.content = facilities->press.providing[i]->its_aid;
    }

    asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len);
    if (enc.encoded == -1) {
        syslog_err("[facilities] [sa] failure to encode SAEM (%s)", enc.failed_type->name);
        rv = 1;
        goto cleanup;
    }
    *b_saem_len = enc.encoded;

cleanup:
    ASN_STRUCT_FREE(asn_DEF_SAEM, saem);
    
    return rv;
}

void *sa_service(void *fc) {
    facilities_t *facilities = (facilities_t*) fc;

    BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t));

    bdr->btpType = BTPType_btpB;

    bdr->gnDestinationAddress.buf = malloc(6);
    for (int i = 0; i < 6; ++i) {
        bdr->gnDestinationAddress.buf[i] = 0xff;
    }
    bdr->gnDestinationAddress.size = 6;

    bdr->gnPacketTransportType = PacketTransportType_shb;

    bdr->destinationPort = Port_saem;

    bdr->gnTrafficClass = 2;

    bdr->data.buf = malloc(512);
    bdr->data.size = 512;

    if (facilities->use_security) {
        bdr->gnSecurityProfile = malloc(sizeof(long));
        *bdr->gnSecurityProfile = 1;
    }

    uint8_t bdr_oer[1024];
    bdr_oer[0] = 4; // Facilities

    int rv = 0;

    while (!facilities->exit) {
        sleep(1);

        rv = mk_saem(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size);
        if (rv) {
            continue;
        }

        asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 1023);
        if (enc.encoded == -1) {
            syslog_err("[facilities] encoding BTPDataRequest for SAEM failed");
            continue;
        }

        queue_add(facilities->tx_queue, bdr_oer, enc.encoded+1, 3);
        pthread_cond_signal(&facilities->tx_queue->trigger);

        lightship_reset_timer(facilities->lightship, &facilities->epv);
    }

    ASN_STRUCT_FREE(asn_DEF_BTPDataRequest, bdr);


    return NULL;
}