File content as of revision 0:61364762ee0e:

// modbus
// ---------------------------------------------------------------------------------------------------------------------
// includes

#include "mbed.h"
#include "global.h"
#include "mod.h"

// ---------------------------------------------------------------------------------------------------------------------
// globals

unsigned short int mod_data[MOD_DATA];
unsigned short int mod_ireg[MOD_IREG];

int mod_verbose = 0;

// -------------------------------------------------------------------------------------------------
// modbus buffers

#define MOD_RXNUM 200
#define MOD_TXNUM 200

int mod_rxnum = 0;
int mod_txnum = 0;

unsigned char mod_rxbuf[MOD_RXNUM];
unsigned char mod_txbuf[MOD_TXNUM];

// -------------------------------------------------------------------------------------------------
// print functions

void print_uc(char hex, char bin, char dec, char* pre, unsigned char input, char* post)
{
    if(pre[0] != 0) printf("%s", pre);

    if(hex)
    {
        printf("0x%02X", input);
    }
    if(bin)
    {
        if(hex) printf(" ");
        for(int i=7; i>=0; i--)
        {
            if(i<7 && (i%4)==3) printf(" "); if(input & (1<<i)) printf("1"); else printf("0");
        }
    }
    if(dec)
    {
        if(hex || bin) printf(" "); printf("%u", input);
    }
    if(post[0] != 0) printf("%s", post);
}


void print_us(char hex, char bin, char dec, char* pre, unsigned short int input, char* post)
{
    if(pre[0] != 0) printf("%s", pre);

    if(hex)
    {
        printf("0x%04X", input);
    }
    if(bin)
    {
        if(hex) printf(" ");
        for(int i=15; i>=0; i--)
        {
            if(i<15 && (i%4)==3) printf(" "); if(input & (1<<i)) printf("1"); else printf("0");
        }
    }
    if(dec)
    {
        if(hex || bin) printf(" "); printf("%u", input);
    }
    if(post[0] != 0) printf("%s", post); 
}

// -------------------------------------------------------------------------------------------------
// initialization

RawSerial mod(PC_6, PC_7);

DigitalOut mod_de(PC_8);
DigitalOut mod_re(PC_9);

void mod_init(void)
{
    mod_de = 0;
    mod_re = 0;

    mod.baud(MOD_BAUD);
    mod.attach(&mod_process, RawSerial::RxIrq); 
    
    for(int i=0; i<MOD_IREG; i++) mod_ireg[i] = 0;
}

// -------------------------------------------------------------------------------------------------
// utilities

void mod_clear(void)
{
    mod_rxnum = 0; for(int i=0; i<MOD_RXNUM; i++) mod_rxbuf[i] = 0;
    mod_txnum = 0; for(int i=0; i<MOD_TXNUM; i++) mod_txbuf[i] = 0;
}

unsigned short int mod_calc_crc(unsigned char *buffer, int length)
{
    unsigned short int temp=0xFFFF, flag;

    for(int i=0; i<length; i++)
    {
        temp = temp ^ buffer[i];

        for(int j=1; j<=8; j++)
        {
            flag = temp & 0x0001;
            temp = temp >> 1;
            if(flag) temp = temp ^ 0xA001;
        }
    }

    return(temp);
}

void mod_ireg_dump(void)
{
    for(int i=0; i<MOD_IREG; i++)
    {
        printf("   I%02d = ", i);
        print_us(1,0,1, "", mod_ireg[i], "\r\n");
    }
}

// ---------------------------------------------------------------------------------------------------------------------
// process received data

void mod_process(void)
{
    if(mod.readable())
    {
        if(mod_rxnum < MOD_RXNUM)
        {
            mod_rxbuf[mod_rxnum++] = mod.getc();
        }
        else
        {
            if(mod_verbose) printf("   [mod_process] buffer full\r\n");
            mod_clear();
        }
    }
} 

// ---------------------------------------------------------------------------------------------------------------------
// MOD_FUNC_GET_IREG (NBYT = total bytes of data transmitted)
// MOD_FUNC_GET_HREG
//
//      byte:       0         1         2         3         4         5         6         7
//      received:   ADDR      FUNC      SREG-H    SREG-L    NREG-H    NREG-L    RCRC-L    RCRC-H
//      transmit:   ADDR      FUNC      NBYT      DATA-H    DATA-L    TCRC-L    TCRC-H    NULL
    
int mod_read(int inp_addr, int inp_func, int inp_sreg, int inp_nreg, unsigned char *rd_buf)
{
    int mod_error = 0;
    
    unsigned char tx_addr = inp_addr;
    unsigned char tx_func = inp_func;
    WORD2BYTE tx_sreg; tx_sreg.value = inp_sreg;
    if(inp_nreg < 1) inp_nreg = 1;
    WORD2BYTE tx_nreg; tx_nreg.value = inp_nreg;
    
// fill transmit buffer

    mod_clear();
    
    mod_txbuf[mod_txnum++] = tx_addr;
    mod_txbuf[mod_txnum++] = tx_func;
    mod_txbuf[mod_txnum++] = tx_sreg.byte1;
    mod_txbuf[mod_txnum++] = tx_sreg.byte0;
    mod_txbuf[mod_txnum++] = tx_nreg.byte1;
    mod_txbuf[mod_txnum++] = tx_nreg.byte0;

    WORD2BYTE tx_crc; tx_crc.value = mod_calc_crc(mod_txbuf, mod_txnum);
    mod_txbuf[mod_txnum++] = tx_crc.byte0;
    mod_txbuf[mod_txnum++] = tx_crc.byte1;

// transmit

    mod_de = 1;
    mod_re = 1;
    wait_ms(1);
    for(int i=0; i<mod_txnum; i++) mod.putc(mod_txbuf[i]);
    wait_ms(2);
    mod_de = 0;
    mod_re = 0;

    wait_ms(30);

// output transmit buffer

    if(mod_verbose)
    {
        printf("   mod_txbuf: %d\r\n\r\n", mod_txnum);
        for(int i=0; i<mod_txnum; i++)
        {
            printf("      %02d", i); print_uc(1,0,1, " = ", mod_txbuf[i], "\r\n");
        }
        if(mod_txnum)
        {
            printf("\r\n");
            print_uc(1,0,1, "      addr = ", tx_addr, "\r\n");
            print_uc(1,0,1, "      func = ", tx_func, "\r\n");
            print_us(1,0,1, "      sreg = ", tx_sreg.value, "\r\n");
            print_us(1,0,1, "      nreg = ", tx_nreg.value, "\r\n");
            print_us(1,0,1, "      tcrc = ", tx_crc.value, "\r\n");
        }
    }

// process received buffer

    if(mod_verbose)
    {
        printf("\r\n   mod_rxbuf: %d\r\n\r\n", mod_rxnum);
        for(int i=0; i<mod_rxnum; i++)
        {
            printf("      %02d", i); print_uc(1,0,1, " = ", mod_rxbuf[i], "\r\n");
        }
    }
    if(mod_rxnum)
    {
        int rxnum = 0;
        unsigned char rx_addr = mod_rxbuf[rxnum++];
        unsigned char rx_func = mod_rxbuf[rxnum++];

        if(mod_verbose)
        {
            print_uc(1,0,1, "\r\n      addr = ", rx_addr, "\r\n");
            print_uc(1,0,1,   "      func = ", rx_func, "");
        }
        if(rx_func == tx_func)
        {
            int rd_buf_index = 0;
            unsigned char rx_nbyt = mod_rxbuf[rxnum++];

            if(mod_verbose) print_uc(1,0,1, "\r\n      nbyt = ", rx_nbyt, "\r\n\r\n");

            for(int i=0; i<rx_nbyt/2; i++)
            {
                WORD2BYTE rx_data;
                rd_buf[rd_buf_index++] = rx_data.byte1 = mod_rxbuf[rxnum++];
                rd_buf[rd_buf_index++] = rx_data.byte0 = mod_rxbuf[rxnum++];
                
                int ireg = tx_sreg.value + i;

                if(ireg < MOD_IREG) mod_ireg[ireg] = rx_data.value;
                
                if(mod_verbose)
                {
                    printf("      data(%03d)", ireg);
                    print_us(1,0,1, " = ", rx_data.value, "\r\n");
                }
            }
            WORD2BYTE rx_crc;
            rx_crc.byte0 = mod_rxbuf[rxnum++];
            rx_crc.byte1 = mod_rxbuf[rxnum++];
            
            if(mod_verbose) print_us(1,0,1, "\r\n      rcrc = ", rx_crc.value, "\r\n");
        }
        else
        {
            unsigned char rx_nerr = mod_rxbuf[rxnum++]; mod_error = rx_nerr;

            if(mod_verbose)
            {
                if(rx_func == tx_func+0x80) printf("  (func %d error)\r\n", tx_func);
                else                        printf("  (unknown func error)\r\n");
                print_uc(1,0,1, "      nerr = ", rx_nerr, "  (");

                switch(rx_nerr)
                {
                    case MOD_ERROR_ILL_FUNC:     ; printf("slave received illegal function code")    ; break;
                    case MOD_ERROR_ILL_ADDR:     ; printf("slave received illegal register address") ; break;
                    case MOD_ERROR_ILL_DATA:     ; printf("slave received illegal register value")   ; break;
                    case MOD_ERROR_SLAVE_FAIL:   ; printf("slave device-specific failure")           ; break;
                    case MOD_ERROR_SLAVE_ACK:    ; printf("slave ACK")                               ; break;
                    case MOD_ERROR_SLAVE_BUSY:   ; printf("slave is busy")                           ; break;
                    case MOD_ERROR_SLAVE_NACK:   ; printf("slave NACK")                              ; break;
                    case MOD_ERROR_SLAVE_PARITY: ; printf("slave memory parity error")               ; break;
                    case MOD_ERROR_RX_TIMEOUT:   ; printf("master receive timeout")                  ; break;
                    case MOD_ERROR_TX_TIMEOUT:   ; printf("master transmit timeout")                 ; break;
                    case MOD_ERROR_CRC:          ; printf("master CRC error")                        ; break;
                    default:                     ; printf("unknown error")                           ; break;
                }
                printf(")\r\n");
            }

            WORD2BYTE rx_crc;
            rx_crc.byte0 = mod_rxbuf[rxnum++];
            rx_crc.byte1 = mod_rxbuf[rxnum++];
            
            if(mod_verbose) print_us(1,0,1, "      rcrc = ", rx_crc.value, "\r\n");
        }
    }
    else
    {
        mod_error = MOD_ERROR_RX_TIMEOUT; // no received data
    }

    if(mod_verbose) printf("\r\n   mod_error: %d\r\n", mod_error);

    return(mod_error);
}

// ---------------------------------------------------------------------------------------------------------------------
// MOD_FUNC_SET_HREG
//
//      byte:       0         1         2         3         4         5         6         7         8
//      received:   ADDR      FUNC      SREG-H    SREG-L    DATA-H    DATA-L    RCRC-L    RCRC-H
//      transmit:   ADDR      FUNC      SREG-H    SREG-L    DATA-H    DATA-L    TCRC-L    TCRC-H    NULL
//
// MOD_FUNC_SET_HREGS
//
//      byte:       0         1         2         3         4         5         6         7         8         9         10
//      received:   ADDR      FUNC      SREG-H    SREG-L    NREG-H    NREG-L    NBYT      DATA-H    DATA-L    RCRC-L    RCRC-H
//      transmit:   ADDR      FUNC      SREG-H    SREG-L    NREG-H    NREG-L    TCRC-L    TCRC-H    NULL
// 
int mod_write(int inp_addr, int inp_func, int inp_sreg, int inp_nreg, unsigned char *xmt_data)
{
    int mod_error = 0;

    unsigned char tx_addr = inp_addr;
    unsigned char tx_func = inp_func;
    WORD2BYTE tx_sreg; tx_sreg.value = inp_sreg;
    WORD2BYTE tx_nreg; tx_nreg.value = inp_nreg;
    WORD2BYTE data;
    unsigned char nbyt = 2*inp_nreg;
        
// fill transmit buffer

    mod_clear();
    
    mod_txbuf[mod_txnum++] = tx_addr;
    mod_txbuf[mod_txnum++] = tx_func;
    mod_txbuf[mod_txnum++] = tx_sreg.byte1;
    mod_txbuf[mod_txnum++] = tx_sreg.byte0;
    
    if(tx_func == MOD_FUNC_SET_HREG)
    {
//        data.value = xmt_data[0];
//        mod_txbuf[mod_txnum++] = data.byte1;
//        mod_txbuf[mod_txnum++] = data.byte0;
        mod_txbuf[mod_txnum++] = xmt_data[0];
        mod_txbuf[mod_txnum++] = xmt_data[1];
    }
    else
    {
        mod_txbuf[mod_txnum++] = tx_nreg.byte1;
        mod_txbuf[mod_txnum++] = tx_nreg.byte0;
        mod_txbuf[mod_txnum++] = nbyt;
        for(int i=0,i2=0; i<inp_nreg; i++)
        {
 //           data.value = xmt_data[i];
 //           mod_txbuf[mod_txnum++] = data.byte1;
 //           mod_txbuf[mod_txnum++] = data.byte0;
            mod_txbuf[mod_txnum++] = xmt_data[i2];
            mod_txbuf[mod_txnum++] = xmt_data[(i2+1)];
            i2=i2+2;
        }
    }
    
    WORD2BYTE tx_crc; tx_crc.value = mod_calc_crc(mod_txbuf, mod_txnum);
    mod_txbuf[mod_txnum++] = tx_crc.byte0;
    mod_txbuf[mod_txnum++] = tx_crc.byte1;

// transmit

    mod_de = 1;
    mod_re = 1;
    wait_ms(1);
    for(int i=0; i<mod_txnum; i++) mod.putc(mod_txbuf[i]);
    wait_ms(2);
    mod_de = 0;
    mod_re = 0;

    wait_ms(30);

// output transmit buffer

    if(mod_verbose)
    {
        printf("   mod_txbuf: %d\r\n\r\n", mod_txnum);
        for(int i=0; i<mod_txnum; i++)
        {
            printf("      %02d", i); print_uc(1,0,1, " = ", mod_txbuf[i], "\r\n");
        }

        if(mod_txnum)
        {
            print_uc(1,0,1, "\r\n      addr = ", tx_addr, "\r\n");
            print_uc(1,0,1,   "      func = ", tx_func, "\r\n");
            print_us(1,0,1,   "      sreg = ", tx_sreg.value, "\r\n");
        
            if(tx_func == MOD_FUNC_SET_HREG)
            {
                data.value = xmt_data[0];
                print_us(1,0,1, "      data = ", data.value, "\r\n");
            }
            else
            {
                print_us(1,0,1, "      nreg = ", tx_nreg.value, "\r\n");
                print_uc(1,0,1, "      nbyt = ", nbyt, "\r\n\r\n");

                for(int i=0; i<inp_nreg; i++)
                {
                    printf("      data(%03d)", tx_sreg.value+i);
                    data.value = xmt_data[i];
                    print_us(1,0,1, " = ", data.value, "\r\n");
                }
            }
            print_us(1,0,1, "      tcrc = ", tx_crc.value, "\r\n");
        }
    }

// process received buffer

    if(mod_verbose)
    {
        printf("\r\n   mod_rxbuf: %d\r\n\r\n", mod_rxnum);
        for(int i=0; i<mod_rxnum; i++)
        {
            printf("      %02d", i); print_uc(1,0,1, " = ", mod_rxbuf[i], "\r\n");
        }
    }
    if(mod_rxnum)
    {
        int rxnum = 0;
        unsigned char rx_addr = mod_rxbuf[rxnum++];
        unsigned char rx_func = mod_rxbuf[rxnum++];
        
        if(mod_verbose)
        {
            print_uc(1,0,1, "\r\n      addr = ", rx_addr, "\r\n");
            print_uc(1,0,1,   "      func = ", rx_func, "");
        }
        
        if(rx_func == tx_func)
        {
            WORD2BYTE rx_sreg;
            rx_sreg.byte1 = mod_rxbuf[rxnum++];
            rx_sreg.byte0 = mod_rxbuf[rxnum++];

            WORD2BYTE rx_nreg;
            rx_nreg.byte1 = mod_rxbuf[rxnum++];
            rx_nreg.byte0 = mod_rxbuf[rxnum++];

            WORD2BYTE rx_crc;
            rx_crc.byte0 = mod_rxbuf[rxnum++];
            rx_crc.byte1 = mod_rxbuf[rxnum++];

            if(mod_verbose)
            {
                print_us(1,0,1, "\r\n      sreg = ", rx_sreg.value, "\r\n");
                print_us(1,0,1,   "      nreg = ", rx_nreg.value, "\r\n");
                print_us(1,0,1,   "      rcrc = ", rx_crc.value, "\r\n");
            }
        }
        else
        {
            unsigned char rx_nerr = mod_rxbuf[rxnum++]; mod_error = rx_nerr;

            if(mod_verbose)
            {
                if(rx_func == tx_func+0x80) printf("  (func %d error)\r\n", tx_func);
                else                        printf("  (unknown func error)\r\n");

                print_uc(1,0,1, "      nerr = ", rx_nerr, "  (");

                switch(rx_nerr)
                {
                    case MOD_ERROR_ILL_FUNC:     ; printf("slave received illegal function code")    ; break;
                    case MOD_ERROR_ILL_ADDR:     ; printf("slave received illegal register address") ; break;
                    case MOD_ERROR_ILL_DATA:     ; printf("slave received illegal register value")   ; break;
                    case MOD_ERROR_SLAVE_FAIL:   ; printf("slave device-specific failure")           ; break;
                    case MOD_ERROR_SLAVE_ACK:    ; printf("slave ACK")                               ; break;
                    case MOD_ERROR_SLAVE_BUSY:   ; printf("slave is busy")                           ; break;
                    case MOD_ERROR_SLAVE_NACK:   ; printf("slave NACK")                              ; break;
                    case MOD_ERROR_SLAVE_PARITY: ; printf("slave memory parity error")               ; break;
                    case MOD_ERROR_RX_TIMEOUT:   ; printf("master receive timeout")                  ; break;
                    case MOD_ERROR_TX_TIMEOUT:   ; printf("master transmit timeout")                 ; break;
                    case MOD_ERROR_CRC:          ; printf("master CRC error")                        ; break;
                    default:                     ; printf("unknown error")                           ; break;
                }
                printf(")\r\n");
            }

            WORD2BYTE rx_crc;
            rx_crc.byte0 = mod_rxbuf[rxnum++];
            rx_crc.byte1 = mod_rxbuf[rxnum++];
            
            if(mod_verbose) print_us(1,0,1, "      rcrc = ", rx_crc.value, "\r\n");
        }
    }
    else
    {
        mod_error = MOD_ERROR_RX_TIMEOUT; // no received data
    }

    if(mod_verbose) printf("\r\n   mod_error: %d\r\n", mod_error);

    return(mod_error);
}

// ---------------------------------------------------------------------------------------------------------------------