def add_read_callback(self, key, callback):
if key in self.data_callback:
logging.info("Add callback for message: " + str(brick_protocol.get_type_from_data(key)))
self.data_callback[key].put(callback)
else:
logging.info("Add queue and callback for message: " + str(brick_protocol.get_type_from_data(key)))
q = Queue()
q.put(callback)
self.data_callback[key] = q
def add_read_callback(self, key, callback):
if key in self.data_callback:
logging.info("Add callback for message: " +
str(brick_protocol.get_type_from_data(key)))
self.data_callback[key].put(callback)
else:
logging.info("Add queue and callback for message: " +
str(brick_protocol.get_type_from_data(key)))
q = Queue()
q.put(callback)
self.data_callback[key] = q
def read_callback(self, transfer):
if not self.alive:
return
status = transfer.getStatus()
if status == libusb1.LIBUSB_TRANSFER_COMPLETED:
data = transfer.getBuffer()
data = data[0:brick_protocol.get_length_from_data(data)]
# Apply routing table
data = self.apply_routing_table_in(data)
stack_id = brick_protocol.get_stack_id_from_data(data)
type = brick_protocol.get_type_from_data(data)
if stack_id == 0 and type == USBDevice.TYPE_ENUMERATE_CALLBACK:
self.new_device(data)
key = brick_protocol.get_callback_key_from_data(data)
# Data is return value of function call
if key in self.data_callback:
callback = self.data_callback[key].get()
twisted.internet.reactor.callFromThread(callback, data)
# Data is Signal or Broadcast Message
else:
# Stack ID = 0 -> Broadcast Message
if stack_id == 0:
for bp in brick_protocol.brick_protocol_list:
callback = bp.callback
twisted.internet.reactor.callFromThread(callback, data)
elif stack_id in brick_protocol.device_dict:
callbacks = brick_protocol.device_dict[stack_id][3]
for callback in callbacks:
twisted.internet.reactor.callFromThread(callback, data)
else:
logging.warn("Read callback with unknown Stack ID: " +
str(stack_id))
logging.info("Read callback: " + str(type))
else:
# TODO: Better error handling
logging.warn("Read callback not successful (status " +
str(status) + "): Probably disconnect")
self.alive = False
try:
transfer.submit()
except libusb1.USBError:
logging.warn("Transfer exception: Probably disconnect")
self.alive = False
def read_callback(self, transfer):
if not self.alive:
return
status = transfer.getStatus()
if status == libusb1.LIBUSB_TRANSFER_COMPLETED:
data = transfer.getBuffer()
data = data[0 : brick_protocol.get_length_from_data(data)]
# Apply routing table
data = self.apply_routing_table_in(data)
stack_id = brick_protocol.get_stack_id_from_data(data)
type = brick_protocol.get_type_from_data(data)
if stack_id == 0 and type == USBDevice.TYPE_ENUMERATE_CALLBACK:
self.new_device(data)
key = brick_protocol.get_callback_key_from_data(data)
# Data is return value of function call
if key in self.data_callback:
callback = self.data_callback[key].get()
twisted.internet.reactor.callFromThread(callback, data)
# Data is Signal or Broadcast Message
else:
# Stack ID = 0 -> Broadcast Message
if stack_id == 0:
for bp in brick_protocol.brick_protocol_list:
callback = bp.callback
twisted.internet.reactor.callFromThread(callback, data)
elif stack_id in brick_protocol.device_dict:
callbacks = brick_protocol.device_dict[stack_id][3]
for callback in callbacks:
twisted.internet.reactor.callFromThread(callback, data)
else:
logging.warn("Read callback with unknown Stack ID: " + str(stack_id))
logging.info("Read callback: " + str(type))
else:
# TODO: Better error handling
logging.warn("Read callback not successful (status " + str(status) + "): Probably disconnect")
self.alive = False
try:
transfer.submit()
except libusb1.USBError:
logging.warn("Transfer exception: Probably disconnect")
self.alive = False
def apply_routing_table_in(self, data):
stack_id = brick_protocol.get_stack_id_from_data(data)
type = brick_protocol.get_type_from_data(data)
if stack_id == 0:
if type == USBDevice.TYPE_ENUMERATE_CALLBACK:
old_stack_id = ord(self.routing_table_in[ord(data[52])])
if old_stack_id in brick_protocol.device_dict:
uid = data[4:12]
if brick_protocol.device_dict[old_stack_id][1] != uid:
self.extend_routing_table(old_stack_id)
return data[0:52] + self.routing_table_in[old_stack_id] + data[53:]
elif type == USBDevice.TYPE_GET_STACK_ID:
old_stack_id = ord(data[55])
return data[0:55] + self.routing_table_in[old_stack_id]
else:
return data
else:
return self.apply_routing_table(self.routing_table_in, data)
def write_loop(self):
"""
Write data from queue to usb device
"""
try:
while self.alive:
transfer = self.write_transfer_queue.get()
data = self.write_data_queue.get()
# Apply routing table
data = self.apply_routing_table_out(data)
logging.info("Write to device: " + str(brick_protocol.get_type_from_data(data)))
transfer.setBulk(USBDevice.USB_ENDPOINT_OUT, data, self.write_callback)
transfer.submit()
self.write_data_queue.task_done()
self.write_transfer_queue.task_done()
except:
self.alive = False
def apply_routing_table_in(self, data):
stack_id = brick_protocol.get_stack_id_from_data(data)
type = brick_protocol.get_type_from_data(data)
if stack_id == 0:
if type == USBDevice.TYPE_ENUMERATE_CALLBACK:
old_stack_id = ord(self.routing_table_in[ord(data[52])])
if old_stack_id in brick_protocol.device_dict:
uid = data[4:12]
if brick_protocol.device_dict[old_stack_id][1] != uid:
self.extend_routing_table(old_stack_id)
return data[0:52] + \
self.routing_table_in[old_stack_id] + \
data[53:]
elif type == USBDevice.TYPE_GET_STACK_ID:
old_stack_id = ord(data[55])
return data[0:55] + self.routing_table_in[old_stack_id]
else:
return data
else:
return self.apply_routing_table(self.routing_table_in, data)
def write_loop(self):
"""
Write data from queue to usb device
"""
try:
while self.alive:
transfer = self.write_transfer_queue.get()
data = self.write_data_queue.get()
# Apply routing table
data = self.apply_routing_table_out(data)
logging.info("Write to device: " +
str(brick_protocol.get_type_from_data(data)))
transfer.setBulk(USBDevice.USB_ENDPOINT_OUT, data,
self.write_callback)
transfer.submit()
self.write_data_queue.task_done()
self.write_transfer_queue.task_done()
except:
self.alive = False
