File content as of revision 0:e72ea6decc70:

/* Main application file for the device side of the
 * MWC demo 2015.
 *
 * Copyright (C) u-blox Melbourn Ltd
 * u-blox Melbourn Ltd, Melbourn, UK
 *
 * All rights reserved.
 *
 * This source file is the sole property of u-blox Melbourn Ltd.
 * Reproduction or utilization of this source in whole or part is
 * forbidden without the written consent of u-blox Melbourn Ltd.
 */

#include "mbed.h"
#include "IapApi.h"
#include "MDM.h"
#include "IotMeterApi.hpp"
#include "WaterMeterApi.hpp"

#define FILE_REV "Revision 1.0"
#define FILE_CL  "Pushed by RobMeades"
#define FILE_DATETIME "2015/05/15"

// ----------------------------------------------------------------
// GENERAL COMPILE-TIME CONSTANTS
// ----------------------------------------------------------------

// The time for which the flash turns the LED on or off for
#define FLASH_DURATION_MS 250

// The maximum number of datagrams that are available for use
// at any one time (i.e. the total of send and receive)
#define MAX_NUM_DATAGRAMS 10

// The number of milliseconds to wait for a received message
// after a send
#define RECEIVE_DURATION_MS 1000

// The number of seconds to wait for device registration
#define WAIT_REG_S 300

// The number of SOS flashes at an assert before restarting
// if not in development mode; in development mode there
// is no restart, just flashing forever
#define NUM_ASSERT_SOS_BEFORE_RESTART 3

// Baud rate of the PC interface
//#define PC_BAUD_RATE 9600
#define PC_BAUD_RATE 9600

// The maximum length of a MACTEST string (as raw bytes)
#define MACTEST_STRING_MAX_SIZE 48

// The time to wait for a user to interrupt to change
// the mactest string
#define USER_WAIT_DECISECONDS 50

// The size of the stack required by IAP
#define SIZE_STACK_FOR_IAP 32

// An ID for the NVRAM contents so that we
// can tell if it's there.
#define NVRAM_ID 0x12345678

// NVRAM bytes to write (can be a set of fixed sizes, min 256)
#define NVRAM_SIZE 256

// Macro to convert to upper case
#define TO_UPPER(x) (((x) >= 'a' && (x) <= 'z') ? x & ~0x20 : x)

// Macro to tell is something is odd
#define IS_ODD(x) (((x) & 0x01) == 0x01 ? true : false)

// Macro to tell is something is even
#define IS_EVEN(x) (((x) & 0x01) == 0x01 ? false : true)

// Highlight for talking to the user at the PC side
#define COLOUR_HIGHLIGHT "\033[33m"

// Default colour
#define COLOUR_DEFAULT "\033[39m"

// ----------------------------------------------------------------
// TYPES
// ----------------------------------------------------------------

/// A type to hold a datagram
// Used for encoding AT commands on the send side and
// receiving messages on the receive side.
typedef struct DatagramTag_t
{
    uint32_t size;
    char pBody[MAX_DATAGRAM_SIZE_RAW];
} Datagram_t;

/// A linked list entry holding a datagram
typedef struct DatagramEntryTag_t
{
    bool inUse;
    Datagram_t datagram;
    DatagramEntryTag_t * pPrevEntry;
    DatagramEntryTag_t * pNextEntry;

} DatagramEntry_t;

/// Debug flashes
typedef enum DebugSosTypeTag_t
{
    DEBUG_SOS_WATER_METER_PROBLEM = WAKE_UP_CODE_WATER_METER_PROBLEM,
    DEBUG_SOS_AT_COMMAND_PROBLEM = WAKE_UP_CODE_AT_COMMAND_PROBLEM,
    DEBUG_SOS_NETWORK_SEND_PROBLEM = WAKE_UP_CODE_NETWORK_SEND_PROBLEM,
    DEBUG_SOS_MEMORY_ALLOC_PROBLEM = WAKE_UP_CODE_MEMORY_ALLOC_PROBLEM,
    DEBUG_SOS_PROTOCOL_PROBLEM = WAKE_UP_CODE_PROTOCOL_PROBLEM,
    DEBUG_SOS_GENERIC_FAILURE = WAKE_UP_CODE_GENERIC_FAILURE,
    DEBUG_SOS_REBOOT = WAKE_UP_CODE_REBOOT,
} DebugSosType_t;

/// Context data, which will be in NVRAM
// and must be no bigger than NVRAM_SIZE
typedef struct NvContextTag_t
{
    uint32_t id;                             //<! An ID for the NVRAM
    bool devModeOnNotOff;                    //<! Development mode
    WakeUpCode_t wakeUpCode;                 //<! Wake-up code
    uint32_t readingIntervalSeconds;         //<! The read interval
    bool ledOnNotOff;                        //<! The LED state
    bool flashOnNotOff;                      //<! The Flash state
    char mactestString[MACTEST_STRING_MAX_SIZE]; //<! The MACTEST string, only used with phase 1 modules
                                             //<! this is a string of bytes (so NOT the hex coded form)
                                             //<! and no terminator is stored
    uint32_t mactestStringSize;              //<! The size of the MACTEST string
} NvContext_t;


typedef union NvramTag_t
{
    char rawBytes[NVRAM_SIZE];
    NvContext_t nvContext;
} Nvram_t;

// Note that order is important below: modules of newer phases
// should be of higher values
typedef enum ModulePhase_t
{
    MODULE_PHASE_UNKNOWN,
    MODULE_PHASE_1,     //<! As used at Mobile World Congress Barcelona 2015
    MODULE_PHASE_2,    //<! As used at Mobile World Congress Shanghai 2015
    MODULE_PHASE_3
} ModulePhase;

// ----------------------------------------------------------------
// PRIVATE VARIABLES
// ----------------------------------------------------------------

// Non-volatile storage area, aligned on a 4-byte boundary
Nvram_t  __attribute__((aligned(4))) nvram;

// Instantiate the NVRAM handling code
static IAP iap;

// Pointer to instance of the Neul modem, populated once we
// get into main() as its constructor requires some initialisation
// that is only complete on entry into main()
MDMSerial *pgNeul = NULL;

// Instantiate the interface to the water meter
static WaterMeterHandler gWaterMeter;

// Instantiate the messaging API to the C027N
// as an IoT metering device
static MessageCodec gMessageCodec;

// The module phase
static ModulePhase gModulePhase = MODULE_PHASE_UNKNOWN;

// A good default MACTEST string
static char defaultMactestString[] = {0x02, 0x00, 0x00, 0x00, 0x60, 0xb3, 0xc8, 0x37, 0x20, 0x0e, 0x1a, 0x35, 0x16, 0x00, 0x00, 0x00, 0x45, 0x23, 0x01, 0x00, 0x01, 0x02, 0x01, 0x00};

// The LED
static DigitalOut led(LED);
static const int gpioTable[] = {D0, D1, D2, D3, D4, D5, D6, D7, D8, D9, D10, D11, D12, D13};

// The connected GPIO(s)
static DigitalOut pump((PinName) gpioTable[GPIO_WATER_PUMP]);

// Head of free datagram linked list
static DatagramEntry_t gFreeDatagramListHeadUnused;
// Head of send datagram linked list
static DatagramEntry_t gSendDatagramListHead;
// Head of receive datagram linked list
static DatagramEntry_t gRecvDatagramListHead;

const char hexTable[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', \
                         '9', 'a', 'b', 'c', 'd', 'e', 'f'};

// ----------------------------------------------------------------
// GENERIC PRIVATE FUNCTION PROTOTYPES
// ----------------------------------------------------------------

static void doDebugFlash (uint32_t numberOfFlashes);
static void debugSos(DebugSosType_t debugSosType);
#ifdef DEBUG
  static void debugPrintLists (void);
#endif
static void getModulePhase (void);
static bool isValidHex (char digit);
static uint8_t hexToNibble (char digit);
static uint8_t hexTobyte (char digit1, char digit2);
static char peekChar (void);
static char getChar (void);
static uint32_t getUserHexString (uint32_t size, char * buf);
static void printByteString (uint32_t size, const char * buf);
static void printMactestString (uint32_t size, const char * string);
static void configureModule (void);
static void assert (bool condition, DebugSosType_t debugSosType, const char * pFormat, ...);
static void assertAlways (DebugSosType_t debugSosType, const char * pFormat, ...);
static bool writeNvContext (Nvram_t * pNvram);
static NvContext_t * resetNvContext (void);
static NvContext_t * readNvContext (void);
static void doSendFlash (void);
static void doRecvFlash (void);
static bool setGpio (GpioState_t * pGpioState);
static bool getGpio (GpioState_t * pGpioState);
static void setWakeUpCode (WakeUpCode_t wakeUpCode);
static void setDevMode (bool onNotOff);
static void setReadingIntervalSeconds (uint32_t readingIntervalSeconds);
static void setLed (bool onNotOff);
static void setFlash (bool onNotOff);
static bool init (void);
static void deInit (void);
static Datagram_t * allocDatagram (DatagramEntry_t * pDatagramListHead);
static Datagram_t * allocSendDatagram (void);
static Datagram_t * allocRecvDatagram (void);
static void freeDatagram (Datagram_t *pDatagram, DatagramEntry_t * pDatagramListHead);
static void freeRecvDatagram (Datagram_t *pDatagram);
static void freeAllDatagrams (DatagramEntry_t *pDatagramListHead);
static void freeUnusedDatagrams (DatagramEntry_t *pDatagramListHead);
static void freeUnusedSendDatagrams (void);
static void freeUnusedRecvDatagrams (void);
static bool waitReg (uint32_t durationSeconds);
static bool sendAndReceive (void);
static bool queueInitialDatagram (void);
static bool queuePeriodicDatagram (void);
static bool handleSerialNumberGetReqDlMsg (void);
static bool handleRebootReqDlMsg (bool devModeOnNotOff);
static bool enqueueReadingIntervalSetGetCnfUlMsg (bool setNotGet);
static bool handleReadingIntervalSetReqDlMsg (uint32_t readingIntervalSeconds);
static bool handleReadingIntervalGetReqDlMsg (void);
static bool enqueueGpioSetGetCnfUlMsg (bool setNotGet, GpioState_t * gpioState);
static bool handleGpioSetReqDlMsg (GpioState_t * pGpioState);
static bool handleGpioGetReqDlMsg (uint8_t gpio);
static bool enqueueLedSetGetCnfUlMsg (bool setNotGet);
static bool handleLedSetReqDlMsg (bool onNotOff);
static bool handleLedGetReqDlMsg (void);
static bool enqueueFlashSetGetCnfUlMsg (bool setNotGet);
static bool handleFlashSetReqDlMsg (bool onNotOff);
static bool handleFlashGetReqDlMsg (void);
static bool processReceivedDatagrams (void);
static MDMSerial *getMDMSerial(void);

// ----------------------------------------------------------------
// GENERIC PRIVATE FUNCTIONS
// ----------------------------------------------------------------

// Debug
static void doDebugFlash (uint32_t numberOfFlashes)
{
    uint32_t x;
    uint32_t y;
    bool devModeOnNotOff = readNvContext()->devModeOnNotOff;

    led = 0;
    wait_ms (1000);
    for (y = 0; (y < NUM_ASSERT_SOS_BEFORE_RESTART) | devModeOnNotOff; y++)
    {
        for (x = 0; x < numberOfFlashes; x++)
        {
            led = !led;
            wait_ms (FLASH_DURATION_MS);
            led = !led;
            wait_ms (FLASH_DURATION_MS);
        }
        wait_ms (2000);
    }

    // Restart the system and try again
    printf ("Resetting system...\n");
    wait_ms (2000);
    NVIC_SystemReset();

}

static void debugSos(DebugSosType_t debugSosType)
{
    switch (debugSosType)
    {
        case (DEBUG_SOS_WATER_METER_PROBLEM):
        {
            doDebugFlash (3);
        }
        break;
        case (DEBUG_SOS_AT_COMMAND_PROBLEM):
        {
            doDebugFlash (4);
        }
        break;
        case (DEBUG_SOS_NETWORK_SEND_PROBLEM):
        {
            doDebugFlash (5);
        }
        break;
        case (DEBUG_SOS_MEMORY_ALLOC_PROBLEM):
        {
            doDebugFlash (6);
        }
        break;
        case (DEBUG_SOS_PROTOCOL_PROBLEM):
        {
            doDebugFlash (7);
        }
        break;
        case (DEBUG_SOS_GENERIC_FAILURE):
        {
            doDebugFlash (8);
        }
        break;
        case (DEBUG_SOS_REBOOT):
        {
            doDebugFlash (9);
        }
        break;
        default:
        {
            doDebugFlash (10);
        }
        break;
    }
}

/// Print out the datagram lists for debugging
#ifdef DEBUG
static void debugPrintLists (void)
{
    uint32_t x;
    DatagramEntry_t * pEntry;

    printf ("LIST CONTENTS:\n");

    printf ("Free list head is at 0x%ld.\n", &gFreeDatagramListHeadUnused);
    pEntry = &gFreeDatagramListHeadUnused;
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        printf (" %d: prev 0x%lx, next 0x%lx, inUse 0x%x, datagram.size %d, datagram \"%s\".\n",
                x, pEntry->pPrevEntry, pEntry->pNextEntry, pEntry->inUse, pEntry->datagram.size, pEntry->datagram.pBody);
        pEntry = pEntry->pNextEntry;
    }

    printf ("Send list head is at 0x%ld.\n", &gSendDatagramListHead);
    pEntry = &gSendDatagramListHead;
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        printf (" %d: prev 0x%lx, next 0x%lx, inUse 0x%x, datagram.size %d, datagram \"%s\".\n",
                x, pEntry->pPrevEntry, pEntry->pNextEntry, pEntry->inUse, pEntry->datagram.size, pEntry->datagram.pBody);
        pEntry = pEntry->pNextEntry;
    }

    printf ("Recv list head is at 0x%ld.\n", &gRecvDatagramListHead);
    pEntry = &gRecvDatagramListHead;
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        printf (" %d: prev 0x%lx, next 0x%lx, inUse 0x%x, datagram.size %d, datagram \"%s\".\n",
                x, pEntry->pPrevEntry, pEntry->pNextEntry, pEntry->inUse, pEntry->datagram.size, pEntry->datagram.pBody);
        pEntry = pEntry->pNextEntry;
    }
}
#endif

// Set the phase of the module in a global variable
static void getModulePhase (void)
{
    char * pVersion = NULL;

    pVersion = pgNeul->getVersion();

    if (pVersion != NULL)
    {
        printf ("Version: \"%s\", so ", pVersion);
        if (strcmp(pVersion, "Neul CIoT Phase 2") == 0)
        {
            gModulePhase = MODULE_PHASE_2;
            printf ("module is MODULE_PHASE_2.\n");
        }
        else if (strcmp(pVersion, "Neul CIoT") == 0)
        {
            gModulePhase = MODULE_PHASE_1;
            printf ("module is MODULE_PHASE_1.\n");
        }
        else
        {
            printf ("module is MODULE_PHASE_UNKNOWN.\n");
        }
    }
}

#if 0
// Convert to upper case
// Not using this as found it didn't seem to work for the Chinese using their
// terminal program.  Potential character set issues maybe?
// Instead, please do a direct comparison against upper and lower case versions
static char toUpper (char digit)
{
    if (digit >= 'a' && digit <= 'z')
    {
        digit &= ~0x20;
    }

    return digit;
}
#endif

// Return true if digit is a valid hex
// character, otherwise false
static bool isValidHex (char digit)
{
    uint32_t x;
    bool isValid = false;

    for (x = 0; (x < sizeof (hexTable)) && !isValid; x++)
    {
        if (TO_UPPER(digit) == TO_UPPER(hexTable[x]))
        {
            isValid = true;
        }
    }

    return isValid;
}

// Take a hex nibble and convert to a byte.
static uint8_t hexToNibble (char digit)
{
    uint8_t nibbleValue = 0;

    digit = TO_UPPER (digit);
    if (isValidHex (digit))
    {
        if (digit >= '0' && digit <= '9')
        {
            nibbleValue = digit - '0';
        }
        else
        {
            if (digit >= 'A' && digit <= 'F')
            {
                nibbleValue = digit - 'A' + 10;
            }
        }
    }

    return nibbleValue;
}

// Take a hex value made up of digit1 and digit2
// and return the corresponding byte value.
// So if digit1 were 'a' and digit2 '3' the hex value
// would be 0xA3.  Invalid hex digits are ignored.
static uint8_t hexTobyte (char digit1, char digit2)
{
    return (hexToNibble (digit1) << 4) + hexToNibble (digit2);
}

// See if a character has been entered
// at the PC serial port and return it,
// or otherwise return 0.
static char peekChar (void)
{
    char input = 0;
    Serial pc (USBTX, USBRX);

    pc.baud (PC_BAUD_RATE);

    if (pc.readable())
    {
        input = pc.getc();
    }

    return input;
}

// Wait for a character to be entered
// at the PC serial port and return it.
static char getChar (void)
{
    Serial pc (USBTX, USBRX);

    pc.baud (PC_BAUD_RATE);

    return pc.getc();
}

// Get a string of numbers from the user as a hex
// string and store it as a string of bytes in buf
// String entry is terminated with <enter>, which is
// NOT stored with the string and also no
// terminator is stored.
// If characters that aren't a valid part of a hex
// string are entered they will be ignored.
static uint32_t getUserHexString (uint32_t size, char * buf)
{
    char input;
    char hexChar1 = 0;
    bool done = false;
    uint32_t charCount = 0;
    uint32_t byteCount = 0;
    Serial pc (USBTX, USBRX);

    pc.baud (PC_BAUD_RATE);

    while (!done && (byteCount < size))
    {
        input = pc.getc();
        if (input == '\r')
        {
            done = true;
        }
        else
        {
            if (isValidHex(input))
            {
                pc.putc(input);
                if (IS_EVEN (charCount))
                {
                    hexChar1 = input;
                }
                else
                {
                    buf[byteCount] = hexTobyte(hexChar1, input);
                    byteCount++;
                }
                charCount++;
            }
        }
    }

    pc.putc('\n');

    return byteCount;
}

/// Print a byte string as hex
static void printByteString (uint32_t size, const char * buf)
{
    char hexBuf[MACTEST_STRING_MAX_SIZE * 2];
    uint32_t x = 0;
    uint32_t y = 0;

    for (x = 0; (x < size) && (y < sizeof (hexBuf)); x++)
    {
        hexBuf[y] = hexTable[(buf[x] >> 4) & 0x0f]; // upper nibble
        y++;
        if (y < sizeof (hexBuf))
        {
            hexBuf[y] = hexTable[buf[x] & 0x0f]; // lower nibble
            y++;
        }
    }

    printf ("%.*s", (int) y, hexBuf);
}

// Print out the AT+MACTEST string
static void printMactestString (uint32_t size, const char * string)
{
    if (size > 0)
    {
        printf ("The MACTEST string will be:\n");
        printf ("AT+MACTEST=%d,", (int) size);
        printByteString(size, string);
        printf ("\n");
    }
    else
    {
        printf ("No MACTEST string has been set.\n");
    }
}

// Enter transparent AT command mode until
// CTRL-C is detected.
static void transparentMode (void)
{
    char input;
    bool done = false;
    Serial pc (USBTX, USBRX);

    pc.baud (PC_BAUD_RATE);

    while (!done)
    {
        // transfer data from pc to modem
        if (pc.readable() && pgNeul->writeable())
        {
            input = pc.getc();
            if (input == '\x03') // CTRL-C
            {
                done = true;
            }
            else
            {
                pc.putc (input); // echo
                pgNeul->putc (input);
            }
        }

        // transfer data from modem to pc
        if (pgNeul->readable() && pc.writeable())
        {
            pc.putc (pgNeul->getc());
        }
    }
}

// User configuration of the module
// Should only be called after the module has been initialised
static void configureModule (void)
{
    bool done = false;
    uint32_t x;

    printf ("%s\n", COLOUR_HIGHLIGHT);
    /* get the phase of the module */
    getModulePhase();

    printf ("\n");
    if (gModulePhase == MODULE_PHASE_UNKNOWN)
    {
        printf ("Phase of module SW unknown, skipping config...\n");
    }
    else
    {
        if (gModulePhase == MODULE_PHASE_1)
        {
            // Print out the AT+MACTEST command on the debug port and give the PC side
            // some time to change it
            printMactestString (readNvContext()->mactestStringSize, readNvContext()->mactestString);
            printf ("Press 'y' within 5 seconds to change this, 'n' to continue. \n");
        }
        else
        {
            printf ("Press 'y' within 5 seconds to enter AT command mode. \n");
        }

        for (x = 0; (x < USER_WAIT_DECISECONDS) && !done; x++)
        {
            char input;
            led = !led;
            wait_ms (100);

            input = peekChar();

            if (input != 0)
            {
                printf ("You pressed '%c' (0x%x).\n", input, input);
            }

            if ((input == 'Y') || (input == 'y'))
            {
                uint32_t y;

                led = 1;

                if (gModulePhase == MODULE_PHASE_1)
                {
                    while (!done)
                    {
                        char string[MACTEST_STRING_MAX_SIZE];

                        printf ("\n");
                        printf ("Enter the new MACTEST string (just the part beyond the\n");
                        printf ("comma) and press <enter>, or just press <enter> to clear\n");
                        printf ("the current MACTEST string.  Backspace/delete will not work\n");
                        printf ("but you can check your entry, and try again, before it is\n");
                        printf ("committed: ");
                        y = getUserHexString (sizeof (string), string);

                        printf ("\n");
                        printMactestString (y, string);

                        printf ("\n");
                        printf ("Is this correct? Press 'y' to save it, 'n' to try again,\n");
                        printf ("any other key to continue without changes. ");
                        input = getChar();
                        printf ("\nYou pressed '%d'.\n", input);
                        if ((input == 'Y') || (input == 'y'))
                        {
                            printf ("Saving...\n");
                            nvram.nvContext.mactestStringSize = y;
                            memcpy (nvram.nvContext.mactestString, string, y);
                            writeNvContext (&nvram);
                            done = true;
                        }
                        else
                        {
                            if (input != 'N')
                            {
                                printf ("OK, continuing...\n");
                                done = true;
                            }
                        }
                    }
                }
                else
                {
                    printf ("\n");
                    printf ("You are now in transparent AT command mode with the modem.\n");
                    printf ("Enter whatever commands are required to configure the modem\n");
                    printf ("and press CTRL-C to continue.\n");
                    transparentMode();
                    done = true;
                    printf ("\nContinuing...\n");
                }
            }
            else
            {
                if ((input == 'N') || (input == 'n'))
                {
                    done = true;
                    printf ("OK, continuing...\n");
                }
            }
       }

        if (x == USER_WAIT_DECISECONDS)
        {
            printf ("Timed out, no key-press detected...\n");
        }

        if ((gModulePhase == MODULE_PHASE_1) && (readNvContext()->mactestStringSize > 0))
        {
            // Send the mactest string to the module
            // No need to do anything for a phase 2 module SW as those values are stored
            // in NVRAM inside the module
            printf ("Sending the MACTEST string to the module...\n");
            if (pgNeul->sendMactest (readNvContext()->mactestStringSize, readNvContext()->mactestString))
            {
                printf ("Done.\n");
            }
            else
            {
                printf ("Module responded with ERROR.\n");
            }
        }
    }

    led = 0;

    printf (COLOUR_DEFAULT);
}

// Asserts
static void assert (bool condition, DebugSosType_t debugSosType, const char * pFormat, ...)
{
    if (!condition)
    {
        printf ("\n!!! ASSERT !!! ASSERT !!! ASSERT !!! ASSERT !!! ASSERT !!!\n");
        va_list args;
        va_start (args, pFormat);
        printf (pFormat, args);
        va_end (args);
        printf ("\n!!! ASSERT !!! ASSERT !!! ASSERT !!! ASSERT !!! ASSERT !!!\n\n\n");
        wait_ms (2000);
        setWakeUpCode ((WakeUpCode_t) debugSosType);
        debugSos (debugSosType);
    }
}

static void assertAlways (DebugSosType_t debugSosType, const char * pFormat, ...)
{
    assert (false, debugSosType, pFormat);
}

/// Write to NVRAM, returning true if successful,
// otherwise false.  The pointer pNvContext must be
// on a 4 byte boundary and the structure size must
// be a multiple of 4.
static bool writeNvContext (Nvram_t * pNvram)
{
    bool success = false;
    uint32_t iapResult;

#ifdef DEBUG
    printf ("Writing structure at 0x%08x to NVRAM.\n", pNvram);
#endif
    assert (((((uint32_t) pNvram) & 0x03) == 0), DEBUG_SOS_GENERIC_FAILURE, "pNvContext must be on a 4 byte boundary.");
    assert (((sizeof (pNvram->rawBytes) & 0x03) == 0), DEBUG_SOS_GENERIC_FAILURE, "sizeof (pNvram->rawBytes) must be a multiple of 4.");

    // Blank check: mbed erases all flash contents after downloading a new executable
    iapResult = iap.blank_check (FLASH_SECTOR (USER_FLASH_AREA_START), FLASH_SECTOR (USER_FLASH_AREA_START));
#ifdef DEBUG
    printf ("NVRAM: blank check result = 0x%x\n", iapResult);
#endif

    // Erase sector, if required
    if (iapResult == SECTOR_NOT_BLANK)
    {
        iap.prepare (FLASH_SECTOR (USER_FLASH_AREA_START), FLASH_SECTOR (USER_FLASH_AREA_START));
        iapResult = iap.erase (FLASH_SECTOR (USER_FLASH_AREA_START), FLASH_SECTOR (USER_FLASH_AREA_START));
#ifdef DEBUG
        printf ("NVRAM: erase result = 0x%x\n", iapResult);
#endif
    }

    // Copy RAM to Flash
    iap.prepare (FLASH_SECTOR (USER_FLASH_AREA_START), FLASH_SECTOR (USER_FLASH_AREA_START));
    iapResult = iap.write (&(pNvram->rawBytes[0]), sector_start_adress[FLASH_SECTOR (USER_FLASH_AREA_START)], sizeof (pNvram->rawBytes));
#ifdef DEBUG
    printf ("NVRAM: copied: SRAM (0x%08x) -> Flash (0x%08x), %d bytes, result = 0x%x\n", pNvram, sector_start_adress[FLASH_SECTOR (USER_FLASH_AREA_START)], sizeof (*pNvram), iapResult);
#endif

    // Compare
    iapResult = iap.compare (&(pNvram->rawBytes[0]), sector_start_adress[FLASH_SECTOR (USER_FLASH_AREA_START)], sizeof (pNvram->rawBytes));
    printf ("NVRAM: compare result = \"%s\"\n", iapResult ? "FAILED" : "OK" );
    if (iapResult == CMD_SUCCESS)
    {
        success = true;
    }

    return success;
}

/// Write defaults to NVRAM and return
// a pointer to the reset contents.
static NvContext_t * resetNvContext (void)
{
    printf ("*** Resetting NVRAM to defaults.\n");

    nvram.nvContext.id = NVRAM_ID;
    nvram.nvContext.devModeOnNotOff = false;
    nvram.nvContext.wakeUpCode = WAKE_UP_CODE_OK;
    nvram.nvContext.readingIntervalSeconds = DEFAULT_READING_INTERVAL_SECONDS;
    nvram.nvContext.flashOnNotOff = true;
    nvram.nvContext.ledOnNotOff = false;
    memcpy (nvram.nvContext.mactestString, defaultMactestString, sizeof (defaultMactestString));
    nvram.nvContext.mactestStringSize = sizeof (defaultMactestString);

    // Ignore the return value - must always return an answer
    writeNvContext (&nvram);

    return &(nvram.nvContext);
}

/// Read from NVRAM, always guaranteed to produce
// a result (may be default values).
static NvContext_t * readNvContext (void)
{
    NvContext_t * pNvContext = NULL;
    uint32_t iapResult;

    // Blank check
    iapResult = iap.blank_check (FLASH_SECTOR (USER_FLASH_AREA_START), FLASH_SECTOR (USER_FLASH_AREA_START));

    if (iapResult == SECTOR_NOT_BLANK)
    {
        if (nvram.nvContext.id != NVRAM_ID)
        {
#ifdef DEBUG
            printf ("Loading NVRAM...\n");
#endif
            memcpy (&(nvram.rawBytes[0]), (const char *) USER_FLASH_AREA_START, sizeof (nvram.rawBytes));
        }

        if (nvram.nvContext.id == NVRAM_ID)
        {
            // Contents are valid
            pNvContext = &(nvram.nvContext);
        }
        else
        {
#ifdef DEBUG
            printf ("NVRAM contents invalid.\n");
            // Contents are not valid so set them up
#endif
            pNvContext = resetNvContext ();
        }
    }
    else
    {
#ifdef DEBUG
        printf ("NVRAM blank.\n");
#endif
        // Contents are not there so set them up
        pNvContext = resetNvContext ();
    }

    return pNvContext;
}

/// Do the flash thing, should be called on a Send
static void doSendFlash (void)
{
    if (readNvContext()->flashOnNotOff)
    {
        led = !led;
        wait_ms (FLASH_DURATION_MS);
        led = !led;
    }
}

/// Do the other flash thing, should be called on a Receive
static void doRecvFlash (void)
{
    if (readNvContext()->flashOnNotOff)
    {
        led = !led;
        wait_ms (FLASH_DURATION_MS /4);
        led = !led;
        wait_ms (FLASH_DURATION_MS /4);
        led = !led;
        wait_ms (FLASH_DURATION_MS /4);
        led = !led;
    }
}

/// Set the state of a GPIO
static bool setGpio (GpioState_t * pGpioState)
{
    bool success = false;

    if (pGpioState->gpio < (sizeof (gpioTable) / sizeof (gpioTable[0])))
    {
        if (pGpioState->inputNotOutput)
        {
            DigitalIn((PinName) gpioTable[pGpioState->gpio]);
        }
        else
        {
            DigitalOut * pOutput = new DigitalOut((PinName) gpioTable[pGpioState->gpio]);
            *pOutput = pGpioState->onNotOff;
        }
        success = true;
    }

    return success;
}

/// Get the state of a GPIO
static bool getGpio (GpioState_t * pGpioState)
{
    bool success = false;

    if (pGpioState->gpio < (sizeof (gpioTable) / sizeof (gpioTable[0])))
    {
        if (pGpioState->inputNotOutput)
        {
            DigitalIn *pInput = new DigitalIn((PinName) gpioTable[pGpioState->gpio]);
            pGpioState->onNotOff = *pInput;
        }
        else
        {
            DigitalOut *pOutput = new DigitalOut((PinName) gpioTable[pGpioState->gpio]);
            pGpioState->onNotOff = *pOutput;
        }
        success = true;
    }

    return success;
}

/// Set the wake-up code
static void setWakeUpCode (WakeUpCode_t wakeUpCode)
{
    if (nvram.nvContext.wakeUpCode != wakeUpCode)
    {
        nvram.nvContext.id = NVRAM_ID;
        nvram.nvContext.wakeUpCode = wakeUpCode;
        writeNvContext (&nvram);
    }
}

/// Set development mode
static void setDevMode (bool onNotOff)
{
    if (nvram.nvContext.devModeOnNotOff != onNotOff)
    {
        nvram.nvContext.id = NVRAM_ID;
        nvram.nvContext.devModeOnNotOff = onNotOff;
        writeNvContext (&nvram);
    }
}

/// Set the reading interval in seconds
static void setReadingIntervalSeconds (uint32_t readingIntervalSeconds)
{
    if (nvram.nvContext.readingIntervalSeconds != readingIntervalSeconds)
    {
        nvram.nvContext.id = NVRAM_ID;
        nvram.nvContext.readingIntervalSeconds = readingIntervalSeconds;
        writeNvContext (&nvram);
    }
}

/// Set the LED state
static void setLed (bool onNotOff)
{
    if (nvram.nvContext.ledOnNotOff != onNotOff)
    {
        nvram.nvContext.id = NVRAM_ID;
        nvram.nvContext.ledOnNotOff = onNotOff;
        writeNvContext (&nvram);
    }
    led = nvram.nvContext.ledOnNotOff;
}

/// Set the Flash state
static void setFlash (bool onNotOff)
{
    if (nvram.nvContext.flashOnNotOff != onNotOff)
    {
        nvram.nvContext.id = NVRAM_ID;
        nvram.nvContext.flashOnNotOff = onNotOff;
        writeNvContext (&nvram);
    }
}

/// Initialise ourselves
static bool init (void)
{
    bool success = true;
    uint32_t x;
    GpioState_t waterPumpGpio = {GPIO_WATER_PUMP, false, false};
    DatagramEntry_t * pPrevEntry = &gFreeDatagramListHeadUnused;
    DatagramEntry_t ** ppEntry = &(gFreeDatagramListHeadUnused.pNextEntry);

    memset (&gFreeDatagramListHeadUnused, 0, sizeof (gFreeDatagramListHeadUnused));
    memset (&gSendDatagramListHead, 0, sizeof (gSendDatagramListHead));
    memset (&gRecvDatagramListHead, 0, sizeof (gRecvDatagramListHead));

    // Create a malloc()ed free list attached to the (unused) static
    // head of the free list
    for (x = 1; (x < MAX_NUM_DATAGRAMS) && success; x++)  // from 1 as it's from head.pNextEntry onwards
    {
        *ppEntry = (DatagramEntry_t *) malloc (sizeof (DatagramEntry_t));
        if (*ppEntry != NULL)
        {
            memset (*ppEntry, 0, sizeof (**ppEntry));
            // Link it in
            (*ppEntry)->pPrevEntry = pPrevEntry;
            (*ppEntry)->pNextEntry = NULL;
            (*ppEntry)->inUse = false;
            if (pPrevEntry != NULL)
            {
                pPrevEntry->pNextEntry = *ppEntry;
            }
            pPrevEntry = *ppEntry;

            // Next
            ppEntry = &((*ppEntry)->pNextEntry);
        }
        else
        {
            assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "failed malloc().");
            success = false;
        }
    }

#ifdef DEBUG
    debugPrintLists ();
#endif

    if (success)
    {
        /* set the water pump control pin to an output and to off */
        success = setGpio (&waterPumpGpio);
        assert (success, DEBUG_SOS_GENERIC_FAILURE, "failed to set water pump pin.");

        if (success)
        {
            // Check NVRAM
            printf ("\n=== IAP: NVRAM check ===\n");
            printf ("  device-ID = 0x%08x\n", iap.read_ID());
            printf ("  serial# = 0x%08x\n", iap.read_serial());
            printf ("  CPU running at %ld MHz\n", SystemCoreClock / 1000000);
            printf ("  user reserved flash area: start_address = 0x%08lx\n", (long unsigned int) iap.reserved_flash_area_start());
            printf ("                            size = %d bytes\n", iap.reserved_flash_area_size());
            printf ("  read_BootVer = 0x%08x\n", iap.read_BootVer());
            printf ("  local ID value = 0x%08lx\n", readNvContext()->id);

            // And finally, setup the LED.
            led = readNvContext()->ledOnNotOff;
        }
    }

    return success;
}

/// Tidy up
static void deInit (void)
{
    uint32_t x = 0;
    DatagramEntry_t * pEntry = NULL;

    // Return send datagram memory to the free list
    freeAllDatagrams (&gSendDatagramListHead);
    // Return receive datagram memory to the free list
    freeAllDatagrams (&gRecvDatagramListHead);

    // Free the free list from the end
    pEntry = &gFreeDatagramListHeadUnused;
    for (x = 0; (pEntry->pNextEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        pEntry = pEntry->pNextEntry;
    }
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        DatagramEntry_t * pNextOne = pEntry->pPrevEntry;
        free (pEntry);
        pEntry = pNextOne;
    }
}

/// Allocate room for a datagram from the given list, returning a pointer
// to it or NULL if unsuccessful
static Datagram_t * allocDatagram (DatagramEntry_t * pDatagramListHead)
{
    Datagram_t * pAlloc = NULL;
    uint32_t x = 0;
    DatagramEntry_t * pEntry;

    // If there is already an unused entry in the list, just return it
    pEntry = pDatagramListHead;
    for (x = 0; (pEntry != NULL) && (pEntry->inUse) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        pEntry = pEntry->pNextEntry;
    }

    if ((pEntry != NULL) && (!pEntry->inUse))
    {
#ifdef DEBUG
        printf ("allocDatagram: found existing unused entry in list.\n");
#endif
        // Good, use this one
        memset (&(pEntry->datagram), 0, sizeof (pEntry->datagram));
        pEntry->inUse = true;
        pAlloc = &(pEntry->datagram);
    }
    else
    {
#ifdef DEBUG
        printf ("allocDatagram: finding malloc()ed entry in the free list...\n");
#endif
        // Find the malloc()ed entry at the end of the free list
        pEntry = &gFreeDatagramListHeadUnused;
        for (x = 0; (pEntry->pNextEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
        {
            pEntry = pEntry->pNextEntry;
        }

        // If there is one, move it to the given list
        if ((pEntry != NULL) && (pEntry->pNextEntry == NULL) && pEntry != &gFreeDatagramListHeadUnused)
        {
            DatagramEntry_t * pWantedEntry = pEntry;

            // Unlink it from the end of the free list
            if (pWantedEntry->pPrevEntry != NULL)
            {
                pWantedEntry->pPrevEntry->pNextEntry = pWantedEntry->pNextEntry;
                pWantedEntry->pPrevEntry = NULL;
            }

            // Attach it to the end of the given list
            pEntry = pDatagramListHead;
            for (x = 0; (pEntry->pNextEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
            {
                pEntry = pEntry->pNextEntry;
            }

            if (pEntry->pNextEntry == NULL)
            {
                pEntry->pNextEntry = pWantedEntry;
                pWantedEntry->pPrevEntry = pEntry;
                pWantedEntry->inUse = true;
                pAlloc = &(pWantedEntry->datagram);
            }
            else
            {
                assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "allocDatagram: failed to find end of list (x = %d).", x);
            }
        }
        else
        {
            assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "allocDatagram: failed to find malloc()ed entry in free list (x = %d).", x);
        }

    }

    return pAlloc;
}

/// Allocate room for a send datagram, returning a pointer
// to it or to NULL if unsuccessful
static Datagram_t * allocSendDatagram (void)
{
    return allocDatagram (&gSendDatagramListHead);
}

/// Allocate room for a receive datagram, returning a pointer
// to it or to NULL if unsuccessful
static Datagram_t * allocRecvDatagram (void)
{
    return allocDatagram (&gRecvDatagramListHead);
}

/// Free a datagram from the given list
static void freeDatagram (Datagram_t *pDatagram, DatagramEntry_t * pDatagramListHead)
{
    uint32_t x = 0;
    DatagramEntry_t * pEntry;

    if (pDatagram != NULL)
    {
#ifdef DEBUG
        printf ("Freeing a datagram (at 0x%lx)...\n", pDatagram);
#endif
        // Find the entry in the list
        pEntry = pDatagramListHead;
        for (x = 0; (pEntry != NULL) && (&(pEntry->datagram) != pDatagram) && (x < MAX_NUM_DATAGRAMS); x++)
        {
            pEntry = pEntry->pNextEntry;
        }

        if ((pEntry != NULL) && (&(pEntry->datagram) == pDatagram))
        {
            DatagramEntry_t * pWantedEntry = pEntry;

#ifdef DEBUG
            printf ("Found the datagram.\n");
#endif
            // Found it, mark it as not in use and, if it is a malloc()ed
            // entry, unlink it from the current list and move it to the
            // free list
            pWantedEntry->inUse = false;
            memset (&(pWantedEntry->datagram), 0, sizeof (pWantedEntry->datagram));
            if (pWantedEntry != pDatagramListHead)
            {
#ifdef DEBUG
                printf ("freeDatagram: moving malloc()ed entry to free list.\n");
#endif
                if (pWantedEntry->pPrevEntry != NULL)
                {
                    pWantedEntry->pPrevEntry->pNextEntry = pWantedEntry->pNextEntry;
                    pWantedEntry->pPrevEntry = NULL;
                }
                if (pWantedEntry->pNextEntry != NULL)
                {
                    pWantedEntry->pNextEntry->pPrevEntry = pWantedEntry->pPrevEntry;
                    pWantedEntry->pNextEntry = NULL;
                }

                // Find the end of the free list
                pEntry = &gFreeDatagramListHeadUnused;
                for (x = 0; (pEntry->pNextEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
                {
                    pEntry = pEntry->pNextEntry;
                }

                // Link it there
                if (pEntry->pNextEntry == NULL)
                {
                    pEntry->pNextEntry = pWantedEntry;
                    pWantedEntry->pPrevEntry = pEntry;
                }
                else
                {
                    assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "freeDatagram: failed to find end of free list (x = %d).", x);
                }
            }
#ifdef DEBUG
            else
            {
                printf ("freeDatagram: just marking head entry as unused.\n");
            }
#endif
        }
        else
        {
            assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM,
                          "freeDatagram: couldn't find entry in list (pDatagram = 0x%lx, gDatagramListHead = 0x%lx, x = %d).",
                          pDatagram, pDatagramListHead, x);
        }
    }
    else
    {
        assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "freeDatagram: NULL pointer passed in.");
    }
}

/// Free a specific datagram from the receive list
static void freeRecvDatagram (Datagram_t *pDatagram)
{
    freeDatagram (pDatagram, &gRecvDatagramListHead);
}

// Empty a datagram list (either send or receive)
static void freeAllDatagrams (DatagramEntry_t *pDatagramListHead)
{
    uint32_t x;
    DatagramEntry_t * pEntry = NULL;

    pEntry = pDatagramListHead;
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        pEntry->inUse = false;
        pEntry = pEntry->pNextEntry;
    }

    freeUnusedDatagrams(pDatagramListHead);
}

/// Free unused datagrams from the send or receive lists
static void freeUnusedDatagrams (DatagramEntry_t *pDatagramListHead)
{
    uint32_t x = 0;
    DatagramEntry_t * pEntry = NULL;

    if ((pDatagramListHead != NULL) && (pDatagramListHead != &gFreeDatagramListHeadUnused))
    {
        // Go to the end of the list
        pEntry = pDatagramListHead;
        for (x = 0; (pEntry->pNextEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
        {
            pEntry = pEntry->pNextEntry;
        }

        if ((pEntry != NULL) && (pEntry->pNextEntry == NULL))
        {
            // Now work backwards up the list freeing unused things until we get
            // to the head
            for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
            {
                DatagramEntry_t * pNextOne = pEntry->pPrevEntry;
                // Check that this entry is unused and not the first static entry then,
                // if it is, free it
                if ((!(pEntry->inUse)) && (pEntry != pDatagramListHead))
                {
                    freeDatagram (&(pEntry->datagram), pDatagramListHead);
#ifdef DEBUG
                    printf ("freeUnusedDatagrams: freeing an entry.\n");
#endif
                }
                pEntry = pNextOne;
            }
        }
        else
        {
            assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "freeUnusedDatagrams : failed to find end of list (x = %d).", x);
        }
    }
    else
    {
        assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "freeUnusedDatagrams: NULL or free list head pointer passed in (== 0x%lx).", pDatagramListHead);
    }
}

/// Free unused datagrams from the send list
static void freeUnusedSendDatagrams (void)
{
    freeUnusedDatagrams (&gSendDatagramListHead);
}

/// Free unused datagrams from the receive list
static void freeUnusedRecvDatagrams (void)
{
    freeUnusedDatagrams (&gRecvDatagramListHead);
}

/// Wait for the device to register on the network
static bool waitReg (uint32_t durationSeconds)
{
    uint32_t count = 0;
    bool success = false;

    while (!success && (count < durationSeconds))
    {
        success = pgNeul->checkNetStatus (NULL);
        wait_ms (1000);
        count++;
    }

    return success;
}

/// Do Sending (working through the inUse datagrams in the Send queue) and
// then a Receive of a single datagram (putting it in the Receive queue).
// \return true if the send was successful, otherwise false.
static bool sendAndReceive (void)
{
    bool success = true; // Set to true as otherwise will fail if there's nothing to send
    uint32_t x;
    DatagramEntry_t * pEntry;
    Datagram_t *pRecvDatagram;
    bool somethingInRxQueue = false;

#ifdef DEBUG
    printf ("\nSending datagram(s)...\n");
#endif
    // Do the sending first
    pEntry = &gSendDatagramListHead;
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS) && success; x++)
    {
        if (pEntry->inUse)
        {
            success = false;
            printf ("\nSending datagram %ld...\n", x);
            // Send the datagram.
            if (pgNeul != NULL)
            {
                success = pgNeul->datagramSend (pEntry->datagram.size, pEntry->datagram.pBody);
            }

            if (success)
            {
                doSendFlash();
                printf ("Sent.\n");
            }
            else
            {
                assertAlways (DEBUG_SOS_NETWORK_SEND_PROBLEM, "Couldn't send datagram.");
            }

            // Lose it always, don't want to clog up
            pEntry->inUse = false;
        }
        pEntry = pEntry->pNextEntry; // Move to the next entry
    }

    freeUnusedSendDatagrams();

    // Now the receiving
    do
    {
        somethingInRxQueue = false;
        pRecvDatagram = allocRecvDatagram ();
        if (pRecvDatagram != NULL)
        {
#ifdef DEBUG
            printf ("Checking for downlink...\n");
#endif
            pRecvDatagram->size = sizeof (pRecvDatagram->pBody);
            // Don't check the return value here (as we don't want to fail
            // if nothing has been received) check the size returned instead
            if (pgNeul != NULL)
            {
                somethingInRxQueue  = pgNeul->datagramRecv ((int *) &(pRecvDatagram->size), pRecvDatagram->pBody, RECEIVE_DURATION_MS);
            }
            // If something was received say so, otherwise free the buffer
            if (somethingInRxQueue && (pRecvDatagram->size > 0))
            {
                doRecvFlash ();
                printf ("A datagram was received.\n");
            }
            else
            {
                freeRecvDatagram (pRecvDatagram);
            }
        }
        else
        {
            assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate a datagram for receive.");
        }
    }
    while (somethingInRxQueue);

    return success;
}

/// Prepare an InitInd, plus a SerialNumberInd.
// \param indNotCnf  if true encode an initInt, else encode an initCnf
// \return true if the preparation was successful, otherwise false.
static bool queueInitialDatagram (void)
{
    bool success = false;
    InitIndUlMsg_t initIndUlMsg;
    SerialNumberIndUlMsg_t serialNumberIndUlMsg;
    Datagram_t * pDatagram;
    uint32_t bytesEncoded = 0;

    printf ("\nPreparing initial datagram...\n");

    pDatagram = allocSendDatagram();
    if (pDatagram != NULL)
    {
        printf ("Reading serial number from water meter...\n");
        // Read the serial number from the water meter
        gWaterMeter.readSerialNumber (&(serialNumberIndUlMsg.serialNumber));
        // Ignore the return value and just set success == true so that the code
        // can run without a water meter attached.
        success = true;
        if (success)
        {
            printf ("Serial number is %ld.\n", serialNumberIndUlMsg.serialNumber);

            // Encode the InitInd and SerialNumberInd messages into a datagram
            printf ("Encoding datagram...\n");

            // Set up the InitInd contents
            initIndUlMsg.wakeUpCode = readNvContext()->wakeUpCode;
            printf ("Wake-up code is 0x%x.\n", initIndUlMsg.wakeUpCode);

            setWakeUpCode (WAKE_UP_CODE_OK);
            bytesEncoded += gMessageCodec.encodeInitIndUlMsg (&(pDatagram->pBody[bytesEncoded]), &initIndUlMsg);
            bytesEncoded += gMessageCodec.encodeSerialNumberIndUlMsg (&(pDatagram->pBody[bytesEncoded]), &serialNumberIndUlMsg);
            pDatagram->size = bytesEncoded;
        }
        else
        {
            assertAlways (DEBUG_SOS_WATER_METER_PROBLEM, "Failed to read serial number from water meter.");
        }
    }
    else
    {
        assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
    }

    return success;
}

/// Prepare the periodic datagram.
// \return true if the preparation was successful, otherwise false.
static bool queuePeriodicDatagram (void)
{
    bool success = false;
    VolumeIndUlMsg_t volumeIndMsg;
    RssiIndUlMsg_t rssiIndMsg;
    uint32_t bytesEncoded =  0;
    Datagram_t * pDatagram;

    printf ("\nPreparing periodic datagram...\n");

    pDatagram = allocSendDatagram();
    if (pDatagram != NULL)
    {
        // Read the volume in litres
        printf ("Reading volume from water meter...\n");
        gWaterMeter.readLitres (&(volumeIndMsg.volumeLitres));
        printf ("Volume is %ld litres.\n", volumeIndMsg.volumeLitres);

        // Ignore the return value and just set success == true so that the code
        // can run without a water meter attached.
        success = true;

        if (success)
        {
            // Read the RSSI
            printf ("Reading RSSI from module...\n");
            if (pgNeul != NULL)
            {
                success = pgNeul->getRssi ((int *) &(rssiIndMsg.rssi));
                printf ("RSSI is %ld.\n", rssiIndMsg.rssi);
            }
            if (success)
            {
                printf ("Encoding datagram...\n");
                // Encode the VolumeInd and RssiInd messages into a datagram
                bytesEncoded += gMessageCodec.encodeVolumeIndUlMsg (&(pDatagram->pBody[bytesEncoded]), &volumeIndMsg);
                bytesEncoded += gMessageCodec.encodeRssiIndUlMsg (&(pDatagram->pBody[bytesEncoded]), &rssiIndMsg);
                pDatagram->size = bytesEncoded;
            }
            else
            {
                assertAlways (DEBUG_SOS_AT_COMMAND_PROBLEM, "Failed to read RSSI from module.");
            }
        }
        else
        {
            assertAlways (DEBUG_SOS_WATER_METER_PROBLEM, "Failed to read volume from water meter.");
        }
    }
    else
    {
        assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
    }

    return success;
}

/// Handle a SerialNumberGetReq message
static bool handleSerialNumberGetReqDlMsg (void)
{
    bool success = false;
    SerialNumberGetCnfUlMsg_t serialNumberGetCnfUlMsg;
    uint32_t bytesEncoded =  0;
    Datagram_t * pDatagram;

    printf ("\nHandling SerialNumberGetReqDlMsg.\n");
    printf ("Reading serial number from water meter...\n");
    // Read the serial number from the water meter
    gWaterMeter.readSerialNumber (&(serialNumberGetCnfUlMsg.serialNumber));

    // Ignore the return value and just set success == true so that the code
    // can run without a water meter attached.
    success = true;

    if (success)
    {
        printf ("Serial number is %ld.\n", serialNumberGetCnfUlMsg.serialNumber);
        printf ("Preparing SerialNumberGetCnfUlMsg response in a datagram...\n");
        pDatagram = allocSendDatagram();
        if (pDatagram != NULL)
        {
            // Encode the serialNumberGetCnfUlMsg into a datagram
            printf ("Encoding datagram...\n");

            bytesEncoded += gMessageCodec.encodeSerialNumberGetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &serialNumberGetCnfUlMsg);

            pDatagram->size = bytesEncoded;
            success = true;
        }
        else
        {
            assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
        }
    }
    else
    {
        assertAlways (DEBUG_SOS_WATER_METER_PROBLEM, "Failed to read volume from water meter.");
    }

    return success;
}

/// Handle a RebootReq message
static bool handleRebootReqDlMsg (bool devModeOnNotOff)
{
    bool success = true;
    printf ("\nHandling RebootReqDlMsg, devMode is 0x%d.\n", devModeOnNotOff);

    setDevMode (devModeOnNotOff);

    wait_ms (1000);

    printf ("Resetting system...\n");
    wait_ms (2000);
    NVIC_SystemReset();

    return success;
}

/// Enqueue a ReadingIntervalSetCnf or a ReadingIntervalGetCnf message
static bool enqueueReadingIntervalSetGetCnfUlMsg (bool setNotGet)
{
    bool success = false;
    uint32_t bytesEncoded =  0;
    Datagram_t * pDatagram;

     printf ("Preparing ReadingIntervalxxxCnfUlMsg response...\n");
     pDatagram = allocSendDatagram();
     if (pDatagram != NULL)
     {
         // Encode the ledxxxCnfUlMsg into a datagram
         printf ("Encoding datagram...\n");
         if (setNotGet)
         {
             ReadingIntervalSetCnfUlMsg_t readingIntervalSetCnfUlMsg;

             readingIntervalSetCnfUlMsg.readingIntervalSeconds = readNvContext()->readingIntervalSeconds;
             bytesEncoded += gMessageCodec.encodeReadingIntervalSetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &readingIntervalSetCnfUlMsg);
         }
         else
         {
             ReadingIntervalGetCnfUlMsg_t readingIntervalGetCnfUlMsg;

             readingIntervalGetCnfUlMsg.readingIntervalSeconds = readNvContext()->readingIntervalSeconds;
             bytesEncoded += gMessageCodec.encodeReadingIntervalGetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &readingIntervalGetCnfUlMsg);
         }

         pDatagram->size = bytesEncoded;
         success = true;
     }
     else
     {
         assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
     }

     return success;
}

/// Handle a ReadingIntervalSetReq message
static bool handleReadingIntervalSetReqDlMsg (uint32_t readingIntervalSeconds)
{
    printf ("\nHandling ReadingIntervalSetReqDlMsg, set to %ld.\n", readingIntervalSeconds);
    setReadingIntervalSeconds (readingIntervalSeconds);

    return enqueueReadingIntervalSetGetCnfUlMsg (true);
}

/// Handle a ReadingIntervalGetReq message
static bool handleReadingIntervalGetReqDlMsg (void)
{
     printf ("\nHandling ReadingIntervalGetReqDlMsg.\n");

     return enqueueReadingIntervalSetGetCnfUlMsg (false);
}

/// Enqueue a GpioSetCnf or a GpioGetCnf message
static bool enqueueGpioSetGetCnfUlMsg (bool setNotGet, GpioState_t * gpioState)
{
    bool success = false;
    uint32_t bytesEncoded =  0;
    Datagram_t * pDatagram;

     printf ("Preparing GpioxxxCnfUlMsg response in a datagram...\n");
     pDatagram = allocSendDatagram();
     if (pDatagram != NULL)
     {
         // Encode the gpioxxxCnfUlMsg into a datagram
         printf ("Encoding datagram...\n");
         if (setNotGet)
         {
             GpioSetCnfUlMsg_t gpioSetCnfUlMsg;

             gpioSetCnfUlMsg.gpioState = *gpioState;
             bytesEncoded += gMessageCodec.encodeGpioSetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &gpioSetCnfUlMsg);
         }
         else
         {
             GpioGetCnfUlMsg_t gpioGetCnfUlMsg;

             gpioGetCnfUlMsg.gpioState = *gpioState;
             bytesEncoded += gMessageCodec.encodeGpioGetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &gpioGetCnfUlMsg);
         }

         pDatagram->size = bytesEncoded;
         success = true;
     }
     else
     {
         assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
     }

     return success;
}

/// Handle a GpioSetReq message
static bool handleGpioSetReqDlMsg (GpioState_t * pGpioState)
{
    bool success = false;

    printf ("\nHandling GpioSetReqDlMsg, gpio %d, inputNotOutput 0x%x, onNotOff 0x%x.\n",
            pGpioState->gpio,
            pGpioState->inputNotOutput,
            pGpioState->onNotOff);

    success = setGpio (pGpioState);

    if (success)
    {
        success = enqueueGpioSetGetCnfUlMsg (true, pGpioState);
    }

    return success;
}

/// Handle a GpioGetReq message
static bool handleGpioGetReqDlMsg (uint8_t gpio)
{
    bool success = false;
    GpioState_t gpioState;

    printf ("\nHandling GpioGetReqDlMsg, gpio %d.\n", gpio);

    gpioState.gpio = gpio;
    success = getGpio (&gpioState);

    if (success)
    {
        success = enqueueGpioSetGetCnfUlMsg (false, &gpioState);
    }

    return success;
}

/// Enqueue an LedSetCnf or an LedGetCnf message
static bool enqueueLedSetGetCnfUlMsg (bool setNotGet)
{
    bool success = false;
    uint32_t bytesEncoded =  0;
    Datagram_t * pDatagram;

     printf ("Preparing LedxxxCnfUlMsg response in a datagram...\n");
     pDatagram = allocSendDatagram();
     if (pDatagram != NULL)
     {
         // Encode the ledxxxCnfUlMsg into a datagram
         printf ("Encoding datagram...\n");
         if (setNotGet)
         {
             LedSetCnfUlMsg_t ledSetCnfUlMsg;

             ledSetCnfUlMsg.onNotOff = readNvContext()->ledOnNotOff;
             bytesEncoded += gMessageCodec.encodeLedSetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &ledSetCnfUlMsg);
         }
         else
         {
             LedGetCnfUlMsg_t ledGetCnfUlMsg;

             ledGetCnfUlMsg.onNotOff = readNvContext()->ledOnNotOff;
             bytesEncoded += gMessageCodec.encodeLedGetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &ledGetCnfUlMsg);
         }

         pDatagram->size = bytesEncoded;
         success = true;
     }
     else
     {
         assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
     }

     return success;
}

/// Handle an LedSetReq message
static bool handleLedSetReqDlMsg (bool onNotOff)
{
    printf ("\nHandling LedSetReqDlMsg, set gLedOnNotOff to 0x%x.\n", onNotOff);
    setLed (onNotOff);

    return enqueueLedSetGetCnfUlMsg (true);
}

/// Handle an LedGetReq message
static bool handleLedGetReqDlMsg (void)
{
     printf ("\nHandling LedGetReqDlMsg.\n");

     return enqueueLedSetGetCnfUlMsg (false);
}

/// Enqueue a FlashSetCnf or a FlashGetCnf message
static bool enqueueFlashSetGetCnfUlMsg (bool setNotGet)
{
    bool success = false;
    uint32_t bytesEncoded =  0;
    Datagram_t * pDatagram;

     printf ("Preparing FlashxxxCnfUlMsg response in a datagram...\n");
     pDatagram = allocSendDatagram();
     if (pDatagram != NULL)
     {
         // Encode the flashSetCnfUlMsg or flashGetCnfUlMsg into a datagram
         printf ("Encoding datagram...\n");
         if (setNotGet)
         {
             FlashSetCnfUlMsg_t flashSetCnfUlMsg;

             flashSetCnfUlMsg.onNotOff = readNvContext()->flashOnNotOff;
             bytesEncoded += gMessageCodec.encodeFlashSetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &flashSetCnfUlMsg);
         }
         else
         {
             FlashGetCnfUlMsg_t flashGetCnfUlMsg;

             flashGetCnfUlMsg.onNotOff = readNvContext()->flashOnNotOff;
             bytesEncoded += gMessageCodec.encodeFlashGetCnfUlMsg (&(pDatagram->pBody[bytesEncoded]), &flashGetCnfUlMsg);
         }

         pDatagram->size = bytesEncoded;
         success = true;
     }
     else
     {
         assertAlways (DEBUG_SOS_MEMORY_ALLOC_PROBLEM, "Couldn't allocate send datagram.");
     }

     return success;
}

/// Handle a FlashSetReq message
static bool handleFlashSetReqDlMsg (bool onNotOff)
{
    printf ("\nHandling FlashSetReqDlMsg, set gFlashOnNotOff to 0x%x.\n", onNotOff);
    setFlash (onNotOff);

    return enqueueFlashSetGetCnfUlMsg (true);
}

/// Handle a FlashGetReq message
static bool handleFlashGetReqDlMsg (void)
{
    printf ("\nHandling FlashGetReqDlMsg.\n");

    return enqueueFlashSetGetCnfUlMsg (false);
}

/// Process any received datagrams, queueing any response datagrams
// as appropriate.
static bool processReceivedDatagrams (void)
{
    bool success = true;
    uint32_t x;
    DlMsgUnion_t dlMsg;
    MessageCodec::DecodeResult_t decodeResult;
    DatagramEntry_t * pEntry;
    const char * pBuffer;

#ifdef DEBUG
    printf ("\nProcessing received datagrams...\n");
#endif

    pEntry = &gRecvDatagramListHead;
    for (x = 0; (pEntry != NULL) && (x < MAX_NUM_DATAGRAMS); x++)
    {
        if (pEntry->inUse)
        {
            printf ("\nProcessing %ld (%ld bytes).\n", x, pEntry->datagram.size);
            pBuffer = pEntry->datagram.pBody;

            // Go through the entire datagram buffer looking for messages
            while (pBuffer < (pEntry->datagram.pBody + pEntry->datagram.size))
            {
                decodeResult = gMessageCodec.decodeDlMsg (&pBuffer, pEntry->datagram.size, &dlMsg);

                switch (decodeResult)
                {
                    case (MessageCodec::DECODE_RESULT_REBOOT_REQ_DL_MSG):
                    {
                        success = handleRebootReqDlMsg (dlMsg.rebootReqDlMsg.devModeOnNotOff);
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_SERIAL_NUMBER_GET_REQ_DL_MSG):
                    {
                        success = handleSerialNumberGetReqDlMsg();
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_READING_INTERVAL_SET_REQ_DL_MSG):
                    {
                        success = handleReadingIntervalSetReqDlMsg (dlMsg.readingIntervalSetReqDlMsg.readingIntervalSeconds);
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_READING_INTERVAL_GET_REQ_DL_MSG):
                    {
                        success = handleReadingIntervalGetReqDlMsg();
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_GPIO_SET_REQ_DL_MSG):
                    {
                        success = handleGpioSetReqDlMsg (&(dlMsg.gpioSetReqDlMsg.gpioState));
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_GPIO_GET_REQ_DL_MSG):
                    {
                        success = handleGpioGetReqDlMsg(dlMsg.gpioGetReqDlMsg.gpio);
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_LED_SET_REQ_DL_MSG):
                    {
                        success = handleLedSetReqDlMsg (dlMsg.ledSetReqDlMsg.onNotOff);
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_LED_GET_REQ_DL_MSG):
                    {
                        success = handleLedGetReqDlMsg();
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_FLASH_SET_REQ_DL_MSG):
                    {
                        success = handleFlashSetReqDlMsg (dlMsg.flashSetReqDlMsg.onNotOff);
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_FLASH_GET_REQ_DL_MSG):
                    {
                        success = handleFlashGetReqDlMsg ();
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_FAILURE):
                    {
                        success = false;
                        assertAlways (DEBUG_SOS_PROTOCOL_PROBLEM, "Message decode failure.");
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_INPUT_TOO_SHORT):
                    {
                        success = false;
                        assertAlways (DEBUG_SOS_PROTOCOL_PROBLEM, "Message decode failure due to input too short.");
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_OUTPUT_TOO_SHORT):
                    {
                        success = false;
                        assertAlways (DEBUG_SOS_PROTOCOL_PROBLEM, "Message decode failure due to output buffer too short.");
                    }
                    break;
                    case (MessageCodec::DECODE_RESULT_UNKNOWN_MSG_ID):
                    {
                        success = false;
                        assertAlways (DEBUG_SOS_PROTOCOL_PROBLEM, "Unknown message ID.");
                    }
                    break;
                    default:
                    {
                        success = false;
                        assertAlways (DEBUG_SOS_PROTOCOL_PROBLEM, "Unknown decode result: 0x%x.", decodeResult);
                    }
                    break;
                }
            }
            pEntry->inUse = false;  // Done with this one now
        }
        pEntry = pEntry->pNextEntry; // Move to the next entry
    }

    freeUnusedRecvDatagrams();

    return success;
}

#ifdef C027N_USE_SOFT_RADIO
  //  SoftModem should be connected to header pins 0 (Rx) and 1 (Tx)
  MDMSerial gNeulSoftRadio (D1, D0, 57600, NC, NC);
#else
  // When compiling under GCC off-line, this works.
  // However, when building under the on-line IDE
  // it seems one has to define MDMSerial using the D1 and D0
  // pins _outside_ a function, static is just not good enough,
  // whereas we must define MDMSerial using MDMTXD and MDMRXD
  // _inside_ a function since it depends on some initialisation
  // that is performed just before entry to main.
  // Hence the lines above.  But I'm leaving this here to
  // show how it is meant to work.

  // Create the Neul modem instance
  static MDMSerial *getMDMSerial(void)
  {
  //#ifdef C027N_USE_SOFT_RADIO
  //    printf ("\nC027N_USE_SOFT_RADIO is defined.\n");
  //    printf ("The on-board modem will be ignored.\n");
  //    printf ("SoftModem should be connected to header pins 0 (Rx) and 1 (Tx).\n");
  //    static MDMSerial gNeul (D1, D0, 57600, NC, NC);
  //#else
      static MDMSerial gNeul (MDMTXD, MDMRXD, 57600, NC, NC);
  //#endif
      return &gNeul;
  }
#endif

// ----------------------------------------------------------------
// PUBLIC FUNCTIONS
// ----------------------------------------------------------------
int realMain (uint8_t * pReserve)
{
    bool success = false;

    Serial pc (USBTX, USBRX);

    pc.baud (PC_BAUD_RATE);

    // Use pReserve to stop any attempts by the compiler to get rid of it
    printf ("Reserved 0x%08lX-0x%08lX for IAP stack.\n", (long unsigned int) pReserve, (long unsigned int) (pReserve + SIZE_STACK_FOR_IAP - 1));

    printf ("\n%s revision details:\n", __FILE__);
    printf ("%s (European time).\n", FILE_DATETIME);
    printf ("%s, %s.\n\n", FILE_REV, FILE_CL);

    printf ("This build will still run if no water meter is connected.\n");
    printf ("The water volume and serial number will be garbage.\n");

    // Instantiate the Neul modem class
#ifdef C027N_USE_SOFT_RADIO
    pgNeul = &gNeulSoftRadio;
#else
    pgNeul = getMDMSerial();
#endif

    if (pgNeul != NULL)
    {
        // Initialise main
        success = init();
    }
    else
    {
        assertAlways (DEBUG_SOS_GENERIC_FAILURE, "Unable to instantiate modem class.");
    }

    if (success)
    {
        // Initialise the water meter
        printf ("Initialising water meter...\n");
        //gWaterMeter.setDebugOn(true);
        gWaterMeter.init();

        // Initialise the Neul module
        printf ("Initialising module...\n");
        pgNeul->setDebug(3);  // Normally set to 2, set to 3 for maximum debug
        success = pgNeul->init();

        if (success)
        {
            // Allow the user to configure the module
            configureModule();

            if (gModulePhase > MODULE_PHASE_1)
            {
                // Wait for registration to complete but
                // only if phase 2 or higher as the CONNECT
                // string was never returned in earlier versions
                printf ("Waiting for device to register...\n");
                success = waitReg (WAIT_REG_S);
            }

            if (success)
            {
                // Prepare the initial messages in a datagram
                success = queueInitialDatagram();

                while (success)
                {
                    Timer timer;
                    uint32_t readingIntervalSeconds = readNvContext()->readingIntervalSeconds;

                    timer.start();

                    // Prepare the periodic messages in a datagram
                    success = queuePeriodicDatagram ();

                    // Now do sends and then receives for the interval
                    printf ("\nSending and then receiving datagrams for %ld seconds...\n", readingIntervalSeconds);
                    while ((timer.read() < readingIntervalSeconds) && success)
                    {
                        success = sendAndReceive() && processReceivedDatagrams();
                        printf (".");
                    }
                    printf ("\n");
                }

                assertAlways (DEBUG_SOS_GENERIC_FAILURE, "Exited main loop with success = false.");
            }
            else
            {
                assertAlways (DEBUG_SOS_NETWORK_SEND_PROBLEM, "Registration failed.");
            }
        }
        else
        {
            assertAlways (DEBUG_SOS_AT_COMMAND_PROBLEM, "Modem initialisation failed.");
        }

        // Tidy up
        deInit();
    }
    else
    {
        assertAlways (DEBUG_SOS_GENERIC_FAILURE, "Initialisation failed.");
    }

    return 0;
}

// This is necessary as the IAP functions use the top SIZE_STACK_FOR_IAP bytes of
// RAM for their own purposes
int main()
{
    uint8_t reserve[SIZE_STACK_FOR_IAP];

    return realMain (reserve);
}

// End Of File