lib/main/MAVLink/mavlink_sha256.h

   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 #ifdef MAVLINK_USE_CXX_NAMESPACE
  46 namespace mavlink {
  47 #endif
  48
  49 #ifndef MAVLINK_HELPER
  50 #define MAVLINK_HELPER
  51 #endif
  52
  53 typedef struct {
  54   uint32_t sz[2];
  55   uint32_t counter[8];
  56   union {
  57       unsigned char save_bytes[64];
  58       uint32_t save_u32[16];
  59   } u;
  60 } mavlink_sha256_ctx;
  61
  62 #define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z)))
  63 #define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z)))
  64
  65 #define ROTR(x,n)   (((x)>>(n)) | ((x) << (32 - (n))))
  66
  67 #define Sigma0(x)       (ROTR(x,2)  ^ ROTR(x,13) ^ ROTR(x,22))
  68 #define Sigma1(x)       (ROTR(x,6)  ^ ROTR(x,11) ^ ROTR(x,25))
  69 #define sigma0(x)       (ROTR(x,7)  ^ ROTR(x,18) ^ ((x)>>3))
  70 #define sigma1(x)       (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10))
  71
  72 static const uint32_t mavlink_sha256_constant_256[64] = {
  73     0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
  74     0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
  75     0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
  76     0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
  77     0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
  78     0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
  79     0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
  80     0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
  81     0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
  82     0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
  83     0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
  84     0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
  85     0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
  86     0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
  87     0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
  88     0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
  89 };
  90
  91 MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m)
  92 {
  93     m->sz[0] = 0;
  94     m->sz[1] = 0;
  95     m->counter[0] = 0x6a09e667;
  96     m->counter[1] = 0xbb67ae85;
  97     m->counter[2] = 0x3c6ef372;
  98     m->counter[3] = 0xa54ff53a;
  99     m->counter[4] = 0x510e527f;
 100     m->counter[5] = 0x9b05688c;
 101     m->counter[6] = 0x1f83d9ab;
 102     m->counter[7] = 0x5be0cd19;
 103 }
 104
 105 static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in)
 106 {
 107     uint32_t AA, BB, CC, DD, EE, FF, GG, HH;
 108     uint32_t data[64];
 109     int i;
 110
 111     AA = m->counter[0];
 112     BB = m->counter[1];
 113     CC = m->counter[2];
 114     DD = m->counter[3];
 115     EE = m->counter[4];
 116     FF = m->counter[5];
 117     GG = m->counter[6];
 118     HH = m->counter[7];
 119
 120     for (i = 0; i < 16; ++i)
 121         data[i] = in[i];
 122     for (i = 16; i < 64; ++i)
 123         data[i] = sigma1(data[i-2]) + data[i-7] +
 124             sigma0(data[i-15]) + data[i - 16];
 125
 126     for (i = 0; i < 64; i++) {
 127         uint32_t T1, T2;
 128
 129         T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i];
 130         T2 = Sigma0(AA) + Maj(AA,BB,CC);
 131
 132         HH = GG;
 133         GG = FF;
 134         FF = EE;
 135         EE = DD + T1;
 136         DD = CC;
 137         CC = BB;
 138         BB = AA;
 139         AA = T1 + T2;
 140     }
 141
 142     m->counter[0] += AA;
 143     m->counter[1] += BB;
 144     m->counter[2] += CC;
 145     m->counter[3] += DD;
 146     m->counter[4] += EE;
 147     m->counter[5] += FF;
 148     m->counter[6] += GG;
 149     m->counter[7] += HH;
 150 }
 151
 152 MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len)
 153 {
 154     const unsigned char *p = (const unsigned char *)v;
 155     uint32_t old_sz = m->sz[0];
 156     uint32_t offset;
 157
 158     m->sz[0] += len * 8;
 159     if (m->sz[0] < old_sz)
 160         ++m->sz[1];
 161     offset = (old_sz / 8) % 64;
 162     while(len > 0){
 163         uint32_t l = 64 - offset;
 164         if (len < l) {
 165             l = len;
 166         }
 167         memcpy(m->u.save_bytes + offset, p, l);
 168         offset += l;
 169         p += l;
 170         len -= l;
 171         if(offset == 64){
 172             int i;
 173             uint32_t current[16];
 174             const uint32_t *u = m->u.save_u32;
 175             for (i = 0; i < 16; i++){
 176                 const uint8_t *p1 = (const uint8_t *)&u[i];
 177                 uint8_t *p2 = (uint8_t *)&current[i];
 178                 p2[0] = p1[3];
 179                 p2[1] = p1[2];
 180                 p2[2] = p1[1];
 181                 p2[3] = p1[0];
 182             }
 183             mavlink_sha256_calc(m, current);
 184             offset = 0;
 185         }
 186     }
 187 }
 188
 189 /*
 190   get first 48 bits of final sha256 hash
 191  */
 192 MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6])
 193 {
 194     unsigned char zeros[72];
 195     unsigned offset = (m->sz[0] / 8) % 64;
 196     unsigned int dstart = (120 - offset - 1) % 64 + 1;
 197     uint8_t *p = (uint8_t *)&m->counter[0];
 198
 199     *zeros = 0x80;
 200     memset (zeros + 1, 0, sizeof(zeros) - 1);
 201     zeros[dstart+7] = (m->sz[0] >> 0) & 0xff;
 202     zeros[dstart+6] = (m->sz[0] >> 8) & 0xff;
 203     zeros[dstart+5] = (m->sz[0] >> 16) & 0xff;
 204     zeros[dstart+4] = (m->sz[0] >> 24) & 0xff;
 205     zeros[dstart+3] = (m->sz[1] >> 0) & 0xff;
 206     zeros[dstart+2] = (m->sz[1] >> 8) & 0xff;
 207     zeros[dstart+1] = (m->sz[1] >> 16) & 0xff;
 208     zeros[dstart+0] = (m->sz[1] >> 24) & 0xff;
 209
 210     mavlink_sha256_update(m, zeros, dstart + 8);
 211
 212     // this ordering makes the result consistent with taking the first
 213     // 6 bytes of more conventional sha256 functions. It assumes
 214     // little-endian ordering of m->counter
 215     result[0] = p[3];
 216     result[1] = p[2];
 217     result[2] = p[1];
 218     result[3] = p[0];
 219     result[4] = p[7];
 220     result[5] = p[6];
 221 }
 222
 223 // prevent conflicts with users of the header
 224 #undef Ch
 225 #undef ROTR
 226 #undef Sigma0
 227 #undef Sigma1
 228 #undef sigma0
 229 #undef sigma1
 230
 231 #ifdef MAVLINK_USE_CXX_NAMESPACE
 232 } // namespace mavlink
 233 #endif
 234
 235 #endif // HAVE_MAVLINK_SHA256