From f80f67c1bf5cc3533054da774a50e2f1b9812a28 Mon Sep 17 00:00:00 2001 From: emanuel Date: Fri, 26 Mar 2021 21:57:47 +0000 Subject: [PATCH] New management svc, distributed services, new config lib --- src/CMakeLists.txt | 4 +- src/cam.c | 278 ++++++++---------------------------- src/cam.h | 2 +- src/config.c | 340 ++++++++++++++++++++++++--------------------- src/config.h | 6 +- src/denm.h | 4 +- src/facilities.c | 155 ++++++++++++++------- src/facilities.h | 35 +++-- 8 files changed, 376 insertions(+), 448 deletions(-) diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt index 25977f8..36b7f08 100644 --- a/src/CMakeLists.txt +++ b/src/CMakeLists.txt @@ -12,10 +12,10 @@ TARGET_LINK_LIBRARIES(it2s-itss-facilities -lit2s-asn-itss-facilities -lit2s-asn-itss-transport -lit2s-asn-itss-security + -lit2s-asn-itss-management -lzmq - -lit2s-gps -lpthread - -ltoml + -lit2s-config -lit2s-asn-camv2 -lit2s-asn-ivim -lit2s-asn-denmv2 diff --git a/src/cam.c b/src/cam.c index bd2d4a6..d1b087d 100644 --- a/src/cam.c +++ b/src/cam.c @@ -5,7 +5,6 @@ #include #include #include -#include #include #include @@ -77,181 +76,6 @@ static int permissions_check(int cid, uint8_t* permissions, uint8_t permissions_ } -static AltitudeConfidence_t getAltitudeConfidence(double conf) { - if (conf >= 200) return AltitudeConfidence_outOfRange; - if (conf >= 100) return AltitudeConfidence_alt_200_00; - if (conf >= 50) return AltitudeConfidence_alt_100_00; - if (conf >= 20) return AltitudeConfidence_alt_050_00; - if (conf >= 10) return AltitudeConfidence_alt_020_00; - if (conf >= 5) return AltitudeConfidence_alt_010_00; - if (conf >= 1) return AltitudeConfidence_alt_005_00; - - return AltitudeConfidence_alt_001_00; -} - -static HeadingConfidence_t getHeadingConfidence(uint32_t conf) { - if (conf > 125) return 126; - if (conf == 125) return 125; - if (conf == 0) return 127; - if (conf < 1) return 1; - - return conf; -} - -static SpeedConfidence_t getSpeedConfidence(uint32_t conf) { - if (conf > 125) return 126; - if (conf == 125) return 125; - if (conf == 0) return 127; - if (conf < 1) return 1; - - return conf; -} - -static int get_speed(struct it2s_gps_data* gps_data, long* speed_value, long* speed_confidence) { - int rv = 0; - - /* - * SpeedValue ::= INTEGER { - * standstill(0), - * oneCentimeterPerSec(1), - * unavailable(16383) - * } (0..16383) - * - * SpeedConfidence ::= INTEGER { - * equalOrWithinOneCentimeterPerSec(1), - * equalOrWithinOneMeterPerSec(100), - * outOfRange(126), - * unavailable(127) - * } (1..127) - */ - - if (!isnan(gps_data->gps_speed)) { - *speed_value = ((uint16_t)(gps_data->gps_speed * 100)); // cm/s - *speed_confidence = getSpeedConfidence((uint8_t)(gps_data->gps_eps * 100)); - } else { - *speed_value = SpeedValue_unavailable; - *speed_confidence = SpeedConfidence_unavailable; - - rv = 1; - } - - return rv; -} - -static int get_heading(struct it2s_gps_data* gps_data, long* heading_value, long* heading_confidence) { - int rv = 0; - - /* - * HeadingValue ::= INTEGER { - * wgs84North(0), - * wgs84East(900), - * wgs84South(1800), - * wgs84West(2700), - * unavailable(3601) - * } (0..3601) - * - * HeadingConfidence ::= INTEGER { - * equalOrWithinZeroPointOneDegree (1), - * equalOrWithinOneDegree (10), - * outOfRange(126), - * unavailable(127) - * } (1..127) - * - */ - - if (!isnan(gps_data->gps_track)) { - *heading_value = ((uint16_t)(gps_data->gps_track * 10)); - *heading_confidence = getHeadingConfidence((uint8_t)(gps_data->gps_epd * 10)); - } else { - *heading_value = HeadingValue_unavailable; - *heading_confidence = HeadingConfidence_unavailable; - - rv = 1; - } - - return rv; -} - -static int get_wgs84_coordinates(struct it2s_gps_data* gps_data, long* latitude, long* longitude) { - int rv = 0; - - /* - * Latitude ::= INTEGER { - * oneMicrodegreeNorth (10), - * oneMicrodegreeSouth (-10), - * unavailable(900000001) - * } (-900000000..900000001) - * Longitude ::= INTEGER { - * oneMicrodegreeEast (10), - * oneMicrodegreeWest (-10), - * unavailable(1800000001) - * } (-1800000000..1800000001) - * - */ - - if (!isnan(gps_data->gps_latitude)) { - *latitude = (int32_t)((gps_data->gps_latitude) * 10000000); - } else { - *latitude = Latitude_unavailable; - - rv = 1; - } - - if (!isnan(gps_data->gps_longitude)) { - *longitude = (int32_t)((gps_data->gps_longitude) * 10000000); - } else { - *longitude = Longitude_unavailable; - - rv = 1; - } - - return rv; -} - -static int get_altitude(struct it2s_gps_data* gps_data, long* altitude_value, long* altitude_confidence) { - int rv = 0; - - /* - * AltitudeValue ::= INTEGER { - * referenceEllipsoidSurface(0), - * oneCentimeter(1), - * unavailable(800001) - * } (-100000..800001) - * - * AltitudeConfidence ::= ENUMERATED { - * alt-000-01 (0), - * alt-000-02 (1), - * alt-000-05 (2), - * alt-000-10 (3), - * alt-000-20 (4), - * alt-000-50 (5), - * alt-001-00 (6), - * alt-002-00 (7), - * alt-005-00 (8), - * alt-010-00 (9), - * alt-020-00 (10), - * alt-050-00 (11), - * alt-100-00 (12), - * alt-200-00 (13), - * outOfRange (14), - * unavailable (15) - * } - */ - - if (!isnan(gps_data->gps_altitude)) { - *altitude_value = (int32_t) ((gps_data->gps_altitude) * 10); - *altitude_confidence = (uint8_t) getAltitudeConfidence(gps_data->gps_epv); - } else { - *altitude_value = AltitudeValue_unavailable; - *altitude_confidence = AltitudeConfidence_unavailable; - - rv = 1; - } - - return rv; -} - - static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) { int rv = 0; @@ -260,9 +84,6 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) syslog_emerg("clock_gettime() failed"); } - struct it2s_gps_data gps_data; - it2s_gps_read(&gps_data); - CAM_t *cam = calloc(1, sizeof(CAM_t)); cam->header.protocolVersion = 2; @@ -280,26 +101,36 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) BasicContainer_t* bc = &cam->cam.camParameters.basicContainer; - get_altitude(&gps_data, &bc->referencePosition.altitude.altitudeValue, &bc->referencePosition.altitude.altitudeConfidence); - - // Set GPS coordinates - if (!facilities->gps_fixed) { - get_wgs84_coordinates(&gps_data, &bc->referencePosition.latitude, &bc->referencePosition.longitude); - } else { - bc->referencePosition.latitude = (int32_t)((facilities->latitude) * 10000000); - bc->referencePosition.longitude = (int32_t)((facilities->longitude) * 10000000); - } - - bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; - bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; - bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; lightship_t* lightship = facilities->lightship; pthread_mutex_lock(&lightship->lock); if (facilities->station_type != StationType_roadSideUnit) { + pthread_mutex_lock(&facilities->epv.lock); + bc->referencePosition.altitude.altitudeValue = facilities->epv.altitude; + bc->referencePosition.altitude.altitudeConfidence = facilities->epv.altitude_conf; + + // Set GPS coordinates + bc->referencePosition.latitude = facilities->epv.latitude; + bc->referencePosition.longitude = facilities->epv.longitude; + bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; + + cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_basicVehicleContainerHighFrequency; BasicVehicleContainerHighFrequency_t* bvc_hf = &cam->cam.camParameters.highFrequencyContainer.choice.basicVehicleContainerHighFrequency; + + // Set speed + bvc_hf->speed.speedValue = facilities->epv.speed; + bvc_hf->speed.speedConfidence = facilities->epv.speed_conf; + + // Set heading + bvc_hf->heading.headingValue = facilities->epv.heading; + bvc_hf->heading.headingConfidence = facilities->epv.heading_conf; + + pthread_mutex_unlock(&facilities->epv.lock); + bvc_hf->vehicleWidth = 20; bvc_hf->vehicleLength.vehicleLengthValue = 46; bvc_hf->vehicleLength.vehicleLengthConfidenceIndication = VehicleLengthConfidenceIndication_unavailable; @@ -312,11 +143,6 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) bvc_hf->yawRate.yawRateValue = YawRateValue_unavailable; bvc_hf->yawRate.yawRateConfidence = YawRateConfidence_unavailable; - // Set speed - get_speed(&gps_data, &bvc_hf->speed.speedValue, &bvc_hf->speed.speedConfidence); - - // Set heading - get_heading(&gps_data, &bvc_hf->heading.headingValue, &bvc_hf->heading.headingConfidence); // Save current values if (lightship->pos_history_len == POS_HISTORY_MAX_LEN) { @@ -384,6 +210,18 @@ static int mk_cam(facilities_t* facilities, uint8_t *cam_oer, uint32_t *cam_len) } } else { + pthread_mutex_lock(&facilities->epv.lock); + bc->referencePosition.altitude.altitudeValue = facilities->epv.altitude; + bc->referencePosition.altitude.altitudeConfidence = facilities->epv.altitude_conf; + + // Set GPS coordinates + bc->referencePosition.latitude = facilities->epv.latitude; + bc->referencePosition.longitude = facilities->epv.longitude; + bc->referencePosition.positionConfidenceEllipse.semiMinorConfidence = SemiAxisLength_unavailable; + bc->referencePosition.positionConfidenceEllipse.semiMajorConfidence = SemiAxisLength_unavailable; + bc->referencePosition.positionConfidenceEllipse.semiMajorOrientation = HeadingValue_unavailable; + pthread_mutex_unlock(&facilities->epv.lock); + cam->cam.camParameters.highFrequencyContainer.present = HighFrequencyContainer_PR_rsuContainerHighFrequency; if (facilities->lightship->pz_len > 0) { @@ -445,7 +283,7 @@ lightship_t* lightship_init() { } -int lightship_check(lightship_t* lightship) { +int lightship_check(lightship_t* lightship, void* epv_s) { int rv = 0; struct timespec systemtime; @@ -464,32 +302,27 @@ int lightship_check(lightship_t* lightship) { rv = 1; } else if (now > lightship->next_cam_min) { - struct it2s_gps_data gps_data; - it2s_gps_read(&gps_data); + epv_t* epv = (epv_t*) epv_s; + pthread_mutex_lock(&epv->lock); // Check heading delta > 4ยบ - long heading_value, heading_confidence; - if (!get_heading(&gps_data, &heading_value, &heading_confidence)) { - int diff = heading_value - lightship->pos_history[0]->heading; - if (abs(diff) > 40) rv = 1; - } + int diff = epv->heading - lightship->pos_history[0]->heading; + if (abs(diff) > 40) rv = 1; if (!rv) { // Check speed delta > 0.5 m/s - long speed_value, speed_confidence; - if (!get_speed(&gps_data, &speed_value, &speed_confidence)) { - int diff = speed_value - lightship->pos_history[0]->speed; - if (abs(diff) > 50) rv = 1; - } + diff = epv->speed - lightship->pos_history[0]->speed; + if (abs(diff) > 50) rv = 1; if (!rv) { // Check position delta > 4 m // TODO make an *accurate* distance calculator using GPS coords - int32_t avg_speed = (speed_value + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */ + int32_t avg_speed = (epv->speed + lightship->pos_history[0]->speed)/2 / 100; /* cm/s to m/s */ uint64_t delta_time = (now - lightship->last_cam) / 1000; /* ms to s */ if (avg_speed * delta_time > 4) rv = 1; } } + pthread_mutex_unlock(&epv->lock); } @@ -662,20 +495,23 @@ int check_cam(void* fc, BTPDataIndication_t *bdi, CAM_t* cam, ServiceSpecificPer return rv; } -static int check_pz(lightship_t *lightship) { +static int check_pz(lightship_t *lightship, void* epv_s) { bool is_inside = false; - struct it2s_gps_data gps_data; - it2s_gps_read(&gps_data); + epv_t* epv = (epv_t*) epv_s; + pthread_mutex_lock(&epv->lock); + long lat = epv->latitude; + long lon = epv->longitude; + pthread_mutex_unlock(&epv->lock); pthread_mutex_lock(&lightship->lock); for (int i = 0; i < lightship->pz_len; ++i) { - double d_lat = (gps_data.gps_latitude - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG; - double d_lon = (gps_data.gps_longitude - (double) lightship->pz[i]->protectedZoneLongitude/10000000) * RAD_PER_DEG; + double d_lat = (lat - (double) lightship->pz[i]->protectedZoneLatitude/10000000) * RAD_PER_DEG; + double d_lon = (lon - (double) lightship->pz[i]->protectedZoneLongitude/10000000) * RAD_PER_DEG; double a = pow(sin(d_lat/2.0), 2) + - cos(gps_data.gps_latitude * RAD_PER_DEG) * + cos(lat * RAD_PER_DEG) * cos((double) lightship->pz[i]->protectedZoneLatitude/10000000 * RAD_PER_DEG) * pow(sin(d_lon/2.0), 2); @@ -696,14 +532,10 @@ static int check_pz(lightship_t *lightship) { } void *ca_service(void *fc) { - int rv = 0; uint8_t code = 0; facilities_t *facilities = (facilities_t*) fc; - void* h = zmq_socket(facilities->ctx, ZMQ_REQ); - zmq_connect(h, TRANSPORT_ADDRESS); - BTPDataRequest_t *bdr = calloc(1, sizeof(BTPDataRequest_t)); bdr->btpType = BTPType_btpB; @@ -730,7 +562,7 @@ void *ca_service(void *fc) { while (!facilities->exit) { usleep(1000*50); - if (lightship_check(facilities->lightship) && facilities->lightship->active) { + if (lightship_check(facilities->lightship, &facilities->epv) && facilities->lightship->active) { rv = mk_cam(facilities, bdr->data.buf, (uint32_t *) &bdr->data.size); if (rv) { continue; @@ -738,7 +570,7 @@ void *ca_service(void *fc) { // Check if inside PZ bdr->gnCommunicationProfile = 0; - if (facilities->station_type != 15 && check_pz(facilities->lightship)) bdr->gnCommunicationProfile = 1; + if (facilities->station_type != 15 && check_pz(facilities->lightship, &facilities->epv)) bdr->gnCommunicationProfile = 1; asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_BTPDataRequest, NULL, bdr, bdr_oer+1, 1023); if (enc.encoded == -1) { diff --git a/src/cam.h b/src/cam.h index f2a87f6..336b008 100644 --- a/src/cam.h +++ b/src/cam.h @@ -76,7 +76,7 @@ typedef struct lightship { lightship_t* lightship_init(); -int lightship_check(lightship_t *lightship); +int lightship_check(lightship_t *lightship, void* epv_s); void lightship_reset_timer(lightship_t *lightship); int check_cam(void* fc, BTPDataIndication_t* bdi, CAM_t* cam, ServiceSpecificPermissions_t* ssp); diff --git a/src/config.c b/src/config.c index ec40d4d..1f778ed 100644 --- a/src/config.c +++ b/src/config.c @@ -1,13 +1,19 @@ +#include "config.h" #include "facilities.h" #include #include -#include #include -#include +#include +#include #include +#include #include #include +#include + +#include +#include #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__) @@ -18,195 +24,192 @@ #define syslog_debug(msg, ...) #endif -static int extract_val_string(char ** output, toml_table_t *table, char* name) { - toml_raw_t raw; - if (0 == (raw = toml_raw_in(table, name))) { - syslog_err("[facilities] [config] error extracting %s from file", name); - return 1; +static int fetch_target_address(char** addresses, uint16_t addresses_len) { + int index = -1; + + for (int i = 0; i < addresses_len; ++i) { + char* addr = addresses[i]; + + // If TCP, ignore receiver addresses, e.g. tcp://*:[port] + if (!memcmp(addr, "tcp", 3)) { + bool found_astk = false; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '*') { + found_astk = true; + break; + } + } + + if (found_astk) continue; + } + + index = i; + break; } - if (toml_rtos(raw, (char**) output)) { - syslog_err("[facilities] [config] error extracting %s to string", name); - return 1; - } - - return 0; + return index; } -static int extract_val_bool(int * output, toml_table_t *table, char* name) { - toml_raw_t raw; - if (0 == (raw = toml_raw_in(table, name))) { - syslog_err("[facilities] [config] error extracting %s from file", name); - return 1; - } - - if (toml_rtob(raw, output)) { - syslog_err("[facilities] [config] error extracting %s to bool", name); - return 1; - } - - return 0; -} - -static int extract_val_int(int64_t * output, toml_table_t *table, char* name) { - toml_raw_t raw; - if (0 == (raw = toml_raw_in(table, name))) { - syslog_err("[facilities] [config] error extracting %s from file", name); - return 1; - } - - if (toml_rtoi(raw, output)) { - syslog_err("[facilities] [config] error extracting %s to int", name); - return 1; - } - - return 0; -} - -static int extract_val_double(double * output, toml_table_t *table, char* name) { - toml_raw_t raw; - if (0 == (raw = toml_raw_in(table, name))) { - syslog_err("[itss] [config] error extracting %s from file", name); - return 1; - } - - if (toml_rtod(raw, output)) { - syslog_err("[itss] [config] error extracting %s to double", name); - return 1; - } - - return 0; -} - -int itss_config(void* facilities_s, char* config_file) { +int facilities_config(void* facilities_s) { int rv = 0; facilities_t *facilities = (facilities_t*) facilities_s; - FILE* fp; - toml_table_t* conf; + + it2s_config_t* config = calloc(1, sizeof(it2s_config_t)); + rv = it2s_config_read("/etc/it2s/itss.toml", config); + if (rv) goto cleanup; - char errbuf[200]; + facilities->zmq.responders = calloc(config->facilities.zmq.addresses_len, sizeof(zmq_pollitem_t)); + facilities->zmq.n_responders = 0; - /* Open the file. */ - if (0 == (fp = fopen(config_file, "r"))) { - syslog_err("[facilities] [config] couldn't open config file"); - return 1; + for (int i = 0; i < config->facilities.zmq.addresses_len; ++i) { + char* addr = config->facilities.zmq.addresses[i]; + + // IPC + if (!memcmp(addr, "ipc", 3)) { + + // Create dir + int lp = 0; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '/') lp = j; + } + + char dir[256]; + memcpy(dir, addr+6, lp-6); + dir[lp-6] = 0; + + struct stat st = {0}; + if (stat(dir, &st) == -1) { + mkdir(dir, 0777); + } + + // Bind + void* socket = zmq_socket(facilities->zmq.ctx, ZMQ_REP); + zmq_bind(socket, addr); + + facilities->zmq.responders[facilities->zmq.n_responders].socket = socket; + facilities->zmq.responders[facilities->zmq.n_responders].events = ZMQ_POLLIN; + ++facilities->zmq.n_responders; + + } else if (!memcmp(addr, "tcp", 3)) { + + bool found_astk = false; + for (int j = 0; j < strlen(addr); ++j) { + if (addr[j] == '*') { + found_astk = true; + break; + } + } + + if (found_astk) { + // Bind + void* socket = zmq_socket(facilities->zmq.ctx, ZMQ_REP); + zmq_bind(socket, addr); + + facilities->zmq.responders[facilities->zmq.n_responders].socket = socket; + facilities->zmq.responders[facilities->zmq.n_responders].events = ZMQ_POLLIN; + ++facilities->zmq.n_responders; + } + } } - /* Run the file through the parser. */ - conf = toml_parse_file(fp, errbuf, sizeof(errbuf)); - if (0 == conf) { - syslog_err("[facilities] [config] error while parsing config file"); - return 1; + // Fetch [transport] address + int index = fetch_target_address(config->transport.zmq.addresses, config->transport.zmq.addresses_len); + if (index != -1) { + facilities->zmq.transport_address = malloc(strlen(config->transport.zmq.addresses[index])+1); + strcpy(facilities->zmq.transport_address, config->transport.zmq.addresses[index]); + } else { + syslog_err("[facilities] [config] a valid address for [transport] was not found"); + rv = 1; + goto cleanup; } - fclose(fp); + // Fetch [applications] address + index = fetch_target_address(config->applications.zmq.addresses, config->applications.zmq.addresses_len); + if (index != -1) { + facilities->zmq.applications_address = malloc(strlen(config->applications.zmq.addresses[index])+1); + strcpy(facilities->zmq.applications_address, config->applications.zmq.addresses[index]); + } else { + syslog_err("[facilities] [config] a valid address for [applications] was not found"); + rv = 1; + goto cleanup; + } - // Tables - toml_table_t *general, *security, *denm, *cam, *beacon, *replay, *ivim, *protected_zones, *gps; + // Fetch [security] address + index = fetch_target_address(config->security.zmq.addresses, config->security.zmq.addresses_len); + if (index != -1) { + facilities->zmq.security_address = malloc(strlen(config->security.zmq.addresses[index])+1); + strcpy(facilities->zmq.security_address, config->security.zmq.addresses[index]); + } else { + syslog_err("[facilities] [config] a valid address for [security] was not found"); + rv = 1; + goto cleanup; + } - if (0 == (general = toml_table_in(conf, "general"))) {syslog_err("[facilities] [config] failed locating [general] table"); return 1;} - if (0 == (security = toml_table_in(conf, "security"))) {syslog_err("[facilities] [config] failed locating [security] table"); return 1;} - if (0 == (denm = toml_table_in(conf, "denm"))) {syslog_err("[facilities] [config] failed locating [denm] table"); return 1;} - if (0 == (cam = toml_table_in(conf, "cam"))) {syslog_err("[facilities] [config] failed locating [cam] table"); return 1;} - if (0 == (replay = toml_table_in(conf, "replay"))) {syslog_err("[facilities] [config] failed locating [replay] table"); return 1;} - if (0 == (ivim = toml_table_in(conf, "ivim"))) {syslog_err("[facilities] [config] failed locating [ivim] table"); return 1;} - if (0 == (protected_zones = toml_table_in(conf, "protected-zones"))) {syslog_err("[facilities] [config] failed locating [protected-zones] table"); return 1;} - if (0 == (gps = toml_table_in(conf, "gps"))) {syslog_err("[facilities] [config] failed locating [gps] table"); return 1;} + // Fetch [management] address + index = fetch_target_address(config->management.zmq.addresses, config->management.zmq.addresses_len); + if (index != -1) { + facilities->zmq.management_address = malloc(strlen(config->management.zmq.addresses[index])+1); + strcpy(facilities->zmq.management_address, config->management.zmq.addresses[index]); + } else { + syslog_err("[facilities] [config] a valid address for [management] was not found"); + rv = 1; + goto cleanup; + } // Values // General - char *itss_type = NULL; - rv += extract_val_string(&itss_type, general, "itss-type"); - if (strcmp("obu", itss_type) == 0) { + if (!strcmp("obu", config->general.itss_type)) { facilities->station_type = 5; - } else if (strcmp("rsu", itss_type) == 0) { + } else if (!strcmp("rsu", config->general.itss_type)) { facilities->station_type = 15; } else { - syslog_err("[facilities] [config] Unrecognized ITSS type, running as unkown"); - facilities->station_type = 0; + syslog_err("[facilities] [config] unrecognized ITSS type, running as OBU"); + facilities->station_type = 5; } - free(itss_type); - int use_security = 1; - rv += extract_val_bool(&use_security, security, "use-security"); - facilities->use_security = use_security; + facilities->use_security = config->security.use_security; - int station_id_random = 1; - rv += extract_val_bool(&station_id_random, security, "id-random"); - facilities->id_random = station_id_random; - if (station_id_random) { + facilities->id_random = config->security.identity.random; + if (facilities->id_random) { srand(time(NULL)); facilities->station_id = rand(); } else { - int64_t station_id_number; - rv += extract_val_int(&station_id_number, security, "station-id"); - facilities->station_id = station_id_number; + facilities->station_id = config->security.identity.station_id; } // DENM - int64_t denm_default_event_duration; - rv += extract_val_int(&denm_default_event_duration, denm, "default-event-duration"); - facilities->den->default_event_duration = denm_default_event_duration; - - int64_t denm_nmax_active_events; - rv += extract_val_int(&denm_nmax_active_events, denm, "nmax-active-events"); - facilities->den->n_max_events = denm_nmax_active_events; + facilities->den->default_event_duration = config->facilities.denm.default_event_duration; + facilities->den->n_max_events = config->facilities.denm.nmax_active_events; // CAM - int cam_activate = 1; - rv += extract_val_bool(&cam_activate, cam, "activate"); - facilities->lightship->active = cam_activate; - - int64_t obu_cam_period_min; - rv += extract_val_int(&obu_cam_period_min, cam, "obu-period-min"); - facilities->lightship->vehicle_gen_min = obu_cam_period_min; - - int64_t obu_cam_period_max; - rv += extract_val_int(&obu_cam_period_max, cam, "obu-period-max"); - facilities->lightship->vehicle_gen_max = obu_cam_period_max; - - int64_t rsu_cam_period_min; - rv += extract_val_int(&rsu_cam_period_min, cam, "rsu-period-min"); - facilities->lightship->rsu_gen_min = rsu_cam_period_min; - - int64_t rsu_vehicle_permanence; - rv += extract_val_int(&rsu_vehicle_permanence, cam, "rsu-vehicle-permanence"); - facilities->lightship->rsu_vehicle_permanence = rsu_vehicle_permanence; + facilities->lightship->active = config->facilities.cam.activate; + facilities->lightship->vehicle_gen_min = config->facilities.cam.obu_period_min; + facilities->lightship->vehicle_gen_max = config->facilities.cam.obu_period_max; + facilities->lightship->rsu_gen_min = config->facilities.cam.rsu_period_min; + facilities->lightship->rsu_vehicle_permanence = config->facilities.cam.rsu_vehicle_permanence; // IVIM - int64_t nmax_active_services; - rv += extract_val_int(&nmax_active_services, ivim, "nmax-active-services"); - facilities->infrastructure->n_max_services = nmax_active_services; - - int64_t replay_interval; - rv += extract_val_int(&replay_interval, ivim, "replay-interval"); - facilities->infrastructure->replay_interval = replay_interval; - - int64_t default_service_duration; - rv += extract_val_int(&default_service_duration, ivim, "default-service-duration"); - facilities->infrastructure->default_service_duration = default_service_duration * 60000; + facilities->infrastructure->n_max_services = config->facilities.ivim.nmax_active_services; + facilities->infrastructure->replay_interval = config->facilities.ivim.replay_interval; + facilities->infrastructure->default_service_duration = config->facilities.ivim.default_service_duration * 60000; // Replay - int replay_active = 1; - rv += extract_val_bool(&replay_active, replay, "activate"); - facilities->replay = replay_active; + facilities->replay = config->networking.replay.activate; // PZ if (facilities->station_type == 15) { - char *pz_path; - rv += extract_val_string(&pz_path, protected_zones, "path"); int i = 0; - DIR *d = opendir(pz_path); + DIR *d = opendir(config->facilities.protected_zones.path); struct dirent *dir; char file[256]; char pz_xml[2048]; if (d) { while ((dir = readdir(d)) != NULL && i < 16) { if (dir->d_name[0] == '.') continue; - sprintf(file, "%s/%s", pz_path, dir->d_name); + sprintf(file, "%s/%s", config->facilities.protected_zones.path, dir->d_name); FILE *fp = fopen(file, "r"); if (!fp) continue; fseek(fp, 0, SEEK_END); @@ -237,24 +240,49 @@ int itss_config(void* facilities_s, char* config_file) { } closedir(d); } - free(pz_path); } - int gps_fixed = 0; - rv += extract_val_bool(&gps_fixed, gps, "fixed"); - facilities->gps_fixed = gps_fixed; - if (gps_fixed) { - double latitude; - rv += extract_val_double(&latitude, gps, "latitude"); - facilities->latitude = latitude; + pthread_mutex_init(&facilities->epv.lock, NULL); - double longitude; - rv += extract_val_double(&longitude, gps, "longitude"); - facilities->longitude = longitude; + ManagementRequest_t* mreq = calloc(1, sizeof(ManagementRequest_t)); + mreq->present = ManagementRequest_PR_attributes; + mreq->choice.attributes.coordinates = 1; + mreq->choice.attributes.altitude = 1; + mreq->choice.attributes.heading = 1; + mreq->choice.attributes.speed = 1; + void* management_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ); + zmq_connect(management_socket, facilities->zmq.management_address); + uint8_t buffer[256]; + asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementRequest, NULL, mreq, buffer, 256); + + zmq_send(management_socket, buffer, enc.encoded, 0); + zmq_recv(management_socket, buffer, 256, 0); + + ManagementReply_t* mrep = calloc(1, sizeof(ManagementReply_t)); + oer_decode(NULL, &asn_DEF_ManagementReply, (void**) &mrep, buffer, 256); + long lat, lon, alt, alt_conf; + if (mrep->returnCode == ManagementReplyReturnCode_accepted && mrep->attributes && + mrep->attributes->coordinates && mrep->attributes->altitude && + mrep->attributes->heading && mrep->attributes->speed) { + facilities->epv.latitude = mrep->attributes->coordinates->latitude; + facilities->epv.longitude = mrep->attributes->coordinates->longitude; + facilities->epv.altitude = mrep->attributes->altitude->altitudeValue; + facilities->epv.altitude_conf = mrep->attributes->altitude->altitudeConfidence; + facilities->epv.heading = mrep->attributes->heading->headingValue; + facilities->epv.heading_conf = mrep->attributes->heading->headingConfidence; + facilities->epv.speed = mrep->attributes->speed->speedValue; + facilities->epv.speed_conf = mrep->attributes->speed->speedConfidence; + } else { + syslog_err("[applications] rejected MR attribute request"); + rv = 1; + goto cleanup; } + ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq); + ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep); + zmq_close(management_socket); - toml_free(conf); - +cleanup: + it2s_config_free(config); return rv; } diff --git a/src/config.h b/src/config.h index fd914b1..2fae92e 100644 --- a/src/config.h +++ b/src/config.h @@ -1,6 +1,6 @@ -#ifndef ITSS_CONFIG_H -#define ITSS_CONFIG_H +#ifndef FACILITIES_CONFIG_H +#define FACILITIES_CONFIG_H -int itss_config(void *facilities_s, char * config_file); +int facilities_config(void *facilities_s); #endif diff --git a/src/denm.h b/src/denm.h index 4203fe1..c73ec11 100644 --- a/src/denm.h +++ b/src/denm.h @@ -21,8 +21,8 @@ typedef struct event { uint64_t detection_time; uint64_t reference_time; uint64_t expiration_time; - uint32_t latitude; - uint32_t longitude; + int32_t latitude; + int32_t longitude; bool enabled; enum EVENT_STATE state; } event_t; diff --git a/src/facilities.c b/src/facilities.c index 6743970..e6494a3 100644 --- a/src/facilities.c +++ b/src/facilities.c @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -201,8 +202,9 @@ static int security_indication(facilities_t *facilities, void* responder_secured case SecurityIdChangeEventType_prepare: pthread_mutex_lock(&facilities->lock); pthread_mutex_lock(&facilities->lightship->lock); - + facilities->in_idchange = true; break; + case SecurityIdChangeEventType_commit: ; @@ -221,10 +223,12 @@ static int security_indication(facilities_t *facilities, void* responder_secured facilities->lightship->next_cam_max = 0; facilities->lightship->next_cam_min = 0; + facilities->in_idchange = false; pthread_mutex_unlock(&facilities->lightship->lock); - break; + case SecurityIdChangeEventType_abort: + facilities->in_idchange = false; pthread_mutex_unlock(&facilities->lock); break; @@ -256,37 +260,67 @@ cleanup: return rv; } - -static void* securer(void *fc) { +static int management_indication(facilities_t* facilities, void* responder, uint8_t* msg, uint32_t msg_len) { int rv = 0; - facilities_t *facilities = (facilities_t*) fc; + uint8_t code = 0; - void *responder_secured = zmq_socket(facilities->ctx, ZMQ_REP); - int rc = zmq_bind(responder_secured, FACILITIES_SECURED_ADDRESS); + ManagementIndication_t* mi = calloc(1, sizeof(ManagementIndication_t)); - uint8_t buffer[PACKET_MAX_LEN]; + asn_dec_rval_t dec = oer_decode(NULL, &asn_DEF_ManagementIndication, (void**) &mi, msg, msg_len); + if (dec.code) { + rv = 1; + code = 1; + zmq_send(responder, &code, 1, 0); + goto cleanup; + } + zmq_send(responder, &code, 1, 0); - while (!facilities->exit) { - zmq_recv(responder_secured, buffer, PACKET_MAX_LEN, 0); - rv = security_indication(facilities, responder_secured, buffer, PACKET_MAX_LEN); + if (mi->present == ManagementIndication_PR_attributes) { + pthread_mutex_lock(&facilities->epv.lock); + if (mi->choice.attributes.coordinates) { + facilities->epv.latitude = mi->choice.attributes.coordinates->latitude; + facilities->epv.longitude = mi->choice.attributes.coordinates->longitude; + } + if (mi->choice.attributes.speed) { + facilities->epv.speed = mi->choice.attributes.speed->speedValue; + facilities->epv.speed_conf = mi->choice.attributes.speed->speedConfidence; + } + if (mi->choice.attributes.heading) { + facilities->epv.heading = mi->choice.attributes.heading->headingValue; + facilities->epv.heading_conf = mi->choice.attributes.heading->headingConfidence; + } + if (mi->choice.attributes.altitude) { + facilities->epv.altitude = mi->choice.attributes.altitude->altitudeValue; + facilities->epv.altitude_conf = mi->choice.attributes.altitude->altitudeConfidence; + } + pthread_mutex_unlock(&facilities->epv.lock); } - return NULL; +cleanup: + ASN_STRUCT_FREE(asn_DEF_ManagementIndication, mi); + + return rv; } + void* tx(void* fc) { + int rv = 0; + facilities_t *facilities = (facilities_t*) fc; queue_t* queue = facilities->tx_queue; uint8_t code; + int wait_ms = 1000; - void* applications_socket = zmq_socket(facilities->ctx, ZMQ_REQ); - zmq_connect(applications_socket, APPLICATION_ADDRESS); + void* applications_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ); + zmq_setsockopt(applications_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int)); + zmq_connect(applications_socket, facilities->zmq.applications_address); - void* transport_socket = zmq_socket(facilities->ctx, ZMQ_REQ); - zmq_connect(transport_socket, TRANSPORT_ADDRESS); + void* transport_socket = zmq_socket(facilities->zmq.ctx, ZMQ_REQ); + zmq_setsockopt(transport_socket, ZMQ_RCVTIMEO, &wait_ms, sizeof(int)); + zmq_connect(transport_socket, facilities->zmq.transport_address); queue_t* stream = queue_init(); @@ -310,12 +344,14 @@ void* tx(void* fc) { case 3: syslog_debug("[facilities]-> sending BDR to ->[transport] (%dB)", stream->packet_len[i]); zmq_send(transport_socket, stream->packet[i], stream->packet_len[i], 0); - zmq_recv(transport_socket, &code, 1, 0); + rv = zmq_recv(transport_socket, &code, 1, 0); + if (rv == -1) {syslog_err("[facilities]-> timeout sending BDR to ->[transport]");} break; case 5: - syslog_debug("[facilities]-> sending FDI to ->[application] (%dB)", stream->packet_len[i]); + syslog_debug("[facilities]-> sending FDI to ->[applications] (%dB)", stream->packet_len[i]); zmq_send(applications_socket, stream->packet[i], stream->packet_len[i], 0); - zmq_recv(applications_socket, &code, 1, 0); + rv = zmq_recv(applications_socket, &code, 1, 0); + if (rv == -1) {syslog_err("[facilities]-> timeout sending FDI to ->[applications]");} break; } } @@ -325,11 +361,14 @@ void* tx(void* fc) { } int main() { + int rv = 0; + syslog_info("[facilities] starting"); facilities_t facilities; facilities.exit = false; + facilities.zmq.ctx = zmq_ctx_new(); facilities.lightship = lightship_init(); facilities.tx_queue = queue_init(); facilities.den = calloc(1, sizeof(den_t)); @@ -337,33 +376,17 @@ int main() { pthread_mutex_init(&facilities.lock, NULL); - struct stat st = {0}; - if (stat("/tmp/itss", &st) == -1) { - mkdir("/tmp/itss", 0777); - } - time_t t; srand((unsigned) time(&t)); - if (itss_config(&facilities, "/etc/it2s/itss.toml")) return 1; - - void *context = zmq_ctx_new(); - facilities.ctx = context; - void *responder = zmq_socket(context, ZMQ_REP); - int rc = zmq_bind(responder, FACILITIES_ADDRESS); - - void *security_socket = zmq_socket(context, ZMQ_REQ); - rc = zmq_bind(security_socket, SECURITY_ADDRESS); - + if (facilities_config(&facilities)) return 1; facilities.lightship->type = facilities.station_type; + facilities.in_idchange = false; // Tx pthread_create(&facilities.transmitting, NULL, tx, (void*) &facilities); - // Securing - pthread_create(&facilities.securing, NULL, securer, (void*) &facilities); - // CA pthread_create(&facilities.ca_service, NULL, ca_service, (void*) &facilities); @@ -374,27 +397,57 @@ int main() { pthread_create(&facilities.infrastructure_service, NULL, infrastructure_service, (void*) &facilities); uint8_t buffer[PACKET_MAX_LEN]; - uint8_t code; syslog_info("[facilities] listening"); + uint8_t code; while (!facilities.exit) { - zmq_recv(responder, buffer, PACKET_MAX_LEN, 0); + zmq_poll(facilities.zmq.responders, facilities.zmq.n_responders, -1); - switch (buffer[0]) { - case 3: - code = transport_indication(&facilities, responder, buffer+1, PACKET_MAX_LEN-1); - break; - case 5: - code = facilities_request(&facilities, responder, buffer+1, PACKET_MAX_LEN-1); - break; - default: - syslog_debug("[facilities] unrecognized service"); - code = 1; - zmq_send(responder, &code, 1, 0); - continue; + for (int i = 0; i < facilities.zmq.n_responders; ++i) { + if (facilities.zmq.responders[i].revents) { + zmq_recv(facilities.zmq.responders[i].socket, buffer, PACKET_MAX_LEN, 0); + + switch (buffer[0]) { + case 3: + if (!facilities.in_idchange) { + transport_indication(&facilities, facilities.zmq.responders[i].socket, buffer+1, PACKET_MAX_LEN-1); + } else { + code = 1; + zmq_send(facilities.zmq.responders[i].socket, &code, 1, 0); + } + break; + + case 5: + if (!facilities.in_idchange) { + facilities_request(&facilities, facilities.zmq.responders[i].socket, buffer+1, PACKET_MAX_LEN-1); + } else { + code = 1; + zmq_send(facilities.zmq.responders[i].socket, &code, 1, 0); + } + break; + + case 6: + management_indication(&facilities, facilities.zmq.responders[i].socket, buffer+1, PACKET_MAX_LEN-1); + break; + + case 7: + security_indication(&facilities, facilities.zmq.responders[i].socket, buffer+1, PACKET_MAX_LEN-1); + break; + + default: + syslog_debug("[facilities] unrecognized service"); + code = 1; + zmq_send(facilities.zmq.responders[i].socket, &code, 1, 0); + continue; + } + } } } + pthread_join(facilities.transmitting, NULL); + pthread_join(facilities.ca_service, NULL); + pthread_join(facilities.den_service, NULL); + pthread_join(facilities.infrastructure_service, NULL); return 0; } diff --git a/src/facilities.h b/src/facilities.h index 685571e..f2d4e57 100644 --- a/src/facilities.h +++ b/src/facilities.h @@ -3,27 +3,43 @@ #include #include +#include #include "cam.h" #include "denm.h" #include "infrastructure.h" #include "queue.h" -#define FACILITIES_ADDRESS "ipc:///tmp/itss/facilities" -#define FACILITIES_SECURED_ADDRESS "ipc:///tmp/itss/facilities-secured" -#define TRANSPORT_ADDRESS "ipc:///tmp/itss/transport" -#define APPLICATION_ADDRESS "ipc:///tmp/itss/application" -#define SECURITY_ADDRESS "ipc:///tmp/itss/security" +typedef struct epv { + pthread_mutex_t lock; + + long latitude; + long longitude; + long altitude; + long altitude_conf; + long speed; + long speed_conf; + long heading; + long heading_conf; +} epv_t; typedef struct facilities { pthread_t ca_service; pthread_t den_service; pthread_t infrastructure_service; pthread_t transmitting; - pthread_t securing; // ZMQ - void* ctx; + struct { + void* ctx; + zmq_pollitem_t* responders; + uint16_t n_responders; + + char* applications_address; + char* transport_address; + char* security_address; + char* management_address; + } zmq; // Transmitter queue_t* tx_queue; @@ -44,10 +60,9 @@ typedef struct facilities { pthread_mutex_t lock; uint64_t station_id; bool id_random; + bool in_idchange; - bool gps_fixed; - double latitude; - double longitude; + epv_t epv; bool exit; } facilities_t;