File content as of revision 1:59cee932f289:

/*
 * FreeModbus Libary: mbed Port
 * Copyright (C) 2006 Christian Walter <wolti@sil.at>
 *
 * This library is free software; you can redistribute it and/or
 * modify it under the terms of the GNU Lesser General Public
 * License as published by the Free Software Foundation; either
 * version 2.1 of the License, or (at your option) any later version.
 *
 * This library 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
 * Lesser General Public License for more details.
 *
 * You should have received a copy of the GNU Lesser General Public
 * License along with this library; if not, write to the Free Software
 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
 *
 * File: $Id: porttcp.c,v 1.1 2006/08/30 23:18:07 wolti Exp $
 *
 * modified by Yuji Hosogaya 3-16-2012
 *
 * Originally this file was porttcp.c, then combined with other files
 * in order to make a light version of Modbus TCP.
 */
#include "mbtcp.h"

Ethernet ethernet;
struct netif netif_data;

/* ----------------------- MBAP Header --------------------------------------*/
#define MB_TCP_UID          6
#define MB_TCP_LEN          4
#define MB_TCP_FUNC         7

/* ----------------------- Defines  -----------------------------------------*/
#define MB_TCP_DEFAULT_PORT 502 /* TCP listening port. */
#define MB_TCP_BUF_SIZE     ( 256 + 7 ) /* Must hold a complete Modbus TCP frame. */

/* ----------------------- Static variables ---------------------------------*/
static UCHAR aucTCPBuf[MB_TCP_BUF_SIZE];
static USHORT usTCPBufPos;

/* ----------------------- Begin implementation -----------------------------*/
void EventPost(struct tcp_pcb *pcb)
{
    USHORT usAddress,usNRegs;
    UCHAR txbuf[MB_TCP_BUF_SIZE];
    int len=0;
    UCHAR ucTemp;
    MB_STRUCT *rxmbs=(MB_STRUCT *)aucTCPBuf;
    MB_STRUCT *txmbs=(MB_STRUCT *)txbuf;
    MB_WORD_REQ *wd_req=(MB_WORD_REQ *)GET_DATA_PTR(rxmbs);
    MB_WORD_RES *wd_res=(MB_WORD_RES *)GET_DATA_PTR(txmbs);

    usAddress=TO_USHORT(wd_req->ADDR_H,wd_req->ADDR_L)+1;
    usNRegs=TO_USHORT(wd_req->LEN_H,wd_req->LEN_L);

    printf("TID=%04X | PID=%04X | LEN=%04X | UID=%02X | FUNC=%02X\nDATA=",
        GET_TID(rxmbs), GET_PID(rxmbs), GET_LEN(rxmbs), GET_UID(rxmbs), GET_FUNC(rxmbs) );
    for(int i=0;i<usTCPBufPos-8;i++){
        printf("%02X",aucTCPBuf[i+8]);
    }
    printf("\n");
    
    switch(GET_FUNC(rxmbs))
    {
    case FC_RD_COILS:
        memcpy((char*)txmbs,(char*)rxmbs,8);
        wd_res->BYTES=usNRegs*2;
        len=usNRegs/8;
        if(usNRegs%8!=0)len++;
        SET_LEN(txmbs,len+3);
        wd_res->BYTES=(UCHAR)len;
        
        eMBRegCoilsCB( (UCHAR *) &wd_res->DATA, usAddress, usNRegs, MB_REG_READ);
        len+=9;
        break;

    case FC_RD_DISC_INPUTS:
        memcpy((char*)txmbs,(char*)rxmbs,8);
        wd_res->BYTES=usNRegs*2;
        len=usNRegs/8;
        if(usNRegs%8!=0)len++;
        SET_LEN(txmbs,len+3);
        wd_res->BYTES=(UCHAR)len;

        eMBRegDiscreteCB( (UCHAR *) &wd_res->DATA, usAddress, usNRegs );
        len+=9;
        break;

    case FC_RD_HOLDING_REGS:
        memcpy((char*)txmbs,(char*)rxmbs,8);
        wd_res->BYTES=usNRegs*2;
        len=usNRegs*2;
        SET_LEN(txmbs,len+3);

        eMBRegHoldingCB( (UCHAR *)&wd_res->DATA, usAddress, usNRegs, MB_REG_READ);
        len+=9;
        break;

    case FC_RD_INPUT_REGS:
        memcpy((char*)txmbs,(char*)rxmbs,8);
        wd_res->BYTES=usNRegs*2;
        len=usNRegs*2;
        SET_LEN(txmbs,len+3);

        eMBRegInputCB( (UCHAR *)&wd_res->DATA, usAddress, usNRegs );
        len+=9;
        break;

    case FC_WR_SINGLE_COIL:
        ucTemp=(wd_req->LEN_H==0x00)?0:1;
        len=12;
        memcpy((char*)txmbs,(char*)rxmbs,len);
        eMBRegCoilsCB( &ucTemp, usAddress, 1, MB_REG_WRITE);
        break;

    case FC_WR_SINGLE_REG:
        len=12;
        memcpy((char*)txmbs,(char*)rxmbs,12);
        eMBRegHoldingCB( &wd_req->LEN_H, usAddress, 1, MB_REG_WRITE );
        break;

    case FC_WR_MULTI_COILS:
        len=12;
        memcpy((char*)txmbs,(char*)rxmbs,len);
        SET_LEN(txmbs,6);
        
        eMBRegCoilsCB( &wd_req->DATA[1], usAddress, usNRegs, MB_REG_WRITE);
        break;

    case FC_WR_MULTI_REGS:
        len=12;
        memcpy((char*)txmbs,(char*)rxmbs,len);
        SET_LEN(txmbs,6);

        eMBRegHoldingCB( &wd_req->DATA[1], usAddress, usNRegs, MB_REG_WRITE);
        break;
    case FC_RD_WR_MULTI_REGS:
        memcpy((char*)txmbs,(char*)rxmbs,8);

        // Write operation
        eMBRegHoldingCB( &wd_req->DATA[5], usAddress, usNRegs, MB_REG_WRITE);

        // Read operation
        usAddress=TO_USHORT(wd_req->DATA[0],wd_req->DATA[1])+1;
        usNRegs=TO_USHORT(wd_req->DATA[2],wd_req->DATA[3]);
        eMBRegHoldingCB( &wd_res->DATA, usAddress, usNRegs, MB_REG_READ);

        len=usNRegs*2;
        wd_res->BYTES=len;
        SET_LEN(txmbs,len+3);

        len+=9;
        break;

    default:
        printf("UNSUPPORTED FUNCTION CODE\n");
        break;
    }
    if(len>0){
        for(int i=0;i<len;i++){
            printf("%02X ",txbuf[i]);
        }
        printf("\n\n");
        if (tcp_write(pcb, (void *)txbuf, len, 1) == ERR_OK) {
            tcp_output(pcb);
        }else{
            printf("Failed to send a response!!\n");
        }
    }
}


err_t recv_callback(void *arg, struct tcp_pcb *pcb, struct pbuf *p, err_t err)
{
    struct netif   *netif = &netif_data;
    unsigned short usLength;
    int i;

    /* Check if status is ok and data is arrived. */
    if (err == ERR_OK && p != NULL) {
        /* Inform TCP that we have taken the data. */
        tcp_recved(pcb, p->tot_len);

        memcpy( &aucTCPBuf[usTCPBufPos], p->payload, p->tot_len );
        usTCPBufPos += p->tot_len;

        if( usTCPBufPos >= MB_TCP_FUNC )
        {
            /* Length is a byte count of Modbus PDU (function code + data) and the
             * unit identifier. */
            usLength = aucTCPBuf[MB_TCP_LEN] << 8U;
            usLength |= aucTCPBuf[MB_TCP_LEN + 1];

            /* Is the frame already complete. */
            if( usTCPBufPos < ( MB_TCP_UID + usLength ) )
            {
            }
            else if( usTCPBufPos == ( MB_TCP_UID + usLength ) )
            {
                for(i=0;i<usTCPBufPos;i++){
                    printf("%02X ",aucTCPBuf[i]);
                }
                printf("\n");
                EventPost(pcb);
                usTCPBufPos=0;
            }
            else
            {
                printf("Received to many bytes! Droping client.\n" );
                /* This should not happen. We can't deal with such a client and
                 * drop the connection for security reasons.
                 */
                //vvMBPortReleaseClient( pxPCB );
            }
        }
        pbuf_free(p);
    }else{
            /* No data arrived */
            /* That means the client closes the connection and sent us a packet with FIN flag set to 1. */
            /* We have to cleanup and destroy out TCPConnection. */
            printf("Connection closed by client.\r\n");
            pbuf_free(p);
        }

    return ERR_OK;
}
/* Accept an incomming call on the registered port */
err_t accept_callback(void *arg, struct tcp_pcb *npcb, err_t err) {
    LWIP_UNUSED_ARG(arg);
    /* Subscribe a receive callback function */
    tcp_recv(npcb, &recv_callback);
    /* Don't panic! Everything is fine. */
    printf("accepted connection\n");
    usTCPBufPos=0;
    return ERR_OK;
}

void mb_init()
{
    struct netif   *netif = &netif_data;
    struct ip_addr  ipaddr;
    struct ip_addr  netmask;
    struct ip_addr  gateway;
    Ticker tickFast, tickSlow, tickARP, eth_tick, dns_tick, dhcp_coarse, dhcp_fine;
    
    char *hostname = "my-mbed";
    printf("Setting up...\n");

    /* Start Network with DHCP */
    IP4_ADDR(&netmask, 255,255,255,255);
    IP4_ADDR(&gateway, 0,0,0,0);
    IP4_ADDR(&ipaddr, 0,0,0,0);
    /* Initialise after configuration */
    lwip_init();
    netif->hwaddr_len = ETHARP_HWADDR_LEN;
    device_address((char *)netif->hwaddr);
    netif = netif_add(netif, &ipaddr, &netmask, &gateway, NULL, device_init, ip_input);
    netif->hostname = hostname;
    netif_set_default(netif);
    dhcp_start(netif); // <-- Use DHCP
    
        /* Initialise all needed timers */
    tickARP.attach_us( &etharp_tmr,  ARP_TMR_INTERVAL  * 1000);
    tickFast.attach_us(&tcp_fasttmr, TCP_FAST_INTERVAL * 1000);
    tickSlow.attach_us(&tcp_slowtmr, TCP_SLOW_INTERVAL * 1000);
    dns_tick.attach_us(&dns_tmr, DNS_TMR_INTERVAL * 1000);
    dhcp_coarse.attach_us(&dhcp_coarse_tmr, DHCP_COARSE_TIMER_MSECS * 1000);
    dhcp_fine.attach_us(&dhcp_fine_tmr, DHCP_FINE_TIMER_MSECS * 1000);

    while (!netif_is_up(netif)) { 
         device_poll(); 
    } 
    printf("Setup OK\n");
    printf("mbed IP Address is: %d.%d.%d.%d\n", (netif->ip_addr.addr)&0xFF, (netif->ip_addr.addr>>8)&0xFF, (netif->ip_addr.addr>>16)&0xFF, (netif->ip_addr.addr>>24)&0xFF);

    /* Bind a function to a tcp port */
    struct tcp_pcb *pcb = tcp_new();
    if (tcp_bind(pcb, IP_ADDR_ANY, 502) == ERR_OK) {
        pcb = tcp_listen(pcb);
        tcp_accept(pcb, &accept_callback);
    }
}