MODBUS protocol implementation

MBPROTO

#require UARTISR
#require CRC16
#require ]B!
#require WIPE

NVM
  : mbslv ( -- c )
  ;

  \ get MODBUS FC
  : mbfc ( -- c )

  ;

  \ 1st MODBUS FC parameter
  : mbp1 ( -- n )
    rxbuf 2+ @
  ;

  \ 2nd MODBUS FC parameter
  : mbp2 ( -- n )
    rxbuf 4 + @
  ;

  \ calc CRC16 from buffer a0 to a1
  : MBCRC ( a1 a0 -- crc-le )
    -1 ROT ROT ( -1 a1 a0 ) DO
      I C@ CRC16
    LOOP
    ( CRC16 ) EXG ( CRC-LE )
  ;

  \ flag MODBUS Exception and set Code
  : MBEC ( ec -- )
    [ 1 txbuf 1+ 7 ]B!
    ( ec ) txc+
  ;

  \ default FC handler - raise EC 1 "ILLEGAL FUNCTION"
  : FCNUL ( -- )
    1 MBEC
  ;

  \ FC-XT Table
  CREATE FCXT ' FCNUL
    DUP , DUP , DUP , DUP , DUP , DUP , DUP , DUP ,
    DUP , DUP , DUP , DUP , DUP , DUP , DUP ,     ,

  \ turn FC into XT
  : FC>XT ( fc -- xt )
    1- 2* FCXT +
  ;

  : MBPROTO ( -- )
    rxbuf rxp @ - ( rx )
    1 TIM tstamp @ - <   AND ( message trigger )
    IF
      rxp @ 2- ( a1 ) DUP rxbuf ( a1 a1 a0 )
      MBCRC ( a1 crc-le ) SWAP @ =
      ( crc-ok ) IF
        rxbuf    C@  ( DUP  ." S: " . CR )     txc+
        rxbuf 1+ C@  ( DUP  ." F: " . CR ) DUP txc+  ( fc )
        DUP 1 17 WITHIN IF
           FC>XT @ EXECUTE
        ELSE
          FCNUL
        THEN
        tbp @ txbuf ( a1 a0 ) MBCRC ( CRC-LE ) tx+
    \ ELSE
    \  ." CRC!" CR
      THEN
      start-tx rxres
    \ bufdump
    THEN
    ;

WIPE RAM