Steven Blair / Mbed 2 deprecated rapid61850-SV-LE

An mbed implementation of IEC 61850-9-2LE Sample Values. Creating using the rapid61850 library, available at: https://github.com/stevenblair/rapid61850.

Dependencies:   mbed

An mbed implementation of IEC 61850-9-2LE Sample Values. Creating using the rapid61850 library, available at: https://github.com/stevenblair/rapid61850.

rapid61850/ctypes.c@0:f09b7bb8bcce, 2012-10-02 (annotated)

Committer:
sblair
Date:
Tue Oct 02 21:31:05 2012 +0000
Revision:
0:f09b7bb8bcce
converted library to folder

Who changed what in which revision?

UserRevisionLine numberNew contents of line
sblair 0:f09b7bb8bcce 1 /**
sblair 0:f09b7bb8bcce 2 * Rapid-prototyping protection schemes with IEC 61850
sblair 0:f09b7bb8bcce 3 *
sblair 0:f09b7bb8bcce 4 * Copyright (c) 2012 Steven Blair
sblair 0:f09b7bb8bcce 5 *
sblair 0:f09b7bb8bcce 6 * This program is free software; you can redistribute it and/or
sblair 0:f09b7bb8bcce 7 * modify it under the terms of the GNU General Public License
sblair 0:f09b7bb8bcce 8 * as published by the Free Software Foundation; either version 2
sblair 0:f09b7bb8bcce 9 * of the License, or (at your option) any later version.
sblair 0:f09b7bb8bcce 10
sblair 0:f09b7bb8bcce 11 * This program is distributed in the hope that it will be useful,
sblair 0:f09b7bb8bcce 12 * but WITHOUT ANY WARRANTY; without even the implied warranty of
sblair 0:f09b7bb8bcce 13 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
sblair 0:f09b7bb8bcce 14 * GNU General Public License for more details.
sblair 0:f09b7bb8bcce 15
sblair 0:f09b7bb8bcce 16 * You should have received a copy of the GNU General Public License
sblair 0:f09b7bb8bcce 17 * along with this program; if not, write to the Free Software
sblair 0:f09b7bb8bcce 18 * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
sblair 0:f09b7bb8bcce 19 */
sblair 0:f09b7bb8bcce 20
sblair 0:f09b7bb8bcce 21 #include "ctypes.h"
sblair 0:f09b7bb8bcce 22 #include "gse.h"
sblair 0:f09b7bb8bcce 23 #include "sv.h"
sblair 0:f09b7bb8bcce 24 #if TIMESTAMP_SUPPORTED == 1
sblair 0:f09b7bb8bcce 25 #include <sys\time.h>
sblair 0:f09b7bb8bcce 26 #endif
sblair 0:f09b7bb8bcce 27
sblair 0:f09b7bb8bcce 28 #define ENDIAN_BUFFER_SIZE 8
sblair 0:f09b7bb8bcce 29
sblair 0:f09b7bb8bcce 30 unsigned char LOCAL_MAC_ADDRESS[] = LOCAL_MAC_ADDRESS_VALUE;
sblair 0:f09b7bb8bcce 31
sblair 0:f09b7bb8bcce 32 int ber_integer_length(void *value, int maxLength) {
sblair 0:f09b7bb8bcce 33 unsigned char endian_buf[ENDIAN_BUFFER_SIZE] = {0};
sblair 0:f09b7bb8bcce 34 netmemcpy(endian_buf, value, maxLength); // ensure bytes are in big-endian order
sblair 0:f09b7bb8bcce 35
sblair 0:f09b7bb8bcce 36 unsigned char *buf = endian_buf;
sblair 0:f09b7bb8bcce 37 unsigned char *end1 = buf + maxLength - 1;
sblair 0:f09b7bb8bcce 38 int shift = 0;
sblair 0:f09b7bb8bcce 39
sblair 0:f09b7bb8bcce 40 /* Compute the number of superfluous leading bytes */
sblair 0:f09b7bb8bcce 41 for(; buf < end1; buf++) {
sblair 0:f09b7bb8bcce 42 /*
sblair 0:f09b7bb8bcce 43 * If the contents octets of an integer value encoding
sblair 0:f09b7bb8bcce 44 * consist of more than one octet, then the bits of the
sblair 0:f09b7bb8bcce 45 * first octet and bit 8 of the second octet:
sblair 0:f09b7bb8bcce 46 * a) shall not all be ones; and
sblair 0:f09b7bb8bcce 47 * b) shall not all be zero.
sblair 0:f09b7bb8bcce 48 */
sblair 0:f09b7bb8bcce 49 switch(*buf) {
sblair 0:f09b7bb8bcce 50 case 0x00:
sblair 0:f09b7bb8bcce 51 if((buf[1] & 0x80) == 0) {
sblair 0:f09b7bb8bcce 52 continue;
sblair 0:f09b7bb8bcce 53 }
sblair 0:f09b7bb8bcce 54 break;
sblair 0:f09b7bb8bcce 55 case 0xff:
sblair 0:f09b7bb8bcce 56 if((buf[1] & 0x80)) {
sblair 0:f09b7bb8bcce 57 continue;
sblair 0:f09b7bb8bcce 58 }
sblair 0:f09b7bb8bcce 59 break;
sblair 0:f09b7bb8bcce 60 }
sblair 0:f09b7bb8bcce 61 break;
sblair 0:f09b7bb8bcce 62 }
sblair 0:f09b7bb8bcce 63
sblair 0:f09b7bb8bcce 64 shift = buf - endian_buf;
sblair 0:f09b7bb8bcce 65
sblair 0:f09b7bb8bcce 66 return maxLength - shift;
sblair 0:f09b7bb8bcce 67 }
sblair 0:f09b7bb8bcce 68
sblair 0:f09b7bb8bcce 69 int ber_encode_integer_fixed_size(unsigned char *bufDst, void *value, int maxLength) {
sblair 0:f09b7bb8bcce 70 unsigned char *firstByte = (unsigned char*) value;
sblair 0:f09b7bb8bcce 71 unsigned char padding = (firstByte[0] & 0x80) ? 0xFF : 0x00;
sblair 0:f09b7bb8bcce 72
sblair 0:f09b7bb8bcce 73 bufDst[0] = padding;
sblair 0:f09b7bb8bcce 74 netmemcpy(&bufDst[1], value, maxLength);
sblair 0:f09b7bb8bcce 75
sblair 0:f09b7bb8bcce 76 return maxLength + 1;
sblair 0:f09b7bb8bcce 77 }
sblair 0:f09b7bb8bcce 78
sblair 0:f09b7bb8bcce 79 int ber_encode_integer(unsigned char *bufDst, void *value, int maxLength) {
sblair 0:f09b7bb8bcce 80 unsigned char endian_buf[ENDIAN_BUFFER_SIZE] = {0};
sblair 0:f09b7bb8bcce 81 netmemcpy(endian_buf, value, maxLength); // ensure bytes are in big-endian order
sblair 0:f09b7bb8bcce 82
sblair 0:f09b7bb8bcce 83 unsigned char *buf = endian_buf;
sblair 0:f09b7bb8bcce 84 unsigned char *end1 = buf + maxLength - 1;
sblair 0:f09b7bb8bcce 85 int shift = 0;
sblair 0:f09b7bb8bcce 86
sblair 0:f09b7bb8bcce 87 /* Compute the number of superfluous leading bytes */
sblair 0:f09b7bb8bcce 88 for(; buf < end1; buf++) {
sblair 0:f09b7bb8bcce 89 /*
sblair 0:f09b7bb8bcce 90 * If the contents octets of an integer value encoding
sblair 0:f09b7bb8bcce 91 * consist of more than one octet, then the bits of the
sblair 0:f09b7bb8bcce 92 * first octet and bit 8 of the second octet:
sblair 0:f09b7bb8bcce 93 * a) shall not all be ones; and
sblair 0:f09b7bb8bcce 94 * b) shall not all be zero.
sblair 0:f09b7bb8bcce 95 */
sblair 0:f09b7bb8bcce 96 switch(*buf) {
sblair 0:f09b7bb8bcce 97 case 0x00:
sblair 0:f09b7bb8bcce 98 if((buf[1] & 0x80) == 0) {
sblair 0:f09b7bb8bcce 99 continue;
sblair 0:f09b7bb8bcce 100 }
sblair 0:f09b7bb8bcce 101 break;
sblair 0:f09b7bb8bcce 102 case 0xff:
sblair 0:f09b7bb8bcce 103 if((buf[1] & 0x80)) {
sblair 0:f09b7bb8bcce 104 continue;
sblair 0:f09b7bb8bcce 105 }
sblair 0:f09b7bb8bcce 106 break;
sblair 0:f09b7bb8bcce 107 }
sblair 0:f09b7bb8bcce 108 break;
sblair 0:f09b7bb8bcce 109 }
sblair 0:f09b7bb8bcce 110
sblair 0:f09b7bb8bcce 111 shift = buf - endian_buf;
sblair 0:f09b7bb8bcce 112
sblair 0:f09b7bb8bcce 113 unsigned char *nb = endian_buf;
sblair 0:f09b7bb8bcce 114 unsigned char *end;
sblair 0:f09b7bb8bcce 115
sblair 0:f09b7bb8bcce 116 maxLength -= shift; /* New size, minus bad bytes */
sblair 0:f09b7bb8bcce 117 end = nb + maxLength;
sblair 0:f09b7bb8bcce 118
sblair 0:f09b7bb8bcce 119 int i = 0;
sblair 0:f09b7bb8bcce 120 for(; nb < end; nb++, buf++, i++) {
sblair 0:f09b7bb8bcce 121 //*nb = *buf;
sblair 0:f09b7bb8bcce 122 bufDst[i] = *buf;
sblair 0:f09b7bb8bcce 123 }
sblair 0:f09b7bb8bcce 124
sblair 0:f09b7bb8bcce 125 return maxLength;
sblair 0:f09b7bb8bcce 126 }
sblair 0:f09b7bb8bcce 127
sblair 0:f09b7bb8bcce 128 //#if GOOSE_FIXED_SIZE == 1
sblair 0:f09b7bb8bcce 129 //void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
sblair 0:f09b7bb8bcce 130 // ;
sblair 0:f09b7bb8bcce 131 //}
sblair 0:f09b7bb8bcce 132 //#else
sblair 0:f09b7bb8bcce 133 void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) {
sblair 0:f09b7bb8bcce 134 unsigned char endian_buf[ENDIAN_BUFFER_SIZE] = {0};
sblair 0:f09b7bb8bcce 135 unsigned char padding = (buf[0] & 0x80) ? 0xFF : 0x00;
sblair 0:f09b7bb8bcce 136 int i = 0;
sblair 0:f09b7bb8bcce 137 unsigned char *dest = (unsigned char *) value;
sblair 0:f09b7bb8bcce 138
sblair 0:f09b7bb8bcce 139 for (i = maxLength - 1; i >= 0; i--) {
sblair 0:f09b7bb8bcce 140 if ((i + length) < maxLength) {
sblair 0:f09b7bb8bcce 141 endian_buf[i] = padding;
sblair 0:f09b7bb8bcce 142 }
sblair 0:f09b7bb8bcce 143 else {
sblair 0:f09b7bb8bcce 144 endian_buf[i] = buf[i - (maxLength - length)];
sblair 0:f09b7bb8bcce 145 }
sblair 0:f09b7bb8bcce 146 }
sblair 0:f09b7bb8bcce 147
sblair 0:f09b7bb8bcce 148 netmemcpy(dest, endian_buf, maxLength);
sblair 0:f09b7bb8bcce 149 }
sblair 0:f09b7bb8bcce 150 //#endif
sblair 0:f09b7bb8bcce 151
sblair 0:f09b7bb8bcce 152 //TODO need cast to, for example, (unsigned char *) for calls to reversememcpy()?
sblair 0:f09b7bb8bcce 153
sblair 0:f09b7bb8bcce 154 // a simple memcpy implementation, that reverses endian-ness
sblair 0:f09b7bb8bcce 155 void reversememcpy(unsigned char *dst, const unsigned char *src, unsigned int len) {
sblair 0:f09b7bb8bcce 156 while (len--) {
sblair 0:f09b7bb8bcce 157 *dst++ = src[len];
sblair 0:f09b7bb8bcce 158 }
sblair 0:f09b7bb8bcce 159 }
sblair 0:f09b7bb8bcce 160
sblair 0:f09b7bb8bcce 161 void setTimestamp(CTYPE_TIMESTAMP *dest) {
sblair 0:f09b7bb8bcce 162 #if TIMESTAMP_SUPPORTED == 1
sblair 0:f09b7bb8bcce 163 unsigned char *buf = (unsigned char *) dest;
sblair 0:f09b7bb8bcce 164 struct timeval tv;
sblair 0:f09b7bb8bcce 165 CTYPE_INT32U frac = 0;
sblair 0:f09b7bb8bcce 166
sblair 0:f09b7bb8bcce 167 gettimeofday(&tv, NULL);
sblair 0:f09b7bb8bcce 168 frac = (CTYPE_INT32U) ((float) tv.tv_usec * 4294.967296); // * 2^32 / 1000000;
sblair 0:f09b7bb8bcce 169
sblair 0:f09b7bb8bcce 170 netmemcpy(&buf[0], &tv.tv_sec, 4);
sblair 0:f09b7bb8bcce 171 netmemcpy(&buf[4], &frac, 4);
sblair 0:f09b7bb8bcce 172
sblair 0:f09b7bb8bcce 173 buf[7] = 0x18; // quality: 24 bits of accuracy
sblair 0:f09b7bb8bcce 174 #endif
sblair 0:f09b7bb8bcce 175 }
sblair 0:f09b7bb8bcce 176
sblair 0:f09b7bb8bcce 177 // if the recommended MAC address ranges are used, this function filters GOOSE and SV packets
sblair 0:f09b7bb8bcce 178 void gse_sv_packet_filter(unsigned char *buf, int len) {
sblair 0:f09b7bb8bcce 179 if (buf[0] == 0x01 && buf[1] == 0x0C && buf[2] == 0xCD) {
sblair 0:f09b7bb8bcce 180 if (buf[3] == 0x01) {
sblair 0:f09b7bb8bcce 181 //GOOSE: 01-0C-CD-01-00-00 to 01-0C-CD-01-01-FF
sblair 0:f09b7bb8bcce 182 gseDecode(buf, len);
sblair 0:f09b7bb8bcce 183 }
sblair 0:f09b7bb8bcce 184 else if (buf[3] == 0x04) {
sblair 0:f09b7bb8bcce 185 //SV: 01-0C-CD-04-00-00 to 01-0C-CD-04-01-FF
sblair 0:f09b7bb8bcce 186 svDecode(buf, len);
sblair 0:f09b7bb8bcce 187 }
sblair 0:f09b7bb8bcce 188 }
sblair 0:f09b7bb8bcce 189 }
sblair 0:f09b7bb8bcce 190
sblair 0:f09b7bb8bcce 191 // copies bytes to network format (big-endian)
sblair 0:f09b7bb8bcce 192 void netmemcpy(void *dst, const void *src, unsigned int len) {
sblair 0:f09b7bb8bcce 193 #ifdef LITTLE_ENDIAN
sblair 0:f09b7bb8bcce 194 reversememcpy((unsigned char *) dst, (const unsigned char *) src, len);
sblair 0:f09b7bb8bcce 195 #else
sblair 0:f09b7bb8bcce 196 memcpy((unsigned char *) dst, (const unsigned char *) src, len);
sblair 0:f09b7bb8bcce 197 #endif
sblair 0:f09b7bb8bcce 198 }
sblair 0:f09b7bb8bcce 199
sblair 0:f09b7bb8bcce 200 // copies bytes to host format (little-endian)
sblair 0:f09b7bb8bcce 201 void hostmemcpy(void *dst, const void *src, unsigned int len) {
sblair 0:f09b7bb8bcce 202 #ifdef LITTLE_ENDIAN
sblair 0:f09b7bb8bcce 203 memcpy((unsigned char *) dst, (const unsigned char *) src, len);
sblair 0:f09b7bb8bcce 204 #else
sblair 0:f09b7bb8bcce 205 reversememcpy((unsigned char *) dst, (const unsigned char *) src, len);
sblair 0:f09b7bb8bcce 206 #endif
sblair 0:f09b7bb8bcce 207 }