Diff: mbtcp.cpp

Revision:

0:8ee5ec4bcec2

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbtcp.cpp	Fri Mar 16 08:30:23 2012 +0000
@@ -0,0 +1,285 @@
+/*
+ * 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);
+    }
+}