first source codes

git-svn-id: https://zerberus.eonerc.rwth-aachen.de:8443/svn/s2ss/trunk@3 8ec27952-4edc-4aab-86aa-e87bb2611832

include/config.h

@@ -0,0 +1,32 @@
+/**
+ * Configuration
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @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_ */

include/msg.h

@@ -0,0 +1,58 @@
+/**
+ * Message format
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#ifndef _MSG_H_
+#define _MSG_H_
+
+#include <stdint.h>
+#include <stdio.h>
+
+#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_ */

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 <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#ifndef _NODE_H_
+#define _NODE_H_
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+
+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_ */

include/path.h

@@ -0,0 +1,93 @@
+/**
+ * Message paths
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#ifndef _PATH_H_
+#define _PATH_H_
+
+#include <pthread.h>
+
+#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_ */

include/utils.h

@@ -0,0 +1,35 @@
+/**
+ * Some helper functions
+ *
+ * @author Steffen Vogel <steffen.vogel@rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#ifndef _UTILS_H_
+#define _UTILS_H_
+
+#include <stdarg.h>
+
+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_ */
+

src/main.c

@@ -0,0 +1,71 @@
+/**
+ * Main routine
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <error.h>
+
+#include <signal.h>
+#include <unistd.h>
+
+#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;
+}

src/msg.c

@@ -0,0 +1,37 @@
+/**
+ * Message related functions
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <assert.h>
+
+#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);
+}
+

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 <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <netdb.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#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);
+}

src/path.c

@@ -0,0 +1,85 @@
+/**
+ * Message paths
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <poll.h>
+
+#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
+}

src/rt.c

@@ -0,0 +1,50 @@
+/**
+ * Realtime related functions
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <string.h>
+#include <sched.h>
+
+#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, &param) == -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();
+}

src/test.c

@@ -0,0 +1,65 @@
+/**
+ * Simple UDP test client (simulator emulation)
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+
+#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;
+}

src/utils.c

@@ -0,0 +1,40 @@
+/**
+ * Some helper functions
+ *
+ * @author Steffen Vogel <stvogel@eonerc.rwth-aachen.de>
+ * @copyright 2014, Institute for Automation of Complex Power Systems, EONERC
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <time.h>
+
+#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);
+}