Diff: main.cpp

Revision:

26:11f4eb2663a7

Parent:

25:0b0450e1b08b

Child:

27:78d194dd8799

--- a/main.cpp	Mon Jan 02 19:36:21 2017 +0000
+++ b/main.cpp	Tue Jan 03 02:54:53 2017 +0000
@@ -32,6 +32,7 @@
 
 struct {
     int online; 
+    int ident;
     struct {
         char * buf;
         volatile int head; 
@@ -46,7 +47,16 @@
     } pkt; // ppp buffer
 } ppp;
 
-void pppInitStruct(){ ppp.online=0; ppp.rx.buf=rxbuf; ppp.rx.tail=0; ppp.rx.head=0; ppp.rx.total=0; ppp.pkt.buf=frbuf; ppp.pkt.len=0;}
+struct tcpType {
+    int connect;
+    int ack;
+    int seq;
+};
+
+tcpType tcp;
+    
+
+void pppInitStruct(){ ppp.online=0; ppp.rx.buf=rxbuf; ppp.rx.tail=0; ppp.rx.head=0; ppp.rx.total=0; ppp.pkt.buf=frbuf; ppp.pkt.len=0; ppp.ident=0;}
 
 int crcG; // frame check sequence (CRC) holder
 void crcDo(int x){for (int i=0;i<8;i++){crcG=((crcG&1)^(x&1))?(crcG>>1)^0x8408:crcG>>1;x>>=1;}} // crc calculator
@@ -130,6 +140,7 @@
 }
 
 void sendFrame(){
+    ppp.ident++;
     int crc = crcBuf(ppp.pkt.buf, ppp.pkt.len-2); // update crc
     ppp.pkt.buf[ ppp.pkt.len-2 ] = (~crc>>0); // fcs lo (crc)
     ppp.pkt.buf[ ppp.pkt.len-1 ] = (~crc>>8); // fcs hi (crc)
@@ -274,22 +285,193 @@
     debug(("IGMP type=%d \n", ppp.pkt.buf[28])); 
 }    
 
-void TCPpacket() {
-    debug(("TCP\n"));
-    /*
-    switch (protocol) {
-        case  2: TCPsyn();  break;
-        case 17: TCPack();   break;
-        case  6: TCPpacket();   break;
-        default: debug(((( "Other \n");
-    }        
-    */
+
+void dumpHeaderIP () {
+    char * ipPkt = ppp.pkt.buf+4; // ip packet start
+    char * version =    ipPkt; // top 4 bits
+    char * ihl =        ipPkt; // bottom 4 bits
+    char * dscp =       ipPkt+1; // top 6 bits
+    char * ecn =        ipPkt+1; // lower 2 bits
+    char * pktLen =     ipPkt+2; // 2 bytes
+    char * ident =      ipPkt+4; // 2 bytes
+    char * flags =      ipPkt+6; // 2 bits
+    //char * fragment =   ipPkt+6; // 14 bits
+    char * ttl =        ipPkt+8; // 1 byte
+    char * protocol =   ipPkt+9; // 1 byte
+    char * headercheck= ipPkt+10; // 2 bytes 
+    char * srcAdr =     ipPkt+12; // 4 bytes
+    char * dstAdr =     ipPkt+16; // 4 bytes = total of 20 bytes
+    
+    int versionIP = (version[0]>>4)&0xf; 
+    int headerSizeIP = (ihl[0]&0xf)*4;
+    int dscpIP = (dscp[0]>>2)&0x3f;
+    int ecnIP = ecn[0]&3;
+    int packetLength = (pktLen[0]<<8)|pktLen[1]; // ip total packet length
+    int identIP = (ident[0]<<8)|ident[1];
+    int flagsIP = flags[0]>>14&3;
+    //int fragmentIP = ((fragment[0]&0x3f)<<8)|fragment[1];
+    int ttlIP = ttl[0];
+    int protocolIP = protocol[0];
+    int checksumIP = (headercheck[0]<<8)|headercheck[1];
+    char srcIP [16]; snprintf(srcIP,16, "%d.%d.%d.%d", srcAdr[0],srcAdr[1],srcAdr[2],srcAdr[3]);
+    char dstIP [16]; snprintf(dstIP,16, "%d.%d.%d.%d", dstAdr[0],dstAdr[1],dstAdr[2],dstAdr[3]);
+    debug(("IP %s %s v%d h%d d%d e%d L%d ",srcIP,dstIP,versionIP,headerSizeIP,dscpIP,ecnIP,packetLength));
+    debug(("i%04x f%d t%d p%d C%04x\n",identIP,flagsIP,ttlIP,protocolIP,checksumIP));
+}    
+
+void dumpHeaderTCP() {
+    int ipHdrLen     = (ppp.pkt.buf[4]&0xf)*4; // overall length of ip packet
+    
+    char * s             = ppp.pkt.buf+4+ipHdrLen; // start of tcp packet
+    //char * srctcp        = s + 0;  // 2 bytes
+    //char * dsttcp        = s + 2;  // 2 bytes
+    char * seqtcp        = s + 4;  // 4 bytes
+    char * acktcp        = s + 8;  // 4 bytes
+    //char * offset        = s + 12; // 4 bits
+    char * flagbitstcp   = s + 12; // 9 bits
+    //char * winsizetcp    = s + 14; // 2 bytes
+    //char * checksumtcp   = s + 16; // 2 bytes
+    //char * urgentpointer = s + 18; // 2 bytes
+
+    //int srcPort = (srctcp[0]<<8)|srctcp[1];
+    //int dstPort = (dsttcp[0]<<8)|dsttcp[1];
+    int seq = (seqtcp[0]<<24)|(seqtcp[1]<<16)|(seqtcp[2]<<8)|(seqtcp[3]);
+    int ack = (acktcp[0]<<24)|(acktcp[1]<<16)|(acktcp[2]<<8)|(acktcp[3]);
+    //int tcpHdrSize = ((offset[0]>>4)&0x0f)*4; // size of tcp header only
+    int flags = ((flagbitstcp[0]&1)<<8)|flagbitstcp[1];
+    //int winsize = (winsizetcp[0]<<8)|winsizetcp[1];
+    //int checkTCP = (checksumtcp[0]<<8)|checksumtcp[1];
+    //int urgentTCP = (urgentpointer[0]<<8)|urgentpointer[1];
+    
+    int idx = 0; char flagInfo [40];
+    if (flags & (1<<0)) idx=snprintf(flagInfo+idx,40, "FIN "); if (flags & (1<<1)) idx=snprintf(flagInfo+idx,40, "SYN ");
+    if (flags & (1<<2)) idx=snprintf(flagInfo+idx,40, "RST "); if (flags & (1<<3)) idx=snprintf(flagInfo+idx,40, "PSH ");
+    if (flags & (1<<4)) idx=snprintf(flagInfo+idx,40, "ACK "); if (flags & (1<<5)) idx=snprintf(flagInfo+idx,40, "URG ");
+    if (flags & (1<<6)) idx=snprintf(flagInfo+idx,40, "ECE "); if (flags & (1<<7)) idx=snprintf(flagInfo+idx,40, "CWR ");
+    if (flags & (1<<8)) idx=snprintf(flagInfo+idx,40, "NS ");
+    
+    debug(("Flag %s Seq %08x Ack %08x ", flagInfo, seq, ack));
+    
 }    
 
-void otherProtocol() {
-    debug(("Other IP protocol"));
+void tcpHandler() {
+    
+    char * ipPkt = ppp.pkt.buf+4; // ip packet start
+    char * headercheck= ipPkt+10; // 2 bytes 
+    
+    char * ihl =        ipPkt;    // bottom 4 bits
+    char * ident =      ipPkt+4;  // 2 bytes
+    char * pktLen =     ipPkt+2;  // 2 bytes
+    char * protocol =   ipPkt+9; // 1 byte
+    char * srcAdr =     ipPkt+12; // 4 bytes
+    char * dstAdr =     ipPkt+16; // 4 bytes = total of 20 bytes
+    
+    int headerSizeIP = (ihl[0]&0xf)*4;
+    int packetLength = (pktLen[0]<<8)|pktLen[1]; // ip total packet length
+
+    ident[0] = ppp.ident>>8; ident[1] = ppp.ident>>0; // stuff in our ident
+    
+    char src[4]; char dst[4]; memcpy(src, srcAdr,4); memcpy(dst, dstAdr,4);
+    memcpy(srcAdr, dst,4); memcpy(dstAdr, src,4); // swap ip address source/dest
+    headercheck[0]=0; headercheck[1]=0; headerCheckSum(); // redo the tcp header checksum
+    
+    headercheck[0]=0; headercheck[1]=0; headerCheckSum(); // redo the tcp header checksum
+
+    int ipHdrLen     = (ppp.pkt.buf[4]&0xf)*4; // length of ip header
+    char * s             = ppp.pkt.buf+4+ipHdrLen; // start of tcp packet
+    char * srctcp        = s + 0;  // 2 bytes
+    char * dsttcp        = s + 2;  // 2 bytes
+    char * flagbitstcp   = s + 12; // 9 bits
+    char * checksumtcp   = s + 16; // 2 bytes
+
+    int tcpSize = packetLength - headerSizeIP;
+    int flagsTCP = ((flagbitstcp[0]&1)<<8)|flagbitstcp[1];
+    
+    int syncFound = 0;
+    #define TCP_FLAG_SYN 2
+    if ((flagsTCP & TCP_FLAG_SYN)!=0) syncFound=1;
+    
+    #define TCP_FLAG_ACK (1<<4) 
+    flagbitstcp[1] = TCP_FLAG_ACK; // change any flag to an ACK
+
+    char psrc[2]; char pdst[2]; memcpy(psrc, srctcp,2); memcpy(pdst, dsttcp,2);
+    memcpy(srctcp, pdst,2); memcpy(dsttcp, psrc,2); // swap ip port source/dest
+
+    char pseudoHeader[12]; int sum; char temp[12]; // for the terrible pseudoheader checksum
+    
+    memcpy( pseudoHeader+0, srcAdr, 8); // source and destination addresses.
+    pseudoHeader[8]=0; pseudoHeader[9]=protocol[0]; 
+    pseudoHeader[10]=tcpSize>>8; pseudoHeader[11]=tcpSize;
+    memcpy(temp, s-12, 12); // keep a copy
+    memcpy( s-12, pseudoHeader, 12); // put the header on the tcp packet
+    checksumtcp[0]=0; checksumtcp[1]=0; 
+    sum=dataCheckSum(s-12,tcpSize+12); // update TCP checksum
+    checksumtcp[0]=sum>>8; checksumtcp[1]=sum;
+    memcpy( s-12, temp, 12); // overwrite the pseudoheader
+    
+    sendFrame(); // return the TCP packet
+    
+    if (syncFound==0) return;
+    
+    flagbitstcp[1] = TCP_FLAG_SYN; // change the ACK to our SYN
+    
+    memcpy( pseudoHeader+0, srcAdr, 8); // source and destination addresses.
+    pseudoHeader[8]=0; pseudoHeader[9]=protocol[0]; 
+    pseudoHeader[10]=tcpSize>>8; pseudoHeader[11]=tcpSize;
+    memcpy(temp, s-12, 12); // keep a copy
+    memcpy( s-12, pseudoHeader, 12); // put the header on the tcp packet
+    checksumtcp[0]=0; checksumtcp[1]=0; 
+    sum=dataCheckSum(s-12,tcpSize+12); // update TCP checksum
+    checksumtcp[0]=sum>>8; checksumtcp[1]=sum;
+    memcpy( s-12, temp, 12); // overwrite the pseudoheader
+
+    sendFrame(); // send our request as a response to a SYN
+}    
+
+void dumpDataTCP() {
+    int ipPktLen     = (ppp.pkt.buf[6]<<8)|ppp.pkt.buf[7]; // overall length of ip packet
+    int ipHeaderLen  = (ppp.pkt.buf[4]&0xf)*4; // length of ip header
+    int tcpHeaderLen = ((ppp.pkt.buf[4+ipHeaderLen+12]>>4)&0xf)*4;; // length of tcp header
+    int dataLen = ipPktLen - ipHeaderLen - tcpHeaderLen; // data is what's left after the two headers
+    debug(("TCP %d ipHeader %d tcpHeader %d Data %d\n", ipPktLen, ipHeaderLen, tcpHeaderLen, dataLen));
+}    
+
+void TCPpacket(){
+    char * ipPkt = ppp.pkt.buf+4; // ip packet start
+    char * version =    ipPkt;    // top 4 bits
+    char * ihl =        ipPkt;    // bottom 4 bits
+    char * dscp =       ipPkt+1;  // top 6 bits
+    char * ecn =        ipPkt+1;  // lower 2 bits
+    char * pktLen =     ipPkt+2;  // 2 bytes
+    char * ident =      ipPkt+4;  // 2 bytes
+    char * flags =      ipPkt+6;  // 2 bits
+    char * ttl =        ipPkt+8;  // 1 byte
+    char * protocol =   ipPkt+9;  // 1 byte
+    char * headercheck= ipPkt+10; // 2 bytes 
+    char * srcAdr =     ipPkt+12; // 4 bytes
+    char * dstAdr =     ipPkt+16; // 4 bytes = total of 20 bytes
+    
+    int versionIP = (version[0]>>4)&0xf; 
+    int headerSizeIP = (ihl[0]&0xf)*4;
+    int dscpIP = (dscp[0]>>2)&0x3f;
+    int ecnIP = ecn[0]&3;
+    int packetLength = (pktLen[0]<<8)|pktLen[1]; // ip total packet length
+    int identIP = (ident[0]<<8)|ident[1];
+    int flagsIP = flags[0]>>14&3;
+    int ttlIP = ttl[0];
+    int protocolIP = protocol[0];
+    int checksumIP = (headercheck[0]<<8)|headercheck[1];
+    char srcIP [16]; snprintf(srcIP,16, "%d.%d.%d.%d", srcAdr[0],srcAdr[1],srcAdr[2],srcAdr[3]);
+    char dstIP [16]; snprintf(dstIP,16, "%d.%d.%d.%d", dstAdr[0],dstAdr[1],dstAdr[2],dstAdr[3]);
+    debug(("IP %s %s v%d h%d d%d e%d L%d ",srcIP,dstIP,versionIP,headerSizeIP,dscpIP,ecnIP,packetLength));
+    debug(("i%04x f%d t%d p%d C%04x\n",identIP,flagsIP,ttlIP,protocolIP,checksumIP));
+    dumpHeaderTCP();
+    dumpDataTCP();
+    tcpHandler();
 }
 
+void otherProtocol() { debug(("Other IP protocol")); }
+
 void IPframe() {
     int protocol = ppp.pkt.buf[13];
     switch (protocol) {
@@ -373,6 +555,8 @@
     }
 }
 
+int myIdent = 0;
+
 int main()
 {
     pc.baud(115200); // USB virtual serial port