svDecodePacket.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 "svDecodeBasic.h"
00022 #include "ied.h"
00023 #include "svDecode.h"
00024 #include "svPacketData.h"
00025 #include "decodePacket.h"
00026 #include "gseDecodeBasic.h"
00027 #include <stddef.h>
00028 
00029 
00030 void svDecodeASDU(unsigned char *buf, int len, int noASDU) {
00031     unsigned char tag;    // assumes only one byte is used
00032     int lengthFieldSize;
00033     int lengthValue;
00034     int offsetForNonSequence;
00035     unsigned char *svID = NULL;
00036     int svIDLength = 0;
00037     CTYPE_INT16U smpCnt = 0;
00038 
00039     int i = 0;
00040     for (i = 0; i < len;) {
00041         tag = (unsigned char) buf[i];
00042         lengthFieldSize = getLengthFieldSize((unsigned char) buf[i + 1]);
00043         lengthValue = decodeLength((unsigned char *) &buf[i + 1]);
00044         offsetForNonSequence = 1 + lengthFieldSize + lengthValue;
00045 
00046         //printf("\ttag: %x, noASDU: %u, lengthFieldSize: %i, lengthValue: %i, offset: %i\n", tag, noASDU, lengthFieldSize, lengthValue, offsetForNonSequence);
00047 
00048         switch (tag) {
00049             case SV_TAG_SVID:
00050                 svID = &buf[i + 1 + lengthFieldSize];
00051                 svIDLength = lengthValue;
00052                 break;
00053             case SV_TAG_DATSET:
00054 
00055                 break;
00056             case SV_TAG_SMPCNT:
00057                 ber_decode_integer(&buf[i + 1 + lengthFieldSize], lengthValue, &smpCnt, SV_GET_LENGTH_INT16U);
00058                 break;
00059             case SV_TAG_CONFREV:
00060 
00061                 break;
00062             case SV_TAG_REFRTM:
00063 
00064                 break;
00065             case SV_TAG_SMPSYNCH:
00066 
00067                 break;
00068             case SV_TAG_SMPRATE:
00069 
00070                 break;
00071             case SV_TAG_SEQUENCEOFDATA:
00072                 if (svID != NULL) {
00073                     svDecodeDataset(&buf[i + 1 + lengthFieldSize], lengthValue, noASDU, svID, svIDLength, smpCnt);
00074                 }
00075                 break;
00076             default:
00077                 break;
00078         }
00079 
00080         i += offsetForNonSequence;
00081     }
00082 }
00083 
00084 void svDecodeAPDU(unsigned char *buf, int len, unsigned int ASDU, unsigned int totalASDUs) {
00085     unsigned char tag = (unsigned char) buf[0];    // assumes only one byte is used
00086     int lengthFieldSize = getLengthFieldSize((unsigned char) buf[1]);
00087     int lengthValue = decodeLength((unsigned char *) &buf[1]);
00088     int offsetForSequence = 1 + lengthFieldSize;
00089     int offsetForNonSequence = 1 + lengthFieldSize + lengthValue;
00090     unsigned int noASDU = 0;
00091     //static unsigned int ASDU = 0;
00092 
00093     //printf("tag: %x, ASDU: %u,  totalASDUs: %u, lengthFieldSize: %i, lengthValue: %i, offset: %i\n", tag, ASDU, totalASDUs, lengthFieldSize, lengthValue, offsetForNonSequence);
00094 
00095     switch (tag) {
00096         case SV_TAG_SAVPDU:
00097             svDecodeAPDU(&buf[offsetForSequence], lengthValue, ASDU, totalASDUs);
00098             break;
00099         case SV_TAG_NOASDU:
00100             noASDU = (unsigned int) buf[1 + lengthFieldSize];    // assuming noASDU is < 126
00101             //ASDU = 0;
00102             svDecodeAPDU(&buf[offsetForNonSequence], len - offsetForNonSequence, ASDU, noASDU);
00103             break;
00104         case SV_TAG_SEQUENCEOFASDU:
00105             svDecodeAPDU(&buf[offsetForSequence], lengthValue, ASDU, totalASDUs);
00106             break;
00107         case SV_TAG_ASDU:
00108             svDecodeASDU(&buf[offsetForSequence], lengthValue, ASDU);
00109             ASDU++;
00110 
00111             // process any more ASDUs, until max number
00112             if (ASDU < totalASDUs) {
00113                 svDecodeAPDU(&buf[offsetForNonSequence], len - offsetForNonSequence, ASDU, totalASDUs);
00114             }
00115             break;
00116         default:
00117             break;
00118     }
00119 }
00120 
00121 void svDecode(unsigned char *buf, int len) {
00122     int offset = 16;   // start of 'length' field in payload
00123 
00124     // check for VLAN tag
00125     if (buf[12] == 0x81 && buf[13] == 0x00) {
00126         offset = 20;
00127     }
00128 
00129     unsigned short APDULength = ((buf[offset] << 8) | buf[offset + 1]) - 8;    // must use length in PDU because total bytes (len) may contain CRC
00130 
00131     svDecodeAPDU(&buf[offset + 6], APDULength, 0, 0);    // cuts out frame header
00132 }