TiagoRG
/
it2s-itss-facilities
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

New management svc, distributed services, new config lib

This commit is contained in:
emanuel 2021-03-26 21:57:47 +00:00
parent c569b2c725
commit f80f67c1bf
8 changed files with 376 additions and 448 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/CMakeLists.txt
View File

@ -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

274
src/cam.c
View File

@ -5,7 +5,6 @@
#include <itss-transport/BTPDataRequest.h>
#include <itss-transport/BTPDataIndication.h>
#include <camv2/CAM.h>
#include <it2s-gps.h>
#include <stdint.h>
#include <time.h>
@ -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;
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;
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) {

2
src/cam.h
View File

@ -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);

336
src/config.c
View File

@ -1,13 +1,19 @@
#include "config.h"
#include "facilities.h"
#include <syslog.h>
#include <string.h>
#include <toml.h>
#include <stdlib.h>
#include <it2s-gps.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <it2s-tiles.h>
#include <it2s-config.h>
#include <dirent.h>
#include <camv2/ProtectedCommunicationZone.h>
#include <zmq.h>
#include <itss-management/ManagementRequest.h>
#include <itss-management/ManagementReply.h>
#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 (toml_rtos(raw, (char**) output)) {
syslog_err("[facilities] [config] error extracting %s to string", name);
return 1;
if (found_astk) continue;
}
return 0;
index = i;
break;
}
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;
char errbuf[200];
it2s_config_t* config = calloc(1, sizeof(it2s_config_t));
rv = it2s_config_read("/etc/it2s/itss.toml", config);
if (rv) goto cleanup;
/* Open the file. */
if (0 == (fp = fopen(config_file, "r"))) {
syslog_err("[facilities] [config] couldn't open config file");
return 1;
facilities->zmq.responders = calloc(config->facilities.zmq.addresses_len, sizeof(zmq_pollitem_t));
facilities->zmq.n_responders = 0;
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;
}
/* 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;
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);
}
fclose(fp);
// Bind
void* socket = zmq_socket(facilities->zmq.ctx, ZMQ_REP);
zmq_bind(socket, addr);
// Tables
toml_table_t *general, *security, *denm, *cam, *beacon, *replay, *ivim, *protected_zones, *gps;
facilities->zmq.responders[facilities->zmq.n_responders].socket = socket;
facilities->zmq.responders[facilities->zmq.n_responders].events = ZMQ_POLLIN;
++facilities->zmq.n_responders;
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;}
} 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;
}
}
}
// 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;
}
// 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;
}
// 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;
}
// 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;
}

6
src/config.h
View File

@ -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

4
src/denm.h
View File

@ -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;

137
src/facilities.c
View File

@ -11,6 +11,7 @@
#include <itss-facilities/FacilitiesDataResult.h>
#include <itss-security/SecurityIndication.h>
#include <itss-security/SecurityResponse.h>
#include <itss-management/ManagementIndication.h>
#include <camv2/CAM.h>
#include <denmv2/DENM.h>
@ -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);
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:
code = transport_indication(&facilities, responder, buffer+1, PACKET_MAX_LEN-1);
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:
code = facilities_request(&facilities, responder, buffer+1, PACKET_MAX_LEN-1);
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(responder, &code, 1, 0);
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;
}

33
src/facilities.h
View File

@ -3,27 +3,43 @@
#include <pthread.h>
#include <stdbool.h>
#include <zmq.h>
#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
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;