diff --git a/CMakeLists.txt b/CMakeLists.txt
index 618916e9..45a1c20a 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -73,6 +73,7 @@ include_directories(
${CMAKE_CURRENT_LIST_DIR}/src/common/inc
${CMAKE_CURRENT_LIST_DIR}/src/goose
${CMAKE_CURRENT_LIST_DIR}/src/sampled_values
+ ${CMAKE_CURRENT_LIST_DIR}/src/r_session
${CMAKE_CURRENT_LIST_DIR}/src/hal/inc
${CMAKE_CURRENT_LIST_DIR}/src/iec61850/inc
${CMAKE_CURRENT_LIST_DIR}/src/iec61850/inc_private
@@ -111,6 +112,7 @@ set(API_HEADERS
src/goose/goose_publisher.h
src/sampled_values/sv_subscriber.h
src/sampled_values/sv_publisher.h
+ src/r_session/r_session.h
src/logging/logging_api.h
)
diff --git a/README.md b/README.md
index e423d312..08139382 100644
--- a/README.md
+++ b/README.md
@@ -53,6 +53,8 @@ The library support the following IEC 61850 protocol features:
* Setting group handling
* Support for service tracking
* GOOSE and SV control block handling
+* Support for R-session protocol/R-GOOSE/R-SMV
+* Simple SNTP client code
* TLS support
* C and C#/.NET API
diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt
index d787fb9b..23e9d3aa 100644
--- a/examples/CMakeLists.txt
+++ b/examples/CMakeLists.txt
@@ -26,6 +26,12 @@ add_subdirectory(iec61850_client_example_array)
add_subdirectory(iec61850_client_example_files)
add_subdirectory(iec61850_client_example_async)
add_subdirectory(iec61850_client_file_async)
+add_subdirectory(sntp_example)
+
+add_subdirectory(rsv_publisher_example)
+add_subdirectory(rsv_subscriber_example)
+add_subdirectory(r_goose_publisher_example)
+add_subdirectory(r_goose_receiver_example)
if (NOT WIN32)
add_subdirectory(mms_utility)
diff --git a/hal/inc/hal_socket.h b/hal/inc/hal_socket.h
index 2f681cce..33aaf12c 100644
--- a/hal/inc/hal_socket.h
+++ b/hal/inc/hal_socket.h
@@ -1,7 +1,7 @@
/*
* socket_hal.h
*
- * Copyright 2013-2021 Michael Zillgith
+ * Copyright 2013-2022 Michael Zillgith
*
* This file is part of Platform Abstraction Layer (libpal)
* for libiec61850, libmms, and lib60870.
@@ -128,9 +128,44 @@ Handleset_destroy(HandleSet self);
PAL_API ServerSocket
TcpServerSocket_create(const char* address, int port);
+/**
+ * \brief Create an IPv4 UDP socket instance
+ *
+ * \return new UDP socket instance
+ */
PAL_API UdpSocket
UdpSocket_create(void);
+/**
+ * \brief Create an IPv6 UDP socket instance
+ *
+ * \return new UDP socket instance
+ */
+PAL_API UdpSocket
+UdpSocket_createIpV6(void);
+
+/**
+ * \brief Add the socket to an IPv4 or IPv6 multicast group
+ *
+ * \param self UDP socket instance
+ * \param multicastAddress IPv4 or IPv6 multicast address
+ *
+ * \return true on success, false otherwise
+ */
+PAL_API bool
+UdpSocket_addGroupMembership(UdpSocket self, const char* multicastAddress);
+
+/**
+ * \brief Sets the multicast TTL (number of hops) for this UDP socket
+ *
+ * \param self UDP socket instance
+ * \param ttl number of hops for multicast messages. Default is 1 (not routable!)
+ *
+ * \return true on success, false otherwise
+ */
+PAL_API bool
+UdpSocket_setMulticastTtl(UdpSocket self, int ttl);
+
PAL_API bool
UdpSocket_bind(UdpSocket self, const char* address, int port);
diff --git a/hal/inc/hal_time.h b/hal/inc/hal_time.h
index b19bcd4b..bf7df781 100644
--- a/hal/inc/hal_time.h
+++ b/hal/inc/hal_time.h
@@ -1,7 +1,7 @@
/*
* time.c
*
- * Copyright 2013-2021 Michael Zillgith
+ * Copyright 2013-2022 Michael Zillgith
*
* This file is part of Platform Abstraction Layer (libpal)
* for libiec61850, libmms, and lib60870.
diff --git a/hal/socket/linux/socket_linux.c b/hal/socket/linux/socket_linux.c
index c188c24f..3da1afc9 100644
--- a/hal/socket/linux/socket_linux.c
+++ b/hal/socket/linux/socket_linux.c
@@ -47,6 +47,7 @@ struct sServerSocket {
struct sUdpSocket {
int fd;
+ int namespace; /* IPv4: AF_INET; IPv6: AF_INET6 */
};
struct sHandleSet {
@@ -736,17 +737,26 @@ Socket_destroy(Socket self)
GLOBAL_FREEMEM(self);
}
-UdpSocket
-UdpSocket_create()
+static UdpSocket
+UdpSocket_createUsingNamespace(int namespace)
{
UdpSocket self = NULL;
- int sock = socket(AF_INET, SOCK_DGRAM, 0);
+ int sock = socket(namespace, SOCK_DGRAM, IPPROTO_UDP);
if (sock != -1) {
self = (UdpSocket) GLOBAL_MALLOC(sizeof(struct sSocket));
- self->fd = sock;
+ if (self) {
+ self->fd = sock;
+ self->namespace = namespace;
+ }
+ else {
+ if (DEBUG_SOCKET)
+ printf("SOCKET: failed to allocate memory\n");
+
+ close(sock);
+ }
}
else {
if (DEBUG_SOCKET)
@@ -756,9 +766,88 @@ UdpSocket_create()
return self;
}
+UdpSocket
+UdpSocket_create()
+{
+ return UdpSocket_createUsingNamespace(AF_INET);
+}
+
+UdpSocket
+UdpSocket_createIpV6()
+{
+ return UdpSocket_createUsingNamespace(AF_INET6);
+}
+
+bool
+UdpSocket_addGroupMembership(UdpSocket self, const char* multicastAddress)
+{
+ if (self->namespace == AF_INET) {
+ struct ip_mreq mreq;
+
+ if (!inet_aton(multicastAddress, &(mreq.imr_multiaddr))) {
+ printf("SOCKET: Invalid IPv4 multicast address\n");
+ return false;
+ }
+ else {
+ mreq.imr_interface.s_addr = htonl(INADDR_ANY);
+
+ if (setsockopt(self->fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) == -1) {
+ printf("SOCKET: failed to set IPv4 multicast group (errno: %i)\n", errno);
+ return false;
+ }
+
+ }
+
+ return true;
+ }
+ else if (self->namespace == AF_INET6) {
+ struct ipv6_mreq mreq;
+
+ if (inet_pton(AF_INET6, multicastAddress, &(mreq.ipv6mr_multiaddr)) < 1) {
+ printf("SOCKET: failed to set IPv6 multicast group (errno: %i)\n", errno);
+ return false;
+ }
+
+ mreq.ipv6mr_interface = 0;
+
+ if (setsockopt(self->fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) == -1) {
+ printf("SOCKET: failed to set IPv6 multicast group (errno: %i)\n", errno);
+ return false;
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
+bool
+UdpSocket_setMulticastTtl(UdpSocket self, int ttl)
+{
+ if (self->namespace == AF_INET) {
+ if (setsockopt(self->fd, IPPROTO_IP, IP_MULTICAST_TTL, &ttl, sizeof(ttl)) == -1) {
+ printf("SOCKET: failed to set IPv4 multicast TTL (errno: %i)\n", errno);
+ return false;
+ }
+
+ return true;
+ }
+ else if (self->namespace == AF_INET6) {
+ if (setsockopt(self->fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, &ttl, sizeof(ttl)) == -1) {
+ printf("SOCKET: failed to set IPv6 multicast TTL(hops) (errno: %i)\n", errno);
+ return false;
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
bool
UdpSocket_bind(UdpSocket self, const char* address, int port)
{
+ //TODO add support for IPv6
struct sockaddr_in localAddress;
if (!prepareAddress(address, port, &localAddress)) {
@@ -785,6 +874,7 @@ UdpSocket_bind(UdpSocket self, const char* address, int port)
bool
UdpSocket_sendTo(UdpSocket self, const char* address, int port, uint8_t* msg, int msgSize)
{
+ //TODO add support for IPv6
struct sockaddr_in remoteAddress;
if (!prepareAddress(address, port, &remoteAddress)) {
@@ -815,7 +905,9 @@ UdpSocket_sendTo(UdpSocket self, const char* address, int port, uint8_t* msg, in
int
UdpSocket_receiveFrom(UdpSocket self, char* address, int maxAddrSize, uint8_t* msg, int msgSize)
{
+ //TODO add support for IPv6
struct sockaddr_storage remoteAddress;
+
socklen_t structSize = sizeof(struct sockaddr_storage);
int result = recvfrom(self->fd, msg, msgSize, MSG_DONTWAIT, (struct sockaddr*)&remoteAddress, &structSize);
@@ -843,7 +935,33 @@ UdpSocket_receiveFrom(UdpSocket self, char* address, int maxAddrSize, uint8_t* m
isIPv6 = true;
}
else
- return result ;
+ return -1;
+
+ if (isIPv6)
+ snprintf(address, maxAddrSize, "[%s]:%i", addrString, port);
+ else
+ snprintf(address, maxAddrSize, "%s:%i", addrString, port);
+ }
+
+ if (address) {
+ bool isIPv6;
+ char addrString[INET6_ADDRSTRLEN + 7];
+ int port;
+
+ if (remoteAddress.ss_family == AF_INET) {
+ struct sockaddr_in* ipv4Addr = (struct sockaddr_in*) &remoteAddress;
+ port = ntohs(ipv4Addr->sin_port);
+ inet_ntop(AF_INET, &(ipv4Addr->sin_addr), addrString, INET_ADDRSTRLEN);
+ isIPv6 = false;
+ }
+ else if (remoteAddress.ss_family == AF_INET6) {
+ struct sockaddr_in6* ipv6Addr = (struct sockaddr_in6*) &remoteAddress;
+ port = ntohs(ipv6Addr->sin6_port);
+ inet_ntop(AF_INET6, &(ipv6Addr->sin6_addr), addrString, INET6_ADDRSTRLEN);
+ isIPv6 = true;
+ }
+ else
+ return result;
if (isIPv6)
snprintf(address, maxAddrSize, "[%s]:%i", addrString, port);
diff --git a/hal/socket/win32/socket_win32.c b/hal/socket/win32/socket_win32.c
index 65be99d7..fb727452 100644
--- a/hal/socket/win32/socket_win32.c
+++ b/hal/socket/win32/socket_win32.c
@@ -42,12 +42,13 @@ struct sServerSocket {
};
struct sHandleSet {
- fd_set handles;
- SOCKET maxHandle;
+ fd_set handles;
+ SOCKET maxHandle;
};
struct sUdpSocket {
- SOCKET fd;
+ SOCKET fd;
+ int ns; /* IPv4: AF_INET; IPv6: AF_INET6 */
};
HandleSet
@@ -677,21 +678,107 @@ UdpSocket_create()
if (sock != INVALID_SOCKET) {
self = (UdpSocket) GLOBAL_MALLOC(sizeof(struct sSocket));
- self->fd = sock;
+ if (self) {
+ self->fd = sock;
+ self->ns = ns;
+ }
+ else {
+ if (DEBUG_SOCKET)
+ printf("SOCKET: failed to allocate memory\n");
- setSocketNonBlocking((Socket)self);
+ closesocket(sock);
+ }
}
else {
if (DEBUG_SOCKET)
- printf("SOCKET: failed to create UDP socket (errno=%i)\n", errno);
+ printf("SOCKET: failed to create UDP socket (errno=%i)\n", WSAGetLastError());
}
return self;
}
+UdpSocket
+UdpSocket_create()
+{
+ return UdpSocket_createUsingNamespace(AF_INET);
+}
+
+UdpSocket
+UdpSocket_createIpV6()
+{
+ return UdpSocket_createUsingNamespace(AF_INET6);
+}
+
+bool
+UdpSocket_addGroupMembership(UdpSocket self, const char* multicastAddress)
+{
+ if (self->ns == AF_INET) {
+ struct ip_mreq mreq;
+
+ if (inet_pton(AF_INET, multicastAddress, &(mreq.imr_multiaddr)) < 1) {
+ printf("SOCKET: Invalid IPv4 multicast address\n");
+ return false;
+ }
+ else {
+ mreq.imr_interface.s_addr = htonl(INADDR_ANY);
+
+ if (setsockopt(self->fd, IPPROTO_IP, IP_ADD_MEMBERSHIP, (const char*)&mreq, sizeof(mreq)) == -1) {
+ printf("SOCKET: failed to set IPv4 multicast group (errno: %i)\n", WSAGetLastError());
+ return false;
+ }
+
+ }
+
+ return true;
+ }
+ else if (self->ns == AF_INET6) {
+ struct ipv6_mreq mreq;
+
+ if (inet_pton(AF_INET6, multicastAddress, &(mreq.ipv6mr_multiaddr)) < 1) {
+ printf("SOCKET: failed to set IPv6 multicast group (errno: %i)\n", WSAGetLastError());
+ return false;
+ }
+
+ mreq.ipv6mr_interface = 0;
+
+ if (setsockopt(self->fd, IPPROTO_IPV6, IPV6_ADD_MEMBERSHIP, (const char*)&mreq, sizeof(mreq)) == -1) {
+ printf("SOCKET: failed to set IPv6 multicast group (errno: %i)\n", WSAGetLastError());
+ return false;
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
+bool
+UdpSocket_setMulticastTtl(UdpSocket self, int ttl)
+{
+ if (self->ns == AF_INET) {
+ if (setsockopt(self->fd, IPPROTO_IP, IP_MULTICAST_TTL, (const char*)&ttl, sizeof(ttl)) == -1) {
+ printf("SOCKET: failed to set IPv4 multicast TTL (errno: %i)\n", WSAGetLastError());
+ return false;
+ }
+
+ return true;
+ }
+ else if (self->ns == AF_INET6) {
+ if (setsockopt(self->fd, IPPROTO_IPV6, IPV6_MULTICAST_HOPS, (const char*)&ttl, sizeof(ttl)) == -1) {
+ printf("SOCKET: failed to set IPv6 multicast TTL(hops) (errno: %i)\n", WSAGetLastError());
+ return false;
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
bool
UdpSocket_bind(UdpSocket self, const char* address, int port)
{
+ //TODO add support for IPv6
struct sockaddr_in localAddress;
if (!prepareAddress(address, port, &localAddress)) {
@@ -718,6 +805,7 @@ UdpSocket_bind(UdpSocket self, const char* address, int port)
bool
UdpSocket_sendTo(UdpSocket self, const char* address, int port, uint8_t* msg, int msgSize)
{
+ //TODO add support for IPv6
struct sockaddr_in remoteAddress;
if (!prepareAddress(address, port, &remoteAddress)) {
@@ -748,9 +836,13 @@ UdpSocket_sendTo(UdpSocket self, const char* address, int port, uint8_t* msg, in
int
UdpSocket_receiveFrom(UdpSocket self, char* address, int maxAddrSize, uint8_t* msg, int msgSize)
{
+ //TODO add support for IPv6
struct sockaddr_storage remoteAddress;
socklen_t structSize = sizeof(struct sockaddr_storage);
+ if (address)
+ address[0] = 0;
+
int result = recvfrom(self->fd, (char*) msg, msgSize, 0, (struct sockaddr*)&remoteAddress, &structSize);
if (result == 0) /* peer has closed socket */
diff --git a/hal/tls/mbedtls/mbedtls_config.h b/hal/tls/mbedtls/mbedtls_config.h
index 4cb77222..1773bc2c 100644
--- a/hal/tls/mbedtls/mbedtls_config.h
+++ b/hal/tls/mbedtls/mbedtls_config.h
@@ -21,6 +21,7 @@
#define MBEDTLS_TLS_DEFAULT_ALLOW_SHA1_IN_CERTIFICATES
/* mbed TLS modules */
+#define MBEDTLS_GCM_C
#define MBEDTLS_AES_C
#define MBEDTLS_ASN1_PARSE_C
#define MBEDTLS_ASN1_WRITE_C
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index ff2ee08b..1210883d 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,3 +1,9 @@
+if(WITH_MBEDTLS)
+include_directories(
+ ${CMAKE_CURRENT_LIST_DIR}/tls/mbedtls
+ ${CMAKE_CURRENT_LIST_DIR}/../third_party/mbedtls/mbedtls-2.16/include
+)
+endif(WITH_MBEDTLS)
set (lib_common_SRCS
./common/string_map.c
@@ -77,6 +83,7 @@ set (lib_common_SRCS
./iec61850/server/mms_mapping/mms_sv.c
./iec61850/server/mms_mapping/logging.c
./logging/log_storage.c
+./sntp/sntp_client.c
)
set (lib_asn1c_SRCS
@@ -183,6 +190,11 @@ set (lib_sv_SRCS
./sampled_values/sv_publisher.c
)
+set (lib_rsession_SRCS
+./r_session/r_session.c
+./r_session/r_session_crypto_mbedtls.c
+)
+
set (lib_linux_SRCS
)
@@ -228,6 +240,7 @@ set (library_SRCS
${lib_asn1c_SRCS}
${lib_goose_SRCS}
${lib_sv_SRCS}
+ ${lib_rsession_SRCS}
${lib_windows_SRCS}
)
@@ -236,6 +249,7 @@ set (library_SRCS
${lib_common_SRCS}
${lib_asn1c_SRCS}
${lib_windows_SRCS}
+ ${lib_rsession_SRCS}
)
ENDIF(WITH_WPCAP)
@@ -247,6 +261,7 @@ set (library_SRCS
${lib_asn1c_SRCS}
${lib_goose_SRCS}
${lib_sv_SRCS}
+ ${lib_rsession_SRCS}
${lib_bsd_SRCS}
)
ELSEIF(${CMAKE_SYSTEM_NAME} MATCHES "FreeBSD")
@@ -255,6 +270,7 @@ set (library_SRCS
${lib_asn1c_SRCS}
${lib_goose_SRCS}
${lib_sv_SRCS}
+ ${lib_rsession_SRCS}
${lib_bsd_SRCS}
)
ELSE()
@@ -263,6 +279,7 @@ set (library_SRCS
${lib_asn1c_SRCS}
${lib_goose_SRCS}
${lib_sv_SRCS}
+ ${lib_rsession_SRCS}
${lib_linux_SRCS}
)
ENDIF(APPLE)
diff --git a/src/goose/goose_publisher.c b/src/goose/goose_publisher.c
index 1abf067d..85182d7a 100644
--- a/src/goose/goose_publisher.c
+++ b/src/goose/goose_publisher.c
@@ -29,6 +29,8 @@
#include "mms_server_internal.h"
#include "mms_value_internal.h"
+#include "r_session_internal.h"
+
#ifndef DEBUG_GOOSE_PUBLISHER
#define DEBUG_GOOSE_PUBLISHER 0
#endif
@@ -41,7 +43,13 @@ prepareGooseBuffer(GoosePublisher self, CommParameters* parameters, const char*
struct sGoosePublisher {
uint8_t* buffer;
+ /* only for R-GOOSE */
+ RSession remoteSession;
+ uint16_t appId;
+
+ /* only for Ethernet based GOOSE */
EthernetSocket ethernetSocket;
+
int lengthField;
int payloadStart;
int payloadLength;
@@ -60,6 +68,33 @@ struct sGoosePublisher {
MmsValue* timestamp; /* time when stNum is increased */
};
+GoosePublisher
+GoosePublisher_createRemote(RSession session, uint16_t appId)
+{
+ GoosePublisher self = (GoosePublisher) GLOBAL_CALLOC(1, sizeof(struct sGoosePublisher));
+
+ if (self) {
+ self->remoteSession = session;
+
+ self->buffer = (uint8_t*) GLOBAL_MALLOC(GOOSE_MAX_MESSAGE_SIZE);
+
+ /* parameters are destination IP and dataSetRef */
+
+ self->timestamp = MmsValue_newUtcTimeByMsTime(Hal_getTimeInMs());
+
+ GoosePublisher_reset(self);
+
+ self->payloadStart = 0;
+ self->remoteSession = session;
+ self->lengthField = 0;
+
+ self->simulation = false;
+ self->appId = appId;
+ }
+
+ return self;
+}
+
GoosePublisher
GoosePublisher_createEx(CommParameters* parameters, const char* interfaceID, bool useVlanTag)
{
@@ -423,17 +458,26 @@ GoosePublisher_publish(GoosePublisher self, LinkedList dataSet)
if (self->sqNum == 0)
self->sqNum = 1;
- int lengthIndex = self->lengthField;
+ if (self->ethernetSocket) {
+ int lengthIndex = self->lengthField;
+
+ size_t gooseLength = self->payloadLength + 8;
- size_t gooseLength = self->payloadLength + 8;
+ self->buffer[lengthIndex] = gooseLength / 256;
+ self->buffer[lengthIndex + 1] = gooseLength & 0xff;
- self->buffer[lengthIndex] = gooseLength / 256;
- self->buffer[lengthIndex + 1] = gooseLength & 0xff;
+ Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength);
- if (DEBUG_GOOSE_PUBLISHER)
- printf("GOOSE_PUBLISHER: send GOOSE message\n");
+ if (DEBUG_GOOSE_PUBLISHER)
+ printf("GOOSE_PUBLISHER: send GOOSE message\n");
+ }
+ else if (self->remoteSession) {
+
+ RSession_sendMessage(self->remoteSession, RSESSION_SPDU_ID_GOOSE, self->simulation, self->appId, buffer, self->payloadLength);
- Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength);
+ if (DEBUG_GOOSE_PUBLISHER)
+ printf("GOOSE_PUBLISHER: send R-GOOSE message\n");
+ }
return 0;
}
diff --git a/src/goose/goose_publisher.h b/src/goose/goose_publisher.h
index 77eb2766..97c58c79 100644
--- a/src/goose/goose_publisher.h
+++ b/src/goose/goose_publisher.h
@@ -1,7 +1,7 @@
/*
* goose_publisher.h
*
- * Copyright 2013-2020 Michael Zillgith
+ * Copyright 2013-2022 Michael Zillgith
*
* This file is part of libIEC61850.
*
@@ -25,6 +25,7 @@
#define GOOSE_PUBLISHER_H_
#include "iec61850_common.h"
+#include "r_session.h"
#include "linked_list.h"
#include "mms_value.h"
@@ -58,7 +59,7 @@ LIB61850_API GoosePublisher
GoosePublisher_create(CommParameters* parameters, const char* interfaceID);
/**
- * \brief Create a new GoosePublisher instance
+ * \brief Create a new GoosePublisher instance for Ethernet GOOSE
*
* \param parameters GOOSE communication parameters
* \param interfaceId name of the Ethernet interface to use (e.g. "eth0")
@@ -67,6 +68,15 @@ GoosePublisher_create(CommParameters* parameters, const char* interfaceID);
LIB61850_API GoosePublisher
GoosePublisher_createEx(CommParameters* parameters, const char* interfaceID, bool useVlanTag);
+/**
+ * \brief Create a new GoosePublisher instance for R-GOOSE
+ *
+ * \param session R-session protocol instance to use
+ * \param appId the appID value to use
+ */
+LIB61850_API GoosePublisher
+GoosePublisher_createRemote(RSession session, uint16_t appId);
+
/**
* \brief Release all resources of the GoosePublisher instance
*
diff --git a/src/goose/goose_receiver.c b/src/goose/goose_receiver.c
index 06160008..78b7a690 100644
--- a/src/goose/goose_receiver.c
+++ b/src/goose/goose_receiver.c
@@ -37,6 +37,8 @@
#include "goose_receiver.h"
#include "goose_receiver_internal.h"
+#include "r_session_internal.h"
+
#ifndef DEBUG_GOOSE_SUBSCRIBER
#define DEBUG_GOOSE_SUBSCRIBER 0
#endif
@@ -51,7 +53,13 @@ struct sGooseReceiver
bool stop;
char* interfaceId;
uint8_t* buffer;
+
+ /* for Ethernet GOOSE only */
EthernetSocket ethSocket;
+
+ /* for R-GOOSE only */
+ RSession session;
+
LinkedList subscriberList;
#if (CONFIG_MMS_THREADLESS_STACK == 0)
Thread thread;
@@ -90,6 +98,18 @@ GooseReceiver_create()
return self;
}
+GooseReceiver
+GooseReceiver_createRemote(RSession session)
+{
+ GooseReceiver self = GooseReceiver_create();
+
+ if (self) {
+ self->session = session;
+ }
+
+ return self;
+}
+
void
GooseReceiver_addSubscriber(GooseReceiver self, GooseSubscriber subscriber)
{
@@ -989,13 +1009,42 @@ static void*
gooseReceiverLoop(void *threadParameter)
{
GooseReceiver self = (GooseReceiver) threadParameter;
- EthernetHandleSet handleSet = EthernetHandleSet_new();
- EthernetHandleSet_addSocket(handleSet, self->ethSocket);
- if (self->running) {
+ if (self->ethSocket) {
+ EthernetHandleSet handleSet = EthernetHandleSet_new();
+ EthernetHandleSet_addSocket(handleSet, self->ethSocket);
+
+ if (self->running) {
+
+ while (self->running) {
+ switch (EthernetHandleSet_waitReady(handleSet, 100))
+ {
+ case -1:
+ if (DEBUG_GOOSE_SUBSCRIBER)
+ printf("GOOSE_SUBSCRIBER: EhtnernetHandleSet_waitReady() failure\n");
+ break;
+ case 0:
+ break;
+ default:
+ GooseReceiver_tick(self);
+ }
+
+ if (self->stop)
+ break;
+ }
+
+ GooseReceiver_stopThreadless(self);
+ }
+
+ EthernetHandleSet_destroy(handleSet);
+ }
+ else if (self->session) {
+ HandleSet handleSet = Handleset_new();
+
+ Handleset_addSocket(handleSet, RSession_getSocket(self->session));
while (self->running) {
- switch (EthernetHandleSet_waitReady(handleSet, 100))
+ switch (Handleset_waitReady(handleSet, 100))
{
case -1:
if (DEBUG_GOOSE_SUBSCRIBER)
@@ -1006,14 +1055,15 @@ gooseReceiverLoop(void *threadParameter)
default:
GooseReceiver_tick(self);
}
+
if (self->stop)
break;
}
GooseReceiver_stopThreadless(self);
- }
- EthernetHandleSet_destroy(handleSet);
+ Handleset_destroy(handleSet);
+ }
return NULL;
}
@@ -1028,10 +1078,26 @@ GooseReceiver_start(GooseReceiver self)
self->thread = Thread_create((ThreadExecutionFunction) gooseReceiverLoop, (void*) self, false);
if (self->thread != NULL) {
- if (DEBUG_GOOSE_SUBSCRIBER)
- printf("GOOSE_SUBSCRIBER: GOOSE receiver started for interface %s\n", self->interfaceId);
- Thread_start(self->thread);
+ if (self->ethSocket) {
+ if (DEBUG_GOOSE_SUBSCRIBER)
+ printf("GOOSE_SUBSCRIBER: GOOSE receiver started for interface %s\n", self->interfaceId);
+
+ Thread_start(self->thread);
+ }
+ else if (self->session) {
+ if (DEBUG_GOOSE_SUBSCRIBER)
+ printf("GOOSE_SUBSCRIBER: R-GOOSE receiver started\n");
+
+ Thread_start(self->thread);
+ }
+ else {
+ if (DEBUG_GOOSE_SUBSCRIBER)
+ printf("GOOSE_SUBSCRIBER: ERROR - No link/transport layer specified -> cannot start!\n");
+
+ Thread_destroy(self->thread);
+ self->thread = NULL;
+ }
}
else {
if (DEBUG_GOOSE_SUBSCRIBER)
@@ -1087,37 +1153,52 @@ GooseReceiver_destroy(GooseReceiver self)
EthernetSocket
GooseReceiver_startThreadless(GooseReceiver self)
{
- if (self->interfaceId == NULL)
- self->ethSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, NULL);
- else
- self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
+ if (self->session) {
+ if (RSession_startListening(self->session) == R_SESSION_ERROR_OK) {
+ self->running = true;
- if (self->ethSocket != NULL) {
- Ethernet_setProtocolFilter(self->ethSocket, ETH_P_GOOSE);
+ return (EthernetSocket)1;
+ }
+ else {
+ self->running = false;
- /* set multicast addresses for subscribers */
- Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_MULTICAST);
+ return (EthernetSocket)0;
+ }
+ }
+ else {
+ if (self->interfaceId == NULL)
+ self->ethSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, NULL);
+ else
+ self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
- LinkedList element = LinkedList_getNext(self->subscriberList);
+ if (self->ethSocket != NULL) {
+ Ethernet_setProtocolFilter(self->ethSocket, ETH_P_GOOSE);
- while (element != NULL) {
- GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
+ /* set multicast addresses for subscribers */
+ Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_MULTICAST);
- if (subscriber->dstMacSet == false) {
- /* no destination MAC address defined -> we have to switch to all multicast mode */
- Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_ALL_MULTICAST);
- }
- else {
- Ethernet_addMulticastAddress(self->ethSocket, subscriber->dstMac);
+ LinkedList element = LinkedList_getNext(self->subscriberList);
+
+ while (element != NULL) {
+ GooseSubscriber subscriber = (GooseSubscriber) LinkedList_getData(element);
+
+ if (subscriber->dstMacSet == false) {
+ /* no destination MAC address defined -> we have to switch to all multicast mode */
+ Ethernet_setMode(self->ethSocket, ETHERNET_SOCKET_MODE_ALL_MULTICAST);
+ }
+ else {
+ Ethernet_addMulticastAddress(self->ethSocket, subscriber->dstMac);
+ }
+
+ element = LinkedList_getNext(element);
}
- element = LinkedList_getNext(element);
+ self->running = true;
+ }
+ else {
+ self->running = false;
}
-
- self->running = true;
}
- else
- self->running = false;
return self->ethSocket;
}
@@ -1131,18 +1212,35 @@ GooseReceiver_stopThreadless(GooseReceiver self)
self->running = false;
}
+static void
+handleSessionPayloadElement(void* parameter, uint16_t appId, uint8_t* payloadData, int payloadSize)
+{
+ GooseReceiver self = (GooseReceiver) parameter;
+
+ parseGoosePayload(self, payloadData, payloadSize);
+}
+
+
/* call after reception of ethernet frame */
bool
GooseReceiver_tick(GooseReceiver self)
{
- int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH);
+ if (self->session) {
+ if (RSession_receiveMessage(self->session, handleSessionPayloadElement, (void*) self) == R_SESSION_ERROR_OK)
+ return true;
+ else
+ return false;
+ }
+ else {
+ int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH);
- if (packetSize > 0) {
- parseGooseMessage(self, self->buffer, packetSize);
- return true;
+ if (packetSize > 0) {
+ parseGooseMessage(self, self->buffer, packetSize);
+ return true;
+ }
+ else
+ return false;
}
- else
- return false;
}
void
diff --git a/src/goose/goose_receiver.h b/src/goose/goose_receiver.h
index 5d041d26..4637c6a8 100644
--- a/src/goose/goose_receiver.h
+++ b/src/goose/goose_receiver.h
@@ -1,7 +1,7 @@
/*
* goose_receiver.h
*
- * Copyright 2014-2019 Michael Zillgith
+ * Copyright 2014-2022 Michael Zillgith
*
* This file is part of libIEC61850.
*
@@ -32,6 +32,7 @@ extern "C" {
#include "hal_ethernet.h"
#include "goose_subscriber.h"
+#include "r_session.h"
/**
* \addtogroup goose_api_group
@@ -64,6 +65,16 @@ GooseReceiver_create(void);
LIB61850_API GooseReceiver
GooseReceiver_createEx(uint8_t* buffer);
+/**
+ * \brief Create a new R-GOOSE receiver instance.
+ *
+ * \param session the remote session protocol instance
+ *
+ * \return the newly created receiver instance
+ */
+LIB61850_API GooseReceiver
+GooseReceiver_createRemote(RSession session);
+
/**
* \brief sets the interface for the GOOSE receiver
*
diff --git a/src/goose/goose_subscriber.h b/src/goose/goose_subscriber.h
index e58f4f96..1c44158c 100644
--- a/src/goose/goose_subscriber.h
+++ b/src/goose/goose_subscriber.h
@@ -1,7 +1,7 @@
/*
* goose_subscriber.h
*
- * Copyright 2013-2021 Michael Zillgith
+ * Copyright 2013-2022 Michael Zillgith
*
* This file is part of libIEC61850.
*
diff --git a/src/r_session/r_session.c b/src/r_session/r_session.c
new file mode 100644
index 00000000..8f5013be
--- /dev/null
+++ b/src/r_session/r_session.c
@@ -0,0 +1,1162 @@
+/*
+ * r_session.c
+ *
+ * Copyright 2013-2022 Michael Zillgith
+ *
+ * This file is part of libIEC61850.
+ *
+ * libIEC61850 is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libIEC61850 is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libIEC61850. If not, see .
+ *
+ * See COPYING file for the complete license text.
+ */
+
+#include "r_session.h"
+#include "hal_socket.h"
+#include "hal_thread.h"
+#include "lib_memory.h"
+#include "string_utilities.h"
+#include "libiec61850_platform_includes.h"
+#include "r_session_crypto.h"
+#include "r_session_internal.h"
+
+#define DEBUG_RSESSION
+
+#ifdef DEBUG_RSESSION
+#include
+#define DEBUG_PRINTF(...) printf("RSESSION:"__VA_ARGS__);printf("\n");
+#else
+#define DEBUG_PRINTF(...)
+#endif
+
+
+typedef struct sRSessionKeyMaterial* RSessionKeyMaterial;
+
+struct sRSessionKeyMaterial {
+ uint32_t keyId;
+ RSecurityAlgorithm secAlgo;
+ RSignatureAlgorithm sigAlgo;
+ uint8_t* key;
+ int keyLength;
+};
+
+struct sRSession {
+ uint32_t spduNumber;
+
+ int protocolVersion; /* default is 2 */
+
+ char* remoteAddress;
+ int remotePort;
+
+ char* localAddress;
+ int localPort;
+
+ UdpSocket socket;
+ Semaphore socketLock;
+
+ uint16_t bufferSize; /* maximum buffer size (range: 128 - 65535) */
+ uint8_t* sendBuffer;
+ uint8_t* payloadBuffer; /* only required when secAlgo != NONE */
+
+ LinkedList keyList; /* list of RSessionKeyMaterial */
+ Semaphore keyListLock;
+
+ RSecurityAlgorithm secAlgo;
+ RSignatureAlgorithm sigAlgo;
+ uint32_t timeOfCurrentKey;
+ uint32_t currentKeyId;
+ uint8_t* currentKey;
+ int currentKeySize;
+
+ bool updateBufferedKeyMaterial;
+
+ RSecurityAlgorithm currentSecAlgo;
+ RSignatureAlgorithm currentSigAlgo;
+
+ int timeToNextKey;
+};
+
+#ifdef DEBUG_RSESSION
+static void
+printBuffer(uint8_t* buffer, int bufSize)
+{
+ int i;
+
+ for (i = 0; i < bufSize; i++) {
+ printf("%02x ", buffer[i]);
+ if ((((i + 1) % 16) == 0) || (i + 1 == bufSize))
+ printf(" (%i)\n", i + 1);
+ }
+}
+#endif
+
+RSessionKeyMaterial
+RSessionKeyMaterial_create(uint32_t keyId, uint8_t* key, int keyLength, RSecurityAlgorithm secAlgo, RSignatureAlgorithm sigAlgo)
+{
+ RSessionKeyMaterial self = (RSessionKeyMaterial) GLOBAL_CALLOC(1, sizeof(struct sRSessionKeyMaterial));
+
+ if (self) {
+ self->keyId = keyId;
+ self->keyLength = keyLength;
+ self->secAlgo = secAlgo;
+ self->sigAlgo = sigAlgo;
+
+ self->key = (uint8_t*) GLOBAL_MALLOC(keyLength);
+
+ if (self->key) {
+ memcpy(self->key, key, keyLength);
+ }
+ else {
+ GLOBAL_FREEMEM(self);
+ self = NULL;
+ }
+ }
+
+ return self;
+}
+
+void
+RSessionKeyMaterial_destroy(RSessionKeyMaterial self)
+{
+ if (self) {
+ GLOBAL_FREEMEM(self->key);
+ GLOBAL_FREEMEM(self);
+ }
+ }
+
+RSession
+RSession_create()
+{
+ UdpSocket udpSocket = UdpSocket_create();
+
+ if (udpSocket) {
+
+ RSession self = (RSession) GLOBAL_CALLOC(1, sizeof(struct sRSession));
+
+ if (self) {
+ self->socket = udpSocket;
+ self->socketLock = Semaphore_create(1);
+
+ self->secAlgo = R_SESSION_SEC_ALGO_NONE;
+ self->sigAlgo = R_SESSION_SIG_ALGO_NONE;
+ self->protocolVersion = 2;
+ self->bufferSize = (uint16_t)65000;
+ self->keyListLock = Semaphore_create(1);
+ self->keyList = LinkedList_create();
+ self->localAddress = NULL;
+ self->localPort = 102;
+ }
+
+ return self;
+ }
+
+ return NULL;
+}
+
+/* Required only for version 1 of the protocol! */
+RSessionError
+RSession_setSecurity(RSession self, RSecurityAlgorithm secAlgo, RSignatureAlgorithm sigAlgo)
+{
+ self->secAlgo = secAlgo;
+ self->sigAlgo = sigAlgo;
+
+ return R_SESSION_ERROR_OK;
+}
+
+RSessionError
+RSession_setLocalAddress(RSession self, const char* localAddress, int localPort)
+{
+ if (self->localAddress) {
+ GLOBAL_FREEMEM(self->localAddress);
+ self->localAddress = NULL;
+ }
+
+ if (localAddress)
+ self->localAddress = StringUtils_copyString(localAddress);
+
+ self->localPort = localPort;
+
+ return R_SESSION_ERROR_OK;
+}
+
+RSessionError
+RSession_addMulticastGroup(RSession self, const char* multicastAddress)
+{
+ if (UdpSocket_addGroupMembership(self->socket, multicastAddress))
+ return R_SESSION_ERROR_OK;
+ else
+ return R_SESSION_ERROR_SET_FAILED;
+}
+
+RSessionError
+RSession_setMulticastTtl(RSession self, int ttl)
+{
+ RSessionError err = R_SESSION_ERROR_OK;
+
+ if (self->socket) {
+ if (!UdpSocket_setMulticastTtl(self->socket, ttl)) {
+ err = R_SESSION_ERROR_SET_FAILED;
+ }
+ }
+ else {
+ err = R_SESSION_ERROR_NO_SOCKET;
+ }
+
+ return err;
+}
+
+RSessionError
+RSession_setRemoteAddress(RSession self, const char* remoteAddress, int remotePort)
+{
+ if (self->remoteAddress) {
+ GLOBAL_FREEMEM(self->remoteAddress);
+ self->remoteAddress = NULL;
+ }
+
+ self->remoteAddress = StringUtils_copyString(remoteAddress);
+ self->remotePort = remotePort;
+
+ return R_SESSION_ERROR_OK;
+}
+
+RSessionError
+RSession_startListening(RSession self)
+{
+ if (self->socket) {
+
+ bool success = false;
+
+ if (self->localAddress)
+ success = UdpSocket_bind(self->socket, self->localAddress, self->localPort);
+ else
+ success = UdpSocket_bind(self->socket, "0.0.0.0", self->localPort);
+
+ if (success)
+ return R_SESSION_ERROR_OK;
+ else
+ return R_SESSION_ERROR_SET_FAILED;
+ }
+ else {
+ return R_SESSION_ERROR_NO_SOCKET;
+ }
+}
+
+RSessionError
+RSession_stopListening(RSession self)
+{
+ Semaphore_wait(self->socketLock);
+
+ if (self->socket) {
+ Socket_destroy((Socket)self->socket);
+ self->socket = NULL;
+
+ Semaphore_post(self->socketLock);
+
+ return R_SESSION_ERROR_OK;
+ }
+ else {
+ Semaphore_post(self->socketLock);
+
+ return R_SESSION_ERROR_NO_SOCKET;
+ }
+}
+
+#define PAYLOAD_TYPE_TUNNELED_GOOSE_OR_SV 0xa0
+#define PAYLOAD_TYPE_GOOSE_APDU 0xa1
+#define PAYLOAD_TYPE_SV_APDU 0xa2
+#define PAYLOAD_TYPE_MNGT_APDU 0xa3
+
+static int
+encodeUInt16FixedSize(uint16_t value, uint8_t* buffer, int bufPos)
+{
+ uint8_t* valueArray = (uint8_t*) &value;
+
+#if (ORDER_LITTLE_ENDIAN == 1)
+ buffer[bufPos++] = valueArray[1];
+ buffer[bufPos++] = valueArray[0];
+#else
+ buffer[bufPos++] = valueArray[0];
+ buffer[bufPos++] = valueArray[1];
+#endif
+
+ return bufPos;
+}
+
+static int
+encodeInt16FixedSize(int16_t value, uint8_t* buffer, int bufPos)
+{
+ uint8_t* valueArray = (uint8_t*) &value;
+
+#if (ORDER_LITTLE_ENDIAN == 1)
+ buffer[bufPos++] = valueArray[1];
+ buffer[bufPos++] = valueArray[0];
+#else
+ buffer[bufPos++] = valueArray[0];
+ buffer[bufPos++] = valueArray[1];
+#endif
+
+ return bufPos;
+}
+
+static int
+encodeUInt32FixedSize(uint32_t value, uint8_t* buffer, int bufPos)
+{
+ uint8_t* valueArray = (uint8_t*) &value;
+
+#if (ORDER_LITTLE_ENDIAN == 1)
+ buffer[bufPos++] = valueArray[3];
+ buffer[bufPos++] = valueArray[2];
+ buffer[bufPos++] = valueArray[1];
+ buffer[bufPos++] = valueArray[0];
+#else
+ buffer[bufPos++] = valueArray[0];
+ buffer[bufPos++] = valueArray[1];
+ buffer[bufPos++] = valueArray[2];
+ buffer[bufPos++] = valueArray[3];
+#endif
+
+ return bufPos;
+}
+
+static int
+decodeUInt32FixedSize(uint32_t* outValue, uint8_t* buffer, int bufPos)
+{
+ uint32_t value = 0;
+
+ value += buffer[bufPos++] * 0x1000000;
+ value += buffer[bufPos++] * 0x10000;
+ value += buffer[bufPos++] * 0x100;
+ value += buffer[bufPos++];
+
+ *outValue = value;
+
+ return bufPos;
+}
+
+static int
+decodeUInt16FixedSize(uint16_t* outValue, uint8_t* buffer, int bufPos)
+{
+ uint16_t value = 0;
+
+ value += buffer[bufPos++] * 0x100;
+ value += buffer[bufPos++];
+
+ *outValue = value;
+
+ return bufPos;
+}
+
+static int
+decodeInt16FixedSize(int16_t* outValue, uint8_t* buffer, int bufPos)
+{
+ uint16_t value = 0;
+
+ value += buffer[bufPos++] * 0x100;
+ value += buffer[bufPos++];
+
+ *outValue = (int16_t) value;
+
+ return bufPos;
+}
+
+static bool
+lookupKey(RSession self, uint32_t keyId, uint8_t** key, int* keySize, RSecurityAlgorithm* secAlgo, RSignatureAlgorithm* sigAlgo)
+{
+ if (keyId == 0) {
+ DEBUG_PRINTF("Invalid key ID");
+ return false;
+ }
+
+ if (self->currentKeyId != keyId) {
+
+ if (RSession_setActiveKey(self, keyId) != R_SESSION_ERROR_OK) {
+ DEBUG_PRINTF("unknown key-ID %u", keyId);
+ /* TODO audit-log? */
+ return false;
+ }
+
+ }
+
+ *key = self->currentKey;
+ *keySize = self->currentKeySize;
+ *secAlgo = self->currentSecAlgo;
+ *sigAlgo = self->currentSigAlgo;
+
+ return true;
+}
+
+static RSessionError
+parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayloadElementHandler handler, void* handlerParam)
+{
+ int bufPos = 0;
+
+ if (msgSize < 4)
+ goto exit_parse_error;
+
+ if (buffer[bufPos++] != 0x01)
+ goto exit_parse_error;
+
+ if (buffer[bufPos++] != 0x40)
+ goto exit_parse_error;
+
+ /* SI */
+ uint8_t payloadType = buffer[bufPos++];
+
+ if ((payloadType == 0xa2) || (payloadType == 0xa1) || (payloadType == 0xa0)) {
+
+ }
+ else {
+ DEBUG_PRINTF("unknown payload type %i", payloadType);
+ goto exit_error;
+ }
+
+ int sessionHeaderLength = buffer[bufPos++];
+
+ if ((msgSize < (sessionHeaderLength + 4)) || (sessionHeaderLength < 10)) {
+ DEBUG_PRINTF("message too small");
+ goto exit_error;
+ }
+
+ if (buffer[bufPos++] != 0x80) {
+ DEBUG_PRINTF("protocol error");
+ goto exit_error;
+ }
+
+ int commonSessionHeaderLength = buffer[bufPos++];
+
+ if (commonSessionHeaderLength < 10) {
+ DEBUG_PRINTF("common session header too small");
+ goto exit_error;
+ }
+
+ /* SPDU length */
+ uint32_t spduLength = 0;
+ bufPos = decodeUInt32FixedSize(&spduLength, buffer, bufPos);
+
+ /* SPDU number */
+ uint32_t spduNumber = 0;
+ bufPos = decodeUInt32FixedSize(&spduNumber, buffer, bufPos);
+
+ /* protocol version */
+ int16_t protocolVersion = 0;
+ bufPos = decodeInt16FixedSize(&protocolVersion, buffer, bufPos);
+
+ if (protocolVersion == 1) {
+ /* parse version 1 common header parts */
+
+ /* TimeOfCurrentKey */
+ uint32_t timeOfCurrentKey;
+ bufPos = decodeUInt32FixedSize(&timeOfCurrentKey, buffer, bufPos);
+
+ /* TimeToNextKey */
+ int16_t timeToNextKey;
+ bufPos = decodeInt16FixedSize(&timeToNextKey, buffer, bufPos);
+
+ RSecurityAlgorithm secAlgo = (RSecurityAlgorithm) buffer[bufPos++];
+
+ RSignatureAlgorithm sigAlgo = (RSignatureAlgorithm) buffer[bufPos++];
+
+ /* Check if algorithms match the configured algorithms */
+ if (secAlgo != self->secAlgo) {
+ DEBUG_PRINTF("encryption algorithm doesn't match with configuration");
+ goto exit_error;
+ }
+
+ if (sigAlgo != self->sigAlgo) {
+ DEBUG_PRINTF("signature algorithm(%i) doesn't match with configuration(%i)", sigAlgo, self->sigAlgo);
+ goto exit_error;
+ }
+
+ /* Key ID */
+
+ uint32_t keyId;
+
+ bufPos = decodeUInt32FixedSize(&keyId, buffer, bufPos);
+
+ uint8_t* key = NULL;
+ int keySize = 0;
+
+ if (sigAlgo != R_SESSION_SIG_ALGO_NONE) {
+ if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false) {
+ DEBUG_PRINTF("ERROR - key not found");
+ goto exit_error;
+ }
+ }
+ else if (secAlgo != R_SESSION_SEC_ALGO_NONE) {
+ if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false) {
+ DEBUG_PRINTF("ERROR - key not found");
+ goto exit_error;
+ }
+ }
+
+ uint32_t payloadLength;
+
+ bufPos = decodeUInt32FixedSize(&payloadLength, buffer, bufPos);
+
+ /* parse payload elements */
+ uint32_t payloadEnd = bufPos + payloadLength;
+
+ if (payloadEnd > msgSize) {
+ DEBUG_PRINTF("ERROR - payload size field invalid");
+ goto exit_error;
+ }
+
+ uint8_t signatureBuffer[128];
+
+ /* Check signature */
+ if (sigAlgo != R_SESSION_SIG_ALGO_NONE) {
+ if (key) {
+ if (RSessionCrypto_createHMAC(buffer, payloadEnd, key, keySize, signatureBuffer, 32)) {
+ if (buffer[payloadEnd] != 0x85) {
+ DEBUG_PRINTF("ERROR - no signature found");
+ goto exit_error;
+ }
+ else {
+ if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128) {
+ /* TODO is payloadEnd +2 correct? */
+ if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 16)) {
+ DEBUG_PRINTF("ERROR - signature not matching!");
+ goto exit_error;
+ }
+ }
+ else if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256) {
+ /* TODO is payloadEnd +2 correct? */
+ if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 32)) {
+ DEBUG_PRINTF("ERROR - signature not matching!");
+ goto exit_error;
+ }
+ }
+ }
+ }
+ else {
+ DEBUG_PRINTF("ERROR - failed to calculate HMAC!");
+ goto exit_error;
+ }
+ }
+ else {
+ DEBUG_PRINTF("ERROR - key not found!");
+ goto exit_error;
+ }
+ }
+
+ while (bufPos < payloadEnd) {
+ int payloadElementType = buffer[bufPos++];
+
+ bool simulation;
+
+ if (buffer[bufPos++])
+ simulation = true;
+ else
+ simulation = false;
+
+ uint16_t appId;
+ bufPos = decodeUInt16FixedSize(&appId, buffer, bufPos);
+
+ uint16_t asduLength;
+ bufPos = decodeUInt16FixedSize(&asduLength, buffer, bufPos);
+
+ DEBUG_PRINTF("ASDU %02x sim: %i APPID: %04x length: %i", payloadElementType, simulation, appId, asduLength);
+
+ if (payloadElementType == 0x82) {
+ /* user payload */
+
+ //TODO copy ASDU payload to ???
+ handler(handlerParam, appId, buffer + bufPos, asduLength);
+ }
+ else {
+ DEBUG_PRINTF("unexpected payload type! (expect 82h)");
+ }
+
+ bufPos += asduLength;
+ }
+
+ }
+ else if (protocolVersion == 2) {
+ /* parse version 2 common header parts */
+
+ /* TimeOfCurrentKey */
+ uint32_t timeOfCurrentKey;
+ bufPos = decodeUInt32FixedSize(&timeOfCurrentKey, buffer, bufPos);
+
+ /* TimeToNextKey */
+ int16_t timeToNextKey;
+ bufPos = decodeInt16FixedSize(&timeToNextKey, buffer, bufPos);
+
+ RSecurityAlgorithm secAlgo = R_SESSION_SEC_ALGO_NONE;
+ RSignatureAlgorithm sigAlgo = R_SESSION_SIG_ALGO_NONE;
+
+ uint8_t* key = NULL;
+ int keySize = 0;
+
+ /* Key ID */
+ uint32_t keyId;
+ bufPos = decodeUInt32FixedSize(&keyId, buffer, bufPos);
+
+ if (keyId != 0) {
+ /* get key material associated with the key ID */
+
+ if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false) {
+ DEBUG_PRINTF("ERROR - key not found");
+ goto exit_error;
+ }
+ }
+ else {
+ DEBUG_PRINTF("ERROR - invalid key ID");
+ goto exit_error;
+ }
+
+ DEBUG_PRINTF("PV: 2 sec-algo: %i sig-algo: %i", secAlgo, sigAlgo);
+
+ uint8_t* iv = NULL;
+
+ /* IV */
+ int ivLen = buffer[bufPos++];
+
+ DEBUG_PRINTF("IV: size = %i\n", ivLen);
+
+ if (ivLen > 0) {
+ iv = buffer + bufPos;
+ bufPos += ivLen;
+ }
+
+ uint32_t payloadLength;
+
+ bufPos = decodeUInt32FixedSize(&payloadLength, buffer, bufPos);
+
+ uint8_t* payloadStart = buffer + bufPos;
+ int payloadStartPos = bufPos;
+
+ /* parse payload elements */
+ uint32_t payloadEnd = bufPos + payloadLength;
+
+ if (payloadEnd > msgSize) {
+ DEBUG_PRINTF("ERROR - payload size field invalid");
+ goto exit_error;
+ }
+
+ uint8_t signatureBuffer[128];
+
+ /* Check signature */
+ if (sigAlgo != R_SESSION_SIG_ALGO_NONE) {
+ if (key) {
+ if (RSessionCrypto_createHMAC(buffer, payloadEnd, key, keySize, signatureBuffer, 32)) {
+ if (buffer[payloadEnd] != 0x85) {
+ DEBUG_PRINTF("ERROR - no signature found");
+ goto exit_error;
+ }
+ else {
+ if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128) {
+ /* TODO is payloadEnd +2 correct? */
+ if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 16)) {
+ DEBUG_PRINTF("ERROR - signature not matching!");
+ goto exit_error;
+ }
+ }
+ else if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256) {
+ /* TODO is payloadEnd +2 correct? */
+ if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 32)) {
+ DEBUG_PRINTF("ERROR - signature not matching!");
+ goto exit_error;
+ }
+ }
+ }
+ }
+ else {
+ DEBUG_PRINTF("ERROR - failed to calculate HMAC!");
+ goto exit_error;
+ }
+ }
+ else {
+ DEBUG_PRINTF("ERROR - key not found!");
+ goto exit_error;
+ }
+ }
+
+ /* Check signature and decrypt application layer */
+ if (secAlgo != R_SESSION_SEC_ALGO_NONE) {
+ /* Check for HMAC */
+ if (payloadEnd + 18 <= msgSize) {
+ if (self->payloadBuffer == NULL)
+ self->payloadBuffer = GLOBAL_MALLOC(65000);
+
+ if (self->payloadBuffer) {
+ //TODO check MMAC tag
+ uint8_t* mac = buffer + payloadEnd + 2;
+ int macSize = buffer[payloadEnd + 1];
+
+ int payloadSize = payloadEnd - payloadStartPos;
+
+ if (RSessionCrypto_gcmAuthAndDecrypt(key, keySize, iv, ivLen, buffer, payloadStartPos, payloadStart, payloadSize, self->payloadBuffer, mac, macSize)) {
+ memcpy(buffer + bufPos, self->payloadBuffer, payloadSize);
+ }
+ else {
+ DEBUG_PRINTF("ERROR - auth and decrypt failed!");
+ goto exit_error;
+ }
+ }
+ }
+ else {
+ DEBUG_PRINTF("ERROR - sec algo - message too small!");
+ goto exit_error;
+ }
+ }
+
+ while (bufPos < payloadEnd) {
+ int payloadElementType = buffer[bufPos++];
+
+ bool simulation;
+
+ if (buffer[bufPos++])
+ simulation = true;
+ else
+ simulation = false;
+
+ uint16_t appId;
+ bufPos = decodeUInt16FixedSize(&appId, buffer, bufPos);
+
+ uint16_t asduLength;
+ bufPos = decodeUInt16FixedSize(&asduLength, buffer, bufPos);
+
+ DEBUG_PRINTF("ASDU %02x sim: %i APPID: %04x length: %i", payloadElementType, simulation, appId, asduLength);
+
+ if (payloadElementType == 0x82) {
+ /* user payload */
+
+ //TODO copy ASDU payload to ???
+ handler(handlerParam, appId, buffer + bufPos, asduLength);
+ }
+ else {
+ DEBUG_PRINTF("unexpected payload type! (expect 82h)");
+ }
+
+ bufPos += asduLength;
+ }
+ }
+ else {
+ DEBUG_PRINTF("only protocol version 1 and 2 supported (received version %i)", protocolVersion);
+ goto exit_error;
+ }
+
+ return R_SESSION_ERROR_OK;
+
+exit_parse_error:
+
+ DEBUG_PRINTF("ERROR - Failed to parse message");
+
+exit_error:
+
+ return R_SESSION_ERROR_INVALID_MESSAGE;
+}
+
+int
+encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RSessionPayloadElement elements)
+{
+ /* calculate total payload length */
+ int payloadLength = 0;
+
+ int startPos = bufPos;
+
+ uint8_t* iv = NULL;
+ int ivSize = 0;
+
+ RSessionPayloadElement element = elements;
+
+ while (element) {
+ payloadLength += 6; /* payload type, simulation, APPID, length */
+ payloadLength += element->payloadSize;
+
+ element = element->nextElement;
+ }
+
+ /* Connection less transport protocol */
+ buffer[bufPos++] = 0x01;
+ buffer[bufPos++] = 0x40;
+
+ /* SI */
+ buffer[bufPos++] = payloadType;
+
+ /* Session header length */
+ buffer[bufPos++] = 24;
+
+ /* Common session header tag */
+ buffer[bufPos++] = 0x80;
+
+ /* Common session header length */
+ buffer[bufPos++] = 18; // 12
+
+ /* SPDU length */
+ int spduLength = 4 + 2 + 4 + 4 + 1 + 1 + 4 + payloadLength;
+ DEBUG_PRINTF("payloadlength: %i spdu_len: %i", payloadLength, spduLength);
+ bufPos = encodeUInt32FixedSize(spduLength, buffer, bufPos);
+
+ /* SPDU number */
+ bufPos = encodeUInt32FixedSize(self->spduNumber, buffer, bufPos);
+
+ /* version */
+ bufPos = encodeUInt16FixedSize(self->protocolVersion, buffer, bufPos);
+
+ /* TimeOfCurrentKey */
+ bufPos = encodeUInt32FixedSize(self->timeOfCurrentKey, buffer, bufPos);
+
+ /* TimeToNextKey */
+ bufPos = encodeInt16FixedSize((int16_t)self->timeToNextKey, buffer, bufPos);
+
+ if (self->protocolVersion == 1) {
+ /* encryption algorithm */
+ buffer[bufPos++] = (uint8_t) self->secAlgo; /* 0 = none */
+
+ /* signature algorithm */
+ buffer[bufPos++] = (uint8_t) self->sigAlgo; /* 0 = none */
+
+ /* Key ID */
+ bufPos = encodeUInt32FixedSize(self->currentKeyId, buffer, bufPos);
+ }
+ else { /* protocol version 2 */
+ /* Key ID */
+ bufPos = encodeUInt32FixedSize(self->currentKeyId, buffer, bufPos);
+ self->secAlgo = self->currentSecAlgo;
+ self->sigAlgo = self->currentSigAlgo;
+
+ DEBUG_PRINTF("PV: 2 sec-algo: %i sig-algo: %i\n", self->secAlgo, self->sigAlgo);
+
+ /* IV */
+ if (self->secAlgo != R_SESSION_SEC_ALGO_NONE) {
+ /* TODO get and encode IV (initialization vector) */
+
+ buffer[bufPos++] = 12;
+
+ iv = buffer + bufPos;
+ ivSize = 12;
+
+ if (RSessionCrypto_createRandomData(iv, ivSize) == false) {
+ DEBUG_PRINTF("ERROR - Failed to create random IV");
+ }
+
+ bufPos += ivSize;
+ }
+ else {
+ buffer[bufPos++] = 0; /* empty initialization vector */
+ }
+ }
+
+ /* user payload length */
+ bufPos = encodeUInt32FixedSize(payloadLength, buffer, bufPos);
+
+ int encryptedPartStartPos = bufPos;
+
+ /* encode user payload elements */
+ element = elements;
+
+ while (element) {
+
+ /* payload type ? (according to example in annex G) */
+ buffer[bufPos++] = 0x82;
+
+ /* simulation */
+ buffer[bufPos++] = element->simulation;
+
+ /* APPID */
+ bufPos = encodeUInt16FixedSize(element->appId, buffer, bufPos);
+
+ /* APDU length */
+ bufPos = encodeUInt16FixedSize(element->payloadSize + 2, buffer, bufPos);
+
+ /* copy user payload */
+ memcpy(buffer + bufPos, element->payload, element->payloadSize);
+ bufPos += element->payloadSize;
+
+ element = element->nextElement;
+ }
+
+ if (self->sigAlgo != R_SESSION_SIG_ALGO_NONE) {
+
+ int signatureCoveredLength = bufPos - startPos;
+
+ DEBUG_PRINTF("Signature: %i", signatureCoveredLength);
+
+ /* add signature */
+ if (self->sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256) {
+
+ buffer[bufPos++] = 0x85;
+ buffer[bufPos++] = 16;
+
+ RSessionCrypto_createHMAC(buffer + startPos, signatureCoveredLength, self->currentKey, self->currentKeySize, buffer + bufPos, 32);
+
+ bufPos += 32;
+ }
+ else if (self->sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128) {
+ buffer[bufPos++] = 0x85;
+ buffer[bufPos++] = 16;
+ //buffer[bufPos++] = 0x20; /* 32 octets */
+
+ RSessionCrypto_createHMAC(buffer + startPos, signatureCoveredLength, self->currentKey, self->currentKeySize, buffer + bufPos, 16);
+
+ bufPos += 16;
+ }
+ else {
+ DEBUG_PRINTF("ERROR - unsupported signature type");
+ }
+ }
+
+ int payloadEndPos = bufPos;
+
+ if (self->secAlgo != R_SESSION_SEC_ALGO_NONE) {
+ /* create signature and encrypt payload */
+
+ buffer[bufPos++] = 0x85;
+ buffer[bufPos++] = 16;
+
+ int addPartSize = encryptedPartStartPos - startPos;
+ int encryptedPartSize = payloadEndPos - encryptedPartStartPos;
+
+#ifdef DEBUG_RSESSION
+ printBuffer(buffer + startPos, bufPos - startPos);
+#endif
+
+ DEBUG_PRINTF("===> encrypt ====");
+
+ if (RSessionCrypto_gcmEncryptAndTag(self->currentKey, self->currentKeySize, iv, ivSize, buffer + startPos, addPartSize, buffer + encryptedPartStartPos, encryptedPartSize, buffer + bufPos, 16) == false) {
+ DEBUG_PRINTF("ERROR - encryption failed");
+ }
+ else {
+ bufPos += 16;
+ }
+ }
+
+ self->spduNumber++;
+
+ return bufPos;
+}
+
+/*
+ * NOTE: For library internal use!
+ */
+RSessionError
+RSession_sendMessage(RSession self, RSessionProtocol_SPDU_ID spduId, bool simulation, uint16_t appId, uint8_t* payload, int payloadSize)
+{
+ if (self->socket == NULL) {
+ self->socket = UdpSocket_create();
+ }
+
+ if (self->sendBuffer == NULL) {
+ self->sendBuffer = (uint8_t*) GLOBAL_MALLOC(self->bufferSize);
+
+ if (self->sendBuffer == NULL)
+ return R_SESSION_ERROR_OUT_OF_MEMORY;
+ }
+
+ if (self->socket) {
+
+ struct sRSessionPayloadElement element;
+
+ element.simulation = simulation;
+ element.appId = appId;
+ element.payload = payload;
+ element.payloadSize = payloadSize;
+ element.nextElement = NULL;
+
+ int msgSize = encodePacket(self, (uint8_t) spduId, self->sendBuffer, 0, &element);
+
+#ifdef DEBUG_RSESSION
+ printBuffer(self->sendBuffer, msgSize);
+#endif
+
+ if (UdpSocket_sendTo(self->socket, self->remoteAddress, self->remotePort, self->sendBuffer, msgSize)) {
+ return R_SESSION_ERROR_OK;
+ }
+ else {
+ return R_SESSION_ERROR_FAILED_TO_SEND;
+ }
+
+ }
+ else {
+ return R_SESSION_ERROR_NO_SOCKET;
+ }
+}
+
+void
+RSession_setBufferSize(RSession self, uint16_t bufferSize)
+{
+ if (bufferSize > 127) {
+ self->bufferSize = bufferSize;
+ }
+ else {
+ self->bufferSize = 128;
+ }
+}
+
+RSessionError
+RSession_receiveMessage(RSession self, RSessionPayloadElementHandler handler, void* parameter)
+{
+ Semaphore_wait(self->socketLock);
+
+ if (self->socket) {
+ char ipAddrBuf[128];
+
+ if (self->sendBuffer == NULL) {
+ self->sendBuffer = (uint8_t*) GLOBAL_MALLOC(self->bufferSize);
+
+ if (self->sendBuffer == NULL) {
+ Semaphore_post(self->socketLock);
+
+ return R_SESSION_ERROR_OUT_OF_MEMORY;
+ }
+ }
+
+ //TODO use handleset
+
+ int msgSize = UdpSocket_receiveFrom(self->socket, ipAddrBuf, 128, self->sendBuffer, self->bufferSize);
+
+ Semaphore_post(self->socketLock);
+
+ if (msgSize < 1) {
+ printf("RESSSION: Failed to receive message\n");
+
+
+ return R_SESSION_ERROR_FAILED_TO_RECEIVE;
+ }
+ else {
+ return parseSessionMessage(self, self->sendBuffer, msgSize, handler, parameter);
+ }
+ }
+ else {
+ Semaphore_post(self->socketLock);
+
+ return R_SESSION_ERROR_NO_SOCKET;
+ }
+}
+
+RSessionError
+RSession_addKey(RSession self, uint32_t keyId, uint8_t* key, int keyLength, RSecurityAlgorithm secAlgo, RSignatureAlgorithm sigAlgo)
+{
+ RSessionKeyMaterial keyMaterial = RSessionKeyMaterial_create(keyId, key, keyLength, secAlgo, sigAlgo);
+
+ if (keyMaterial) {
+ Semaphore_wait(self->keyListLock);
+ LinkedList_add(self->keyList, keyMaterial);
+ Semaphore_post(self->keyListLock);
+
+ return R_SESSION_ERROR_OK;
+ }
+ else {
+ return R_SESSION_ERROR_OUT_OF_MEMORY;
+ }
+}
+
+static RSessionKeyMaterial
+getKeyById(RSession self, uint32_t keyId)
+{
+ LinkedList keyElem = LinkedList_getNext(self->keyList);
+
+ while (keyElem) {
+ RSessionKeyMaterial keyMaterial = (RSessionKeyMaterial) LinkedList_getData(keyElem);
+
+ if (keyMaterial->keyId == keyId) {
+ return keyMaterial;
+ }
+
+ keyElem = LinkedList_getNext(keyElem);
+ }
+
+ return NULL;
+}
+
+RSessionError
+RSession_removeKey(RSession self, uint32_t keyId)
+{
+ Semaphore_wait(self->keyListLock);
+
+ RSessionKeyMaterial keyMaterial = getKeyById(self, keyId);
+
+ if (keyMaterial) {
+ LinkedList_remove(self->keyList, keyMaterial);
+
+ RSessionKeyMaterial_destroy(keyMaterial);
+ }
+
+ Semaphore_post(self->keyListLock);
+
+ if (self->currentKeyId == keyId) {
+ /* active key removed! */
+ self->currentKeyId = 0;
+ }
+
+ return R_SESSION_ERROR_OK;
+}
+
+/**
+ * \brief Set the active key for the sender
+ */
+RSessionError
+RSession_setActiveKey(RSession self, uint32_t keyId)
+{
+ RSessionError retVal = R_SESSION_ERROR_OK;
+
+ Semaphore_wait(self->keyListLock);
+
+ RSessionKeyMaterial keyMaterial = getKeyById(self, keyId);
+
+ if (keyMaterial) {
+ self->currentKeySize = keyMaterial->keyLength;
+ self->currentKey = keyMaterial->key;
+ self->currentKeyId = keyMaterial->keyId;
+ self->currentSecAlgo = keyMaterial->secAlgo;
+ self->currentSigAlgo = keyMaterial->sigAlgo;
+ }
+ else {
+ retVal = R_SESSION_ERROR_INVALID_KEY;
+ }
+
+ Semaphore_post(self->keyListLock);
+
+ return retVal;
+}
+
+void
+RSession_destroy(RSession self)
+{
+ if (self) {
+ if (self->socket)
+ Socket_destroy((Socket)self->socket);
+
+ if (self->sendBuffer)
+ GLOBAL_FREEMEM(self->sendBuffer);
+
+ if (self->remoteAddress)
+ GLOBAL_FREEMEM(self->remoteAddress);
+
+ if (self->localAddress)
+ GLOBAL_FREEMEM(self->localAddress);
+
+ if (self->payloadBuffer)
+ GLOBAL_FREEMEM(self->payloadBuffer);
+
+ if (self->keyList)
+ LinkedList_destroyDeep(self->keyList, (LinkedListValueDeleteFunction)RSessionKeyMaterial_destroy);
+
+ if (self->keyListLock)
+ Semaphore_destroy(self->keyListLock);
+
+ if (self->socketLock)
+ Semaphore_destroy(self->socketLock);
+
+ GLOBAL_FREEMEM(self);
+ }
+}
+
+Socket
+RSession_getSocket(RSession self)
+{
+ return (Socket)self->socket;
+}
+
diff --git a/src/r_session/r_session.h b/src/r_session/r_session.h
new file mode 100644
index 00000000..716aed6d
--- /dev/null
+++ b/src/r_session/r_session.h
@@ -0,0 +1,207 @@
+/*
+ * r_session.h
+ *
+ * Copyright 2013-2021 Michael Zillgith
+ *
+ * This file is part of libIEC61850.
+ *
+ * libIEC61850 is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libIEC61850 is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libIEC61850. If not, see .
+ *
+ * See COPYING file for the complete license text.
+ */
+
+#ifndef LIBIEC61850_SRC_SAMPLED_VALUES_R_SESSION_H_
+#define LIBIEC61850_SRC_SAMPLED_VALUES_R_SESSION_H_
+
+#include "libiec61850_common_api.h"
+#include "hal_socket.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+typedef struct sRSession* RSession;
+
+typedef enum {
+ R_SESSION_SEC_ALGO_NONE = 0,
+ R_SESSION_SEC_ALGO_AES_128_GCM = 1,
+ R_SESSION_SEC_ALGO_AES_256_GCM = 2
+} RSecurityAlgorithm;
+
+typedef enum {
+ R_SESSION_SIG_ALGO_NONE = 0,
+ R_SESSION_SIG_ALGO_HMAC_SHA256_80 = 1,
+ R_SESSION_SIG_ALGO_HMAC_SHA256_128 = 2,
+ R_SESSION_SIG_ALGO_HMAC_SHA256_256 = 3,
+ R_SESSION_SIG_ALGO_AES_GMAC_64 = 4,
+ R_SESSION_SIG_ALGO_AES_GMAC_128 = 5,
+ R_SESSION_SIG_ALGO_HMAC_SHA3_80 = 6,
+ R_SESSION_SIG_ALGO_HMAC_SHA3_128 = 7,
+ R_SESSION_SIG_ALGO_HMAC_SHA3_256 = 8
+} RSignatureAlgorithm;
+
+typedef enum {
+ R_SESSION_ERROR_OK = 0,
+ R_SESSION_ERROR_INVALID_KEY = 1,
+ R_SESSION_ERROR_KEY_QUEUE_FULL = 2,
+ R_SESSION_ERROR_NO_SOCKET = 3,
+ R_SESSION_ERROR_OUT_OF_MEMORY = 4,
+ R_SESSION_ERROR_FAILED_TO_SEND = 5,
+ R_SESSION_ERROR_FAILED_TO_RECEIVE = 6,
+ R_SESSION_ERROR_INVALID_MESSAGE = 7,
+ R_SESSION_ERROR_SET_FAILED = 8
+} RSessionError;
+
+typedef struct sRSessionPayloadElement* RSessionPayloadElement;
+
+struct sRSessionPayloadElement
+{
+ bool simulation;
+ uint16_t appId;
+ uint8_t* payload;
+ int payloadSize;
+ RSessionPayloadElement nextElement; /* NULL when no more elements follow */
+};
+
+/**
+ * \brief Create a new RSession instance to provide R-GOOSE/R-SMV support for GOOSE/SMV publisher/subscriber
+ *
+ * \return new RSession instance
+ */
+LIB61850_API RSession
+RSession_create();
+
+/**
+ * \brief Set the maximum buffer size for session messages (range: 128 - 65535)
+ *
+ * \param self the RSession instance
+ * \param bufferSize the size of the buffer for RSession UDP messages (range: 128 - 65535)
+ */
+LIB61850_API void
+RSession_setBufferSize(RSession self, uint16_t bufferSize);
+
+/* Required only for version 1 of the protocol! */
+LIB61850_API RSessionError
+RSession_setSecurity(RSession self, RSecurityAlgorithm secAlgo, RSignatureAlgorithm sigAlgo);
+
+LIB61850_API RSessionError
+RSession_setLocalAddress(RSession self, const char* localAddress, int localPort);
+
+/**
+ * \brief Add this instance to an IPv4/IPv6 multicast group
+ *
+ * \param self the RSession instance
+ * \param multicastAddress IPv4 or IPv6 multicast address
+ *
+ * \return R_SESSION_ERROR_OK on success, R_SESSION_ERROR_SET_FAILED otherwise
+ */
+LIB61850_API RSessionError
+RSession_addMulticastGroup(RSession self, const char* multicastAddress);
+
+/**
+ * \brief Sets the multicast TTL (number of hops) for this RSession instance
+ *
+ * \param self the RSession instance
+ * \param ttl number of hops for multicast messages. Default is 1 (not routable!)
+ *
+ * \return R_SESSION_ERROR_OK on success, error code otherwise
+ */
+LIB61850_API RSessionError
+RSession_setMulticastTtl(RSession self, int ttl);
+
+/**
+ * \brief Set the destionation address and port for publishers
+ *
+ * \param self the RSession instance
+ * \param remoteAddress remote IP address
+ * \param remotePort remote UDP port
+ *
+ * \return R_SESSION_ERROR_OK on success, error code otherwise
+ */
+LIB61850_API RSessionError
+RSession_setRemoteAddress(RSession self, const char* remoteAddress, int remotePort);
+
+/**
+ * \brief Start listening on the local UDP port to receive remote messages (only for subscriber)
+ *
+ * \param self the RSession instance
+ *
+ * \return R_SESSION_ERROR_OK on success, error code otherwise
+ */
+LIB61850_API RSessionError
+RSession_startListening(RSession self);
+
+/**
+ * \brief Stop listening on the local UDP port (only for subscriber)
+ *
+ * \param self the RSession instance
+ *
+ * \return R_SESSION_ERROR_OK on success, error code otherwise
+ */
+LIB61850_API RSessionError
+RSession_stopListening(RSession self);
+
+/**
+ * \brief Manually add a key to the RSession instance
+ *
+ * \param self the RSession instance
+ * \param keyId the key ID is unique for the security association
+ * \param key the key data
+ * \param keyLength the length of the key in bytes
+ * \param secAlgo the applicable security (encryption) algorithm
+ * \param sigAlgo the applicable signature algorithm
+ */
+LIB61850_API RSessionError
+RSession_addKey(RSession self, uint32_t keyId, uint8_t* key, int keyLength, RSecurityAlgorithm secAlgo, RSignatureAlgorithm sigAlgo);
+
+LIB61850_API RSessionError
+RSession_removeKey(RSession self, uint32_t keyId);
+
+typedef enum
+{
+ RSESSION_KEY_EVENT__NEED_KEY = 1
+} RSessionKeyEvent;
+
+typedef void (*RSession_KeyEventHandler) (void* parameter, RSession rSession, RSessionKeyEvent event, uint32_t keyID);
+
+/**
+ * \brief Set a callback handler to receive key events from the RSession instance
+ *
+ * e.g. when the RSession instance has no valid key for the received messages or to publish messages.
+ */
+LIB61850_API void
+RSession_setKeyEventHandler(RSession self, RSession_KeyEventHandler handler, void* parameter);
+
+/**
+ * \brief Set the active key for the sender/publisher
+ *
+ * \param self the RSession instance
+ * \param keyId the key ID of the new active key (has to be added with \ref RSession_addKey before).
+ */
+LIB61850_API RSessionError
+RSession_setActiveKey(RSession self, uint32_t keyId);
+
+/**
+ * \brief Destroy the RSession instance and free all resource
+ *
+ * \param self the RSession instance
+ */
+LIB61850_API void
+RSession_destroy(RSession self);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* LIBIEC61850_SRC_SAMPLED_VALUES_R_SESSION_H_ */
diff --git a/src/r_session/r_session_crypto.h b/src/r_session/r_session_crypto.h
new file mode 100644
index 00000000..d7a76a48
--- /dev/null
+++ b/src/r_session/r_session_crypto.h
@@ -0,0 +1,44 @@
+/*
+ * r_session_crypto.h
+ *
+ * Copyright 2013-2021 Michael Zillgith
+ *
+ * This file is part of libIEC61850.
+ *
+ * libIEC61850 is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libIEC61850 is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libIEC61850. If not, see .
+ *
+ * See COPYING file for the complete license text.
+ */
+
+#ifndef SRC_R_SESSION_R_SESSION_CRYPTO_H_
+#define SRC_R_SESSION_R_SESSION_CRYPTO_H_
+
+#include
+#include
+
+#include "libiec61850_common_api.h"
+
+LIB61850_INTERNAL bool
+RSessionCrypto_createHMAC(uint8_t* buffer, int bufSize, uint8_t* key, int keySize, uint8_t* hmac, int hmacMaxSize);
+
+LIB61850_INTERNAL bool
+RSessionCrypto_gcmEncryptAndTag(uint8_t* key, int keySize, uint8_t* iv, int ivSize, uint8_t* addData, int addDataSize, uint8_t* encryptData, int encryptDataSize, uint8_t* tag, int tagSize);
+
+LIB61850_INTERNAL bool
+RSessionCrypto_gcmAuthAndDecrypt(uint8_t* key, int keySize, uint8_t* iv, int ivSize, uint8_t* addData, int addDataSize, uint8_t* encryptData, int encryptDataSize, uint8_t* decryptedData, uint8_t* tag, int tagSize);
+
+LIB61850_INTERNAL bool
+RSessionCrypto_createRandomData(uint8_t* data, int dataSize);
+
+#endif /* SRC_R_SESSION_R_SESSION_CRYPTO_H_ */
diff --git a/src/r_session/r_session_crypto_mbedtls.c b/src/r_session/r_session_crypto_mbedtls.c
new file mode 100644
index 00000000..0fe577cb
--- /dev/null
+++ b/src/r_session/r_session_crypto_mbedtls.c
@@ -0,0 +1,143 @@
+/*
+ * r_session_crpyto_mbedtls.c
+ *
+ * Implementation of RSessionCrypto interface using mbedtls
+ *
+ * Copyright 2013-2021 Michael Zillgith
+ *
+ * This file is part of libIEC61850.
+ *
+ * libIEC61850 is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libIEC61850 is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libIEC61850. If not, see .
+ *
+ * See COPYING file for the complete license text.
+ */
+
+
+#include "mbedtls/cipher.h"
+#include "mbedtls/md.h"
+#include "mbedtls/md_internal.h"
+#include "mbedtls/platform_util.h"
+#include "mbedtls/gcm.h"
+#include "mbedtls/entropy.h"
+#include "mbedtls/ctr_drbg.h"
+
+#include "libiec61850_platform_includes.h"
+
+#include "r_session_crypto.h"
+
+bool
+RSessionCrypto_createHMAC(uint8_t* buffer, int bufSize, uint8_t* key, int keySize, uint8_t* hmac, int hmacMaxSize)
+{
+ const mbedtls_md_info_t* md_info = &mbedtls_sha256_info;
+
+ mbedtls_md_context_t md_ctx;
+
+ mbedtls_md_init( &md_ctx );
+
+ mbedtls_md_setup(&md_ctx, md_info, 1);
+
+ if (mbedtls_md_hmac_starts(&md_ctx, key, keySize)) {
+ printf("Error in initializing HMAC\n");
+ return false;
+ }
+
+ if (mbedtls_md_hmac_update(&md_ctx, buffer, bufSize)) {
+ printf("Failed to calculate HMAC\n");
+ return false;
+ }
+
+ if (mbedtls_md_hmac_finish(&md_ctx, hmac)) {
+ printf("Failed to finish HMAC\n");
+ return false;
+ }
+
+ mbedtls_md_free(&md_ctx);
+
+ return true;
+}
+
+bool
+RSessionCrypto_gcmEncryptAndTag(uint8_t* key, int keySize, uint8_t* iv, int ivSize, uint8_t* addData, int addDataSize, uint8_t* encryptData, int encryptDataSize, uint8_t* tag, int tagSize)
+{
+ mbedtls_gcm_context gcmCtx;
+
+ mbedtls_gcm_init(&gcmCtx);
+
+ if (mbedtls_gcm_setkey(&gcmCtx, MBEDTLS_CIPHER_ID_AES , (const unsigned char*) key, keySize * 8)) {
+ printf("AES-GCM: Failed to set key\n");
+ mbedtls_gcm_free(&gcmCtx);
+ return false;
+ }
+
+ if (mbedtls_gcm_crypt_and_tag(&gcmCtx, MBEDTLS_GCM_ENCRYPT, (size_t) encryptDataSize, iv, (size_t) ivSize, addData, (size_t) addDataSize, encryptData, encryptData, (size_t)tagSize, tag)) {
+ printf("AES-GCM: Failed to authenticate/encrypt data\n");
+ mbedtls_gcm_free(&gcmCtx);
+ return false;
+ }
+
+ mbedtls_gcm_free(&gcmCtx);
+
+ return true;
+}
+
+bool
+RSessionCrypto_gcmAuthAndDecrypt(uint8_t* key, int keySize, uint8_t* iv, int ivSize, uint8_t* addData, int addDataSize, uint8_t* encryptData, int encryptDataSize, uint8_t* decryptedData, uint8_t* tag, int tagSize)
+{
+ mbedtls_gcm_context gcmCtx;
+
+ mbedtls_gcm_init(&gcmCtx);
+
+ if (mbedtls_gcm_setkey(&gcmCtx, MBEDTLS_CIPHER_ID_AES , (const unsigned char*) key, keySize * 8)) {
+ printf("AES-GCM: Failed to set key\n");
+ mbedtls_gcm_free(&gcmCtx);
+ return false;
+ }
+
+ if (mbedtls_gcm_auth_decrypt(&gcmCtx, (size_t) encryptDataSize, iv, (size_t) ivSize, addData, (size_t) addDataSize, tag, (size_t) tagSize, encryptData, decryptedData)) {
+ printf("AES-GCM: Failed to authentication and decrypt!\n");
+ mbedtls_gcm_free(&gcmCtx);
+ return false;
+ }
+
+ mbedtls_gcm_free(&gcmCtx);
+
+ return true;
+}
+
+bool
+RSessionCrypto_createRandomData(uint8_t* data, int dataSize)
+{
+ int ret;
+ mbedtls_ctr_drbg_context ctr_drbg;
+ mbedtls_entropy_context entropy;
+
+ mbedtls_entropy_init( &entropy );
+ mbedtls_ctr_drbg_init( &ctr_drbg );
+
+ if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy,
+ NULL, 0) ) != 0 )
+ {
+ printf( " failed\n ! mbedtls_ctr_drbg_init returned -0x%04x\n", -ret );
+ return false;
+ }
+
+ if( ( ret = mbedtls_ctr_drbg_random( &ctr_drbg, data, dataSize ) ) != 0 )
+ {
+ printf( " failed\n ! mbedtls_ctr_drbg_random returned -0x%04x\n", -ret );
+ return false;
+ }
+
+ return true;
+}
+
diff --git a/src/r_session/r_session_internal.h b/src/r_session/r_session_internal.h
new file mode 100644
index 00000000..f02c03a6
--- /dev/null
+++ b/src/r_session/r_session_internal.h
@@ -0,0 +1,55 @@
+/*
+ * r_session_internal.h
+ *
+ * Copyright 2013-2021 Michael Zillgith
+ *
+ * This file is part of libIEC61850.
+ *
+ * libIEC61850 is free software: you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * libIEC61850 is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with libIEC61850. If not, see .
+ *
+ * See COPYING file for the complete license text.
+ */
+
+#ifndef SRC_R_SESSION_R_SESSION_INTERNAL_H_
+#define SRC_R_SESSION_R_SESSION_INTERNAL_H_
+
+#include "r_session.h"
+
+LIB61850_INTERNAL Socket
+RSession_getSocket(RSession self);
+
+typedef enum {
+ RSESSION_SPDU_ID_TUNNELED = 0xa0,
+ RSESSION_SPDU_ID_GOOSE = 0xa1,
+ RSESSION_SPDU_ID_SV = 0xa2,
+ RSESSION_SPDU_ID_MGMT = 0xa3
+} RSessionProtocol_SPDU_ID;
+
+/*
+ * NOTE: For library internal use!
+ *
+ * \param spduId SPDU Identifier(SI) - 0xa0(tunneled), 0xa1(GOOSE), 0xa2(Sampled Values)
+ */
+LIB61850_INTERNAL RSessionError
+RSession_sendMessage(RSession self, RSessionProtocol_SPDU_ID spduId, bool simulation, uint16_t appId, uint8_t* payload, int payloadSize);
+
+//RSessionError
+//RSession_sendMultiplePayloadMessage(RSession self, RSessionPayloadElement elements);
+
+typedef void (*RSessionPayloadElementHandler) (void* parameter, uint16_t appId, uint8_t* payloadData, int payloadSize);
+
+LIB61850_INTERNAL RSessionError
+RSession_receiveMessage(RSession self, RSessionPayloadElementHandler handler, void* parameter);
+
+#endif /* SRC_R_SESSION_R_SESSION_INTERNAL_H_ */
diff --git a/src/sampled_values/sv_publisher.c b/src/sampled_values/sv_publisher.c
index 9b692bea..eb67db4c 100644
--- a/src/sampled_values/sv_publisher.c
+++ b/src/sampled_values/sv_publisher.c
@@ -1,7 +1,7 @@
/*
* sv_publisher.c
*
- * Copyright 2016 Michael Zillgith
+ * Copyright 2016-2022 Michael Zillgith
*
* This file is part of libIEC61850.
*
@@ -30,6 +30,8 @@
#include "hal_ethernet.h"
#include "ber_encoder.h"
+#include "r_session_internal.h"
+
#ifndef DEBUG_SV_PUBLISHER
#define DEBUG_SV_PUBLISHER 1
#endif
@@ -70,9 +72,16 @@ struct sSVPublisher_ASDU {
struct sSVPublisher {
uint8_t* buffer;
+
uint16_t appId;
+ bool simulation;
+
+ /* only for Ethernet based SV */
EthernetSocket ethernetSocket;
+ /* only for R-SV */
+ RSession remoteSession;
+
int lengthField; /* can probably be removed since packets have fixed size! */
int payloadStart;
@@ -264,6 +273,27 @@ encodeInt64FixedSize(int64_t value, uint8_t* buffer, int bufPos)
return bufPos;
}
+SVPublisher
+SVPublisher_createRemote(RSession session, uint16_t appId)
+{
+ SVPublisher self = (SVPublisher) GLOBAL_CALLOC(1, sizeof(struct sSVPublisher));
+
+ if (self) {
+ self->asduList = NULL;
+
+ self->buffer = (uint8_t*) GLOBAL_MALLOC(SV_MAX_MESSAGE_SIZE);
+
+ self->payloadStart = 0;
+ self->remoteSession = session;
+ self->lengthField = 0;
+
+ self->simulation = false;
+ self->appId = appId;
+ }
+
+ return self;
+}
+
SVPublisher
SVPublisher_createEx(CommParameters* parameters, const char* interfaceId, bool useVlanTag)
{
@@ -271,6 +301,7 @@ SVPublisher_createEx(CommParameters* parameters, const char* interfaceId, bool u
if (self) {
self->asduList = NULL;
+ self->lengthField = 0;
if (preparePacketBuffer(self, parameters, interfaceId, useVlanTag) == false) {
SVPublisher_destroy(self);
@@ -465,13 +496,14 @@ SVPublisher_setupComplete(SVPublisher self)
size_t msgLength = payloadLength + 8;
- int lengthIndex = self->lengthField;
+ if (self->lengthField != 0) {
+ int lengthIndex = self->lengthField;
- self->buffer[lengthIndex] = msgLength / 256;
- self->buffer[lengthIndex + 1] = msgLength & 0xff;
+ self->buffer[lengthIndex] = msgLength / 256;
+ self->buffer[lengthIndex + 1] = msgLength & 0xff;
+ }
self->payloadLength = payloadLength;
-
}
void
@@ -480,7 +512,16 @@ SVPublisher_publish(SVPublisher self)
if (DEBUG_SV_PUBLISHER)
printf("SV_PUBLISHER: send SV message\n");
- Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength);
+ if (self->ethernetSocket) {
+ Ethernet_sendPacket(self->ethernetSocket, self->buffer, self->payloadStart + self->payloadLength);
+ }
+ else if (self->remoteSession) {
+ RSession_sendMessage(self->remoteSession, RSESSION_SPDU_ID_SV, self->simulation, self->appId, self->buffer, self->payloadLength);
+ }
+ else {
+ if (DEBUG_SV_PUBLISHER)
+ printf("SV_PUBLISHER: no network layer!\n");
+ }
}
void
diff --git a/src/sampled_values/sv_publisher.h b/src/sampled_values/sv_publisher.h
index f753dc5d..8985e333 100644
--- a/src/sampled_values/sv_publisher.h
+++ b/src/sampled_values/sv_publisher.h
@@ -1,7 +1,7 @@
/*
* sv_publisher.h
*
- * Copyright 2016-2018 Michael Zillgith
+ * Copyright 2016-2022 Michael Zillgith
*
* This file is part of libIEC61850.
*
@@ -26,6 +26,7 @@
#define LIBIEC61850_SRC_SAMPLED_VALUES_SV_PUBLISHER_H_
#include "iec61850_common.h"
+#include "r_session.h"
#ifdef __cplusplus
extern "C" {
@@ -90,6 +91,9 @@ SVPublisher_create(CommParameters* parameters, const char* interfaceId);
LIB61850_API SVPublisher
SVPublisher_createEx(CommParameters* parameters, const char* interfaceId, bool useVlanTag);
+LIB61850_API SVPublisher
+SVPublisher_createRemote(RSession session, uint16_t appId);
+
/**
* \brief Create an Application Service Data Unit (ASDU) and add it to an existing Sampled Values publisher.
*
diff --git a/src/sampled_values/sv_subscriber.c b/src/sampled_values/sv_subscriber.c
index 99f5d34f..d9ff72a4 100644
--- a/src/sampled_values/sv_subscriber.c
+++ b/src/sampled_values/sv_subscriber.c
@@ -29,11 +29,14 @@
#include "hal_ethernet.h"
#include "hal_thread.h"
+#include "hal_socket.h"
#include "ber_decode.h"
#include "ber_encoder.h"
#include "sv_subscriber.h"
+#include "r_session_internal.h"
+
#ifndef DEBUG_SV_SUBSCRIBER
#define DEBUG_SV_SUBSCRIBER 1
#endif
@@ -53,6 +56,8 @@ struct sSVReceiver {
uint8_t* buffer;
EthernetSocket ethSocket;
+ RSession session;
+
LinkedList subscriberList;
#if (CONFIG_MMS_THREADLESS_STACK == 0)
@@ -62,7 +67,10 @@ struct sSVReceiver {
};
struct sSVSubscriber {
+ RSession session;
+
uint8_t ethAddr[6];
+
uint16_t appId;
SVUpdateListener listener;
@@ -105,6 +113,27 @@ SVReceiver_create(void)
return self;
}
+SVReceiver
+SVReceiver_createRemote(RSession session)
+{
+ SVReceiver self = (SVReceiver) GLOBAL_CALLOC(1, sizeof(struct sSVReceiver));
+
+ if (self != NULL) {
+ self->subscriberList = LinkedList_create();
+ self->buffer = NULL;
+
+ self->checkDestAddr = false;
+
+ self->session = session;
+
+#if (CONFIG_MMS_THREADLESS_STACK == 0)
+ self->subscriberListLock = Semaphore_create(1);
+#endif
+ }
+
+ return self;
+}
+
void
SVReceiver_setInterfaceId(SVReceiver self, const char* interfaceId)
{
@@ -158,12 +187,14 @@ static void*
svReceiverLoop(void* threadParameter)
{
SVReceiver self = (SVReceiver) threadParameter;
- EthernetHandleSet handleSet = EthernetHandleSet_new();
- EthernetHandleSet_addSocket(handleSet, self->ethSocket);
- self->stopped = false;
+ if (self->ethSocket) {
+ EthernetHandleSet handleSet = EthernetHandleSet_new();
+ EthernetHandleSet_addSocket(handleSet, self->ethSocket);
+
+ self->stopped = false;
- while (self->running) {
+ while (self->running) {
switch (EthernetHandleSet_waitReady(handleSet, 100))
{
case -1:
@@ -175,12 +206,35 @@ svReceiverLoop(void* threadParameter)
default:
SVReceiver_tick(self);
}
+ }
+ EthernetHandleSet_destroy(handleSet);
}
+ else if (self->session) {
+ self->stopped = false;
- self->stopped = true;
+ HandleSet handleSet = Handleset_new();
+
+ Handleset_addSocket(handleSet, RSession_getSocket(self->session));
+
+ while (self->running) {
+ switch (Handleset_waitReady(handleSet, 100))
+ {
+ case -1:
+ if (DEBUG_SV_SUBSCRIBER)
+ printf("SV_SUBSCRIBER: HandleSet_waitReady() failure\n");
+ break;
+ case 0:
+ break;
+ default:
+ SVReceiver_tick(self);
+ }
+ }
- EthernetHandleSet_destroy(handleSet);
+ Handleset_destroy(handleSet);
+ }
+
+ self->stopped = true;
return NULL;
}
@@ -190,8 +244,14 @@ SVReceiver_start(SVReceiver self)
{
if (SVReceiver_startThreadless(self)) {
- if (DEBUG_SV_SUBSCRIBER)
- printf("SV_SUBSCRIBER: SV receiver started for interface %s\n", self->interfaceId);
+ if (self->interfaceId) {
+ if (DEBUG_SV_SUBSCRIBER)
+ printf("SV_SUBSCRIBER: SV receiver started for interface %s\n", self->interfaceId);
+ }
+ else {
+ if (DEBUG_SV_SUBSCRIBER)
+ printf("SV_SUBSCRIBER: R-SV receiver started\n");
+ }
Thread thread = Thread_create((ThreadExecutionFunction) svReceiverLoop, (void*) self, true);
@@ -244,22 +304,37 @@ SVReceiver_destroy(SVReceiver self)
GLOBAL_FREEMEM(self);
}
-EthernetSocket
+bool
SVReceiver_startThreadless(SVReceiver self)
{
- if (self->interfaceId == NULL)
- self->ethSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, NULL);
- else
- self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
+ if (self->session) {
+ if (RSession_startListening(self->session) == R_SESSION_ERROR_OK) {
+ self->running = true;
- if (self->ethSocket) {
+ return true;
+ }
+ else {
+ return false;
+ }
+ }
+ else {
+ if (self->interfaceId == NULL)
+ self->ethSocket = Ethernet_createSocket(CONFIG_ETHERNET_INTERFACE_ID, NULL);
+ else
+ self->ethSocket = Ethernet_createSocket(self->interfaceId, NULL);
+
+ if (self->ethSocket) {
- Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV);
+ Ethernet_setProtocolFilter(self->ethSocket, ETH_P_SV);
+
+ self->running = true;
+ }
- self->running = true;
+ if (self->ethSocket)
+ return true;
+ else
+ return false;
}
-
- return self->ethSocket;
}
void
@@ -268,6 +343,10 @@ SVReceiver_stopThreadless(SVReceiver self)
if (self->ethSocket)
Ethernet_destroySocket(self->ethSocket);
+ if (self->session) {
+ RSession_stopListening(self->session);
+ }
+
self->running = false;
}
@@ -458,6 +537,73 @@ exit_error:
return;
}
+static void
+handleSVApdu(SVReceiver self, uint16_t appId, uint8_t* apdu, int apduLength, uint8_t* dstAddr)
+{
+ if (DEBUG_SV_SUBSCRIBER) {
+ printf("SV_SUBSCRIBER: SV message: ----------------\n");
+ printf("SV_SUBSCRIBER: APPID: %u\n", appId);
+ printf("SV_SUBSCRIBER: APDU length: %i\n", apduLength);
+ }
+
+ /* check if there is a matching subscriber */
+
+#if (CONFIG_MMS_THREADLESS_STACK == 0)
+ Semaphore_wait(self->subscriberListLock);
+#endif
+
+ LinkedList element = LinkedList_getNext(self->subscriberList);
+
+ SVSubscriber subscriber;
+
+ bool subscriberFound = false;
+
+ while (element != NULL) {
+ subscriber = (SVSubscriber) LinkedList_getData(element);
+
+ if (subscriber->appId == appId) {
+
+
+ if (self->checkDestAddr) {
+
+ if (self->ethSocket) {
+ if (memcmp(dstAddr, subscriber->ethAddr, 6) == 0) {
+ subscriberFound = true;
+ break;
+ }
+ else
+ if (DEBUG_SV_SUBSCRIBER)
+ printf("SV_SUBSCRIBER: Checking ethernet dest address failed!\n");
+ }
+ else {
+ //TODO check destination IP address for R-SV
+
+ }
+
+ }
+ else {
+ subscriberFound = true;
+ break;
+ }
+
+
+ }
+
+ element = LinkedList_getNext(element);
+ }
+
+#if (CONFIG_MMS_THREADLESS_STACK == 0)
+ Semaphore_post(self->subscriberListLock);
+#endif
+
+ if (subscriberFound)
+ parseSVPayload(self, subscriber, apdu, apduLength);
+ else {
+ if (DEBUG_SV_SUBSCRIBER)
+ printf("SV_SUBSCRIBER: SV message ignored due to unknown APPID value or dest address mismatch\n");
+ }
+}
+
static void
parseSVMessage(SVReceiver self, int numbytes)
{
@@ -506,70 +652,34 @@ parseSVMessage(SVReceiver self, int numbytes)
return;
}
- if (DEBUG_SV_SUBSCRIBER) {
- printf("SV_SUBSCRIBER: SV message: ----------------\n");
- printf("SV_SUBSCRIBER: APPID: %u\n", appId);
- printf("SV_SUBSCRIBER: LENGTH: %u\n", length);
- printf("SV_SUBSCRIBER: APDU length: %i\n", apduLength);
- }
-
- /* check if there is a matching subscriber */
-
-#if (CONFIG_MMS_THREADLESS_STACK == 0)
- Semaphore_wait(self->subscriberListLock);
-#endif
-
- SVSubscriber subscriber = NULL;
-
- LinkedList element = LinkedList_getNext(self->subscriberList);
-
- while (element != NULL) {
- SVSubscriber subscriberElem = (SVSubscriber) LinkedList_getData(element);
-
- if (subscriberElem->appId == appId) {
-
- if (self->checkDestAddr) {
- if (memcmp(dstAddr, subscriberElem->ethAddr, 6) == 0) {
- subscriber = subscriberElem;
- break;
- }
- else
- if (DEBUG_SV_SUBSCRIBER)
- printf("SV_SUBSCRIBER: Checking ethernet dest address failed!\n");
- }
- else {
- subscriber = subscriberElem;
- break;
- }
-
- }
-
- element = LinkedList_getNext(element);
- }
+ handleSVApdu(self, appId, buffer + bufPos, apduLength, dstAddr);
+}
-#if (CONFIG_MMS_THREADLESS_STACK == 0)
- Semaphore_post(self->subscriberListLock);
-#endif
+static void
+handleSessionPayloadElement(void* parameter, uint16_t appId, uint8_t* payloadData, int payloadSize)
+{
+ SVReceiver self = (SVReceiver) parameter;
- if (subscriber)
- parseSVPayload(self, subscriber, buffer + bufPos, apduLength);
- else {
- if (DEBUG_SV_SUBSCRIBER)
- printf("SV_SUBSCRIBER: SV message ignored due to unknown APPID value or dest address mismatch\n");
- }
+ handleSVApdu(self, appId, payloadData, payloadSize, NULL);
}
bool
SVReceiver_tick(SVReceiver self)
{
- int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH);
+ if (self->ethSocket) {
+ int packetSize = Ethernet_receivePacket(self->ethSocket, self->buffer, ETH_BUFFER_LENGTH);
- if (packetSize > 0) {
- parseSVMessage(self, packetSize);
- return true;
+ if (packetSize > 0) {
+ parseSVMessage(self, packetSize);
+ return true;
+ }
}
- else
- return false;
+ else if (self->session) {
+ if (RSession_receiveMessage(self->session, handleSessionPayloadElement, (void*) self) == R_SESSION_ERROR_OK)
+ return true;
+ }
+
+ return false;
}
SVSubscriber
diff --git a/src/sampled_values/sv_subscriber.h b/src/sampled_values/sv_subscriber.h
index 5c0b6905..0ec4d5ef 100644
--- a/src/sampled_values/sv_subscriber.h
+++ b/src/sampled_values/sv_subscriber.h
@@ -1,7 +1,7 @@
/*
* sv_subscriber.h
*
- * Copyright 2015-2018 Michael Zillgith
+ * Copyright 2015-2022 Michael Zillgith
*
* This file is part of libIEC61850.
*
@@ -26,6 +26,7 @@
#include "libiec61850_common_api.h"
#include "iec61850_common.h"
+#include "r_session.h"
#include "hal_ethernet.h"
#ifdef __cplusplus
@@ -132,6 +133,16 @@ typedef struct sSVReceiver* SVReceiver;
LIB61850_API SVReceiver
SVReceiver_create(void);
+/**
+ * \brief Create a new R-SV receiver instance.
+ *
+ * \param session the remote session protocol instance
+ *
+ * \return the newly created receiver instance
+ */
+LIB61850_API SVReceiver
+SVReceiver_createRemote(RSession session);
+
/**
* \brief Disable check for destination address of the received SV messages
*
@@ -228,7 +239,7 @@ SVReceiver_destroy(SVReceiver self);
* Functions for non-threaded operation
***************************************/
-LIB61850_API EthernetSocket
+LIB61850_API bool
SVReceiver_startThreadless(SVReceiver self);
LIB61850_API void