//    Copyright (C) 2023 Andy Pugh

//    This program is free software; you can redistribute it and/or modify
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation; either version 2 of the License, or
//    (at your option) any later version.
//
//    This program is distributed in the hope that it will be useful,
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
//
//    You should have received a copy of the GNU General Public License
//    along with this program; if not, write to the Free Software
//    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301 USA
//

/* A configurable component to use Mesa PktUART for modbus control */


#include <rtapi.h>
#include <rtapi_slab.h>
#include <rtapi_app.h>
#include <rtapi_string.h>
#include <hal.h>
#include <hostmot2-serial.h>

#if !defined(__KERNEL__)
#include <stdio.h>
#include <stdlib.h>
#endif

/* module information */
MODULE_AUTHOR("Andy Pugh");
MODULE_DESCRIPTION("Modbus control using Mesa PktUART\n"
                    "halcompiled from mesa_modbus.c and renamed");
MODULE_LICENSE("GPL");

#define MAX_CHAN 8

static const char *error_codes[]={
    "Invalid exception code 0",
    "Illegal Function",
    "Illegal Data Address",
    "Illegal Data Value",
    "Server Device Failure",
    "Acknowledge",
    "Server Device Busy",
    "Negative Acknowledge",
    "Memory Parity Error",
    "Unknown exception code 9",
    "Gateway Path Unavailable",
    "Gateway Failed to Respond",
    "Comm Timeout"
};

// This is needed by the header file
typedef struct {
    hal_type_t type;
    int func;
    rtapi_u16 addr;
    rtapi_u8 count;
    char name[HAL_NAME_LEN * 16];
} hm2_modbus_chan_descriptor_t;

#define xstr(x) #x
#define str(x) xstr(x)

#ifdef _COMP_NAME_
#define COMP_NAME str(_COMP_NAME_)
#else
#define COMP_NAME "mesa_modbus"
#endif

// get the channel / register definitions
#ifdef MODFILE
#include str(MODFILE)
#else
#error No Modbus definition file provided. \
       Compile this file with "modcompile my_data.mod"
#include <stop_here>
#endif

#ifndef MAX_MSG_LEN
#define MAX_MSG_LEN 16
#endif

#ifndef DEBUG
#define DEBUG 1
#endif

// Values within +/-EPSILON are considered zero
#define EPSILON  (1e-12)

// Rate is the update-hz pin frequency. There is no point in slower than once
// every 1000 seconds or faster than 10000 times per second. The slowness of
// the RS485/Modbus protocol prevents any real fast communication.
// Anyway, the current implementation does not honor the frequency correctly.
// This limit will at least prevent a divide-by-zero.
#define RATE_MIN (1e-3)
#define RATE_MAX (1e4)

enum {
    START,
    WAIT_FOR_SEND_BEGIN,
    WAIT_FOR_SEND_COMPLETE,
    WAIT_FOR_DATA_FRAME,
    WAIT_FOR_FRAME_SIZES,
    WAIT_FOR_DATA,
    FETCH_MORE_DATA,
    WAIT_A_BIT,
    WAIT_FOR_RX_CLEAR,
    RESET_WAIT,
};

typedef struct {
    hal_data_u **pins;
    hal_float_t **scale;
    hal_data_u **pin2;
    rtapi_s64 *buff;
    hal_bit_t *fault;
    hal_u32_t *last_err;
    hal_s32_t address;
    hal_s32_t baudrate;
    hal_s32_t parity;
    hal_s32_t txdelay;
    hal_s32_t rxdelay;
    hal_s32_t drive_delay;
    hal_float_t rate;
} hm2_modbus_hal_t;

typedef struct {
    hal_type_t type;
    rtapi_u8 func;
    rtapi_u16 addr;
    int count;
    int start_pin;
    int ptr; // pointer to last byte
    rtapi_u8 data[MAX_MSG_LEN + 4]; // add enough space for header and checksum
} hm2_modbus_channel_t;

typedef struct{ 
    char port[HAL_NAME_LEN];
    int num_chans;
    int num_pins;
    hm2_modbus_hal_t *hal;
    hm2_modbus_channel_t *chans;
    int baudrate;
    int parity;
    int txdelay;
    int rxdelay;
    int drive_delay;
    int state;
    int index;
    int pin;
    int frame_index;
    rtapi_u32 fsizes[16];
    rtapi_u32 rxdata[257];
    int iter; // used for timeout
    int old_state; // used for timeout
} hm2_modbus_inst_t;

typedef struct {
    int num_insts;
    hm2_modbus_inst_t *insts;
} hm2_modbus_t;

static int comp_id;
static hm2_modbus_t *m;
static void process(void *arg, long period);
static int parse_data_frame(hm2_modbus_inst_t *inst);
static int build_data_frame(hm2_modbus_inst_t *inst);
static int do_setup(hm2_modbus_inst_t *inst);
static uint16_t RTU_CRC(rtapi_u8* buf, int len);

static char *ports[MAX_CHAN];
RTAPI_MP_ARRAY_STRING(ports, MAX_CHAN, "PktUART names");

int rtapi_app_main(void){
    int retval;
    int i; // instance loops
    int c; // channel loop
    int p; // pin loops
    int j; // generic loops

    rtapi_set_msg_level(DEBUG);

    if (!ports[0]) {
        rtapi_print_msg(RTAPI_MSG_ERR, "The "COMP_NAME" component requires at least"
                " one valid pktuart port, eg ports=\"hm2_5i25.0.pktuart.7\"\n");
        return -EINVAL;
    }

    comp_id = hal_init(COMP_NAME);
    if (comp_id < 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, COMP_NAME": ERROR: hal_init() failed\n");
        return -1;
    }

    // allocate shared memory for the base struct
    m = (hm2_modbus_t*)rtapi_kmalloc(sizeof(hm2_modbus_t), RTAPI_GFP_KERNEL);
    //m = (hm2_modbus_t*)hal_malloc(sizeof(hm2_modbus_t));
    if (m == 0) {
        rtapi_print_msg(RTAPI_MSG_ERR, COMP_NAME": Out of Memory\n");
        hal_exit(comp_id);
        return -1;
    }

    // Count the instances.
    for (m->num_insts = 0; ports[m->num_insts];m->num_insts++) {}
    m->insts = (hm2_modbus_inst_t*)rtapi_kmalloc(m->num_insts * sizeof(hm2_modbus_inst_t), RTAPI_GFP_KERNEL);
    // Parse the config string
    for (i = 0; i < m->num_insts; i++) {
        hm2_modbus_inst_t *inst = &m->insts[i];
        inst->num_chans = sizeof(channels)/sizeof(channels[0]);
        // there may be more pins than channels
        inst->num_pins = 0;
        for (c = 0; c < inst->num_chans; c++){
            inst->num_pins += channels[c].count;
        }
        // Malloc structs and pins, some in main or kernel memory, some in HAL
        inst->chans = (hm2_modbus_channel_t *)rtapi_kmalloc(inst->num_chans * sizeof(hm2_modbus_channel_t), RTAPI_GFP_KERNEL);
        inst->hal =   (hm2_modbus_hal_t *) hal_malloc(sizeof(hm2_modbus_hal_t));
        inst->hal->pins =  (hal_data_u **) hal_malloc(inst->num_pins * sizeof(hal_data_u));
        inst->hal->scale = (hal_float_t **)hal_malloc(inst->num_pins * sizeof(hal_float_t));
        inst->hal->pin2 =  (hal_data_u **) hal_malloc(inst->num_pins * sizeof(hal_data_u));
        inst->hal->buff =  (rtapi_s64 *) hal_malloc(inst->num_pins * sizeof(rtapi_s64));
        
        rtapi_strlcpy(inst->port, ports[i], HAL_NAME_LEN);

        retval = hal_export_functf(process, inst, 1, 0, comp_id, COMP_NAME".%02i", i);
        if (retval < 0) {
            rtapi_print_msg(RTAPI_MSG_ERR, COMP_NAME" ERROR: function export failed\n");
            goto fail0;
        }

        retval =  hal_param_s32_newf(HAL_RW, &(inst->hal->address), comp_id, COMP_NAME".%02i.address", i);
        retval += hal_param_s32_newf(HAL_RW, &(inst->hal->baudrate), comp_id, COMP_NAME".%02i.baudrate", i);
        retval += hal_param_s32_newf(HAL_RW, &(inst->hal->parity), comp_id, COMP_NAME".%02i.parity", i);
        retval += hal_param_s32_newf(HAL_RW, &(inst->hal->txdelay), comp_id, COMP_NAME".%02i.txdelay", i);
        retval += hal_param_s32_newf(HAL_RW, &(inst->hal->rxdelay), comp_id, COMP_NAME".%02i.rxdelay", i);
        retval += hal_param_s32_newf(HAL_RW, &(inst->hal->drive_delay), comp_id, COMP_NAME".%02i.drive-delay", i);
        retval += hal_param_float_newf(HAL_RW, &(inst->hal->rate), comp_id, COMP_NAME".%02i.update-hz", i);
        retval += hal_pin_bit_newf(HAL_OUT, &(inst->hal->fault), comp_id, COMP_NAME".%02i.fault", i);
        retval += hal_pin_u32_newf(HAL_OUT, &(inst->hal->last_err), comp_id, COMP_NAME".%02i.last-error", i);
        if (retval < 0) {
            rtapi_print_msg(RTAPI_MSG_ERR, COMP_NAME" ERROR: failed to create one or more pins/parameters\n");
            goto fail0;
        }

        inst->hal->address = 0x01;
        inst->hal->baudrate = 9600;
        inst->hal->parity = 0;
        inst->hal->txdelay = 20;      // should generally be larger than Rx Delay
        inst->hal->rxdelay = 15;
        inst->hal->drive_delay = 0;   // delay between setting drive enable and sending data
        *(inst->hal->fault) = 0;
        *(inst->hal->last_err) = 0;

        inst->state = START;
                    
        do_setup(inst);
 
        p = 0; // HAL pin index, not aligned to channel index
        for (c = 0; c < inst->num_chans; c++){
            hm2_modbus_channel_t *ch = &(inst->chans[c]);
            hal_pin_dir_t dir;
            rtapi_print_msg(RTAPI_MSG_INFO, "ch %i is at %p\n", c, ch);
            ch->type = channels[c].type;
            ch->func = channels[c].func;
            ch->addr = channels[c].addr;
            ch->count = channels[c].count;
            ch->start_pin = p;
            dir = HAL_OUT; //default, is changed depending on function
            switch (ch->func) {
                case 5: // write coil
                    if (ch->count > 1) {
                        rtapi_print_msg(RTAPI_MSG_ERR, "Count > 1 invalid"
                        " for function 5, resetting\n");
                        ch->count = 1;
                    }
                    /* Fallthrough */
                case 15: // write multiple coils
                    if (ch->count > 8 * MAX_MSG_LEN){
                        ch-> count = 8 * MAX_MSG_LEN;
                        rtapi_print_msg(RTAPI_MSG_ERR,"The Modbus protocol"
                        " enforces a hard limit of 2008 coils per transaction"
                        " but with the current setup file this component is"
                        " limited to %i coils per message. resetting\n",
                        ch->count);
                    }
                    dir = HAL_IN;
                    /* Fallthrough */
                case 1: // read coils
                case 2: // read inputs
                    if (ch->count > 1){
                        for (j = 0; j < ch->count; j++){
                            retval = hal_pin_bit_newf(dir,
                                    (hal_bit_t**)&(inst->hal->pins[p++]),
                                    comp_id, COMP_NAME".%02i.%s-%02i",
                                    i, channels[c].name, j);
                        }
                    } else {
                        retval = hal_pin_bit_newf(dir,
                                    (hal_bit_t**)&(inst->hal->pins[p++]),
                                            comp_id, COMP_NAME".%02i.%s",
                                            i, channels[c].name);
                    }
                    break;
                case 6: // write single register
                    if (ch->count > 1) {
                        rtapi_print_msg(RTAPI_MSG_ERR, "Count > 1 invalid"
                        " for function %i, resetting\n", ch->func);
                        ch->count = 1;
                    }
                    /* Fallthrough */
                case 16: // write multiple registers
                    if (ch->count > MAX_MSG_LEN / 2){
                        ch-> count = MAX_MSG_LEN/2;
                        rtapi_print_msg(RTAPI_MSG_ERR,"The Modbus protocol"
                        " enforces a hard limit of 123 registers per transaction"
                        " but with the current setup file this component is"
                        " limited to %i coils per messsge. resetting\n",
                        ch->count);
                    }
                    dir = HAL_IN;
                    /* Fallthrough */
                case 3: // read holding registers
                case 4: // read input registers
                    for (j = 0; j < ch->count; j++){
                        switch (ch->type){
                        case HAL_U32:
                            if (ch->count > 1) {
                                retval = hal_pin_u32_newf(dir,
                                    (hal_u32_t**)&(inst->hal->pins[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i",
                                    i, channels[c].name, j);
                            } else {
                                retval = hal_pin_u32_newf(dir,
                                    (hal_u32_t**)&(inst->hal->pins[p]),
                                    comp_id, COMP_NAME".%02i.%s",
                                    i, channels[c].name);
                            }
                            p++;
                            break;
                        case HAL_S32:
                            if (ch->count > 1) {
                                retval = hal_pin_s32_newf(dir,
                                    (hal_s32_t**)&(inst->hal->pins[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i",
                                    i, channels[c].name, j);
                                retval = hal_pin_float_newf(HAL_IN,
                                    (hal_float_t**)&(inst->hal->scale[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i-scale",
                                    i, channels[c].name, j);
                                retval = hal_pin_float_newf(dir,
                                    (hal_float_t**)&(inst->hal->pin2[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i-scaled",
                                    i, channels[c].name, j);
                            } else {
                                retval = hal_pin_s32_newf(dir,
                                    (hal_s32_t**)&(inst->hal->pins[p]),
                                    comp_id, COMP_NAME".%02i.%s",
                                    i, channels[c].name);
                                retval = hal_pin_float_newf(HAL_IN,
                                    (hal_float_t**)&(inst->hal->scale[p]),
                                    comp_id, COMP_NAME".%02i.%s-scale",
                                    i, channels[c].name);
                                retval = hal_pin_float_newf(dir,
                                    (hal_float_t**)&(inst->hal->pin2[p]),
                                    comp_id, COMP_NAME".%02i.%s-scaled",
                                    i, channels[c].name);
                            }
                            *inst->hal->scale[p] = 1.0;
                            inst->hal->buff[p] = 0;
                            p++;
                            break;
                        case HAL_FLOAT:
                            if (ch->count > 1) {
                                retval = hal_pin_float_newf(dir,
                                    (hal_float_t**)&(inst->hal->pins[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i",
                                    i, channels[c].name, j);
                                retval = hal_pin_float_newf(HAL_IN,
                                    (hal_float_t**)&(inst->hal->scale[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i-scale",
                                    i, channels[c].name, j);
                                retval = hal_pin_float_newf(HAL_IN,
                                    (hal_float_t**)&(inst->hal->pin2[p]),
                                    comp_id, COMP_NAME".%02i.%s-%02i-offset",
                                    i, channels[c].name, j);
                            } else {
                                retval = hal_pin_float_newf(dir,
                                    (hal_float_t**)&(inst->hal->pins[p]),
                                    comp_id, COMP_NAME".%02i.%s",
                                    i, channels[c].name);
                                retval = hal_pin_float_newf(HAL_IN,
                                    (hal_float_t**)&(inst->hal->scale[p]),
                                    comp_id, COMP_NAME".%02i.%s-scale",
                                    i, channels[c].name);
                                retval = hal_pin_float_newf(HAL_IN,
                                    (hal_float_t**)&(inst->hal->pin2[p]),
                                    comp_id, COMP_NAME".%02i.%s-offset",
                                    i, channels[c].name);
                            }
                            *(inst->hal->scale[p]) = 1;
                            p++;
                            break;
                        default:
                           rtapi_print_msg(RTAPI_MSG_ERR,
                           "Unsupported HAL pin type (%i) in mesa_modbus definition file\n",
                                         ch->type);
                            goto fail0;
                        } // type switch
                    } // pin count loop
                    break;
                default:
                    rtapi_print_msg(RTAPI_MSG_ERR,
                    "Unsupported modbus function %i in mesa_modbus definition file\n",
                                    ch->func);
                    goto fail0;
            } // func switch
        } // channel loop
    } // instance loop
    hal_ready(comp_id);
    return 0;

    fail0:
    hal_exit(comp_id);
    return -1;

}

static int do_setup(hm2_modbus_inst_t *inst){
    
    int retval;
    
    if    (inst->baudrate    == inst->hal->baudrate
        && inst->parity      == inst->hal->parity
        && inst->txdelay     == inst->hal->txdelay
        && inst->rxdelay     == inst->hal->rxdelay
        && inst->drive_delay == inst->hal->drive_delay) return 0;

    if (inst->hal->txdelay > 0xFF) inst->hal->txdelay = 0xFF;
    if (inst->hal->rxdelay > 0xFF) inst->hal->rxdelay = 0xFF;
    inst->baudrate    = inst->hal->baudrate;
    inst->parity      = inst->hal->parity;
    inst->txdelay     = inst->hal->txdelay;
    inst->rxdelay     = inst->hal->rxdelay;
    inst->drive_delay = inst->hal->drive_delay;

    unsigned flg;
    flg  = inst->parity != 0 ? HM2_PKTUART_CONFIG_PARITYEN : 0;
    flg |= inst->parity == 1 ? HM2_PKTUART_CONFIG_PARITYODD : 0;
    hm2_pktuart_config_t cfgrx = {
        .baudrate   = inst->baudrate,
        .filterrate = inst->baudrate * 2,
        .drivedelay = 0,
        .ifdelay    = inst->rxdelay,
        .flags      = flg | HM2_PKTUART_CONFIG_RXEN | HM2_PKTUART_CONFIG_RXMASKEN
    };
    hm2_pktuart_config_t cfgtx = {
        .baudrate   = inst->baudrate,
        .filterrate = 0,
        .drivedelay = inst->drive_delay,
        .ifdelay    = inst->txdelay,
        .flags      = flg | HM2_PKTUART_CONFIG_DRIVEEN | HM2_PKTUART_CONFIG_DRIVEAUTO
    };
    retval = hm2_pktuart_config(inst->port, &cfgrx, &cfgtx, 0);

    if (retval<0)
    {
     rtapi_print_msg(RTAPI_MSG_ERR, COMP_NAME"PktUART setup problem: %d\n", retval);
     return -1;
    }

    return 0;
}

static int send_modbus_pkt(hm2_modbus_inst_t *inst){
    hm2_modbus_channel_t *ch = &(inst->chans[inst->index]);
    rtapi_u16 checksum;
    rtapi_u16 fsizes[1];
    rtapi_u8  frames;
    int i;

    checksum = RTU_CRC(ch->data, ch->ptr + 1);
    ch->data[++(ch->ptr)] = checksum & 0xFF;
    ch->data[++(ch->ptr)] = (checksum >> 8) & 0xFF;

    rtapi_print_msg(RTAPI_MSG_INFO, "Sending to %s %i bytes ", inst->port, ch->ptr + 1);
    for (i = 0; i <= ch->ptr; i++) rtapi_print_msg(RTAPI_MSG_INFO, " %02X ", ch->data[i]);
    rtapi_print_msg(RTAPI_MSG_INFO, "\n");

    frames = 1;
    fsizes[0] = ch->ptr + 1;
    hm2_pktuart_send(inst->port, ch->data, &frames, fsizes);
    return 0;
}

static void do_timeout(hm2_modbus_inst_t *inst){
    if (inst->state != inst->old_state){
        inst->iter = 0;
        inst->old_state = inst->state;
    }
    if (inst->iter++ > 1000){
        rtapi_print_msg(RTAPI_MSG_INFO, "\n %i TIMEOUT_RESET %i\n", inst->iter, inst->state);
        hm2_pktuart_reset(inst->port);
        inst->state = RESET_WAIT;
        inst->iter = 0;
        *(inst->hal->last_err) = 11;
        *(inst->hal->fault) = 1;
        inst->chans[inst->index].data[1] = 0xFF; // Write invalid data to force re-send
    }
    rtapi_print_msg(RTAPI_MSG_INFO, "%i timeout %i\r", inst->iter, inst->state);
}

static void process(void *arg, long period) {
    static long timer = 0;
    hm2_modbus_inst_t *inst = arg;
    real_t rate;
    int r;

    rtapi_u32 rxstatus = hm2_pktuart_get_rx_status(inst->port);
    rtapi_u32 txstatus = hm2_pktuart_get_tx_status(inst->port);
    
    switch (inst->state) {
        case START:

            rate = inst->hal->rate;
            if (rate >= RATE_MIN && rate <= RATE_MAX && (timer -= period) > 0) break;

            rtapi_print_msg(RTAPI_MSG_INFO, "START txstatus = %08X rxstatus = %08X\n", txstatus, rxstatus);

            do_setup(inst);
            
            // Check for received data
            if (rxstatus & 0x200000) {
                inst->state = WAIT_FOR_DATA_FRAME;
                break;
            }
            
            // No incoming data, so service the outputs
            
            if (++(inst->index) >= inst->num_chans) {
                inst->index = 0; // channel counter
            }
            
            if (build_data_frame(inst)){ // if data has changed
                r = send_modbus_pkt(inst);
                inst->state = WAIT_FOR_SEND_BEGIN;
                if(rate >= RATE_MIN && rate <= RATE_MAX)
                    timer = 1e9 / rate;
            }

            break;
        case WAIT_FOR_SEND_BEGIN:
            // single cycle delay to allow for queued data
            rtapi_print_msg(RTAPI_MSG_INFO, "WAIT_FOR_SEND_BEGIN RX %X TX %X\n", txstatus, rxstatus);
            do_timeout(inst); // just to reset the counter
            inst->state = WAIT_FOR_SEND_COMPLETE;
            break;

        case WAIT_FOR_SEND_COMPLETE:
            rtapi_print_msg(RTAPI_MSG_INFO, "WAIT_FOR_SEND_COMPLETE RX %X TX %X\n", rxstatus, txstatus);
            do_timeout(inst);
            if ( ! (txstatus & 0x80)){
                inst->state = WAIT_FOR_DATA_FRAME;
            }
            break;

        case WAIT_FOR_DATA_FRAME:
            do_timeout(inst);
            rtapi_print_msg(RTAPI_MSG_INFO, "WAIT_FOR_DATA_FRAME - rxmode = %X\n", rxstatus);
            if ( ! ( rxstatus & 0x1F0000)) { // no data yet
                break;
            }
            inst->frame_index = 0;
            memset(inst->fsizes, 0, sizeof(inst->fsizes));
            // find the frame size
            hm2_pktuart_queue_get_frame_sizes(inst->port, inst->fsizes);
            inst->state = WAIT_FOR_FRAME_SIZES;
            break;
            
        case WAIT_FOR_FRAME_SIZES:
        case FETCH_MORE_DATA:
            rtapi_print_msg(RTAPI_MSG_INFO, "WAIT_FOR_FRAME_SIZES Index %i Frames %X %X %X %X\n",
                inst->frame_index, inst->fsizes[0], inst->fsizes[1], inst->fsizes[2], inst->fsizes[3]);
            do_timeout(inst);
            if ((inst->fsizes[inst->frame_index] & 0xC000)
                ||  ((inst->fsizes[inst->frame_index] & 0x3FF) > MAX_MSG_LEN)) { // indicates an overrun
                rtapi_print_msg(RTAPI_MSG_INFO, "RESET\n");
                inst->state = START;
                hm2_pktuart_reset(inst->port);
                break;
            }
            r = hm2_pktuart_queue_read_data(inst->port, inst->rxdata, inst->fsizes[inst->frame_index] & 0x3FF);
            rtapi_print_msg(RTAPI_MSG_INFO, "return value %i\n", r);
            inst->state = WAIT_A_BIT;
            break;

        case WAIT_A_BIT:
            inst->state = WAIT_FOR_DATA;
            break;
            
        case WAIT_FOR_DATA:
            rtapi_print_msg(RTAPI_MSG_INFO, "WAIT_FOR_DATA\n");
            parse_data_frame(inst);
            if ((inst->fsizes[++(inst->frame_index)] & 0x3FF) > 0){
                inst->state = FETCH_MORE_DATA;
            } else {
                inst->state = WAIT_FOR_RX_CLEAR;
            }
            break;

        case WAIT_FOR_RX_CLEAR:
            rtapi_print_msg(RTAPI_MSG_INFO, "WAIT_FOR_RX_CLEAR txdata = %08X\r", rxstatus);
            do_timeout(inst);
            rtapi_print_msg(RTAPI_MSG_INFO, "\r");
            if (rxstatus & 0x200000) break;
            inst->state = START;
            *(inst->hal->fault) = 0;
            break;
            
        case RESET_WAIT:
            if (inst->iter++ > 5) inst->state = START;
            break;

        default:
            rtapi_print_msg(RTAPI_MSG_ERR, "Unknown State in mesa_modbus\n");
            inst->state = START;
    }

}

static int ch_append8(hm2_modbus_channel_t *ch, rtapi_u8 v){
    int r = 0;
    if (ch->ptr++ > MAX_MSG_LEN) return -MAX_MSG_LEN;
    if (ch->data[ch->ptr] != v) r = 1; // flag data changed
    ch->data[ch->ptr] = v;
    return r;
}

static int ch_append16(hm2_modbus_channel_t *ch, rtapi_u16 v){
    int r = 0;
    r =  ch_append8(ch, (rtapi_u8)(v >> 8));
    r += ch_append8(ch, (rtapi_u8)(v & 0xFF));
    return r;
}
    
static int ch_init(hm2_modbus_channel_t *ch, hm2_modbus_hal_t hal){
    int r;
    ch->ptr = -1;
    r = ch_append8(ch, hal.address);
    r = ch_append8(ch, ch->func);
    return r;
}

static int build_data_frame(hm2_modbus_inst_t *inst){
    hm2_modbus_channel_t *ch = &(inst->chans[inst->index]);
    hm2_modbus_hal_t hal = *inst->hal;
    rtapi_u8 acc = 0;
    int r = 0;
    int p = ch->start_pin;
    int i;

    rtapi_print_msg(RTAPI_MSG_INFO, "building packet %X %X start pin %i\n", ch->func, ch->addr, p);
    
    ch_init(ch, hal);
    
    switch (ch->func){
        case 1: // Read coils
        case 2: // read discrete inputs
        case 3: // read holding registers
        case 4: // read input registers
            r += ch_append16(ch, ch->addr);
            r += ch_append16(ch, ch->count);
            r += 1; // trigger a read packet every time
            break;
        case 5: // Write single coil
            r += ch_append16(ch, ch->addr);
            if (hal.pins[p]->b){
                r += ch_append16(ch, 0xFF00);
            } else {
                r += ch_append16(ch, 0x0000);
            }
            break;
        case 6: //Write single register
            r += ch_append16(ch, ch->addr);
            switch (ch->type){
            case HAL_U32:
                r += ch_append16(ch, (rtapi_u16)hal.pins[p]->u);
                break;
            case HAL_S32:
                r += ch_append16(ch, (rtapi_s16)hal.pins[p]->s);
                break;
            case HAL_FLOAT:
                rtapi_print_msg(RTAPI_MSG_INFO, "671 %f %f %f \n", hal.pins[p]->f, hal.pin2[p]->f, *hal.scale[p]);
                if (*hal.scale[p] <= -EPSILON || *hal.scale[p] >= +EPSILON){
                    r+= ch_append16(ch, (rtapi_u16)((hal.pins[p]->f - hal.pin2[p]->f) / *hal.scale[p])) ;
                }
                rtapi_print_msg(RTAPI_MSG_INFO, "678\n");
                break;
            default:
                break;
            }
            break;
        case 15: // Write multiple coils
            r += ch_append16(ch, ch->addr);
            r += ch_append16(ch, ch->count);
            r += ch_append8(ch, ( ch->count + 8 - 1) / 8);
            for (i = 0; i < ch->count; i++){
                if (hal.pins[p++]->b) acc += 1 << (i % 8);
                if (i % 8 == 7 || i == (ch->count -1)) { // time for the next byte
                    r += ch_append8(ch, acc);
                    acc = 0;
                }
            }
            break;
        case 16: // write multiple holding registers
            r += ch_append16(ch, ch->addr);
            r += ch_append16(ch, ch->count);
            r += ch_append8(ch, ch->count * 2);
            for (i = 0; i < ch->count; i++){
                switch (ch->type){
                case HAL_U32:
                    r += ch_append16(ch, (rtapi_u16)hal.pins[p]->u);
                    break;
                case HAL_S32:
                    r += ch_append16(ch, (rtapi_s16)hal.pins[p]->s);
                    break;
                case HAL_FLOAT:
                    if (*hal.scale[p] <= -EPSILON  || *hal.scale[p] >= +EPSILON){
                        r+= ch_append16(ch, (rtapi_u16)((hal.pins[p]->f - hal.pin2[p]->f) / *hal.scale[p])) ;
                    } else {
                        // Prevent (near) divide-by-zero and assume scale 1.0
                        r+= ch_append16(ch, (rtapi_u16)(hal.pins[p]->f - hal.pin2[p]->f));
                    }
                    break;
                default:
                    break;
                }
                p++ ; // increment pin pointer
            }
            break;
        default:
            rtapi_print_msg(RTAPI_MSG_ERR, "Unknown Modbus instruction\n");
    }
    return r;
}

static int parse_data_frame(hm2_modbus_inst_t *inst){
    hm2_modbus_channel_t *ch = &(inst->chans[inst->index]);
    hm2_modbus_hal_t hal = *inst->hal;
    rtapi_u32 *data = inst->rxdata;
    int count = inst->fsizes[inst->frame_index] & 0x3FF;
    int i;
    int w = 0;
    int b = 0;
    int p;
    int tmp;
    rtapi_u8 bytes[MAX_MSG_LEN + 4] = {};
    rtapi_u16 checksum;

    rtapi_print_msg(RTAPI_MSG_INFO, "Return packet is ");
    for (i = 0; i < count; i++){
        bytes[i] = (data[w] >> b) & 0xFF;
        rtapi_print_msg(RTAPI_MSG_INFO, "%02X ", bytes[i]);
        if ((b += 8) == 32) { b = 0; w++;}
    }
    rtapi_print_msg(RTAPI_MSG_INFO, "\n");
    if ((bytes[1] & 0x7F ) != ch->func) {
        rtapi_print_msg(RTAPI_MSG_ERR, "call/response function number mismatch\n");
        ch->data[1] = 0xFF; // Write invalid data to force re-send
        return -1;
    }

    checksum = RTU_CRC(bytes, count - 2);
    if (bytes[count - 2] != (checksum & 0xFF) || bytes[count - 1] != (checksum >> 8)){
        rtapi_print_msg(RTAPI_MSG_ERR, "Modbus checksum error\n");
        rtapi_print_msg(RTAPI_MSG_ERR, "%2X%02X != %04X\n", bytes[count - 1], bytes[count - 2], checksum);
        ch->data[1] = 0xFF; // Write invalid data to force re-send
        return -1;
    }

    p = ch->start_pin;

    switch (bytes[1]){
        case 1: // read coils
        case 2: // read inputs
            w = 3;
            b = 0;
            for (i = 0; i < ch->count; i++){
                hal.pins[p++]->b = (bytes[w] & 1 << b);
                if (++b >= 8) { b = 0; w++;}
            }
            break;
        case 3: // read holding registers
        case 4: // read registers
            w = 3;
            for (i = 0; i < bytes[2] / 2; i++){
                switch (ch->type){
                case HAL_U32:
                    hal.pins[p]->u = 256 * bytes[w] + bytes[w + 1];
                    w += 2;
                    p++;
                    break;
                case HAL_S32: // wrap the result into the (s32) offset too
                    tmp = hal.pins[p]->s;
                    hal.pins[p]->s = bytes[w] * 256 + bytes[w + 1];
                    w += 2;
                    tmp = hal.pins[p]->s - tmp;
                    if (tmp > 32768) tmp -= 65536;
                    if (tmp < -32768) tmp += 65536;
                    hal.buff[p] += tmp;
                    hal.pin2[p]->f = hal.buff[p] * *hal.scale[p];
                    p++;
                    break;
                case HAL_FLOAT:
                    hal.pins[p]->f = (256 * bytes[w] + bytes[w + 1])
                                    * *(hal.scale[p]) + hal.pin2[p]->f;
                    w += 2;
                    p++;
                    break;
                default:
                    break;
                }
            }
            break;
        // Nothing to do for write commands 5, 6, 15, 16 ??
        case 5: // set single coil
        case 6: // write single register
        case 15: // write multiple coils echo
        case 16: // write multiple registers echo
            break;

        // The following are error codes    
        case 129: // 1 + error bit
        case 130: // 2 + error bit
        case 131: // 3 + error bit
        case 132: // 4 + error bit
        case 133: // 5 + error bit
        case 134: // 6 + error bit
        case 143: // 15 + error bit
        case 144: // 16 + error bit
            ch->data[1] = 0xFF; // Write invalid data to force re-send
            if(bytes[2] < sizeof(error_codes) / sizeof(*error_codes)) {
                rtapi_print_msg(RTAPI_MSG_ERR, "Modbus error response function %i error %s\n",
                                bytes[1] & 0x7F, error_codes[bytes[2]]);
            } else {
                rtapi_print_msg(RTAPI_MSG_ERR, "Modbus error response function %i returned invalid exception code %u\n",
                                bytes[1] & 0x7F, (unsigned)bytes[2]);
            }
            break;
        default:
            rtapi_print_msg(RTAPI_MSG_ERR, "Unknown or unsupported Modbus function code\n");
    }
    return 0;
}

void rtapi_app_exit(void){
int i;
for (i = 0; i < m->num_insts;i++){
    if (m->insts[i].chans != NULL) rtapi_kfree(m->insts[i].chans);
    // rtapi_kfree(m->insts[i].hal); Automatically freed as was hal_malloc-ed
}
if (m != NULL) rtapi_kfree(m);

hal_exit(comp_id);
}

static uint16_t RTU_CRC(rtapi_u8* buf, int len){
  uint16_t crc = 0xFFFF;
  int pos;
  int i;
  
  for (pos = 0; pos < len; pos++) {
    crc ^= (uint16_t)buf[pos];          // XOR byte into least sig. byte of crc
  
    for (i = 8; i != 0; i--) {    // Loop over each bit
      if ((crc & 0x0001) != 0) {      // If the LSB is set
        crc >>= 1;                    // Shift right and XOR 0xA001
        crc ^= 0xA001;
      }
      else                            // Else LSB is not set
        crc >>= 1;                    // Just shift right
    }
  }
  // Note, this number has low and high bytes swapped, so use it accordingly (or swap bytes)
  return crc;  
}

// vim: syn=c ts=4
