From 13a0547f5d9146e43160dcc0e2767660d69dead7 Mon Sep 17 00:00:00 2001
From: Douglas Clowes <dcl@ansto.gov.au>
Date: Fri, 29 Aug 2014 12:34:53 +1000
Subject: [PATCH] A bit of reformatting and set the ADUlen in the default case

---
 site_ansto/hardsup/modbus_asyncprotocol.c | 51 ++++++++++++++---------
 1 file changed, 31 insertions(+), 20 deletions(-)

diff --git a/site_ansto/hardsup/modbus_asyncprotocol.c b/site_ansto/hardsup/modbus_asyncprotocol.c
index 13cdec68..cde567e9 100644
--- a/site_ansto/hardsup/modbus_asyncprotocol.c
+++ b/site_ansto/hardsup/modbus_asyncprotocol.c
@@ -17,7 +17,7 @@
  * data
  * ETX
  * BCC
-*/
+ */
 #define PROTOCOL_NAME "MODBUS_AP"
 #define PROTOCOL_INIT MODBUSInitProtocol
 #define ADUSIZE 17
@@ -44,7 +44,17 @@ struct cmdPar {
   double MBdata[2000];
 };
 
-enum MBFCODES { RdCoil=1, RdInp=2, RdHldReg=3, RdInpReg=4, WrCoil=5, WrReg=6, WrMCoils=15, WrMRegs=16 };
+enum MBFCODES {
+  RdCoil=1,     /* Read Coil - single output bit */
+  RdInp=2,      /* Read Input - single input bit */
+  RdHldReg=3,   /* Read Holding Register */
+  RdInpReg=4,   /* Read Input Register */
+  WrCoil=5,     /* Write Coil - force output bit */
+  WrReg=6,      /* Write Register */
+  WrLoop=8,     /* Write Loopback */
+  WrMCoils=15,  /* Write Multiple Coils */
+  WrMRegs=16    /* Write Multiple Registers */
+};
 
 typedef struct modbus_private_t Private, *pPrivate;
 struct modbus_private_t {
@@ -143,8 +153,8 @@ static double ieee2double(unsigned char ieee[4]) {
   int exponent, bias = 127;
 
   mantissa = ((ieee[1] << 16) & 0x7F0000)
-           | ((ieee[2] <<  8) & 0xFF00)
-           | ((ieee[3]      ) & 0xFF);
+    | ((ieee[2] <<  8) & 0xFF00)
+    | ((ieee[3]      ) & 0xFF);
 
   exponent = (ieee[0] & 0x7F) * 2 + ((ieee[1] >> 7) & 1); /* raw exponent */
   if (exponent == 255) {
@@ -171,8 +181,8 @@ static double ieee2double(unsigned char ieee[4]) {
 /*-------------------------------------------------------------------------*/
 
 /** @brief Parse a command string of the form "MBAddr:Fcode:SAddr:Dtype:MBdata"
-Dtype U16, U32, F32 with optional byte order array eg U32[3,2,1,0]
-*/
+  Dtype U16, U32, F32 with optional byte order array eg U32[3,2,1,0]
+  */
 static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
   int len;
   unsigned int i;
@@ -213,11 +223,11 @@ static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
   }
   switch (MBC->Fcode) {
     case RdCoil:
-    /* Coils are numbered from 1 but addressed from 0 */
+      /* Coils are numbered from 1 but addressed from 0 */
       MBC->SAddr--;
       MBC->NumBytes = MBC->NumVal;
       MBC->NumRd = MBC->NumVal;
-    break;
+      break;
     case WrCoil:
       MBC->SAddr--;
       dVal = strtod(cmdLine, &nextPar);
@@ -228,7 +238,7 @@ static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
       } else {
         /* TODO ERROR */
       }
-    break;
+      break;
     case RdHldReg:
       if (MBC->Dtype == F32 || MBC->Dtype == U32) {
         MBC->NumRd = MBC->NumVal * 2;
@@ -237,7 +247,7 @@ static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
       } else {
         /* TODO ERROR */
       }
-    break;
+      break;
     case WrMRegs:
       for (i=0; i < MBC->NumVal; i++) {
         MBC->MBdata[i] = strtod(cmdLine, &nextPar);
@@ -250,7 +260,7 @@ static void parseCmd(char *cmdLine, struct cmdPar *MBC, int *BO) {
         MBC->NumWr = MBC->NumVal;
         MBC->NumBytes = MBC->NumVal * 2;
       }
-    break;
+      break;
   }
 }
 
@@ -274,20 +284,20 @@ static int ModbusOutput(pPrivate myPriv, pDynString wrBuffer, pDynString send_bu
   switch (myPriv->MBcmd.Fcode) {
     case RdCoil:
     case RdHldReg:
-    /* TODO Max NumRd = 125 */
+      /* TODO Max NumRd = 125 */
       ADU[8]  = (myPriv->MBcmd.SAddr & 0xFF00) >> 8;
       ADU[9]  =  myPriv->MBcmd.SAddr & 0xFF;
       ADU[10] = (myPriv->MBcmd.NumRd & 0xFF00) >> 8;
       ADU[11] =  myPriv->MBcmd.NumRd & 0xFF;
       ADUlen  = 12;
-    break;
+      break;
     case WrCoil:
       ADU[8]  = (myPriv->MBcmd.SAddr & 0xFF00) >> 8;
       ADU[9]  =  myPriv->MBcmd.SAddr & 0xFF;
       ADU[10] = myPriv->MBcmd.MBdata[0];
       ADU[11] = 0;
       ADUlen  = 12;
-    break;
+      break;
     case WrMRegs:
       ADU[8]  = (myPriv->MBcmd.SAddr & 0xFF00) >> 8;
       ADU[9]  =  myPriv->MBcmd.SAddr & 0xFF;
@@ -300,7 +310,7 @@ static int ModbusOutput(pPrivate myPriv, pDynString wrBuffer, pDynString send_bu
             ADU[j + myPriv->BO[0]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF00)     >>  8;
             ADU[j + myPriv->BO[1]] =  (unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF;
           }
-        break;
+          break;
         case U32:
           for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) {
             ADU[j + myPriv->BO[0]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF000000) >> 24;
@@ -308,7 +318,7 @@ static int ModbusOutput(pPrivate myPriv, pDynString wrBuffer, pDynString send_bu
             ADU[j + myPriv->BO[2]] = ((unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF00)     >>  8;
             ADU[j + myPriv->BO[3]] =  (unsigned int)myPriv->MBcmd.MBdata[i] & 0xFF;
           }
-        break;
+          break;
         case F32:
           for (i=0, j=13; i < myPriv->MBcmd.NumVal; i++, j+=4) {
             double2ieee(myPriv->MBcmd.MBdata[i], ieee);
@@ -317,13 +327,14 @@ static int ModbusOutput(pPrivate myPriv, pDynString wrBuffer, pDynString send_bu
             ADU[j + myPriv->BO[2]] = ieee[2];
             ADU[j + myPriv->BO[3]] = ieee[3];
           }
-        break;
+          break;
       }
       ADUlen  = 13 + myPriv->MBcmd.NumBytes;
-    break;
+      break;
     default:
-    // Not Implemented
-    break;
+      // Not Implemented
+      ADUlen = 7;
+      break;
   }
   /* Length field in MBAP includes byte 7 of the MBAP, so Length = ADU length - 6. */
   PDUlenPlusUID = ADUlen - 6;