diff --git a/src/facilities.c b/src/facilities.c index fddce90..fae4bcd 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -182,7 +182,7 @@ static int transport_indication(facilities_t *facilities, void* responder, uint8 break; case Port_saem: - saem_check(facilities, &facilities->press, its_msg); + saem_check(facilities, &facilities->bulletin, its_msg); break; default: diff --git a/src/facilities.h b/src/facilities.h index 44956c5..6164f4d 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -57,7 +57,7 @@ typedef struct facilities { dissemination_t* dissemination; // SA - press_t press; + bulletin_t bulletin; int station_type; bool use_security; diff --git a/src/sa.c b/src/sa.c index 928d0c3..aea1b6a 100644 --- a/src/sa.c +++ b/src/sa.c @@ -17,7 +17,7 @@ #define syslog_debug(msg, ...) #endif -SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { +SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem) { facilities_t* facilities = (facilities_t*) fc; @@ -53,22 +53,40 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { } int index = -1; - for (int a = 0; a < press->available_len; ++a) { + for (int a = 0; a < bulletin->to_consume_len; ++a) { /* Check existence through stationID and itsAid */ - if (saem->header.stationID == press->available[a]->station_id && - its_aid == press->available[a]->its_aid) { + if (saem->header.stationID == bulletin->to_consume[a]->station_id && + its_aid == bulletin->to_consume[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; + if (bulletin->to_consume_len + 1 < MAX_ANNOUNCEMENTS_LEN) { + bulletin->to_consume[bulletin->to_consume_len]->its_aid = its_aid; + bulletin->to_consume[bulletin->to_consume_len]->station_id = saem->header.stationID; + bulletin->to_consume[bulletin->to_consume_len]->timestamp = it2s_tender_get_clock(&facilities->epv); + + if (si->chOptions.extensions) { + for (int e = 0; e < si->chOptions.extensions->list.count; ++e) { + switch (si->chOptions.extensions->list.array[e]->present) { + case ServiceInfoExt_PR_addressIPv6: + memcpy(bulletin->to_consume[bulletin->to_consume_len]->endpoint.ipv6_addr, si->chOptions.extensions->list.array[e]->choice.addressIPv6.buf, 16); + break; + + case ServiceInfoExt_PR_servicePort: + bulletin->to_consume[bulletin->to_consume_len]->endpoint.port = si->chOptions.extensions->list.array[e]->choice.servicePort; + break; + + default: + break; + } + } + } + + index = bulletin->to_consume_len; + ++bulletin->to_consume_len; } } @@ -86,12 +104,13 @@ SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem) { return rv; } -void press_init(press_t* press) { +void bulletin_init(bulletin_t* bulletin) { - press->available_len = 0; + pthread_mutex_init(&bulletin->lock, NULL); + + bulletin->to_consume_len = 0; for (int i = 0; i < MAX_ANNOUNCEMENTS_LEN; ++i) { - press->available[i] = calloc(1, sizeof(announcement_t)); - + bulletin->to_consume[i] = calloc(1, sizeof(announcement_t)); } } @@ -108,19 +127,38 @@ int mk_saem(facilities_t* facilities, uint8_t* b_saem, uint32_t* b_saem_len) { saem->header.stationID = facilities->id.value; pthread_mutex_unlock(&facilities->id.lock); + uint8_t ipv6_addr[] = {0,1,2,3,4,5,6,7,8,9,10,11,12,13,14,15}; + /* 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*)); + saem->sam.body.serviceInfos->list.count = facilities->bulletin.to_provide_len; + saem->sam.body.serviceInfos->list.size = facilities->bulletin.to_provide_len * sizeof(void*); + saem->sam.body.serviceInfos->list.array = malloc(facilities->bulletin.to_provide_len * sizeof(void*)); uint8_t buf[1024]; - for (int i = 0; i < facilities->press.providing_len; ++i) { + for (int i = 0; i < facilities->bulletin.to_provide_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; + saem->sam.body.serviceInfos->list.array[i]->serviceID.choice.content = facilities->bulletin.to_provide[i]->its_aid; + + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions = calloc(1, sizeof(ServiceInfoExts_t)); + + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.count = 2; + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.size = 2 * sizeof(void*); + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array = malloc(2 * sizeof(void*)); + + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0] = calloc(1, sizeof(ServiceInfoExt_t)); + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0]->present = ServiceInfoExt_PR_addressIPv6; + + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0]->choice.addressIPv6.size = 16; + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0]->choice.addressIPv6.buf = malloc(16); + memcpy(saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[0]->choice.addressIPv6.buf, ipv6_addr, 16); + + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1] = calloc(1, sizeof(ServiceInfoExt_t)); + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1]->present = ServiceInfoExt_PR_servicePort; + saem->sam.body.serviceInfos->list.array[i]->chOptions.extensions->list.array[1]->choice.servicePort = 7777; } asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_UNALIGNED_CANONICAL_PER, &asn_DEF_SAEM, saem, b_saem, *b_saem_len); @@ -140,10 +178,14 @@ cleanup: void *sa_service(void *fc) { facilities_t *facilities = (facilities_t*) fc; + pthread_mutex_init(&facilities->bulletin.lock, NULL); + TransportDataRequest_t *tdr = calloc(1, sizeof(TransportDataRequest_t)); tdr->present = TransportDataRequest_PR_btp; BTPDataRequest_t *bdr = &tdr->choice.btp; + bulletin_t* bulletin = &facilities->bulletin; + bdr->btpType = BTPType_btpB; bdr->gn.destinationAddress.buf = malloc(6); @@ -172,23 +214,59 @@ void *sa_service(void *fc) { int rv = 0; while (!facilities->exit) { + + if (bulletin->to_provide_len) { + rv = mk_saem(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size); + if (!rv) { + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportDataRequest, NULL, tdr, tdr_oer+1, 1023); + if (enc.encoded == -1) { + syslog_err("[facilities] encoding TDR for SAEM failed"); + continue; + } else { + queue_add(facilities->tx_queue, tdr_oer, enc.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); + } + } + } + + pthread_mutex_lock(&bulletin->lock); + for (int a = 0; a < bulletin->to_consume_len; ++a) { + /* do some checks, e.g. + * is advertised service close by? + * do we want to enjoy the advertised service? */ + + if (bulletin->to_consume[a]->its_aid == 0 && !bulletin->to_consume[a]->n_trigger) { + + TransportDataRequest_t* tdr_etc = calloc(1, sizeof(TransportDataRequest_t)); + tdr_etc->present = TransportDataRequest_PR_tcp; + + tdr_etc->choice.tcp.data.buf = malloc(7); + tdr_etc->choice.tcp.data.size = 7; + char hello[] = "Hello!"; + memcpy(tdr_etc->choice.tcp.data.buf, hello, 7); + + tdr_etc->choice.tcp.gn = calloc(1, sizeof(GeonetworkingOutboundOptions_t)); + tdr_etc->choice.tcp.gn->packetTransportType = PacketTransportType_shb; + tdr_etc->choice.tcp.gn->destinationAddress.buf = calloc(1, 6); + tdr_etc->choice.tcp.gn->destinationAddress.size = 6; + + ++bulletin->to_consume[a]->n_trigger; + + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_TransportDataRequest, NULL, tdr_etc, tdr_oer+1, 1023); + if (enc.encoded == -1) { + syslog_err("[facilities] encoding TDR for ETC-Req failed"); + continue; + } else { + queue_add(facilities->tx_queue, tdr_oer, enc.encoded+1, 3); + pthread_cond_signal(&facilities->tx_queue->trigger); + } + + ASN_STRUCT_FREE(asn_DEF_TransportDataRequest, tdr_etc); + } + } + pthread_mutex_unlock(&bulletin->lock); + 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_TransportDataRequest, NULL, tdr, tdr_oer+1, 1023); - if (enc.encoded == -1) { - syslog_err("[facilities] encoding BTPDataRequest for SAEM failed"); - continue; - } - - queue_add(facilities->tx_queue, tdr_oer, enc.encoded+1, 3); - pthread_cond_signal(&facilities->tx_queue->trigger); - - lightship_reset_timer(facilities->lightship, &facilities->epv); } ASN_STRUCT_FREE(asn_DEF_TransportDataRequest, tdr); diff --git a/src/sa.h b/src/sa.h index d36cbce..41ca20f 100644 --- a/src/sa.h +++ b/src/sa.h @@ -10,18 +10,26 @@ typedef struct announcement { uint8_t protocol_id; uint8_t traffic_class; + struct { + uint8_t ipv6_addr[16]; + uint16_t port; + } endpoint; + uint64_t station_id; uint64_t timestamp; + + uint32_t n_trigger; } announcement_t; -typedef struct press { - announcement_t* available[MAX_ANNOUNCEMENTS_LEN]; - uint16_t available_len; +typedef struct bulletin { + announcement_t* to_consume[MAX_ANNOUNCEMENTS_LEN]; + uint8_t to_consume_len; - announcement_t* providing[16]; - uint8_t providing_len; + announcement_t* to_provide[16]; + uint8_t to_provide_len; -} press_t; + pthread_mutex_t lock; +} bulletin_t; typedef enum SAEM_CODE { SAEM_OK, @@ -29,8 +37,8 @@ typedef enum SAEM_CODE { SAEM_INVALID_HEADER_VERSION } SAEM_CODE_R; -void press_init(press_t* press); +void bulletin_init(bulletin_t* bulletin); -SAEM_CODE_R saem_check(void* fc, press_t* press, SAEM_t* saem); +SAEM_CODE_R saem_check(void* fc, bulletin_t* bulletin, SAEM_t* saem); void* sa_service(void* fc);