[46] | 1 | #pragma once |
---|
| 2 | |
---|
| 3 | /* |
---|
| 4 | sha-256 implementation for MAVLink based on Heimdal sources, with |
---|
| 5 | modifications to suit mavlink headers |
---|
| 6 | */ |
---|
| 7 | /* |
---|
| 8 | * Copyright (c) 1995 - 2001 Kungliga Tekniska Högskolan |
---|
| 9 | * (Royal Institute of Technology, Stockholm, Sweden). |
---|
| 10 | * All rights reserved. |
---|
| 11 | * |
---|
| 12 | * Redistribution and use in source and binary forms, with or without |
---|
| 13 | * modification, are permitted provided that the following conditions |
---|
| 14 | * are met: |
---|
| 15 | * |
---|
| 16 | * 1. Redistributions of source code must retain the above copyright |
---|
| 17 | * notice, this list of conditions and the following disclaimer. |
---|
| 18 | * |
---|
| 19 | * 2. Redistributions in binary form must reproduce the above copyright |
---|
| 20 | * notice, this list of conditions and the following disclaimer in the |
---|
| 21 | * documentation and/or other materials provided with the distribution. |
---|
| 22 | * |
---|
| 23 | * 3. Neither the name of the Institute nor the names of its contributors |
---|
| 24 | * may be used to endorse or promote products derived from this software |
---|
| 25 | * without specific prior written permission. |
---|
| 26 | * |
---|
| 27 | * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND |
---|
| 28 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
---|
| 29 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
---|
| 30 | * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE |
---|
| 31 | * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL |
---|
| 32 | * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS |
---|
| 33 | * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) |
---|
| 34 | * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT |
---|
| 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY |
---|
| 36 | * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF |
---|
| 37 | * SUCH DAMAGE. |
---|
| 38 | */ |
---|
| 39 | |
---|
| 40 | /* |
---|
| 41 | allow implementation to provide their own sha256 with the same API |
---|
| 42 | */ |
---|
| 43 | #ifndef HAVE_MAVLINK_SHA256 |
---|
| 44 | |
---|
| 45 | #ifndef MAVLINK_HELPER |
---|
| 46 | #define MAVLINK_HELPER |
---|
| 47 | #endif |
---|
| 48 | |
---|
| 49 | typedef struct { |
---|
| 50 | unsigned int sz[2]; |
---|
| 51 | uint32_t counter[8]; |
---|
| 52 | union { |
---|
| 53 | unsigned char save_bytes[64]; |
---|
| 54 | uint32_t save_u32[16]; |
---|
| 55 | } u; |
---|
| 56 | } mavlink_sha256_ctx; |
---|
| 57 | |
---|
| 58 | #define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z))) |
---|
| 59 | #define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) |
---|
| 60 | |
---|
| 61 | #define ROTR(x,n) (((x)>>(n)) | ((x) << (32 - (n)))) |
---|
| 62 | |
---|
| 63 | #define Sigma0(x) (ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22)) |
---|
| 64 | #define Sigma1(x) (ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25)) |
---|
| 65 | #define sigma0(x) (ROTR(x,7) ^ ROTR(x,18) ^ ((x)>>3)) |
---|
| 66 | #define sigma1(x) (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10)) |
---|
| 67 | |
---|
| 68 | #define A m->counter[0] |
---|
| 69 | #define B m->counter[1] |
---|
| 70 | #define C m->counter[2] |
---|
| 71 | #define D m->counter[3] |
---|
| 72 | #define E m->counter[4] |
---|
| 73 | #define F m->counter[5] |
---|
| 74 | #define G m->counter[6] |
---|
| 75 | #define H m->counter[7] |
---|
| 76 | |
---|
| 77 | static const uint32_t mavlink_sha256_constant_256[64] = { |
---|
| 78 | 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, |
---|
| 79 | 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, |
---|
| 80 | 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, |
---|
| 81 | 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, |
---|
| 82 | 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, |
---|
| 83 | 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, |
---|
| 84 | 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, |
---|
| 85 | 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, |
---|
| 86 | 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, |
---|
| 87 | 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, |
---|
| 88 | 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, |
---|
| 89 | 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, |
---|
| 90 | 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, |
---|
| 91 | 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, |
---|
| 92 | 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, |
---|
| 93 | 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 |
---|
| 94 | }; |
---|
| 95 | |
---|
| 96 | MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m) |
---|
| 97 | { |
---|
| 98 | m->sz[0] = 0; |
---|
| 99 | m->sz[1] = 0; |
---|
| 100 | A = 0x6a09e667; |
---|
| 101 | B = 0xbb67ae85; |
---|
| 102 | C = 0x3c6ef372; |
---|
| 103 | D = 0xa54ff53a; |
---|
| 104 | E = 0x510e527f; |
---|
| 105 | F = 0x9b05688c; |
---|
| 106 | G = 0x1f83d9ab; |
---|
| 107 | H = 0x5be0cd19; |
---|
| 108 | } |
---|
| 109 | |
---|
| 110 | static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in) |
---|
| 111 | { |
---|
| 112 | uint32_t AA, BB, CC, DD, EE, FF, GG, HH; |
---|
| 113 | uint32_t data[64]; |
---|
| 114 | int i; |
---|
| 115 | |
---|
| 116 | AA = A; |
---|
| 117 | BB = B; |
---|
| 118 | CC = C; |
---|
| 119 | DD = D; |
---|
| 120 | EE = E; |
---|
| 121 | FF = F; |
---|
| 122 | GG = G; |
---|
| 123 | HH = H; |
---|
| 124 | |
---|
| 125 | for (i = 0; i < 16; ++i) |
---|
| 126 | data[i] = in[i]; |
---|
| 127 | for (i = 16; i < 64; ++i) |
---|
| 128 | data[i] = sigma1(data[i-2]) + data[i-7] + |
---|
| 129 | sigma0(data[i-15]) + data[i - 16]; |
---|
| 130 | |
---|
| 131 | for (i = 0; i < 64; i++) { |
---|
| 132 | uint32_t T1, T2; |
---|
| 133 | |
---|
| 134 | T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i]; |
---|
| 135 | T2 = Sigma0(AA) + Maj(AA,BB,CC); |
---|
| 136 | |
---|
| 137 | HH = GG; |
---|
| 138 | GG = FF; |
---|
| 139 | FF = EE; |
---|
| 140 | EE = DD + T1; |
---|
| 141 | DD = CC; |
---|
| 142 | CC = BB; |
---|
| 143 | BB = AA; |
---|
| 144 | AA = T1 + T2; |
---|
| 145 | } |
---|
| 146 | |
---|
| 147 | A += AA; |
---|
| 148 | B += BB; |
---|
| 149 | C += CC; |
---|
| 150 | D += DD; |
---|
| 151 | E += EE; |
---|
| 152 | F += FF; |
---|
| 153 | G += GG; |
---|
| 154 | H += HH; |
---|
| 155 | } |
---|
| 156 | |
---|
| 157 | MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len) |
---|
| 158 | { |
---|
| 159 | const unsigned char *p = (const unsigned char *)v; |
---|
| 160 | uint32_t old_sz = m->sz[0]; |
---|
| 161 | uint32_t offset; |
---|
| 162 | |
---|
| 163 | m->sz[0] += len * 8; |
---|
| 164 | if (m->sz[0] < old_sz) |
---|
| 165 | ++m->sz[1]; |
---|
| 166 | offset = (old_sz / 8) % 64; |
---|
| 167 | while(len > 0){ |
---|
| 168 | uint32_t l = 64 - offset; |
---|
| 169 | if (len < l) { |
---|
| 170 | l = len; |
---|
| 171 | } |
---|
| 172 | memcpy(m->u.save_bytes + offset, p, l); |
---|
| 173 | offset += l; |
---|
| 174 | p += l; |
---|
| 175 | len -= l; |
---|
| 176 | if(offset == 64){ |
---|
| 177 | int i; |
---|
| 178 | uint32_t current[16]; |
---|
| 179 | const uint32_t *u = m->u.save_u32; |
---|
| 180 | for (i = 0; i < 16; i++){ |
---|
| 181 | const uint8_t *p1 = (const uint8_t *)&u[i]; |
---|
| 182 | uint8_t *p2 = (uint8_t *)¤t[i]; |
---|
| 183 | p2[0] = p1[3]; |
---|
| 184 | p2[1] = p1[2]; |
---|
| 185 | p2[2] = p1[1]; |
---|
| 186 | p2[3] = p1[0]; |
---|
| 187 | } |
---|
| 188 | mavlink_sha256_calc(m, current); |
---|
| 189 | offset = 0; |
---|
| 190 | } |
---|
| 191 | } |
---|
| 192 | } |
---|
| 193 | |
---|
| 194 | /* |
---|
| 195 | get first 48 bits of final sha256 hash |
---|
| 196 | */ |
---|
| 197 | MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6]) |
---|
| 198 | { |
---|
| 199 | unsigned char zeros[72]; |
---|
| 200 | unsigned offset = (m->sz[0] / 8) % 64; |
---|
| 201 | unsigned int dstart = (120 - offset - 1) % 64 + 1; |
---|
| 202 | uint8_t *p = (uint8_t *)&m->counter[0]; |
---|
| 203 | |
---|
| 204 | *zeros = 0x80; |
---|
| 205 | memset (zeros + 1, 0, sizeof(zeros) - 1); |
---|
| 206 | zeros[dstart+7] = (m->sz[0] >> 0) & 0xff; |
---|
| 207 | zeros[dstart+6] = (m->sz[0] >> 8) & 0xff; |
---|
| 208 | zeros[dstart+5] = (m->sz[0] >> 16) & 0xff; |
---|
| 209 | zeros[dstart+4] = (m->sz[0] >> 24) & 0xff; |
---|
| 210 | zeros[dstart+3] = (m->sz[1] >> 0) & 0xff; |
---|
| 211 | zeros[dstart+2] = (m->sz[1] >> 8) & 0xff; |
---|
| 212 | zeros[dstart+1] = (m->sz[1] >> 16) & 0xff; |
---|
| 213 | zeros[dstart+0] = (m->sz[1] >> 24) & 0xff; |
---|
| 214 | |
---|
| 215 | mavlink_sha256_update(m, zeros, dstart + 8); |
---|
| 216 | |
---|
| 217 | // this ordering makes the result consistent with taking the first |
---|
| 218 | // 6 bytes of more conventional sha256 functions. It assumes |
---|
| 219 | // little-endian ordering of m->counter |
---|
| 220 | result[0] = p[3]; |
---|
| 221 | result[1] = p[2]; |
---|
| 222 | result[2] = p[1]; |
---|
| 223 | result[3] = p[0]; |
---|
| 224 | result[4] = p[7]; |
---|
| 225 | result[5] = p[6]; |
---|
| 226 | } |
---|
| 227 | |
---|
| 228 | // prevent conflicts with users of the header |
---|
| 229 | #undef A |
---|
| 230 | #undef B |
---|
| 231 | #undef C |
---|
| 232 | #undef D |
---|
| 233 | #undef E |
---|
| 234 | #undef F |
---|
| 235 | #undef G |
---|
| 236 | #undef H |
---|
| 237 | #undef Ch |
---|
| 238 | #undef ROTR |
---|
| 239 | #undef Sigma0 |
---|
| 240 | #undef Sigma1 |
---|
| 241 | #undef sigma0 |
---|
| 242 | #undef sigma1 |
---|
| 243 | |
---|
| 244 | #endif // HAVE_MAVLINK_SHA256 |
---|