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 43 void hopping_init( hopping_t *c ) { 44 assert( c != NULL ); 45 46 memset( c, 0, sizeof(hopping_t) ); 47 } 48 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 70 void hopping_csa1_set_hop_increment( hopping_t *c, uint8_t hopIncrement ){ 71 assert( c != NULL ); 72 73 c->hopIncrement = hopIncrement; 74 } 75 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) 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 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 102 static uint16_t hopping_csa2_mam(uint16_t a, uint16_t b){ 103 return (17 * a) + b; 104 } 105 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 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