Diff: PPP-Blinky/ppp-blinky.cpp

Revision:

171:46b36d4edd1d

Parent:

170:3d3b2126181c

Child:

172:449dd7a28955

--- a/PPP-Blinky/ppp-blinky.cpp	Mon Sep 04 05:45:59 2017 +0000
+++ b/PPP-Blinky/ppp-blinky.cpp	Mon Sep 04 08:49:25 2017 +0000
@@ -559,6 +559,54 @@
     dataCheckSum(pseudoHeader.start, 12, 1); // calculate this header's checksum
 }
 
+/// initialize an IP packet to send
+void initIP (unsigned int srcIp, unsigned int dstIp, unsigned int srcPort, unsigned int dstPort, unsigned int protocol)
+{
+    ppp.ppp->address = 0xff;
+    ppp.ppp->control = 3;
+    ppp.ppp->protocolR = __REV16( 0x0021 );
+    ppp.ip->version = 4;
+    ppp.ip->headerLength = 5; // 5 words = 20 bytes
+    ppp.ip->identR = __REV16(ppp.ipData.ident++); // insert our ident
+    ppp.ip->dontFragment=1;
+    ppp.ip->ttl=128;
+    ppp.ip->protocol = protocol; // udp
+    ppp.ip->srcAdrR = __REV(srcIp);
+    ppp.ip->dstAdrR = __REV(dstIp);
+    ppp.udpStart = ppp.ipStart + 20; // calculate start of udp header
+    ppp.udp->srcPortR = __REV16(srcPort); // source port
+    ppp.udp->dstPortR = __REV16(dstPort); // dest port
+}
+
+
+/// Build a UDP packet from scratch
+void sendUdp(unsigned int srcIp, unsigned int dstIp, unsigned int srcPort, unsigned int dstPort, char * message,int msgLen)
+{
+    struct {
+        unsigned int ipAll; // length of entire ip packet
+        unsigned int ipHeader; // length of ip header
+        unsigned int udpAll; // length of entire udp packet
+        unsigned int udpData; // length of udp data segment
+    } len;
+    len.ipHeader = 20; // ip header length
+    len.udpData = msgLen; // udp data size
+    len.udpAll = len.udpData+8; // update local udp packet length
+    len.ipAll = len.ipHeader + len.udpAll; // update IP Length
+    initIP(srcIp, dstIp, srcPort, dstPort, 17); // init a UDP packet
+    ppp.ip->lengthR = __REV16(len.ipAll); // update IP length in buffer
+    ppp.udpStart = ppp.ipStart + len.ipHeader; // calculate start of udp header
+    memcpy( ppp.udp->data, message, len.udpData ); // copy the message to the buffer
+    ppp.udp->lengthR = __REV16(len.udpAll); // update UDP length in buffer
+    ppp.pkt.len = len.ipAll+2+4; // update ppp packet length
+    IpHeaderCheckSum();  // refresh IP header checksum
+    checkSumPseudoHeader( len.udpAll ); // get the UDP pseudo-header checksum
+    ppp.udp->checksumR = 0; // before TCP checksum calculations the checksum bytes must be set cleared
+    unsigned int pseudoHeaderSum=dataCheckSum(ppp.udpStart,len.udpAll, 0); // continue the TCP checksum on the whole TCP packet
+    ppp.udp->checksumR = __REV16( pseudoHeaderSum); // tcp checksum done, store it in the TCP header
+    ppp.responseCounter++; // count UDP packet as a response
+    sendPppFrame(); // send the UDP message back
+}
+
 /// Process an incoming UDP packet.
 /// If the packet starts with the string "echo " or "test" we echo back a special packet
 void UDPpacket()
@@ -575,15 +623,15 @@
 
     ipLength.header = 4 * ppp.ip->headerLength; // length of ip header
     ppp.udpStart = ppp.ipStart + ipLength.header; // calculate start of udp header
-    udpLength.all = __REV16( ppp.udp->lengthR ); //((udpLen[0]<<8) | udpLen[1]); // size of udp packet
+    udpLength.all = __REV16( ppp.udp->lengthR ); // size of udp packet
     udpLength.data = udpLength.all - 8; // size of udp data
 
 #ifdef SERIAL_PORT_MONITOR_YES
-    char * srcIP        = ppp.ip->srcAdrPtr; //ipHeader+12; // IP source
-    char * dstIP        = ppp.ip->dstAdrPtr; //ipHeader+16; // IP destination
+    char * srcIP        = ppp.ip->srcAdrPtr; // IP source
+    char * dstIP        = ppp.ip->dstAdrPtr; //IP destination
 
-    unsigned int udpSrcPort = __REV16( ppp.udp->srcPortR ); //(udpSrcPort[0]<<8)|udpSrcPort[1]; // integer of UDP source port
-    unsigned int udpDstPort = __REV16( ppp.udp->dstPortR ); //(udpDstPort[0]<<8)|udpDstPort[1]; // integer of UDP dest port
+    unsigned int udpSrcPort = __REV16( ppp.udp->srcPortR ); // integer of UDP source port
+    unsigned int udpDstPort = __REV16( ppp.udp->dstPortR ); // integer of UDP dest port
 
     if(v0) {
         debugPrintf("UDP %d.%d.%d.%d:%d ", srcIP[0],srcIP[1],srcIP[2],srcIP[3],udpSrcPort);
@@ -607,35 +655,34 @@
     int echoFound = !strncmp(ppp.udp->data,"echo ",5); // true if UDP message starts with "echo "
     int testFound = !strncmp(ppp.udp->data,"test" ,4); // true if UDP message starts with "test"
     if ( (echoFound) || (testFound)) { // if the UDP message starts with "echo " or "test" we answer back
-        swapIpAddresses(); // swap IP source and destination
-        swapIpPorts(); // swap IP source and destination ports
         if (echoFound) {
+            swapIpAddresses(); // swap IP source and destination
+            swapIpPorts(); // swap IP source and destination ports
             memcpy(ppp.udp->data,"Got{",4); // in the UDP data modify "echo" to "Got:"
             int n=0;
             n=n+sprintf(n+ppp.udp->data+udpLength.data, "} UDP Server: PPP-Blinky\n"); // an appendix
             udpLength.data = udpLength.data + n; // update udp data size with the size of the appendix
-        }
-        if (testFound) {
-            int n=0;
-            n=n+sprintf(n+ppp.udp->data,"Response count %d\n",ppp.responseCounter++);
-            udpLength.data = n; // update udp data size
+            // we may have changed data length, update all the lengths
+            udpLength.all    = udpLength.data+8; // update local udp packet length
+            ipLength.all     = ipLength.header + udpLength.all; // update IP Length
+            ppp.ip->lengthR  = __REV16(ipLength.all); // update IP length in buffer
+            ppp.udp->lengthR = __REV16(udpLength.all); // update UDP length in buffer
+            ppp.pkt.len      = ipLength.all+2+4; // update ppp packet length
+            IpHeaderCheckSum();  // refresh IP header checksum
+            checkSumPseudoHeader( udpLength.all ); // get the UDP pseudo-header checksum
+            ppp.udp->checksumR = 0; // before TCP checksum calculations the checksum bytes must be set cleared
+            unsigned int pseudoHeaderSum=dataCheckSum(ppp.udpStart,udpLength.all, 0); // continue the TCP checksum on the whole TCP packet
+            ppp.udp->checksumR = __REV16( pseudoHeaderSum); // tcp checksum done, store it in the TCP header
+            ppp.responseCounter++; // count UDP packet as a response
+            sendPppFrame(); // send the UDP message back
+        } else if ( testFound ) {
+            unsigned int sI = __REV( ppp.ip->srcAdrR );
+            unsigned int dI = __REV( ppp.ip->dstAdrR );
+            unsigned int sp = __REV16( ppp.udp->srcPortR );
+            unsigned int dp = __REV16( ppp.udp->dstPortR );
+            int n=sprintf(ppp.pkt.buf+200,"Response Count %d\n", ppp.responseCounter++);
+            sendUdp(dI,sI,dp,sp,ppp.pkt.buf+200,n); // build a udp packet from the ground up
         }
-        // we may have changed data length, update all the lengths
-        udpLength.all    = udpLength.data+8; // update local udp packet length
-        ipLength.all     = ipLength.header + udpLength.all; // update IP Length
-        ppp.ip->lengthR  = __REV16(ipLength.all); // update IP length in buffer
-        ppp.udp->lengthR = __REV16(udpLength.all); // update UDP length in buffer
-        ppp.pkt.len      = ipLength.all+2+4; // update ppp packet length
-
-        IpHeaderCheckSum();  // refresh IP header checksum
-        checkSumPseudoHeader( udpLength.all ); // get the UDP pseudo-header checksum
-        ppp.udp->checksumR = 0; // before TCP checksum calculations the checksum bytes must be set cleared
-        unsigned int pseudoHeaderSum=dataCheckSum(ppp.udpStart,udpLength.all, 0); // continue the TCP checksum on the whole TCP packet
-        ppp.udp->checksumR = __REV16( pseudoHeaderSum); // tcp checksum done, store it in the TCP header
-        ppp.responseCounter++; // count UDP packet as a response
-        sendPppFrame(); // send the UDP message back
-    } else {
-        // do nothing with other incoming UDP packets
     }
 }
 
@@ -650,13 +697,11 @@
         unsigned int all; // length of entire udp packet
         unsigned int data; // length of udp data segment
     } icmpLength;
-
     ipLength.all = __REV16( ppp.ip->lengthR );  // length of ip packet
     ipLength.header = 4 * ppp.ip->headerLength; // length of ip header
     ppp.icmpStart = ppp.ipStart + ipLength.header; // calculate start of udp header
     icmpLength.all = ipLength.all - ipLength.header; // length of icmp packet
     icmpLength.data = icmpLength.all - 8; // length of icmp data
-
 #define ICMP_TYPE_PING_REQUEST 8
     if ( ppp.icmp->type == ICMP_TYPE_PING_REQUEST ) {
         ppp.ip->ttl--; // decrement time to live (so we have to update header checksum)
@@ -1019,7 +1064,7 @@
 
     dumpHeaderIP(1); // dump outgoing IP header before sending the frame
     dumpHeaderTCP(1); // dump outgoing TCP header before sending the frame
-    
+
     for (int i=0; i<45*1000/10; i++) { // 45 ms delay before sending frame - a typical internet delay time
         checkPc(); // catch any incoming characters
         wait_us(10); // wait less than 1 character duration at 115200