Fix FTMS control point handling

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;
       }

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

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;

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
 

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<std::vector<uint8_t>>{{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<uint8_t>({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,17 +128,28 @@ int main() {
   static_cast<NimBLEServerCallbacks*>(&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<NimBLEServerCallbacks*>(&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;
   for(int cycle=0;cycle<20;++cycle) {
     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<uint8_t>{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.