Wifi-CAN.py

# Wifi - CAN bus hub, using PIO for SPI interactions to speed up CAN read/write
#    Uses either TCP or UDP over Wifi.

# Copyright (c) 2025-2026 George Helffrich
# Released under the MIT License (MIT) - see LICENSE file

# CPU: Raspberry Pi pico W
# CAN device: RB-P-CAN-RS485 board by Joy-IT (www.joy-it.net)
#          or Pico-CAN-B board by Waveshare (https://www.waveshare.com)

# Runs with:
# MicroPython v1.24.1 on 2024-11-29; Raspberry Pi Pico W with RP2040

# 16 Jan. 2025
# last revision 13 Apr 2026

_VER = const('PR136')            # version ID

################################## Configuration variables start here...

SSID = "****"                    # Wifi network ID
PASS = "****"                    # Wifi network password

_CANBOARD = const('auto')        # Board choice: 'auto' for auto-detect or
                                 # 'JI' or 'WS'
_IPP = const('TCP')              # Protocol choice: TCP or UDP

################################## Configuration variables ...end here

CS2_PORT = const(15731)          # Marklin diktat: TCP
CS2_RPORT = const(15731)         # Marklin diktat: UDP
CS2_SPORT = const(15730)         # Marklin diktat: UDP
CS2_SIZE = const(13)             # Fixed by protocol definition

_DELAY = const(500)              # us delay between sent Wifi packets

QSIZE = const(400)               # Size of various I/O queues (modest)

NODE_ID = const(1)               # "S88" node ID

_INSTR_READ_STATUS = const(0xA0) # PIO for fast reading of 250 kbps CAN bus
_INSTR_WRITE       = const(0x02)
_INSTR_READ        = const(0x03)
_INSTR_RXBUF0      = const(0x90)
_INSTR_RXBUF1      = const(0x94)
_INSTR_BIT_MOD     = const(0x05)

_MCP_CANINTF       = const(0x2C)
_MCP_EFLG          = const(0x2D)
_MCP_TXB0          = const(0x30)
_MCP_TXB1          = const(0x40)
_MCP_TXB2          = const(0x50)

import uasyncio as asyncio
import network
import usocket as socket
import sys, errno, utime
from time import sleep
from asyncio import Loop
from machine import Pin, Timer
from rp2 import PIO, StateMachine
from array import array

from micropython import alloc_emergency_exception_buf as AEEB
AEEB(100)                        # boilerplate for IRQ-level exception reporting

@rp2.asm_pio(
   push_thresh=32, autopush=True, pull_thresh=32, autopull=True,
   sideset_init=(rp2.PIO.OUT_HIGH, rp2.PIO.OUT_LOW),   #CS, SCK
   out_init=rp2.PIO.OUT_LOW
)
def SPI_OP():
    wrap_target()
    pull()                   .side(0x1)         #CS=1, SCK=0
    out(y, 8)                .side(0x1)         #CS=1, SCK=0; y=byte count - 1
    label("byteloop")
    set(x, 7)
    label("bloop1")
    out(pins, 1)             .side(0x0)   [1]   #CS=0, SCK=0
    in_(pins, 1)             .side(0x2)         #CS=0, SCK=1
    jmp(x_dec, "bloop1")     .side(0x2)         #CS=0, SCK=1
    jmp(y_dec, "byteloop")

    push(noblock)            .side(0x1)   [1]   #CS=1, SCK=0; last unaligned
    wrap()

pico_led = Pin("LED", Pin.OUT)

class iCAN:                      # interrupt driven CAN message sniffer
   from collections import namedtuple

   CQSIZE = const(QSIZE)         # Big to read CONFIG DATA bursts

   CAN_pins = namedtuple('CAN pins',
      ['INT_PIN','SPI_CS', 'SPI_SCK', 'SPI_MOSI', 'SPI_MISO', 'FBP', 'name']
   )
   boards = dict(
      JI = CAN_pins(
         name = 'Joy-IT',           # These pin assignments are appropriate for
                                    # a RB-P-CAN-485 Joy-IT board
         INT_PIN = 22,              # Interrupt pin for CAN board
         SPI_CS = 17,
         SPI_SCK = 18,
         SPI_MOSI = 19,
         SPI_MISO = 16,
         FBP = bytes((              # Feedback pins
              #0  1  2  3   4   5   6   7  channel number
               0, 1, 8, 9, 10, 11, 14, 15 #GPIO pin number
         ))
      ),
      WS = CAN_pins(
         name = 'Waveshare',        # These pin assignments are appropriate for
                                    # a Waveshare Pico-CAN-B board
         INT_PIN = 21,              # Interrupt pin for CAN board
         SPI_CS = 5,
         SPI_SCK = 6,
         SPI_MOSI = 7,
         SPI_MISO = 4,
         FBP = bytes((              # Feedback pins
              #0  1  2  3   4   5   6   7  channel number
               0, 1, 2, 3, 10, 11, 12, 13 #GPIO pin number
         ))
      )
   )

   def __init__(self, conf=None):
      from machine import Pin, SPI
      from canbus import Can, CanError
      from canbus.internal import CAN_SPEED

      try:
         CanError.decode()       # Needed for CAN bus error reports
      except:
         raise RuntimeError("Update canbus module - new features used")

      if conf is None:
         raise RuntimeError("Need to provide CAN board type or 'auto'")
      if conf == 'auto':
         for bd in iCAN.boards:
            # Check if the initialization is successful
            pin = iCAN.boards[bd]
            self.spi = SPI(0,    # configure SPI to use this board's pins
                sck=Pin(pin.SPI_SCK),
                mosi=Pin(pin.SPI_MOSI),
                miso=Pin(pin.SPI_MISO)
            )
            # Create an instance of the Can class to interface with the CAN bus
            self.can = Can(spics=pin.SPI_CS)
            if self.can.begin() == CanError.ERROR_OK:
               break
         else:
            raise RuntimeError(
               "***Can't auto-detect board; set explicit board name***"
            )
      elif conf in iCAN.boards:   # Explicit board choice?
         pin = iCAN.boards[conf]
         self.spi = SPI(0,        # configure SPI to use this board's pins
             sck=Pin(pin.SPI_SCK),
             mosi=Pin(pin.SPI_MOSI),
             miso=Pin(pin.SPI_MISO)
         )
         # Create an instance of the Can class to interface with the CAN bus
         self.can = Can(spics=pin.SPI_CS)
         if self.can.begin() != CanError.ERROR_OK:
            raise RuntimeError("Error initializing %s CAN board - check type!" %
               pin.name
            )
      else:
         raise RuntimeError('***%s is an unsupported CAN board***' % conf)

      self._pins = pin
      self.pin = Pin(pin.INT_PIN, Pin.IN, Pin.PULL_UP)
      self.can = Can(spics=pin.SPI_CS) # CS pin for hardware SPI 0
      # Initialize the CAN interface.  Reference says 250 kbps speed.
      ret = self.can.begin(bitrate=CAN_SPEED.CAN_250KBPS)
      if ret != CanError.ERROR_OK:
         raise RuntimeError('Error initializing CAN bus')

      self.SPI_CS = Pin(pin.SPI_CS, Pin.OUT)

      self.sm = rp2.StateMachine(0)
      self.sm.init(SPI_OP,
            freq=32_000_000,     # WARNING: 32 MHz max rate before bit loss 
            in_base=pin.SPI_MISO,
            out_base=pin.SPI_MOSI,
            sideset_base=pin.SPI_CS,
            in_shiftdir=PIO.SHIFT_LEFT,
            out_shiftdir=PIO.SHIFT_LEFT
      )
      self.sm.active(1)

      self.cque = ThreadSafeQueue(iCAN.CQSIZE)
      self.melem = iCAN.CQSIZE*[None]
      for n in range(iCAN.CQSIZE):
         self.melem[n] = array('I',4*[0])
      self.eelem = bytearray(iCAN.CQSIZE)
      self.nque = 0
      self.sflag = 0
      self.icnt = 0
      self.buf = bytearray(13)
      self.a_4 = array('I',4*[0])
      self.a_2 = array('I',2*[0])
      self.b_4 = bytearray(4)
      self.b_2 = bytearray(2)
      self.b_1 = bytearray(1)
      _intr_ref = self._intr
      self._intr_ref = _intr_ref
      self.pin.irq(
         trigger=Pin.IRQ_FALLING,
         handler=self._intr_ref,
      #  handler=lambda pin: schedule(_intr_ref, pin), # 200 us delay - too long
         hard=True)

# Notes on how this IRQ routine works.

# For speed reasons, the PIO machine does the SPI interactions with the MCP2515.
# Calling SPI functions directly from the code is too slow for a CAN bus
# running at 250 kbps.  Decoders talking over the rails speak slowly, and code
# can keep up with the resuting CAN packet traffic.  However, talk between
# multiple MS2s on a CAN bus, or a CS2 and an MS2 connected by a CAN bus runs
# at full speed, and micropython code cannot keep up with the blistering output
# rate.

# Timing of the basic interrupt - micropython SPI read takes about 2-3 ms.  At
# 250 kbps, packets on a CAN bus running at full speed arrive every 256 - 512 us
# depending on packet size.  Hence to keep up, drastic optimization is needed.

# Consequently, this routine runs at the IRQ level.  It can be run by the
# micropython schedule() command to avoid heap corruption; this adds a delay of
# ~200 us per IRQ.  It mightly tries to avoid using the heap, so many local
# vars and constants are allocated as static vars at the time the function is
# instantiated (in init() of the iCAN object).
# Bravely running from an IRQ directly does not seem to lead to any problems.
# Timing shows one IRQ takes ~527 us to process to quiescence.

# For speed optimization, PIO to the rescue.  A PIO state machine can run an
# SPI device fairly efficiently.  With a clock speed of 32 MHz and using 4
# cycles/bit, the PIO can read at 8 Mbps, which is 32x faster than the CAN
# bus bit rate.  However, the micropython code has to be highly optimized to
# respond quickly enough.  If there is 200 us latency to reach a safe place for
# an IRQ to run, that leaves only 60 - 300 us for the IRQ to issue commands to
# read the CAN bus.  Consequently, schedule() is not used.

# The PIO machine takes a 32 bit word as input on its input FIFO, and outputs a
# series of one or more 32 bit words (using PIO autopush, and an explicit push
# at the end) to its output FIFO.  Due to the way the MCP2515 SPI works, the PIO
# reads as it writes: even for writes, data is pushed onto the output FIFO which
# has to be cleared for the next use.  Each command is encoded with a byte
# count (-1 for how PIO looping works), a command, an (optional) address, and
# (optional) data (for writes).  Each byte written to the SPI bus has a
# corresponding read.  Each byte read is shifted into the 32 bit input register
# from the right, so 1 byte will be in the lowest 8 bits, 2 bytes in the lowest
# 16 bits, etc.  For most commands, this yields exactly one output fifo word.
# For the BIT_MODIFY and READ_RXn commands, multiple words are the result.
# The .get() method expects a buffer argument, which VERY CONVENIENTLY means
# that you get all the FIFO's words in one call - no looping needed.  Your
# buffer has to be big enough, however.  If the number of bytes read is NOT
# a multiple of 4, the last 32 bit word read will have leading zeroes in it
# (incompletely shifted).  The gap in the bytes is handled specially
# when processing the READ_RXn command output, because 13 data bytes are read.
# Adding in the 2 bytes for the MCP2515 command and address, this means there
# will be a 1 byte gap in the high bits of the last 32 bit word read.

# The PIO state machine's .get() truncates the 32 bit FIFO word to the type of
# the output buffer.  Hence, a bytearray will receive the low order byte in each
# 32 bit word, and a full 32 bit integer array will receive the entire word.

# If we are unlucky, multiple IRQ interrupts will happen as each MCP2515 of the
# two receive buffers fills up.  There is a lock-out flag to prevent the IRQ
# from being called again while inside itself.  The IRQ code checks for new data
# once it gets an interrupt and keeps reading until it runs to quiescence.
 
# Empirical observations of how the MCP2515 works:

# - You can't read the CANINTF and subsequent registers as a group; only
#   CANINTF singly (would be nice to read CANINTF and EFLG as a pair - oh well).

# - When you do a read of RX0 or RX1, the corresponding CANINTF status bit seems
#   to be automatically cleared, readying a detection of a new CAN packet. A
#   useful (undocumented) feature.

# - READ_RXBn is FAST: timing says ~40 us to read 16 bytes!  By comparison,
#   READ_REGISTER / READ_STATUS timing is ~42 us, yet this reads 1 byte.

   @micropython.native
   def _intr(self, pin,
      READ_STATUS_CMD=(    # PIO FIFO 1 word
         (2-1)              << 24 |
         _INSTR_READ_STATUS << 16 |
         0
      ),
      READ_CANINTF_CMD=(   # PIO FIFO 1 word
         (3-1)              << 24 |
         _INSTR_READ        << 16 | 
         _MCP_CANINTF       <<  8 | 
         0
      ),
      READ_EFLG_CMD=(      # PIO FIFO 1 word
         (3-1)              << 24 |
         _INSTR_READ        << 16 | 
         _MCP_EFLG          <<  8 | 
         0
      ),
      READ_RX0_CMD=(       # PIO FIFO 4 words
         (15-1)             << 24 | # read more for PIO autopush of last byte
         _INSTR_RXBUF0      << 16 |
         0
      ),
      READ_RX1_CMD=(       # PIO FIFO 4 words
         (15-1)             << 24 | # read more for PIO autopush of last byte
         _INSTR_RXBUF1      << 16 |
         0
      ),
      BIT_MOD_CMD=(        # PIO FIFO 2 words; Data byte in 2nd word high byte
         (4-1)              << 24 |
         _INSTR_BIT_MOD     << 16 |
         _MCP_CANINTF       <<  8 |
         0                 # bit clear mask to be ORed in
      ),
      qi=0,
      stat=0
   ):

      self.icnt += 1              # Count interrupts
      if self.sflag: return       # Quick return if IRQ is busy
      self.sflag = 1              # Raise IRQ busy semaphore

      a_4 = self.a_4              # Set up local vars (optimize using locals)
      a_2 = self.a_2
      b_2 = self.b_2
      b_1 = self.b_1
      sm = self.sm
      cque = self.cque
      eelem = self.eelem
      melem = self.melem
         
      sm.put(                     # GET INTERRUPT FLAGS
         READ_CANINTF_CMD
      )
      sm.get(b_1)
      stat = b_1[0]
      if stat & 0x20:             # Error of any sort?
         sm.put(                  # GET ERROR FLAGS
            READ_EFLG_CMD
         )
         sm.get(b_1)
         qi = self.nque % iCAN.CQSIZE
         eelem[qi] = b_1[0]
         cque.put_sync(qi)        # Queue error indication, interpret later
         self.nque += 1
         a_2[0] = BIT_MOD_CMD | 0x20 # CLEAR ERROR FLAG
         a_2[1] = 0
         sm.put(a_2)
         sm.get(b_2)              # this preserves any new read indications

      while stat & 0x03:          # cycle while something to read?

         if stat & 0x01:          # ... in RX0
            qi = self.nque % iCAN.CQSIZE
            a_4[0] = READ_RX0_CMD
            sm.put(a_4)           # READ RX0
            sm.get(melem[qi])
            eelem[qi] = 0x00 # No error
            cque.put_sync(qi)
            self.nque += 1

         if stat & 0x02:          # ... in RX1
            qi = self.nque % iCAN.CQSIZE
            a_4[0] = READ_RX1_CMD
            sm.put(a_4)           # READ RX1
            sm.get(melem[qi])
            eelem[qi] = 0x00 # No error
            cque.put_sync(qi)
            self.nque += 1

         self.sm.put(             # READ INTERRUPT FLAGS (again)
            READ_CANINTF_CMD
         )
         sm.get(b_1)
         stat = b_1[0]
         if stat & 0x20:          # Error of any sort?
            sm.put(               # READ ERROR FLAGS
               READ_EFLG_CMD
            )
            sm.get(b_1)
            qi = self.nque % iCAN.CQSIZE
            eelem[qi] = b_1[0]
            cque.put_sync(qi)
            self.nque += 1
            a_2[0] = BIT_MOD_CMD | 0x20 # CLEAR ERROR FLAG;
            a_2[1] = 0            # others already cleared
            sm.put(a_2)           # by READ RX0 / READ RX1; 
            sm.get(b_2)           # this preserves any new read indications

      self.sflag = 0              # Lower IRQ busy semaphore

   def __aiter__(self):           # enable await for pkt in .... feature
      return self

   async def __anext__(self):     # return next pkt in for pkt in ....
      i = await self.cque.get()
      return (self.melem[i], self.eelem[i])

   @property
   def pins(self):               # allow e.g. can.pins.FBP, can.pins.name
      return self._pins

   @micropython.native
   def send(self, ID=None, data=None, EFF=False, RTR=False,
      READ_EFLG_CMD=(      # PIO FIFO 1 word
         (3-1)              << 24 |
         _INSTR_READ        << 16 | 
         _MCP_EFLG          <<  8 | 
         0
      )
   ):

      dlc = 0 if data is None else len(data)
      buf = self.buf
      if EFF:
         buf[0] = (ID >> 21) & 0xFF
         buf[1] = (
            ((ID & 0x1C0000) >> 13) |
            ((ID & 0x030000) >> 16) |
            0x08
         )
         buf[2] = (ID >> 8) & 0xFF
         buf[3] = ID        & 0xFF
      else:
         buf[0] = (ID >> 3) & 0xFF
         buf[1] = (ID & 0x07) << 5
      buf[4] = (
         (dlc & 0x0F) |
         (0x40 if RTR else 0x00)
      )
      if dlc > 0: buf[5:5+dlc] = data[0:dlc]

      icnt = self.icnt            # save interrupt count
      self.sflag = 1              # raise semaphore to block reads

      sm = self.sm
      b_1 = self.b_1
      for TXB in [_MCP_TXB0, _MCP_TXB1, _MCP_TXB2]:
         READ_TXBF_CMD=(      # PIO FIFO 1 word
            (3-1)              << 24 |
            _INSTR_READ        << 16 | 
            TXB                <<  8 | 
            0
         )
         sm.put(READ_TXBF_CMD)
         sm.get(b_1)
         if not(b_1[0] & 0x08):   # clear buf?
            break
      else:
         sm.put(                  # READ ERROR FLAGS - likely to be one
            READ_EFLG_CMD
         )
         sm.get(b_1)
         stat = b_1[0]
         self.sflag = 0           # lower semaphore to unblock reads
         if self.icnt != icnt:    # check for any read interrupts
            self._intr_ref(self.pin)
         return stat              # return some kind of error

      a_4 = self.a_4
      b_4 = self.b_4
      WRITE_TXBF_CMD=(        # PIO FIFO 4 words
         (2+5+8-1)             << 24 |
         _INSTR_WRITE          << 16 | 
         (TXB + 1)             <<  8 | 
         0
      )
      a_4[0] = WRITE_TXBF_CMD | buf[0]
      a_4[1] = int.from_bytes(buf[1:5])
      a_4[2] = int.from_bytes(buf[5:9])
      a_4[3] = int.from_bytes(buf[9:13])
      sm.put(a_4)
      sm.get(b_4)

      BIT_MOD_CMD=(        # PIO FIFO 2 words; bit clear value in 2nd word
         (4-1)              << 24 |
         _INSTR_BIT_MOD     << 16 |
         TXB                <<  8 |
         0                 # bit clear mask to be ORed in
      )

      a_2 = self.a_2
      b_2 = self.b_2
      a_2[0] = BIT_MOD_CMD | 0x0B
      a_2[1] = (0x08 | ((ID >> 29) & 0x03)) << 24
      sm.put(a_2)                 # mark buffer ready for transmit
      sm.get(b_2)

      sm.put(READ_TXBF_CMD)
      sm.get(b_1)
      stat = (b_1[0] & 0x70) << 8 # TXB_ABTF | TXB_MLOA | TXB_TXERR

      self.sflag = 0              # lower semaphore after read
      if self.icnt != icnt:
         self._intr_ref(self.pin) # check for missed read interrupts

      return stat

   def stop(self):
      # Turns off CAN interrupt handling, disabling board for reading.
      self.pin.irq(handler=None)
      self.sm.active(0)

from threadsafe import ThreadSafeQueue

CtoT, TtoC, DBQ = [], [], []
for i in range(QSIZE):
   CtoT.append(bytearray(CS2_SIZE))
   TtoC.append(bytearray(CS2_SIZE))
   DBQ.append(bytearray(CS2_SIZE))
CANtoTCP, ixCT = ThreadSafeQueue(QSIZE), 0
TCPtoCAN, ixTC = ThreadSafeQueue(QSIZE), 0
debugQUE, ixDB = ThreadSafeQueue(QSIZE), 0

TCP_R, TCP_W = None, None

rrhash = 0

qfTC, qfCT, qfDB = False, False, False
CANmsg = const('''
              ***********************
              ***CAN output q full***
              ***********************
''')
TCPmsg = const('''
              ************************
              ***WiFi output q full***
              ************************
''')
DBGmsg = const('''
                 ****************
                 ***log q full***
                 ****************
''')

def qput(pkt,queue,flag,msg):
   # Code readability device to avoid many try/except indents
   # Does synchronous put to a threadsafe queue and prints error message
   # depending on whether a previous error already reported.
   # Use flag arg idiomatically: FLAG = qput(pkt,queue,FLAG,msg)

   if queue.full():
      if not flag: print(msg)
      return True

   queue.put_sync(pkt)
   return False

TCP_DONE = asyncio.ThreadSafeFlag()
async def TCP_SERVER(R, W):
   # Callback when RocRail client connects to us
   global NOCAN
   global TCP_R, TCP_W

   print('TCP connection made, waiting for traffic.')
   TCP_R, TCP_W = R, W
   NOCAN = 0

   TCP_DONE.clear()
   await TCP_DONE.wait()   
   # Must be a disconnect; finish and wait for a new connection
   print('*** TCP connection lost, waiting for reconnect.')

async def TCP_READER(ip,host):
   # packet layout:
   #    xx xx xx xx  xx  xx xx xx xx xx xx xx xx  -  13 bytes total = CS2_SIZE
   #    -----------  --  -----------------------
   #       CAN ID    len  data (left justified)
   global TCP_R, rrhash, ixTC, ixDB, ixCT

   print('Available at {} as {}, port {:d}.'.format(ip,host,CS2_PORT))
   fbpp = bytearray(CS2_SIZE)

   while True:                   # Wait for connection
      if TCP_R is None:
         await asyncio.sleep_ms(10)
         continue

      try:                       # Serve it
         pkt = await TCP_R.readexactly(CS2_SIZE)
      except EOFError:           # Connection lost/client disconnect
         print('TCP EOF error')
         TCP_R = None
         TCP_DONE.set()
         continue
      except OSError as err:     # Connection reset/client disconnect
         print('TCP read error: %s (%d)' % (
            errno.errorcode[err.errno],err.errno
         ))
         TCP_R = None
         TCP_DONE.set()
         continue

      assert len(pkt) == CS2_SIZE
      rrhash = int.from_bytes(pkt[2:4])

      #  Quick command parse for special responses
      cmd = int.from_bytes(pkt[0:2]) >> 1 & 0xff
      sub = int(pkt[9]) if pkt[4] > 4 else -1
      rsp = pkt[1] & 0x01

      if cmd == 0x1b and not rsp:     # CAN BOOT?
         while not CANtoTCP.empty():
            CANtoTCP.get_sync()       # discard any queued responses
         while not debugQUE.empty():
            debugQUE.get_sync()       # discard any queued responses

      buf = TtoC[ixTC % QSIZE]   # Normal handling 
      buf[0:CS2_SIZE] = pkt
      await TCPtoCAN.put(buf)
      ixTC += 1
      buf = DBQ[ixDB % QSIZE]
      buf[0:CS2_SIZE] = pkt
      await debugQUE.put(buf)
      ixDB += 1

      if cmd == 0x10 and sub == NODE_ID and not rsp and fdbk is not None:
      #  fbpp = copy.copy(fdbk.state_packet) # Check for S88 state poll
         fbpp[0:CS2_SIZE] = fdbk.state_packet
         buf = CtoT[ixCT % QSIZE]
         buf[0:CS2_SIZE] = fbpp
         await CANtoTCP.put(buf)
         ixCT += 1
         buf = DBQ[ixDB % QSIZE]
         buf[0:CS2_SIZE] = fbpp
         await debugQUE.put(buf)
         ixDB += 1

async def UDP_READER(ip,host,timeout=0):
   # packet layout:
   #    xx xx xx xx  xx  xx xx xx xx xx xx xx xx  -  13 bytes total = CS2_SIZE
   #    -----------  --  -----------------------
   #       CAN ID    len  data (left justified)
   import uselect as select
   global rrhash, qfCT, qfTC, qfDB

   s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
   s.setblocking(False)
   s.bind(('0.0.0.0',CS2_RPORT))
   p = select.poll()
   p.register(s, select.POLLIN)
   print(
      'Available at {} as {}, port {:d}/{:d}.'.format(
         ip,host,CS2_RPORT,CS2_SPORT
      )
   )
   print('UDP connection made, waiting for traffic.')
   cpkt, npkt = [], 0
   for n in range(QSIZE): cpkt.append(bytearray(CS2_SIZE))
   fbpp = bytearray(CS2_SIZE)

   while True:                   # Wait for activity on port
      pkt = cpkt[npkt % QSIZE]
      n = s.readinto(pkt, CS2_SIZE)
      if n is not None:
         assert n == CS2_SIZE
         mem = memoryview(pkt)
         rrhash = int.from_bytes(mem[2:4])
         qfTC = qput(pkt,TCPtoCAN,qfTC,CANmsg)
         qfDB = qput(pkt,debugQUE,qfDB,DBGmsg)

         #  Check for S88 state poll
         cmd = int.from_bytes(mem[0:2]) >> 1 & 0xff
         sub = int(pkt[9]) if pkt[4] > 4 else -1
         rsp = pkt[1] & 0x01
         if cmd == 0x10 and sub == NODE_ID and not rsp and fdbk is not None:
         #  fbpp = copy.copy(fdbk.state_packet)
            fbpp[0:CS2_SIZE] = fdbk.state_packet
            qfCT = qput(fbpp, CANtoTCP, qfCT, TCPmsg)
            qfDB = qput(fbpp, debugQUE, qfDB, DBGmsg)

         npkt += 1
         continue
      await asyncio.sleep_ms(timeout)

can = iCAN(_CANBOARD)
async def CAN_READER():
   from canbus import CanError
   global qfCT, ixCT, qfDB, ixDB

   def itob(wrd):
      return wrd.to_bytes(4,'big')

   print(
      "CAN %s board initialized successfully, waiting for traffic." %
      can.pins.name
   )

   buf = bytearray(CS2_SIZE)

   async for msg, err in can:     # Next queued CAN message
      if err:
         print('*** CAN receive error %s' % CanError.decode(error=err))
         continue

      dbu, dbl = itob(msg[0]), itob(msg[1])
      dbd = dbl[2:4] + itob(msg[2]) + itob(msg[3] << 8)
      ID = ((dbu[1] << 3) | (dbu[2] >> 5)) << 2 | (dbu[2] & 0x03)
      dlc = dbl[1] & 0x0f
      buf[0] = ID >> 8 & 0xff
      buf[1] = ID      & 0xff
      buf[2] = dbu[3]
      buf[3] = dbl[0]
      buf[4] = dlc
      buf[5:5+dlc] = dbd[0:dlc]
      buf[5+dlc:CS2_SIZE] = (8-dlc)*b'\x00'

      pkt = CtoT[ixCT % QSIZE]
      pkt[0:CS2_SIZE] = buf
      qfCT = qput(pkt, CANtoTCP, qfCT, TCPmsg)
      ixCT += 1
      pkt = DBQ[ixDB % QSIZE]
      pkt[0:CS2_SIZE] = buf
      qfDB = qput(pkt, debugQUE, qfDB, DBGmsg)
      ixDB += 1

async def TCP_WRITER(DELAY=_DELAY):
   global TCP_W

   last_t = utime.ticks_us()

   async for pkt in CANtoTCP:     # Wait for a packet
      assert len(pkt) == CS2_SIZE

      delta = utime.ticks_diff(utime.ticks_us(), last_t)
      if delta < DELAY:
         await asyncio.sleep_ms((DELAY - delta) // 1000)

      # This section endeavours to NEVER lose a packet sent from the
      # tracks to the controller.  Once we have one in our hands, we
      # keep trying to deliver it, even if the connection is temporarily
      # lost.  During long transmission bursts by, e.g. CONFIG DATA REQUEST,
      # the network/router might be overwhelmed and throttle transmission by
      # closing the connection.  In this case, we just wait for it to come
      # back ... and DONT LOSE A PACKET!

      while True:
         while TCP_W is None:
            await asyncio.sleep_ms(10)
         try:                     # Serve it
            TCP_W.write(pkt)
            await TCP_W.drain()
            break
         except OSError as err:   # Connection lost/client disconnect
            print('TCP write error: %s (%d)' % (
               errno.errorcode[err.errno],err.errno
            ))
            TCP_W = None
            TCP_DONE.set()
            continue              # Hope the connection comes back!
      last_t = utime.ticks_us()

async def UDP_WRITER(DELAY=_DELAY):
                                 # set up for UDP use
   s = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
   s.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
   s.setsockopt(socket.SOL_SOCKET, socket.SO_BROADCAST, 1)
   s.bind(('0.0.0.0',0))         # assumed to be only one interface

   last_t = utime.ticks_us()

   while True:
      async for pkt in CANtoTCP:
         assert len(pkt) == CS2_SIZE

         # Ensure packet interval no shorter than DELAY us
         delta = utime.ticks_diff(utime.ticks_us(), last_t)
         if delta < DELAY:
            await asyncio.sleep_ms((DELAY - delta) // 1000)

         s.sendto(pkt, ('255.255.255.255',CS2_SPORT))

         last_t = utime.ticks_us()

async def CAN_WRITER(MERR=5, MCNT=500):
   from canbus import CanError
   global NOCAN

   async for pkt in TCPtoCAN:     # process next incoming TCP packet
      assert len(pkt) == CS2_SIZE
      cnt = 0
      while True:                 # loop till sent or give up after errors
         errf = can.send(
            ID=int.from_bytes(pkt[0:4]),
            data=pkt[5:5+int(pkt[4])],
            EFF=True
         )
         if not errf:
            break                 # sent successfully
         cnt += 1
         if cnt == 1 or cnt == MERR:
            err = None if errf & 0x00ff == 0 else (errf & 0xff)
            txe = None if errf & 0xff00 == 0 else (errf >> 8 & 0xff)
            print('   >>>CAN write error<<< (%02x: %s)%s' % 
               (errf, CanError.decode(error=err,txctl=txe),
                  ' - quelling further reports' if cnt >= MERR else '')
            )
         await asyncio.sleep_ms(10)
         if cnt > MCNT:          # Abandon packet after this many tries
            NOCAN = 1
            break

async def DEBUG_OUT():
   global rrhash

   #                  0               1
   #                  0123456789ABCDEF0123456789ABCDEF
   def mycode(ba, sp="□.........↲↓↧⇤.................."):
      return ''.join(map(
         lambda c: sp[c] if c < 0x20 else ('.' if c > 0x7f else chr(c)),
         ba
      ))

   async for buf in debugQUE:
      assert len(buf) == CS2_SIZE
      cmd = (int.from_bytes(buf[0:2]) >> 1) & 0x7f
      cdec = ('[%02x]' % buf[9]) if cmd == 0 else ('(%02x)' % cmd)
      data = '%04x %04x %02x %s %s *%s*' % (
         int.from_bytes(buf[0:2]), int.from_bytes(buf[2:4]),
         buf[4],
         ' '.join(map(''.join, zip(*[iter(buf[5:].hex())]*4))),
         cdec, mycode(buf[5:5+int(buf[4])])
      )
      cid = int.from_bytes(buf[2:4])
      dir = '->' if cid == rrhash else '<-'
      print('%s %s CAN %s' % (_IPP,dir,data))
      dec.decode(
         int.from_bytes(buf[0:4]), buf[5:5+int(buf[4])]
      )

async def HEARTBEAT():
   # Slow LED flash heartbeat while running; boot button restarts.
   global NOCAN

   NOCAN = 0
   warn = int(QSIZE * 0.75)      # queue congestion warning level
   while True:
      if rp2.bootsel_button() == 1: sys.exit()
      pico_led.on()
      await asyncio.sleep(1)
      pico_led.off()
      await asyncio.sleep(1)

      if NOCAN:
         pico_led.on()
         await asyncio.sleep(0.5)
         pico_led.off()
         await asyncio.sleep(0.5)

      if TCPtoCAN.qsize() > warn and (TCPtoCAN.qsize()-warn) % 5 == 0:
         print('Wifi to CAN queue congestion: %d waiting' % TCPtoCAN.qsize())
      if CANtoTCP.qsize() > warn and (CANtoTCP.qsize()-warn) % 5 == 0:
         print('CAN to Wifi queue congestion: %d waiting' % CANtoTCP.qsize())

async def FEEDBACK(fdbk):
   global qfCT, qfDB
   # Simulated S88 feedback

   async for pkt in fdbk:        # Wait for change in state of some feedback pin
      qfCT = qput(pkt, CANtoTCP, qfCT, TCPmsg)
      qfDB = qput(pkt, debugQUE, qfDB, DBGmsg)

async def RUNNER(ip):
   srvr = await asyncio.start_server(TCP_SERVER,ip,CS2_PORT)
   while True:
      await asyncio.sleep(60)

print('%s <-> CAN packet hub (%s)' % (_IPP,_VER))
try:
   from marklin import decode, CS2decoder    # Marklin CS2 CAN packet decoder
except:
   print('No Märklin packet decoding, only logging raw data.')
   # Dummy decoder if not available
   class CS2decoder:
      def __init__(self,*pos,**kwd):
         pass
      def decode(self,*pos,**kwd):
         pass
dec = CS2decoder(pfx='    ',detail=True,print=True)

#Connect to WLAN
wlan = network.WLAN(network.STA_IF)
if not wlan.isconnected():
   wlan.active(True)
   mac = wlan.config('mac')
   host = 'CS2hub-' + ''.join('{:02x}'.format(b) for b in mac[3:])
   wlan.config(hostname = host)
   wlan.connect(SSID, PASS)
else:
   host = wlan.config('hostname')

#Power management on chip has to be turned off in order to not
#  drop UDP packets; see https://forums.raspberrypi.com/viewtopic.php?t=365691
if _IPP == 'UDP':
   wlan.config(pm=wlan.PM_NONE)   # disable power management on chip

while not wlan.isconnected():
   # Fast flash while waiting for wifi connection; boot button restarts.
   if rp2.bootsel_button() == 1: sys.exit()

   pico_led.on()
   sleep(0.25)
   pico_led.off()
   sleep(0.25)

pico_led.on()
ip = wlan.ifconfig()[0]

try:
   from feedback import Feedback
   fdbk = Feedback(NODE_ID,can.pins.FBP) # Feedback framework initialization
   feed = asyncio.create_task(FEEDBACK(fdbk))
except:
   fdbk = None

canr = asyncio.create_task(CAN_READER())
canw = asyncio.create_task(CAN_WRITER())
if _IPP == 'TCP':
   tcpr = asyncio.create_task(TCP_READER(ip,host))
   tcpw = asyncio.create_task(TCP_WRITER())
   runr = asyncio.create_task(RUNNER(ip))
   task = runr
else:
   udpr = asyncio.create_task(UDP_READER(ip,host,timeout=1))
   udpw = asyncio.create_task(UDP_WRITER())
   task = udpr
dbug = asyncio.create_task(DEBUG_OUT())
beat = asyncio.create_task(HEARTBEAT())

try:
   Loop.run_until_complete(task)
except KeyboardInterrupt:
   can.stop()
   if fdbk is not None:
      fdbk.stop()