ctypes.c

00001 /**
00002  * Rapid-prototyping protection schemes with IEC 61850
00003  *
00004  * Copyright (c) 2012 Steven Blair
00005  *
00006  * This program is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU General Public License
00008  * as published by the Free Software Foundation; either version 2
00009  * of the License, or (at your option) any later version.
00010 
00011  * This program is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  * GNU General Public License for more details.
00015 
00016  * You should have received a copy of the GNU General Public License
00017  * along with this program; if not, write to the Free Software
00018  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, USA.
00019  */
00020 
00021 #include "ctypes.h"
00022 #include "gse.h"
00023 #include "sv.h"
00024 #if TIMESTAMP_SUPPORTED == 1
00025 #include <sys\time.h>
00026 #endif
00027 
00028 #define ENDIAN_BUFFER_SIZE        8
00029 
00030 unsigned char    LOCAL_MAC_ADDRESS[] = LOCAL_MAC_ADDRESS_VALUE;
00031 
00032 int ber_integer_length(void *value, int maxLength) {
00033     unsigned char    endian_buf[ENDIAN_BUFFER_SIZE] = {0};
00034     netmemcpy(endian_buf, value, maxLength);    // ensure bytes are in big-endian order
00035 
00036     unsigned char *buf = endian_buf;
00037     unsigned char *end1 = buf + maxLength - 1;
00038     int shift = 0;
00039 
00040     /* Compute the number of superfluous leading bytes */
00041     for(; buf < end1; buf++) {
00042         /*
00043          * If the contents octets of an integer value encoding
00044          * consist of more than one octet, then the bits of the
00045          * first octet and bit 8 of the second octet:
00046          * a) shall not all be ones; and
00047          * b) shall not all be zero.
00048          */
00049         switch(*buf) {
00050         case 0x00:
00051             if((buf[1] & 0x80) == 0) {
00052                 continue;
00053             }
00054             break;
00055         case 0xff:
00056             if((buf[1] & 0x80)) {
00057                 continue;
00058             }
00059             break;
00060         }
00061         break;
00062     }
00063 
00064     shift = buf - endian_buf;
00065 
00066     return maxLength - shift;
00067 }
00068 
00069 int ber_encode_integer_fixed_size(unsigned char *bufDst, void *value, int maxLength) {
00070     unsigned char *firstByte = (unsigned char*) value;
00071     unsigned char padding = (firstByte[0] & 0x80) ? 0xFF : 0x00;
00072 
00073     bufDst[0] = padding;
00074     netmemcpy(&bufDst[1], value, maxLength);
00075 
00076     return maxLength + 1;
00077 }
00078 
00079 int ber_encode_integer(unsigned char *bufDst, void *value, int maxLength) {
00080     unsigned char    endian_buf[ENDIAN_BUFFER_SIZE] = {0};
00081     netmemcpy(endian_buf, value, maxLength);    // ensure bytes are in big-endian order
00082 
00083     unsigned char *buf = endian_buf;
00084     unsigned char *end1 = buf + maxLength - 1;
00085     int shift = 0;
00086 
00087     /* Compute the number of superfluous leading bytes */
00088     for(; buf < end1; buf++) {
00089         /*
00090          * If the contents octets of an integer value encoding
00091          * consist of more than one octet, then the bits of the
00092          * first octet and bit 8 of the second octet:
00093          * a) shall not all be ones; and
00094          * b) shall not all be zero.
00095          */
00096         switch(*buf) {
00097         case 0x00:
00098             if((buf[1] & 0x80) == 0) {
00099                 continue;
00100             }
00101             break;
00102         case 0xff:
00103             if((buf[1] & 0x80)) {
00104                 continue;
00105             }
00106             break;
00107         }
00108         break;
00109     }
00110 
00111     shift = buf - endian_buf;
00112 
00113     unsigned char *nb = endian_buf;
00114     unsigned char *end;
00115 
00116     maxLength -= shift;    /* New size, minus bad bytes */
00117     end = nb + maxLength;
00118 
00119     int i = 0;
00120     for(; nb < end; nb++, buf++, i++) {
00121         //*nb = *buf;
00122         bufDst[i] = *buf;
00123     }
00124 
00125     return maxLength;
00126 }
00127 
00128 //#if GOOSE_FIXED_SIZE == 1
00129 //void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
00130 //    ;
00131 //}
00132 //#else
00133 void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
00134     unsigned char    endian_buf[ENDIAN_BUFFER_SIZE] = {0};
00135     unsigned char padding = (buf[0] & 0x80) ? 0xFF : 0x00;
00136     int i = 0;
00137     unsigned char *dest = (unsigned char *) value;
00138 
00139     for (i = maxLength - 1; i >= 0; i--) {
00140         if ((i + length) < maxLength) {
00141             endian_buf[i] = padding;
00142         }
00143         else {
00144             endian_buf[i] = buf[i - (maxLength - length)];
00145         }
00146     }
00147 
00148     netmemcpy(dest, endian_buf, maxLength);
00149 }
00150 //#endif
00151 
00152 //TODO need cast to, for example, (unsigned char *) for calls to reversememcpy()?
00153 
00154 // a simple memcpy implementation, that reverses endian-ness
00155 void reversememcpy(unsigned char *dst, const unsigned char *src, unsigned int len) {
00156     while (len--) {
00157         *dst++ = src[len];
00158     }
00159 }
00160 
00161 void setTimestamp(CTYPE_TIMESTAMP *dest) {
00162 #if TIMESTAMP_SUPPORTED == 1
00163     unsigned char *buf = (unsigned char *) dest;
00164     struct timeval tv;
00165     CTYPE_INT32U frac = 0;
00166 
00167     gettimeofday(&tv, NULL);
00168     frac = (CTYPE_INT32U) ((float) tv.tv_usec * 4294.967296);    // * 2^32 / 1000000;
00169 
00170     netmemcpy(&buf[0], &tv.tv_sec, 4);
00171     netmemcpy(&buf[4], &frac, 4);
00172 
00173     buf[7] = 0x18;    // quality: 24 bits of accuracy
00174 #endif
00175 }
00176 
00177 // if the recommended MAC address ranges are used, this function filters GOOSE and SV packets
00178 void gse_sv_packet_filter(unsigned char *buf, int len) {
00179     if (buf[0] == 0x01 && buf[1] == 0x0C && buf[2] == 0xCD) {
00180         if (buf[3] == 0x01) {
00181             //GOOSE: 01-0C-CD-01-00-00 to 01-0C-CD-01-01-FF
00182             gseDecode(buf, len);
00183         }
00184         else if (buf[3] == 0x04) {
00185             //SV: 01-0C-CD-04-00-00 to 01-0C-CD-04-01-FF
00186             svDecode(buf, len);
00187         }
00188     }
00189 }
00190 
00191 // copies bytes to network format (big-endian)
00192 void netmemcpy(void *dst, const void *src, unsigned int len) {
00193 #ifdef LITTLE_ENDIAN
00194     reversememcpy((unsigned char *) dst, (const unsigned char *) src, len);
00195 #else
00196     memcpy((unsigned char *) dst, (const unsigned char *) src, len);
00197 #endif
00198 }
00199 
00200 // copies bytes to host format (little-endian)
00201 void hostmemcpy(void *dst, const void *src, unsigned int len) {
00202 #ifdef LITTLE_ENDIAN
00203     memcpy((unsigned char *) dst, (const unsigned char *) src, len);
00204 #else
00205     reversememcpy((unsigned char *) dst, (const unsigned char *) src, len);
00206 #endif
00207 }