mavlink_sha256.h
Go to the documentation of this file.
00001 #pragma once
00002 
00003 /*
00004   sha-256 implementation for MAVLink based on Heimdal sources, with
00005   modifications to suit mavlink headers
00006  */
00007 /*
00008  * Copyright (c) 1995 - 2001 Kungliga Tekniska Högskolan
00009  * (Royal Institute of Technology, Stockholm, Sweden).
00010  * All rights reserved.
00011  * 
00012  * Redistribution and use in source and binary forms, with or without
00013  * modification, are permitted provided that the following conditions
00014  * are met:
00015  * 
00016  * 1. Redistributions of source code must retain the above copyright
00017  *    notice, this list of conditions and the following disclaimer.
00018  * 
00019  * 2. Redistributions in binary form must reproduce the above copyright
00020  *    notice, this list of conditions and the following disclaimer in the
00021  *    documentation and/or other materials provided with the distribution.
00022  * 
00023  * 3. Neither the name of the Institute nor the names of its contributors
00024  *    may be used to endorse or promote products derived from this software
00025  *    without specific prior written permission.
00026  * 
00027  * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND
00028  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00029  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00030  * ARE DISCLAIMED.  IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE
00031  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00032  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
00033  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
00034  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00035  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
00036  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
00037  * SUCH DAMAGE.
00038  */
00039 
00040 /*
00041   allow implementation to provide their own sha256 with the same API
00042 */
00043 #ifndef HAVE_MAVLINK_SHA256
00044 
00045 #ifndef MAVLINK_HELPER
00046 #define MAVLINK_HELPER
00047 #endif
00048 
00049 typedef struct {
00050   unsigned int sz[2];
00051   uint32_t counter[8];
00052   unsigned char save[64];
00053 } mavlink_sha256_ctx;
00054 
00055 #define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z)))
00056 #define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z)))
00057 
00058 #define ROTR(x,n)   (((x)>>(n)) | ((x) << (32 - (n))))
00059 
00060 #define Sigma0(x)       (ROTR(x,2)  ^ ROTR(x,13) ^ ROTR(x,22))
00061 #define Sigma1(x)       (ROTR(x,6)  ^ ROTR(x,11) ^ ROTR(x,25))
00062 #define sigma0(x)       (ROTR(x,7)  ^ ROTR(x,18) ^ ((x)>>3))
00063 #define sigma1(x)       (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10))
00064 
00065 #define A m->counter[0]
00066 #define B m->counter[1]
00067 #define C m->counter[2]
00068 #define D m->counter[3]
00069 #define E m->counter[4]
00070 #define F m->counter[5]
00071 #define G m->counter[6]
00072 #define H m->counter[7]
00073 
00074 static const uint32_t mavlink_sha256_constant_256[64] = {
00075     0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5,
00076     0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5,
00077     0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3,
00078     0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174,
00079     0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc,
00080     0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da,
00081     0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7,
00082     0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967,
00083     0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13,
00084     0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85,
00085     0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3,
00086     0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070,
00087     0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5,
00088     0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3,
00089     0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208,
00090     0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2
00091 };
00092 
00093 MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m)
00094 {
00095     m->sz[0] = 0;
00096     m->sz[1] = 0;
00097     A = 0x6a09e667;
00098     B = 0xbb67ae85;
00099     C = 0x3c6ef372;
00100     D = 0xa54ff53a;
00101     E = 0x510e527f;
00102     F = 0x9b05688c;
00103     G = 0x1f83d9ab;
00104     H = 0x5be0cd19;
00105 }
00106 
00107 static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in)
00108 {
00109     uint32_t AA, BB, CC, DD, EE, FF, GG, HH;
00110     uint32_t data[64];
00111     int i;
00112 
00113     AA = A;
00114     BB = B;
00115     CC = C;
00116     DD = D;
00117     EE = E;
00118     FF = F;
00119     GG = G;
00120     HH = H;
00121 
00122     for (i = 0; i < 16; ++i)
00123         data[i] = in[i];
00124     for (i = 16; i < 64; ++i)
00125         data[i] = sigma1(data[i-2]) + data[i-7] + 
00126             sigma0(data[i-15]) + data[i - 16];
00127 
00128     for (i = 0; i < 64; i++) {
00129         uint32_t T1, T2;
00130 
00131         T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i];
00132         T2 = Sigma0(AA) + Maj(AA,BB,CC);
00133                              
00134         HH = GG;
00135         GG = FF;
00136         FF = EE;
00137         EE = DD + T1;
00138         DD = CC;
00139         CC = BB;
00140         BB = AA;
00141         AA = T1 + T2;
00142     }
00143 
00144     A += AA;
00145     B += BB;
00146     C += CC;
00147     D += DD;
00148     E += EE;
00149     F += FF;
00150     G += GG;
00151     H += HH;
00152 }
00153 
00154 MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len)
00155 {
00156     const unsigned char *p = (const unsigned char *)v;
00157     uint32_t old_sz = m->sz[0];
00158     uint32_t offset;
00159 
00160     m->sz[0] += len * 8;
00161     if (m->sz[0] < old_sz)
00162         ++m->sz[1];
00163     offset = (old_sz / 8) % 64;
00164     while(len > 0){
00165         uint32_t l = 64 - offset;
00166         if (len < l) {
00167             l = len;
00168         }
00169         memcpy(m->save + offset, p, l);
00170         offset += l;
00171         p += l;
00172         len -= l;
00173         if(offset == 64){
00174             int i;
00175             uint32_t current[16];
00176             const uint32_t *u = (const uint32_t *)m->save;
00177             for (i = 0; i < 16; i++){
00178                 const uint8_t *p1 = (const uint8_t *)&u[i];
00179                 uint8_t *p2 = (uint8_t *)&current[i];
00180                 p2[0] = p1[3];
00181                 p2[1] = p1[2];
00182                 p2[2] = p1[1];
00183                 p2[3] = p1[0];
00184             }
00185             mavlink_sha256_calc(m, current);
00186             offset = 0;
00187         }
00188     }
00189 }
00190 
00191 /*
00192   get first 48 bits of final sha256 hash
00193  */
00194 MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6])
00195 {
00196     unsigned char zeros[72];
00197     unsigned offset = (m->sz[0] / 8) % 64;
00198     unsigned int dstart = (120 - offset - 1) % 64 + 1;
00199     uint8_t *p = (uint8_t *)&m->counter[0];
00200     
00201     *zeros = 0x80;
00202     memset (zeros + 1, 0, sizeof(zeros) - 1);
00203     zeros[dstart+7] = (m->sz[0] >> 0) & 0xff;
00204     zeros[dstart+6] = (m->sz[0] >> 8) & 0xff;
00205     zeros[dstart+5] = (m->sz[0] >> 16) & 0xff;
00206     zeros[dstart+4] = (m->sz[0] >> 24) & 0xff;
00207     zeros[dstart+3] = (m->sz[1] >> 0) & 0xff;
00208     zeros[dstart+2] = (m->sz[1] >> 8) & 0xff;
00209     zeros[dstart+1] = (m->sz[1] >> 16) & 0xff;
00210     zeros[dstart+0] = (m->sz[1] >> 24) & 0xff;
00211 
00212     mavlink_sha256_update(m, zeros, dstart + 8);
00213 
00214     // this ordering makes the result consistent with taking the first
00215     // 6 bytes of more conventional sha256 functions. It assumes
00216     // little-endian ordering of m->counter
00217     result[0] = p[3];
00218     result[1] = p[2];
00219     result[2] = p[1];
00220     result[3] = p[0];
00221     result[4] = p[7];
00222     result[5] = p[6];
00223 }
00224 
00225 // prevent conflicts with users of the header
00226 #undef A
00227 #undef B
00228 #undef C
00229 #undef D
00230 #undef E
00231 #undef F
00232 #undef G
00233 #undef H
00234 #undef Ch
00235 #undef ROTR
00236 #undef Sigma0
00237 #undef Sigma1
00238 #undef sigma0
00239 #undef sigma1
00240 
00241 #endif // HAVE_MAVLINK_SHA256


mavlink
Author(s): Lorenz Meier
autogenerated on Sun May 22 2016 04:05:43