From d3038317738249e12063ef0b10f5045f485ce198 Mon Sep 17 00:00:00 2001
From: Harald Barth <haba@kth.se>
Date: Mon, 11 Mar 2024 14:52:55 +0100
Subject: [PATCH] Functions for DC frequency: Use func up to F31

---
 CommandDistributor.cpp |  4 +---
 DCC.cpp                |  2 +-
 DCCEXParser.cpp        | 25 +++++++++++--------------
 3 files changed, 13 insertions(+), 18 deletions(-)

diff --git a/CommandDistributor.cpp b/CommandDistributor.cpp
index 67bcc54..e7087ad 100644
--- a/CommandDistributor.cpp
+++ b/CommandDistributor.cpp
@@ -209,9 +209,7 @@ int16_t CommandDistributor::retClockTime() {
 
 void  CommandDistributor::broadcastLoco(byte slot) {
   DCC::LOCO * sp=&DCC::speedTable[slot];
-  uint32_t func = sp->functions;
-  func = func & 0x1fffffff; // mask out bits 0-28
-  broadcastReply(COMMAND_TYPE, F("<l %d %d %d %l>\n"), sp->loco,slot,sp->speedCode,func);
+  broadcastReply(COMMAND_TYPE, F("<l %d %d %d %l>\n"), sp->loco,slot,sp->speedCode,sp->functions);
 #ifdef SABERTOOTH
   if (Serial2 && sp->loco == SABERTOOTH) {
     static uint8_t rampingmode = 0;
diff --git a/DCC.cpp b/DCC.cpp
index 5ab7eff..36d91fb 100644
--- a/DCC.cpp
+++ b/DCC.cpp
@@ -242,7 +242,7 @@ void DCC::changeFn( int cab, int16_t functionNumber) {
 // Report function state (used from withrottle protocol)
 // returns 0 false, 1 true or -1 for do not know
 int8_t DCC::getFn( int cab, int16_t functionNumber) {
-  if (cab<=0 || functionNumber>28)
+  if (cab<=0 || functionNumber>31)
     return -1;  // unknown
   int reg = lookupSpeedTable(cab);
   if (reg<0)
diff --git a/DCCEXParser.cpp b/DCCEXParser.cpp
index 2eeedc9..eb1b86b 100644
--- a/DCCEXParser.cpp
+++ b/DCCEXParser.cpp
@@ -283,25 +283,22 @@ void DCCEXParser::parseOne(Print *stream, byte *com, RingStream * ringStream)
         return; // filterCallback asked us to ignore
     case 't':   // THROTTLE <t [REGISTER] CAB SPEED DIRECTION>
     {
-        if (params==1) {  // <t cab>  display state
-        
-        int16_t slot=DCC::lookupSpeedTable(p[0],false);
-        if (slot>=0) {
-            DCC::LOCO * sp=&DCC::speedTable[slot];
-            StringFormatter::send(stream,F("<l %d %d %d %l>\n"),
-			sp->loco,slot,sp->speedCode,sp->functions);
-            }
-        else // send dummy state speed 0 fwd no functions. 
-            StringFormatter::send(stream,F("<l %d -1 128 0>\n"),p[0]);
-        return; 
-        }
-        
         int16_t cab;
         int16_t tspeed;
         int16_t direction;
-        
+
+        if (params==1) {  // <t cab>  display state
+	  int16_t slot=DCC::lookupSpeedTable(p[0],false);
+	  if (slot>=0)
+	    CommandDistributor::broadcastLoco(slot);
+	  else // send dummy state speed 0 fwd no functions.
+            StringFormatter::send(stream,F("<l %d -1 128 0>\n"),p[0]);
+	  return;
+        }
+
         if (params == 4)
         { // <t REGISTER CAB SPEED DIRECTION>
+	    // ignore register p[0]
             cab = p[1];
             tspeed = p[2];
             direction = p[3];