svDecodePacket.c

00001 /**
00002  * Rapid-prototyping protection schemes with IEC 61850
00003  *
00004  * Copyright (c) 2011 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 <stddef.h>
00027 
00028 
00029 void svDecodeASDU(unsigned char *buf, int len, int noASDU) {
00030     unsigned char tag;    // assumes only one byte is used
00031     int lengthFieldSize;
00032     int lengthValue;
00033     int offsetForNonSequence;
00034     unsigned char *svID = NULL;
00035     int svIDLength = 0;
00036 
00037     int i = 0;
00038     for (i = 0; i < len;) {
00039         tag = (unsigned char) buf[i];
00040         lengthFieldSize = getLengthFieldSize((unsigned char) buf[i + 1]);
00041         lengthValue = decodeLength((unsigned char *) &buf[i + 1]);
00042         offsetForNonSequence = 1 + lengthFieldSize + lengthValue;
00043 
00044         //printf("\ttag: %x, noASDU: %u, lengthFieldSize: %i, lengthValue: %i, offset: %i\n", tag, noASDU, lengthFieldSize, lengthValue, offsetForNonSequence);
00045 
00046         switch (tag) {
00047         case 0x80:
00048             svID = &buf[i + 1 + lengthFieldSize];
00049             svIDLength = lengthValue;
00050             break;
00051         case 0x81:
00052 
00053             break;
00054         case 0x82:
00055             // TODO: may be useful to store smpCnt value
00056             break;
00057         case 0x83:
00058 
00059             break;
00060         case 0x84:
00061 
00062             break;
00063         case 0x85:
00064 
00065             break;
00066         case 0x86:
00067 
00068             break;
00069         case 0x87:
00070             if (svID != NULL) {
00071                 svDecodeDataset(&buf[i + 1 + lengthFieldSize], lengthValue, noASDU, svID, svIDLength);
00072             }
00073             break;
00074         default:
00075             break;
00076         }
00077 
00078         i += offsetForNonSequence;
00079     }
00080 }
00081 
00082 void svDecodeAPDU(unsigned char *buf, int len) {
00083     unsigned char tag = (unsigned char) buf[0];    // assumes only one byte is used
00084     int lengthFieldSize = getLengthFieldSize((unsigned char) buf[1]);
00085     int lengthValue = decodeLength((unsigned char *) &buf[1]);
00086     int offsetForSequence = 1 + lengthFieldSize;
00087     int offsetForNonSequence = 1 + lengthFieldSize + lengthValue;
00088     //static unsigned int noASDU = 0;
00089     static unsigned int ASDU = 0;
00090 
00091     //printf("tag: %x, noASDU: %u, lengthFieldSize: %i, lengthValue: %i, offset: %i\n", tag, noASDU, lengthFieldSize, lengthValue, offsetForNonSequence);
00092 
00093     switch (tag) {
00094     case 0x60:
00095         svDecodeAPDU(&buf[offsetForSequence], lengthValue);
00096         break;
00097     case 0x80:
00098         //noASDU = (unsigned int) buf[1 + lengthFieldSize];    // assuming noASDU is < 126
00099         ASDU = 0;
00100         svDecodeAPDU(&buf[offsetForNonSequence], len - offsetForNonSequence);
00101         break;
00102     case 0xA2:
00103         svDecodeAPDU(&buf[offsetForSequence], lengthValue);
00104         break;
00105     case 0x30:
00106         svDecodeASDU(&buf[offsetForSequence], lengthValue, ASDU++);
00107 
00108         svDecodeAPDU(&buf[offsetForNonSequence], len - offsetForNonSequence);
00109         break;
00110     default:
00111         break;
00112     }
00113 }
00114 
00115 void svDecode(unsigned char *buf, int len) {
00116     unsigned short APDULength = ((buf[21] << 8) | buf[22]) - 8;    // must use length in PDU because total bytes (len) may contain CRC
00117 
00118     svDecodeAPDU(&buf[26], APDULength);    // cuts out frame header (fixed size of 26 bytes before start of APDU)
00119 }