void packet_disassemble(packet_t* packet) { DPRINT_DATA_DLL(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1); // TODO tmp if (packet->hw_radio_packet.rx_meta.crc_status == HW_CRC_UNAVAILABLE) { uint16_t crc = __builtin_bswap16(crc_calculate(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1 - 2)); if(memcmp(&crc, packet->hw_radio_packet.data + packet->hw_radio_packet.length + 1 - 2, 2) != 0) { DPRINT_DLL("CRC invalid"); DPRINT_DATA_DLL(&crc, 2); goto cleanup; } } else if (packet->hw_radio_packet.rx_meta.crc_status == HW_CRC_INVALID) { DPRINT_DLL("CRC invalid"); goto cleanup; } uint8_t data_idx = 1; if(!dll_disassemble_packet_header(packet, &data_idx)) goto cleanup; // TODO assuming D7ANP for now if(!d7anp_disassemble_packet_header(packet, &data_idx)) goto cleanup; if(!d7atp_disassemble_packet_header(packet, &data_idx)) goto cleanup; // TODO footers // extract payload packet->payload_length = packet->hw_radio_packet.length + 1 - data_idx - 2; // exclude the headers CRC bytes // TODO exclude footers memcpy(packet->payload, packet->hw_radio_packet.data + data_idx, packet->payload_length); DPRINT_FWK("Done disassembling packet"); d7atp_process_received_packet(packet); return; cleanup: DPRINT_FWK("Skipping packet"); packet_queue_free_packet(packet); return; }
void packet_disassemble(packet_t* packet) { log_print_data(packet->hw_radio_packet.data, packet->hw_radio_packet.length + 1); // TODO tmp uint16_t crc = __builtin_bswap16(crc_calculate(packet->hw_radio_packet.data, packet->hw_radio_packet.length - 2)); if(memcmp(&crc, packet->hw_radio_packet.data + packet->hw_radio_packet.length + 1 - 2, 2) != 0) { DPRINT(LOG_STACK_DLL, "CRC invalid"); goto cleanup; } uint8_t data_idx = 1; if(!dll_disassemble_packet_header(packet, &data_idx)) { DPRINT(LOG_STACK_DLL, "disassemble header failed"); goto cleanup; } // TODO assuming D7ANP for now if(!d7anp_disassemble_packet_header(packet, &data_idx)) { DPRINT(LOG_STACK_NWL, "disassemble header failed"); goto cleanup; } if(!d7atp_disassemble_packet_header(packet, &data_idx)) { DPRINT(LOG_STACK_TRANS, "disassemble header failed"); goto cleanup; } // TODO footers // extract payload packet->payload_length = packet->hw_radio_packet.length + 1 - data_idx - 2; // exclude the headers CRC bytes // TODO exclude footers memcpy(packet->payload, packet->hw_radio_packet.data + data_idx, packet->payload_length); DPRINT(LOG_STACK_FWK, "Done disassembling packet"); d7atp_process_received_packet(packet); return; cleanup: DPRINT(LOG_STACK_FWK, "Skipping packet"); packet_queue_free_packet(packet); return; }