//
// Derivitive work of Michael Peters for Technologic Systems
// Used for configring TS-Modbus ZPU devices
//
// 20130309 J. Lydic Modification of load_zpu to mb_config
// 20130507 J. Lydic Cleanup and addition of TS-1700 configuration
//
// Requires libmodbus 3.0.2
// Compile commandline:
// gcc mb_config.c -o mb_config -lmodbus -I./src -g
//
// Usage: Refer to usage function in the code

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <modbus.h>
#include <asm/termios.h>
#include <time.h>
#include <fcntl.h>
#include <errno.h>
#include <unistd.h>
#include <sys/types.h>
#include <sys/stat.h>

typedef enum {
    false,
    true
} bool;

modbus_t *createModbusChannel(uint8_t *aMbConnectionStr, uint32_t aBitrate, int deviceAddress, bool debug, bool ts7670);

extern char *optarg;

#define MODEL_STR_7670 "7670"

#define CMD_BUF_SIZE 64

#define CONNECT_STR_LENGTH 64
#define CONNECT_STR_LRNGTH 32
#define CONFIG_STR_LRNGTH  101
#define SMHR_TERM_REG             0x71
#define SMHR_FMT_ADDR_REG         0x72
#define SMHR_BIT_RATE_REG         0x73
#define SHR_UNIT_MODEL_REG        0x74
#define SHR_1700_DIO_REG          0x79
#define SHR_1700_TEMP_CFG_REG     0x7a
#define SMHR_CONFIG_REG           0x7d
#define SET_CONFIG_TO_LEGACY_MODE 0xfeed
#define SET_CONFIG_STATUS_FAIL    0xfa17

#define MODEL_1400                0x1400
#define MODEL_1420                0x1420
#define MODEL_1700                0x1700
#define MODEL_1720                0x1720
#define MODEL_1740                0x1740
#define MODEL_1760                0x1760
#define MODEL_1800                0x1800 
#define UNIT_ADDRESS              0xf7
#define UNIT_BIT_RATE             9600
//#define UNIT_BIT_RATE             115200

#define BPS300         300
#define BPS600         600
#define BPS1200       1200
#define BPS2400       2400
#define BPS4800       4800
#define BPS9600       9600
#define BPS19200     19200
#define BPS38400     38400
#define BPS57600     57600
#define BPS115200   115200
#define BPS230400   230400
#define BPS460800   460800
#define BPS1040000 1040000
#define BPS2080000 2080000
#define BPS4160000 4160000

#define BPS_FOR_COMMS       BPS1040000
#define BPS_INDEX_FOR_COMMS BIT_RATE_INDEX_1040000

uint32_t BitRates[] = {
    BPS300,
    BPS600,
    BPS1200,
    BPS2400,
    BPS4800,
    BPS9600,
    BPS19200,
    BPS38400,
    BPS57600,
    BPS115200,
    BPS230400,
    BPS460800,
    BPS1040000,
    BPS2080000,
    BPS4160000,
    0
};

void Usage()
{
    fprintf(stderr, "\nUsage: mb_config -switch paramater [OPTIOSN] ...\n"
            "Configure a Modbus device for legacy mode by setting operational parameters\n"
            "Default parameter values in [brackets]\n"
            "  -?                     This help screen\n"
            "  -b  bit_rate           Bit rate [9600]\n" 
            "                         300, 600, 1200, 2400, 4800, 9600, 19200\n"
            "                         38400, 57600, 115200, 230400, 460800\n"
            "                         1040000, 2080000, 4160000\n"
            "  -C  Configuration      Model 1700: 8 character field starting @ DIO bit 7 thru 0\n" 
            "                         Use single letters 'T' Temperature; 'I' Input: 'O' Output\n"
            "                         All Other model: Not required\n"
            "  -c  connection IP:Port IP address:port [127.0.0.1:7350]\n" 
            "  -d                     Display packet debugging information [Off]\n" 
            "  -f  format (data)      Data format: 8N1, 8N2, 8E1, 8O1, [8N1]\n" 
            "  -h                     This help screen\n"
            "  -i                     Display I/O accesses\n"
            "  -I                     Inhibit actual writing\n"
            "  -m  Board model #      7670 with serial [any other using TCP socket connection]\n"
            "  -n                     Noninteractive mode [off]\n" 
            "  -p                     Display progress messages [Off]\n" 
            "  -s  slave_address      hexadecimal 00-f6; 00 is broadcast [0x10]\n" 
            "  -t  termination        Terminstion on / off [off]\n"
            "  -v                     verboce output mode\n"
            "  DIO Configration for Model TS-1700\n");
    exit(0);
}

bool cOption = false;   // Connexrion path entered
bool COption = false;   // Configuration has been entered
bool dOption = false;   // Display packet debugging information
bool iOption = false;   // Display I/O information
bool IOption = false;   // Inhibit writing
bool nOption = false;   // Not interactive mode
bool vOption = false;   // Display progress messages
bool mOption7670 = false;   // model: initially 7670 woth/tty direct port connection

const char *optionsList = "b:c:C:df:hiIm:nps:t:?v";

unsigned char *SerialFormat[] = {
    "8N1", "8N2", "8E1", "8O1"
};

int16_t GetValidBitrate(uint32_t value)
{
    uint16_t loop = 0;

    if (value == 0)
        return false;

    do
    {
        if (BitRates[loop] == value)
            return loop;
    }
    while (BitRates[++loop] != 0);

    return -1;
}

int16_t GetValidFormat(uint8_t *aString)
{
    uint16_t loop;

    if (aString == NULL)
        return -1;

    for (loop = 0; loop < 4; loop++)
        if (strcmp(SerialFormat[loop], aString) == 0)
            return loop;

    return -1;
}


int16_t SetHoldingRegister(modbus_t *theConnection, uint32_t theAddress, uint16_t theData)
{
    int rtnStatus = 0;

    if (iOption || IOption)
        fprintf(stderr, "SetHoldingRegister - Address: %x (%d)  Data: %04x\n", theAddress, theAddress, theData);

    if (!IOption)
        rtnStatus = modbus_write_register(theConnection, theAddress, theData);

    if (rtnStatus < 0)
        printf("Error writing %04x to register: %x\nErrno: %d - %s\n", theData, theAddress,  errno, modbus_strerror(errno));

    return rtnStatus;
}

int16_t GetHoldingRegister(modbus_t *theConnection, int theAddress, uint16_t *theData)
{
    int rtnStatus = 0;

    rtnStatus = modbus_read_registers(theConnection, theAddress, 1, theData);

    if (rtnStatus < 0)
        printf("Error reading register: %x Errno: %d - %s\n", theAddress,  errno, modbus_strerror(errno));
    else if (iOption || IOption)
        fprintf(stderr, "GetHoldingRegister - Address: %x (%d)  Data: %04x\n", theAddress, theAddress, *theData);

    return rtnStatus;
}

int main(int argc, char **argv)
{
    int aChar;
    uint8_t *argsPtr;
    uint8_t bitRateStr[40];
    uint8_t mbConnectionStr[CONNECT_STR_LRNGTH+1];
    uint8_t slaveAddressStr[40];
    uint8_t terminationStr[40];
    uint8_t ConfigurationString[CONFIG_STR_LRNGTH];
    uint8_t yesNoBuf[32];
    int16_t bitrateIndex;
    int16_t dataFormat;
    int8_t dataFormatStr[40];
    uint16_t Index;
    uint16_t slaveAddress = 0xfe;
    uint16_t Temperature1700, Dio1700;
    uint16_t termination = 0;
    uint16_t theData;
    uint16_t theModel;
    uint32_t bitrate;

    modbus_t *mbConnection;

    //  Setup defaults
    strcpy(bitRateStr,      "9600");
    strcpy(mbConnectionStr, "127.0.0.1:7350");
    strcpy(slaveAddressStr, "10");
    strcpy(dataFormatStr,   "8N1");

    //  Process options
    while ((aChar = getopt(argc, argv, optionsList)) != -1)
    {
        //printf("aChar: 0x%02x  Option: \'%c\'  Optarg: %s\n", aChar,  aChar, optarg);
        switch (aChar)
        {
            case 'b':  // 
                if (strlen(optarg) > 40)
                {
                    fprintf(stderr, "Invalid bit rate size: %s\n", optarg);
                    return 1;
                }
                strcpy(bitRateStr, optarg);
                break;

            case 'c':  // 
                if (strlen(optarg) > CONNECT_STR_LRNGTH)
                {
                    fprintf(stderr, "Invalid connection definition: %s\n", optarg);
                    return 1;
                }
                cOption = true;
                strcpy(mbConnectionStr, optarg);
                break;

            case 'C':  // Configuration
                if (strlen(optarg) != 8)
                {
                    fprintf(stderr, "Invalid Configuration definition: '%s'\n", optarg);
                    return 0;
                }
                COption = true;
                strcpy(ConfigurationString, optarg);
                break;

            case 'd':  // 
                dOption = true;
                break;

            case 'f':  // 
                if (strlen(optarg) > 40)
                {
                    fprintf(stderr, "Invalid data Format size: %s\n", optarg);
                    return 1;
                }
                strcpy(dataFormatStr, optarg);
                break;

            case 'i':  // 
                iOption = true;
                break;

            case 'I':  // 
                IOption = true;
                fprintf(stderr, "Running with writing inhibited\n");
                break;

            case 'm':  // model
                if (strncmp(optarg, MODEL_STR_7670, 4))
                {
                    fprintf(stderr, "Invalid model number: %s\n", optarg);
                    return 0;
                }
                else
                    mOption7670 = true;
                break;

            case 'n':  // 
                nOption = true;
                break;

            case 's':  // 
                if (strlen(optarg) > 40)
                {
                    fprintf(stderr, "Invalid slave address size: %s\n", optarg);
                    return 1;
                }
                strcpy(slaveAddressStr, optarg);
                break;

            case 't':  // 
                if (strlen(optarg) > 40)
                {
                    fprintf(stderr, "Invalid slave address size: %s\n", optarg);
                    return 1;
                }
                strcpy(terminationStr, optarg);
                break;

            case 'v':  // 
                vOption = true;
                break;

            case 'h':  //  Display help information
            case '?':  //  Display help information
            default:
                Usage();
        }
    }

    //  Process option values

    bitrate = atoi(bitRateStr);
    if ((bitrateIndex = GetValidBitrate(bitrate)) < 0)
    {
        fprintf(stderr,"Invalid bit rate: %d\n", bitrate);
        return 1;
    }

    dataFormat = GetValidFormat(dataFormatStr);
    if (dataFormat < 0)
    {
        fprintf(stderr,"Invalid data format: %s\n", dataFormatStr);
        return 1;
    }

    slaveAddress = strtoul(slaveAddressStr, NULL, 16);
    if (slaveAddress > 0xf6 || slaveAddress == 0)
    {
        fprintf(stderr,"Slave ID must be hexadecomal (Range: 01-f6).\n");
        return 1;
    }

    if (mOption7670)
        strcpy(mbConnectionStr, "/dev/ttySP2");
    else
        strcpy(mbConnectionStr, "127.0.0.1:7350");

    if (strcmp(terminationStr, "yes") == 0)
        termination = 1;

    // Set up modbus.

    mbConnection = createModbusChannel(mbConnectionStr, UNIT_BIT_RATE, UNIT_ADDRESS, dOption, mOption7670);

    if (mbConnection == 0)
    {
        fprintf(stderr, "Failure opening device: %s\n", mbConnectionStr);
        exit(0);
    }

    if (!nOption)
    {
        fprintf(stderr, "Do you want to set the following configuration parameters:\n %d %s @ %02x %s\n[y/N]: ",
                BitRates[bitrateIndex], SerialFormat[dataFormat], slaveAddress,
                termination ? "Termination" : "No Termination");
        fgets(yesNoBuf, 8, stdin);
        if (yesNoBuf[0] != 'y' && yesNoBuf[0] != 'Y')
        {
            fprintf(stderr,"No device configuration update performed\n");
            return 0;
        }
        else if (vOption)
            fprintf(stderr,"Updating device configuration\n");

    }
    else
    {
        if (vOption)
            fprintf(stderr, "Setting the following configuration parameters: %d %s @ %02x %s\n",
                    BitRates[bitrateIndex], SerialFormat[dataFormat], slaveAddress,
                    termination ? "Termination" : "No Termination");
    }

    if (GetHoldingRegister(mbConnection, SHR_UNIT_MODEL_REG, &theModel) < 0)
    {
        fprintf(stderr, "Unable to read model\n");
        return 0;
    }
    else if (vOption)
        fprintf(stderr, "Attached to model: TS-%04x\n", theModel);

    if (theModel == MODEL_1700)
    {
        if (COption)
        {
            Temperature1700 = 0;
            Dio1700 = 0;

            argsPtr = ConfigurationString;

            for (Index = 8; Index; Index--)
            {
                switch (*argsPtr)
                {
                    case 't':
                    case 'T':
                        Temperature1700 |= (1 << (Index - 1));
                        break;

                    case 'i':
                    case 'I':
                        Dio1700 |= (1 << (Index - 1));
                        break;

                    case 'o':
                    case 'O':
                        break;

                    default:
                        fprintf(stderr, "Invalid Temperature / DIO configuration for Model 1700: '%s'\n", ConfigurationString);
                        return 0;
                }
                argsPtr++;
            }
        }
        else
        {
            fprintf(stderr, "Model TS-1700 requires Temperature / DIO option: 'C'\n");
            Usage();
        }
    }

    //  Write new address

    if (vOption)
        fprintf(stderr, "Set new data formant & unit address: %02x to %04x\n", SMHR_FMT_ADDR_REG, ((dataFormat << 8) | slaveAddress));
    SetHoldingRegister(mbConnection, SMHR_FMT_ADDR_REG, ((dataFormat << 8) | slaveAddress));

    //  Write bit rate
    if (vOption)
        fprintf(stderr, "Set new bit rate index: %02x to %04x\n", SMHR_BIT_RATE_REG, bitrateIndex);
    SetHoldingRegister(mbConnection, SMHR_BIT_RATE_REG, bitrateIndex);

    //  Write new termination
    if (vOption)
        fprintf(stderr, "Set new bus termniation %s - %02x to %04x\n", termination ? "On" : "Off", SMHR_TERM_REG, termination);
    SetHoldingRegister(mbConnection, SMHR_TERM_REG, termination);

    //  Write Temperature / DIO configuration
    if (theModel == MODEL_1700)
    {
        if (vOption)
        {
            fprintf(stderr, "Set TS-1700 DIO: %02x to %04x\n", SHR_1700_DIO_REG, Dio1700);
            fprintf(stderr, "Set TS-1700 Temperature override: %02x to %04x\n", SHR_1700_TEMP_CFG_REG, Dio1700);
        }
        SetHoldingRegister(mbConnection, SHR_1700_DIO_REG, Dio1700);
        SetHoldingRegister(mbConnection, SHR_1700_TEMP_CFG_REG, Temperature1700);
    }

    //  Set the write legacy persist configration command
    SetHoldingRegister(mbConnection, SMHR_CONFIG_REG, SET_CONFIG_TO_LEGACY_MODE);

    usleep(1500000); 

    GetHoldingRegister(mbConnection, SMHR_CONFIG_REG, &theData);

    if (theData == SET_CONFIG_STATUS_FAIL)
        fprintf(stderr, "Failed to persist legacy configuration\n");
    else
    {
        if (vOption)
            fprintf(stderr, "Persisted legacy configuration successfully\n");
    }

    modbus_close(mbConnection);
    if (vOption)
        printf("Modbus port closed.\n");

    modbus_free(mbConnection);
}

//
//   Create a modbus conection at the specificed bit rate and slave address
//
modbus_t *createModbusChannel(uint8_t *aMbConnectionStr, uint32_t aBitrate, int deviceAddress, bool debug, bool ts7670)
{
    modbus_t *aMbConnection;
    struct termios2 tio2;
    uint32_t theBitrate = aBitrate;
    int status, mbPortFd;
    unsigned char cmdBuf[CMD_BUF_SIZE];

    if (aBitrate > BPS115200)
        theBitrate = BPS38400;

    if ((aMbConnection = modbus_new_rtu(aMbConnectionStr, theBitrate, 'N', 8, 1)) == NULL)
    {
        fprintf(stderr,"Modbus port %s failed to open. Errno: %d, %s\n",
                aMbConnectionStr, errno, modbus_strerror(errno));
        return 0;
    }

    if (vOption)
        fprintf(stderr, "Opened modbus connection @ %d bps on port %s\n", aBitrate, aMbConnectionStr);

    // Set modbus post-setup options.
    modbus_set_slave(aMbConnection, deviceAddress);
    if (debug)
        modbus_set_debug(aMbConnection, 1);
    if (status  = modbus_connect(aMbConnection))
    {
        fprintf(stderr,"File %s failed to open.\n", optarg);
        fprintf(stderr,"modbus_connect failed Errno: %d, %s\n", errno, modbus_strerror(errno));
        return 0;
    }

    if (aBitrate > BPS115200)
    {
        mbPortFd = modbus_get_socket(aMbConnection); // Get the port fd
printf("Setting %d bps using fd: %d\n", aBitrate, mbPortFd);
        tcflush(mbPortFd, TCIFLUSH);
        ioctl(mbPortFd, TCGETS2, &tio2);
        tio2.c_cflag &= ~CBAUD;
        tio2.c_cflag |= BOTHER;
        tio2.c_ospeed = tio2.c_ispeed = aBitrate;
        ioctl(mbPortFd, TCSETS2, &tio2);
    }

    if (vOption)
        printf("Opening Modbus connection on: %s @ %d to device address 0x%x (%d)\n",
                aMbConnectionStr, theBitrate, deviceAddress, deviceAddress);

    // If TS-7670, set RS-485 enable logic to proper bit rate
    if (ts7670)
    {
        sprintf(cmdBuf, "tshwctl --485speed=%d", theBitrate);
        system(cmdBuf);
        if (vOption)
            printf("Setting bit rate windows with: %s\n", cmdBuf);
    }

    return aMbConnection;
}