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