Diff: main.cpp

Revision:

11:f58998c24f0b

Parent:

10:74f8233f72c0

Child:

12:db0dc91f0231

--- a/main.cpp	Sat Dec 31 03:13:46 2016 +0000
+++ b/main.cpp	Sat Dec 31 14:35:27 2016 +0000
@@ -99,11 +99,14 @@
     }
 }
 
+void dumpFrame() {
+    for(int i=0;i<ppp.pkt.len/2;i++) xx.printf("%02x ", ppp.pkt.buf[i]);
+    xx.printf(" C %02x %02x L=%d\n", ppp.pkt.crc&0xff, (ppp.pkt.crc>>8)&0xff, ppp.pkt.len);
+}
 
 void generalFrame() {
     debug("== General Frame ==\n");
-    for(int i=0;i<ppp.pkt.len;i++) xx.printf("%02x ", ppp.pkt.buf[i]);
-    xx.printf(" C %02x %02x L=%d\n", ppp.pkt.crc&0xff, (ppp.pkt.crc>>8)&0xff, ppp.pkt.len);
+    dumpFrame();
 }    
 
 void sendFrame(){
@@ -112,6 +115,7 @@
     ppp.pkt.buf[ ppp.pkt.len-1 ] = (~crc>>8); // fcs hi
     pc.putc(0x7e); // frame flag
     for(int i=0;i<ppp.pkt.len;i++) {
+        //xx.printf( "%2x ", ppp.pkt.buf[i]);
         unsigned int cc = (unsigned int)ppp.pkt.buf[i];
         if (cc>32) pc.putc(cc); else {pc.putc(0x7d); pc.putc(cc+32);}
     } 
@@ -155,49 +159,120 @@
 }    
 
 void UDPpacket() {
-    xx.printf("UDP %d.%d.%d.%d %d.%d.%d.%d\n", ppp.pkt.buf[16],ppp.pkt.buf[17],ppp.pkt.buf[18],ppp.pkt.buf[19],ppp.pkt.buf[20],ppp.pkt.buf[21],ppp.pkt.buf[22],ppp.pkt.buf[23] );
     int idx = 4+((ppp.pkt.buf[4]&0xf)*4);
     int srcPort =  (ppp.pkt.buf[idx+0]<<8)|ppp.pkt.buf[idx+1];
-    int destPort = (ppp.pkt.buf[idx+2]<<8)|ppp.pkt.buf[idx+3];
-    xx.printf("Src %04x Dest %04x\n", srcPort, destPort);
+    int dstPort = (ppp.pkt.buf[idx+2]<<8)|ppp.pkt.buf[idx+3];
+    xx.printf("UDP %d.%d.%d.%d:%d ", ppp.pkt.buf[16],ppp.pkt.buf[17],ppp.pkt.buf[18],ppp.pkt.buf[19],srcPort);
+    xx.printf("%d.%d.%d.%d:%d\n",    ppp.pkt.buf[20],ppp.pkt.buf[21],ppp.pkt.buf[22],ppp.pkt.buf[23],dstPort);
+}    
+
+
+int dataCheckSum(char * ptr, int len) {
+    int sum=0;
+
+    for (int i=0;i<len/2;i++) {
+        int hi = *ptr; ptr++;
+        int lo = *ptr; ptr++;
+        int val = ( lo & 0xff ) | ( (hi<<8) & 0xff00 );
+        sum = sum + val;
+    }
+    sum = sum + (sum>>16);
+    sum = ~sum;
+    return sum;
+}    
+
+
+void headerCheckSum() {
+    int len =(ppp.pkt.buf[4]&0xf)*4; // length of header in bytes
+    char * ptr = ppp.pkt.buf+4; // start of ip packet
+    int sum=0;
+
+    for (int i=0;i<len/2;i++) {
+        int hi = *ptr; ptr++;
+        int lo = *ptr; ptr++;
+        int val = ( lo & 0xff ) | ( (hi<<8) & 0xff00 );
+        sum = sum + val;
+    }
+    sum = sum + (sum>>16);
+    sum = ~sum;
+    ppp.pkt.buf[14]= (sum>>8);
+    ppp.pkt.buf[15]= (sum   );
+    
+    
+    
 }    
 
-void ICMPpacket() {
-    xx.printf("ICMP\n"); 
+void ICMPpacket() { // internet control message protocol
+ 
+    int headerSize = ((ppp.pkt.buf[4]&0xf)*4);
+    int idx = headerSize + 4;
+    xx.printf("ICMP type=%d \n", ppp.pkt.buf[idx]); 
+    if ( ppp.pkt.buf[idx] == 8 ) { 
+        ppp.pkt.buf[12] = ppp.pkt.buf[12]-1; // TTL decrement
+        char src[4]; char dst[4];
+        memcpy(src, ppp.pkt.buf+16,4);
+        memcpy(dst, ppp.pkt.buf+20,4);
+        memcpy(ppp.pkt.buf+16,dst,4);
+        memcpy(ppp.pkt.buf+20,src,4); // swap src & dest ip
+        ppp.pkt.buf[15]=0; 
+        ppp.pkt.buf[14]=0;
+        headerCheckSum();  // calc ip header checksum
+
+
+        int packetLength = (ppp.pkt.buf[6]<<8)|ppp.pkt.buf[7];
+        int dataLength = packetLength - headerSize;
+        ppp.pkt.buf[idx]=0; ppp.pkt.buf[idx+2]=0; ppp.pkt.buf[idx+3]=0;
+        int sum = dataCheckSum( ppp.pkt.buf+idx, dataLength); // calc icmp data checksum
+        ppp.pkt.buf[idx+2]=(sum>>8); ppp.pkt.buf[idx+3]=(sum   );
+        sendFrame();
+    }
+}
+
+void IGMPpacket() { // internet group management protocol
+    xx.printf("IGMP type=%d \n", ppp.pkt.buf[28]); 
+}    
+
+void TCPpacket() {
+    debug("TCP\n");
     /*
-    xx.printf("ICMP len %4d %d.%d.%d.%d  %d.%d.%d.%d\n", (ppp.pkt.buf[6]<<8)+ppp.pkt.buf[7],ppp.pkt.buf[16],ppp.pkt.buf[17],ppp.pkt.buf[18],ppp.pkt.buf[19],ppp.pkt.buf[20],ppp.pkt.buf[21],ppp.pkt.buf[22],ppp.pkt.buf[23] );
-    xx.printf("Byte0 = %02x\n", ppp.pkt.buf[4]);
-    xx.printf("Type %x Code %d\n", ppp.pkt.buf[28], ppp.pkt.buf[29]);
-    xx.printf("Number of records %d %d\n", ppp.pkt.buf[34],ppp.pkt.buf[35]);
-    xx.printf("Group %d.%d.%d.%d\n", ppp.pkt.buf[36],ppp.pkt.buf[37],ppp.pkt.buf[38],ppp.pkt.buf[39]);
+    switch (protocol) {
+        case  2: TCPsyn();  break;
+        case 17: TCPack();   break;
+        case  6: TCPpacket();   break;
+        default: debug( "Other \n");
+    }        
     */
-    
 }    
 
+void otherProtocol() {
+    debug("Other protocol");
+}
+
 void IPframe() {
     int protocol = ppp.pkt.buf[13];
     switch (protocol) {
-        case  2: ICMPpacket();  break;
-        case 17: UDPpacket();   break;
-        case  6: debug( "TCP   \n"  );   break;
-        default: debug( "Other \n");
+        case    1: ICMPpacket();  break;
+        case    2: IGMPpacket();  break;
+        case   17: UDPpacket();   break;
+        case    6: TCPpacket();   break;
+        default: otherProtocol();
     }        
     //xx.printf("IP frame proto %3d len %4d %d.%d.%d.%d  %d.%d.%d.%d\n", ppp.pkt.buf[13],(ppp.pkt.buf[6]<<8)+ppp.pkt.buf[7],ppp.pkt.buf[16],ppp.pkt.buf[17],ppp.pkt.buf[18],ppp.pkt.buf[19],ppp.pkt.buf[20],ppp.pkt.buf[21],ppp.pkt.buf[22],ppp.pkt.buf[23] );
 }    
 
 
 void LCPconfReq() {
-    debug("LCP Conf ");
+    debug("LCP Config ");
     if (ppp.pkt.buf[7] != 4) {
-        ppp.pkt.buf[4]=4; // allow only minimal requests
-        debug("Rej\n");
+        ppp.pkt.buf[4]=4; // allow only no options
+        debug("Reject\n");
         sendFrame(); 
     } else {
         ppp.pkt.buf[4]=2; // ack zero conf
         debug("Ack\n");
         sendFrame();
         debug("LCP Ask\n");
-        ppp.pkt.buf[4]=1; // request zero conf
+        ppp.pkt.buf[4]=1; // request no options
         sendFrame();
     }
 }