forked from floe/BTLE
-
Notifications
You must be signed in to change notification settings - Fork 4
/
BTLE.cpp
256 lines (198 loc) · 6.77 KB
/
BTLE.cpp
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
/*
* Copyright (C) 2013 Florian Echtler <floe@butterbrot.org>
*
* This program is free software; you can redistribute it and/or
* modify it under the terms of the GNU General Public License
* version 3 as published by the Free Software Foundation.
*/
#include <BTLE.h>
const uint8_t channel[3] = {37,38,39}; // logical BTLE channel number (37-39)
const uint8_t frequency[3] = { 2,26,80}; // physical frequency (2400+x MHz)
// This is a rather convoluted hack to extract the month number from the build date in
// the __DATE__ macro using a small hash function + lookup table. Since all inputs are
// const, this can be fully resolved by the compiler and saves over 200 bytes of code.
#define month(m) month_lookup[ (( ((( (m[0] % 24) * 13) + m[1]) % 24) * 13) + m[2]) % 24 ]
const uint8_t month_lookup[24] = { 0,6,0,4,0,1,0,17,0,8,0,0,3,0,0,0,18,2,16,5,9,0,1,7 };
// change buffer contents to "wire bit order"
void BTLE::swapbuf( uint8_t len ) {
uint8_t* buf = (uint8_t*)&buffer;
while (len--) {
uint8_t a = *buf;
uint8_t v = 0;
if (a & 0x80) v |= 0x01;
if (a & 0x40) v |= 0x02;
if (a & 0x20) v |= 0x04;
if (a & 0x10) v |= 0x08;
if (a & 0x08) v |= 0x10;
if (a & 0x04) v |= 0x20;
if (a & 0x02) v |= 0x40;
if (a & 0x01) v |= 0x80;
*(buf++) = v;
}
}
// constructor
BTLE::BTLE( RF24* _radio ):
radio(_radio),
current(0)
{ }
// Simple converter from arduino float to a nRF_Float.
// Supports values from -167772 to +167772, with two decimal places.
nRF_Float
BTLE::to_nRF_Float(float t) {
int32_t ret;
int32_t exponent = -2;
ret = ((exponent & 0xff) << 24) | (((int32_t)(t * 100)) & 0xffffff);
return ret;
}
// set BTLE-compatible radio parameters
void BTLE::begin( const char* _name ) {
name = _name;
radio->begin();
// set standard parameters
radio->setAutoAck(false);
radio->setDataRate(RF24_1MBPS);
radio->disableCRC();
radio->setChannel( frequency[current] );
radio->setRetries(0,0);
radio->setPALevel(RF24_PA_MAX);
// set advertisement address: 0x8E89BED6 (bit-reversed -> 0x6B7D9171)
radio->setAddressSize(4);
radio->openReadingPipe(0,0x6B7D9171);
radio->openWritingPipe( 0x6B7D9171);
radio->powerUp();
}
// set the current channel (from 37 to 39)
void BTLE::setChannel( uint8_t num ) {
current = min(2,max(0,num-37));
radio->setChannel( frequency[current] );
}
// hop to the next channel
void BTLE::hopChannel() {
current++;
if (current >= sizeof(channel)) current = 0;
radio->setChannel( frequency[current] );
}
// Broadcast an advertisement packet with optional payload
// Data type will be 0xFF (Manufacturer Specific Data)
bool BTLE::advertise( void* buf, uint8_t buflen ) {
return advertise(0xFF, buf, buflen);
}
// Broadcast an advertisement packet with a specific data type
// Standardized data types can be seen here:
// https://www.bluetooth.org/en-us/specification/assigned-numbers/generic-access-profile
bool BTLE::advertise( uint8_t data_type, void* buf, uint8_t buflen ) {
// name & total payload size
uint8_t namelen = strlen(name);
uint8_t pls = 0;
// insert pseudo-random MAC address
buffer.mac[0] = ((__TIME__[6]-0x30) << 4) | (__TIME__[7]-0x30);
buffer.mac[1] = ((__TIME__[3]-0x30) << 4) | (__TIME__[4]-0x30);
buffer.mac[2] = ((__TIME__[0]-0x30) << 4) | (__TIME__[1]-0x30);
buffer.mac[3] = ((__DATE__[4]-0x30) << 4) | (__DATE__[5]-0x30);
buffer.mac[4] = month(__DATE__);
buffer.mac[5] = ((__DATE__[9]-0x30) << 4) | (__DATE__[10]-0x30);
// add device descriptor chunk
chunk(buffer,pls)->size = 0x02; // chunk size: 2
chunk(buffer,pls)->type = 0x01; // chunk type: device flags
chunk(buffer,pls)->data[0]= 0x05; // flags: LE-only, limited discovery mode
pls += 3;
// add "complete name" chunk
chunk(buffer,pls)->size = namelen+1; // chunk size
chunk(buffer,pls)->type = 0x09; // chunk type
for (uint8_t i = 0; i < namelen; i++)
chunk(buffer,pls)->data[i] = name[i];
pls += namelen+2;
// add custom data, if applicable
if (buflen > 0) {
chunk(buffer,pls)->size = buflen+1; // chunk size
chunk(buffer,pls)->type = data_type; // chunk type
for (uint8_t i = 0; i < buflen; i++)
chunk(buffer,pls)->data[i] = ((uint8_t*)buf)[i];
pls += buflen+2;
}
// total payload size must be 21 bytes or less
if (pls > 21)
return false;
// assemble header
buffer.pdu_type = 0x42; // PDU type: ADV_NONCONN_IND, TX address is random
buffer.pl_size = pls+6; // set final payload size in header incl. MAC
// calculate CRC over header+MAC+payload, append after payload
uint8_t* outbuf = (uint8_t*)&buffer;
crc( pls+8, outbuf+pls+8);
// whiten header+MAC+payload+CRC, swap bit order
whiten( pls+11 );
swapbuf( pls+11 );
// flush buffers and send
radio->stopListening();
radio->write( outbuf, pls+11 );
return true;
}
// listen for advertisement packets
bool BTLE::listen(int timeout) {
radio->startListening();
delay(timeout);
if (!radio->available())
return false;
bool done = false;
uint8_t total_size = 0;
uint8_t* inbuf = (uint8_t*)&buffer;
while (!done) {
// fetch the payload, and check if there are more left
done = radio->read( inbuf, sizeof(buffer) );
// decode: swap bit order, un-whiten
swapbuf( sizeof(buffer) );
whiten( sizeof(buffer) );
// size is w/o header+CRC -> add 2 bytes header
total_size = inbuf[1]+2;
uint8_t in_crc[3];
// calculate & compare CRC
crc( total_size, in_crc );
for (uint8_t i = 0; i < 3; i++)
if (inbuf[total_size+i] != in_crc[i])
return false;
}
return true;
}
// see BT Core Spec 4.0, Section 6.B.3.2
void BTLE::whiten( uint8_t len ) {
uint8_t* buf = (uint8_t*)&buffer;
// initialize LFSR with current channel, set bit 6
uint8_t lfsr = channel[current] | 0x40;
while (len--) {
uint8_t res = 0;
// LFSR in "wire bit order"
for (uint8_t i = 1; i; i <<= 1) {
if (lfsr & 0x01) {
lfsr ^= 0x88;
res |= i;
}
lfsr >>= 1;
}
*(buf++) ^= res;
}
}
// see BT Core Spec 4.0, Section 6.B.3.1.1
void BTLE::crc( uint8_t len, uint8_t* dst ) {
uint8_t* buf = (uint8_t*)&buffer;
// initialize 24-bit shift register in "wire bit order"
// dst[0] = bits 23-16, dst[1] = bits 15-8, dst[2] = bits 7-0
dst[0] = 0xAA;
dst[1] = 0xAA;
dst[2] = 0xAA;
while (len--) {
uint8_t d = *(buf++);
for (uint8_t i = 1; i; i <<= 1, d >>= 1) {
// save bit 23 (highest-value), left-shift the entire register by one
uint8_t t = dst[0] & 0x01; dst[0] >>= 1;
if (dst[1] & 0x01) dst[0] |= 0x80; dst[1] >>= 1;
if (dst[2] & 0x01) dst[1] |= 0x80; dst[2] >>= 1;
// if the bit just shifted out (former bit 23) and the incoming data
// bit are not equal (i.e. bit_out ^ bit_in == 1) => toggle tap bits
if (t != (d & 1)) {
// toggle register tap bits (=XOR with 1) according to CRC polynom
dst[2] ^= 0xDA; // 0b11011010 inv. = 0b01011011 ^= x^6+x^4+x^3+x+1
dst[1] ^= 0x60; // 0b01100000 inv. = 0b00000110 ^= x^10+x^9
}
}
}
}