From 77bf847c151ca2f5cefce4e2d76d41ea733b9872 Mon Sep 17 00:00:00 2001 From: Antonio Mancini Date: Sat, 10 Oct 2026 12:37:41 +0200 Subject: [PATCH] fix(ftms): send Request Control, match CP responses, fail pending commands on disconnect - Send Request Control to the trainer after realConnected is set; it was previously declined by writeRealControlPoint's own connection guard. - Ignore real CP responses whose opcode does not match the pending one. - Fail in-flight and queued commands to their originating clients when the trainer disconnects instead of silently dropping them. - Never broadcast CP responses that have no originating client. - Coalesce queued commands per client so each app receives a response. Claude-Session: https://claude.ai/code/session_01Dgkv5W86Zhd8Rv27uCE3rj --- TrainerBridge/CommandQueue.cpp | 4 +-- TrainerBridge/CommandQueue.h | 2 +- TrainerBridge/TrainerBridge.ino | 53 ++++++++++++++++++++++----------- tests/VALIDATION.md | 28 ++++++++++------- tests/firmware_test.cpp | 28 ++++++++++++++--- 5 files changed, 81 insertions(+), 34 deletions(-) diff --git a/TrainerBridge/CommandQueue.cpp b/TrainerBridge/CommandQueue.cpp index 1c747a6..b3d5ab2 100644 --- a/TrainerBridge/CommandQueue.cpp +++ b/TrainerBridge/CommandQueue.cpp @@ -24,15 +24,15 @@ bool CommandQueue::push(const uint8_t *data, size_t len, uint8_t opcode, int16_t portENTER_CRITICAL(&_mux); // If opcode is coalescent, search for an existing pending command of the same opcode + // from the same client, so every client still receives a response for its command if (isCoalescentOpcode(opcode) && _count > 0) { for (size_t i = 0; i < _count; i++) { size_t idx = (_head + i) % CAPACITY; - if (_buffer[idx].opcode == opcode) { + if (_buffer[idx].opcode == opcode && _buffer[idx].connHandle == connHandle) { // Coalesce in-place with latest parameter values memcpy(_buffer[idx].data, data, len); _buffer[idx].len = (uint8_t)len; _buffer[idx].targetValue = targetValue; - _buffer[idx].connHandle = connHandle; portEXIT_CRITICAL(&_mux); return true; } diff --git a/TrainerBridge/CommandQueue.h b/TrainerBridge/CommandQueue.h index fdbaed0..64f59e4 100644 --- a/TrainerBridge/CommandQueue.h +++ b/TrainerBridge/CommandQueue.h @@ -21,7 +21,7 @@ class CommandQueue { CommandQueue(); - // Pushes a command to the queue with coalescing for continuous updates (0x11, 0x05, 0x04). + // Pushes a command to the queue, coalescing continuous updates (0x11, 0x05, 0x04) from the same client. // Thread-safe across ESP32 cores. Returns true on success, false if queue is full. bool push(const uint8_t *data, size_t len, uint8_t opcode, int16_t targetValue = 0, uint16_t connHandle = NO_CONN_HANDLE); diff --git a/TrainerBridge/TrainerBridge.ino b/TrainerBridge/TrainerBridge.ino index 96b2f48..dc48e7a 100644 --- a/TrainerBridge/TrainerBridge.ino +++ b/TrainerBridge/TrainerBridge.ino @@ -376,16 +376,13 @@ static void sendVirtualControlPointResponse(uint8_t requestedOpcode, uint8_t res requestedOpcode, resultCode, connHandle); virtualControlPointChr->setValue(resp, sizeof(resp)); - if (connHandle != CommandQueue::NO_CONN_HANDLE) - { - bool indOk = virtualControlPointChr->indicate(resp, sizeof(resp), connHandle); - Serial.printf("[CP INDICATE] connHandle=%d, ok=%d\n", connHandle, indOk ? 1 : 0); - } - else - { - bool indOk = virtualControlPointChr->indicate(resp, sizeof(resp)); - Serial.printf("[CP INDICATE ALL] ok=%d\n", indOk ? 1 : 0); - } + + // Responses belong to the requesting client only. Commands from the serial CLI or + // from a client that already left have no recipient, so nothing is indicated. + if (connHandle == CommandQueue::NO_CONN_HANDLE) return; + + bool indOk = virtualControlPointChr->indicate(resp, sizeof(resp), connHandle); + Serial.printf("[CP INDICATE] connHandle=%d, ok=%d\n", connHandle, indOk ? 1 : 0); } // ===================================================== @@ -541,6 +538,14 @@ static void realControlPointCallback( uint8_t opcode = pData[1]; uint8_t result = pData[2]; + // Ignore responses that do not belong to the pending transaction + // (e.g. the trainer's reply to the Request Control sent at connection time) + if (cpTxState == CpTxState::IDLE || opcode != pendingCpOpcode) + { + LOGLN("Unsolicited real CP response ignored"); + return; + } + // If the command already timed out, do not send a duplicate indication to app if (cpTxState == CpTxState::TIMED_OUT) { @@ -757,8 +762,20 @@ class RealClientCallbacks : public NimBLEClientCallbacks realIndoorBikeDataChr = nullptr; realControlPointChr = nullptr; + // Fail the in-flight and queued commands so apps do not wait for their own timeout + if (cpTxState == CpTxState::WAITING_RESPONSE) + { + sendVirtualControlPointResponse(pendingCpOpcode, FTMS_CP_RES_OPERATION_FAILED, pendingCpConnHandle); + } cpTxState = CpTxState::IDLE; - commandQueue.clear(); + pendingCpConnHandle = CommandQueue::NO_CONN_HANDLE; + + QueuedCommand dropped; + while (commandQueue.pop(dropped)) + { + sendVirtualControlPointResponse(dropped.opcode, FTMS_CP_RES_OPERATION_FAILED, dropped.connHandle); + } + powerFilter.reset(); virtualSpeedModel.stop(millis()); @@ -915,17 +932,19 @@ static bool connectToRealTrainer() LOGLN("Subscribe Real Control Point notifications FAILED"); } } - - if (realControlPointChr->canWrite()) - { - uint8_t reqCtrl[1] = {FTMS_CP_OP_REQUEST_CONTROL}; - writeRealControlPoint(reqCtrl, sizeof(reqCtrl)); - } } realConnected = true; proxyReady = true; + // Take control of the trainer once; app Request Control writes are answered locally. + // Must follow realConnected = true, which writeRealControlPoint requires. + if (realControlPointChr != nullptr && realControlPointChr->canWrite()) + { + uint8_t reqCtrl[1] = {FTMS_CP_OP_REQUEST_CONTROL}; + writeRealControlPoint(reqCtrl, sizeof(reqCtrl)); + } + LOGLN("Trainer reale connesso. Proxy pronto per Garmin/Zwift/MyWhoosh."); return true; diff --git a/tests/VALIDATION.md b/tests/VALIDATION.md index 32357c9..a8bbef0 100644 --- a/tests/VALIDATION.md +++ b/tests/VALIDATION.md @@ -57,16 +57,24 @@ Host assertions cover: execution environment; neither this run nor the mocks validate physical leaks. - Hardware checks below: not executed. -## Existing behavior outside this compatibility change - -These are not claims of complete FTMS correctness: the queue coalesces by opcode -across clients, retaining the latest handle; pending trainer commands are cleared -on trainer disconnect without an immediate failed indication. The initial Request -Control attempt occurs before `realConnected` is set, and the write helper declines -it. The response state machine does not compare a received opcode with the pending -opcode. These pre-existing semantics are unchanged; concurrent controllers, -out-of-order/late replies and trainer control acquisition require separate work -if physical tests expose failures. This patch does not change protocol policy. +## FTMS control point fixes (follow-up) + +The pre-existing control point issues documented with the patch upgrade were fixed +separately, with host coverage: + +- The bridge now actually sends Request Control to the trainer after connecting + (the write previously ran before `realConnected` was set and was declined). +- Trainer responses are matched against the pending opcode; unsolicited or + mismatched responses no longer complete the pending transaction. +- On trainer disconnect, the in-flight and queued commands fail immediately with + Operation Failed, each to its originating client. +- Responses with no originating client (serial CLI commands, or a client that + disconnected mid-transaction) are no longer indicated to every connected client. +- The command queue coalesces only within one client, so every app gets a response. + +Still unhandled: a late reply arriving after a timeout *and* after the next command +with the same opcode was sent is attributed to the newer command; FTMS responses +carry no sequence number to tell them apart. ## Physical checks still required diff --git a/tests/firmware_test.cpp b/tests/firmware_test.cpp index 27aaf4e..cb239cc 100644 --- a/tests/firmware_test.cpp +++ b/tests/firmware_test.cpp @@ -55,6 +55,10 @@ int main() { assert(realConnected && proxyReady && realClient); assert(realIndoorBikeDataChr->callback==realIndoorBikeDataCallback); assert(realControlPointChr->callback==realControlPointCallback); + // The bridge takes control of the trainer once connected; the reply is not forwarded. + assert((realControlPointChr->writes==std::vector>{{FTMS_CP_OP_REQUEST_CONTROL}})); + reply(FTMS_CP_OP_REQUEST_CONTROL); + assert(virtualControlPointChr->indications.empty() && cpTxState==CpTxState::IDLE); proxyServer->connectedCount=2; loop(); assert(!NimBLEDevice::advertising.active); proxyServer->connectedCount=1; testNow+=500; loop(); assert(NimBLEDevice::advertising.active); @@ -90,6 +94,9 @@ int main() { processQueuedCommands(); assert(cpTxState==CpTxState::WAITING_RESPONSE); assert(realControlPointChr->writes.back()==std::vector({FTMS_CP_OP_SET_TARGET_POWER,250,0})); + auto before=virtualControlPointChr->indications.size(); + reply(FTMS_CP_OP_START_RESUME); // Mismatched opcode must not complete the transaction. + assert(cpTxState==CpTxState::WAITING_RESPONSE && virtualControlPointChr->indications.size()==before); auto statuses=virtualStatusChr->notifications.size(); reply(FTMS_CP_OP_SET_TARGET_POWER); assertCp(FTMS_CP_OP_SET_TARGET_POWER,FTMS_CP_RES_SUCCESS,22); @@ -121,6 +128,12 @@ int main() { static_cast(&proxyServerCallbacks)->onDisconnect(proxyServer,departing,19); reply(FTMS_CP_OP_SET_TARGET_POWER); assert(virtualControlPointChr->indications.size()==indications); + // A departing app's timeout is not broadcast to the remaining clients. + writeCp({FTMS_CP_OP_SET_TARGET_POWER,160,0}); processQueuedCommands(); + static_cast(&proxyServerCallbacks)->onDisconnect(proxyServer,departing,19); + testNow+=CP_TIMEOUT_MS+1; processQueuedCommands(); + assert(virtualControlPointChr->indications.size()==indications && cpTxState==CpTxState::TIMED_OUT); + reply(FTMS_CP_OP_SET_TARGET_POWER); // Repeat disconnect -> delayed scan -> rediscovery -> reconnect, keeping the same host/client. auto* client=realClient; @@ -128,10 +141,15 @@ int main() { writeCp({FTMS_CP_OP_SET_TARGET_POWER,180,0}); processQueuedCommands(); assert(cpTxState==CpTxState::WAITING_RESPONSE); - writeCp({FTMS_CP_OP_STOP_PAUSE,1}); + writeCp({FTMS_CP_OP_STOP_PAUSE,1},22); assert(!commandQueue.isEmpty()); clearDeliveries(); disconnectTrainer(); + // In-flight and queued commands fail immediately, each to its originating client. + assert(virtualControlPointChr->indications.size()==2); + assert((virtualControlPointChr->indications[0].bytes==std::vector{FTMS_CP_OP_RESPONSE_CODE,FTMS_CP_OP_SET_TARGET_POWER,FTMS_CP_RES_OPERATION_FAILED})); + assert(virtualControlPointChr->indications[0].handle==11); + assertCp(FTMS_CP_OP_STOP_PAUSE,FTMS_CP_RES_OPERATION_FAILED,22); assert(!realConnected && !proxyReady && !realIndoorBikeDataChr && !realControlPointChr); assert(commandQueue.isEmpty() && cpTxState==CpTxState::IDLE); assert(powerFilter.getLastOutput()==0 && virtualSpeedModel.getSpeedMps()==0); @@ -150,9 +168,11 @@ int main() { CommandQueue queue; uint8_t command[]={FTMS_CP_OP_SET_TARGET_POWER,1,0}; assert(queue.push(command,3,command[0],1,11)); - command[1]=2; assert(queue.push(command,3,command[0],2,22)); - QueuedCommand out{}; assert(queue.count()==1 && queue.pop(out)); - assert(out.targetValue==2 && out.connHandle==22); // Existing cross-client coalescing behavior. + command[1]=2; assert(queue.push(command,3,command[0],2,11)); + command[1]=3; assert(queue.push(command,3,command[0],3,22)); + QueuedCommand out{}; assert(queue.count()==2 && queue.pop(out)); + assert(out.targetValue==2 && out.connHandle==11); // Coalesces only within one client. + assert(queue.pop(out) && out.targetValue==3 && out.connHandle==22); assert(!queue.push(nullptr,3,command[0])); assert(!queue.push(command,21,command[0])); // Failed reconnect and failed telemetry subscription return to the scan retry path.