; modbus_server.vnm
; example code for MODBUS/TCP system
;
; This program is a skeleton MODBUS/TCP server application, using
; the Micro-Robotics ethernet interface
;
; By adding application specific functions you can turn it into a
; full MODBUS/TCP server application.
;
; It has several distinct sections:
; (1) definitions for your system (IP addresses etc)
;     These must be changed to suit your network. 
;     There is also a debug flag you can set here which will give 
;     verbose runtime information on the serial port
;
; (2) MODBUS TCP interface driver
;     The mainstay of this code is the server function, one or more instances 
;     of which  which is run as a separate task.
;     You shouldn't need to change anything in this section, which handles
;     getting and sending messages at the TCP level
;
; (3) MODBUS Functions
;     These process messages and generate replies in the correct message format
;     They will work unchanged but you might want to add more error handling 
;     capability or more functions.
;
; (4) Application functions
;     You must rewrite these!
;     They are nominal I/O functions with dummy test code in them. They implement 
;     the mapping of MODBUS numbered inputs and outputs to application specific
;     values and controls.
;     There is also an application_background function which runs as the program's 
;     main task. You can put any code you like in here, that may or may not have 
;     to do with the MODBUS servers and will run asynchronously with MODBUS activity.
;
; (5) Test code.
;     This was written to test the rest of the code. In a real application you 
;     would remove it and the statement in application_background that
;     starts the tester.
;
; (c) 2007 Micro-Robotics Limited
; This code comes with no warranty and you use it at you own risk.
; You are free to copy and modify it however you like.
; Please report bugs or suggestions for improvement
; to techsupport@microrobotics.co.uk
;


; ***************** APPLICATION DEPENDENT SETTINGS **********
; these settings MUST be customised for your system
#DEFINE my_ip_address "172.16.1.150"

; the next three settings are only relevant if your network 
; is connected to the internet and you want to set the time and date
; from the internet or use the internet for any other purpose
#DEFINE use_internet TRUE         ; set TRUE or FALSE
#DEFINE my_gateway "172.16.1.199" ; if connected to the internet
#DEFINE my_dns "172.16.1.146"     ; Name server address if you have one

; number of simultaneous server processes. 2 - 8 is a good range. 
; Increase if your clients get "connection refused"
; Reduce if you get "RAM FULL" run time errors
#DEFINE number_of_server_tasks 2

; tcp timeout value in milliseconds (for receiving data)
#DEFINE tcp_timeout_ms 1000

#DEFINE debug_mode FALSE

;***************** MODBUS/TCP DRIVER CODE ********************
; This section contains code you shouldn't need to modify


; standard port number for MODBUS
#DEFINE MODBUS_PORT 502
#DEFINE x_illegal_function 1
#DEFINE x_illegal_address 2
#DEFINE x_illegal_datavalue 3
#DEFINE x_illegal_responselength 4

TO init
  init_modbus   ; init MODBUS driver
  init_application  ; put all your inits in this function
END

; set up ethernet
TO init_modbus
  ; set up ethernet
  MAKE eth Protocol("eth", 1, $d, my_ip_address)
  IF use_internet
  [
    eth.Address('N', my_dns)
    eth.Address('D', my_gateway)
  ]
END

; The main function. Edit 
TO main
  REPEAT number_of_server_tasks
    START server
  application_background    ; application function
END


; this does the MODBUS/TCP stuff
; you shouldn't need to change this
TO server
  AUTODESTRUCT
  LOCAL tcp := NEW Protocol("tcp")
  LOCAL id := 0         ; request ID
  LOCAL pid := 0        ; Protocol Identifier (always 0)
  LOCAL reqlength := 0  ; length of MODBUS function request
  LOCAL uid := 0        ; Unit ID
  LOCAL request := NEW Buffer(8)    ; request message
  LOCAL response := NEW Buffer(8) ; response message

  tcp.TimeOut := tcp_timeout_ms
  FOREVER
  [
    IF debug_mode
      PRINT "server listening", CR
    tcp.Open(MODBUS_PORT)
    WHILE tcp.Open = 0
      WAIT 1
    IF debug_mode
      PRINT "connection from ", tcp.Open:"IP", CR
    WHILE tcp.Open <> -1
    [
      request.Empty
      response.Empty
      ; get the modbus header.
      WHILE (tcp.Queue = 0)
        WAIT 1
      IF tcp.Queue < 0  ; disconnected
      [
        debugstr("remote end closed; no data")
        tcp.Close
        tcp.Reset
        BREAK
      ]
      ; collect header
      debugstr("collect header")
      id := get16(tcp)
      pid := get16(tcp)
      reqlength := get16(tcp)
      IF debug_mode
        PRINT "HEADER: id ", id:1, "  pid ", pid:1, 
              "  len ", reqlength:1, CR
      IF (reqlength > 255) OR (pid <> 0)
      [
        debugstr("invalid header, closing")
        tcp.Close
        tcp.Reset
        BREAK
      ]
      ; We should have the rest of the request in the TCP buffer
      IF tcp.Queue < reqlength
      [
        IF debug_mode
          PRINT "too short: len=", reqlength:1, "tcp.queue=", tcp.Queue:1, CR
        tcp.Close
        tcp.Reset
        BREAK
      ]
      uid := tcp.Get
      fcode := tcp.Get
      request.Put(fcode)
      REPEAT reqlength - 2
        request.Put(tcp.Get)
      IF debug_mode
      [
        PRINT "uid=", ~uid:1, "  fcode=", ~fcode:1, CR
        showmsg("request", request)
      ]
      SELECT CASE fcode
      CASE 2 read_input_discretes(request, response)
      CASE 3 read_multiple_regs(request, response)
      CASE 5 write_coil(request, response)
      CASE 16 write_multiple_regs(request, response)
      CASE ELSE
      [ ; create exception response
        response.Put(fcode + $80)
        response.Put(x_illegal_function)
      ]
      IF debug_mode
        showmsg("response",response)
      ; send the response message
      put16(tcp, id)
      put16(tcp, 0) ; PID
      put16(tcp, response.Length + 1)
      tcp.Put(uid)
      tcp.Put(response)
      tcp.Flush
    ]
    IF debug_mode
      PRINT "connection closed", CR
  ]
END



; get a 16 bit big-endian value from tcp or buffer
; Valid values 0 - 65535
; caller should check data is avaialable
TO get16(obj)
  AUTODESTRUCT
  LOCAL v := obj.Get
  RETURN v * 256 + obj.Get
END

; put a 16 bit value to TCP or buffer, big-endian
TO put16(obj, val)
  obj.Put(val DIV 256)
  obj.Put(val AND $ff)
END

; MODBUS Functions
; In each of these, the request buffer contains the request data 
; following the function code.
; The response buffer must be filled in starting with the 
; function code (may be normal or exception response)

; ***** NB you may want to add code to detect invalid values and send
; ***** an exception response

; MODBUS function 2
TO read_input_discretes(request, response)
  AUTODESTRUCT
  LOCAL fcode := request.Get
  LOCAL ref := get16(request)
  LOCAL bitcount := get16(request)
  LOCAL bytecount := (bitcount + 7) DIV 8
  LOCAL b := 0    ; byte temp holder for bits
  LOCAL mask := 1
  LOCAL bitnum := ref

  IF bytecount > 254  ; exception: result too long
  [
    reponse.Put($82)
    response.Put(x_illegal_responselength)
    RETURN 0
  ]
  response.Put(2) ; function code
  response.Put(bytecount)
  IF debug_mode
    PRINT "ref=", ref:1, "  bitcount=", bitcount:1, 
      " bytecount=", bytecount:1, CR

  WHILE bitnum < (ref + bitcount)
  [
    IF read_bit(bitnum)
      b := b OR mask
    mask := mask * 2
    IF mask = $100
    [
      response.Put(b)
      b := 0
      mask := 1
    ]
    bitnum := bitnum + 1
  ]
  IF mask > 1
    response.Put(b)
END ; read_input_discretes

; read 16 bit registers
TO read_multiple_regs(request, response)
  AUTODESTRUCT
  LOCAL fcode := request.Get
  LOCAL ref := get16(request)
  LOCAL wcount := get16(request)
  LOCAL bytecount := wcount * 2

  IF debug_mode
    PRINT "read_multiple_regs: ref=", ref:1, " wcount=", wcount:1, CR
  response.Put(fcode)
  response.Put(bytecount)
  REPEAT wcount
    put16(response, read_register(ref + INDEX0))
END

; MODBUS function code 5 (write coil)
TO write_coil(request, response)
  AUTODESTRUCT
  LOCAL fcode := request.Get
  LOCAL ref := get16(request)
  LOCAL coilstate := request.Get  ; 0 or $ff

  IF debug_mode
    PRINT "write_coil ref=", ref:1, "  state=", coilstate:1, CR
  set_relay(ref, coilstate)
  response.Put(fcode)
  put16(response, ref)
  response.Put(coilstate)
  response.Put(0)
END ; write_coil

; MODBUS function code 16
TO write_multiple_regs(request, response)
  AUTODESTRUCT
  LOCAL fcode := request.Get
  LOCAL ref := get16(request)
  LOCAL wcount := get16(request)
  LOCAL bcount := request.Get

  IF debug_mode
    PRINT "write_multiple_regs (", ref:1, ", ", wcount:1, ")", CR
  REPEAT wcount
    set_register(ref + INDEX0, get16(request))
  response.Put(fcode)
  put16(response, ref)
END

; ************** debug stuff **********

; show the contents of a MODBUS message
; msg is expected to be buffer(8) type
TO showmsg(id, msg)
  serial.Lock
  PRINT id, ": "
  REPEAT msg.Length
    PRINT ~msg.(INDEX0):1, " "
  PRINT CR
  serial.UnLock
END

; print string if in debug mode
TO debugstr(s)
  IF debug_mode
    PRINT "dbg:", s, CR
END

;***************** END OF MODBUS DRIVER CODE **********

;***************** APPLICATION CODE *******************

; Below here is code you need to modify or write to suit your 
; application


TO init_application
  ; code to create objects here

  ; global variables are initialised here

END

; set a 16 bit register.
; reg is the MODBUS reference for the register
; val is the 16 bit value to write to the register
TO set_register(reg, val)
  ; dummy test code
  PRINT "set_register ", reg:1, " = ", val:1, CR
END

; read a 16 bit register
; reg is the modbus reference for the register
; returns 16 bit value
; the registers correspond to ADC channels
; pulse counter/timer inputs and similar
TO read_register(reg)
  ; test code returns a dummy value = ref + 1
  RETURN reg + 1
END

; read a single bit value equivalent to MODBUS input discrete
; ref is the MODBUS reference number
; these will map to digital inputs
; returns zero or non-zero
TO read_bit(ref)
  ; dummy test code returns lsb of ref
  RETURN ref AND 1
END

TO set_relay(ref, state)
  ; code to set relay(ref) on if state non-zero, off if zero
  ; test code:
  PRINT "set relay ", ref:1
  IF state
    PRINT " on", CR
  ELSE
    PRINT " off", CR
END

; this does any background tasks that aren't driven directly by 
; MODBUS requests. The function should loop and never return.
TO application_background

  START tester  ; only need this if we're testing!
  
  ; currently a dummy loop that does nothing
  FOREVER
  [
    WAIT 100  ; replace with your code if needed
  ]
END

;**************************** TEST CODE ***********************
; not needed for real applications
; Uses loopback address to send test messages

TO tester
  AUTODESTRUCT
  LOCAL tcp := NEW Protocol("tcp")
  LOCAL msg := NEW Buffer(8)

  IF tcp.Open("localhost", modbus_port)
  [
    msg.Put(2)
    put16(msg, 0)
    put16(msg, 12)
    send_msg(msg, tcp)
    msg.Empty   ; 2nd test on same connection
    msg.Put(2)
    put16(msg, 2)
    put16(msg, 33)
    send_msg(msg, tcp)
    tcp.Close
    tcp.Reset
    ; more messages sent on new connection
    IF tcp.Open(my_ip_address, modbus_port)
    [
      msg.Empty
      msg.Put(2)
      put16(msg, 2)
      put16(msg, 33)
      send_msg(msg, tcp)
      msg.Empty
      msg.Put(3)  ; read multiple regs(0, 2)
      put16(msg, 0)
      put16(msg, 2)
      send_msg(msg, tcp)
      msg.Empty
      msg.Put(3)  ; read multiple regs(5, 6)
      put16(msg, 5)
      put16(msg, 6)
      send_msg(msg, tcp)
      msg.Empty
      msg.Put(5)  ; write coil 0 on
      put16(msg, 0)
      msg.Put($ff)
      send_msg(msg, tcp)
      msg.Empty
      msg.Put(5)  ; write coil 1 off
      put16(msg, 1)
      msg.Put(0)
      send_msg(msg, tcp)
      msg.Empty
      msg.Put(16) ; write multiple registers
      put16(msg, 2)
      put16(msg, 2) ; word count
      msg.Put(4)    ; byte count
      put16(msg, 1234)
      put16(msg, 5678)
      send_msg(msg, tcp)
    ]
  ]
  ELSE
    PRINT "tester: couldn't open tcp connection", CR
END

; test: send a request message and print response
TO send_msg(msg, tcp)
  AUTODESTRUCT
  LOCAL resp := NEW Buffer(8)
  LOCAL c := 0

  IF debug_mode
  [
    showmsg("tester tx", msg)
  ]
  put16(tcp, 0) ; id
  put16(tcp, 0) ; pid
  put16(tcp, msg.Length + 1)  ; length of following data
  tcp.Put(0)    ; id
  tcp.Put(msg)  ; rest of msg starting with fcode
  tcp.Flush
  tcp.TimeOut := 500
  c := tcp.Get  ; get 1st char or timeout
  IF c >= 0
  [
    resp.Put(c)
    WHILE tcp.Queue > 0
      resp.Put(tcp.Get)
    showmsg("tester rx ", resp)
  ]
END