Tools/NetTest_Runner/tcp.c

   1 /*
   2  */
   3 #include "common.h"
   4 #include "tcp.h"
   5 #include "ip.h"
   6 #include "test.h"       // TEST_ASSERT
   7 #include <string.h>
   8
   9 typedef struct {
  10         uint16_t        SPort;
  11         uint16_t        DPort;
  12         uint32_t        Seq;
  13         uint32_t        Ack;
  14         uint8_t         DataOfs;
  15         uint8_t         Flags;
  16         uint16_t        Window;
  17         uint16_t        Checksum;
  18         uint16_t        UrgPtr;
  19 } __attribute__((packed)) tTCPHeader;
  20
  21 // === CODE ===
  22 uint16_t TCP_int_GetPseudoHeader(int AF, const void *SrcAddr, const void *DstAddr, uint8_t pctl, size_t Len)
  23 {
  24         if( AF == 4 ) {
  25                 uint8_t phdr[12];
  26                 memcpy(phdr+0, SrcAddr, 4);
  27                 memcpy(phdr+4, DstAddr, 4);
  28                 phdr[8] = 0;
  29                 phdr[9] = pctl;
  30                 *(uint16_t*)(phdr+10) = htons(Len);
  31
  32                 //test_trace_hexdump("TCP IPv4 PHdr", phdr, sizeof(phdr));
  33
  34                 return IP_Checksum(IP_CHECKSUM_START, 12, phdr);
  35         }
  36         else {
  37                 TEST_WARN("TCP unknown AF %i", AF);
  38                 return 0;
  39         }
  40 }
  41
  42 void TCP_SendC(const tTCPConn *Conn, uint8_t flags, size_t data_len, const void *data)
  43 {
  44         TCP_Send(Conn->IFNum, Conn->AF, Conn->RAddr, Conn->LPort, Conn->RPort,
  45                 Conn->LSeq, Conn->RSeq, flags, Conn->Window, data_len, data);
  46 }
  47 void TCP_Send(int IF, int AF, const void *IP, uint16_t sport, uint16_t dport,
  48         uint32_t seq, uint32_t ack, uint8_t flags, uint16_t window,
  49         size_t data_len, const void *data
  50         )
  51 {
  52         tTCPHeader      hdr;
  53         hdr.SPort = htons(sport);
  54         hdr.DPort = htons(dport);
  55         hdr.Seq = htonl(seq);
  56         hdr.Ack = htonl(ack);
  57         hdr.DataOfs = (sizeof(hdr)/4) << 4;
  58         hdr.Flags = flags;
  59         hdr.Window = htons(window);
  60         hdr.Checksum = htons(0);
  61         hdr.UrgPtr = htons(0);
  62
  63         uint16_t        checksum;
  64         checksum = TCP_int_GetPseudoHeader(AF, BLOB(HOST_IP), IP, IPPROTO_TCP, sizeof(hdr)+data_len);
  65         checksum = IP_Checksum(checksum, sizeof(hdr), &hdr);
  66         checksum = IP_Checksum(checksum, data_len, data);
  67         hdr.Checksum = htons( checksum );
  68
  69         size_t  buflens[] = {sizeof(hdr), data_len};
  70         const void *bufs[] = {&hdr, data};
  71         IP_Send(IF, AF, BLOB(HOST_IP), IP, IPPROTO_TCP, 2, buflens, bufs);
  72 }
  73
  74 struct {
  75         bool    Seq;
  76         bool    Ack;
  77         bool    SPort;
  78 }       gTCP_Skips;
  79
  80 void TCP_SkipCheck_Seq(bool Skip) {
  81         gTCP_Skips.Seq = Skip;
  82 }
  83
  84
  85 bool TCP_Pkt_CheckC(size_t len, const void *data, size_t *out_ofs, size_t *len_out,
  86         const tTCPConn *conn, uint8_t flags)
  87 {
  88         return TCP_Pkt_Check(len, data, out_ofs, len_out,
  89                 conn->AF, conn->RAddr, conn->RPort, conn->LPort, conn->RSeq, conn->LSeq, flags
  90                 );
  91 }
  92
  93 bool TCP_Pkt_Check(size_t len, const void *data, size_t *out_ofs, size_t *len_out,
  94         int AF, const void *IP, uint16_t sport, uint16_t dport,
  95         uint32_t seq, uint32_t ack, uint8_t flags)
  96 {
  97         size_t  ofs, rlen;
  98         if( !IP_Pkt_Check(len, data, &ofs, &rlen, AF, IP, BLOB(HOST_IP), IPPROTO_TCP) )
  99                 return false;
 100
 101         tTCPHeader      hdr;
 102         TEST_ASSERT_REL(rlen, >=, sizeof(hdr));
 103         memcpy(&hdr, (char*)data + ofs, sizeof(hdr));
 104
 105         TEST_ASSERT_REL( hdr.DataOfs >> 4, >=, sizeof(hdr)/4 );
 106         if( !gTCP_Skips.SPort ) TEST_ASSERT_REL( ntohs(hdr.SPort), ==, sport );
 107         TEST_ASSERT_REL( ntohs(hdr.DPort), ==, dport );
 108         TEST_ASSERT_REL( hdr.Flags, ==, flags);
 109         if( !gTCP_Skips.Seq )   TEST_ASSERT_REL( ntohl(hdr.Seq), ==, seq );
 110         if( flags & TCP_ACK )   TEST_ASSERT_REL( ntohl(hdr.Ack), ==, ack );
 111
 112         uint16_t        real_cksum = htons(hdr.Checksum);
 113         hdr.Checksum = 0;
 114         uint16_t        calc_cksum;
 115         calc_cksum = TCP_int_GetPseudoHeader(AF, IP, BLOB(HOST_IP), IPPROTO_TCP, rlen);
 116         calc_cksum = IP_Checksum(calc_cksum, sizeof(hdr), &hdr);
 117         calc_cksum = IP_Checksum(calc_cksum, rlen - sizeof(hdr), (char*)data+ofs+sizeof(hdr));
 118         TEST_ASSERT_REL( real_cksum, ==, calc_cksum );
 119
 120         memset(&gTCP_Skips, 0, sizeof(gTCP_Skips));
 121
 122         *out_ofs = ofs + sizeof(hdr);
 123         *len_out = rlen - sizeof(hdr);
 124         return true;
 125 }
 126
 127 uint32_t TCP_Pkt_GetSeq(size_t len, const void *data, int AF)
 128 {
 129         size_t  ofs, rlen;
 130         IP_Pkt_Check(len, data, &ofs, &rlen, AF, NULL, NULL, IPPROTO_TCP);
 131
 132         tTCPHeader      hdr;
 133         memcpy(&hdr, (char*)data + ofs, sizeof(hdr));
 134         return ntohl(hdr.Seq);
 135 }
 136