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         const uint16_t  *words = Data;
  29         uint32_t        ret = ~Prev;
  30         for( int i = 0; i < Length/2; i ++ )
  31         {
  32                 ret += ntohs(*words);
  33                 words ++;
  34         }
  35         if( Length & 1 )
  36                 ret += ntohs(*(uint8_t*)words);
  37
  38         while( ret >> 16 )
  39                 ret = (ret & 0xFFFF) + (ret >> 16);
  40
  41         return ~ret;
  42 }
  43
  44 void IP_Send(int IfNum, int AF, const void *Src, const void *Dst, uint8_t proto,
  45         int BufCount, size_t BufLens[], const void *Bufs[])
  46 {
  47         size_t  total_length = 0;
  48
  49         size_t  out_buflens[BufCount+1];
  50         const void *out_bufs[BufCount+1];
  51
  52         for( int i = 0; i < BufCount; i ++ )
  53         {
  54                 total_length += BufLens[i];
  55                 out_buflens[1+i] = BufLens[i];
  56                 out_bufs[1+i] = Bufs[i];
  57         }
  58
  59         if( AF == 4 )
  60         {
  61                 tIPv4Hdr        hdr;
  62                 hdr.VerLen = (4 << 4) | (sizeof(hdr)/4);
  63                 hdr.DiffServices = 0;
  64                 hdr.TotalLength = htons(sizeof(hdr) + total_length);
  65                 hdr.Identifcation = 0;  // TODO: WTF is this?
  66                 hdr.FragmentInfo = htons(0);
  67                 hdr.TTL = 18;   // TODO: Progammable TTL
  68                 hdr.Protocol = proto;
  69                 hdr.HeaderChecksum = 0;
  70                 memcpy(hdr.SrcAddr, Src, 4);
  71                 memcpy(hdr.DstAddr, Dst, 4);
  72
  73                 hdr.HeaderChecksum = htons( IP_Checksum(IP_CHECKSUM_START, sizeof(hdr), &hdr) );
  74
  75                 out_buflens[0] = sizeof(hdr);
  76                 out_bufs[0] = &hdr;
  77
  78                 // TODO: Don't hard-code MAC addresses
  79                 Link_Send(IfNum, BLOB(HOST_MAC), BLOB(TEST_MAC), ETHER_PROTO_IPV4,
  80                         1+BufCount, out_buflens, out_bufs);
  81         }
  82         else {
  83                 TEST_WARN("Invalid AF(%i) in IP_Send", AF);
  84         }
  85 }
  86
  87 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)
  88 {
  89         size_t  ofs;
  90         if( AF == 4 ) {
  91                 if( !Link_Pkt_Check(len, data, &ofs, BLOB(TEST_MAC), BLOB(HOST_MAC), ETHER_PROTO_IPV4) )
  92                         return false;
  93
  94                 tIPv4Hdr hdr;
  95                 memcpy(&hdr, (const uint8_t*)data + ofs, sizeof(hdr));
  96                 TEST_ASSERT_REL(hdr.VerLen >> 4, ==, 4);
  97
  98                 return false;
  99         }
 100         else {
 101                 TEST_WARN("Invalid AF(%i) in IP_Pkt_Check", AF);
 102                 return false;
 103         }
 104 }
 105