Tools/NetTest_Runner/ip.c

   1 /*
   2  */
   3 #include "common.h"
   4 #include "ip.h"
   5 #include "link.h"
   6 #include <assert.h>
   7 #include <string.h>
   8 #include "test.h"
   9
  10 // === STRUCTURES ===
  11 typedef struct {
  12         uint8_t         VerLen;
  13         uint8_t         DiffServices;
  14         uint16_t        TotalLength;
  15         uint16_t        Identifcation;
  16         uint16_t        FragmentInfo;   // rsvd:dont:more:[13]offset/8
  17         uint8_t         TTL;
  18         uint8_t         Protocol;
  19         uint16_t        HeaderChecksum;
  20         uint8_t         SrcAddr[4];
  21         uint8_t         DstAddr[4];
  22         uint8_t         Options[];
  23 } __attribute__((packed)) tIPv4Hdr;
  24
  25 // === CODE ===
  26 uint16_t IP_Checksum(uint16_t Prev, size_t Length, const void *Data)
  27 {
  28         //test_trace_hexdump("IP Checksum", Data, Length);
  29
  30         const uint16_t  *words = Data;
  31         uint32_t        ret = 0;
  32         for( int i = 0; i < Length/2; i ++ )
  33         {
  34                 ret += ntohs(*words);
  35                 words ++;
  36         }
  37         if( Length & 1 )
  38                 ret += ntohs(*(uint8_t*)words);
  39
  40         while( ret >> 16 )
  41                 ret = (ret & 0xFFFF) + (ret >> 16);
  42
  43         //test_trace("IP Checksum = %04x + 0x%x", ret, (~Prev) & 0xFFFF);
  44
  45         ret += (~Prev) & 0xFFFF;
  46         while( ret >> 16 )
  47                 ret = (ret & 0xFFFF) + (ret >> 16);
  48
  49         return ~ret;
  50 }
  51
  52 void IP_Send(int IfNum, int AF, const void *Src, const void *Dst, uint8_t proto,
  53         int BufCount, size_t BufLens[], const void *Bufs[])
  54 {
  55         size_t  total_length = 0;
  56
  57         size_t  out_buflens[BufCount+1];
  58         const void *out_bufs[BufCount+1];
  59
  60         for( int i = 0; i < BufCount; i ++ )
  61         {
  62                 total_length += BufLens[i];
  63                 out_buflens[1+i] = BufLens[i];
  64                 out_bufs[1+i] = Bufs[i];
  65         }
  66
  67         if( AF == 4 )
  68         {
  69                 tIPv4Hdr        hdr;
  70                 hdr.VerLen = (4 << 4) | (sizeof(hdr)/4);
  71                 hdr.DiffServices = 0;
  72                 hdr.TotalLength = htons(sizeof(hdr) + total_length);
  73                 hdr.Identifcation = 0;  // TODO: WTF is this?
  74                 hdr.FragmentInfo = htons(0);
  75                 hdr.TTL = 18;   // TODO: Progammable TTL
  76                 hdr.Protocol = proto;
  77                 hdr.HeaderChecksum = 0;
  78                 memcpy(hdr.SrcAddr, Src, 4);
  79                 memcpy(hdr.DstAddr, Dst, 4);
  80
  81                 hdr.HeaderChecksum = htons( IP_Checksum(IP_CHECKSUM_START, sizeof(hdr), &hdr) );
  82
  83                 out_buflens[0] = sizeof(hdr);
  84                 out_bufs[0] = &hdr;
  85
  86                 // TODO: Don't hard-code MAC addresses
  87                 Link_Send(IfNum, BLOB(HOST_MAC), BLOB(TEST_MAC), ETHER_PROTO_IPV4,
  88                         1+BufCount, out_buflens, out_bufs);
  89         }
  90         else {
  91                 TEST_WARN("Invalid AF(%i) in IP_Send", AF);
  92         }
  93 }
  94
  95 bool IP_Pkt_Check(size_t len, const void *data, size_t *ofs_out, int AF, const void *Src, const void *Dst, uint8_t proto)
  96 {
  97         size_t  ofs;
  98         if( AF == 4 ) {
  99                 if( !Link_Pkt_Check(len, data, &ofs, BLOB(TEST_MAC), BLOB(HOST_MAC), ETHER_PROTO_IPV4) )
 100                         return false;
 101
 102                 tIPv4Hdr hdr;
 103                 memcpy(&hdr, (const uint8_t*)data + ofs, sizeof(hdr));
 104                 TEST_ASSERT_REL(hdr.VerLen >> 4, ==, 4);
 105                 TEST_ASSERT_REL(IP_Checksum(IP_CHECKSUM_START, sizeof(hdr), &hdr), ==, 0);
 106
 107                 TEST_ASSERT_REL(ntohs(hdr.TotalLength), <=, len - ofs);
 108                 TEST_ASSERT_REL(ntohs(hdr.FragmentInfo), ==, 0);
 109
 110                 TEST_ASSERT_REL(hdr.TTL, >, 1); // >1 because there's no intervening hops
 111                 TEST_ASSERT_REL(hdr.Protocol, ==, proto);
 112
 113                 if(Src) TEST_ASSERT( memcmp(hdr.SrcAddr, Src, 4) == 0 );
 114                 if(Dst) TEST_ASSERT( memcmp(hdr.DstAddr, Dst, 4) == 0 );
 115
 116                 *ofs_out = ofs + (hdr.VerLen & 0xF) * 4;
 117                 return true;
 118         }
 119         else {
 120                 TEST_WARN("Invalid AF(%i) in IP_Pkt_Check", AF);
 121                 return false;
 122         }
 123 }
 124