csum.h

   1 /*
   2  * netsniff-ng - the packet sniffing beast
   3  * Copyright 2010 Emmanuel Roullit.
   4  * Subject to the GPL, version 2.
   5  */
   6
   7 #ifndef CSUM_H
   8 #define CSUM_H
   9
  10 #include <netinet/in.h>
  11 #include <netinet/ip.h>
  12
  13 #include "built_in.h"
  14
  15 static inline unsigned short csum(unsigned short *buf, int nwords)
  16 {
  17         unsigned long sum;
  18
  19         for (sum = 0; nwords > 0; nwords--)
  20                 sum += *buf++;
  21         sum = (sum >> 16) + (sum & 0xffff);
  22         sum += (sum >> 16);
  23
  24         return ~sum;
  25 }
  26
  27 static inline uint16_t calc_csum(void *addr, size_t len,
  28                                  int ccsum __maybe_unused)
  29 {
  30         return csum(addr, len >> 1);
  31 }
  32
  33 static inline uint16_t csum_expected(uint16_t sum, uint16_t computed_sum)
  34 {
  35         uint32_t shouldbe;
  36
  37         shouldbe = sum;
  38         shouldbe += ntohs(computed_sum);
  39         shouldbe = (shouldbe & 0xFFFF) + (shouldbe >> 16);
  40         shouldbe = (shouldbe & 0xFFFF) + (shouldbe >> 16);
  41
  42         return shouldbe;
  43 }
  44
  45 /* Taken and modified from tcpdump, Copyright belongs to them! */
  46
  47 struct cksum_vec {
  48         const u8 *ptr;
  49         int len;
  50 };
  51
  52 #define ADDCARRY(x)             \
  53         do { if ((x) > 65535)   \
  54                 (x) -= 65535;   \
  55         } while (0)
  56
  57 #define REDUCE                                          \
  58         do {                                            \
  59                 l_util.l = sum;                         \
  60                 sum = l_util.s[0] + l_util.s[1];        \
  61                 ADDCARRY(sum);                          \
  62         } while (0)
  63
  64 static inline u16 __in_cksum(const struct cksum_vec *vec, int veclen)
  65 {
  66         const u16 *w;
  67         int sum = 0, mlen = 0;
  68         int byte_swapped = 0;
  69         union {
  70                 u8 c[2];
  71                 u16 s;
  72         } s_util;
  73         union {
  74                 u16 s[2];
  75                 u32 l;
  76         } l_util;
  77
  78         for (; veclen != 0; vec++, veclen--) {
  79                 if (vec->len == 0)
  80                         continue;
  81
  82                 w = (const u16 *) (void *) vec->ptr;
  83
  84                 if (mlen == -1) {
  85                         s_util.c[1] = *(const u8 *) w;
  86                         sum += s_util.s;
  87                         w = (const u16 *) (void *) ((const u8 *) w + 1);
  88                         mlen = vec->len - 1;
  89                 } else
  90                         mlen = vec->len;
  91
  92                 if ((1 & (unsigned long) w) && (mlen > 0)) {
  93                         REDUCE;
  94                         sum <<= 8;
  95                         s_util.c[0] = *(const u8 *) w;
  96                         w = (const u16 *) (void *) ((const u8 *) w + 1);
  97                         mlen--;
  98                         byte_swapped = 1;
  99                 }
 100
 101                 while ((mlen -= 32) >= 0) {
 102                         sum +=  w[0]; sum +=  w[1]; sum +=  w[2]; sum +=  w[3];
 103                         sum +=  w[4]; sum +=  w[5]; sum +=  w[6]; sum +=  w[7];
 104                         sum +=  w[8]; sum +=  w[9]; sum += w[10]; sum += w[11];
 105                         sum += w[12]; sum += w[13]; sum += w[14]; sum += w[15];
 106                         w += 16;
 107                 }
 108
 109                 mlen += 32;
 110
 111                 while ((mlen -= 8) >= 0) {
 112                         sum += w[0]; sum += w[1]; sum += w[2]; sum += w[3];
 113                         w += 4;
 114                 }
 115
 116                 mlen += 8;
 117
 118                 if (mlen == 0 && byte_swapped == 0)
 119                         continue;
 120
 121                 REDUCE;
 122
 123                 while ((mlen -= 2) >= 0) {
 124                         sum += *w++;
 125                 }
 126
 127                 if (byte_swapped) {
 128                         REDUCE;
 129                         sum <<= 8;
 130                         byte_swapped = 0;
 131
 132                         if (mlen == -1) {
 133                                 s_util.c[1] = *(const u8 *) w;
 134                                 sum += s_util.s;
 135                                 mlen = 0;
 136                         } else
 137                                 mlen = -1;
 138                 } else if (mlen == -1)
 139                         s_util.c[0] = *(const u8 *) w;
 140         }
 141
 142         if (mlen == -1) {
 143                 s_util.c[1] = 0;
 144                 sum += s_util.s;
 145         }
 146
 147         REDUCE;
 148
 149         return (~sum & 0xffff);
 150 }
 151
 152 static inline u16 p4_csum(const struct ip *ip, const u8 *data, u16 len,
 153                           u8 next_proto)
 154 {
 155         struct cksum_vec vec[2];
 156         struct pseudo_hdr {
 157                 u32 src;
 158                 u32 dst;
 159                 u8 mbz;
 160                 u8 proto;
 161                 u16 len;
 162         } ph;
 163
 164         memset(&ph, 0, sizeof(ph));
 165         ph.len = htons(len);
 166         ph.mbz = 0;
 167         ph.proto = next_proto;
 168         ph.src = ip->ip_src.s_addr;
 169         ph.dst = ip->ip_dst.s_addr;
 170
 171         vec[0].ptr = (const u8 *) (void *) &ph;
 172         vec[0].len = sizeof(ph);
 173
 174         vec[1].ptr = data;
 175         vec[1].len = len;
 176
 177         return __in_cksum(vec, 2);
 178 }
 179
 180 #endif /* CSUM_H */