1 /**
2  * Calculate big endian 16-bit sum of a buffer (max 128kB),
3  * then fold and negate it, producing a 16-bit result in [0..FFFE].
4  */
FUNC(u16 calc_csum (u32 sum,const u8 * const buf,const s32 len))5 FUNC(u16 calc_csum(u32 sum, const u8* const buf, const s32 len)) {
6     u16 csum;
7     s32 i;
8     for (i = 0; i < len; ++i) sum += buf[i] * ((i & 1) ? 1u : 256u);
9 
10     sum = (sum & 0xFFFF) + (sum >> 16);  /* max after this is 1FFFE */
11     csum = sum + (sum >> 16);
12     return ~csum;  /* assuming sum > 0 on input, this is in [0..FFFE] */
13 }
14 
fix_udp_csum(u16 csum)15 static u16 fix_udp_csum(u16 csum) {
16     return csum ? csum : 0xFFFF;
17 }
18 
19 /**
20  * Calculate and store packet checksums and return dscp.
21  *
22  * @param pkt - pointer to the very start of the to-be-transmitted packet,
23  *              ie. the start of the ethernet header (if one is present)
24  *     WARNING: at minimum 266 bytes of buffer pointed to by 'pkt' pointer
25  *              *MUST* be writable.
26  * (IPv4 header checksum is a 2 byte value, 10 bytes after ip_ofs,
27  * which has a maximum value of 254.  Thus 254[ip_ofs] + 10 + 2[u16] = 266)
28  *
29  * @param len - length of the packet (this may be < 266).
30  * @param ip_ofs - offset from beginning of pkt to IPv4 or IPv6 header:
31  *                 IP version detected based on top nibble of this byte,
32  *                 for IPv4 we will calculate and store IP header checksum,
33  *                 but only for the first 20 bytes of the header,
34  *                 prior to calling this the IPv4 header checksum field
35  *                 must be initialized to the partial checksum of the IPv4
36  *                 options (0 if none)
37  *                 255 means there is no IP header (for example ARP)
38  *                 DSCP will be retrieved from this IP header (0 if none).
39  * @param partial_csum - additional value to include in L4 checksum
40  * @param csum_start - offset from beginning of pkt to begin L4 checksum
41  *                     calculation (until end of pkt specified by len)
42  * @param csum_ofs - offset from beginning of pkt to store L4 checksum
43  *                   255 means do not calculate/store L4 checksum
44  * @param udp - true iff we should generate a UDP style L4 checksum (0 -> 0xFFFF)
45  *
46  * @return 6-bit DSCP value [0..63], garbage on parse error.
47  */
FUNC(int csum_and_return_dscp (u8 * const pkt,const s32 len,const u8 ip_ofs,const u16 partial_csum,const u8 csum_start,const u8 csum_ofs,const bool udp))48 FUNC(int csum_and_return_dscp(u8* const pkt, const s32 len, const u8 ip_ofs,
49   const u16 partial_csum, const u8 csum_start, const u8 csum_ofs, const bool udp)) {
50     if (csum_ofs < 255) {
51         // note that calc_csum() treats negative lengths as zero
52         u32 csum = calc_csum(partial_csum, pkt + csum_start, len - csum_start);
53         if (udp) csum = fix_udp_csum(csum);
54         store_be16(pkt + csum_ofs, csum);
55     }
56     if (ip_ofs < 255) {
57         u8 ip = pkt[ip_ofs] >> 4;
58         if (ip == 4) {
59             store_be16(pkt + ip_ofs + 10, calc_csum(0, pkt + ip_ofs, IPV4_HLEN));
60             return pkt[ip_ofs + 1] >> 2;  /* DSCP */
61         } else if (ip == 6) {
62             return (read_be16(pkt + ip_ofs) >> 6) & 0x3F;  /* DSCP */
63         }
64     }
65     return 0;
66 }
67