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