RYTEC
/
libiec61850
mirror of https://github.com/mz-automation/libiec61850.git
6
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
You cannot select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
v1.6_develop_528
v1.6_develop
v1.6
v1.6_develop_329_GOOSE_signatures
v1.6_develop_471
v1.5
v1.6_develop_rgoose_sntp
v1.6_develop_417_rbac2
v1.5_develop_399
v1.6_develop_387
v1.5_develop_merging
v1.4
v1.3
v1.2
v1.1
v1.1.1
v1.0.1
1.1.2
v1.0.0
v1.1
v1.2.0
v1.2.1
v1.2.2
v1.3.0
v1.3.1
v1.3.2
v1.3.3
v1.4.0
v1.4.1
v1.4.2
v1.4.2.1
v1.5.0
v1.5.1
v1.5.3
v1.6.0
v1.6.1
Branches Tags
${ item.name }
Create tag ${ searchTerm }
Create branch ${ searchTerm }
from 'a7dc9148c2'
${ noResults }
libiec61850/src/r_session/r_session.c

1184 lines
32 KiB
C
Raw Blame History

/*
* 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 <http://www.gnu.org/licenses/>.
*
* 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"
#ifndef DEBUG_RSESSION
#define DEBUG_RSESSION 0
#endif
#if (DEBUG_RSESSION == 1)
#include <stdio.h>
#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 /* DEBUG_RSESSION */
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;
}
/* 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 > (uint32_t)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 ((uint32_t)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 > (uint32_t)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 <= (uint32_t)msgSize) {
if (self->payloadBuffer == NULL)
self->payloadBuffer = (uint8_t*)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 ((uint32_t)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) {
DEBUG_PRINTF("RESSSION: Failed to receive message");
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)
{
RSessionError retVal = R_SESSION_ERROR_OK;
Semaphore_wait(self->keyListLock);
RSessionKeyMaterial keyMaterial = getKeyById(self, keyId);
if (keyMaterial) {
LinkedList_remove(self->keyList, keyMaterial);
RSessionKeyMaterial_destroy(keyMaterial);
}
else {
retVal = R_SESSION_ERROR_INVALID_KEY;
}
Semaphore_post(self->keyListLock);
if (self->currentKeyId == keyId) {
/* active key removed! */
self->currentKeyId = 0;
}
return retVal;
}
void
RSession_removeAllKeys(RSession self)
{
Semaphore_wait(self->keyListLock);
LinkedList_destroyDeep(self->keyList, (LinkedListValueDeleteFunction)RSessionKeyMaterial_destroy);
self->keyList = LinkedList_create();
self->currentKeyId = 0;
Semaphore_post(self->keyListLock);
}
/**
* \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;
}
Reference in New Issue View Git Blame Copy Permalink