diff --git a/include/config.h b/include/config.h new file mode 100644 index 000000000..9a74d476d --- /dev/null +++ b/include/config.h @@ -0,0 +1,32 @@ +/** + * Configuration + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#ifndef _CONFIG_H_ +#define _CONFIG_H_ + +/// The version number of the s2ss server +#define VERSION "0.01" +/// The Procotol version determines the format of a struct msg +#define PROTOCOL 0 +/// The name of this node +#define NAME "acs" + +#define PRIORITY sched_get_priority_max(SCHED_FIFO) +#define CORES ((1 << 6) | (1 << 7)) + +/// Maximum number of double values in a struct msg +#define MAX_VALUES 4 +/// Maximum number of registrable hook functions per path +#define MAX_HOOKS 5 +/// Maximum number of paths +#define MAX_PATHS 2 +/// Maximum number of nodes +#define MAX_NODES 2 +/// Size of the stack which gets prefaulted during initialization +#define MAX_SAFE_STACK (16*1024) /* 16 KiB */ + +#endif /* _CONFIG_H_ */ diff --git a/include/msg.h b/include/msg.h new file mode 100644 index 000000000..57a513017 --- /dev/null +++ b/include/msg.h @@ -0,0 +1,58 @@ +/** + * Message format + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#ifndef _MSG_H_ +#define _MSG_H_ + +#include +#include + +#include "config.h" +#include "node.h" + +/** + * The structure of a message (OPAL-RT example format) + * + * This struct defines the format of a message. + * Its declared as "packed" because it represents the "on wire" data. + */ + +#if PROTOCOL != 0 + #error "Unknown protocol version!" +#endif + +struct msg +{ + /// Sender device ID + uint16_t dev_id; + /// Message ID + uint32_t msg_id; + /// Message length (data only) + uint16_t msg_len; + /// Message length (data only) + double data[MAX_VALUES]; +} __attribute__((packed)); + +/** + * Print a raw UDP packge in human readable form + * + * @param fd The file descriptor + * @param msg A pointer to the UDP message + */ +void msg_fprint(FILE *f, struct msg *m); + +/** + * Craft message with random-walking values + * + * NOTE: random-walking behaviour is not reentrant! + * + * @param msg A pointer to a struct msg + * @param dev_id The device id of the message + */ +void msg_random(struct msg *m, short dev_id); + +#endif /* _MSG_H_ */ diff --git a/include/node.h b/include/node.h new file mode 100644 index 000000000..df4be6183 --- /dev/null +++ b/include/node.h @@ -0,0 +1,79 @@ +/** + * Nodes + * + * The S2SS server connects multiple nodes. + * There are multiple types of nodes: + * - simulators + * - servers + * - workstations + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#ifndef _NODE_H_ +#define _NODE_H_ + +#include +#include + +enum node_type +{ + SIMULATOR, + SERVER, + WORKSTATION +}; + +struct node +{ + /// The socket descriptor + int sd; + // Remote address of the socket + struct sockaddr_in addr; + /// The type of this node + enum node_type type; + /// A short identifier of the node + char *name; +}; + +struct msg; /* forward decl */ + +/** + * @brief Create a new node + * + * Memory is allocated dynamically and has to be freed by node_destroy() + * + * @param name An acroynm, describing the node + * @param type The type of a node (SERVER, SIMULATOR, WORKSTATION) + * @param addr A string containing the node address + * @param port The UDP port of the node + * @return + * - 0 on success + * - otherwise on error occured + */ +struct node* node_create(const char *name, enum node_type type, const char *addr, int port); + +/** + * @brief Delete a node created by node_create() + * + * @param p A pointer to the node struct + */ +void node_destroy(struct node* n); + +/** + * Send a single message to a node + * + * @param sd The descriptor of the UDP socket + * @param msg A pointer to the UDP message + */ +int node_send(struct node *n, struct msg *m); + +/** + * Receive a single message from a node + * + * @param sd The descriptor of the UDP socket + * @param msg A pointer to the UDP message + */ +int node_recv(struct node *n, struct msg *m); + +#endif /* _NODE_H_ */ diff --git a/include/path.h b/include/path.h new file mode 100644 index 000000000..a96b2fd05 --- /dev/null +++ b/include/path.h @@ -0,0 +1,93 @@ +/** + * Message paths + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#ifndef _PATH_H_ +#define _PATH_H_ + +#include + +#include "config.h" +#include "node.h" +#include "msg.h" + +enum path_state +{ + UNKNOWN, + CONNECTED, + RUNNING, + STOPPED +}; + +/** + * @brief The datastructure for a path + */ +struct path +{ + struct node *in; + struct node *out[MAX_NODES]; + + /// Hooks are called for every message which is passed + int (*hooks[MAX_HOOKS])(struct msg *m); + + /// Counter for received messages + int msg_received; + + /// Counter for dropped messages + int msg_dropped; + + /// Last known message number + int seq_no; + + /// The current path state + enum path_state state; + + /// The path thread + pthread_t tid; +}; + +/** + * @brief Create a new path + * + * Memory is allocated dynamically and has to be freed by path_destroy() + * + * @param in The node we are receiving messages from + * @param out The nodes we are sending to + * @param len count of outgoing nodes + * @return + * - a pointer to the new path on success + * - NULL if an error occured + */ +struct path* path_create(struct node *in, struct node *out[], int len); + +/** + * @brief Delete a path created by path_create() + * + * @param p A pointer to the path struct + */ +void path_destroy(struct path *p); + +/** + * @brief Start a path + * + * @param p A pointer to the path struct + * @return + * - 0 on success + * - otherwise an error occured + */ +int path_start(struct path *p); + +/** + * @brief Stop a path + * + * @param p A pointer to the path struct + * @return + * - 0 on success + * - otherwise an error occured + */ +int path_stop(struct path *p); + +#endif /* _PATH_H_ */ diff --git a/include/utils.h b/include/utils.h new file mode 100644 index 000000000..c95d69818 --- /dev/null +++ b/include/utils.h @@ -0,0 +1,35 @@ +/** + * Some helper functions + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#ifndef _UTILS_H_ +#define _UTILS_H_ + +#include + +enum log_level +{ + DEBUG, + INFO, + WARN, + ERROR, + FATAL +}; + +/** + * @brief Logs variadic messages to stdout + * @param lvl The log level + * @param fmt The format string (printf alike) + */ +void print(enum log_level lvl, const char *fmt, ...); + +/** + * @brief Print short usage info to stdout + */ +void usage(); + +#endif /* _UTILS_H_ */ + diff --git a/src/main.c b/src/main.c new file mode 100644 index 000000000..a012cdee5 --- /dev/null +++ b/src/main.c @@ -0,0 +1,71 @@ +/** + * Main routine + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include +#include + +#include +#include + +#include "msg.h" +#include "utils.h" +#include "config.h" +#include "path.h" +#include "node.h" + +static struct node *nodes[MAX_NODES] = { NULL }; +static struct path *paths[MAX_PATHS] = { NULL }; + +/** + * Do your configuration here + */ +void init() +{ + nodes[0] = node_create("opal", SERVER, "localhost", 10200); + nodes[1] = node_create("sintef", SERVER, "localhost", 10201); + + paths[0] = path_create(nodes[0], &nodes[1], 1); + paths[1] = path_create(nodes[1], &nodes[0], 1); + + for (int i = 0; i < MAX_PATHS && paths[i]; i++) { + path_start(paths[i]); + } +} + +void quit() +{ + for (int i = 0; i < MAX_PATHS && paths[i]; i++) { + path_stop(paths[i]); + path_destroy(paths[i]); + } + + for (int i = 0; i < MAX_NODES && nodes[i]; i++) { + node_destroy(nodes[i]); + } +} + +int main(int argc, char *argv[]) +{ + if (argc != 1) { + printf("Usage: s2ss [config]\n"); + printf(" config is an optional path to a configuration file\n\n"); + printf("s2ss Simulator2Simulator Server v%s\n", VERSION); + printf("Copyright 2014, Institute for Automation of Complex Power Systems, EONERC\n"); + exit(EXIT_FAILURE); + } + + print(INFO, "Good morning! This is s2ss v%s", VERSION); + + init(); + signal(SIGINT, quit); + pause(); + + print(INFO, "Good night!"); + + return 0; +} diff --git a/src/msg.c b/src/msg.c new file mode 100644 index 000000000..e997c9648 --- /dev/null +++ b/src/msg.c @@ -0,0 +1,37 @@ +/** + * Message related functions + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include +#include + +#include "msg.h" + +void msg_fprint(FILE *f, struct msg *msg) +{ + fprintf(f, "p: dev_id = %u, msg_id = %u, data", msg->dev_id, msg->msg_id); + + for (int i = 0; i < msg->msg_len / sizeof(double); i++) + fprintf(f, "\t%f", msg->data[i]); + + fprintf(f, "\n"); +} + +void msg_random(struct msg *msg, short dev_id) +{ + static uint16_t msg_id; + static double data[4]; + + for (int i = 0; i < MAX_VALUES; i++) + data[i] += (double) random() / RAND_MAX - .5; + + msg->msg_id = ++msg_id; + msg->dev_id = dev_id; + msg->msg_len = sizeof(data); + memcpy(&msg->data, data, msg->msg_len); +} + diff --git a/src/node.c b/src/node.c new file mode 100644 index 000000000..a86c2ce8f --- /dev/null +++ b/src/node.c @@ -0,0 +1,112 @@ +/** + * Nodes + * + * The S2SS server connects multiple nodes. + * There are multiple types of nodes: + * - simulators + * - servers + * - workstations + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include +#include +#include +#include +#include +#include + +#include "utils.h" +#include "msg.h" +#include "node.h" + +struct node* node_create(const char *name, enum node_type type, const char *addr, int port) +{ + struct node *n = malloc(sizeof(struct node)); + if (!n) + return NULL; + + memset(n, 0, sizeof(struct node)); + + n->name = strdup(name); + n->type = type; + + /* get ip */ + struct addrinfo *result; + struct addrinfo hint = { + .ai_family = AF_INET, + .ai_socktype = SOCK_DGRAM, + .ai_protocol = 0 + }; + + int ret = getaddrinfo(addr, NULL, &hint, &result); + if (ret) { + print(ERROR, "Failed to get address for node %s: %s", name, gai_strerror(ret)); + return NULL; + } + + memcpy(&n->addr, result->ai_addr, sizeof(struct sockaddr_in)); + n->addr.sin_family = AF_INET; + n->addr.sin_port = htons(port); + + freeaddrinfo(result); + + print(DEBUG, "Node %s is reachable at %s:%u", name, inet_ntoa(n->addr.sin_addr), ntohs(n->addr.sin_port)); + + /* create and connect socket */ + n->sd = socket(AF_INET, SOCK_DGRAM, 0); + if (n->sd < 0) { + print(ERROR, "failed to create socket: %s", strerror(errno)); + node_destroy(n); + return NULL; + } + + /*ret = connect(n->sd, &n->addr, sizeof(struct sockaddr_in)); + if (ret < 0) { + print(ERROR, "Failed to connect socket: %s", strerror(errno)); + node_destroy(n); + return NULL; + }*/ + + ret = bind(n->sd, &n->addr, sizeof(struct sockaddr_in)); + if (ret < 0) { + print(ERROR, "Failed to bind socket: %s", strerror(errno)); + node_destroy(n); + return NULL; + } + + return n; +} + +void node_destroy(struct node* n) +{ + if (!n) + return; + + close(n->sd); + + if (n->name) + free(n->name); + + free(n); +} + +int node_send(struct node *n, struct msg *m) +{ + send(n->sd, m, sizeof(struct msg), 0); + print(DEBUG, "Message sent to node %s", n->name); + msg_fprint(stdout, m); +} + +int node_recv(struct node *n, struct msg *m) +{ + size_t ret = recv(n->sd, m, sizeof(struct msg), 0); + if (ret < 0) + print(ERROR, "Recv failed: %s", strerror(errno)); + + print(DEBUG, "Message received from node %s", n->name); + msg_fprint(stdout, m); +} diff --git a/src/path.c b/src/path.c new file mode 100644 index 000000000..5b0cdbaea --- /dev/null +++ b/src/path.c @@ -0,0 +1,85 @@ +/** + * Message paths + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include +#include + +#include "utils.h" +#include "path.h" + +struct path* path_create(struct node *in, struct node *out[], int len) +{ + struct path *p = malloc(sizeof(struct path)); + if (!p) + return NULL; + + p->in = in; + + for (int i = 0; i < len; i++) { + p->out[i] = out[i]; + } + + return p; +} + +void path_destroy(struct path *p) +{ + if (!p) + return; + + free(p); +} + +static void * path_run(void *arg) +{ + struct path *p = (struct path *) arg; + struct pollfd pfd; + struct msg m; + + pfd.fd = p->in->sd; + pfd.events = POLLIN; + + // TODO: add support for multiple outgoing nodes + print(DEBUG, "Established path: %12s => %s => %-12s", p->in->name, NAME, p->out[0]->name); + + /* main thread loop */ + while (p->state == RUNNING) { + /* wait for new incoming messages */ + //poll(&pfd, 1, 1); + + /* receive message */ + node_recv(p->in, &m); + + /* call hooks */ + + /* send messages */ + /*for (struct node **n = p->out; *n; n++) { + node_send(*n, &m); + }*/ + } + + return NULL; +} + +int path_start(struct path *p) +{ + p->state = RUNNING; + pthread_create(&p->tid, NULL, &path_run, (void *) p); +} + +int path_stop(struct path *p) +{ + void * ret; + + p->state = STOPPED; + + pthread_cancel(p->tid); + pthread_join(p->tid, &ret); + + return 0; // TODO +} diff --git a/src/rt.c b/src/rt.c new file mode 100644 index 000000000..917219ade --- /dev/null +++ b/src/rt.c @@ -0,0 +1,50 @@ +/** + * Realtime related functions + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include + +#include "config.h" +#include "utils.h" + +void rt_stack_prefault() +{ + unsigned char dummy[MAX_SAFE_STACK]; + + memset(dummy, 0, MAX_SAFE_STACK); + + +} + +void rt_set_priority() +{ + struct sched_param param; + + param.sched_priority = PRIORITY; + if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) { + perror("sched_setscheduler failed"); + exit(-1); + } +} + +void rt_lock_memory() +{ + // TODO +} + +void rt_set_affinity() +{ + // TODO +} + +void rt_init() +{ + rt_set_priority(); + rt_set_affinity(); + rt_lock_memory(); + rt_stack_prefault(); +} diff --git a/src/test.c b/src/test.c new file mode 100644 index 000000000..483b7b253 --- /dev/null +++ b/src/test.c @@ -0,0 +1,65 @@ +/** + * Simple UDP test client (simulator emulation) + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "config.h" +#include "utils.h" +#include "msg.h" + +int main(int argc, char *argv[]) +{ + if (argc != 4) { + printf("Usage: test DEVID IP PORT\n"); + printf(" DEVID is the device id of this node\n"); + printf(" IP is the destination ip of our packets\n"); + printf(" PORT is the port to recv/send from\n\n"); + printf("s2ss Simulator2Simulator Server v%s\n", VERSION); + printf("Copyright 2014, Institute for Automation of Complex Power Systems, EONERC\n"); + exit(EXIT_FAILURE); + } + + int dev_id = atoi(argv[1]); + int ret; + + print(INFO, "Test node started on %s:%s with id=%u", argv[2], argv[3], dev_id); + + int sd = socket(AF_INET, SOCK_DGRAM, 0); + if (sd < 0) + print(FATAL, "Failed to create socket: %s", strerror(errno)); + + struct sockaddr_in sa; + struct msg m; + + sa.sin_family = AF_INET; + sa.sin_port = htons(atoi(argv[3])); + inet_aton(argv[2], &sa.sin_addr); + + ret = connect(sd, &sa, sizeof(struct sockaddr_in)); + if (ret < 0) + print(FATAL, "Failed to connect socket: %s", strerror(errno)); + + while (1) { + msg_random(&m, dev_id); + msg_fprint(stdout, &m); + + send(sd, &m, sizeof(struct msg), 0); + + sleep(3); + } + + pause(); + return 0; +} diff --git a/src/utils.c b/src/utils.c new file mode 100644 index 000000000..233dc589b --- /dev/null +++ b/src/utils.c @@ -0,0 +1,40 @@ +/** + * Some helper functions + * + * @author Steffen Vogel + * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC + */ + +#include +#include +#include + +#include "config.h" +#include "utils.h" + +static const char *log_prefix[] = { + "Debug", + "Info", + "Warning", + "Error", + "Fatal" +}; + +void print(enum log_level lvl, const char *fmt, ...) +{ + struct timespec ts; + + va_list ap; + va_start(ap, fmt); + + clock_gettime(CLOCK_REALTIME, &ts); + + printf("%lu.%lu [%-7s] ", (long) ts.tv_sec, (long) ts.tv_nsec / 1000, log_prefix[lvl]); + vprintf(fmt, ap); + printf("\n"); + + va_end(ap); + + if (lvl >= FATAL) + exit(EXIT_FAILURE); +}