From a8b4e397336a8355f87d9f5a9db2ebae89041a5b Mon Sep 17 00:00:00 2001
From: Asbelos <asbelos@btinternet.com>
Date: Thu, 4 Jul 2024 17:20:37 +0100
Subject: [PATCH] Speedup SETFREQ

Avoid calling the DCC packets for setfreq macro.
---
 DCC.cpp     | 14 ++++++++++++++
 DCC.h       |  1 +
 EXRAIL2.cpp | 36 +-----------------------------------
 version.h   |  3 ++-
 4 files changed, 18 insertions(+), 36 deletions(-)

diff --git a/DCC.cpp b/DCC.cpp
index 0aa623fe..99ee71fd 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -271,6 +271,20 @@ uint32_t DCC::getFunctionMap(int cab) {
   return (reg<0)?0:speedTable[reg].functions;
 }
 
+// saves DC frequency (0..3) in spare functions 29,30,31
+void DCC::setDCFreq(int cab,byte freq) {
+  if (cab==0 || freq>3) return;
+  auto reg=lookupSpeedTable(cab,true);
+  // drop and replace F29,30,31 (top 3 bits) 
+  auto newFunctions=speedTable[reg].functions & 0x1FFFFFFFUL;
+  if (freq==1)      newFunctions |= (1UL<<29); // F29
+  else if (freq==2) newFunctions |= (1UL<<30); // F30
+  else if (freq==3) newFunctions |= (1UL<<31); // F31
+  if (newFunctions==speedTable[reg].functions) return; // no change 
+  speedTable[reg].functions=newFunctions;
+  CommandDistributor::broadcastLoco(reg);
+}
+
 void DCC::setAccessory(int address, byte port, bool gate, byte onoff /*= 2*/) {
   // onoff is tristate:
   // 0  => send off packet
diff --git a/DCC.h b/DCC.h
index 4bc222cf..38c1952f 100644
--- a/DCC.h
+++ b/DCC.h
@@ -70,6 +70,7 @@ class DCC
   static void changeFn(int cab, int16_t functionNumber);
   static int8_t getFn(int cab, int16_t functionNumber);
   static uint32_t getFunctionMap(int cab);
+  static void setDCFreq(int cab,byte freq);
   static void updateGroupflags(byte &flags, int16_t functionNumber);
   static void setAccessory(int address, byte port, bool gate, byte onoff = 2);
   static bool setExtendedAccessory(int16_t address, int16_t value, byte repeats=3);
diff --git a/EXRAIL2.cpp b/EXRAIL2.cpp
index b2f29e2f..676d31c4 100644
--- a/EXRAIL2.cpp
+++ b/EXRAIL2.cpp
@@ -716,41 +716,7 @@ void RMFT2::loop2() {
 
   case OPCODE_SETFREQ:
       // Frequency is default 0, or 1, 2,3
-      //if (loco) DCC::setFn(loco,operand,true);
-      switch (operand) {
-        case 0:  // default - all F-s off
-            if (loco) {
-                DCC::setFn(loco,29,false);
-                DCC::setFn(loco,30,false);
-                DCC::setFn(loco,31,false);
-            }
-        break;
-        case 1:
-            if (loco) {
-	      DCC::setFn(loco,29,true);
-              DCC::setFn(loco,30,false);
-              DCC::setFn(loco,31,false);
-            }
-        break;
-        case 2:
-            if (loco) {
-	      DCC::setFn(loco,29,false);
-              DCC::setFn(loco,30,true);
-              DCC::setFn(loco,31,false);
-            }
-        break;
-        case 3:
-            if (loco) {
-	      DCC::setFn(loco,29,false);
-              DCC::setFn(loco,30,false);
-              DCC::setFn(loco,31,true);
-            }
-        break;
-      default:
-	; // do nothing
-	break;
-      }
-
+      DCC::setDCFreq(loco,operand);
       break;
 
   case OPCODE_RESUME:
diff --git a/version.h b/version.h
index 1a75b010..de4fdcc1 100644
--- a/version.h
+++ b/version.h
@@ -3,7 +3,8 @@
 
 #include "StringFormatter.h"
 
-#define VERSION "5.2.64"
+#define VERSION "5.2.65"
+// 5.2.65 - Speedup Exrail SETFREQ 
 // 5.2.64 - Bugfix: <0 PROG> updated to undo JOIN 
 // 5.2.63 - Implement WIFI_LED for ESP32, ESPduino32 and EX-CSB1, that is turned on when STA mode connects or AP mode is up
 //        - Add BOOSTER_INPUT definitions for ESPduino32 and EX-CSB1 to config.example.h