Steven Blair
The goal of this software is to automatically generate C/C++ code which reads and writes GOOSE and Sampled Value packets. Any valid IEC 61850 Substation Configuration Description (SCD) file, describing GOOSE and/or SV communications, can be used as the input. The output code is lightweight and platform-independent, so it can run on a variety of devices, including low-cost microcontrollers. It\'s ideal for rapid-prototyping new protection and control systems that require communications. This mbed project is a simple example of this functionality. Other code: https://github.com/stevenblair/rapid61850 Project homepage: http://personal.strath.ac.uk/steven.m.blair/
ctypes.c@0:230c10b228ea, 2011-10-07 (annotated)
- Committer:
- sblair
- Date:
- Fri Oct 07 13:41:08 2011 +0000
- Revision:
- 0:230c10b228ea
- Child:
- 1:9399d44c2b1a
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| sblair | 0:230c10b228ea | 1 | #include "ctypes.h" |
| sblair | 0:230c10b228ea | 2 | #include "gse.h" |
| sblair | 0:230c10b228ea | 3 | #include "sv.h" |
| sblair | 0:230c10b228ea | 4 | #ifdef TIMESTAMP_SUPPORTED |
| sblair | 0:230c10b228ea | 5 | #include <sys\time.h> |
| sblair | 0:230c10b228ea | 6 | #endif |
| sblair | 0:230c10b228ea | 7 | |
| sblair | 0:230c10b228ea | 8 | unsigned char LOCAL_MAC_ADDRESS[] = {0x01, 0x0C, 0xCD, 0x01, 0x00, 0x02}; |
| sblair | 0:230c10b228ea | 9 | unsigned char endian_buf[16] = {0}; |
| sblair | 0:230c10b228ea | 10 | |
| sblair | 0:230c10b228ea | 11 | |
| sblair | 0:230c10b228ea | 12 | int ber_integer_length(void *value, int maxLength) { |
| sblair | 0:230c10b228ea | 13 | netmemcpy(endian_buf, value, maxLength); // ensure bytes are in big-endian order |
| sblair | 0:230c10b228ea | 14 | |
| sblair | 0:230c10b228ea | 15 | unsigned char *buf = endian_buf; |
| sblair | 0:230c10b228ea | 16 | unsigned char *end1 = buf + maxLength - 1; |
| sblair | 0:230c10b228ea | 17 | int shift = 0; |
| sblair | 0:230c10b228ea | 18 | |
| sblair | 0:230c10b228ea | 19 | /* Compute the number of superfluous leading bytes */ |
| sblair | 0:230c10b228ea | 20 | for(; buf < end1; buf++) { |
| sblair | 0:230c10b228ea | 21 | /* |
| sblair | 0:230c10b228ea | 22 | * If the contents octets of an integer value encoding |
| sblair | 0:230c10b228ea | 23 | * consist of more than one octet, then the bits of the |
| sblair | 0:230c10b228ea | 24 | * first octet and bit 8 of the second octet: |
| sblair | 0:230c10b228ea | 25 | * a) shall not all be ones; and |
| sblair | 0:230c10b228ea | 26 | * b) shall not all be zero. |
| sblair | 0:230c10b228ea | 27 | */ |
| sblair | 0:230c10b228ea | 28 | switch(*buf) { |
| sblair | 0:230c10b228ea | 29 | case 0x00: |
| sblair | 0:230c10b228ea | 30 | if((buf[1] & 0x80) == 0) { |
| sblair | 0:230c10b228ea | 31 | continue; |
| sblair | 0:230c10b228ea | 32 | } |
| sblair | 0:230c10b228ea | 33 | break; |
| sblair | 0:230c10b228ea | 34 | case 0xff: |
| sblair | 0:230c10b228ea | 35 | if((buf[1] & 0x80)) { |
| sblair | 0:230c10b228ea | 36 | continue; |
| sblair | 0:230c10b228ea | 37 | } |
| sblair | 0:230c10b228ea | 38 | break; |
| sblair | 0:230c10b228ea | 39 | } |
| sblair | 0:230c10b228ea | 40 | break; |
| sblair | 0:230c10b228ea | 41 | } |
| sblair | 0:230c10b228ea | 42 | |
| sblair | 0:230c10b228ea | 43 | shift = buf - endian_buf; |
| sblair | 0:230c10b228ea | 44 | |
| sblair | 0:230c10b228ea | 45 | return maxLength - shift; |
| sblair | 0:230c10b228ea | 46 | } |
| sblair | 0:230c10b228ea | 47 | |
| sblair | 0:230c10b228ea | 48 | int ber_encode_integer(unsigned char *bufDst, void *value, int maxLength) { |
| sblair | 0:230c10b228ea | 49 | netmemcpy(endian_buf, value, maxLength); // ensure bytes are in big-endian order |
| sblair | 0:230c10b228ea | 50 | |
| sblair | 0:230c10b228ea | 51 | unsigned char *buf = endian_buf; |
| sblair | 0:230c10b228ea | 52 | unsigned char *end1 = buf + maxLength - 1; |
| sblair | 0:230c10b228ea | 53 | int shift = 0; |
| sblair | 0:230c10b228ea | 54 | |
| sblair | 0:230c10b228ea | 55 | /* Compute the number of superfluous leading bytes */ |
| sblair | 0:230c10b228ea | 56 | for(; buf < end1; buf++) { |
| sblair | 0:230c10b228ea | 57 | /* |
| sblair | 0:230c10b228ea | 58 | * If the contents octets of an integer value encoding |
| sblair | 0:230c10b228ea | 59 | * consist of more than one octet, then the bits of the |
| sblair | 0:230c10b228ea | 60 | * first octet and bit 8 of the second octet: |
| sblair | 0:230c10b228ea | 61 | * a) shall not all be ones; and |
| sblair | 0:230c10b228ea | 62 | * b) shall not all be zero. |
| sblair | 0:230c10b228ea | 63 | */ |
| sblair | 0:230c10b228ea | 64 | switch(*buf) { |
| sblair | 0:230c10b228ea | 65 | case 0x00: |
| sblair | 0:230c10b228ea | 66 | if((buf[1] & 0x80) == 0) { |
| sblair | 0:230c10b228ea | 67 | continue; |
| sblair | 0:230c10b228ea | 68 | } |
| sblair | 0:230c10b228ea | 69 | break; |
| sblair | 0:230c10b228ea | 70 | case 0xff: |
| sblair | 0:230c10b228ea | 71 | if((buf[1] & 0x80)) { |
| sblair | 0:230c10b228ea | 72 | continue; |
| sblair | 0:230c10b228ea | 73 | } |
| sblair | 0:230c10b228ea | 74 | break; |
| sblair | 0:230c10b228ea | 75 | } |
| sblair | 0:230c10b228ea | 76 | break; |
| sblair | 0:230c10b228ea | 77 | } |
| sblair | 0:230c10b228ea | 78 | |
| sblair | 0:230c10b228ea | 79 | shift = buf - endian_buf; |
| sblair | 0:230c10b228ea | 80 | |
| sblair | 0:230c10b228ea | 81 | /* Remove leading superfluous bytes from the integer */ |
| sblair | 0:230c10b228ea | 82 | //if (shift) { |
| sblair | 0:230c10b228ea | 83 | unsigned char *nb = endian_buf; |
| sblair | 0:230c10b228ea | 84 | unsigned char *end; |
| sblair | 0:230c10b228ea | 85 | |
| sblair | 0:230c10b228ea | 86 | maxLength -= shift; /* New size, minus bad bytes */ |
| sblair | 0:230c10b228ea | 87 | end = nb + maxLength; |
| sblair | 0:230c10b228ea | 88 | |
| sblair | 0:230c10b228ea | 89 | int i = 0; |
| sblair | 0:230c10b228ea | 90 | for(; nb < end; nb++, buf++, i++) { |
| sblair | 0:230c10b228ea | 91 | //*nb = *buf; |
| sblair | 0:230c10b228ea | 92 | bufDst[i] = *buf; |
| sblair | 0:230c10b228ea | 93 | } |
| sblair | 0:230c10b228ea | 94 | |
| sblair | 0:230c10b228ea | 95 | //printf("st->size: %d, end: %d\n", st->size, *end); |
| sblair | 0:230c10b228ea | 96 | //} |
| sblair | 0:230c10b228ea | 97 | |
| sblair | 0:230c10b228ea | 98 | //memcpy((unsigned char *) bufDst, (const unsigned char *) endian_buf, maxLength); |
| sblair | 0:230c10b228ea | 99 | |
| sblair | 0:230c10b228ea | 100 | //printf("%d %d %d\n", (int) *st->buf, st->size, shift); |
| sblair | 0:230c10b228ea | 101 | return maxLength; |
| sblair | 0:230c10b228ea | 102 | } |
| sblair | 0:230c10b228ea | 103 | |
| sblair | 0:230c10b228ea | 104 | void ber_decode_integer(unsigned char *buf, int length, void *value, int maxLength) { |
| sblair | 0:230c10b228ea | 105 | unsigned char padding = (buf[0] & 0x80) ? 0xFF : 0x00; |
| sblair | 0:230c10b228ea | 106 | int i = 0; |
| sblair | 0:230c10b228ea | 107 | unsigned char *dest = (unsigned char *) value; |
| sblair | 0:230c10b228ea | 108 | |
| sblair | 0:230c10b228ea | 109 | for (i = maxLength - 1; i >= 0; i--) { |
| sblair | 0:230c10b228ea | 110 | if ((i + length) < maxLength) { |
| sblair | 0:230c10b228ea | 111 | endian_buf[i] = padding; |
| sblair | 0:230c10b228ea | 112 | } |
| sblair | 0:230c10b228ea | 113 | else { |
| sblair | 0:230c10b228ea | 114 | endian_buf[i] = buf[i - (maxLength - length)]; |
| sblair | 0:230c10b228ea | 115 | } |
| sblair | 0:230c10b228ea | 116 | } |
| sblair | 0:230c10b228ea | 117 | |
| sblair | 0:230c10b228ea | 118 | netmemcpy(dest, endian_buf, maxLength); |
| sblair | 0:230c10b228ea | 119 | } |
| sblair | 0:230c10b228ea | 120 | |
| sblair | 0:230c10b228ea | 121 | //TODO need cast to, for example, (unsigned char *) for calls to reversememcpy()? |
| sblair | 0:230c10b228ea | 122 | |
| sblair | 0:230c10b228ea | 123 | // a simple memcpy implementation, that reverses endian-ness |
| sblair | 0:230c10b228ea | 124 | void reversememcpy(unsigned char *dst, const unsigned char *src, unsigned int len) { |
| sblair | 0:230c10b228ea | 125 | while (len--) { |
| sblair | 0:230c10b228ea | 126 | *dst++ = src[len]; |
| sblair | 0:230c10b228ea | 127 | } |
| sblair | 0:230c10b228ea | 128 | } |
| sblair | 0:230c10b228ea | 129 | |
| sblair | 0:230c10b228ea | 130 | void setTimestamp(CTYPE_TIMESTAMP *dest) { |
| sblair | 0:230c10b228ea | 131 | #ifdef TIMESTAMP_SUPPORTED |
| sblair | 0:230c10b228ea | 132 | unsigned char *buf = (unsigned char *) dest; |
| sblair | 0:230c10b228ea | 133 | struct timeval tv; |
| sblair | 0:230c10b228ea | 134 | CTYPE_INT32U frac = 0; |
| sblair | 0:230c10b228ea | 135 | |
| sblair | 0:230c10b228ea | 136 | gettimeofday(&tv, NULL); |
| sblair | 0:230c10b228ea | 137 | frac = (CTYPE_INT32U) ((float) tv.tv_usec * 4294.967296); // * 2^32 / 1000000; |
| sblair | 0:230c10b228ea | 138 | |
| sblair | 0:230c10b228ea | 139 | netmemcpy(&buf[0], &tv.tv_sec, 4); |
| sblair | 0:230c10b228ea | 140 | netmemcpy(&buf[4], &frac, 4); |
| sblair | 0:230c10b228ea | 141 | |
| sblair | 0:230c10b228ea | 142 | buf[7] = 0x18; // quality: 24 bits of accuracy |
| sblair | 0:230c10b228ea | 143 | #endif |
| sblair | 0:230c10b228ea | 144 | } |
| sblair | 0:230c10b228ea | 145 | |
| sblair | 0:230c10b228ea | 146 | // if the recommended MAC address ranges are used, this function filters GOOSE and SV packets |
| sblair | 0:230c10b228ea | 147 | void gse_sv_packet_filter(unsigned char *buf, int len) { |
| sblair | 0:230c10b228ea | 148 | if (buf[0] == 0x01 && buf[1] == 0x0C && buf[2] == 0xCD) { |
| sblair | 0:230c10b228ea | 149 | if (buf[3] == 0x01) { |
| sblair | 0:230c10b228ea | 150 | //GOOSE: 01-0C-CD-01-00-00 to 01-0C-CD-01-01-FF |
| sblair | 0:230c10b228ea | 151 | gseDecode(buf, len); |
| sblair | 0:230c10b228ea | 152 | } |
| sblair | 0:230c10b228ea | 153 | else if (buf[3] == 0x04) { |
| sblair | 0:230c10b228ea | 154 | //SV: 01-0C-CD-04-00-00 to 01-0C-CD-04-01-FF |
| sblair | 0:230c10b228ea | 155 | svDecode(buf, len); |
| sblair | 0:230c10b228ea | 156 | } |
| sblair | 0:230c10b228ea | 157 | } |
| sblair | 0:230c10b228ea | 158 | } |
| sblair | 0:230c10b228ea | 159 | |
| sblair | 0:230c10b228ea | 160 | // copies bytes to network format (big-endian) |
| sblair | 0:230c10b228ea | 161 | void netmemcpy(void *dst, const void *src, unsigned int len) { |
| sblair | 0:230c10b228ea | 162 | #ifdef LITTLE_ENDIAN |
| sblair | 0:230c10b228ea | 163 | reversememcpy((unsigned char *) dst, (const unsigned char *) src, len); |
| sblair | 0:230c10b228ea | 164 | #else |
| sblair | 0:230c10b228ea | 165 | memcpy((unsigned char *) dst, (const unsigned char *) src, len); |
| sblair | 0:230c10b228ea | 166 | #endif |
| sblair | 0:230c10b228ea | 167 | } |
| sblair | 0:230c10b228ea | 168 | |
| sblair | 0:230c10b228ea | 169 | // copies bytes to host format (little-endian) |
| sblair | 0:230c10b228ea | 170 | void hostmemcpy(void *dst, const void *src, unsigned int len) { |
| sblair | 0:230c10b228ea | 171 | #ifdef LITTLE_ENDIAN |
| sblair | 0:230c10b228ea | 172 | memcpy((unsigned char *) dst, (const unsigned char *) src, len); |
| sblair | 0:230c10b228ea | 173 | #else |
| sblair | 0:230c10b228ea | 174 | reversememcpy((unsigned char *) dst, (const unsigned char *) src, len); |
| sblair | 0:230c10b228ea | 175 | #endif |
| sblair | 0:230c10b228ea | 176 | } |