From 02a330e4146141545acbe0c73d2a46b63b14614c Mon Sep 17 00:00:00 2001 From: Michael Zillgith Date: Sun, 18 Mar 2018 12:08:44 +0100 Subject: [PATCH] - IEC 61850 server: added memory alignement for buffered reporting --- make/target_system.mk | 4 +-- src/common/inc/simple_allocator.h | 3 ++ src/common/simple_allocator.c | 6 ++-- src/iec61850/server/mms_mapping/reporting.c | 29 ++++++++--------- src/mms/iso_mms/common/mms_value.c | 35 +++++++++++---------- 5 files changed, 39 insertions(+), 38 deletions(-) diff --git a/make/target_system.mk b/make/target_system.mk index b01e9887..107f9b0b 100644 --- a/make/target_system.mk +++ b/make/target_system.mk @@ -1,7 +1,7 @@ UNAME := $(shell uname) MIPSEL_TOOLCHAIN_PREFIX=mipsel-openwrt-linux- -ARM_TOOLCHAIN_PREFIX=arm-linux-gnueabihf- +ARM_TOOLCHAIN_PREFIX=arm-linux- #ARM_TOOLCHAIN_PREFIX=arm-linux-gnueabi- #ARM_TOOLCHAIN_PREFIX=arm-poky-linux-gnueabi- #ARM_TOOLCHAIN_PREFIX=arm-linux-gnueabi- @@ -57,7 +57,7 @@ endif ifeq ($(TARGET), LINUX-ARM) TOOLCHAIN_PREFIX=$(ARM_TOOLCHAIN_PREFIX) -CFLAGS += -mno-unaligned-access +#CFLAGS += -mno-unaligned-access #CFLAGS += -mcpu=arm926ej-s endif diff --git a/src/common/inc/simple_allocator.h b/src/common/inc/simple_allocator.h index 7158ac7b..59da7ce4 100644 --- a/src/common/inc/simple_allocator.h +++ b/src/common/inc/simple_allocator.h @@ -33,6 +33,9 @@ typedef struct { void MemoryAllocator_init(MemoryAllocator* self, char* memoryBlock, int size); +int +MemoryAllocator_getAlignedSize(int size); + char* MemoryAllocator_allocate(MemoryAllocator* self, int size); diff --git a/src/common/simple_allocator.c b/src/common/simple_allocator.c index 89c99403..2c4c5a24 100644 --- a/src/common/simple_allocator.c +++ b/src/common/simple_allocator.c @@ -32,8 +32,8 @@ MemoryAllocator_init(MemoryAllocator* self, char* memoryBlock, int size) self->size = size; } -static int -getAlignedSize(int size) +int inline +MemoryAllocator_getAlignedSize(int size) { if ((size % sizeof(void*)) > 0) return sizeof(void*) * ((size + sizeof(void*) - 1) / sizeof(void*)); @@ -44,7 +44,7 @@ getAlignedSize(int size) char* MemoryAllocator_allocate(MemoryAllocator* self, int size) { - size = getAlignedSize(size); + size = MemoryAllocator_getAlignedSize(size); if (((self->currentPtr - self->memoryBlock) + size) <= self->size) { char* ptr = self->currentPtr; diff --git a/src/iec61850/server/mms_mapping/reporting.c b/src/iec61850/server/mms_mapping/reporting.c index d5eae3e8..48ad2c7f 100644 --- a/src/iec61850/server/mms_mapping/reporting.c +++ b/src/iec61850/server/mms_mapping/reporting.c @@ -1688,9 +1688,9 @@ enqueueReport(ReportControl* reportControl, bool isIntegrity, bool isGI, uint64_ ReportBuffer* buffer = reportControl->reportBuffer; /* calculate size of complete buffer entry */ - int bufferEntrySize = sizeof(ReportBufferEntry); + int bufferEntrySize = MemoryAllocator_getAlignedSize(sizeof(ReportBufferEntry)); - int inclusionFieldSize = MmsValue_getBitStringByteSize(reportControl->inclusionField); + int inclusionFieldSize = MemoryAllocator_getAlignedSize(MmsValue_getBitStringByteSize(reportControl->inclusionField)); MmsValue inclusionFieldStatic; @@ -1708,7 +1708,7 @@ enqueueReport(ReportControl* reportControl, bool isIntegrity, bool isGI, uint64_ for (i = 0; i < MmsValue_getBitStringSize(reportControl->inclusionField); i++) { assert(dataSetEntry != NULL); - bufferEntrySize += 1; /* reason-for-inclusion */ + bufferEntrySize += MemoryAllocator_getAlignedSize(1); /* reason-for-inclusion */ bufferEntrySize += MmsValue_getSizeInMemory(dataSetEntry->value); @@ -1723,7 +1723,7 @@ enqueueReport(ReportControl* reportControl, bool isIntegrity, bool isGI, uint64_ for (i = 0; i < MmsValue_getBitStringSize(reportControl->inclusionField); i++) { if (reportControl->inclusionFlags[i] != REPORT_CONTROL_NONE) { - bufferEntrySize += 1; /* reason-for-inclusion */ + bufferEntrySize += MemoryAllocator_getAlignedSize(1); /* reason-for-inclusion */ assert(reportControl->bufferedDataSetValues[i] != NULL); @@ -1946,12 +1946,9 @@ enqueueReport(ReportControl* reportControl, bool isIntegrity, bool isGI, uint64_ else entry->flags = 0; - if ((bufferEntrySize % sizeof(void*)) > 0) - bufferEntrySize = sizeof(void*) * ((bufferEntrySize + sizeof(void*) - 1) / sizeof(void*)); + entry->entryLength = MemoryAllocator_getAlignedSize(bufferEntrySize); - entry->entryLength = bufferEntrySize; - - entryBufPos += sizeof(ReportBufferEntry); + entryBufPos += MemoryAllocator_getAlignedSize(sizeof(ReportBufferEntry)); if (isIntegrity || isGI) { DataSetEntry* dataSetEntry = reportControl->dataSet->fcdas; @@ -1963,7 +1960,7 @@ enqueueReport(ReportControl* reportControl, bool isIntegrity, bool isGI, uint64_ assert(dataSetEntry != NULL); *entryBufPos = (uint8_t) reportControl->inclusionFlags[i]; - entryBufPos++; + entryBufPos += MemoryAllocator_getAlignedSize(1); entryBufPos = MmsValue_cloneToBuffer(dataSetEntry->value, entryBufPos); @@ -1985,7 +1982,7 @@ enqueueReport(ReportControl* reportControl, bool isIntegrity, bool isGI, uint64_ assert(reportControl->bufferedDataSetValues[i] != NULL); *entryBufPos = (uint8_t) reportControl->inclusionFlags[i]; - entryBufPos++; + entryBufPos += MemoryAllocator_getAlignedSize(1); entryBufPos = MmsValue_cloneToBuffer(reportControl->bufferedDataSetValues[i], entryBufPos); @@ -2078,7 +2075,7 @@ sendNextReportEntry(ReportControl* self) MmsValue* inclusionField = &inclusionFieldStack; if (report->flags == 0) - currentReportBufferPos += MmsValue_getBitStringByteSize(inclusionField); + currentReportBufferPos += MemoryAllocator_getAlignedSize(MmsValue_getBitStringByteSize(inclusionField)); else { inclusionFieldStack.value.bitString.buf = (uint8_t*) MemoryAllocator_allocate(&ma, MmsValue_getBitStringByteSize(inclusionField)); @@ -2244,7 +2241,7 @@ sendNextReportEntry(ReportControl* self) for (i = 0; i < self->dataSet->elementCount; i++) { if (report->flags > 0) { - currentReportBufferPos++; + currentReportBufferPos += MemoryAllocator_getAlignedSize(1);; if (MemAllocLinkedList_add(reportElements, currentReportBufferPos) == NULL) goto return_out_of_memory; @@ -2252,7 +2249,7 @@ sendNextReportEntry(ReportControl* self) } else { if (MmsValue_getBitStringBit(inclusionField, i)) { - currentReportBufferPos++; + currentReportBufferPos += MemoryAllocator_getAlignedSize(1);; if (MemAllocLinkedList_add(reportElements, currentReportBufferPos) == NULL) goto return_out_of_memory; currentReportBufferPos += MmsValue_getSizeInMemory((MmsValue*) currentReportBufferPos); @@ -2289,7 +2286,7 @@ sendNextReportEntry(ReportControl* self) if (MemAllocLinkedList_add(reportElements, reason) == NULL) goto return_out_of_memory; - currentReportBufferPos++; + currentReportBufferPos += MemoryAllocator_getAlignedSize(1); MmsValue* dataSetElement = (MmsValue*) currentReportBufferPos; @@ -2324,7 +2321,7 @@ sendNextReportEntry(ReportControl* self) break; } - currentReportBufferPos++; + currentReportBufferPos += MemoryAllocator_getAlignedSize(1); MmsValue* dataSetElement = (MmsValue*) currentReportBufferPos; diff --git a/src/mms/iso_mms/common/mms_value.c b/src/mms/iso_mms/common/mms_value.c index 41cf22ec..4bf62ca7 100644 --- a/src/mms/iso_mms/common/mms_value.c +++ b/src/mms/iso_mms/common/mms_value.c @@ -1,7 +1,7 @@ /* * mms_value.c * - * Copyright 2013-2016 Michael Zillgith + * Copyright 2013-2018 Michael Zillgith * * This file is part of libIEC61850. * @@ -32,6 +32,8 @@ #include "conversions.h" +#include "simple_allocator.h" + #include /* for ctime_r */ static inline int @@ -962,13 +964,13 @@ MmsValue_toUnixTimestamp(const MmsValue* self) int MmsValue_getSizeInMemory(const MmsValue* self) { - int memorySize = sizeof(MmsValue); + int memorySize = MemoryAllocator_getAlignedSize(sizeof(MmsValue)); switch(self->type) { case MMS_ARRAY: case MMS_STRUCTURE: { - memorySize += (sizeof(MmsValue*) * self->value.structure.size); + memorySize += (MemoryAllocator_getAlignedSize(sizeof(MmsValue*)) * self->value.structure.size); int i; for (i = 0; i < self->value.structure.size; i++) @@ -977,27 +979,26 @@ MmsValue_getSizeInMemory(const MmsValue* self) break; case MMS_BIT_STRING: - memorySize += bitStringByteSize(self); + memorySize += MemoryAllocator_getAlignedSize(bitStringByteSize(self)); break; case MMS_INTEGER: case MMS_UNSIGNED: - memorySize += sizeof(Asn1PrimitiveValue); - memorySize += self->value.integer->maxSize; + memorySize += MemoryAllocator_getAlignedSize(sizeof(Asn1PrimitiveValue)); + memorySize += MemoryAllocator_getAlignedSize(self->value.integer->maxSize); break; case MMS_FLOAT: - memorySize += (self->value.floatingPoint.formatWidth / 8); + memorySize += MemoryAllocator_getAlignedSize(self->value.floatingPoint.formatWidth / 8); break; case MMS_OCTET_STRING: - memorySize += self->value.octetString.maxSize; + memorySize += MemoryAllocator_getAlignedSize(self->value.octetString.maxSize); break; case MMS_STRING: case MMS_VISIBLE_STRING: - memorySize += strlen(self->value.visibleString.buf); - memorySize += 1; /* add space for 0 character */ + memorySize += MemoryAllocator_getAlignedSize(strlen(self->value.visibleString.buf) + 1); /* add space for 0 character */ break; default: @@ -1013,7 +1014,7 @@ MmsValue_cloneToBuffer(const MmsValue* self, uint8_t* destinationAddress) MmsValue* newValue = (MmsValue*) destinationAddress; memcpy(destinationAddress, self, sizeof(MmsValue)); - destinationAddress += sizeof(MmsValue); + destinationAddress += MemoryAllocator_getAlignedSize(sizeof(MmsValue)); switch (self->type) { case MMS_ARRAY: @@ -1033,7 +1034,7 @@ MmsValue_cloneToBuffer(const MmsValue* self, uint8_t* destinationAddress) case MMS_BIT_STRING: memcpy(destinationAddress, self->value.bitString.buf, bitStringByteSize(self)); newValue->value.bitString.buf = destinationAddress; - destinationAddress += bitStringByteSize(self); + destinationAddress += MemoryAllocator_getAlignedSize(bitStringByteSize(self)); break; case MMS_INTEGER: @@ -1042,10 +1043,10 @@ MmsValue_cloneToBuffer(const MmsValue* self, uint8_t* destinationAddress) newValue->value.integer = (Asn1PrimitiveValue*) destinationAddress; Asn1PrimitiveValue* newAsn1Value = (Asn1PrimitiveValue*) destinationAddress; memcpy(destinationAddress, self->value.integer, sizeof(Asn1PrimitiveValue)); - destinationAddress += sizeof(Asn1PrimitiveValue); + destinationAddress += MemoryAllocator_getAlignedSize(sizeof(Asn1PrimitiveValue)); newAsn1Value->octets = destinationAddress; memcpy(destinationAddress, self->value.integer->octets, self->value.integer->maxSize); - destinationAddress += self->value.integer->maxSize; + destinationAddress += MemoryAllocator_getAlignedSize(self->value.integer->maxSize); } break; @@ -1055,14 +1056,14 @@ MmsValue_cloneToBuffer(const MmsValue* self, uint8_t* destinationAddress) newValue->value.floatingPoint.buf = destinationAddress; memcpy(destinationAddress, self->value.floatingPoint.buf, floatSizeInBytes); - destinationAddress += floatSizeInBytes; + destinationAddress += MemoryAllocator_getAlignedSize(floatSizeInBytes); } break; case MMS_OCTET_STRING: newValue->value.octetString.buf = destinationAddress; memcpy(destinationAddress, self->value.octetString.buf, self->value.octetString.maxSize); - destinationAddress += self->value.octetString.maxSize; + destinationAddress += MemoryAllocator_getAlignedSize(self->value.octetString.maxSize); break; case MMS_STRING: @@ -1070,7 +1071,7 @@ MmsValue_cloneToBuffer(const MmsValue* self, uint8_t* destinationAddress) newValue->value.visibleString.buf = (char*) destinationAddress; newValue->value.visibleString.size = self->value.visibleString.size; strcpy((char*) destinationAddress, self->value.visibleString.buf); - destinationAddress += (strlen(self->value.visibleString.buf) + 1); + destinationAddress += MemoryAllocator_getAlignedSize(strlen(self->value.visibleString.buf) + 1); break; default: