diff --git a/simulator/extctrl2014.py b/simulator/extctrl2014.py
index 4ed94e443b5b56bd5736ba65350565e1515b0c01..67af2c59b159d1adbd1e5f921f52ee2468e2eadc 100644
--- a/simulator/extctrl2014.py
+++ b/simulator/extctrl2014.py
@@ -32,6 +32,8 @@ import threading
# +
#
+ETHERTYPE = 'EX'
+
flag_MASK = 0x07
flag_LAST_FRAG = 0x01
flag_NEED_ACK = 0x02
@@ -130,7 +132,7 @@ class Fragmenter(Encoder):
fragment = Encoder()
fragment.write(self.destination)
fragment.write('\x00\x00\x00\x00\x00\x00')
- fragment.write('EX')
+ fragment.write(ETHERTYPE)
if len(remaining) == 0:
fragment.encode_byte(self.flags | flag_LAST_FRAG)
else:
@@ -156,7 +158,7 @@ class Packet:
self.destination = buf.read(6)
self.source = buf.read(6)
self.eth_type = buf.read(2)
- if self.eth_type == 'EX':
+ if self.eth_type == ETHERTYPE:
flags = buf.decode_byte()
self.flags = flags & flag_MASK
self.kind = flags & kind_MASK
@@ -186,22 +188,50 @@ class Packet:
self.frag_num = other.frag_num
self.data += other.data
return self
-
+
def __repr__(self):
- return "Packet(%s %s %d index=%x flags=%x kind=%x data='%s'" % (
- ":".join(map(lambda b: "%02.2x" % ord(b), self.destination)),
- ":".join(map(lambda b: "%02.2x" % ord(b), self.source)),
+ macaddr2str = lambda b: ":".join(["%02.2x" % ord(byte) for byte in b])
+
+ if self.eth_type != ETHERTYPE:
+ int_ethertype = ((ord(self.eth_type[0]) << 8) |
+ ord(self.eth_type[1]))
+ return "Packet(%s %s 0x%x)" % (
+ macaddr2str(self.destination),
+ macaddr2str(self.source),
+ int_ethertype,
+ )
+
+ flagify = lambda strlist: '(%s)' % ' | '.join(strlist)
+
+ list_flags = []
+ if self.flags & flag_LAST_FRAG:
+ list_flags.append('LAST_FRAG')
+ if self.flags & flag_NEED_ACK:
+ list_flags.append('NEED_ACK')
+ if self.flags & flag_IS_ACK:
+ list_flags.append('IS_ACK')
+
+ list_kind = []
+ if self.kind & kind_INIT:
+ list_kind.append('INIT')
+ if self.kind & kind_DATA:
+ list_kind.append('DATA')
+
+ return "Packet(%s %s channel=%d cookie=0x%x index=0x%x flags=%s kind=%s data='%s'" % (
+ macaddr2str(self.destination),
+ macaddr2str(self.source),
self.channel,
+ self.cookie,
self.index,
- self.flags,
- self.kind,
+ flagify(list_flags),
+ flagify(list_kind),
"".join(map(lambda b: "\\x%02.2x" % ord(b), self.data)),
)
class ExtCtrl(object):
def __init__(self, ethernet, channel, controller=None, robot=None,
- max_retries=1000, max_length=1480):
+ max_retries=1000, max_length=1480, packetdebug=False):
if len(filter(None, [ robot, controller])) != 1:
raise Exception('Either robot(%s) or controller(%s) '
'should be defined' %
@@ -214,11 +244,13 @@ class ExtCtrl(object):
self.robot = robot
self.max_retries = max_retries
self.max_length = max_length
+ self.packetdebug=packetdebug
self.read_data = list()
self.cond = threading.Condition()
self.local_index = random.randint(1,2**32-1)
self.remote_index = None
- self.cookie = None
+ self.cookie_remote_index = None
+ self.cookie_local_index = None
self.ack = None
t = threading.Thread(target=self.recv_loop)
t.daemon = True
@@ -228,6 +260,12 @@ class ExtCtrl(object):
elif self.robot:
self.await_INIT()
+ @property
+ def cookie(self):
+ if self.cookie_remote_index is None:
+ return 0
+ return self.cookie_local_index ^ self.cookie_remote_index
+
def next_index(self):
self.local_index = max(1, (self.local_index + 1) & 0xffffffff)
@@ -236,15 +274,28 @@ class ExtCtrl(object):
data = None
while True:
fragment = Packet(self.ethernet.recv(2000))
+ if self.packetdebug:
+ print "Got", fragment
# Sanity check fragment
- if fragment.eth_type != 'EX':
+ if fragment.eth_type != ETHERTYPE:
+ if self.packetdebug:
+ print "Bad eth"
continue
if fragment.source != self.other:
+ if self.packetdebug:
+ print "Bad source"
continue
if fragment.channel != self.channel:
+ if self.packetdebug:
+ print "Bad channel"
continue
if fragment.cookie != 0 and fragment.cookie != self.cookie:
+ if self.packetdebug:
+ print "Bad cookie local=%x (seed_local=%x, seed_remote=%x) remote=%x" % (
+ self.cookie, self.cookie_local_index,
+ self.cookie_remote_index, fragment.cookie
+ )
continue
# Join fragments
@@ -281,7 +332,8 @@ class ExtCtrl(object):
decoder = Decoder(packet.data)
with self.cond:
self.remote_index = decoder.decode_uint32()
- self.cookie = self.remote_index ^ self.local_index
+ self.cookie_remote_index = self.remote_index
+ self.cookie_local_index = self.local_index
self.cond.notify_all()
elif packet.kind == kind_INIT:
remote_index = packet.index
@@ -289,7 +341,8 @@ class ExtCtrl(object):
# Save
with self.cond:
self.remote_index = remote_index
- self.cookie = self.remote_index ^ self.local_index
+ self.cookie_remote_index = self.remote_index
+ self.cookie_local_index = self.local_index
self.cond.notify_all()
if remote_index == self.remote_index:
# ACK first INIT or its resends
diff --git a/simulator/robot.py b/simulator/robot.py
index 3300ffc336cccc43250e597b95f67be83fe61312..20912d548f6c67fa4f27e8187adc5285bca3e58a 100755
--- a/simulator/robot.py
+++ b/simulator/robot.py
@@ -107,6 +107,9 @@ if __name__ == '__main__':
optParser.add_argument('-v', '--verbose',
action='store_true',
help='be verbose')
+ optParser.add_argument('--packetdebug',
+ action='store_true',
+ help='Print incoming packets')
options = optParser.parse_args(sys.argv[1:])
@@ -116,7 +119,8 @@ if __name__ == '__main__':
recv_loss=options.recv_loss)
extctrl = extctrl2014.ExtCtrl(ethernet=eth,
channel=options.channel,
- controller=options.connect)
+ controller=options.connect,
+ packetdebug=options.packetdebug)
encoder = labcomm2014.Encoder(extctrl.writer())
decoder = labcomm2014.Decoder(extctrl.reader())
print encoder, decoder