csum.h

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