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 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 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 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))