mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
OP-1522 New channel allocation
Generate a pseudo random list of channels from hmac_sha1 with the coordid as key. Enforce channel spacing based on the frequency deviation derived from the configured data rate.
This commit is contained in:
Karl Knutsson
parent
9bf18490b1
commit
a3715598c7
37
flight/libraries/inc/sha1.h
Normal file
37
flight/libraries/inc/sha1.h
Normal file
@ -0,0 +1,37 @@
|
||||
/* $NetBSD: sha1.h,v 1.14 2009/11/06 20:31:19 joerg Exp $ */
|
||||
|
||||
/*
|
||||
* SHA-1 in C
|
||||
* By Steve Reid <steve@edmweb.com>
|
||||
* 100% Public Domain
|
||||
*/
|
||||
|
||||
#ifndef _SYS_SHA1_H_
|
||||
#define _SYS_SHA1_H_
|
||||
|
||||
#include <sys/cdefs.h>
|
||||
#include <sys/types.h>
|
||||
|
||||
#define SHA1_DIGEST_LENGTH 20
|
||||
#define SHA1_DIGEST_STRING_LENGTH 41
|
||||
|
||||
typedef struct {
|
||||
uint32_t state[5];
|
||||
uint32_t count[2];
|
||||
uint8_t buffer[64];
|
||||
} SHA1_CTX;
|
||||
|
||||
__BEGIN_DECLS
|
||||
void SHA1Transform(uint32_t[5], const uint8_t[64]);
|
||||
void SHA1Init(SHA1_CTX *);
|
||||
void SHA1Update(SHA1_CTX *, const uint8_t *, unsigned int);
|
||||
void SHA1Final(uint8_t[SHA1_DIGEST_LENGTH], SHA1_CTX *);
|
||||
#ifndef _KERNEL
|
||||
char *SHA1End(SHA1_CTX *, char *);
|
||||
char *SHA1FileChunk(const char *, char *, off_t, off_t);
|
||||
char *SHA1File(const char *, char *);
|
||||
char *SHA1Data(const uint8_t *, size_t, char *);
|
||||
#endif /* _KERNEL */
|
||||
__END_DECLS
|
||||
|
||||
#endif /* _SYS_SHA1_H_ */
|
||||
278
flight/libraries/sha1.c
Normal file
278
flight/libraries/sha1.c
Normal file
@ -0,0 +1,278 @@
|
||||
/* $NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $ */
|
||||
/* $OpenBSD: sha1.c,v 1.9 1997/07/23 21:12:32 kstailey Exp $ */
|
||||
|
||||
/*
|
||||
* SHA-1 in C
|
||||
* By Steve Reid <steve@edmweb.com>
|
||||
* 100% Public Domain
|
||||
*
|
||||
* Test Vectors (from FIPS PUB 180-1)
|
||||
* "abc"
|
||||
* A9993E36 4706816A BA3E2571 7850C26C 9CD0D89D
|
||||
* "abcdbcdecdefdefgefghfghighijhijkijkljklmklmnlmnomnopnopq"
|
||||
* 84983E44 1C3BD26E BAAE4AA1 F95129E5 E54670F1
|
||||
* A million repetitions of "a"
|
||||
* 34AA973C D4C4DAA4 F61EEB2B DBAD2731 6534016F
|
||||
*/
|
||||
|
||||
#define SHA1HANDSOFF /* Copies data before messing with it. */
|
||||
|
||||
#include <sys/cdefs.h>
|
||||
|
||||
#if defined(_KERNEL) || defined(_STANDALONE)
|
||||
__KERNEL_RCSID(0, "$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $");
|
||||
|
||||
#include <lib/libkern/libkern.h>
|
||||
|
||||
#else
|
||||
|
||||
#if defined(LIBC_SCCS) && !defined(lint)
|
||||
__RCSID("$NetBSD: sha1.c,v 1.6 2009/11/06 20:31:18 joerg Exp $");
|
||||
#endif /* LIBC_SCCS and not lint */
|
||||
|
||||
//#include "namespace.h"
|
||||
#include <assert.h>
|
||||
#include <string.h>
|
||||
|
||||
#endif
|
||||
|
||||
#include <stdint.h>
|
||||
#include <sha1.h>
|
||||
|
||||
|
||||
#if HAVE_NBTOOL_CONFIG_H
|
||||
#include "nbtool_config.h"
|
||||
#endif
|
||||
|
||||
#if !HAVE_SHA1_H
|
||||
|
||||
#define rol(value, bits) (((value) << (bits)) | ((value) >> (32 - (bits))))
|
||||
|
||||
/*
|
||||
* blk0() and blk() perform the initial expand.
|
||||
* I got the idea of expanding during the round function from SSLeay
|
||||
*/
|
||||
#if BYTE_ORDER == LITTLE_ENDIAN
|
||||
# define blk0(i) (block->l[i] = (rol(block->l[i],24)&0xFF00FF00) \
|
||||
|(rol(block->l[i],8)&0x00FF00FF))
|
||||
#else
|
||||
# define blk0(i) block->l[i]
|
||||
#endif
|
||||
#define blk(i) (block->l[i&15] = rol(block->l[(i+13)&15]^block->l[(i+8)&15] \
|
||||
^block->l[(i+2)&15]^block->l[i&15],1))
|
||||
|
||||
/*
|
||||
* (R0+R1), R2, R3, R4 are the different operations (rounds) used in SHA1
|
||||
*/
|
||||
#define R0(v,w,x,y,z,i) z+=((w&(x^y))^y)+blk0(i)+0x5A827999+rol(v,5);w=rol(w,30);
|
||||
#define R1(v,w,x,y,z,i) z+=((w&(x^y))^y)+blk(i)+0x5A827999+rol(v,5);w=rol(w,30);
|
||||
#define R2(v,w,x,y,z,i) z+=(w^x^y)+blk(i)+0x6ED9EBA1+rol(v,5);w=rol(w,30);
|
||||
#define R3(v,w,x,y,z,i) z+=(((w|x)&y)|(w&x))+blk(i)+0x8F1BBCDC+rol(v,5);w=rol(w,30);
|
||||
#define R4(v,w,x,y,z,i) z+=(w^x^y)+blk(i)+0xCA62C1D6+rol(v,5);w=rol(w,30);
|
||||
|
||||
|
||||
#if !defined(_KERNEL) && !defined(_STANDALONE)
|
||||
#if defined(__weak_alias)
|
||||
__weak_alias(SHA1Transform,_SHA1Transform)
|
||||
__weak_alias(SHA1Init,_SHA1Init)
|
||||
__weak_alias(SHA1Update,_SHA1Update)
|
||||
__weak_alias(SHA1Final,_SHA1Final)
|
||||
#endif
|
||||
#endif
|
||||
|
||||
typedef union {
|
||||
uint8_t c[64];
|
||||
uint32_t l[16];
|
||||
} CHAR64LONG16;
|
||||
|
||||
/* old sparc64 gcc could not compile this */
|
||||
#undef SPARC64_GCC_WORKAROUND
|
||||
#if defined(__sparc64__) && defined(__GNUC__) && __GNUC__ < 3
|
||||
#define SPARC64_GCC_WORKAROUND
|
||||
#endif
|
||||
|
||||
#ifdef SPARC64_GCC_WORKAROUND
|
||||
void do_R01(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
|
||||
void do_R2(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
|
||||
void do_R3(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
|
||||
void do_R4(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *);
|
||||
|
||||
#define nR0(v,w,x,y,z,i) R0(*v,*w,*x,*y,*z,i)
|
||||
#define nR1(v,w,x,y,z,i) R1(*v,*w,*x,*y,*z,i)
|
||||
#define nR2(v,w,x,y,z,i) R2(*v,*w,*x,*y,*z,i)
|
||||
#define nR3(v,w,x,y,z,i) R3(*v,*w,*x,*y,*z,i)
|
||||
#define nR4(v,w,x,y,z,i) R4(*v,*w,*x,*y,*z,i)
|
||||
|
||||
void
|
||||
do_R01(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
|
||||
{
|
||||
nR0(a,b,c,d,e, 0); nR0(e,a,b,c,d, 1); nR0(d,e,a,b,c, 2); nR0(c,d,e,a,b, 3);
|
||||
nR0(b,c,d,e,a, 4); nR0(a,b,c,d,e, 5); nR0(e,a,b,c,d, 6); nR0(d,e,a,b,c, 7);
|
||||
nR0(c,d,e,a,b, 8); nR0(b,c,d,e,a, 9); nR0(a,b,c,d,e,10); nR0(e,a,b,c,d,11);
|
||||
nR0(d,e,a,b,c,12); nR0(c,d,e,a,b,13); nR0(b,c,d,e,a,14); nR0(a,b,c,d,e,15);
|
||||
nR1(e,a,b,c,d,16); nR1(d,e,a,b,c,17); nR1(c,d,e,a,b,18); nR1(b,c,d,e,a,19);
|
||||
}
|
||||
|
||||
void
|
||||
do_R2(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
|
||||
{
|
||||
nR2(a,b,c,d,e,20); nR2(e,a,b,c,d,21); nR2(d,e,a,b,c,22); nR2(c,d,e,a,b,23);
|
||||
nR2(b,c,d,e,a,24); nR2(a,b,c,d,e,25); nR2(e,a,b,c,d,26); nR2(d,e,a,b,c,27);
|
||||
nR2(c,d,e,a,b,28); nR2(b,c,d,e,a,29); nR2(a,b,c,d,e,30); nR2(e,a,b,c,d,31);
|
||||
nR2(d,e,a,b,c,32); nR2(c,d,e,a,b,33); nR2(b,c,d,e,a,34); nR2(a,b,c,d,e,35);
|
||||
nR2(e,a,b,c,d,36); nR2(d,e,a,b,c,37); nR2(c,d,e,a,b,38); nR2(b,c,d,e,a,39);
|
||||
}
|
||||
|
||||
void
|
||||
do_R3(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
|
||||
{
|
||||
nR3(a,b,c,d,e,40); nR3(e,a,b,c,d,41); nR3(d,e,a,b,c,42); nR3(c,d,e,a,b,43);
|
||||
nR3(b,c,d,e,a,44); nR3(a,b,c,d,e,45); nR3(e,a,b,c,d,46); nR3(d,e,a,b,c,47);
|
||||
nR3(c,d,e,a,b,48); nR3(b,c,d,e,a,49); nR3(a,b,c,d,e,50); nR3(e,a,b,c,d,51);
|
||||
nR3(d,e,a,b,c,52); nR3(c,d,e,a,b,53); nR3(b,c,d,e,a,54); nR3(a,b,c,d,e,55);
|
||||
nR3(e,a,b,c,d,56); nR3(d,e,a,b,c,57); nR3(c,d,e,a,b,58); nR3(b,c,d,e,a,59);
|
||||
}
|
||||
|
||||
void
|
||||
do_R4(uint32_t *a, uint32_t *b, uint32_t *c, uint32_t *d, uint32_t *e, CHAR64LONG16 *block)
|
||||
{
|
||||
nR4(a,b,c,d,e,60); nR4(e,a,b,c,d,61); nR4(d,e,a,b,c,62); nR4(c,d,e,a,b,63);
|
||||
nR4(b,c,d,e,a,64); nR4(a,b,c,d,e,65); nR4(e,a,b,c,d,66); nR4(d,e,a,b,c,67);
|
||||
nR4(c,d,e,a,b,68); nR4(b,c,d,e,a,69); nR4(a,b,c,d,e,70); nR4(e,a,b,c,d,71);
|
||||
nR4(d,e,a,b,c,72); nR4(c,d,e,a,b,73); nR4(b,c,d,e,a,74); nR4(a,b,c,d,e,75);
|
||||
nR4(e,a,b,c,d,76); nR4(d,e,a,b,c,77); nR4(c,d,e,a,b,78); nR4(b,c,d,e,a,79);
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
* Hash a single 512-bit block. This is the core of the algorithm.
|
||||
*/
|
||||
void SHA1Transform(uint32_t state[5], const uint8_t buffer[64])
|
||||
{
|
||||
uint32_t a, b, c, d, e;
|
||||
CHAR64LONG16 *block;
|
||||
|
||||
#ifdef SHA1HANDSOFF
|
||||
CHAR64LONG16 workspace;
|
||||
#endif
|
||||
|
||||
#ifdef SHA1HANDSOFF
|
||||
block = &workspace;
|
||||
(void)memcpy(block, buffer, 64);
|
||||
#else
|
||||
block = (CHAR64LONG16 *)(void *)buffer;
|
||||
#endif
|
||||
|
||||
/* Copy context->state[] to working vars */
|
||||
a = state[0];
|
||||
b = state[1];
|
||||
c = state[2];
|
||||
d = state[3];
|
||||
e = state[4];
|
||||
|
||||
#ifdef SPARC64_GCC_WORKAROUND
|
||||
do_R01(&a, &b, &c, &d, &e, block);
|
||||
do_R2(&a, &b, &c, &d, &e, block);
|
||||
do_R3(&a, &b, &c, &d, &e, block);
|
||||
do_R4(&a, &b, &c, &d, &e, block);
|
||||
#else
|
||||
/* 4 rounds of 20 operations each. Loop unrolled. */
|
||||
R0(a,b,c,d,e, 0); R0(e,a,b,c,d, 1); R0(d,e,a,b,c, 2); R0(c,d,e,a,b, 3);
|
||||
R0(b,c,d,e,a, 4); R0(a,b,c,d,e, 5); R0(e,a,b,c,d, 6); R0(d,e,a,b,c, 7);
|
||||
R0(c,d,e,a,b, 8); R0(b,c,d,e,a, 9); R0(a,b,c,d,e,10); R0(e,a,b,c,d,11);
|
||||
R0(d,e,a,b,c,12); R0(c,d,e,a,b,13); R0(b,c,d,e,a,14); R0(a,b,c,d,e,15);
|
||||
R1(e,a,b,c,d,16); R1(d,e,a,b,c,17); R1(c,d,e,a,b,18); R1(b,c,d,e,a,19);
|
||||
R2(a,b,c,d,e,20); R2(e,a,b,c,d,21); R2(d,e,a,b,c,22); R2(c,d,e,a,b,23);
|
||||
R2(b,c,d,e,a,24); R2(a,b,c,d,e,25); R2(e,a,b,c,d,26); R2(d,e,a,b,c,27);
|
||||
R2(c,d,e,a,b,28); R2(b,c,d,e,a,29); R2(a,b,c,d,e,30); R2(e,a,b,c,d,31);
|
||||
R2(d,e,a,b,c,32); R2(c,d,e,a,b,33); R2(b,c,d,e,a,34); R2(a,b,c,d,e,35);
|
||||
R2(e,a,b,c,d,36); R2(d,e,a,b,c,37); R2(c,d,e,a,b,38); R2(b,c,d,e,a,39);
|
||||
R3(a,b,c,d,e,40); R3(e,a,b,c,d,41); R3(d,e,a,b,c,42); R3(c,d,e,a,b,43);
|
||||
R3(b,c,d,e,a,44); R3(a,b,c,d,e,45); R3(e,a,b,c,d,46); R3(d,e,a,b,c,47);
|
||||
R3(c,d,e,a,b,48); R3(b,c,d,e,a,49); R3(a,b,c,d,e,50); R3(e,a,b,c,d,51);
|
||||
R3(d,e,a,b,c,52); R3(c,d,e,a,b,53); R3(b,c,d,e,a,54); R3(a,b,c,d,e,55);
|
||||
R3(e,a,b,c,d,56); R3(d,e,a,b,c,57); R3(c,d,e,a,b,58); R3(b,c,d,e,a,59);
|
||||
R4(a,b,c,d,e,60); R4(e,a,b,c,d,61); R4(d,e,a,b,c,62); R4(c,d,e,a,b,63);
|
||||
R4(b,c,d,e,a,64); R4(a,b,c,d,e,65); R4(e,a,b,c,d,66); R4(d,e,a,b,c,67);
|
||||
R4(c,d,e,a,b,68); R4(b,c,d,e,a,69); R4(a,b,c,d,e,70); R4(e,a,b,c,d,71);
|
||||
R4(d,e,a,b,c,72); R4(c,d,e,a,b,73); R4(b,c,d,e,a,74); R4(a,b,c,d,e,75);
|
||||
R4(e,a,b,c,d,76); R4(d,e,a,b,c,77); R4(c,d,e,a,b,78); R4(b,c,d,e,a,79);
|
||||
#endif
|
||||
|
||||
/* Add the working vars back into context.state[] */
|
||||
state[0] += a;
|
||||
state[1] += b;
|
||||
state[2] += c;
|
||||
state[3] += d;
|
||||
state[4] += e;
|
||||
|
||||
/* Wipe variables */
|
||||
a = b = c = d = e = 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* SHA1Init - Initialize new context
|
||||
*/
|
||||
void SHA1Init(SHA1_CTX *context)
|
||||
{
|
||||
|
||||
/* SHA1 initialization constants */
|
||||
context->state[0] = 0x67452301;
|
||||
context->state[1] = 0xEFCDAB89;
|
||||
context->state[2] = 0x98BADCFE;
|
||||
context->state[3] = 0x10325476;
|
||||
context->state[4] = 0xC3D2E1F0;
|
||||
context->count[0] = context->count[1] = 0;
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Run your data through this.
|
||||
*/
|
||||
void SHA1Update(SHA1_CTX *context, const uint8_t *data, unsigned int len)
|
||||
{
|
||||
unsigned int i, j;
|
||||
|
||||
j = context->count[0];
|
||||
if ((context->count[0] += len << 3) < j)
|
||||
context->count[1] += (len>>29)+1;
|
||||
j = (j >> 3) & 63;
|
||||
if ((j + len) > 63) {
|
||||
(void)memcpy(&context->buffer[j], data, (i = 64-j));
|
||||
SHA1Transform(context->state, context->buffer);
|
||||
for ( ; i + 63 < len; i += 64)
|
||||
SHA1Transform(context->state, &data[i]);
|
||||
j = 0;
|
||||
} else {
|
||||
i = 0;
|
||||
}
|
||||
(void)memcpy(&context->buffer[j], &data[i], len - i);
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
* Add padding and return the message digest.
|
||||
*/
|
||||
void SHA1Final(uint8_t digest[20], SHA1_CTX *context)
|
||||
{
|
||||
unsigned int i;
|
||||
uint8_t finalcount[8];
|
||||
|
||||
for (i = 0; i < 8; i++) {
|
||||
finalcount[i] = (uint8_t)((context->count[(i >= 4 ? 0 : 1)]
|
||||
>> ((3-(i & 3)) * 8) ) & 255); /* Endian independent */
|
||||
}
|
||||
SHA1Update(context, (const uint8_t *)"\200", 1);
|
||||
while ((context->count[0] & 504) != 448)
|
||||
SHA1Update(context, (const uint8_t *)"\0", 1);
|
||||
SHA1Update(context, finalcount, 8); /* Should cause a SHA1Transform() */
|
||||
|
||||
if (digest) {
|
||||
for (i = 0; i < 20; i++)
|
||||
digest[i] = (uint8_t)
|
||||
((context->state[i>>2] >> ((3-(i & 3)) * 8) ) & 255);
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* HAVE_SHA1_H */
|
||||
@ -61,6 +61,7 @@
|
||||
#include <pios_rfm22b_priv.h>
|
||||
#include <pios_ppm_out.h>
|
||||
#include <ecc.h>
|
||||
#include <sha1.h>
|
||||
|
||||
/* Local Defines */
|
||||
#define STACK_SIZE_BYTES 200
|
||||
@ -125,6 +126,7 @@
|
||||
#endif
|
||||
|
||||
#define CONNECTED_TIMEOUT (250 / portTICK_RATE_MS) /* ms */
|
||||
#define MAX_CHANNELS 32
|
||||
|
||||
/* Local type definitions */
|
||||
|
||||
@ -165,21 +167,6 @@ static const uint8_t OUT_FF[64] = {
|
||||
0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF
|
||||
};
|
||||
|
||||
// The randomized channel list.
|
||||
static const uint8_t channel_list[] = {
|
||||
68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92,
|
||||
191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74,
|
||||
155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95,
|
||||
3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205,
|
||||
240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161,
|
||||
124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238,
|
||||
173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24,
|
||||
217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223,
|
||||
150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228,
|
||||
75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80,
|
||||
136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 14
|
||||
};
|
||||
|
||||
/* Local function forwared declarations */
|
||||
static void pios_rfm22_task(void *parameters);
|
||||
static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev);
|
||||
@ -217,6 +204,10 @@ static void rfm22_clearLEDs();
|
||||
// Utility functions.
|
||||
static uint32_t pios_rfm22_time_difference_ms(portTickType start_time, portTickType end_time);
|
||||
static struct pios_rfm22b_dev *pios_rfm22_alloc(void);
|
||||
static void rfm22_hmac_sha1(const uint8_t *data, size_t len, uint8_t key[SHA1_DIGEST_LENGTH],
|
||||
uint8_t digest[SHA1_DIGEST_LENGTH]);
|
||||
static bool rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate datarate, uint8_t min,
|
||||
uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen);
|
||||
|
||||
// SPI read/write functions
|
||||
static void rfm22_assertCs(struct pios_rfm22b_dev *rfm22b_dev);
|
||||
@ -341,6 +332,18 @@ static const uint32_t data_rate[] = {
|
||||
256000, // 256 kbps, 433 MHz, 150 khz freq dev
|
||||
};
|
||||
|
||||
static const uint8_t channel_spacing[] = {
|
||||
1, /* 9.6kbps */
|
||||
2, /* 19.2kps */
|
||||
2, /* 32kps */
|
||||
2, /* 57.6kps */
|
||||
2, /* 64kps */
|
||||
3, /* 100kps */
|
||||
4, /* 128kps */
|
||||
4, /* 192kps */
|
||||
4 /* 256kps */
|
||||
};
|
||||
|
||||
static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth
|
||||
|
||||
static const uint8_t reg_1D[] = { 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40 }; // rfm22_afc_loop_gearshift_override
|
||||
@ -580,7 +583,7 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr)
|
||||
* @param[in] ppm_mode Should this modem send/receive ppm packets?
|
||||
* @param[in] oneway Only the coordinator can send packets if true.
|
||||
*/
|
||||
void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
|
||||
void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, __attribute__ ((unused)) uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only)
|
||||
{
|
||||
struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id;
|
||||
|
||||
@ -605,15 +608,9 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar
|
||||
}
|
||||
rfm22b_dev->packet_time = (ppm_mode ? packet_time_ppm[datarate] : packet_time[datarate]);
|
||||
|
||||
// Find the first N channels that meet the min/max criteria out of the random channel list.
|
||||
uint8_t num_found = 0;
|
||||
for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_channels[datarate]); ++i) {
|
||||
uint8_t idx = (i + chan_set) % RFM22B_NUM_CHANNELS;
|
||||
uint8_t chan = channel_list[idx];
|
||||
if ((chan >= min_chan) && (chan <= max_chan)) {
|
||||
rfm22b_dev->channels[num_found++] = chan;
|
||||
}
|
||||
}
|
||||
rfm22_gen_channels(rfm22_destinationID(rfm22b_dev), datarate, min_chan, max_chan,
|
||||
rfm22b_dev->channels, &num_found);
|
||||
|
||||
rfm22b_dev->num_channels = num_found;
|
||||
|
||||
@ -2577,6 +2574,88 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr)
|
||||
return in[1];
|
||||
}
|
||||
|
||||
|
||||
static void
|
||||
rfm22_hmac_sha1(const uint8_t *data, size_t len,
|
||||
uint8_t key[SHA1_DIGEST_LENGTH],
|
||||
uint8_t digest[SHA1_DIGEST_LENGTH])
|
||||
{
|
||||
uint8_t ipad[64] = {0};
|
||||
uint8_t opad[64] = {0};
|
||||
static SHA1_CTX *ctx;
|
||||
|
||||
ctx = pios_malloc(sizeof(SHA1_CTX));
|
||||
|
||||
memcpy(ipad, key, SHA1_DIGEST_LENGTH);
|
||||
memcpy(opad, key, SHA1_DIGEST_LENGTH);
|
||||
|
||||
for (int i = 0; i < 64; i++) {
|
||||
ipad[i] ^= 0x36;
|
||||
opad[i] ^= 0x5c;
|
||||
}
|
||||
|
||||
SHA1Init(ctx);
|
||||
SHA1Update(ctx, ipad, sizeof(ipad));
|
||||
SHA1Update(ctx, data, len);
|
||||
SHA1Final(digest, ctx);
|
||||
|
||||
SHA1Init(ctx);
|
||||
SHA1Update(ctx, opad, sizeof(opad));
|
||||
SHA1Update(ctx, digest, SHA1_DIGEST_LENGTH);
|
||||
SHA1Final(digest, ctx);
|
||||
|
||||
pios_free(ctx);
|
||||
}
|
||||
|
||||
static bool
|
||||
rfm22_gen_channels(uint32_t coordid, enum rfm22b_datarate rate, uint8_t min,
|
||||
uint8_t max, uint8_t channels[MAX_CHANNELS], uint8_t *clen)
|
||||
{
|
||||
uint32_t data = 0;
|
||||
uint8_t cpos = 0;
|
||||
uint8_t chan_range = (max / channel_spacing[rate] - min / channel_spacing[rate]) + 1;
|
||||
uint8_t key[SHA1_DIGEST_LENGTH] = {0};
|
||||
uint8_t digest[SHA1_DIGEST_LENGTH];
|
||||
uint8_t *all_channels;
|
||||
|
||||
all_channels = pios_malloc(RFM22B_NUM_CHANNELS);
|
||||
|
||||
memcpy(key, &coordid, sizeof(coordid));
|
||||
|
||||
for (int i = 0; i < chan_range; i++) {
|
||||
all_channels[i] = min / channel_spacing[rate] + i;
|
||||
}
|
||||
|
||||
int j = SHA1_DIGEST_LENGTH;
|
||||
for (int i = 0; i < chan_range && i < MAX_CHANNELS; i++) {
|
||||
uint8_t rnd;
|
||||
uint8_t r;
|
||||
uint8_t tmp;
|
||||
|
||||
if (j == SHA1_DIGEST_LENGTH) {
|
||||
rfm22_hmac_sha1((uint8_t *) &data, sizeof(data), key, digest);
|
||||
j = 0;
|
||||
data++;
|
||||
}
|
||||
rnd = digest[j];
|
||||
j++;
|
||||
r = rnd % (chan_range - i) + i;
|
||||
tmp = all_channels[i];
|
||||
all_channels[i] = all_channels[r];
|
||||
all_channels[r] = tmp;
|
||||
}
|
||||
|
||||
for (int i = 0; i < chan_range && cpos < MAX_CHANNELS; i++, cpos++) {
|
||||
channels[cpos] =all_channels[i] * channel_spacing[rate];
|
||||
}
|
||||
|
||||
*clen = cpos & 0xfe;
|
||||
|
||||
pios_free(all_channels);
|
||||
|
||||
return (*clen > 0);
|
||||
}
|
||||
|
||||
#endif /* PIOS_INCLUDE_RFM22B */
|
||||
|
||||
/**
|
||||
|
||||
@ -40,7 +40,7 @@
|
||||
// ************************************
|
||||
|
||||
#define RFM22B_MAX_PACKET_LEN 64
|
||||
#define RFM22B_NUM_CHANNELS 250
|
||||
#define RFM22B_NUM_CHANNELS 251
|
||||
|
||||
// ************************************
|
||||
|
||||
|
||||
@ -48,6 +48,9 @@ ifndef TESTAPP
|
||||
SRC += $(OPUAVOBJ)/uavobjectpersistence.c
|
||||
SRC += $(OPUAVOBJ)/eventdispatcher.c
|
||||
|
||||
## Misc library functions
|
||||
SRC += $(FLIGHTLIB)/sha1.c
|
||||
|
||||
## UAVObjects
|
||||
SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c
|
||||
SRC += $(OPUAVSYNTHDIR)/oplinksettings.c
|
||||
|
||||
@ -438,8 +438,8 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
// Set the radio configuration parameters.
|
||||
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
|
||||
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
|
||||
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
|
||||
|
||||
/* Set the PPM callback if we should be receiving PPM. */
|
||||
if (ppm_mode) {
|
||||
|
||||
@ -91,6 +91,7 @@ ifndef TESTAPP
|
||||
SRC += $(FLIGHTLIB)/insgps13state.c
|
||||
SRC += $(FLIGHTLIB)/auxmagsupport.c
|
||||
SRC += $(FLIGHTLIB)/lednotification.c
|
||||
SRC += $(FLIGHTLIB)/sha1.c
|
||||
|
||||
## UAVObjects
|
||||
include ./UAVObjects.inc
|
||||
|
||||
@ -770,8 +770,8 @@ void PIOS_Board_Init(void)
|
||||
}
|
||||
|
||||
/* Set the radio configuration parameters. */
|
||||
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
|
||||
PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID);
|
||||
PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only);
|
||||
|
||||
/* Set the PPM callback if we should be receiving PPM. */
|
||||
if (ppm_mode) {
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user