237 lines
7.1 KiB
C
237 lines
7.1 KiB
C
#include <itss-transport/BTPDataRequest.h>
|
|
#include <itss-transport/BTPDataIndication.h>
|
|
#include <itss-facilities/FacilitiesDataIndication.h>
|
|
#include <itss-facilities/FacilitiesDataRequest.h>
|
|
#include <itss-management/ManagementRequest.h>
|
|
#include <itss-management/ManagementReply.h>
|
|
|
|
#include <pthread.h>
|
|
#include <zmq.h>
|
|
#include <unistd.h>
|
|
|
|
void hex2bin(char* hex, uint8_t** bin, uint16_t* bin_len) {
|
|
*bin_len = strlen(hex) / 2;
|
|
*bin = malloc(*bin_len);
|
|
char substr[3];
|
|
substr[2] = '\0';
|
|
for (int i = 0; i < *bin_len; ++i) {
|
|
memcpy(substr, hex + i * 2, 2);
|
|
(*bin)[i] = (uint8_t)strtoul(substr, NULL, 16);
|
|
}
|
|
}
|
|
|
|
int init(void* ctx) {
|
|
printf("Initializing management simulator \n");
|
|
fflush(stdout);
|
|
|
|
void* management_socket = zmq_socket(ctx, ZMQ_REP);
|
|
zmq_bind(management_socket, "ipc:///tmp/itss/management");
|
|
uint8_t buffer[2048];
|
|
|
|
// Ignore first request (set)
|
|
zmq_recv(management_socket, buffer, 2048, 0);
|
|
printf(" - Received MREQ.set \n");
|
|
fflush(stdout);
|
|
int code = 0;
|
|
zmq_send(management_socket, &code, 1, 0);
|
|
|
|
// Request get
|
|
zmq_recv(management_socket, buffer, 2048, 0);
|
|
int rv = 0;
|
|
|
|
ManagementRequest_t* mreq = NULL;
|
|
asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_ManagementRequest, (void**) &mreq, buffer, 2048);
|
|
if (dec.code) {
|
|
rv = 1;
|
|
printf(" FAIL: MREQ decode error\n");
|
|
goto cleanup;
|
|
}
|
|
printf(" - Received MREQ\n");
|
|
|
|
ManagementReply_t* mrep = calloc(1, sizeof(ManagementReply_t));
|
|
switch (mreq->present) {
|
|
case ManagementRequest_PR_get:
|
|
mrep->returnCode = ManagementReplyReturnCode_accepted;
|
|
mrep->attributes = calloc(1, sizeof(ManagementReplyAttributes_t));
|
|
|
|
if (mreq->choice.get.stationId) {
|
|
mrep->attributes->stationId = malloc(sizeof(long));
|
|
*mrep->attributes->stationId = 78;
|
|
}
|
|
|
|
if (mreq->choice.get.gpsType) {
|
|
mrep->attributes->gpsType = malloc(sizeof(long));
|
|
*mrep->attributes->gpsType = 0;
|
|
}
|
|
|
|
if (mreq->choice.get.clockType) {
|
|
mrep->attributes->clockType = malloc(sizeof(long));
|
|
*mrep->attributes->clockType = 0;
|
|
}
|
|
|
|
if (mreq->choice.get.clock) {
|
|
mrep->attributes->clock = calloc(1, sizeof(TimestampIts_t));
|
|
asn_ulong2INTEGER(mrep->attributes->clock, 666);
|
|
}
|
|
|
|
if (mreq->choice.get.clockOffset) {
|
|
mrep->attributes->clockOffset = calloc(1, sizeof(TimestampIts_t));
|
|
asn_ulong2INTEGER(mrep->attributes->clockOffset, 0);
|
|
}
|
|
|
|
if (mreq->choice.get.coordinates) {
|
|
mrep->attributes->coordinates = calloc(1, sizeof(WGS84Coordinates_t));
|
|
mrep->attributes->coordinates->latitude = 0;
|
|
mrep->attributes->coordinates->longitude = 0;
|
|
}
|
|
|
|
if (mreq->choice.get.altitude) {
|
|
mrep->attributes->altitude = calloc(1, sizeof(Altitude_t));
|
|
mrep->attributes->altitude->altitudeValue = 0;
|
|
mrep->attributes->altitude->altitudeConfidence = 1;
|
|
}
|
|
|
|
if (mreq->choice.get.speed) {
|
|
mrep->attributes->speed = calloc(1, sizeof(Speed_t));
|
|
mrep->attributes->speed->speedValue = 0;
|
|
mrep->attributes->speed->speedConfidence = 1;
|
|
}
|
|
|
|
if (mreq->choice.get.heading) {
|
|
mrep->attributes->heading = calloc(1, sizeof(Heading_t));
|
|
mrep->attributes->heading->headingValue = 0;
|
|
mrep->attributes->heading->headingConfidence = 1;
|
|
}
|
|
break;
|
|
|
|
case ManagementRequest_PR_set:
|
|
default:
|
|
rv = 1;
|
|
printf(" FAIL: MREQ. not GET (%d)\n", mreq->present);
|
|
goto cleanup;
|
|
}
|
|
|
|
asn_enc_rval_t enc = oer_encode_to_buffer(&asn_DEF_ManagementReply, NULL, mrep, buffer, 256);
|
|
zmq_send(management_socket, buffer, enc.encoded, 0);
|
|
|
|
printf(" OK\n");
|
|
|
|
cleanup:
|
|
ASN_STRUCT_FREE(asn_DEF_ManagementRequest, mreq);
|
|
ASN_STRUCT_FREE(asn_DEF_ManagementReply, mrep);
|
|
|
|
return rv;
|
|
}
|
|
|
|
int cam_gen(void* ctx) {
|
|
printf("Testing CAM generation:\n");
|
|
|
|
void* transport_socket = zmq_socket(ctx, ZMQ_REP);
|
|
|
|
zmq_bind(transport_socket, "ipc:///tmp/itss/transport");
|
|
uint8_t buffer[2048];
|
|
zmq_recv(transport_socket, buffer, 2048, 0);
|
|
int code = 0;
|
|
zmq_send(transport_socket, &code, 1, 0);
|
|
|
|
zmq_close(transport_socket);
|
|
|
|
BTPDataRequest_t* bdr = NULL;
|
|
asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_BTPDataRequest, (void**) &bdr, buffer+1, 2047);
|
|
if (dec.code) {
|
|
printf(" FAIL\n");
|
|
return 1;
|
|
}
|
|
printf(" - Received CAM BDR (%ldB)\n", bdr->data.size);
|
|
|
|
if (bdr->destinationPort == Port_cam) {
|
|
printf(" OK\n");
|
|
} else {
|
|
printf(" FAIL\n");
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
int forward_up(void* ctx) {
|
|
printf("Testing forwarding up:\n");
|
|
BTPDataIndication_t* bdi = calloc(1, sizeof(BTPDataIndication_t));
|
|
|
|
char* cam_hex = "02027dfddf4403ca4059bba5f38cc40dba9ffffffc2230eff200e11fc0078082a88a8337fee3fff600004dffea800618d08018efff14003ac68800c77ff8e002263460063bffd1000fb1a30031dffe2800958d30018efff840048c68800c77ffae002c63480063bffbd001a31a40031dfff28002d8cf0018c0";
|
|
|
|
uint8_t* cam;
|
|
uint16_t cam_len;
|
|
hex2bin(cam_hex, &cam, &cam_len);
|
|
|
|
bdi->data.buf = cam;
|
|
bdi->data.size = cam_len;
|
|
bdi->gnDestinationAddress.buf = calloc(1, 6);
|
|
bdi->gnDestinationAddress.size = 6;
|
|
bdi->destinationPort = Port_cam;
|
|
|
|
uint8_t b_bdi_cam[512];
|
|
b_bdi_cam[0] = 3;
|
|
|
|
asn_enc_rval_t enc = asn_encode_to_buffer(NULL, ATS_CANONICAL_OER, &asn_DEF_BTPDataIndication, bdi, b_bdi_cam+1, 511);
|
|
|
|
void* facilities_socket = zmq_socket(ctx, ZMQ_REQ);
|
|
|
|
int code = 0;
|
|
|
|
zmq_connect(facilities_socket, "ipc:///tmp/itss/facilities");
|
|
zmq_send(facilities_socket, b_bdi_cam, enc.encoded+1, 0);
|
|
zmq_recv(facilities_socket, &code, 1, 0);
|
|
if (code) {
|
|
printf(" FAIL\n");
|
|
return 1;
|
|
}
|
|
printf(" - Sent CAM BDI (%ldB)\n", bdi->data.size);
|
|
|
|
void* applications_socket = zmq_socket(ctx, ZMQ_REP);
|
|
|
|
zmq_bind(applications_socket, "ipc:///tmp/itss/applications");
|
|
uint8_t buffer[2048];
|
|
zmq_recv(applications_socket, buffer, 2048, 0);
|
|
code = 0;
|
|
zmq_send(applications_socket, &code, 1, 0);
|
|
|
|
zmq_close(applications_socket);
|
|
zmq_close(facilities_socket);
|
|
|
|
FacilitiesDataIndication_t* fdi = NULL;
|
|
asn_dec_rval_t dec = asn_decode(NULL, ATS_CANONICAL_OER, &asn_DEF_FacilitiesDataIndication, (void**) &fdi, buffer+1, 2047);
|
|
if (dec.code) {
|
|
printf(" FAIL\n");
|
|
return 1;
|
|
}
|
|
printf(" - Received CAM FDI (%ldB)\n", fdi->data.size);
|
|
|
|
if (fdi->data.size == bdi->data.size) {
|
|
printf(" OK\n");
|
|
} else {
|
|
printf(" FAIL\n");
|
|
return 1;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
int main() {
|
|
|
|
int rv = 0;
|
|
|
|
void* ctx = zmq_ctx_new();
|
|
|
|
//rv += init(ctx);
|
|
if (rv) return 1;
|
|
|
|
rv += forward_up(ctx);
|
|
rv += cam_gen(ctx);
|
|
|
|
return rv;
|
|
}
|
|
|