| 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
|
---|