Index: firmware/App/Services/MsgQueues.c
===================================================================
diff -u -ra303cd4258157a8fbcbd8af4dd2bbaadec1a736c -r0dec8744af40d0c87a6d7cd1923920c1c2bd1d2f
--- firmware/App/Services/MsgQueues.c	(.../MsgQueues.c)	(revision a303cd4258157a8fbcbd8af4dd2bbaadec1a736c)
+++ firmware/App/Services/MsgQueues.c	(.../MsgQueues.c)	(revision 0dec8744af40d0c87a6d7cd1923920c1c2bd1d2f)
@@ -1,35 +1,32 @@
-/**************************************************************************
- *
- *  Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved.
- *
- *  THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
- *  WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
- *
- *  @file MsgQueues.c
- *
- *  @date 08-Oct-2019
- *  @author S. Nash
- *
- *  @brief MsgQueues service module.  Provides message queue functionality. \n
- *  These queues are NOT thread safe.  However, these queue functions are \n
- *  intended to be called from the General Task thread so no thread safety \n
- *  is required.
- *
- **************************************************************************/
+/**************************************************************************
+*
+* Copyright (c) 2019-2020 Diality Inc. - All Rights Reserved.
+*
+* THIS CODE MAY NOT BE COPIED OR REPRODUCED IN ANY FORM, IN PART OR IN
+* WHOLE, WITHOUT THE EXPLICIT PERMISSION OF THE COPYRIGHT OWNER.
+*
+* @file MsgQueues.c
+*
+* @author (last) Quang Nguyen
+* @date (last) 22-Jul-2020
+*
+* @author (original) Dara Navaei
+* @date (original) 05-Nov-2019
+*
+***************************************************************************/
 
-#include "Common.h"
 #include "MsgQueues.h"
 
 // ********** private definitions **********
 
-#define MAX_MSG_QUEUE_SIZE          100      // messages
+#define MAX_MSG_QUEUE_SIZE          100                                             ///< Maximum message queue size
 
 // ********** private data **********
 
-static U32 msgQueueCounts[NUM_OF_MSG_QUEUES];
-static U32 msgQueueStarts[NUM_OF_MSG_QUEUES];
-static U32 msgQueueNexts[NUM_OF_MSG_QUEUES];
-static MESSAGE_WRAPPER_T msgQueues[NUM_OF_MSG_QUEUES][MAX_MSG_QUEUE_SIZE];
+static U32 msgQueueCounts[ NUM_OF_MSG_QUEUES ];                                     ///< Message queue count
+static U32 msgQueueStarts[ NUM_OF_MSG_QUEUES ];                                     ///< Start index of a queue
+static U32 msgQueueNexts[ NUM_OF_MSG_QUEUES ];                                      ///< Index to add new message to a queue
+static MESSAGE_WRAPPER_T msgQueues[ NUM_OF_MSG_QUEUES ][ MAX_MSG_QUEUE_SIZE ];      ///< Message queues
 
 // ********** private function prototypes **********
 
@@ -49,12 +46,12 @@
     // reset message queues
     for ( q = 0; q < NUM_OF_MSG_QUEUES; q++ )
     {
-        msgQueueCounts[q] = 0;
-        msgQueueStarts[q] = 0;
-        msgQueueNexts[q] = 0;
+        msgQueueCounts[ q ] = 0;
+        msgQueueStarts[ q ] = 0;
+        msgQueueNexts[ q ] = 0;
         for ( m = 0; m < MAX_MSG_QUEUE_SIZE; m++ )
         {
-            blankMessageInWrapper( &msgQueues[q][m] );
+            blankMessageInWrapper( &msgQueues[ q ][ m ] );
         }
     }
 }
@@ -81,20 +78,20 @@
         {
             result = TRUE;
             // add message to queue
-            msgQueues[queue][msgQueueNexts[queue]] = *msg;
+            msgQueues[ queue ][ msgQueueNexts[ queue ] ] = *msg;
             // increment next index to add to
-            msgQueueNexts[queue] = INC_WRAP(msgQueueNexts[queue],0,MAX_MSG_QUEUE_SIZE-1);
+            msgQueueNexts[ queue ] = INC_WRAP( msgQueueNexts[ queue ], 0, MAX_MSG_QUEUE_SIZE - 1 );
             // increment queue count
-            msgQueueCounts[queue]++;
+            msgQueueCounts[ queue ]++;
         }
         else // msg queue is full
         {
-            // TODO - s/w fault?
+            SET_ALARM_WITH_1_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_MSG_QUEUES_ADD_QUEUE_FULL )
         }
     }
     else // invalid message queue
     {
-        // TODO - s/w fault
+        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_MSG_QUEUES_ADD_INVALID_QUEUE, queue )
     }
 
     return result;
@@ -123,11 +120,11 @@
         {
             result = TRUE;
             // get message from queue
-            *msg = msgQueues[queue][msgQueueStarts[queue]];
+            *msg = msgQueues[ queue ][ msgQueueStarts[ queue ] ];
             // increment queue next index to get from
-            msgQueueStarts[queue] = INC_WRAP(msgQueueStarts[queue],0,MAX_MSG_QUEUE_SIZE-1);
+            msgQueueStarts[ queue ] = INC_WRAP( msgQueueStarts[ queue ], 0, MAX_MSG_QUEUE_SIZE - 1 );
             // decrement queue count
-            msgQueueCounts[queue]--;
+            msgQueueCounts[ queue ]--;
         }
         else // message queue is empty
         {
@@ -136,7 +133,7 @@
     }
     else // invalid message queue
     {
-        // TODO - s/w fault
+        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_MSG_QUEUES_GET_INVALID_QUEUE, queue )
     }
 
     return result;
@@ -158,14 +155,14 @@
     // verify given message queue
     if ( queue < NUM_OF_MSG_QUEUES )
     {
-        if ( msgQueueCounts[queue] == 0 )
+        if ( msgQueueCounts[ queue ] == 0 )
         {
             result = TRUE;
         }
     }
     else // invalid message queue
     {
-        // TODO - s/w fault
+        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_MSG_QUEUES_IS_EMPTY_INVALID_QUEUE, queue )
     }
 
     return result;
@@ -187,14 +184,14 @@
     // verify given message queue
     if ( queue < NUM_OF_MSG_QUEUES )
     {
-        if ( msgQueueCounts[queue] < MAX_MSG_QUEUE_SIZE )
+        if ( msgQueueCounts[ queue ] < MAX_MSG_QUEUE_SIZE )
         {
             result = FALSE;
         }
     }
     else // invalid message queue
     {
-        // TODO - s/w fault
+        SET_ALARM_WITH_2_U32_DATA( ALARM_ID_DG_SOFTWARE_FAULT, SW_FAULT_ID_MSG_QUEUES_IS_FULL_INVALID_QUEUE, queue )
     }
 
     return result;
@@ -220,9 +217,6 @@
     {
         *msgContent++ = 0x0;
     }
-
-    // set msg ID out of bounds in case blank message goes somewhere
-    message->hdr.msgID = 0xFFFF;
 }
 
 /*************************************************************************
@@ -249,6 +243,3 @@
     // set msg ID out of bounds in case blank message goes somewhere
     message->msg.hdr.msgID = 0xFFFF;
 }
-
-
-