text: /*
   *  ber_encoder.c
   *
   *  Copyright 2013 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 "libiec61850_platform_includes.h"
  #include "ber_encoder.h"
  
  int
  BerEncoder_encodeLength(uint32_t length, uint8_t* buffer, int bufPos)
  {
      if (length < 128) {
          buffer[bufPos++] = (uint8_t) length;
      }
      else if (length < 256) {
          buffer[bufPos++] = 0x81;
          buffer[bufPos++] = (uint8_t) length;
      }
      else {
          buffer[bufPos++] = 0x82;
  
          buffer[bufPos++] = length / 256;
          buffer[bufPos++] = length % 256;
      }
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeTL(uint8_t tag, uint32_t length, uint8_t* buffer, int bufPos)
  {
      buffer[bufPos++] = tag;
      bufPos = BerEncoder_encodeLength(length, buffer, bufPos);
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeBoolean(uint8_t tag, bool value, uint8_t* buffer, int bufPos)
  {
      buffer[bufPos++] = tag;
      buffer[bufPos++] = 1;
  
      if (value)
          buffer[bufPos++] = 0xff;
      else
          buffer[bufPos++] = 0x00;
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeStringWithTag(uint8_t tag, const char* string, uint8_t* buffer, int bufPos)
  {
      buffer[bufPos++] = tag;
  
      if (string != NULL) {
          int length = strlen(string);
  
          bufPos = BerEncoder_encodeLength(length, buffer, bufPos);
  
          int i;
          for (i = 0; i < length; i++) {
              buffer[bufPos++] = string[i];
          }
      }
      else
          buffer[bufPos++] = 0;
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeAsn1PrimitiveValue(uint8_t tag, Asn1PrimitiveValue* value, uint8_t* buffer, int bufPos)
  {
      buffer[bufPos++] = tag;
  
      bufPos = BerEncoder_encodeLength(value->size, buffer, bufPos);
  
      int i;
      for (i = 0; i < value->size; i++) {
          buffer[bufPos++] = value->octets[i];
      }
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeOctetString(uint8_t tag, uint8_t* octetString, uint32_t octetStringSize, uint8_t* buffer, int bufPos)
  {
      buffer[bufPos++] = tag;
  
      bufPos = BerEncoder_encodeLength(octetStringSize, buffer, bufPos);
  
      uint32_t i;
      for (i = 0; i < octetStringSize; i++) {
          buffer[bufPos++] = octetString[i];
      }
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeBitString(uint8_t tag, int bitStringSize, uint8_t* bitString, uint8_t* buffer, int bufPos)
  {
      buffer[bufPos++] = tag;
  
      int byteSize = bitStringSize / 8;
  
      if (bitStringSize % 8)
          byteSize++;
  
      int padding = (byteSize * 8) - bitStringSize;
  
      bufPos = BerEncoder_encodeLength(byteSize + 1, buffer, bufPos);
  
      buffer[bufPos++] = padding;
  
      int i;
  
      for (i = 0; i < byteSize; i++) {
          buffer[bufPos++] = bitString[i];
      }
  
      uint8_t paddingMask = 0;
  
      for (i = 0; i < padding; i++) {
          paddingMask += (1 << i);
      }
  
      paddingMask = ~paddingMask;
  
      buffer[bufPos -1] = buffer[bufPos -1] & paddingMask;
  
      return bufPos;
  }
  
  int
  BerEncoder_determineEncodedBitStringSize(int bitStringSize)
  {
      int size = 2; /* for tag and padding */
  
      int byteSize = bitStringSize / 8;
  
      if (bitStringSize % 8)
          byteSize++;
  
      size += BerEncoder_determineLengthSize(byteSize);
  
      size += byteSize;
  
      return size;
  }
  
  void
  BerEncoder_revertByteOrder(uint8_t* octets, const int size)
  {
      int i;
      uint8_t temp;
  
      for (i = 0; i < size / 2; i++) {
          temp = octets[i];
          octets[i] = octets[(size - 1) - i];
          octets[(size - 1) - i] = temp;
      }
  }
  
  
  int
  BerEncoder_compressInteger(uint8_t* integer, int originalSize)
  {
  
      uint8_t* integerEnd = integer + originalSize - 1;
      uint8_t* bytePosition;
  
      for (bytePosition = integer; bytePosition < integerEnd; bytePosition++) {
  
          if (bytePosition[0] == 0x00) {
              if ((bytePosition[1] & 0x80) == 0)
                  continue;
          }
          else if (bytePosition[0] == 0xff) {
              if (bytePosition[1] & 0x80)
                  continue;
          }
  
          break;
      }
  
      int bytesToDelete = bytePosition - integer;
      int newSize = originalSize;
  
      if (bytesToDelete) {
          newSize -= bytesToDelete;
          uint8_t* newEnd = integer + newSize;
  
          uint8_t *newBytePosition;
  
          for(newBytePosition = integer; newBytePosition < newEnd; newBytePosition++) {
              *newBytePosition = *bytePosition;
               bytePosition++;
          }
  
      }
  
      return newSize;
  }
  
  int
  BerEncoder_encodeUInt32(uint32_t value, uint8_t* buffer, int bufPos)
  {
      uint8_t* valueArray = (uint8_t*) &value;
      uint8_t valueBuffer[5];
  
      valueBuffer[0] = 0;
  
      int i;
      for (i = 0; i < 4; i++) {
          valueBuffer[i + 1] = valueArray[i];
      }
  
  #if (ORDER_LITTLE_ENDIAN == 1)
      BerEncoder_revertByteOrder(valueBuffer + 1, 4);
  #endif
  
      int size = BerEncoder_compressInteger(valueBuffer, 5);
  
      for (i = 0; i < size; i++) {
          buffer[bufPos++] = valueBuffer[i];
      }
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeUInt32WithTL(uint8_t tag, uint32_t value, uint8_t* buffer, int bufPos)
  {
      uint8_t* valueArray = (uint8_t*) &value;
      uint8_t valueBuffer[5];
  
      valueBuffer[0] = 0;
  
      int i;
      for (i = 0; i < 4; i++) {
          valueBuffer[i + 1] = valueArray[i];
      }
  
  #if (ORDER_LITTLE_ENDIAN == 1)
      BerEncoder_revertByteOrder(valueBuffer + 1, 4);
  #endif
  
      int size = BerEncoder_compressInteger(valueBuffer, 5);
  
      buffer[bufPos++] = tag;
      buffer[bufPos++] = (uint8_t) size;
  
      for (i = 0; i < size; i++) {
          buffer[bufPos++] = valueBuffer[i];
      }
  
      return bufPos;
  }
  
  int
  BerEncoder_encodeFloat(uint8_t* floatValue, uint8_t formatWidth, uint8_t exponentWidth,
          uint8_t* buffer, int bufPos)
  {
      //TODO operate on encoding buffer directly
      uint8_t valueBuffer[9];
  
      int byteSize = formatWidth / 8;
  
      valueBuffer[0] = exponentWidth;
  
      int i;
      for (i = 0; i < byteSize; i++) {
          valueBuffer[i + 1] = floatValue[i];
      }
  
  #if (ORDER_LITTLE_ENDIAN == 1)
      BerEncoder_revertByteOrder(valueBuffer + 1, byteSize);
  #endif
  
      for (i = 0; i < byteSize + 1; i++) {
          buffer[bufPos++] = valueBuffer[i];
      }
  
      return bufPos;
  }
  
  int
  BerEncoder_UInt32determineEncodedSize(uint32_t value)
  {
      uint8_t* valueArray = (uint8_t*) &value;
      uint8_t valueBuffer[5];
  
      valueBuffer[0] = 0;
  
      int i;
      for (i = 0; i < 4; i++) {
         valueBuffer[i + 1] = valueArray[i];
      }
  
  #if (ORDER_LITTLE_ENDIAN == 1)
      BerEncoder_revertByteOrder(valueBuffer + 1, 4);
  #endif
  
      int size = BerEncoder_compressInteger(valueBuffer, 5);
  
      return size;
  }
  
  int
  BerEncoder_determineLengthSize(uint32_t length)
  {
      if (length < 128)
          return 1;
      if (length < 256)
          return 2;
      else
          return 3;
  }
  
  int
  BerEncoder_determineEncodedStringSize(const char* string)
  {
      if (string != NULL) {
          int size = 1;
  
          int length = strlen(string);
  
          size += BerEncoder_determineLengthSize(length);
  
          size += length;
  
          return size;
      }
      else
          return 2;
  }
  
  int
  BerEncoder_encodeOIDToBuffer(const char* oidString, uint8_t* buffer, int maxBufLen)
  {
      int encodedBytes = 0;
  
      int x = atoi(oidString);
  
      const char* separator = strchr(oidString, '.');
  
      if (separator == NULL) return 0;
  
      int y = atoi(separator + 1);
  
      int val = x * 40 + y;
  
      if (encodedBytes < maxBufLen)
          buffer[encodedBytes] = (uint8_t) val;
      else
          return 0;
  
      encodedBytes++;
  
      while (1) {
          separator = strchr(separator + 1, '.');
  
          if (separator == NULL)
              break;
  
          val = atoi(separator + 1);
  
          int requiredBytes = 0;
  
          int val2 = val;
          while (val2 > 0) {
              requiredBytes++;
              val2 = val2 >> 7;
          }
  
          while (requiredBytes > 0) {
              val2 = val >> (7 * (requiredBytes - 1));
  
              val2 = val2 & 0x7f;
  
              if (requiredBytes > 1)
                  val2 += 128;
  
              if (encodedBytes == maxBufLen)
                  return 0;
  
              buffer[encodedBytes++] = (uint8_t) val2;
  
              requiredBytes--;
          }
      }
  
      return encodedBytes;
  }
  