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 34366ce284
commit c5ddf0b71f
2 changed files with 85 additions and 50 deletions
Show Stats Download Patch File Download Diff File

132
src/mms/iso_cotp/cotp.c
Unescape Escape View File

@ -67,8 +67,8 @@ writeOptions(CotpConnection* self)
uint8_t* buffer = self->writeBuffer->buffer; uint8_t* buffer = self->writeBuffer->buffer;
int bufPos = self->writeBuffer->size; int bufPos = self->writeBuffer->size;
if (self->options.tpduSize != 0) { if (self->options.tpduSize != 0)
{
if (DEBUG_COTP) if (DEBUG_COTP)
printf("COTP: send TPDU size: %i\n", CotpConnection_getTpduSize(self)); printf("COTP: send TPDU size: %i\n", CotpConnection_getTpduSize(self));
@ -77,7 +77,8 @@ writeOptions(CotpConnection* self)
buffer[bufPos++] = self->options.tpduSize; buffer[bufPos++] = self->options.tpduSize;
} }
if (self->options.tSelDst.size != 0) { if (self->options.tSelDst.size != 0)
{
buffer[bufPos++] = 0xc2; buffer[bufPos++] = 0xc2;
buffer[bufPos++] = (uint8_t) self->options.tSelDst.size; buffer[bufPos++] = (uint8_t) self->options.tSelDst.size;
@ -86,7 +87,8 @@ writeOptions(CotpConnection* self)
buffer[bufPos++] = (uint8_t) self->options.tSelDst.value[i]; buffer[bufPos++] = (uint8_t) self->options.tSelDst.value[i];
} }
if (self->options.tSelSrc.size != 0) { if (self->options.tSelSrc.size != 0)
{
buffer[bufPos++] = 0xc1; buffer[bufPos++] = 0xc1;
buffer[bufPos++] = (uint8_t) self->options.tSelSrc.size; buffer[bufPos++] = (uint8_t) self->options.tSelSrc.size;
@ -177,28 +179,32 @@ writeToSocket(CotpConnection* self, uint8_t* buf, int size)
static bool static bool
flushBuffer(CotpConnection* self) flushBuffer(CotpConnection* self)
{ {
if (self->socketExtensionBufferFill > 0) { if (self->socketExtensionBufferFill > 0)
{
int sentBytes = writeToSocket(self, self->socketExtensionBuffer, self->socketExtensionBufferFill); int sentBytes = writeToSocket(self, self->socketExtensionBuffer, self->socketExtensionBufferFill);
if (sentBytes > 0) { if (sentBytes > 0)
{
if (sentBytes != self->socketExtensionBufferFill) { if (sentBytes != self->socketExtensionBufferFill)
{
int target = 0; int target = 0;
int i; int i;
uint8_t* buf = self->socketExtensionBuffer; uint8_t* buf = self->socketExtensionBuffer;
for (i = sentBytes; i < self->socketExtensionBufferFill; i++) { for (i = sentBytes; i < self->socketExtensionBufferFill; i++)
{
buf[target++] = buf[i]; buf[target++] = buf[i];
} }
self->socketExtensionBufferFill = self->socketExtensionBufferFill - sentBytes; self->socketExtensionBufferFill = self->socketExtensionBufferFill - sentBytes;
} }
else { else
{
self->socketExtensionBufferFill = 0; self->socketExtensionBufferFill = 0;
} }
} }
else if (sentBytes == -1) { else if (sentBytes == -1)
{
return false; return false;
} }
} }
@ -214,35 +220,40 @@ sendBuffer(CotpConnection* self)
bool retVal = false; bool retVal = false;
if (flushBuffer(self) == false) { if (flushBuffer(self) == false)
{
goto exit_function; goto exit_function;
} }
int sentBytes = 0; int sentBytes = 0;
if (self->socketExtensionBufferFill == 0) { if (self->socketExtensionBufferFill == 0)
{
sentBytes = writeToSocket(self, buffer, remainingSize); sentBytes = writeToSocket(self, buffer, remainingSize);
} }
if (sentBytes == -1) if (sentBytes == -1)
goto exit_function; goto exit_function;
if (sentBytes != remainingSize) { if (sentBytes != remainingSize)
{
/* write additional data to extension buffer */ /* write additional data to extension buffer */
if (self->socketExtensionBuffer) { if (self->socketExtensionBuffer)
{
uint8_t* extBuf = self->socketExtensionBuffer; uint8_t* extBuf = self->socketExtensionBuffer;
int extCurrentPos = self->socketExtensionBufferFill; int extCurrentPos = self->socketExtensionBufferFill;
int bytesNotSent = remainingSize - sentBytes; int bytesNotSent = remainingSize - sentBytes;
int i; int i;
for (i = 0; i < bytesNotSent; i++) { for (i = 0; i < bytesNotSent; i++)
{
extBuf[i + extCurrentPos] = buffer[sentBytes + i]; extBuf[i + extCurrentPos] = buffer[sentBytes + i];
} }
self->socketExtensionBufferFill = extCurrentPos + bytesNotSent; self->socketExtensionBufferFill = extCurrentPos + bytesNotSent;
} }
else { else
{
goto exit_function; goto exit_function;
} }
} }
@ -264,7 +275,9 @@ CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload)
int fragmentPayloadSize = CotpConnection_getTpduSize(self) - COTP_DATA_HEADER_SIZE; int fragmentPayloadSize = CotpConnection_getTpduSize(self) - COTP_DATA_HEADER_SIZE;
if (payload->length > fragmentPayloadSize) { /* Check if segmentation is required? */ if (payload->length > fragmentPayloadSize)
{
/* Check if segmentation is required? */
fragments = payload->length / fragmentPayloadSize; fragments = payload->length / fragmentPayloadSize;
if ((payload->length % fragmentPayloadSize) != 0) if ((payload->length % fragmentPayloadSize) != 0)
@ -275,15 +288,18 @@ CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload)
int totalSize = (fragments * (COTP_DATA_HEADER_SIZE + 4)) + payload->length; int totalSize = (fragments * (COTP_DATA_HEADER_SIZE + 4)) + payload->length;
/* try to flush extension buffer */ /* try to flush extension buffer */
if (flushBuffer(self) == false) { if (flushBuffer(self) == false)
{
return COTP_ERROR; return COTP_ERROR;
} }
/* check if totalSize will fit in extension buffer */ /* check if totalSize will fit in extension buffer */
if (self->socketExtensionBuffer) { if (self->socketExtensionBuffer)
{
int freeExtBufSize = self->socketExtensionBufferSize - self->socketExtensionBufferFill; int freeExtBufSize = self->socketExtensionBufferSize - self->socketExtensionBufferFill;
if (freeExtBufSize < totalSize) { if (freeExtBufSize < totalSize)
{
return COTP_ERROR; return COTP_ERROR;
} }
} }
@ -300,12 +316,15 @@ CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload)
uint8_t* buffer = self->writeBuffer->buffer; uint8_t* buffer = self->writeBuffer->buffer;
while (fragments > 0) { while (fragments > 0)
if (fragments > 1) { {
if (fragments > 1)
{
currentLimit = currentBufPos + fragmentPayloadSize; currentLimit = currentBufPos + fragmentPayloadSize;
lastUnit = 0; lastUnit = 0;
} }
else { else
{
currentLimit = payload->length; currentLimit = payload->length;
lastUnit = 1; lastUnit = 1;
} }
@ -317,9 +336,10 @@ CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload)
int bufPos = 7; int bufPos = 7;
int i; int i;
for (i = currentBufPos; i < currentLimit; i++) { for (i = currentBufPos; i < currentLimit; i++)
{
if (currentChainIndex >= currentChain->partLength) { if (currentChainIndex >= currentChain->partLength)
{
currentChain = currentChain->nextPart; currentChain = currentChain->nextPart;
if (DEBUG_COTP) if (DEBUG_COTP)
printf("COTP: nextBufferPart: len:%i partLen:%i\n", currentChain->length, currentChain->partLength); printf("COTP: nextBufferPart: len:%i partLen:%i\n", currentChain->length, currentChain->partLength);
@ -338,7 +358,8 @@ CotpConnection_sendDataMessage(CotpConnection* self, BufferChain payload)
if (DEBUG_COTP) if (DEBUG_COTP)
printf("COTP: Send COTP fragment %i bufpos: %i\n", fragments, currentBufPos); printf("COTP: Send COTP fragment %i bufpos: %i\n", fragments, currentBufPos);
if (!sendBuffer(self)) { if (!sendBuffer(self))
{
retValue = COTP_ERROR; retValue = COTP_ERROR;
if (DEBUG_COTP) if (DEBUG_COTP)
@ -438,11 +459,13 @@ parseOptions(CotpConnection* self, uint8_t* buffer, int bufLen)
{ {
int bufPos = 0; int bufPos = 0;
while (bufPos < bufLen) { while (bufPos < bufLen)
{
uint8_t optionType = buffer[bufPos++]; uint8_t optionType = buffer[bufPos++];
uint8_t optionLen = buffer[bufPos++]; uint8_t optionLen = buffer[bufPos++];
if (optionLen > (bufLen - bufPos)) { if (optionLen > (bufLen - bufPos))
{
if (DEBUG_COTP) if (DEBUG_COTP)
printf("COTP: option to long optionLen:%i bufPos:%i bufLen:%i\n", optionLen, bufPos, bufLen); printf("COTP: option to long optionLen:%i bufPos:%i bufLen:%i\n", optionLen, bufPos, bufLen);
goto cpo_error; goto cpo_error;
@ -453,7 +476,8 @@ parseOptions(CotpConnection* self, uint8_t* buffer, int bufLen)
switch (optionType) { switch (optionType) {
case 0xc0: case 0xc0:
if (optionLen == 1) { if (optionLen == 1)
{
int requestedTpduSize = (1 << buffer[bufPos++]); int requestedTpduSize = (1 << buffer[bufPos++]);
CotpConnection_setTpduSize(self, requestedTpduSize); CotpConnection_setTpduSize(self, requestedTpduSize);
@ -466,7 +490,8 @@ parseOptions(CotpConnection* self, uint8_t* buffer, int bufLen)
break; break;
case 0xc1: /* remote T-selector */ case 0xc1: /* remote T-selector */
if (optionLen < 5) { if (optionLen < 5)
{
self->options.tSelSrc.size = optionLen; self->options.tSelSrc.size = optionLen;
int i; int i;
@ -478,7 +503,8 @@ parseOptions(CotpConnection* self, uint8_t* buffer, int bufLen)
break; break;
case 0xc2: /* local T-selector */ case 0xc2: /* local T-selector */
if (optionLen < 5) { if (optionLen < 5)
{
self->options.tSelDst.size = optionLen; self->options.tSelDst.size = optionLen;
int i; int i;
@ -650,7 +676,8 @@ parseDataTpdu(CotpConnection* self, uint8_t* buffer, uint8_t len)
static bool static bool
addPayloadToBuffer(CotpConnection* self, uint8_t* buffer, int payloadLength) addPayloadToBuffer(CotpConnection* self, uint8_t* buffer, int payloadLength)
{ {
if (payloadLength < 1) { if (payloadLength < 1)
{
if (DEBUG_COTP) if (DEBUG_COTP)
printf("COTP: missing payload\n"); printf("COTP: missing payload\n");
@ -681,7 +708,8 @@ parseCotpMessage(CotpConnection* self)
len = buffer[0]; len = buffer[0];
if (len > tpduLength) { if (len > tpduLength)
{
if (DEBUG_COTP) if (DEBUG_COTP)
printf("COTP: parseCotpMessage: len=%d tpduLength=%d\n", len, tpduLength); printf("COTP: parseCotpMessage: len=%d tpduLength=%d\n", len, tpduLength);
@ -702,8 +730,8 @@ parseCotpMessage(CotpConnection* self)
else else
return COTP_ERROR; return COTP_ERROR;
case 0xf0: case 0xf0:
if (parseDataTpdu(self, buffer + 2, len)) { if (parseDataTpdu(self, buffer + 2, len))
{
if (addPayloadToBuffer(self, buffer + 3, tpduLength - 3) != 1) if (addPayloadToBuffer(self, buffer + 3, tpduLength - 3) != 1)
return COTP_ERROR; return COTP_ERROR;
@ -743,7 +771,8 @@ readFromSocket(CotpConnection* self, uint8_t* buf, int size)
#if (CONFIG_MMS_SUPPORT_TLS == 1) #if (CONFIG_MMS_SUPPORT_TLS == 1)
if (self->tlsSocket) if (self->tlsSocket)
return TLSSocket_read(self->tlsSocket, buf, size); return TLSSocket_read(self->tlsSocket, buf, size);
else { else
{
switch (Handleset_waitReady(self->handleSet, 10)) switch (Handleset_waitReady(self->handleSet, 10))
{ {
case -1: case -1:
@ -787,7 +816,8 @@ CotpConnection_readToTpktBuffer(CotpConnection* self)
assert (bufferSize > 4); assert (bufferSize > 4);
if (self->socketExtensionBufferFill > 0) { if (self->socketExtensionBufferFill > 0)
{
if (flushBuffer(self) == false) if (flushBuffer(self) == false)
goto exit_error; goto exit_error;
@ -797,33 +827,38 @@ CotpConnection_readToTpktBuffer(CotpConnection* self)
int readBytes; int readBytes;
if (bufPos < 4) { if (bufPos < 4)
{
readBytes = readFromSocket(self, buffer + bufPos, 4 - bufPos); readBytes = readFromSocket(self, buffer + bufPos, 4 - bufPos);
if (readBytes < 0) if (readBytes < 0)
goto exit_closed; goto exit_closed;
if (DEBUG_COTP) { if (DEBUG_COTP)
{
if (readBytes > 0) if (readBytes > 0)
printf("TPKT: read %i bytes from socket\n", readBytes); printf("TPKT: read %i bytes from socket\n", readBytes);
} }
bufPos += readBytes; bufPos += readBytes;
if (bufPos == 4) { if (bufPos == 4)
if ((buffer[0] == 3) && (buffer[1] == 0)) { {
if ((buffer[0] == 3) && (buffer[1] == 0))
{
self->packetSize = (buffer[2] * 0x100) + buffer[3]; self->packetSize = (buffer[2] * 0x100) + buffer[3];
if (DEBUG_COTP) if (DEBUG_COTP)
printf("TPKT: header complete (msg size = %i)\n", self->packetSize); printf("TPKT: header complete (msg size = %i)\n", self->packetSize);
if (self->packetSize > bufferSize) { if (self->packetSize > bufferSize)
{
if (DEBUG_COTP) printf("TPKT: packet too large\n"); if (DEBUG_COTP) printf("TPKT: packet too large\n");
goto exit_error; goto exit_error;
} }
} }
else { else
{
if (DEBUG_COTP) printf("TPKT: failed to decode TPKT header.\n"); if (DEBUG_COTP) printf("TPKT: failed to decode TPKT header.\n");
goto exit_error; goto exit_error;
} }
@ -869,4 +904,3 @@ exit_waiting:
self->readBuffer->size = bufPos; self->readBuffer->size = bufPos;
return TPKT_WAITING; return TPKT_WAITING;
} }

3
src/mms/iso_mms/server/mms_server.c
Unescape Escape View File

@ -566,7 +566,8 @@ mmsServer_setValue(MmsServer self, MmsDomain* domain, char* itemId, MmsValue* va
{ {
MmsValue_update(cachedValue, value); MmsValue_update(cachedValue, value);
indication = DATA_ACCESS_ERROR_SUCCESS; indication = DATA_ACCESS_ERROR_SUCCESS;
} else }
else
indication = DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID; indication = DATA_ACCESS_ERROR_OBJECT_VALUE_INVALID;
} }

Write Preview
Loading…
Cancel
Save