diff --git a/examples/companion_radio/MyMesh.cpp b/examples/companion_radio/MyMesh.cpp index c0075a22..0afd11de 100644 --- a/examples/companion_radio/MyMesh.cpp +++ b/examples/companion_radio/MyMesh.cpp @@ -670,6 +670,11 @@ void MyMesh::onRawDataRecv(mesh::Packet *packet) { void MyMesh::onTraceRecv(mesh::Packet *packet, uint32_t tag, uint32_t auth_code, uint8_t flags, const uint8_t *path_snrs, const uint8_t *path_hashes, uint8_t path_len) { + uint8_t path_sz = flags & 0x03; // NEW v1.11+ + if (12 + path_len + (path_len >> path_sz) + 1 > sizeof(out_frame)) { + MESH_DEBUG_PRINTLN("onTraceRecv(), path_len is too long: %d", (uint32_t)path_len); + return; + } int i = 0; out_frame[i++] = PUSH_CODE_TRACE_DATA; out_frame[i++] = 0; // reserved @@ -681,8 +686,9 @@ void MyMesh::onTraceRecv(mesh::Packet *packet, uint32_t tag, uint32_t auth_code, i += 4; memcpy(&out_frame[i], path_hashes, path_len); i += path_len; - memcpy(&out_frame[i], path_snrs, path_len); - i += path_len; + + memcpy(&out_frame[i], path_snrs, path_len >> path_sz); + i += path_len >> path_sz; out_frame[i++] = (int8_t)(packet->getSNR() * 4); // extra/final SNR (to this node) if (_serial->isConnected()) { @@ -1446,25 +1452,31 @@ void MyMesh::handleCmdFrame(size_t len) { } else { writeErrFrame(ERR_CODE_BAD_STATE); } - } else if (cmd_frame[0] == CMD_SEND_TRACE_PATH && len > 10 && len - 10 < MAX_PATH_SIZE) { - uint32_t tag, auth; - memcpy(&tag, &cmd_frame[1], 4); - memcpy(&auth, &cmd_frame[5], 4); - auto pkt = createTrace(tag, auth, cmd_frame[9]); - if (pkt) { - uint8_t path_len = len - 10; - sendDirect(pkt, &cmd_frame[10], path_len); - - uint32_t t = _radio->getEstAirtimeFor(pkt->payload_len + pkt->path_len + 2); - uint32_t est_timeout = calcDirectTimeoutMillisFor(t, path_len); - - out_frame[0] = RESP_CODE_SENT; - out_frame[1] = 0; - memcpy(&out_frame[2], &tag, 4); - memcpy(&out_frame[6], &est_timeout, 4); - _serial->writeFrame(out_frame, 10); + } else if (cmd_frame[0] == CMD_SEND_TRACE_PATH && len > 10 && len - 10 < MAX_PACKET_PAYLOAD-5) { + uint8_t path_len = len - 10; + uint8_t flags = cmd_frame[9]; + uint8_t path_sz = flags & 0x03; // NEW v1.11+ + if ((path_len >> path_sz) > MAX_PATH_SIZE || (path_len % (1 << path_sz)) != 0) { // make sure is multiple of path_sz + writeErrFrame(ERR_CODE_ILLEGAL_ARG); } else { - writeErrFrame(ERR_CODE_TABLE_FULL); + uint32_t tag, auth; + memcpy(&tag, &cmd_frame[1], 4); + memcpy(&auth, &cmd_frame[5], 4); + auto pkt = createTrace(tag, auth, flags); + if (pkt) { + sendDirect(pkt, &cmd_frame[10], path_len); + + uint32_t t = _radio->getEstAirtimeFor(pkt->payload_len + pkt->path_len + 2); + uint32_t est_timeout = calcDirectTimeoutMillisFor(t, path_len); + + out_frame[0] = RESP_CODE_SENT; + out_frame[1] = 0; + memcpy(&out_frame[2], &tag, 4); + memcpy(&out_frame[6], &est_timeout, 4); + _serial->writeFrame(out_frame, 10); + } else { + writeErrFrame(ERR_CODE_TABLE_FULL); + } } } else if (cmd_frame[0] == CMD_SET_DEVICE_PIN && len >= 5) { diff --git a/src/Identity.h b/src/Identity.h index 42fb9d9a..60e8783b 100644 --- a/src/Identity.h +++ b/src/Identity.h @@ -23,6 +23,9 @@ public: bool isHashMatch(const uint8_t* hash) const { return memcmp(hash, pub_key, PATH_HASH_SIZE) == 0; } + bool isHashMatch(const uint8_t* hash, uint8_t len) const { + return memcmp(hash, pub_key, len) == 0; + } /** * \brief Performs Ed25519 signature verification. diff --git a/src/Mesh.cpp b/src/Mesh.cpp index f1271574..1bda9e96 100644 --- a/src/Mesh.cpp +++ b/src/Mesh.cpp @@ -52,14 +52,15 @@ DispatcherAction Mesh::onRecvPacket(Packet* pkt) { uint32_t auth_code; memcpy(&auth_code, &pkt->payload[i], 4); i += 4; uint8_t flags = pkt->payload[i++]; + uint8_t path_sz = flags & 0x03; // NEW v1.11+: lower 2 bits is path hash size uint8_t len = pkt->payload_len - i; - if (pkt->path_len >= len) { // TRACE has reached end of given path + uint8_t offset = pkt->path_len << path_sz; + if (offset >= len) { // TRACE has reached end of given path onTraceRecv(pkt, trace_tag, auth_code, flags, pkt->path, &pkt->payload[i], len); - } else if (self_id.isHashMatch(&pkt->payload[i + pkt->path_len]) && allowPacketForward(pkt) && !_tables->hasSeen(pkt)) { + } else if (self_id.isHashMatch(&pkt->payload[i + offset], 1 << path_sz) && allowPacketForward(pkt) && !_tables->hasSeen(pkt)) { // append SNR (Not hash!) - pkt->path[pkt->path_len] = (int8_t) (pkt->getSNR()*4); - pkt->path_len += PATH_HASH_SIZE; + pkt->path[pkt->path_len++] = (int8_t) (pkt->getSNR()*4); uint32_t d = getDirectRetransmitDelay(pkt); return ACTION_RETRANSMIT_DELAYED(5, d); // schedule with priority 5 (for now), maybe make configurable?