csum.h

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