Diff: rapid61850/ctypes.c

Revision:

0:f09b7bb8bcce

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/rapid61850/ctypes.c	Tue Oct 02 21:31:05 2012 +0000
@@ -0,0 +1,207 @@
+/**
+ * Rapid-prototyping protection schemes with IEC 61850
+ *
+ * Copyright (c) 2012 Steven Blair
+ *
+ * This program 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 2
+ * of the License, or (at your option) any later version.
+
+ * This program 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 this program; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
+ */
+
+#include "ctypes.h"
+#include "gse.h"
+#include "sv.h"
+#if TIMESTAMP_SUPPORTED == 1
+#include <sys\time.h>
+#endif
+
+#define ENDIAN_BUFFER_SIZE        8
+
+unsigned char    LOCAL_MAC_ADDRESS[] = LOCAL_MAC_ADDRESS_VALUE;
+
+int ber_integer_length(void *value, int maxLength) {
+    unsigned char    endian_buf[ENDIAN_BUFFER_SIZE] = {0};
+    netmemcpy(endian_buf, value, maxLength);    // ensure bytes are in big-endian order
+
+    unsigned char *buf = endian_buf;
+    unsigned char *end1 = buf + maxLength - 1;
+    int shift = 0;
+
+    /* Compute the number of superfluous leading bytes */
+    for(; buf < end1; buf++) {
+        /*
+         * If the contents octets of an integer value encoding
+         * consist of more than one octet, then the bits of the
+         * first octet and bit 8 of the second octet:
+         * a) shall not all be ones; and
+         * b) shall not all be zero.
+         */
+        switch(*buf) {
+        case 0x00:
+            if((buf[1] & 0x80) == 0) {
+                continue;
+            }
+            break;
+        case 0xff:
+            if((buf[1] & 0x80)) {
+                continue;
+            }
+            break;
+        }
+        break;
+    }
+
+    shift = buf - endian_buf;
+
+    return maxLength - shift;
+}
+
+int ber_encode_integer_fixed_size(unsigned char *bufDst, void *value, int maxLength) {
+    unsigned char *firstByte = (unsigned char*) value;
+    unsigned char padding = (firstByte[0] & 0x80) ? 0xFF : 0x00;
+
+    bufDst[0] = padding;
+    netmemcpy(&bufDst[1], value, maxLength);
+
+    return maxLength + 1;
+}
+
+int ber_encode_integer(unsigned char *bufDst, void *value, int maxLength) {
+    unsigned char    endian_buf[ENDIAN_BUFFER_SIZE] = {0};
+    netmemcpy(endian_buf, value, maxLength);    // ensure bytes are in big-endian order
+
+    unsigned char *buf = endian_buf;
+    unsigned char *end1 = buf + maxLength - 1;
+    int shift = 0;
+
+    /* Compute the number of superfluous leading bytes */
+    for(; buf < end1; buf++) {
+        /*
+         * If the contents octets of an integer value encoding
+         * consist of more than one octet, then the bits of the
+         * first octet and bit 8 of the second octet:
+         * a) shall not all be ones; and
+         * b) shall not all be zero.
+         */
+        switch(*buf) {
+        case 0x00:
+            if((buf[1] & 0x80) == 0) {
+                continue;
+            }
+            break;
+        case 0xff:
+            if((buf[1] & 0x80)) {
+                continue;
+            }
+            break;
+        }
+        break;
+    }
+
+    shift = buf - endian_buf;
+
+    unsigned char *nb = endian_buf;
+    unsigned char *end;
+
+    maxLength -= shift;    /* New size, minus bad bytes */
+    end = nb + maxLength;
+
+    int i = 0;
+    for(; nb < end; nb++, buf++, i++) {
+        //*nb = *buf;
+        bufDst[i] = *buf;
+    }
+
+    return maxLength;
+}
+
+//#if GOOSE_FIXED_SIZE == 1
+//void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
+//    ;
+//}
+//#else
+void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
+    unsigned char    endian_buf[ENDIAN_BUFFER_SIZE] = {0};
+    unsigned char padding = (buf[0] & 0x80) ? 0xFF : 0x00;
+    int i = 0;
+    unsigned char *dest = (unsigned char *) value;
+
+    for (i = maxLength - 1; i >= 0; i--) {
+        if ((i + length) < maxLength) {
+            endian_buf[i] = padding;
+        }
+        else {
+            endian_buf[i] = buf[i - (maxLength - length)];
+        }
+    }
+
+    netmemcpy(dest, endian_buf, maxLength);
+}
+//#endif
+
+//TODO need cast to, for example, (unsigned char *) for calls to reversememcpy()?
+
+// a simple memcpy implementation, that reverses endian-ness
+void reversememcpy(unsigned char *dst, const unsigned char *src, unsigned int len) {
+    while (len--) {
+        *dst++ = src[len];
+    }
+}
+
+void setTimestamp(CTYPE_TIMESTAMP *dest) {
+#if TIMESTAMP_SUPPORTED == 1
+    unsigned char *buf = (unsigned char *) dest;
+    struct timeval tv;
+    CTYPE_INT32U frac = 0;
+
+    gettimeofday(&tv, NULL);
+    frac = (CTYPE_INT32U) ((float) tv.tv_usec * 4294.967296);    // * 2^32 / 1000000;
+
+    netmemcpy(&buf[0], &tv.tv_sec, 4);
+    netmemcpy(&buf[4], &frac, 4);
+
+    buf[7] = 0x18;    // quality: 24 bits of accuracy
+#endif
+}
+
+// if the recommended MAC address ranges are used, this function filters GOOSE and SV packets
+void gse_sv_packet_filter(unsigned char *buf, int len) {
+    if (buf[0] == 0x01 && buf[1] == 0x0C && buf[2] == 0xCD) {
+        if (buf[3] == 0x01) {
+            //GOOSE: 01-0C-CD-01-00-00 to 01-0C-CD-01-01-FF
+            gseDecode(buf, len);
+        }
+        else if (buf[3] == 0x04) {
+            //SV: 01-0C-CD-04-00-00 to 01-0C-CD-04-01-FF
+            svDecode(buf, len);
+        }
+    }
+}
+
+// copies bytes to network format (big-endian)
+void netmemcpy(void *dst, const void *src, unsigned int len) {
+#ifdef LITTLE_ENDIAN
+    reversememcpy((unsigned char *) dst, (const unsigned char *) src, len);
+#else
+    memcpy((unsigned char *) dst, (const unsigned char *) src, len);
+#endif
+}
+
+// copies bytes to host format (little-endian)
+void hostmemcpy(void *dst, const void *src, unsigned int len) {
+#ifdef LITTLE_ENDIAN
+    memcpy((unsigned char *) dst, (const unsigned char *) src, len);
+#else
+    reversememcpy((unsigned char *) dst, (const unsigned char *) src, len);
+#endif
+}