RYTEC
/
libiec61850
mirror of https://github.com/mz-automation/libiec61850.git
6
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

- code format updates

Browse Source
pull/521/head
Michael Zillgith 1 year ago
parent 5d7dd6160b
commit 7276b98c3c
2 changed files with 272 additions and 146 deletions
Show Stats Download Patch File Download Diff File

355
src/r_session/r_session.c
Unescape Escape View File

@ -41,10 +41,10 @@
#define DEBUG_PRINTF(...) #define DEBUG_PRINTF(...)
#endif #endif
typedef struct sRSessionKeyMaterial* RSessionKeyMaterial; typedef struct sRSessionKeyMaterial* RSessionKeyMaterial;
struct sRSessionKeyMaterial { struct sRSessionKeyMaterial
{
uint32_t keyId; uint32_t keyId;
RSecurityAlgorithm secAlgo; RSecurityAlgorithm secAlgo;
RSignatureAlgorithm sigAlgo; RSignatureAlgorithm sigAlgo;
@ -52,7 +52,8 @@ struct sRSessionKeyMaterial {
int keyLength; int keyLength;
}; };
struct sRSession { struct sRSession
{
uint32_t spduNumber; uint32_t spduNumber;
int protocolVersion; /* default is 2 */ int protocolVersion; /* default is 2 */
@ -94,7 +95,8 @@ printBuffer(uint8_t* buffer, int bufSize)
{ {
int i; int i;
for (i = 0; i < bufSize; i++) { for (i = 0; i < bufSize; i++)
{
printf("%02x ", buffer[i]); printf("%02x ", buffer[i]);
if ((((i + 1) % 16) == 0) || (i + 1 == bufSize)) if ((((i + 1) % 16) == 0) || (i + 1 == bufSize))
printf(" (%i)\n", i + 1); printf(" (%i)\n", i + 1);
@ -107,7 +109,8 @@ RSessionKeyMaterial_create(uint32_t keyId, uint8_t* key, int keyLength, RSecurit
{ {
RSessionKeyMaterial self = (RSessionKeyMaterial) GLOBAL_CALLOC(1, sizeof(struct sRSessionKeyMaterial)); RSessionKeyMaterial self = (RSessionKeyMaterial) GLOBAL_CALLOC(1, sizeof(struct sRSessionKeyMaterial));
if (self) { if (self)
{
self->keyId = keyId; self->keyId = keyId;
self->keyLength = keyLength; self->keyLength = keyLength;
self->secAlgo = secAlgo; self->secAlgo = secAlgo;
@ -115,10 +118,12 @@ RSessionKeyMaterial_create(uint32_t keyId, uint8_t* key, int keyLength, RSecurit
self->key = (uint8_t*) GLOBAL_MALLOC(keyLength); self->key = (uint8_t*) GLOBAL_MALLOC(keyLength);
if (self->key) { if (self->key)
{
memcpy(self->key, key, keyLength); memcpy(self->key, key, keyLength);
} }
else { else
{
GLOBAL_FREEMEM(self); GLOBAL_FREEMEM(self);
self = NULL; self = NULL;
} }
@ -130,7 +135,8 @@ RSessionKeyMaterial_create(uint32_t keyId, uint8_t* key, int keyLength, RSecurit
void void
RSessionKeyMaterial_destroy(RSessionKeyMaterial self) RSessionKeyMaterial_destroy(RSessionKeyMaterial self)
{ {
if (self) { if (self)
{
GLOBAL_FREEMEM(self->key); GLOBAL_FREEMEM(self->key);
GLOBAL_FREEMEM(self); GLOBAL_FREEMEM(self);
} }
@ -141,11 +147,12 @@ RSession_create()
{ {
UdpSocket udpSocket = UdpSocket_create(); UdpSocket udpSocket = UdpSocket_create();
if (udpSocket) { if (udpSocket)
{
RSession self = (RSession) GLOBAL_CALLOC(1, sizeof(struct sRSession)); RSession self = (RSession) GLOBAL_CALLOC(1, sizeof(struct sRSession));
if (self) { if (self)
{
self->socket = udpSocket; self->socket = udpSocket;
self->socketLock = Semaphore_create(1); self->socketLock = Semaphore_create(1);
@ -178,7 +185,8 @@ RSession_setSecurity(RSession self, RSecurityAlgorithm secAlgo, RSignatureAlgori
RSessionError RSessionError
RSession_setLocalAddress(RSession self, const char* localAddress, int localPort) RSession_setLocalAddress(RSession self, const char* localAddress, int localPort)
{ {
if (self->localAddress) { if (self->localAddress)
{
GLOBAL_FREEMEM(self->localAddress); GLOBAL_FREEMEM(self->localAddress);
self->localAddress = NULL; self->localAddress = NULL;
} }
@ -205,8 +213,10 @@ RSession_setMulticastTtl(RSession self, int ttl)
{ {
RSessionError err = R_SESSION_ERROR_OK; RSessionError err = R_SESSION_ERROR_OK;
if (self->socket) { if (self->socket)
if (!UdpSocket_setMulticastTtl(self->socket, ttl)) { {
if (!UdpSocket_setMulticastTtl(self->socket, ttl))
{
err = R_SESSION_ERROR_SET_FAILED; err = R_SESSION_ERROR_SET_FAILED;
} }
} }
@ -220,7 +230,8 @@ RSession_setMulticastTtl(RSession self, int ttl)
RSessionError RSessionError
RSession_setRemoteAddress(RSession self, const char* remoteAddress, int remotePort) RSession_setRemoteAddress(RSession self, const char* remoteAddress, int remotePort)
{ {
if (self->remoteAddress) { if (self->remoteAddress)
{
GLOBAL_FREEMEM(self->remoteAddress); GLOBAL_FREEMEM(self->remoteAddress);
self->remoteAddress = NULL; self->remoteAddress = NULL;
} }
@ -234,8 +245,8 @@ RSession_setRemoteAddress(RSession self, const char* remoteAddress, int remotePo
RSessionError RSessionError
RSession_start(RSession self) RSession_start(RSession self)
{ {
if (self->socket) { if (self->socket)
{
bool success = false; bool success = false;
if (self->localAddress) if (self->localAddress)
@ -248,7 +259,8 @@ RSession_start(RSession self)
else else
return R_SESSION_ERROR_SET_FAILED; return R_SESSION_ERROR_SET_FAILED;
} }
else { else
{
return R_SESSION_ERROR_NO_SOCKET; return R_SESSION_ERROR_NO_SOCKET;
} }
} }
@ -258,7 +270,8 @@ RSession_stop(RSession self)
{ {
Semaphore_wait(self->socketLock); Semaphore_wait(self->socketLock);
if (self->socket) { if (self->socket)
{
Socket_destroy((Socket)self->socket); Socket_destroy((Socket)self->socket);
self->socket = NULL; self->socket = NULL;
@ -266,7 +279,8 @@ RSession_stop(RSession self)
return R_SESSION_ERROR_OK; return R_SESSION_ERROR_OK;
} }
else { else
{
Semaphore_post(self->socketLock); Semaphore_post(self->socketLock);
return R_SESSION_ERROR_NO_SOCKET; return R_SESSION_ERROR_NO_SOCKET;
@ -374,19 +388,20 @@ decodeInt16FixedSize(int16_t* outValue, uint8_t* buffer, int bufPos)
static bool static bool
lookupKey(RSession self, uint32_t keyId, uint8_t** key, int* keySize, RSecurityAlgorithm* secAlgo, RSignatureAlgorithm* sigAlgo) lookupKey(RSession self, uint32_t keyId, uint8_t** key, int* keySize, RSecurityAlgorithm* secAlgo, RSignatureAlgorithm* sigAlgo)
{ {
if (keyId == 0) { if (keyId == 0)
{
DEBUG_PRINTF("Invalid key ID"); DEBUG_PRINTF("Invalid key ID");
return false; return false;
} }
if (self->currentKeyId != keyId) { if (self->currentKeyId != keyId)
{
if (RSession_setActiveKey(self, keyId) != R_SESSION_ERROR_OK) { if (RSession_setActiveKey(self, keyId) != R_SESSION_ERROR_OK)
{
DEBUG_PRINTF("unknown key-ID %u", keyId); DEBUG_PRINTF("unknown key-ID %u", keyId);
/* TODO audit-log? */ /* TODO audit-log? */
return false; return false;
} }
} }
*key = self->currentKey; *key = self->currentKey;
@ -414,29 +429,34 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
/* SI */ /* SI */
uint8_t payloadType = buffer[bufPos++]; uint8_t payloadType = buffer[bufPos++];
if ((payloadType == 0xa2) || (payloadType == 0xa1) || (payloadType == 0xa0)) { if ((payloadType == 0xa2) || (payloadType == 0xa1) || (payloadType == 0xa0))
{
/* known payload */
} }
else { else
{
DEBUG_PRINTF("unknown payload type %i", payloadType); DEBUG_PRINTF("unknown payload type %i", payloadType);
goto exit_error; goto exit_error;
} }
int sessionHeaderLength = buffer[bufPos++]; int sessionHeaderLength = buffer[bufPos++];
if ((msgSize < (sessionHeaderLength + 4)) || (sessionHeaderLength < 10)) { if ((msgSize < (sessionHeaderLength + 4)) || (sessionHeaderLength < 10))
{
DEBUG_PRINTF("message too small"); DEBUG_PRINTF("message too small");
goto exit_error; goto exit_error;
} }
if (buffer[bufPos++] != 0x80) { if (buffer[bufPos++] != 0x80)
{
DEBUG_PRINTF("protocol error"); DEBUG_PRINTF("protocol error");
goto exit_error; goto exit_error;
} }
int commonSessionHeaderLength = buffer[bufPos++]; int commonSessionHeaderLength = buffer[bufPos++];
if (commonSessionHeaderLength < 10) { if (commonSessionHeaderLength < 10)
{
DEBUG_PRINTF("common session header too small"); DEBUG_PRINTF("common session header too small");
goto exit_error; goto exit_error;
} }
@ -453,7 +473,8 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
int16_t protocolVersion = 0; int16_t protocolVersion = 0;
bufPos = decodeInt16FixedSize(&protocolVersion, buffer, bufPos); bufPos = decodeInt16FixedSize(&protocolVersion, buffer, bufPos);
if (protocolVersion == 1) { if (protocolVersion == 1)
{
/* parse version 1 common header parts */ /* parse version 1 common header parts */
/* TimeOfCurrentKey */ /* TimeOfCurrentKey */
@ -469,12 +490,14 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
RSignatureAlgorithm sigAlgo = (RSignatureAlgorithm) buffer[bufPos++]; RSignatureAlgorithm sigAlgo = (RSignatureAlgorithm) buffer[bufPos++];
/* Check if algorithms match the configured algorithms */ /* Check if algorithms match the configured algorithms */
if (secAlgo != self->secAlgo) { if (secAlgo != self->secAlgo)
{
DEBUG_PRINTF("encryption algorithm doesn't match with configuration"); DEBUG_PRINTF("encryption algorithm doesn't match with configuration");
goto exit_error; goto exit_error;
} }
if (sigAlgo != self->sigAlgo) { if (sigAlgo != self->sigAlgo)
{
DEBUG_PRINTF("signature algorithm(%i) doesn't match with configuration(%i)", sigAlgo, self->sigAlgo); DEBUG_PRINTF("signature algorithm(%i) doesn't match with configuration(%i)", sigAlgo, self->sigAlgo);
goto exit_error; goto exit_error;
} }
@ -488,14 +511,18 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
uint8_t* key = NULL; uint8_t* key = NULL;
int keySize = 0; int keySize = 0;
if (sigAlgo != R_SESSION_SIG_ALGO_NONE) { if (sigAlgo != R_SESSION_SIG_ALGO_NONE)
if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false) { {
if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false)
{
DEBUG_PRINTF("ERROR - key not found"); DEBUG_PRINTF("ERROR - key not found");
goto exit_error; goto exit_error;
} }
} }
else if (secAlgo != R_SESSION_SEC_ALGO_NONE) { else if (secAlgo != R_SESSION_SEC_ALGO_NONE)
if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false) { {
if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false)
{
DEBUG_PRINTF("ERROR - key not found"); DEBUG_PRINTF("ERROR - key not found");
goto exit_error; goto exit_error;
} }
@ -508,7 +535,8 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
/* parse payload elements */ /* parse payload elements */
uint32_t payloadEnd = bufPos + payloadLength; uint32_t payloadEnd = bufPos + payloadLength;
if (payloadEnd > (uint32_t)msgSize) { if (payloadEnd > (uint32_t)msgSize)
{
DEBUG_PRINTF("ERROR - payload size field invalid"); DEBUG_PRINTF("ERROR - payload size field invalid");
goto exit_error; goto exit_error;
} }
@ -516,42 +544,54 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
uint8_t signatureBuffer[128]; uint8_t signatureBuffer[128];
/* Check signature */ /* Check signature */
if (sigAlgo != R_SESSION_SIG_ALGO_NONE) { if (sigAlgo != R_SESSION_SIG_ALGO_NONE)
if (key) { {
if (RSessionCrypto_createHMAC(buffer, payloadEnd, key, keySize, signatureBuffer, 32)) { if (key)
if (buffer[payloadEnd] != 0x85) { {
if (RSessionCrypto_createHMAC(buffer, payloadEnd, key, keySize, signatureBuffer, 32))
{
if (buffer[payloadEnd] != 0x85)
{
DEBUG_PRINTF("ERROR - no signature found"); DEBUG_PRINTF("ERROR - no signature found");
goto exit_error; goto exit_error;
} }
else { else
if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128) { {
if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128)
{
/* TODO is payloadEnd +2 correct? */ /* TODO is payloadEnd +2 correct? */
if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 16)) { if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 16))
{
DEBUG_PRINTF("ERROR - signature not matching!"); DEBUG_PRINTF("ERROR - signature not matching!");
goto exit_error; goto exit_error;
} }
} }
else if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256) { else if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256)
{
/* TODO is payloadEnd +2 correct? */ /* TODO is payloadEnd +2 correct? */
if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 32)) { if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 32))
{
DEBUG_PRINTF("ERROR - signature not matching!"); DEBUG_PRINTF("ERROR - signature not matching!");
goto exit_error; goto exit_error;
} }
} }
} }
} }
else { else
{
DEBUG_PRINTF("ERROR - failed to calculate HMAC!"); DEBUG_PRINTF("ERROR - failed to calculate HMAC!");
goto exit_error; goto exit_error;
} }
} }
else { else
{
DEBUG_PRINTF("ERROR - key not found!"); DEBUG_PRINTF("ERROR - key not found!");
goto exit_error; goto exit_error;
} }
} }
while ((uint32_t)bufPos < payloadEnd) { while ((uint32_t)bufPos < payloadEnd)
{
int payloadElementType = buffer[bufPos++]; int payloadElementType = buffer[bufPos++];
bool simulation; bool simulation;
@ -570,10 +610,11 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
DEBUG_PRINTF("ASDU %02x sim: %i APPID: %04x length: %i", payloadElementType, simulation, appId, asduLength); DEBUG_PRINTF("ASDU %02x sim: %i APPID: %04x length: %i", payloadElementType, simulation, appId, asduLength);
if (payloadElementType == 0x81 || if (payloadElementType == 0x81 ||
payloadElementType == 0x82) { payloadElementType == 0x82)
{
/* user payload */ /* user payload */
//TODO copy ASDU payload to ??? /* TODO copy ASDU payload to ??? */
handler(handlerParam, appId, buffer + bufPos, asduLength); handler(handlerParam, appId, buffer + bufPos, asduLength);
} }
else { else {
@ -582,9 +623,9 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
bufPos += asduLength; bufPos += asduLength;
} }
} }
else if (protocolVersion == 2) { else if (protocolVersion == 2)
{
/* parse version 2 common header parts */ /* parse version 2 common header parts */
/* TimeOfCurrentKey */ /* TimeOfCurrentKey */
@ -605,15 +646,18 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
uint32_t keyId; uint32_t keyId;
bufPos = decodeUInt32FixedSize(&keyId, buffer, bufPos); bufPos = decodeUInt32FixedSize(&keyId, buffer, bufPos);
if (keyId != 0) { if (keyId != 0)
{
/* get key material associated with the key ID */ /* get key material associated with the key ID */
if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false) { if (lookupKey(self, keyId, &key, &keySize, &secAlgo, &sigAlgo) == false)
{
DEBUG_PRINTF("ERROR - key not found"); DEBUG_PRINTF("ERROR - key not found");
goto exit_error; goto exit_error;
} }
} }
else { else
{
DEBUG_PRINTF("ERROR - invalid key ID"); DEBUG_PRINTF("ERROR - invalid key ID");
goto exit_error; goto exit_error;
} }
@ -627,7 +671,8 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
DEBUG_PRINTF("IV: size = %i\n", ivLen); DEBUG_PRINTF("IV: size = %i\n", ivLen);
if (ivLen > 0) { if (ivLen > 0)
{
iv = buffer + bufPos; iv = buffer + bufPos;
bufPos += ivLen; bufPos += ivLen;
} }
@ -642,7 +687,8 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
/* parse payload elements */ /* parse payload elements */
uint32_t payloadEnd = bufPos + payloadLength; uint32_t payloadEnd = bufPos + payloadLength;
if (payloadEnd > (uint32_t)msgSize) { if (payloadEnd > (uint32_t)msgSize)
{
DEBUG_PRINTF("ERROR - payload size field invalid"); DEBUG_PRINTF("ERROR - payload size field invalid");
goto exit_error; goto exit_error;
} }
@ -650,71 +696,88 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
uint8_t signatureBuffer[128]; uint8_t signatureBuffer[128];
/* Check signature */ /* Check signature */
if (sigAlgo != R_SESSION_SIG_ALGO_NONE) { if (sigAlgo != R_SESSION_SIG_ALGO_NONE)
if (key) { {
if (RSessionCrypto_createHMAC(buffer, payloadEnd, key, keySize, signatureBuffer, 32)) { if (key)
if (buffer[payloadEnd] != 0x85) { {
if (RSessionCrypto_createHMAC(buffer, payloadEnd, key, keySize, signatureBuffer, 32))
{
if (buffer[payloadEnd] != 0x85)
{
DEBUG_PRINTF("ERROR - no signature found"); DEBUG_PRINTF("ERROR - no signature found");
goto exit_error; goto exit_error;
} }
else { else
if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128) { {
if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128)
{
/* TODO is payloadEnd +2 correct? */ /* TODO is payloadEnd +2 correct? */
if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 16)) { if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 16))
{
DEBUG_PRINTF("ERROR - signature not matching!"); DEBUG_PRINTF("ERROR - signature not matching!");
goto exit_error; goto exit_error;
} }
} }
else if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256) { else if (sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256)
{
/* TODO is payloadEnd +2 correct? */ /* TODO is payloadEnd +2 correct? */
if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 32)) { if (memcmp(signatureBuffer, buffer + payloadEnd + 1, 32))
{
DEBUG_PRINTF("ERROR - signature not matching!"); DEBUG_PRINTF("ERROR - signature not matching!");
goto exit_error; goto exit_error;
} }
} }
} }
} }
else { else
{
DEBUG_PRINTF("ERROR - failed to calculate HMAC!"); DEBUG_PRINTF("ERROR - failed to calculate HMAC!");
goto exit_error; goto exit_error;
} }
} }
else { else
{
DEBUG_PRINTF("ERROR - key not found!"); DEBUG_PRINTF("ERROR - key not found!");
goto exit_error; goto exit_error;
} }
} }
/* Check signature and decrypt application layer */ /* Check signature and decrypt application layer */
if (secAlgo != R_SESSION_SEC_ALGO_NONE) { if (secAlgo != R_SESSION_SEC_ALGO_NONE)
{
/* Check for HMAC */ /* Check for HMAC */
if (payloadEnd + 18 <= (uint32_t)msgSize) { if (payloadEnd + 18 <= (uint32_t)msgSize)
{
if (self->payloadBuffer == NULL) if (self->payloadBuffer == NULL)
self->payloadBuffer = (uint8_t*)GLOBAL_MALLOC(65000); self->payloadBuffer = (uint8_t*)GLOBAL_MALLOC(65000);
if (self->payloadBuffer) { if (self->payloadBuffer)
//TODO check MMAC tag {
uint8_t* mac = buffer + payloadEnd + 2; uint8_t* mac = buffer + payloadEnd + 2;
int macSize = buffer[payloadEnd + 1]; int macSize = buffer[payloadEnd + 1];
int payloadSize = payloadEnd - payloadStartPos; int payloadSize = payloadEnd - payloadStartPos;
if (RSessionCrypto_gcmAuthAndDecrypt(key, keySize, iv, ivLen, buffer, payloadStartPos, payloadStart, payloadSize, self->payloadBuffer, mac, macSize)) { if (RSessionCrypto_gcmAuthAndDecrypt(key, keySize, iv, ivLen, buffer, payloadStartPos, payloadStart, payloadSize, self->payloadBuffer, mac, macSize))
{
memcpy(buffer + bufPos, self->payloadBuffer, payloadSize); memcpy(buffer + bufPos, self->payloadBuffer, payloadSize);
} }
else { else
{
DEBUG_PRINTF("ERROR - auth and decrypt failed!"); DEBUG_PRINTF("ERROR - auth and decrypt failed!");
goto exit_error; goto exit_error;
} }
} }
} }
else { else
{
DEBUG_PRINTF("ERROR - sec algo - message too small!"); DEBUG_PRINTF("ERROR - sec algo - message too small!");
goto exit_error; goto exit_error;
} }
} }
while ((uint32_t)bufPos < payloadEnd) { while ((uint32_t)bufPos < payloadEnd)
{
int payloadElementType = buffer[bufPos++]; int payloadElementType = buffer[bufPos++];
bool simulation; bool simulation;
@ -733,20 +796,23 @@ parseSessionMessage(RSession self, uint8_t* buffer, int msgSize, RSessionPayload
DEBUG_PRINTF("ASDU %02x sim: %i APPID: %04x length: %i", payloadElementType, simulation, appId, asduLength); DEBUG_PRINTF("ASDU %02x sim: %i APPID: %04x length: %i", payloadElementType, simulation, appId, asduLength);
if (payloadElementType == 0x81 || if (payloadElementType == 0x81 ||
payloadElementType == 0x82) { payloadElementType == 0x82)
{
/* user payload */ /* user payload */
//TODO copy ASDU payload to ??? //TODO copy ASDU payload to ???
handler(handlerParam, appId, buffer + bufPos, asduLength); handler(handlerParam, appId, buffer + bufPos, asduLength);
} }
else { else
{
DEBUG_PRINTF("unexpected payload type! (expect 81h (GOOSE) or 82h (SV))"); DEBUG_PRINTF("unexpected payload type! (expect 81h (GOOSE) or 82h (SV))");
} }
bufPos += asduLength; bufPos += asduLength;
} }
} }
else { else
{
DEBUG_PRINTF("only protocol version 1 and 2 supported (received version %i)", protocolVersion); DEBUG_PRINTF("only protocol version 1 and 2 supported (received version %i)", protocolVersion);
goto exit_error; goto exit_error;
} }
@ -775,7 +841,8 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
RSessionPayloadElement element = elements; RSessionPayloadElement element = elements;
while (element) { while (element)
{
payloadLength += 6; /* payload type, simulation, APPID, length */ payloadLength += 6; /* payload type, simulation, APPID, length */
payloadLength += element->payloadSize; payloadLength += element->payloadSize;
@ -815,7 +882,8 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
/* TimeToNextKey */ /* TimeToNextKey */
bufPos = encodeInt16FixedSize((int16_t)self->timeToNextKey, buffer, bufPos); bufPos = encodeInt16FixedSize((int16_t)self->timeToNextKey, buffer, bufPos);
if (self->protocolVersion == 1) { if (self->protocolVersion == 1)
{
/* encryption algorithm */ /* encryption algorithm */
buffer[bufPos++] = (uint8_t) self->secAlgo; /* 0 = none */ buffer[bufPos++] = (uint8_t) self->secAlgo; /* 0 = none */
@ -825,7 +893,10 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
/* Key ID */ /* Key ID */
bufPos = encodeUInt32FixedSize(self->currentKeyId, buffer, bufPos); bufPos = encodeUInt32FixedSize(self->currentKeyId, buffer, bufPos);
} }
else { /* protocol version 2 */ else
{
/* protocol version 2 */
/* Key ID */ /* Key ID */
bufPos = encodeUInt32FixedSize(self->currentKeyId, buffer, bufPos); bufPos = encodeUInt32FixedSize(self->currentKeyId, buffer, bufPos);
self->secAlgo = self->currentSecAlgo; self->secAlgo = self->currentSecAlgo;
@ -834,21 +905,24 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
DEBUG_PRINTF("PV: 2 sec-algo: %i sig-algo: %i\n", self->secAlgo, self->sigAlgo); DEBUG_PRINTF("PV: 2 sec-algo: %i sig-algo: %i\n", self->secAlgo, self->sigAlgo);
/* IV */ /* IV */
if (self->secAlgo != R_SESSION_SEC_ALGO_NONE) { if (self->secAlgo != R_SESSION_SEC_ALGO_NONE)
/* TODO get and encode IV (initialization vector) */ {
/* create and encode IV (initialization vector) */
buffer[bufPos++] = 12; buffer[bufPos++] = 12;
iv = buffer + bufPos; iv = buffer + bufPos;
ivSize = 12; ivSize = 12;
if (RSessionCrypto_createRandomData(iv, ivSize) == false) { if (RSessionCrypto_createRandomData(iv, ivSize) == false)
{
DEBUG_PRINTF("ERROR - Failed to create random IV"); DEBUG_PRINTF("ERROR - Failed to create random IV");
} }
bufPos += ivSize; bufPos += ivSize;
} }
else { else
{
buffer[bufPos++] = 0; /* empty initialization vector */ buffer[bufPos++] = 0; /* empty initialization vector */
} }
} }
@ -861,8 +935,8 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
/* encode user payload elements */ /* encode user payload elements */
element = elements; element = elements;
while (element) { while (element)
{
/* payload type ? (according to example in annex G) */ /* payload type ? (according to example in annex G) */
buffer[bufPos++] = element->payloadType; buffer[bufPos++] = element->payloadType;
@ -882,15 +956,15 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
element = element->nextElement; element = element->nextElement;
} }
if (self->sigAlgo != R_SESSION_SIG_ALGO_NONE) { if (self->sigAlgo != R_SESSION_SIG_ALGO_NONE)
{
int signatureCoveredLength = bufPos - startPos; int signatureCoveredLength = bufPos - startPos;
DEBUG_PRINTF("Signature: %i", signatureCoveredLength); DEBUG_PRINTF("Signature: %i", signatureCoveredLength);
/* add signature */ /* add signature */
if (self->sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256) { if (self->sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_256)
{
buffer[bufPos++] = 0x85; buffer[bufPos++] = 0x85;
buffer[bufPos++] = 16; buffer[bufPos++] = 16;
@ -898,7 +972,8 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
bufPos += 32; bufPos += 32;
} }
else if (self->sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128) { else if (self->sigAlgo == R_SESSION_SIG_ALGO_HMAC_SHA256_128)
{
buffer[bufPos++] = 0x85; buffer[bufPos++] = 0x85;
buffer[bufPos++] = 16; buffer[bufPos++] = 16;
//buffer[bufPos++] = 0x20; /* 32 octets */ //buffer[bufPos++] = 0x20; /* 32 octets */
@ -907,14 +982,16 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
bufPos += 16; bufPos += 16;
} }
else { else
{
DEBUG_PRINTF("ERROR - unsupported signature type"); DEBUG_PRINTF("ERROR - unsupported signature type");
} }
} }
int payloadEndPos = bufPos; int payloadEndPos = bufPos;
if (self->secAlgo != R_SESSION_SEC_ALGO_NONE) { if (self->secAlgo != R_SESSION_SEC_ALGO_NONE)
{
/* create signature and encrypt payload */ /* create signature and encrypt payload */
buffer[bufPos++] = 0x85; buffer[bufPos++] = 0x85;
@ -929,10 +1006,12 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
DEBUG_PRINTF("===> encrypt ===="); DEBUG_PRINTF("===> encrypt ====");
if (RSessionCrypto_gcmEncryptAndTag(self->currentKey, self->currentKeySize, iv, ivSize, buffer + startPos, addPartSize, buffer + encryptedPartStartPos, encryptedPartSize, buffer + bufPos, 16) == false) { if (RSessionCrypto_gcmEncryptAndTag(self->currentKey, self->currentKeySize, iv, ivSize, buffer + startPos, addPartSize, buffer + encryptedPartStartPos, encryptedPartSize, buffer + bufPos, 16) == false)
{
DEBUG_PRINTF("ERROR - encryption failed"); DEBUG_PRINTF("ERROR - encryption failed");
} }
else { else
{
bufPos += 16; bufPos += 16;
} }
} }
@ -948,19 +1027,21 @@ encodePacket(RSession self, uint8_t payloadType, uint8_t* buffer, int bufPos, RS
RSessionError RSessionError
RSession_sendMessage(RSession self, RSessionProtocol_SPDU_ID spduId, bool simulation, uint16_t appId, uint8_t* payload, int payloadSize) RSession_sendMessage(RSession self, RSessionProtocol_SPDU_ID spduId, bool simulation, uint16_t appId, uint8_t* payload, int payloadSize)
{ {
if (self->socket == NULL) { if (self->socket == NULL)
{
self->socket = UdpSocket_create(); self->socket = UdpSocket_create();
} }
if (self->sendBuffer == NULL) { if (self->sendBuffer == NULL)
{
self->sendBuffer = (uint8_t*) GLOBAL_MALLOC(self->bufferSize); self->sendBuffer = (uint8_t*) GLOBAL_MALLOC(self->bufferSize);
if (self->sendBuffer == NULL) if (self->sendBuffer == NULL)
return R_SESSION_ERROR_OUT_OF_MEMORY; return R_SESSION_ERROR_OUT_OF_MEMORY;
} }
if (self->socket) { if (self->socket)
{
struct sRSessionPayloadElement element; struct sRSessionPayloadElement element;
element.simulation = simulation; element.simulation = simulation;
@ -976,15 +1057,17 @@ RSession_sendMessage(RSession self, RSessionProtocol_SPDU_ID spduId, bool simula
printBuffer(self->sendBuffer, msgSize); printBuffer(self->sendBuffer, msgSize);
#endif #endif
if (UdpSocket_sendTo(self->socket, self->remoteAddress, self->remotePort, self->sendBuffer, msgSize)) { if (UdpSocket_sendTo(self->socket, self->remoteAddress, self->remotePort, self->sendBuffer, msgSize))
{
return R_SESSION_ERROR_OK; return R_SESSION_ERROR_OK;
} }
else { else
{
return R_SESSION_ERROR_FAILED_TO_SEND; return R_SESSION_ERROR_FAILED_TO_SEND;
} }
} }
else { else
{
return R_SESSION_ERROR_NO_SOCKET; return R_SESSION_ERROR_NO_SOCKET;
} }
} }
@ -992,10 +1075,12 @@ RSession_sendMessage(RSession self, RSessionProtocol_SPDU_ID spduId, bool simula
void void
RSession_setBufferSize(RSession self, uint16_t bufferSize) RSession_setBufferSize(RSession self, uint16_t bufferSize)
{ {
if (bufferSize > 127) { if (bufferSize > 127)
{
self->bufferSize = bufferSize; self->bufferSize = bufferSize;
} }
else { else
{
self->bufferSize = 128; self->bufferSize = 128;
} }
} }
@ -1005,13 +1090,16 @@ RSession_receiveMessage(RSession self, RSessionPayloadElementHandler handler, vo
{ {
Semaphore_wait(self->socketLock); Semaphore_wait(self->socketLock);
if (self->socket) { if (self->socket)
{
char ipAddrBuf[128]; char ipAddrBuf[128];
if (self->sendBuffer == NULL) { if (self->sendBuffer == NULL)
{
self->sendBuffer = (uint8_t*) GLOBAL_MALLOC(self->bufferSize); self->sendBuffer = (uint8_t*) GLOBAL_MALLOC(self->bufferSize);
if (self->sendBuffer == NULL) { if (self->sendBuffer == NULL)
{
Semaphore_post(self->socketLock); Semaphore_post(self->socketLock);
return R_SESSION_ERROR_OUT_OF_MEMORY; return R_SESSION_ERROR_OUT_OF_MEMORY;
@ -1024,17 +1112,19 @@ RSession_receiveMessage(RSession self, RSessionPayloadElementHandler handler, vo
Semaphore_post(self->socketLock); Semaphore_post(self->socketLock);
if (msgSize < 1) { if (msgSize < 1)
{
DEBUG_PRINTF("RESSSION: Failed to receive message"); DEBUG_PRINTF("RESSSION: Failed to receive message");
return R_SESSION_ERROR_FAILED_TO_RECEIVE; return R_SESSION_ERROR_FAILED_TO_RECEIVE;
} }
else { else
{
return parseSessionMessage(self, self->sendBuffer, msgSize, handler, parameter); return parseSessionMessage(self, self->sendBuffer, msgSize, handler, parameter);
} }
} }
else { else
{
Semaphore_post(self->socketLock); Semaphore_post(self->socketLock);
return R_SESSION_ERROR_NO_SOCKET; return R_SESSION_ERROR_NO_SOCKET;
@ -1046,14 +1136,16 @@ RSession_addKey(RSession self, uint32_t keyId, uint8_t* key, int keyLength, RSec
{ {
RSessionKeyMaterial keyMaterial = RSessionKeyMaterial_create(keyId, key, keyLength, secAlgo, sigAlgo); RSessionKeyMaterial keyMaterial = RSessionKeyMaterial_create(keyId, key, keyLength, secAlgo, sigAlgo);
if (keyMaterial) { if (keyMaterial)
{
Semaphore_wait(self->keyListLock); Semaphore_wait(self->keyListLock);
LinkedList_add(self->keyList, keyMaterial); LinkedList_add(self->keyList, keyMaterial);
Semaphore_post(self->keyListLock); Semaphore_post(self->keyListLock);
return R_SESSION_ERROR_OK; return R_SESSION_ERROR_OK;
} }
else { else
{
return R_SESSION_ERROR_OUT_OF_MEMORY; return R_SESSION_ERROR_OUT_OF_MEMORY;
} }
} }
@ -1063,10 +1155,12 @@ getKeyById(RSession self, uint32_t keyId)
{ {
LinkedList keyElem = LinkedList_getNext(self->keyList); LinkedList keyElem = LinkedList_getNext(self->keyList);
while (keyElem) { while (keyElem)
{
RSessionKeyMaterial keyMaterial = (RSessionKeyMaterial) LinkedList_getData(keyElem); RSessionKeyMaterial keyMaterial = (RSessionKeyMaterial) LinkedList_getData(keyElem);
if (keyMaterial->keyId == keyId) { if (keyMaterial->keyId == keyId)
{
return keyMaterial; return keyMaterial;
} }
@ -1085,18 +1179,21 @@ RSession_removeKey(RSession self, uint32_t keyId)
RSessionKeyMaterial keyMaterial = getKeyById(self, keyId); RSessionKeyMaterial keyMaterial = getKeyById(self, keyId);
if (keyMaterial) { if (keyMaterial)
{
LinkedList_remove(self->keyList, keyMaterial); LinkedList_remove(self->keyList, keyMaterial);
RSessionKeyMaterial_destroy(keyMaterial); RSessionKeyMaterial_destroy(keyMaterial);
} }
else { else
{
retVal = R_SESSION_ERROR_INVALID_KEY; retVal = R_SESSION_ERROR_INVALID_KEY;
} }
Semaphore_post(self->keyListLock); Semaphore_post(self->keyListLock);
if (self->currentKeyId == keyId) { if (self->currentKeyId == keyId)
{
/* active key removed! */ /* active key removed! */
self->currentKeyId = 0; self->currentKeyId = 0;
} }
@ -1130,14 +1227,16 @@ RSession_setActiveKey(RSession self, uint32_t keyId)
RSessionKeyMaterial keyMaterial = getKeyById(self, keyId); RSessionKeyMaterial keyMaterial = getKeyById(self, keyId);
if (keyMaterial) { if (keyMaterial)
{
self->currentKeySize = keyMaterial->keyLength; self->currentKeySize = keyMaterial->keyLength;
self->currentKey = keyMaterial->key; self->currentKey = keyMaterial->key;
self->currentKeyId = keyMaterial->keyId; self->currentKeyId = keyMaterial->keyId;
self->currentSecAlgo = keyMaterial->secAlgo; self->currentSecAlgo = keyMaterial->secAlgo;
self->currentSigAlgo = keyMaterial->sigAlgo; self->currentSigAlgo = keyMaterial->sigAlgo;
} }
else { else
{
retVal = R_SESSION_ERROR_INVALID_KEY; retVal = R_SESSION_ERROR_INVALID_KEY;
} }
@ -1149,7 +1248,8 @@ RSession_setActiveKey(RSession self, uint32_t keyId)
void void
RSession_destroy(RSession self) RSession_destroy(RSession self)
{ {
if (self) { if (self)
{
if (self->socket) if (self->socket)
Socket_destroy((Socket)self->socket); Socket_destroy((Socket)self->socket);
@ -1183,4 +1283,3 @@ RSession_getSocket(RSession self)
{ {
return (Socket)self->socket; return (Socket)self->socket;
} }

63
src/r_session/r_session_crypto_mbedtls.c
Unescape Escape View File

@ -23,7 +23,6 @@
* See COPYING file for the complete license text. * See COPYING file for the complete license text.
*/ */
#include "mbedtls/cipher.h" #include "mbedtls/cipher.h"
#include "mbedtls/md.h" #include "mbedtls/md.h"
#include "mbedtls/md_internal.h" #include "mbedtls/md_internal.h"
@ -36,6 +35,10 @@
#include "r_session_crypto.h" #include "r_session_crypto.h"
#ifndef DEBUG_RSESSION_CRYPTO
#define DEBUG_RSESSION_CRYPTO 1
#endif
bool bool
RSessionCrypto_createHMAC(uint8_t* buffer, int bufSize, uint8_t* key, int keySize, uint8_t* hmac, int hmacMaxSize) RSessionCrypto_createHMAC(uint8_t* buffer, int bufSize, uint8_t* key, int keySize, uint8_t* hmac, int hmacMaxSize)
{ {
@ -47,18 +50,27 @@ RSessionCrypto_createHMAC(uint8_t* buffer, int bufSize, uint8_t* key, int keySiz
mbedtls_md_setup(&md_ctx, md_info, 1); mbedtls_md_setup(&md_ctx, md_info, 1);
if (mbedtls_md_hmac_starts(&md_ctx, key, keySize)) { if (mbedtls_md_hmac_starts(&md_ctx, key, keySize))
printf("Error in initializing HMAC\n"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: Error in initializing HMAC\n");
return false; return false;
} }
if (mbedtls_md_hmac_update(&md_ctx, buffer, bufSize)) { if (mbedtls_md_hmac_update(&md_ctx, buffer, bufSize))
printf("Failed to calculate HMAC\n"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: Failed to calculate HMAC\n");
return false; return false;
} }
if (mbedtls_md_hmac_finish(&md_ctx, hmac)) { if (mbedtls_md_hmac_finish(&md_ctx, hmac))
printf("Failed to finish HMAC\n"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: Failed to finish HMAC\n");
return false; return false;
} }
@ -74,14 +86,20 @@ RSessionCrypto_gcmEncryptAndTag(uint8_t* key, int keySize, uint8_t* iv, int ivSi
mbedtls_gcm_init(&gcmCtx); mbedtls_gcm_init(&gcmCtx);
if (mbedtls_gcm_setkey(&gcmCtx, MBEDTLS_CIPHER_ID_AES , (const unsigned char*) key, keySize * 8)) { if (mbedtls_gcm_setkey(&gcmCtx, MBEDTLS_CIPHER_ID_AES , (const unsigned char*) key, keySize * 8))
printf("AES-GCM: Failed to set key\n"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: AES-GCM: Failed to set key\n");
mbedtls_gcm_free(&gcmCtx); mbedtls_gcm_free(&gcmCtx);
return false; 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)) { 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"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: AES-GCM: Failed to authenticate/encrypt data\n");
mbedtls_gcm_free(&gcmCtx); mbedtls_gcm_free(&gcmCtx);
return false; return false;
} }
@ -98,14 +116,20 @@ RSessionCrypto_gcmAuthAndDecrypt(uint8_t* key, int keySize, uint8_t* iv, int ivS
mbedtls_gcm_init(&gcmCtx); mbedtls_gcm_init(&gcmCtx);
if (mbedtls_gcm_setkey(&gcmCtx, MBEDTLS_CIPHER_ID_AES , (const unsigned char*) key, keySize * 8)) { if (mbedtls_gcm_setkey(&gcmCtx, MBEDTLS_CIPHER_ID_AES , (const unsigned char*) key, keySize * 8))
printf("AES-GCM: Failed to set key\n"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: AES-GCM: Failed to set key\n");
mbedtls_gcm_free(&gcmCtx); mbedtls_gcm_free(&gcmCtx);
return false; 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)) { 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"); {
if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: AES-GCM: Failed to authentication and decrypt!\n");
mbedtls_gcm_free(&gcmCtx); mbedtls_gcm_free(&gcmCtx);
return false; return false;
} }
@ -128,16 +152,19 @@ RSessionCrypto_createRandomData(uint8_t* data, int dataSize)
if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy, if( ( ret = mbedtls_ctr_drbg_seed( &ctr_drbg, mbedtls_entropy_func, &entropy,
NULL, 0) ) != 0 ) NULL, 0) ) != 0 )
{ {
printf( " failed\n ! mbedtls_ctr_drbg_init returned -0x%04x\n", -ret ); if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: failed! mbedtls_ctr_drbg_init returned -0x%04x\n", -ret);
return false; return false;
} }
if( ( ret = mbedtls_ctr_drbg_random( &ctr_drbg, data, dataSize ) ) != 0 ) if( ( ret = mbedtls_ctr_drbg_random( &ctr_drbg, data, dataSize ) ) != 0 )
{ {
printf( " failed\n ! mbedtls_ctr_drbg_random returned -0x%04x\n", -ret ); if (DEBUG_RSESSION_CRYPTO)
printf("RSESSION_CRYPTO: failed! mbedtls_ctr_drbg_random returned -0x%04x\n", -ret);
return false; return false;
} }
return true; return true;
} }

Write Preview
Loading…
Cancel
Save