def gap_passkey_entry_req_ev(bd_addr=None, bd_addr_type=None): logging.debug("%s %r %r", gap_passkey_entry_req_ev.__name__, bd_addr, bd_addr_type) iutctl = get_iut() tuple_hdr, tuple_data = iutctl.btp_socket.read() logging.debug("received %r %r", tuple_hdr, tuple_data) btp_hdr_check(tuple_hdr, defs.BTP_SERVICE_ID_GAP, defs.GAP_EV_PASSKEY_ENTRY_REQ) fmt = '<B6s' if len(tuple_data[0]) != struct.calcsize(fmt): raise BTPError("Invalid data length") # Unpack and swap address _addr_type, _addr = struct.unpack(fmt, tuple_data[0]) _addr = binascii.hexlify(_addr[::-1]).lower().decode('utf-8') bd_addr = pts_addr_get(bd_addr) bd_addr_type = pts_addr_type_get(bd_addr_type) if _addr_type != bd_addr_type or _addr != bd_addr: raise BTPError("Received data mismatch") stack = get_stack() if not stack.gap.passkey.data: # Generate some passkey stack.gap.passkey.data = randint(0, 999999) gap_passkey_entry_rsp(bd_addr, bd_addr_type, stack.gap.passkey.data)
def send_wait_rsp(self, svc_id, op, ctrl_index, data, cb=None, user_data=None): super().send(svc_id, op, ctrl_index, data) ret = True while ret: tuple_hdr, tuple_data = self.read() if tuple_hdr.svc_id != svc_id: raise BTPError( "Incorrect service ID %s in the response, expected %s!" % (tuple_hdr.svc_id, svc_id)) if tuple_hdr.op == defs.BTP_STATUS: raise BTPError("Error opcode in response!") if op != tuple_hdr.op: raise BTPError( "Invalid opcode 0x%.2x in the response, expected 0x%.2x!" % (tuple_hdr.op, op)) if cb and callable(cb): ret = cb(tuple_data, user_data) else: return tuple_data
def btp_hdr_check(rcv_hdr, exp_svc_id, exp_op=None): if rcv_hdr.svc_id != exp_svc_id: raise BTPError( "Incorrect service ID %s in the response, expected %s!" % (rcv_hdr.svc_id, exp_svc_id)) if rcv_hdr.op == defs.BTP_STATUS: raise BTPError("Error opcode in response!") if exp_op and exp_op != rcv_hdr.op: raise BTPError( "Invalid opcode 0x%.2x in the response, expected 0x%.2x!" % (rcv_hdr.op, exp_op))
def gap_read_ctrl_info(): logging.debug("%s", gap_read_ctrl_info.__name__) iutctl = get_iut() iutctl.btp_socket.send(*GAP['read_ctrl_info']) tuple_hdr, tuple_data = iutctl.btp_socket.read() logging.debug("received %r %r", tuple_hdr, tuple_data) btp_hdr_check(tuple_hdr, defs.BTP_SERVICE_ID_GAP, defs.GAP_READ_CONTROLLER_INFO) fmt = '<6sII3s249s11s' if len(tuple_data[0]) < struct.calcsize(fmt): raise BTPError("Invalid data length") _addr, _supp_set, _curr_set, _cod, _name, _name_sh = \ struct.unpack_from(fmt, tuple_data[0]) _addr = binascii.hexlify(_addr[::-1]).lower() stack = get_stack() addr_type = Addr.le_random if \ (_curr_set & (1 << defs.GAP_SETTINGS_PRIVACY)) or \ (_curr_set & (1 << defs.GAP_SETTINGS_STATIC_ADDRESS)) else \ Addr.le_public stack.gap.iut_addr_set(_addr, addr_type) logging.debug("IUT address %r", stack.gap.iut_addr_get_str()) __gap_current_settings_update(_curr_set)
def mesh_net_send(ttl, src, dst, payload): logging.debug("%s %r %r %r %r", mesh_net_send.__name__, ttl, src, dst, payload) if ttl is None: ttl = 0xff # Use default TTL elif isinstance(ttl, str): ttl = int(ttl, 16) if isinstance(src, str): src = int(src, 16) if isinstance(dst, str): dst = int(dst, 16) payload = binascii.unhexlify(payload) payload_len = len(payload) if payload_len > 0xff: raise BTPError("Payload exceeds PDU") data = bytearray(struct.pack("<BHHB", ttl, src, dst, payload_len)) data.extend(payload) iutctl = get_iut() iutctl.btp_socket.send_wait_rsp(*MESH['net_send'], data=data)
def gap_device_found_ev_(gap, data, data_len): logging.debug("%s %r", gap_device_found_ev_.__name__, data) fmt = '<B6sBBH' if len(data) < struct.calcsize(fmt): raise BTPError("Invalid data length") addr_type, addr, rssi, flags, eir_len = struct.unpack_from(fmt, data) eir = data[struct.calcsize(fmt):] if len(eir) != eir_len: raise BTPError("Invalid data length") addr = binascii.hexlify(addr[::-1]).lower() logging.debug("found %r type %r eir %r", addr, addr_type, eir) stack = get_stack() stack.gap.found_devices.data.append( LeAdv(addr_type, addr, rssi, flags, eir))
def gap_conn_param_update_ev_(gap, data, data_len): logging.debug("%s", gap_conn_param_update_ev_.__name__) logging.debug("received %r", data) fmt = '<B6sHHH' if len(data) != struct.calcsize(fmt): raise BTPError("Invalid data length") _addr_t, _addr, _itvl, _latency, _timeout = struct.unpack_from(fmt, data) # Convert addresses to lower case _addr = binascii.hexlify(_addr[::-1]).lower() if _addr_t != pts_addr_type_get() or _addr.decode( 'utf-8') != pts_addr_get(): raise BTPError("Received data mismatch") logging.debug("received %r", (_addr_t, _addr, _itvl, _latency, _timeout)) gap.set_conn_params(ConnParams(_itvl, _latency, _timeout))
def gap_identity_resolved_ev_(gap, data, data_len): logging.debug("%s", gap_identity_resolved_ev_.__name__) logging.debug("received %r", data) fmt = '<B6sB6s' if len(data) != struct.calcsize(fmt): raise BTPError("Invalid data length") _addr_t, _addr, _id_addr_t, _id_addr = struct.unpack_from(fmt, data) # Convert addresses to lower case _addr = binascii.hexlify(_addr[::-1]).lower() _id_addr = binascii.hexlify(_id_addr[::-1]).lower() if _addr_t != pts_addr_type_get() or _addr.decode( 'utf-8') != pts_addr_get(): raise BTPError("Received data mismatch") # Update RPA with Identity Address set_pts_addr(_id_addr, _id_addr_t)
def wait_iut_ready_event(self): """Wait until IUT sends ready event after power up""" self.reset() tuple_hdr, tuple_data = self._btp_worker.read() if (tuple_hdr.svc_id != defs.BTP_SERVICE_ID_CORE or tuple_hdr.op != defs.CORE_EV_IUT_READY): err = BTPError("Failed to get ready event") log("Unexpected event received (%s), expected IUT ready!", err) raise err else: log("IUT ready event received OK")
def wait_iut_ready_event(self): """Wait until IUT sends ready event after power up""" tuple_hdr, tuple_data = self.btp_socket.read() try: if (tuple_hdr.svc_id != defs.BTP_SERVICE_ID_CORE or tuple_hdr.op != defs.CORE_EV_IUT_READY): raise BTPError("Failed to get ready event") except BTPError as err: log("Unexpected event received (%s), expected IUT ready!", err) self.stop() else: log("IUT ready event received OK")
def send_wait_rsp(self, svc_id, op, ctrl_index, data): self._lock.acquire() try: super().send(svc_id, op, ctrl_index, data) tuple_hdr, tuple_data = self.read() if tuple_hdr.svc_id != svc_id: raise BTPError( "Incorrect service ID %s in the response, expected %s!" % (tuple_hdr.svc_id, svc_id)) if tuple_hdr.op == defs.BTP_STATUS: raise BTPError("Error opcode in response!") if op != tuple_hdr.op: raise BTPError( "Invalid opcode 0x%.2x in the response, expected 0x%.2x!" % (tuple_hdr.op, op)) return tuple_data finally: self._lock.release()
def gap_pairing_consent_ev_(gap, data, data_len): logging.debug("%s", gap_pairing_consent_ev_.__name__) logging.debug("received %r", data) fmt = '<B6s' if len(data) != struct.calcsize(fmt): raise BTPError("Invalid data length") _addr_t, _addr, = struct.unpack_from(fmt, data) _addr = binascii.hexlify(_addr[::-1]).decode() logging.debug("received %r", (_addr_t, _addr))
def wait_iut_ready_event(self): self.reset() tuple_hdr, tuple_data = self._btp_worker.read() try: if (tuple_hdr.svc_id != defs.BTP_SERVICE_ID_CORE or tuple_hdr.op != defs.CORE_EV_IUT_READY): raise BTPError("Failed to get ready event") except BTPError as err: log("Unexpected event received (%s), expected IUT ready!", err) self.stop() else: log("IUT ready event received OK")
def core_unreg_svc_rsp_succ(): logging.debug("%s", core_unreg_svc_rsp_succ.__name__) iutctl = get_iut() expected_frame = ((defs.BTP_SERVICE_ID_CORE, defs.CORE_UNREGISTER_SERVICE, defs.BTP_INDEX_NONE, 0), (b'', )) tuple_hdr, tuple_data = iutctl.btp_socket.read() logging.debug("received %r %r", tuple_hdr, tuple_data) logging.debug("expected %r", expected_frame) if (tuple_hdr, tuple_data) != expected_frame: logging.error("frames mismatch") raise BTPError("Unexpected response received!") logging.debug("response is valid")
def gap_pairing_failed_ev_(gap, data, data_len): stack = get_stack() logging.debug("%s", gap_pairing_failed_ev_.__name__) logging.debug("received %r", data) fmt = '<B6sB' if len(data) != struct.calcsize(fmt): raise BTPError("Invalid data length") _addr_t, _addr, _reason = struct.unpack_from(fmt, data) _addr = binascii.hexlify(_addr[::-1]).decode() logging.debug("received %r", (_addr_t, _addr, _reason)) stack.gap.pairing_failed_rcvd.data = (_addr_t, _addr, _reason)
def __gap_current_settings_update(settings): logging.debug("%s %r", __gap_current_settings_update.__name__, settings) if isinstance(settings, tuple): fmt = '<I' if len(settings[0]) != struct.calcsize(fmt): raise BTPError("Invalid data length") settings = struct.unpack(fmt, settings[0]) settings = settings[0] # Result of unpack is always a tuple stack = get_stack() for bit in gap_settings_btp2txt: if settings & (1 << bit): stack.gap.current_settings_set(gap_settings_btp2txt[bit]) else: stack.gap.current_settings_clear(gap_settings_btp2txt[bit])
def gattc_notification_ev(bd_addr, bd_addr_type, ev_type): logging.debug("%s %r %r %r", gattc_notification_ev.__name__, bd_addr, bd_addr_type, ev_type) iutctl = get_iut() tuple_hdr, tuple_data = iutctl.btp_socket.read() logging.debug("received %r %r", tuple_hdr, tuple_data) btp_hdr_check(tuple_hdr, defs.BTP_SERVICE_ID_GATT, defs.GATT_EV_NOTIFICATION) data_ba = bytearray() bd_addr_ba = addr2btp_ba(bd_addr) data_ba.extend(chr(bd_addr_type).encode('utf-8')) data_ba.extend(bd_addr_ba) data_ba.extend(chr(ev_type).encode('utf-8')) if tuple_data[0][0:len(data_ba)] != data_ba: raise BTPError("Error in notification event data")