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