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