1 /*******************************************************************************
2 *
3 * Copyright (c) 2018, Raccon BLE Sniffer
4 * All rights reserved.
5 *
6 * Redistribution and use in source and binary forms, with or without
7 * modification, are permitted provided that the following conditions are
8 * met:
9 *
10 * * Redistributions of source code must retain the above copyright
11 * notice, this list of conditions and the following disclaimer.
12 * * Redistributions in binary form must reproduce the above
13 * copyright notice, this list of conditions and the following disclaimer
14 * in the documentation and/or other materials provided with the
15 * distribution.
16 * * Neither the name of "btlejack2" nor the names of its
17 * contributors may be used to endorse or promote products derived from
18 * this software without specific prior written permission.
19 *
20 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
24 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
25 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
26 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
27 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
28 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
29 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 *
32 *******************************************************************************/
33
34 #include <string.h>
35 #include <strings.h>
36 #include <assert.h>
37
38 #include "debug.h"
39 #include "hopping.h"
40
41 #define GET_BIT( b, n ) ((b[(n&UINT32_C(0xFFFFFFF8))>>3]>>(n&0x7))&1)
42
hopping_init(hopping_t * c)43 void hopping_init( hopping_t *c ) {
44 assert( c != NULL );
45
46 memset( c, 0, sizeof(hopping_t) );
47 }
48
hopping_set_channel_map(hopping_t * c,const uint8_t * chm)49 void hopping_set_channel_map( hopping_t *c, const uint8_t *chm ) {
50 assert( c != NULL );
51 assert( chm != NULL );
52
53 memcpy( c->chMap, chm, 5 );
54
55 c->chCnt = 0;
56 for(int i=0; i<37; ++i) {
57 LOG_DBG("%d(%d)\n", i, (i&UINT32_C(0xFFFFFFF8))>>3 );
58 LOG_DBG("%d(%d)\n", i, GET_BIT( c->chMap, i ) );
59 if( 1 == GET_BIT( c->chMap, i ) ) {
60 c->chRemap[c->chCnt++] = i;
61 }
62 }
63
64 // for(int i=0; i<c->chCnt; ++i)
65 // printf("%d\n", c->chRemap[i] );
66
67 // printf("chCnt: %d\n", c->chCnt );
68 }
69
hopping_csa1_set_hop_increment(hopping_t * c,uint8_t hopIncrement)70 void hopping_csa1_set_hop_increment( hopping_t *c, uint8_t hopIncrement ){
71 assert( c != NULL );
72
73 c->hopIncrement = hopIncrement;
74 }
75
hopping_csa1_get_next_channel(hopping_t * c)76 uint8_t hopping_csa1_get_next_channel( hopping_t *c ) {
77 assert( c != NULL );
78
79 c->currentCh = (c->currentCh + c->hopIncrement) % 37;
80 if( 1 == GET_BIT( c->chMap, c->currentCh ) ) {
81 return c->currentCh;
82 } else {
83 return c->chRemap[ c->currentCh % c->chCnt ];
84 }
85 }
86
87 // channelIdentifier = (Access Address31-16) XOR (Access Address15-0)
hopping_csa2_set_access_address(hopping_t * c,uint32_t accessAddress)88 void hopping_csa2_set_access_address( hopping_t *c, uint32_t accessAddress ){
89 assert( c != NULL );
90
91 c->channelIdentifier = (accessAddress >> 16) ^ (accessAddress & 0xffff);
92 }
93
94 // @TODO rbit intrinsic could be used
hopping_csa2_permutation(uint16_t v)95 static uint16_t hopping_csa2_permutation(uint16_t v){
96 v = ((v >> 1) & 0x5555) | ((v & 0x5555) << 1);
97 v = ((v >> 2) & 0x3333) | ((v & 0x3333) << 2);
98 v = ((v >> 4) & 0x0F0F) | ((v & 0x0F0F) << 4);
99 return v;
100 }
101
hopping_csa2_mam(uint16_t a,uint16_t b)102 static uint16_t hopping_csa2_mam(uint16_t a, uint16_t b){
103 return (17 * a) + b;
104 }
105
hopping_csa2_prn_e(hopping_t * c,uint16_t counter)106 static uint16_t hopping_csa2_prn_e(hopping_t *c, uint16_t counter){
107 assert( c != NULL );
108
109 // see Bluetooth Spec v5, 4.5.8.3.3 Unmapped Event Channel Selection
110 uint16_t v = counter ^ c->channelIdentifier;
111 v = hopping_csa2_permutation (v);
112 v = hopping_csa2_mam(v, c->channelIdentifier);
113 v = hopping_csa2_permutation (v);
114 v = hopping_csa2_mam(v, c->channelIdentifier);
115 v = hopping_csa2_permutation (v);
116 v = hopping_csa2_mam(v, c->channelIdentifier);
117 v = v ^ c->channelIdentifier;
118 return v;
119 }
120
hopping_csa2_get_channel_for_counter(hopping_t * c,uint16_t counter)121 uint8_t hopping_csa2_get_channel_for_counter( hopping_t *c, uint16_t counter ){
122 assert( c != NULL );
123
124 uint16_t prn_e = hopping_csa2_prn_e( c, counter);
125 uint16_t unmapped_channel = prn_e % 37;
126 if ( 1 == GET_BIT( c->chMap, unmapped_channel)){
127 return unmapped_channel;
128 } else {
129 uint16_t remappingIndex = (((uint32_t) c->chCnt) * prn_e) >> 16;
130 return c->chRemap[ remappingIndex ];
131 }
132 }
133
134
135
136
137
138