svEncodePacket.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 "svPacketData.h"
00022 #include "svEncode.h"
00023 #include "gseEncode.h"
00024 #include "encodePacket.h"
00025 
00026 
00027 int svASDULength(struct svControl *svControl) {
00028     int len = 0;
00029 
00030     len += strlen((const char *) svControl->ASDU[0].svID) + 2;
00031     //printf("%i, %s\n", strlen(svControl->ASDU[0].svID), svControl->ASDU[0].svID);
00032     //len += strlen((const char *) svControl->ASDU[0].datset) + 2;
00033 
00034 #if SV_FIXED_SMPCNT_CONFREV_SIZE == 1
00035     len += SV_GET_LENGTH_INT16U + 2;    // smpCnt
00036     len += SV_GET_LENGTH_INT32U + 2;    // confRev
00037 #else
00038     len += ber_integer_length((&svControl->ASDU[0].smpCnt), SV_GET_LENGTH_INT16U)/*BER_GET_LENGTH_CTYPE_INT16U(&svControl->ASDU[0].smpCnt)*/ + 2;
00039     len += ber_integer_length((&svControl->ASDU[0].confRev), SV_GET_LENGTH_INT32U)/*BER_GET_LENGTH_CTYPE_INT32U(&svControl->ASDU[0].confRev)*/ + 2;
00040 #endif
00041     len += SV_GET_LENGTH_BOOLEAN + 2;
00042 
00043 
00044 
00045 
00046     //len += BER_GET_LENGTH_CTYPE_INT16U(&svControl->ASDU[0].smpRate) + 2;
00047     len += svControl->ASDU[0].data.size + getLengthBytes(svControl->ASDU[0].data.size);
00048     len++;
00049 
00050     return len;
00051 }
00052 
00053 int svSeqLength(struct svControl *svControl) {
00054     int len = svASDULength(svControl);
00055     len += getLengthBytes(len);
00056     len++;
00057     len = len * svControl->noASDU;    // assume all ASDUs are the same size
00058 
00059     return len;
00060 }
00061 
00062 int svAPDULength(struct svControl *svControl) {
00063     int len = svSeqLength(svControl);
00064     len += getLengthBytes(len);
00065     len++;
00066 
00067     len += 3;
00068 
00069     return len;
00070 }
00071 
00072 // creates an SV packet, including frame header. returns 0 on fail; number of bytes on success
00073 int svEncodePacket(struct svControl *svControl, unsigned char *buf) {
00074     int offset = 0;
00075     int len = svAPDULength(svControl);
00076     len += getLengthBytes(len);
00077     len += 9;        // savPdu tag size (1 byte), plus 8 "header" bytes
00078 
00079     // frame header
00080     memcpy(&buf[offset], svControl->ethHeaderData.destMACAddress, 6);    // destination MAC addresses
00081     offset += 6;
00082     memcpy(&buf[offset], LOCAL_MAC_ADDRESS, 6);                        // source MAC addresses
00083     offset += 6;
00084 
00085 #if SV_USE_VLAN == 1
00086     buf[offset++] = 0x81;    // TPID
00087     buf[offset++] = 0x00;
00088 
00089     netmemcpy(&buf[offset], &svControl->ethHeaderData.VLAN_ID, 2);    // TCI
00090     buf[offset] |= (svControl->ethHeaderData.VLAN_PRIORITY << 5);
00091     offset += 2;
00092 #endif
00093 
00094     buf[offset++] = 0x88;    // EtherType
00095     buf[offset++] = 0xBA;
00096 
00097     netmemcpy(&buf[offset], &svControl->ethHeaderData.APPID, 2);    // APPID
00098     offset += 2;
00099 
00100     netmemcpy(&buf[offset], &len, 2);    // length
00101     offset += 2;
00102 
00103     buf[offset++] = 0x00;    // reserved 1
00104     buf[offset++] = 0x00;
00105     buf[offset++] = 0x00;    // reserved 2
00106     buf[offset++] = 0x00;
00107 
00108     buf[offset++] = SV_TAG_SAVPDU;
00109     offset += encodeLength(&buf[offset], svAPDULength(svControl));
00110 
00111     buf[offset++] = SV_TAG_NOASDU;
00112     offset += encodeLength(&buf[offset], ber_integer_length(&svControl->noASDU, SV_GET_LENGTH_INT16U));
00113     offset += ber_encode_integer(&buf[offset], &svControl->noASDU, ber_integer_length(&svControl->noASDU, SV_GET_LENGTH_INT16U));
00114 
00115     buf[offset++] = SV_TAG_SEQUENCEOFASDU;
00116     offset += encodeLength(&buf[offset], svSeqLength(svControl));
00117 
00118     int i = 0;
00119     int size = 0;
00120     for (i = 0; i < svControl->noASDU; i++) {
00121         buf[offset++] = SV_TAG_ASDU;
00122         offset += encodeLength(&buf[offset], svASDULength(svControl));
00123 
00124         size = strlen((const char *) svControl->ASDU[i].svID);
00125         buf[offset++] = SV_TAG_SVID;
00126         buf[offset++] = size;
00127         memcpy(&buf[offset], svControl->ASDU[i].svID, size);
00128         offset += size;
00129 
00130 #if SV_OPTIONAL_SUPPORTED == 1
00131         if (svControl->ASDU[i].showDatset) {
00132             size = strlen(svControl->ASDU[i].datset);
00133             buf[offset++] = SV_TAG_DATSET;
00134             buf[offset++] = size;
00135             memcpy(&buf[offset], svControl->ASDU[i].datset, size);
00136             offset += size;
00137         }
00138 #endif
00139 
00140         buf[offset++] = SV_TAG_SMPCNT;
00141 #if SV_FIXED_SMPCNT_CONFREV_SIZE == 1
00142         buf[offset++] = SV_GET_LENGTH_INT16U;
00143         netmemcpy(&buf[offset], &svControl->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
00144         offset += SV_GET_LENGTH_INT16U;
00145 #else
00146         offset += encodeLength(&buf[offset], ber_integer_length(&svControl->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U));
00147         offset += ber_encode_integer(&buf[offset], &svControl->ASDU[i].smpCnt, SV_GET_LENGTH_INT16U);
00148 #endif
00149 
00150         buf[offset++] = SV_TAG_CONFREV;
00151 #if SV_FIXED_SMPCNT_CONFREV_SIZE == 1
00152         buf[offset++] = SV_GET_LENGTH_INT32U;
00153         netmemcpy(&buf[offset], &svControl->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
00154         offset += SV_GET_LENGTH_INT32U;
00155 #else
00156         offset += encodeLength(&buf[offset], ber_integer_length(&svControl->ASDU[i].confRev, SV_GET_LENGTH_INT32U));
00157         offset += ber_encode_integer(&buf[offset], &svControl->ASDU[i].confRev, SV_GET_LENGTH_INT32U);
00158 #endif
00159 
00160 #if SV_OPTIONAL_SUPPORTED == 1
00161         if (svControl->ASDU[i].showRefrTm) {
00162             buf[offset++] = SV_TAG_REFRTM;
00163             offset += encodeLength(&buf[offset], BER_GET_LENGTH_CTYPE_TIMESTAMP(&svControl->ASDU[i].refrTm));
00164             setTimestamp(&svControl->ASDU[i].refrTm);
00165             memcpy(&buf[offset], &svControl->ASDU[i].refrTm, BER_GET_LENGTH_CTYPE_TIMESTAMP(&svControl->ASDU[i].refrTm));
00166             offset += BER_GET_LENGTH_CTYPE_TIMESTAMP(&svControl->ASDU[i].refrTm);
00167         }
00168 #endif
00169 
00170         buf[offset++] = SV_TAG_SMPSYNCH;
00171         buf[offset++] = SV_GET_LENGTH_BOOLEAN;
00172         offset += ENCODE_CTYPE_BOOLEAN(&buf[offset], &svControl->ASDU[i].smpSynch);
00173 
00174 #if SV_OPTIONAL_SUPPORTED == 1
00175         if (svControl->ASDU[i].showSmpRate) {
00176             buf[offset++] = SV_TAG_SMPRATE;
00177             buf[offset++] = SV_GET_LENGTH_INT16U;
00178             offset += ENCODE_CTYPE_INT16U(&buf[offset], &svControl->ASDU[i].smpRate);
00179         }
00180 #endif
00181 
00182         buf[offset++] = SV_TAG_SEQUENCEOFDATA;
00183         offset += encodeLength(&buf[offset], svControl->ASDU[i].data.size);
00184         memcpy(&buf[offset], svControl->ASDU[i].data.data, svControl->ASDU[i].data.size);
00185         offset += svControl->ASDU[i].data.size;
00186     }
00187 
00188     // assume network interface, such as WinPcap, generates CRC bytes
00189 
00190     return offset;
00191 }