1#!/usr/bin/env python 2# 3# Simulate network of Bluetooth Controllers 4# 5# Each simulated controller has an HCI H4 interface 6# Network configuration will be stored in a YAML file or similar 7# 8# Copyright 2017 BlueKitchen GmbH 9# 10 11 12import os 13import pty 14import select 15import subprocess 16import sys 17import bisect 18import time 19 20from Crypto.Cipher import AES 21from Crypto import Random 22 23def little_endian_read_16(buffer, pos): 24 return ord(buffer[pos]) + (ord(buffer[pos+1]) << 8) 25 26def as_hex(data): 27 str_list = [] 28 for byte in data: 29 str_list.append("{0:02x} ".format(ord(byte))) 30 return ''.join(str_list) 31 32adv_type_names = ['ADV_IND', 'ADV_DIRECT_IND_HIGH', 'ADV_SCAN_IND', 'ADV_NONCONN_IND', 'ADV_DIRECT_IND_LOW'] 33timers_timeouts = [] 34timers_callbacks = [] 35 36class H4Parser: 37 38 def __init__(self): 39 self.packet_type = "NONE" 40 self.reset() 41 42 def set_packet_handler(self, handler): 43 self.handler = handler 44 45 def reset(self): 46 self.bytes_to_read = 1 47 self.buffer = '' 48 self.state = "H4_W4_PACKET_TYPE" 49 50 def parse(self, data): 51 self.buffer += data 52 self.bytes_to_read -= 1 53 if self.bytes_to_read == 0: 54 if self.state == "H4_W4_PACKET_TYPE": 55 self.buffer = '' 56 if data == chr(1): 57 # cmd 58 self.packet_type = "CMD" 59 self.state = "W4_CMD_HEADER" 60 self.bytes_to_read = 3 61 if data == chr(2): 62 # acl 63 self.packet_type = "ACL" 64 self.state = "W4_ACL_HEADER" 65 self.bytes_to_read = 4 66 return 67 if self.state == "W4_CMD_HEADER": 68 self.bytes_to_read = ord(self.buffer[2]) 69 self.state = "H4_W4_PAYLOAD" 70 if self.bytes_to_read > 0: 71 return 72 # fall through to handle payload len = 0 73 if self.state == "W4_ACL_HEADER": 74 self.bytes_to_read = little_endian_read_16(buffer, 2) 75 self.state = "H4_W4_PAYLOAD" 76 if self.bytes_to_read > 0: 77 return 78 # fall through to handle payload len = 0 79 if self.state == "H4_W4_PAYLOAD": 80 self.handler(self.packet_type, self.buffer) 81 self.reset() 82 return 83 84class HCIController: 85 86 def __init__(self): 87 self.fd = -1 88 self.random = Random.new() 89 self.name = 'BTstack Mesh Simulator' 90 self.bd_addr = 'aaaaaa' 91 self.parser = H4Parser() 92 self.parser.set_packet_handler(self.packet_handler) 93 self.adv_enabled = 0 94 self.adv_type = 0 95 self.adv_interval_min = 0 96 self.adv_interval_max = 0 97 self.adv_data = '' 98 self.scan_enabled = False 99 100 def parse(self, data): 101 self.parser.parse(data) 102 103 def set_fd(self,fd): 104 self.fd = fd 105 106 def set_bd_addr(self, bd_addr): 107 self.bd_addr = bd_addr 108 109 def set_name(self, name): 110 self.name = name 111 112 def set_adv_handler(self, adv_handler, adv_handler_context): 113 self.adv_handler = adv_handler 114 self.adv_handler_context = adv_handler_context 115 116 def is_scanning(self): 117 return self.scan_enabled 118 119 def emit_command_complete(self, opcode, result): 120 # type, event, len, num commands, opcode, result 121 os.write(self.fd, '\x04\x0e' + chr(3 + len(result)) + chr(1) + chr(opcode & 255) + chr(opcode >> 8) + result) 122 123 def emit_adv_report(self, event_type, rssi): 124 # type, event, len, Subevent_Code, Num_Reports, Event_Type[i], Address_Type[i], Address[i], Length[i], Data[i], RSSI[i] 125 event = '\x04\x3e' + chr(12 + len(self.adv_data)) + chr(2) + chr(1) + chr(event_type) + chr(0) + self.bd_addr[::-1] + chr(len(self.adv_data)) + self.adv_data + chr(rssi) 126 self.adv_handler(self.adv_handler_context, event) 127 128 def handle_set_adv_enable(self, enable): 129 self.adv_enabled = enable 130 print('Node %s adv enable %u' % (self.name, self.adv_enabled)) 131 if self.adv_enabled: 132 add_timer(1, self.handle_adv_timer, self) 133 else: 134 remove_timer(self.handle_adv_timer, self) 135 136 def handle_set_adv_data(self, data): 137 self.adv_data = data 138 print('Node %s adv data %s' % (self.name, as_hex(self.adv_data))) 139 140 def handle_set_adv_params(self, interval_min, interval_max, adv_type): 141 self.adv_interval_min = interval_min * 0.625 142 self.adv_interval_max = interval_max * 0.625 143 self.adv_type = adv_type 144 print('Node %s adv interval min/max %u/%u ms, type %s' % (self.name, self.adv_interval_min, self.adv_interval_max, adv_type_names[self.adv_type])) 145 146 def handle_adv_timer(self, context): 147 if self.adv_enabled: 148 self.emit_adv_report(0, 0) 149 add_timer(self.adv_interval_min, self.handle_adv_timer, self) 150 151 def packet_handler(self, packet_type, packet): 152 opcode = little_endian_read_16(packet, 0) 153 # print ("%s, opcode 0x%04x" % (self.name, opcode)) 154 if opcode == 0x0c03: 155 self.emit_command_complete(opcode, '\x00') 156 return 157 if opcode == 0x1001: 158 self.emit_command_complete(opcode, '\x00\x10\x00\x06\x86\x1d\x06\x0a\x00\x86\x1d') 159 return 160 if opcode == 0x0c14: 161 self.emit_command_complete(opcode, '\x00' + self.name) 162 return 163 if opcode == 0x1002: 164 self.emit_command_complete(opcode, '\x00\xff\xff\xff\x03\xfe\xff\xff\xff\xff\xff\xff\xff\xf3\x0f\xe8\xfe\x3f\xf7\x83\xff\x1c\x00\x00\x00\x61\xf7\xff\xff\x7f\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00\x00') 165 return 166 if opcode == 0x1009: 167 # read bd_addr 168 self.emit_command_complete(opcode, '\x00' + self.bd_addr[::-1]) 169 return 170 if opcode == 0x1005: 171 # read buffer size 172 self.emit_command_complete(opcode, '\x00\x36\x01\x40\x0a\x00\x08\x00') 173 return 174 if opcode == 0x1003: 175 # read local supported features 176 self.emit_command_complete(opcode, '\x00\xff\xff\x8f\xfe\xf8\xff\x5b\x87') 177 return 178 if opcode == 0x0c01: 179 self.emit_command_complete(opcode, '\x00') 180 return 181 if opcode == 0x2002: 182 # le read buffer size 183 self.emit_command_complete(opcode, '\x00\x00\x00\x00') 184 return 185 if opcode == 0x200f: 186 # read whitelist size 187 self.emit_command_complete(opcode, '\x00\x19') 188 return 189 if opcode == 0x200b: 190 # set scan parameters 191 self.emit_command_complete(opcode, '\x00') 192 return 193 if opcode == 0x200c: 194 # set scan enabled 195 self.scan_enabled = ord(packet[3]) 196 self.emit_command_complete(opcode, '\x00') 197 return 198 if opcode == 0x0c6d: 199 # write le host supported 200 self.emit_command_complete(opcode, '\x00') 201 return 202 if opcode == 0x2017: 203 # LE Encrypt - key 16, data 16 204 key = packet[18:2:-1] 205 data = packet[35:18:-1] 206 cipher = AES.new(key) 207 result = cipher.encrypt(data) 208 self.emit_command_complete(opcode, result[::-1]) 209 return 210 if opcode == 0x2018: 211 # LE Rand 212 self.emit_command_complete(opcode, '\x00' + self.random.read(8)) 213 return 214 if opcode == 0x2006: 215 # Set Adv Params 216 self.handle_set_adv_params(little_endian_read_16(packet,3), little_endian_read_16(packet,5), ord(packet[6])) 217 self.emit_command_complete(opcode, '\x00') 218 return 219 if opcode == 0x2008: 220 # Set Adv Data 221 len = ord(packet[3]) 222 self.handle_set_adv_data(packet[4:4+len]) 223 self.emit_command_complete(opcode, '\x00') 224 return 225 if opcode == 0x200a: 226 # Set Adv Enable 227 self.handle_set_adv_enable(ord(packet[3])) 228 self.emit_command_complete(opcode, '\x00') 229 return 230 print("Opcode 0x%0x not handled!" % opcode) 231 232class Node: 233 234 def __init__(self): 235 self.name = 'node' 236 self.master = -1 237 self.slave = -1 238 self.slave_ttyname = '' 239 self.controller = HCIController() 240 241 def set_name(self, name): 242 self.controller.set_name(name) 243 self.name = name 244 245 def get_name(self): 246 return self.name 247 248 def set_bd_addr(self, bd_addr): 249 self.controller.set_bd_addr(bd_addr) 250 251 def start_process(self): 252 print('Node: %s' % self.name) 253 (self.master, self.slave) = pty.openpty() 254 self.slave_ttyname = os.ttyname(self.slave) 255 print('- tty %s' % self.slave_ttyname) 256 print('- fd %u' % self.master) 257 self.controller.set_fd(self.master) 258 subprocess.Popen(['./mesh', '-d', self.slave_ttyname]) 259 260 def get_master(self): 261 return self.master 262 263 def parse(self, c): 264 self.controller.parse(c) 265 266 def set_adv_handler(self, adv_handler, adv_handler_context): 267 self.controller.set_adv_handler(adv_handler, adv_handler_context) 268 269 def inject_packet(self, event): 270 os.write(self.master, event) 271 272 def is_scanning(self): 273 return self.controller.is_scanning() 274 275def get_time_millis(): 276 return int(round(time.time() * 1000)) 277 278def add_timer(timeout_ms, callback, context): 279 global timers_timeouts 280 global timers_callbacks 281 282 timeout = get_time_millis() + timeout_ms; 283 pos = bisect.bisect(timers_timeouts, timeout) 284 timers_timeouts.insert(pos, timeout) 285 timers_callbacks.insert(pos, (callback, context)) 286 287def remove_timer(callback, context): 288 if (callback, context) in timers_callbacks: 289 indices = [timers_callbacks.index(t) for t in timers_callbacks if t[0] == callback and t[1] == context] 290 index = indices[0] 291 timers_callbacks.pop(index) 292 timers_timeouts.pop(index) 293 294def run(nodes): 295 # create map fd -> node 296 nodes_by_fd = { node.get_master():node for node in nodes} 297 read_fds = nodes_by_fd.keys() 298 while True: 299 # process expired timers 300 time_ms = get_time_millis() 301 while len(timers_timeouts) and timers_timeouts[0] < time_ms: 302 timers_timeouts.pop(0) 303 (callback,context) = timers_callbacks.pop(0) 304 callback(context) 305 # timer timers_timeouts? 306 if len(timers_timeouts): 307 timeout = (timers_timeouts[0] - time_ms) / 1000.0 308 (read_ready, write_ready, exception_ready) = select.select(read_fds,[],[], timeout) 309 else: 310 (read_ready, write_ready, exception_ready) = select.select(read_fds,[],[]) 311 for fd in read_ready: 312 node = nodes_by_fd[fd] 313 c = os.read(fd, 1) 314 node.parse(c) 315 316def adv_handler(src_node, event): 317 global nodes 318 # print('adv from %s' % src_node.get_name()) 319 for dst_node in nodes: 320 if src_node == dst_node: 321 continue 322 if not dst_node.is_scanning(): 323 continue 324 print('Adv %s -> %s - %s' % (src_node.get_name(), dst_node.get_name(), as_hex(event[14:-1]))) 325 dst_node.inject_packet(event) 326 327# parse configuration file passed in via cmd line args 328# TODO 329 330node1 = Node() 331node1.set_name('node_1') 332node1.set_bd_addr('aaaaaa') 333node1.set_adv_handler(adv_handler, node1) 334node1.start_process() 335 336node2 = Node() 337node2.set_name('node_2') 338node2.set_bd_addr('bbbbbb') 339node2.set_adv_handler(adv_handler, node2) 340node2.start_process() 341 342nodes = [node1, node2] 343 344run(nodes) 345