def deframe_APDU(self, msg_pmt):
if pmt.is_blob(msg_pmt):
blob = msg_pmt
# print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
# print "is pair"
else:
print "Formato desconocido"
return
data_np = pmt.to_python(blob) # numpy.ndarray
data_py = data_np.tolist() # python list
# print "Paquete",data_py,data_py.__class__
if data_py[2:4] == [0xFF, 0xFF]:
index = 16
else:
index = 24
del data_py[:index] # FC(2),Dst_Add(2),Src_Add(2),Radio(1),Sec_N(1),X->Dst_IEEE(8), Src_IEEE(8)
# print "DEPACKED-->",data_py
# Crea un PMT vacio:
send_pmt = pmt.make_u8vector(len(data_py), ord(" "))
# Copy all characters to the u8vector:
for i in range(len(data_py)):
pmt.u8vector_set(send_pmt, i, data_py[i])
# Send the message:
self.message_port_pub(pmt.intern("out"), pmt.cons(pmt.PMT_NIL, send_pmt))
def vector_from_str(value_str):
send_pmt = pmt.make_u8vector(len(value_str), ord(' '))
for i in range(len(value_str)):
pmt.u8vector_set(send_pmt, i, ord(value_str[i]))
return send_pmt
def send_packet(self, data, max_tu, fft_len, data_precision):
fft_data = np.fromstring(data, np.float32)
if data_precision:
#fft_data = fft_data.astype(np.float16, copy=False)
fragments = int(math.ceil(fft_len * 4 / float(max_tu)))
else:
fft_data = fft_data.astype(np.int8, copy=False)
fragments = int(math.ceil(fft_len / float(max_tu)))
data = fft_data.tostring()
j = 0
for i in range(fragments):
n_frags = struct.pack('!B',
fragments) #1 byte for number of fragments
frag_id = struct.pack('!B', i) #1 byte for fragment number
frag = data[j:j + max_tu]
if i == fragments - 1: frag = data[j:]
frame = n_frags + frag_id + frag #construct frame
data_pmt = pmt.make_u8vector(len(frame), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(frame)):
pmt.u8vector_set(data_pmt, i, ord(frame[i]))
self.message_port_pub(pmt.intern("out"),
pmt.cons(pmt.PMT_NIL, data_pmt))
j += max_tu
def post_message(self, msg_str, meta=None):
if self.prefix is not None:
send_str = "[{}] {}".format(self.prefix, msg_str)
else:
send_str = msg_str
# prepend AC to sent packet
if self.access_code:
(packed_access_code, padded
) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(
self.access_code)
print self.access_code
print map(ord, packed_access_code)
print padded
send_str = packed_access_code + send_str
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
# Send the message:
if meta is not None:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.to_pmt(meta), send_pmt))
else:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def post_message(self, msg_str):
if self.ack == 0:
""" Take a string, remove all non-printable characters,
prepend the prefix and post to the next block. """
# Do string sanitization:
msg_str = filter(lambda x: x in string.printable, msg_str)
send_str = "[{}] {}".format(self.prefix, msg_str)
count = 0;
# prepend AC to sent packet
if self.access_code:
(packed_access_code, padded) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(self.access_code)
print self.access_code
print map(ord, packed_access_code)
print padded
send_str = packed_access_code + send_str
#print len(send_str)
#print send_str
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
#print ord(send_str[i])
# Send the message:
while self.ack != 1:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
time.sleep(.06)
count = count + 1
if count == 6:
print 'Message not received by RX'
self.ack = 1
print count
#print self.ack
self.ack = 0
def make_NPDU(self,msg_pmt):
if pmt.is_blob(msg_pmt):
blob = msg_pmt
#print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
#print "is pair"
else:
print "Formato desconocido"
return
# Toma Pmt pair a numpyarray y luego a list
data_np = pmt.to_python(blob) #numpy.ndarray
apdu = data_np.tolist() #python list
Src_Ieee = self.SrcIeeeN # la dir del USRP de 64 bit puede ser cualquiera
global Seq_Num
Seq_Num[0] = Seq_Num[0] +1
if Seq_Num[0] > 254: Seq_Num[0] = 0
Radio = [0x1E] # Saltos Max del mensaje
Src_Add = self.SrcAddN # Puede ser cualquiera
Dst_Add = [0xFF,0xFF] # Coordinador 00 00 / #broadcast FF FF
FC_N = [0x08,0x10] # Frame Control LSB-MSB
NPDU = FC_N + Dst_Add + Src_Add + Radio + Seq_Num + Src_Ieee + apdu
# Crea PMT vacio
send_pmt = pmt.make_u8vector(len(NPDU), ord(' '))
# Copia Caracteres a u8vector:
for i in range(len(NPDU)):
pmt.u8vector_set(send_pmt, i, NPDU[i])
# Envia mensaje:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
def get_pmt_from_data_str(self, msg_str):
send_pmt = pmt.make_u8vector(len(msg_str), ord(' '))
for i in range(len(msg_str)):
pmt.u8vector_set(send_pmt, i, ord(msg_str[i]))
send_pmt = pmt.cons(pmt.PMT_NIL, send_pmt)
return send_pmt
def deframe_MSJ(self, msg_pmt):
if pmt.is_blob(msg_pmt):
blob = msg_pmt
# print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
# print "is pair"
else:
print "Formato desconocido"
return
data_np = pmt.to_python(blob) # numpy.ndarray
data_py = data_np.tolist() # python list
# print "Paquete",data_py,data_py.__class__
index = 8
del data_py[:index] # FC (1), DesEP(1),Cluster(2),Profile(2),SourEP(1),Counter(1)
# print "DEPACKED-->",data_py
# Crea un PMT vacio:
send_pmt = pmt.make_u8vector(len(data_py), ord(" "))
# Copy all characters to the u8vector:
for i in range(len(data_py)):
pmt.u8vector_set(send_pmt, i, data_py[i])
# Send the message:
self.message_port_pub(pmt.intern("out"), pmt.cons(pmt.PMT_NIL, send_pmt))
def deframe_MSJ(self, msg_pmt):
if pmt.is_blob(msg_pmt):
blob = msg_pmt
#print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
#print "is pair"
else:
print "Formato desconocido"
return
data_np = pmt.to_python(blob) #numpy.ndarray
data_py = data_np.tolist() #python list
#print "Paquete",data_py,data_py.__class__
index = 8
del data_py[:
index] # FC (1), DesEP(1),Cluster(2),Profile(2),SourEP(1),Counter(1)
#print "DEPACKED-->",data_py
# Crea un PMT vacio:
send_pmt = pmt.make_u8vector(len(data_py), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(data_py)):
pmt.u8vector_set(send_pmt, i, data_py[i])
# Send the message:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def post_message(self, msg_str):
""" Take a string, remove all non-printable characters,
prepend the prefix and post to the next block. """
# Do string sanitization:
#msg_str = filter(lambda x: x in string.printable, msg_str)
#send_str = "[{}] {}".format(self.prefix, msg_str)
send_str = msg_str
# prepend AC to sent packet
if self.access_code:
(packed_access_code, padded
) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(
self.access_code)
print self.access_code
print map(ord, packed_access_code)
print padded
send_str = packed_access_code + send_str
if self.AESkey:
send_str = self.crypter.encrypt(send_str)
#print len(send_str)
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
# Send the message:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def post_message(self, msg_str):
""" Take a string, remove all non-printable characters,
prepend the prefix and post to the next block. """
# Do string sanitization:
#msg_str = filter(lambda x: x in string.printable, msg_str)
#send_str = "[{}] {}".format(self.prefix, msg_str)
send_str = msg_str
# prepend AC to sent packet
if self.access_code:
(packed_access_code, padded) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(self.access_code)
print self.access_code
print map(ord, packed_access_code)
print padded
send_str = packed_access_code + send_str
if self.AESkey:
send_str = self.crypter.encrypt(send_str)
#print len(send_str)
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
# Send the message:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
def get_pmt_from_data_str(self, msg_str):
send_pmt = pmt.make_u8vector(len(msg_str), ord(' '))
for i in range(len(msg_str)):
pmt.u8vector_set(send_pmt, i, ord(msg_str[i]))
send_pmt = pmt.cons(pmt.PMT_NIL, send_pmt)
return send_pmt
def deframe_APDU(self, msg_pmt):
if pmt.is_blob(msg_pmt):
blob = msg_pmt
#print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
#print "is pair"
else:
print "Formato desconocido"
return
data_np = pmt.to_python(blob) #numpy.ndarray
data_py = data_np.tolist() #python list
#print "Paquete",data_py,data_py.__class__
if data_py[2:4] == [0xFF, 0xFF]:
index = 16
else:
index = 24
del data_py[:
index] # FC(2),Dst_Add(2),Src_Add(2),Radio(1),Sec_N(1),X->Dst_IEEE(8), Src_IEEE(8)
#print "DEPACKED-->",data_py
# Crea un PMT vacio:
send_pmt = pmt.make_u8vector(len(data_py), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(data_py)):
pmt.u8vector_set(send_pmt, i, data_py[i])
# Send the message:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def _byteList_to_pmtvector(self, byteList):
length = len(byteList)
vector = pmt.make_u8vector(length, 0)
byteIterator = iter(range(length))
for byte in byteIterator:
pmt.u8vector_set(vector, byte, byteList[byte])
return vector
def post_message(self, msg):
""" Take a string, remove all non-printable characters,
prepend the prefix and post to the next block. """
# Do sanitization
msg = filter(lambda x: x in string.printable, msg)
send_msg = "[{}] {}".format(self.prefix, msg)
# Prepend AC to send packet
if self.access_code:
(packet_access_code, padded
) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(
self.access_code)
print self.access_code
print map(ord, packet_access_code)
print padded
send_msg = packet_access_code + send_msg
# Create an empty PMT
send_pmt = pmt.make_u8vector(len(send_msg), ord(' '))
# Copy all characters to the vetor
for i in range(len(send_msg)):
pmt.u8vector_set(send_pmt, i, ord(send_msg[i]))
# Send message
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def make_MPDU(self, msg_pmt):
if pmt.is_blob(msg_pmt):
blob = msg_pmt
# print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
# print "is pair"
else:
print "Formato desconocido"
return
# Toma Pmt pair a numpyarray y luego a list
data_np = pmt.to_python(blob) # numpy.ndarray
npdu = data_np.tolist() # python list
Src_Add = self.SrcAddM # Dir 16 bit id de la red
Dst_Add = [0xFF, 0xFF] # Coordinador 00 00 / #broadcast FF FF
Pan_Id = self.PanId # PanID que estabece el coordinador LSB -MSB
global Seq_M
Seq_M[0] = Seq_M[0] + 1
if Seq_M[0] > 254:
Seq_M[0] = 0
FC_M = [0x41, 0x88] # Frame control Mac LSB-MSB
to_fcs = FC_M + Seq_M + Pan_Id + Dst_Add + Src_Add + npdu
s_to_fcs = struct.pack("B" * len(to_fcs), *to_fcs) # Convierte List a String
a = self.crc16(s_to_fcs) # Calcula FCS
FCS = [a & 0xFF, (a >> 8) & 0xFF]
MPDU = FC_M + Seq_M + Pan_Id + Dst_Add + Src_Add + npdu + FCS
send_pmt = pmt.make_u8vector(len(MPDU), ord(" ")) # Crea PMT vacio
for i in range(len(MPDU)):
pmt.u8vector_set(send_pmt, i, MPDU[i]) # Copia Caracteres a u8vector:
# Envia mensaje:
self.message_port_pub(pmt.intern("out"), pmt.cons(pmt.PMT_NIL, send_pmt))
def post(self, message_str):
""" Post a message for tx """
vect = pmt.make_u8vector(max(len(message_str), self.min_tx_len), ord(' '))
for i in range(len(message_str)):
pmt.u8vector_set(vect, i, ord(message_str[i]))
meta = pmt.PMT_NIL
msg = pmt.cons(meta, vect)
self.blocks_pdu_to_tagged_stream_0.to_basic_block()._post(self.pdu_port, msg)
def str_to_pdu(meta, msg):
vector = map(ord, msg)
data = pmt.make_u8vector(len(vector), 0)
for k in range(0, len(vector)):
pmt.u8vector_set(data, k, vector[k])
pdu = pmt.cons(pmt.make_dict(), data)
return pdu
def app_in(self, msg): # Generate messages
global d_msg_len, d_mac_id, d_seq_nr, d_msg
if self.debug:
print "******************************** \nGenerating messages ...\n******************************** "
print ""
print "MAC:app_in: got something:", msg
data = msg
if (pmt.is_pair(msg)):
data = pmt.cdr(msg)
#if self.debug: print "mac.py:app_in: pmt_is_pair \n"
elif (pmt.is_eof_object(msg)):
if self.debug: print "MAC: exiting"
return
elif (pmt.is_blob(msg)):
data = pmt.cdr(msg)
if self.debug: print "data is blob"
else:
if self.debug: print "MAC: unknown input"
return
if pmt.is_u8vector(data):
"data is u8vector"
data_elements = pmt.u8vector_elements(data)
if self.debug:
print "Data from Application Layer: ", data_elements, "\n"
print "Data length :", len(data_elements), "\n"
d_msg = []
if pmt.is_symbol(data):
dataString = pmt.symbol_to_string(data)
if self.debug:
print "len(pmt.symbol_to_string(data)): ", len(
dataString), "pmt.symbol_to_string(data): ", dataString
generate_mac(data, len(dataString), self.debug, d_mac_id, d_seq_nr,
self.no_self_loop)
else:
generate_mac(data, pmt.length(data), self.debug, d_mac_id,
d_seq_nr, self.no_self_loop)
generatedMacPayload = pmt.make_u8vector(len(d_msg), 0)
for i in range(len(d_msg)):
#if (pmt.is_vector(data)):
#print "d_msg[",i,"]: ", d_msg[i], " ; type: ", type(d_msg[i])
# pmt.vector_set(generatedMacPayload, i, pmt.to_pmt(d_msg[i]))
pmt.u8vector_set(generatedMacPayload, i, d_msg[i])
#else: pmt.u8vector_set(generatedMacPayload, i, d_msg[i])
self.message_port_pub(pmt.intern("pdu out"),
pmt.cons(pmt.PMT_NIL, generatedMacPayload))
print
print "**********************************"
print
if self.debug:
print "Data Published to physical Layer: ", pmt.u8vector_elements(
generatedMacPayload), "\n"
def pdu_in(self, msg): # consume messages
global d_mac_id
if self.debug: print "****************************** \nConsuming messages...\n******************************** "
data = 0
if(pmt.is_eof_object(msg)):
self.message_port_pub(pmt.intern('pdu out'), pmt.PMT_EOF)
return
elif(pmt.is_pair(msg)):
#if self.debug: print "pmt_is_pair"
data = pmt.cdr(msg)
elif(pmt.is_bool(msg)):
if self.debug: print "mac.py:pdu in: got a busy notification"
return
data_len = pmt.length(data)
if self.debug:
print ""
print "Data received from pdusical Layer: ", pmt.u8vector_elements(data)
print ""
if (data_len < 13):
if self.debug: print "MAC: frame too short. Dropping! \n"
return
else:
if self.no_self_loop: # Compare to our own "mac ID" and reject if we are self routing packets.
macId = pmt.u8vector_elements(data)[0]
if self.debug:
print "macId of the packet: ", macId
print "our macId: ", d_mac_id
print "data_len: ", data_len
if (macId != d_mac_id):
crc = crc16(pmt.u8vector_elements(data), data_len)
if self.debug: print "#### CRC at Reception: #### " , crc.get_crc(), "\n"
if (crc.get_crc()):
print "MAC: wrong crc. Dropping packet! \n"
return
macPayload = pmt.make_u8vector(data_len - 13, 0)
#Because of the iperf 'E' issue
#macPayload = pmt.make_u8vector(data_len - 14, 0)
#for i in range(data_len - 14):
for i in range(data_len - 13):
pmt.u8vector_set(macPayload, i, pmt.u8vector_elements(data)[i+11])
self.message_port_pub(pmt.intern('app out'), pmt.cons(pmt.PMT_NIL, macPayload))
if self.debug: print "Data sent up to Application Layer : ", pmt.u8vector_elements(macPayload), "\n"
else:
crc = crc16(pmt.u8vector_elements(data), data_len)
if self.debug: print "#### CRC at Reception: #### " , crc.get_crc(), "\n"
if(crc.get_crc()):
if self.debug: print "MAC: wrong crc. Dropping packet! \n"
return
macPayload = pmt.make_u8vector(data_len - 14, 0)
for i in range(data_len - 14):
pmt.u8vector_set(macPayload, i, pmt.u8vector_elements(data)[i+10])
self.message_port_pub(pmt.intern('app out'), pmt.cons(pmt.PMT_NIL, macPayload))
if self.debug: print "Data sent up to Application Layer : ", pmt.u8vector_elements(macPayload), "\n"
def _packetToPDU(self):
self.message_port_pub(pmt.intern('rmsg'), pmt.intern('paring packet'))
if (len(self._packet)%self._size == 0):
numberOfBytes = int(round(len(self._packet)/self._size))
stream = pmt.make_u8vector(numberOfBytes,0)
for by in range(numberOfBytes):
element = int(self._packet[by*self._size:(by+1)*self._size],2)
pmt.u8vector_set(stream, by, element)
pdu = pmt.cons(pmt.make_dict(),stream)
self.message_port_pub(pmt.intern('rmsg'),pmt.intern('sending pdu'))
self.message_port_pub(pmt.intern("PDUout"), pdu)
def generate_ack_packet_pdu(self, seq_num):
packet_str = chr(seq_num)
packet_str += chr(self.PACKET_TYPE_ACK)
packet_str += 'aaa'
send_pmt = pmt.make_u8vector(len(packet_str), ord(' '))
for i in range(len(packet_str)):
pmt.u8vector_set(send_pmt, i, ord(packet_str[i]))
return pmt.cons(pmt.PMT_NIL, send_pmt)
def generate_ack_packet_pdu(self, seq_num):
packet_str = chr(seq_num)
packet_str += chr(self.PACKET_TYPE_ACK)
packet_str += 'aaa'
send_pmt = pmt.make_u8vector(len(packet_str), ord(' '))
for i in range(len(packet_str)):
pmt.u8vector_set(send_pmt, i, ord(packet_str[i]))
return pmt.cons(pmt.PMT_NIL, send_pmt)
def test_packet_format_async_counter(self):
bps = 2
ac = packet_utils.default_access_code
hdr_format = digital.header_format_counter(ac, 0, 2)
formatter = digital.protocol_formatter_async(hdr_format)
snk_hdr = blocks.message_debug()
snk_pld = blocks.message_debug()
self.tb.msg_connect(formatter, 'header', snk_hdr, 'store')
self.tb.msg_connect(formatter, 'payload', snk_pld, 'store')
send_str = "Hello World" + 1000 * "xxx"
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
msg = pmt.cons(pmt.PMT_NIL, send_pmt)
port = pmt.intern("in")
formatter.to_basic_block()._post(port, msg)
self.tb.start()
while (snk_hdr.num_messages() < 1) or (snk_pld.num_messages() < 1):
time.sleep(0.1)
self.tb.stop()
self.tb.wait()
result_hdr_pmt = pmt.cdr(snk_hdr.get_message(0))
result_pld_pmt = pmt.cdr(snk_pld.get_message(0))
result_hdr = pmt.u8vector_elements(result_hdr_pmt)
result_pld = pmt.u8vector_elements(result_pld_pmt)
header = "".join(chr(r) for r in result_hdr)
payload = "".join(chr(r) for r in result_pld)
access_code = packet_utils.conv_1_0_string_to_packed_binary_string(
packet_utils.default_access_code)[0]
rx_access_code = header[0:len(access_code)]
length = len(send_str)
rx_length = struct.unpack_from(b"!H", six.b(header),
len(access_code))[0]
rx_bps = struct.unpack_from(b"!H", six.b(header),
len(access_code) + 4)[0]
rx_counter = struct.unpack_from(b"!H", six.b(header),
len(access_code) + 6)[0]
self.assertEqual(access_code, rx_access_code)
self.assertEqual(length, rx_length)
self.assertEqual(bps, rx_bps)
self.assertEqual(0, rx_counter)
self.assertEqual(length, len(payload))
self.assertEqual(send_str, payload[0:length])
def deframe_NPDU(self, msg_pmt):
global pack_err
global pack_tot
global PER
# veficcación del formato PMT
if pmt.is_blob(msg_pmt):
blob = msg_pmt
#print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
#print "is pair"
else:
print "Formato desconocido"
return
data_len = pmt.blob_length(blob)
if (data_len < 11):
print "MAC: muy corta!"
pack_err = pack_err + 1.0
return
data_np = pmt.to_python(blob) #numpy.ndarray
data_py = data_np.tolist() #python list
#print "Paquete",data_py,data_py.__class__
#print "Dir Gateway",self.SrcAddR," Dir Nodo",data_py[7:9]
if len(set(self.SrcAddR).intersection(
data_py)) < 2: #Validación TX de la misma dirección
# Valodacion FCS y cuenta PER
rec_FCS = [data_py[-2], data_py[-1]] #FCS recibido
del data_py[-2], data_py[-1] #quita FCS
s_to_fcs = struct.pack('B' * len(data_py),
*data_py) # Convierte List a String
a = self.crc16(s_to_fcs) #Calcula FCS
FCS = [a & 0xFF, (a >> 8) & 0xFF]
if FCS == rec_FCS: # paquete correcto
index = 9
del data_py[:
index] # FC(2),Sec_N(1),PanID(2),Dst_Add(2),Src_Add(2)
# Crea un PMT vacio:
send_pmt = pmt.make_u8vector(len(data_py), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(data_py)):
pmt.u8vector_set(send_pmt, i, data_py[i])
# Send the message:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
else:
pack_err = pack_err + 1.0
pack_tot = pack_tot + 1.0
PER = pack_err / pack_tot * 100.0
print ",", pack_tot, ",", pack_err, ",", PER, "%"
def send_string_message(self, msg_str):
""" Take a string, remove all non-printable characters,
prepend the prefix and post to the next block. """
# Do string sanitization:
msg_str = filter(lambda x: x in string.printable, msg_str)
send_str = "[{}] {}".format(self.prefix, msg_str)
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
# Send the message:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
def handle_message(self, msg_pmt):
meta = pmt.to_python(pmt.car(msg_pmt))
msg = pmt.cdr(msg_pmt)
if not pmt.is_u8vector(msg):
print "ERROR wrong pmt format"
return
msg_str = "".join([chr(x) for x in pmt.u8vector_elements(msg)])
packet = digital.packet_utils.dewhiten(msg_str, 0)
(ok, packet) = digital.crc.check_crc32(packet)
if not ok:
print '#######################################################'
print '################## ERROR: Bad CRC #####################'
print '#######################################################'
return
if ok:
#print 'CRC check OK'
frag_byte = ord(packet[0])
frag_index = frag_byte >> 2
#print 'Fragmentation byte: ', frag_byte
if frag_byte & 0x01 == 1 :
#print "got packet fragment. Index = %d. Last index = %d" % (frag_index, self.last_frag_index)
self.last_frag_index = frag_index
if frag_byte & 0x02 == 0 :
self.wait_for_frag = True
#strip fragmentation control and append payload
self.packet_buffer += packet[1:]
return
else:
#last part of fragmented packet
#print 'got last fragment of packet. Index = ', frag_index
self.wait_for_frag = False
self.packet_buffer += packet[1:]
packet = self.packet_buffer
self.packet_buffer = ''
else:
#packet is not fragmented at all.
#just strip off frag byte and hand it off
packet = packet[1:]
self.wait_for_frag = False
if not self.wait_for_frag:
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(packet), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(packet)):
pmt.u8vector_set(send_pmt, i, ord(packet[i]))
# Send the message:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def test_packet_format_async_counter(self):
bps = 2
ac = packet_utils.default_access_code
hdr_format = digital.header_format_counter(ac, 0, 2)
formatter = digital.protocol_formatter_async(hdr_format)
snk_hdr = blocks.message_debug()
snk_pld = blocks.message_debug()
self.tb.msg_connect(formatter, 'header', snk_hdr, 'store')
self.tb.msg_connect(formatter, 'payload', snk_pld, 'store')
send_str = "Hello World" + 1000*"xxx"
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
msg = pmt.cons(pmt.PMT_NIL, send_pmt)
port = pmt.intern("in")
formatter.to_basic_block()._post(port, msg)
self.tb.start()
while (snk_hdr.num_messages() < 1) and (snk_pld.num_messages() < 1):
time.sleep(0.1)
self.tb.stop()
self.tb.wait()
result_hdr_pmt = pmt.cdr(snk_hdr.get_message(0))
result_pld_pmt = pmt.cdr(snk_pld.get_message(0))
result_hdr = pmt.u8vector_elements(result_hdr_pmt)
result_pld = pmt.u8vector_elements(result_pld_pmt)
header = "".join([chr(r) for r in result_hdr])
payload = "".join([chr(r) for r in result_pld])
access_code = packet_utils.conv_1_0_string_to_packed_binary_string(packet_utils.default_access_code)[0]
rx_access_code = header[0:len(access_code)]
length = len(send_str)
rx_length = struct.unpack_from("!H", header, len(access_code))[0]
rx_bps = struct.unpack_from("!H", header, len(access_code)+4)[0]
rx_counter = struct.unpack_from("!H", header, len(access_code)+6)[0]
self.assertEqual(access_code, rx_access_code)
self.assertEqual(length, rx_length)
self.assertEqual(bps, rx_bps)
self.assertEqual(0, rx_counter)
self.assertEqual(length, len(payload))
self.assertEqual(send_str, payload[0:length])
def post_message(self, msg_str):
send_str = (lowFreqMessage(1, 0, 0, 0, 0, 4, BitArray("0b10000000"),
BitArray("0b10000000"), 0, 0, 0, 0,
BROADCAST_ADDRESS, msg_str).bin)
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
while 1:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.make_dict(), send_pmt))
time.sleep(self.msg_per)
def deframe_NPDU(self,msg_pmt):
global pack_err
global pack_tot
global PER
# veficcación del formato PMT
if pmt.is_blob(msg_pmt):
blob = msg_pmt
#print "is blob"
elif pmt.is_pair(msg_pmt):
blob = pmt.cdr(msg_pmt)
#print "is pair"
else:
print "Formato desconocido"
return
data_len = pmt.blob_length(blob)
if(data_len < 11):
print "MAC: muy corta!"
pack_err = pack_err+1.0
return;
data_np = pmt.to_python(blob) #numpy.ndarray
data_py = data_np.tolist() #python list
#print "Paquete",data_py,data_py.__class__
#print "Dir Gateway",self.SrcAddR," Dir Nodo",data_py[7:9]
if len(set(self.SrcAddR).intersection(data_py)) < 2 : #Validación TX de la misma dirección
# Valodacion FCS y cuenta PER
rec_FCS = [data_py[-2],data_py[-1]] #FCS recibido
del data_py[-2],data_py[-1] #quita FCS
s_to_fcs = struct.pack('B'*len(data_py), *data_py) # Convierte List a String
a = self.crc16(s_to_fcs) #Calcula FCS
FCS= [a & 0xFF,(a>>8)&0xFF]
if FCS == rec_FCS: # paquete correcto
index = 9
del data_py[:index] # FC(2),Sec_N(1),PanID(2),Dst_Add(2),Src_Add(2)
# Crea un PMT vacio:
send_pmt = pmt.make_u8vector(len(data_py), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(data_py)):
pmt.u8vector_set(send_pmt, i, data_py[i])
# Send the message:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
else:
pack_err = pack_err+1.0
pack_tot = pack_tot +1.0
PER =pack_err/pack_tot*100.0
print ",",pack_tot,",",pack_err,",",PER,"%"
def handle_message(self, msg_pmt):
meta = pmt.to_python(pmt.car(msg_pmt))
msg = pmt.cdr(msg_pmt)
if not pmt.is_u8vector(msg):
print "ERROR wrong pmt format"
return
msg_str = "".join([chr(x) for x in pmt.u8vector_elements(msg)])
num_mtus = (len(msg_str) // self.max_mtu_size)
if len(msg_str) % self.max_mtu_size != 0:
num_mtus = num_mtus + 1
#Fragment packet?
mtu_index_start = 0
mtu_index_end = 0
last = False
frag = False
frag_index = 0
if num_mtus > 1:
frag = True;
for i in range(num_mtus):
if (len(msg_str) - mtu_index_start) > self.max_mtu_size:
mtu_index_end = mtu_index_start + self.max_mtu_size
else:
mtu_index_end = len(msg_str)
last = True
fragment_str = msg_str[mtu_index_start:mtu_index_end]
packet = self.build_packet(fragment_str, frag, last, i)
mtu_index_start += self.max_mtu_size
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(packet), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(packet)):
pmt.u8vector_set(send_pmt, i, ord(packet[i]))
# Send the message:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def msg_to_pdu(msg_str, debug=False):
'''Inserts string message into a PDU.
@param msg: a string to insert as content of the PDU.
@param debug: print additional information, default False.
@return: a PDU, a pair (metadata, content) in PMT data types; metadata is None.
'''
# create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(msg_str), ord(' '))
# copy all characters to the u8vector:
for i in range(len(msg_str)):
pmt.u8vector_set(send_pmt, i, ord(msg_str[i]))
if debug:
mutex_prt('[PMT message]')
mutex_prt(send_pmt)
pdu = pmt.cons(pmt.PMT_NIL, send_pmt)
return pdu
def test_packet_format_async_default(self):
ac = packet_utils.default_access_code
hdr_format = digital.header_format_default(ac, 0)
formatter = digital.protocol_formatter_async(hdr_format)
snk_hdr = blocks.message_debug()
snk_pld = blocks.message_debug()
self.tb.msg_connect(formatter, 'header', snk_hdr, 'store')
self.tb.msg_connect(formatter, 'payload', snk_pld, 'store')
send_str = b"Hello World"
send_pmt = pmt.make_u8vector(len(send_str), 0)
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, send_str[i])
msg = pmt.cons(pmt.PMT_NIL, send_pmt)
port = pmt.intern("in")
formatter.to_basic_block()._post(port, msg)
self.tb.start()
while (snk_hdr.num_messages() < 1) or (snk_pld.num_messages() < 1):
time.sleep(0.1)
self.tb.stop()
self.tb.wait()
result_hdr_pmt = pmt.cdr(snk_hdr.get_message(0))
result_pld_pmt = pmt.cdr(snk_pld.get_message(0))
result_hdr = pmt.u8vector_elements(result_hdr_pmt)
result_pld = pmt.u8vector_elements(result_pld_pmt)
header = bytes(result_hdr)
payload = bytes(result_pld)
access_code = packet_utils.default_access_code_binary
rx_access_code = header[0:len(access_code)]
length = len(send_str)
rx_length = struct.unpack_from(b"!H", header, len(access_code))[0]
self.assertEqual(access_code, rx_access_code)
self.assertEqual(length, rx_length)
self.assertEqual(length, len(payload))
self.assertEqual(send_str, payload[0:length])
def nordictap_handler(self, msg):
# PMT to byte string
data = pmt.to_python(msg).tostring()
# Unpack the header
values = struct.unpack('BBBBBBBB', data[0:8])
channel = values[0]
data_rate = values[1]
address_length = values[2]
payload_length = values[3]
sequence_number = values[4]
no_ack = values[5]
crc_length = values[6]
# Parse the address, payload, and crc
address = data[7:7 + address_length]
payload = data[7 + address_length:7 + address_length + payload_length]
crc = data[7 + address_length + payload_length:7 + address_length +
payload_length + crc_length]
# ACK if needed
if payload_length > 0 and no_ack == 0:
# Print the channel, sequence number, address and payload
print "ACK'd Packet: ",
print 'CH=' + str(2400 + channel),
print 'SEQ=' + str(sequence_number),
print 'ADDR=' + ':'.join('%02X' % ord(b) for b in address),
print 'PLD=' + ':'.join('%02X' % ord(b) for b in payload),
print 'CRC=' + ':'.join('%02X' % ord(b) for b in crc)
# Build an ACK
nordictap = [0] + [4, 2, 5, 0, sequence_number, 0, 2]
for c in address:
nordictap.append(ord(c))
# Transmit an ACK
vec = pmt.make_u8vector(len(nordictap), 0)
for x in range(len(nordictap)):
pmt.u8vector_set(vec, x, nordictap[x])
self.message_port_pub(pmt.intern("nordictap_out"), vec)
def transmit(self, address, payload, channel_index, sequence_number):
channel = self.channel_map[channel_index]
# Build a payload
nordictap = [channel_index] + [
channel, 2,
len(address),
len(payload), sequence_number, 0, 2
]
for c in address:
nordictap.append(ord(c))
for c in payload:
nordictap.append(ord(c))
# Transmit packet
vec = pmt.make_u8vector(len(nordictap), 0)
for x in range(len(nordictap)):
pmt.u8vector_set(vec, x, nordictap[x])
self.message_port_pub(pmt.intern("nordictap_out"), vec)
def send_raw_bytes(self, byte_list, verbose=0):
if verbose:
print "Sending Raw Bytes:",
print byte_list
# get payload size
payload_size = len(byte_list)
# build an empty vector
data = pmt.make_u8vector(payload_size, 0x00)
# fill the vector with unsigned byte data to send
for i in xrange(payload_size):
pmt.u8vector_set(data, i, byte_list[i])
# build the message, which is a pair consisting of
# the message metadata (not needed here) and the
# information we want to send (the vector)
msg = pmt.cons(pmt.PMT_NIL, data)
# the message must be serialized as a string so that
# it's in the form the gnuradio source expects
msg = pmt.serialize_str(msg)
self.socket.send(msg)
def post_message(self, meta, msg_str):
if self.prefix is not None:
send_str = "[{}] {}".format(self.prefix, msg_str)
else:
send_str = msg_str
# prepend AC to sent packet
if self.access_code:
(packed_access_code, padded) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(self.access_code)
print self.access_code
print map(ord, packed_access_code)
print padded
send_str = packed_access_code + send_str
# Create an empty PMT (contains only spaces):
send_pmt = pmt.make_u8vector(len(send_str), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(send_str)):
pmt.u8vector_set(send_pmt, i, ord(send_str[i]))
# Send the message:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.to_pmt(meta), send_pmt))
def work(self, input_items, output_items):
for i in input_items[0]:
#print i
if i == 0:
i = 1
else:
i = 0
if i == 0:
self.nz += 1
if self.nz == 2:
if self.rev_varicodes.has_key(self.curr):
c = self.rev_varicodes[self.curr]
self.out_string += c
sys.stdout.write(c)
if c == "\n":
print "sent pdu"
payload = pmt.make_u8vector(len(self.out_string), 0)
j = 0
for a in self.out_string:
pmt.u8vector_set(payload, j, ord(a))
j += 1
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, payload))
self.out_string = ""
self.nz = 0
self.curr = ""
sys.stdout.flush()
elif i == 1:
if self.nz == 1 and self.curr != "":
self.curr += "0"
self.curr += "1"
self.nz = 0
return len(input_items[0])
def handle_app_message(self, msg_pmt):
with self.thread_lock:
packets_str = self.fragment_packet(msg_pmt)
for packet_str in packets_str:
packet_str_total = chr(self.tx_seq_num)
if self.tx_seq_num < 255:
self.tx_seq_num = self.tx_seq_num + 1
else:
self.tx_seq_num = 0
packet_str_total += chr(self.PACKET_TYPE_DATA)
packet_str_total += packet_str
send_pmt = pmt.make_u8vector(len(packet_str_total), ord(' '))
for i in range(len(packet_str_total)):
pmt.u8vector_set(send_pmt, i, ord(packet_str_total[i]))
send_pmt = pmt.cons(pmt.PMT_NIL, send_pmt)
self.app_queue.put(send_pmt)
self.handle_queue()
def make_APDU(self,msg_pmt):
# Toma Pmt Symbol a String y luego a list
msg_pmt = pmt.symbol_to_string(msg_pmt)
msg = [ord(c) for c in msg_pmt]
global count_app
count_app[0] = count_app[0] +1
if count_app[0] > 254: count_app[0] = 0
Src_EP = [0xE8] # Source endpoint
Profile = [0x05,0xC1] # Profile ID LSB-MSB
Cluster = [0x11,0x00] # Cluster ID LSB-MSB
Dst_EP = [0xE8] # Destination endpoint
FC_A = [0x08] # Frame Control APP
APDU = FC_A + Dst_EP + Cluster + Profile + Src_EP + count_app + msg
# Crea PMT vacio
send_pmt = pmt.make_u8vector(len(APDU), ord(' '))
# Copia Caracteres a u8vector:
for i in range(len(APDU)):
pmt.u8vector_set(send_pmt, i, APDU[i])
# Envia mensaje:
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
def process_fragment(self, packet_str):
frag_byte = ord(packet_str[0])
frag_index = frag_byte >> 2
if frag_byte & 0x01 == 1 :
self.last_frag_index = frag_index
if frag_byte & 0x02 == 0 :
self.wait_for_frag = True
self.packet_buffer += packet_str[1:]
return
else:
self.wait_for_frag = False
self.packet_buffer += packet_str[1:]
packet_str = self.packet_buffer
self.packet_buffer = ''
else:
packet_str = packet_str[1:]
self.wait_for_frag = False
if not self.wait_for_frag:
send_pmt = pmt.make_u8vector(len(packet_str), ord(' '))
for i in range(len(packet_str)):
pmt.u8vector_set(send_pmt, i, ord(packet_str[i]))
self.message_port_pub(pmt.intern('to_app'), pmt.cons(pmt.PMT_NIL, send_pmt))
def handle_app_message(self, msg_pmt):
with self.thread_lock:
packets_str = self.fragment_packet(msg_pmt)
for packet_str in packets_str:
packet_str_total = chr(self.tx_seq_num)
if self.tx_seq_num < 255:
self.tx_seq_num = self.tx_seq_num + 1
else:
self.tx_seq_num = 0
packet_str_total += chr(self.PACKET_TYPE_DATA)
packet_str_total += packet_str
send_pmt = pmt.make_u8vector(len(packet_str_total), ord(' '))
for i in range(len(packet_str_total)):
pmt.u8vector_set(send_pmt, i, ord(packet_str_total[i]))
send_pmt = pmt.cons(pmt.PMT_NIL, send_pmt)
self.app_queue.put(send_pmt)
self.handle_queue()
def make_APDU(self, msg_pmt):
# Toma Pmt Symbol a String y luego a list
msg_pmt = pmt.symbol_to_string(msg_pmt)
msg = [ord(c) for c in msg_pmt]
global count_app
count_app[0] = count_app[0] + 1
if count_app[0] > 254: count_app[0] = 0
Src_EP = [0xE8] # Source endpoint
Profile = [0x05, 0xC1] # Profile ID LSB-MSB
Cluster = [0x11, 0x00] # Cluster ID LSB-MSB
Dst_EP = [0xE8] # Destination endpoint
FC_A = [0x08] # Frame Control APP
APDU = FC_A + Dst_EP + Cluster + Profile + Src_EP + count_app + msg
# Crea PMT vacio
send_pmt = pmt.make_u8vector(len(APDU), ord(' '))
# Copia Caracteres a u8vector:
for i in range(len(APDU)):
pmt.u8vector_set(send_pmt, i, APDU[i])
# Envia mensaje:
self.message_port_pub(pmt.intern('out'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def process_fragment(self, packet_str):
frag_byte = ord(packet_str[0])
frag_index = frag_byte >> 2
if frag_byte & 0x01 == 1:
self.last_frag_index = frag_index
if frag_byte & 0x02 == 0:
self.wait_for_frag = True
self.packet_buffer += packet_str[1:]
return
else:
self.wait_for_frag = False
self.packet_buffer += packet_str[1:]
packet_str = self.packet_buffer
self.packet_buffer = ''
else:
packet_str = packet_str[1:]
self.wait_for_frag = False
if not self.wait_for_frag:
send_pmt = pmt.make_u8vector(len(packet_str), ord(' '))
for i in range(len(packet_str)):
pmt.u8vector_set(send_pmt, i, ord(packet_str[i]))
self.message_port_pub(pmt.intern('to_app'),
pmt.cons(pmt.PMT_NIL, send_pmt))
def send_packet(self, data, max_tu):
fragments = int(math.ceil(len(data) /
(max_tu))) + 1 #4 bytes per fft bin
j = 0
for i in range(fragments):
n_frags = struct.pack('!B',
fragments) #1 byte for number of fragments
frag_id = struct.pack('!B', i) #1 byte for fragment number
frag = data[j:j + max_tu]
if i == fragments - 1: frag = data[j:]
frame = n_frags + frag_id + frag #construct frame
data_pmt = pmt.make_u8vector(len(frame), ord(' '))
# Copy all characters to the u8vector:
for i in range(len(frame)):
pmt.u8vector_set(data_pmt, i, ord(frame[i]))
self.message_port_pub(pmt.intern("out"),
pmt.cons(pmt.PMT_NIL, data_pmt))
j += max_tu
def post_message(self, msg):
""" Take a string, remove all non-printable characters,
prepend the prefix and post to the next block. """
# Do sanitization
msg = filter(lambda x: x in string.printable, msg)
send_msg = "[{}] {}".format(self.prefix, msg)
# Prepend AC to send packet
if self.access_code:
(packet_access_code, padded) = digital.packet_utils.conv_1_0_string_to_packed_binary_string(self.access_code)
print self.access_code
print map(ord, packet_access_code)
print padded
send_msg = packet_access_code + send_msg
# Create an empty PMT
send_pmt = pmt.make_u8vector(len(send_msg), ord(' '))
# Copy all characters to the vetor
for i in range(len(send_msg)):
pmt.u8vector_set(send_pmt, i, ord(send_msg[i]))
# Send message
self.message_port_pub(pmt.intern('out'), pmt.cons(pmt.PMT_NIL, send_pmt))
