Diff: main.cpp

Revision:

12:db0dc91f0231

Parent:

11:f58998c24f0b

Child:

13:d882b8a042b4

--- a/main.cpp	Sat Dec 31 14:35:27 2016 +0000
+++ b/main.cpp	Sun Jan 01 01:20:33 2017 +0000
@@ -1,5 +1,7 @@
 #include "mbed.h"
 
+// Copyright 2016 Nicolas Nackel. Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+
 // Proof-of-concept for TCP/IP using Windows 7/8/10 Dial Up Networking over MBED USB Virtual COM Port
 
 // Toggles LED1 every time the PC sends an IP packet over the PPP link
@@ -83,7 +85,7 @@
         if (unstuff==0) {
             if (rxbuf[i]==0x7d) unstuff=1; 
             else { *dest++ = rxbuf[i]; ppp.pkt.len++; crcDo(rxbuf[i]);}
-        } else {
+        } else { // unstuff
             *dest++ = rxbuf[i]^0x20; ppp.pkt.len++; crcDo((int)rxbuf[i]^0x20);
             unstuff=0;
         }
@@ -92,10 +94,10 @@
     if (ppp.pkt.crc == 0xf0b8) { // check for good CRC
         void determinePacketType(); // declare early
         determinePacketType();
-    } else {
-         xx.printf("CRC was %x \n",ppp.pkt.crc);
+    } else { // crc error
+         xx.printf("CRC is %x Len is %d\n",ppp.pkt.crc,ppp.pkt.len);
          for(int i=0;i<ppp.pkt.len;i++)xx.printf("%02x ", ppp.pkt.buf[i]);
-         xx.printf("\nLen = %d\n", ppp.pkt.len);
+         xx.printf("\n");
     }
 }
 
@@ -104,22 +106,17 @@
     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");
-    dumpFrame();
-}    
-
 void sendFrame(){
     int crc = crcBuf(ppp.pkt.buf, ppp.pkt.len-2); // get crc
-    ppp.pkt.buf[ ppp.pkt.len-2 ] = (~crc>>0); // fcs lo
-    ppp.pkt.buf[ ppp.pkt.len-1 ] = (~crc>>8); // fcs hi
-    pc.putc(0x7e); // frame flag
+    ppp.pkt.buf[ ppp.pkt.len-2 ] = (~crc>>0); // fcs lo (crc)
+    ppp.pkt.buf[ ppp.pkt.len-1 ] = (~crc>>8); // fcs hi (crc)
+    pc.putc(0x7e); // frame start 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);}
     } 
-    pc.putc(0x7e); // frame flag
+    pc.putc(0x7e); // frame end flag
 }
 
 void ipRequestHandler(){
@@ -132,9 +129,9 @@
         debug("Ack\n");
         ppp.pkt.buf[4]=2; // ack the minimum
         sendFrame(); // acknowledge
+        debug("IPCP Ask\n"); 
         // send our own request now
-        debug("IPCP Ask\n");
-        ppp.pkt.buf[4]=1; // request the minimum
+        ppp.pkt.buf[4]=1; // request no options
         ppp.pkt.buf[5]++; // next sequence
         sendFrame(); // this is our request
     }
@@ -159,17 +156,32 @@
 }    
 
 void UDPpacket() {
-    int idx = 4+((ppp.pkt.buf[4]&0xf)*4);
-    int srcPort =  (ppp.pkt.buf[idx+0]<<8)|ppp.pkt.buf[idx+1];
-    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);
-}    
-
+    char * udpPkt = ppp.pkt.buf+4; // udp packet start
+    //char * pktLen = udpPkt+2; // total packet length
+    int headerSize = (( udpPkt[0]&0xf)*4);
+    char * udpBlock = udpPkt + headerSize; // udp info start 
+    char * udpSrc = udpBlock; // source port
+    char * udpDst = udpBlock+2; // destination port
+    char * udpLen = udpBlock+4; // udp data length
+    char * udpInf = udpBlock+8; // actual start of info
+    //char * udpSum = udpBlock+6; // udp checksum
+    //int packetLength = (pktLen[0]<<8) | pktLen[1]; // udp total packet length
+    int srcPort = (udpSrc[0]<<8) | udpSrc[1];
+    int dstPort = (udpDst[0]<<8) | udpDst[1];
+    char * srcIP = udpPkt+12; // udp src addr
+    char * dstIP = udpPkt+16; // udp dst addr
+    #define UDP_HEADER_SIZE 8
+    int udpLength = ((udpLen[0]<<8) | udpLen[1]) - UDP_HEADER_SIZE; // size of the actual udp data
+    xx.printf("UDP %d.%d.%d.%d:%d ", srcIP[0],srcIP[1],srcIP[2],srcIP[3],srcPort);
+    xx.printf("%d.%d.%d.%d:%d ",    dstIP[1],dstIP[1],dstIP[1],dstIP[1],dstPort);
+    xx.printf("Len %d ", udpLength);
+    int printSize = udpLength > 20 ? 20 : udpLength;
+    for (int i=0; i<printSize; i++) { xx.putc( udpInf[i]); } // printf first 20 characters of the data
+    xx.printf("\n");
+}
 
 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++;
@@ -177,11 +189,9 @@
         sum = sum + val;
     }
     sum = sum + (sum>>16);
-    sum = ~sum;
-    return 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
@@ -197,34 +207,40 @@
     sum = ~sum;
     ppp.pkt.buf[14]= (sum>>8);
     ppp.pkt.buf[15]= (sum   );
-    
-    
-    
 }    
 
 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 * ipPkt = ppp.pkt.buf+4; // ip packet start
+    char * pktLen = ipPkt+2;
+    int packetLength = (pktLen[0]<<8) | pktLen[1]; // icmp packet length
+    int headerSize = (( ipPkt[0]&0xf)*4);
+    char * icmpType = ipPkt + headerSize; // icmp data start
+    char * icmpData = icmpType; // icmp packet type
+    char * icmpSum = icmpData+2; // icmp checksum
+    #define ICMP_TYPE_PING_REQUEST 8
+    if ( icmpType[0] == ICMP_TYPE_PING_REQUEST ) { 
+        xx.printf("ICMP PING\n");
+        char * ipTTL = ipPkt+8; // time to live
+        ipTTL[0]--; // decrement time to live
+        char * srcAdr = ipPkt+12;
+        char * dstAdr = ipPkt+16;
         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   );
+        memcpy(src, srcAdr,4);
+        memcpy(dst, dstAdr,4);
+        memcpy(srcAdr, dst,4);
+        memcpy(dstAdr, src,4); // swap src & dest ip
+        char * chkSum = ipPkt+10;
+        chkSum[0]=0; chkSum[1]=0;
+        headerCheckSum();  // new ip header checksum
+        #define ICMP_TYPE_ECHO_REPLY 0
+        icmpType[0]=ICMP_TYPE_ECHO_REPLY; // icmp echo replay
+        icmpSum[0]=0; icmpSum[1]=0; // zero the checksum for recalculation
+        int dataLength = packetLength - headerSize; // length of ICMP data portion
+        int sum = dataCheckSum( icmpData, dataLength); // new data portion checksum
+        icmpSum[0]=sum>>8; icmpSum[1]=sum; // new checksum for ICMP data portion
         sendFrame();
+    } else {
+        xx.printf("ICMP type=%d \n", icmpType[0]); 
     }
 }
 
@@ -245,7 +261,7 @@
 }    
 
 void otherProtocol() {
-    debug("Other protocol");
+    debug("Other IP protocol");
 }
 
 void IPframe() {
@@ -260,7 +276,6 @@
     //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 Config ");
     if (ppp.pkt.buf[7] != 4) {
@@ -283,28 +298,28 @@
 
 void LCPend(){
      debug("LCP End\n");
-     ppp.online=0; 
+     ppp.online=0; // start hunting for connect string again
      ppp.pkt.buf[4]=6;
-     sendFrame();
+     sendFrame(); // acknowledge
 }
 
 void LCPother(){
      debug("LCP Other\n");
-     generalFrame();
+     dumpFrame();
 }
 
 void LCPframe(){
      int code = ppp.pkt.buf[4];
      switch (code) {
-         case 1: LCPconfReq(); break;
-         case 2: LCPconfAck(); break;
-         case 5: LCPend(); break;
-         default: LCPother();
+         case 1:    LCPconfReq(); break; // config request
+         case 2:    LCPconfAck(); break; // config ack
+         case 5:    LCPend();     break; // end connection
+         default:   LCPother();
      }
 }
 
-void xx21frame() {
-    debug("Unknown frame type ff 03 xx 21\n");
+void discardedFrame() {
+    xx.printf("Dropping frame %02x %02x %02x %02x\n", ppp.pkt.buf[0],ppp.pkt.buf[1],ppp.pkt.buf[2],ppp.pkt.buf[3] );
 }
 
 void determinePacketType() {
@@ -313,10 +328,10 @@
     if ( ppp.pkt.buf[3] != 0x21 ) {debug("byte2 != 21\n"); return;}
     int packetType = ppp.pkt.buf[2];
     switch (packetType) {
-        case 0xc0: LCPframe(); break; 
-        case 0x80: IPCPframe(); break;
-        case 0x00: IPframe(); break;
-        default: xx21frame(); break;
+        case 0xc0:  LCPframe();     break;  // link control
+        case 0x80:  IPCPframe();    break;  // IP control
+        case 0x00:  IPframe();      break;  // IP itself
+        default:    discardedFrame();
     }
 }    
 
@@ -334,7 +349,7 @@
 
 int main()
 {
-    pc.baud(9600);
+    pc.baud(115200);
     xx.baud(115200); 
     xx.puts("\x1b[2J\x1b[HReady\n"); // VT100 code for clear screen & home