ham/protocol/meshcore/node.go

package meshcore

import (
	"encoding/binary"
	"encoding/hex"
	"errors"
	"fmt"
	"io"
	"math/rand/v2"
	"slices"
	"strings"
	"sync"
	"time"

	"git.maze.io/go/ham/protocol"
	"git.maze.io/go/ham/radio"
)

const (
	maxCompanionFrameSize = 172
)

// Node can be any type of MeshCore node.
type Node struct {
	OnPacket (*Packet)

	driver nodeDriver
}

// NewCompanion connects to a companion device type (over serial, TCP or BLE).
func NewCompanion(conn io.ReadWriteCloser) (*Node, error) {
	driver := newCompanionDriver(conn)

	if err := driver.Setup(); err != nil {
		return nil, err
	}

	return &Node{
		driver: driver,
	}, nil
}

func (dev *Node) Close() error {
	return dev.driver.Close()
}

func (dev *Node) Packets() <-chan *Packet {
	return dev.driver.Packets()
}

func (dev *Node) RawPackets() <-chan *protocol.Packet {
	return dev.driver.RawPackets()
}

func (dev *Node) Info() *radio.Info {
	return dev.driver.Info()
}

func (dev *Node) Trace(path []byte) (snr []float64, err error) {
	if tracer, ok := dev.driver.(nodeTracer); ok {
		return tracer.Trace(path)
	}
	return nil, errors.New("meshcore: node doesn't support running traces")
}

type nodeDriver interface {
	radio.Device
	protocol.PacketReceiver

	Setup() error

	Packets() <-chan *Packet
}

type nodeTracer interface {
	Trace(path []byte) (snr []float64, err error)
}

type CompanionError struct {
	Code byte
}

func (err CompanionError) Error() string {
	switch err.Code {
	case companionErrCodeUnsupported:
		return "meshcore: companion: unsupported"
	case companionErrCodeNotFound:
		return "meshcore: companion: not found"
	case companionErrCodeTableFull:
		return "meshcore: companion: table full"
	case companionErrCodeBadState:
		return "meshcore: companion: bad state"
	case companionErrCodeFileIOError:
		return "meshcore: companion: file input/output error"
	case companionErrCodeIllegalArgument:
		return "meshcore: companion: illegal argument"
	default:
		return fmt.Sprintf("meshcore: companion: unknown error code %#02x", err.Code)
	}
}

type companionDriver struct {
	conn          io.ReadWriteCloser
	mu            sync.Mutex
	packets       chan *Packet
	rawPackets    chan *protocol.Packet
	waiting       chan *companionDriverWaiting
	info          companionInfo
	traceTag      uint32
	traceAuthCode uint32
}

type companionDriverWaiting struct {
	expect   []byte
	response chan []byte
	err      chan error
}

func newCompanionDriverWaiting(expect []byte) *companionDriverWaiting {
	return &companionDriverWaiting{
		expect:   expect,
		response: make(chan []byte),
		err:      make(chan error),
	}
}

func (wait *companionDriverWaiting) Respond(response []byte) {
	select {
	case wait.response <- response:
	default:
		Logger.Warnf("meshcore: waiting for %02x discard response: %02x", wait.expect, response)
	}
}

func (wait *companionDriverWaiting) Error(err error) {
	select {
	case wait.err <- err:
	default:
		Logger.Warnf("meshcore: waiting for %02x discard error: %v", wait.expect, err)
	}
}

func (wait *companionDriverWaiting) Close() {
	Logger.Tracef("meshcore: waiting for %02x closing", wait.expect)
	close(wait.response)
	close(wait.err)
}

func (wait *companionDriverWaiting) Wait() ([]byte, error) {
	Logger.Tracef("meshcore: waiting for %02x", wait.expect)
	select {
	case err := <-wait.err:
		Logger.Tracef("meshcore: waiting for %02x received error: %v", wait.expect, err)
		return nil, err
	case response := <-wait.response:
		Logger.Tracef("meshcore: waiting for %02x received response: %d", wait.expect, len(response))
		return response, nil
	}
}

type companionInfo struct {
	// Fields returns by CMD_APP_START.
	Type                 NodeType
	Power                byte // in dBm
	MaxPower             byte // in dBm
	PublicKey            [32]byte
	Latitude             float64
	Longitude            float64
	HasMultiACKs         bool
	AdvertLocationPolicy byte
	TelemetryFlags       byte
	ManualAddContacts    byte
	Frequency            float64 // in MHz
	Bandwidth            float64 // in kHz
	SpreadingFactor      byte
	CodingRate           byte
	Name                 string

	// Fields returns by CMD_DEVICE_QUERY.
	FirmwareVersion     string
	FirmwareVersionCode byte
	FirmwareBuildDate   string
	Manufacturer        string
	MaxContacts         int
	MaxGroupChannels    int
	BLEPIN              [4]byte
}

func newCompanionDriver(conn io.ReadWriteCloser) *companionDriver {
	return &companionDriver{
		conn:     conn,
		waiting:  make(chan *companionDriverWaiting, 16),
		traceTag: rand.Uint32(),
		//traceAuthCode: rand.Uint32(),
	}
}

func (drv *companionDriver) Close() error {
	return drv.conn.Close()
}

func (drv *companionDriver) Setup() (err error) {
	go drv.poll()
	if err = drv.sendAppStart(); err != nil {
		return
	}
	if err = drv.sendDeviceInfo(); err != nil {
		return
	}
	return
}

func (drv *companionDriver) Packets() <-chan *Packet {
	if drv.packets == nil {
		drv.packets = make(chan *Packet, 16)
	}
	return drv.packets
}

func (drv *companionDriver) RawPackets() <-chan *protocol.Packet {
	if drv.rawPackets == nil {
		drv.rawPackets = make(chan *protocol.Packet, 16)
	}
	return drv.rawPackets
}

func (drv *companionDriver) Info() *radio.Info {
	var pos *radio.Position
	if drv.info.Latitude != 0 && drv.info.Longitude != 0 {
		pos = &radio.Position{
			Latitude:  drv.info.Latitude,
			Longitude: drv.info.Longitude,
		}
	}

	var firmwareDate time.Time
	for _, layout := range []string{
		"02 Jan 2006",
		"02-01-2006",
	} {
		var terr error
		if firmwareDate, terr = time.Parse(layout, drv.info.FirmwareBuildDate); terr == nil {
			break
		}
	}

	var (
		manufacturerPart = strings.SplitN(drv.info.Manufacturer, " ", 2)
		manufacturer     = manufacturerPart[0]
		device           string
	)
	if len(manufacturerPart) > 1 {
		device = manufacturerPart[1]
	}

	return &radio.Info{
		Name:            drv.info.Name,
		Manufacturer:    manufacturer,
		Device:          device,
		FirmwareDate:    firmwareDate,
		FirmwareVersion: drv.info.FirmwareVersion,
		Modulation:      protocol.LoRa,
		Position:        pos,
		Frequency:       drv.info.Frequency,
		Bandwidth:       drv.info.Bandwidth,
		Power:           float64(drv.info.Power),
		LoRaSF:          drv.info.SpreadingFactor,
		LoRaCR:          drv.info.CodingRate,
	}
}

func (drv *companionDriver) Trace(path []byte) (snr []float64, err error) {
	var (
		args = make([]byte, 4+4+1+len(path))
		data []byte
	)
	binary.LittleEndian.PutUint32(args[0:], drv.traceTag)      // tag
	binary.LittleEndian.PutUint32(args[4:], drv.traceAuthCode) // authcode
	args[8] = 0                                                // flags
	copy(args[9:], path)                                       // path

	Logger.Debugf("meshcore: trace %02x tag %08x authcode %08x", path, drv.traceTag, drv.traceAuthCode)
	if data, err = drv.writeCommand(companionSendTracePath, args, companionResponseSent); err != nil {
		return
	}

	Logger.Debugf("trace response:\n%s", hex.Dump(data))
	return
}

func (drv *companionDriver) readFrame() ([]byte, error) {
	var frame [3 + maxCompanionFrameSize]byte
	for {
		n, err := drv.conn.Read(frame[:])
		if err != nil {
			return nil, err
		} else if n < 3 {
			continue
		}

		if frame[0] != '>' {
			// not a companion frame
			continue
		}

		size := int(binary.LittleEndian.Uint16(frame[1:]))
		if size > maxCompanionFrameSize {
			return nil, fmt.Errorf("meshcore: companion sent frame size of %d, which exceeds maximum of %d", size, maxCompanionFrameSize)
		}

		// Make sure we have read all bytes
		o := n
		for (o - 3) < size {
			if n, err = drv.conn.Read(frame[o:]); err != nil {
				return nil, err
			}
			o += n
		}

		Logger.Tracef("read %d:\n%s", size, hex.Dump(frame[:3+size]))
		return frame[3 : 3+size], nil
	}
}

func (drv *companionDriver) writeFrame(b []byte) (err error) {
	if len(b) > maxCompanionFrameSize {
		return fmt.Errorf("meshcore: companion: frame size %d exceed maximum of %d", len(b), maxCompanionFrameSize)
	}

	var frame [3 + maxCompanionFrameSize]byte
	frame[0] = '<'
	binary.LittleEndian.PutUint16(frame[1:], uint16(len(b)))
	n := copy(frame[3:], b)

	//Logger.Tracef("send %d:\n%s", n, hex.Dump(frame[:3+n]))
	_, err = drv.conn.Write(frame[:3+n])
	return
}

func (drv *companionDriver) writeCommand(cmd byte, args []byte, wait ...byte) ([]byte, error) {
	drv.mu.Lock()
	defer drv.mu.Unlock()

	if err := drv.writeFrame(append([]byte{cmd}, args...)); err != nil {
		return nil, err
	}

	return drv.wait(wait...)
}

func (drv *companionDriver) wait(expect ...byte) ([]byte, error) {
	wait := newCompanionDriverWaiting(expect)
	defer wait.Close()

	drv.waiting <- wait
	return wait.Wait()
}

func (drv *companionDriver) handlePushFrame(b []byte) {
	if len(b) < 1 {
		return // illegal
	}
	switch b[0] {
	case companionPushAdvert:
	case companionPushMessageWaiting:
	case companionPushLogRXData:
		drv.handleRXData(b[1:])
	case companionPushNewAdvert:
	default:
		Logger.Warnf("meshcore: unhandled push %02x:\n%s", b[0], hex.Dump(b[1:]))
	}
}

func (drv *companionDriver) handleRXData(b []byte) {
	if drv.packets == nil && drv.rawPackets == nil {
		return // not listening for packets, discard
	}

	if len(b) < 2+minPacketSize {
		return // too short
	}

	var (
		now  = time.Now().UTC()
		snr  = float64(b[0]) / 4
		rssi = int8(b[1])
	)

	// Decode raw
	if drv.rawPackets != nil {
		select {
		case drv.rawPackets <- &protocol.Packet{
			Time:     now,
			Protocol: protocol.MeshCore,
			SNR:      snr,
			RSSI:     rssi,
			Raw:      b[2:],
		}:
		default:
			Logger.Warn("meshcore: raw packet channel full, dropping packet")
		}
	}

	// Decode payload
	if drv.packets != nil {
		packet := new(Packet)
		if err := packet.UnmarshalBytes(b[2:]); err == nil {
			packet.SNR = snr
			packet.RSSI = rssi
			select {
			case drv.packets <- packet:
			default:
				Logger.Warn("meshcore: packet channel full, dropping packet")
			}
		}
	}
}

func (drv *companionDriver) sendAppStart() (err error) {
	var (
		b    []byte
		args = append(make([]byte, 8), []byte("git.maze.io/go/ham")...)
	)
	if b, err = drv.writeCommand(companionAppStart, args, companionResponseSelfInfo); err != nil {
		return fmt.Errorf("meshcore: can't send application start: %v", err)
	}
	//log.Printf("companion app start response:\n%s", hex.Dump(b))

	const expect = 1 + 1 + 1 + 1 + 32 + 4 + 4 + 1 + 1 + 1 + 1 + 4 + 4 + 1 + 1
	if len(b) < expect {
		return fmt.Errorf("companion: expected %d bytes of self info, got %d", expect, len(b))
	}
	if b[0] != companionResponseSelfInfo {
		return fmt.Errorf("companion: expected self info response, got %#02x", b[0])
	}
	b = b[1:]

	drv.info.Type = NodeType(b[0])
	drv.info.Power = b[1]
	drv.info.MaxPower = b[2]
	copy(drv.info.PublicKey[:], b[3:])
	drv.info.Latitude, drv.info.Longitude = decodeLatLon(b[35:])
	drv.info.HasMultiACKs = b[43] != 0
	drv.info.AdvertLocationPolicy = b[44]
	drv.info.TelemetryFlags = b[45]
	drv.info.ManualAddContacts = b[46]
	drv.info.Frequency = decodeFrequency(b[47:])
	drv.info.Bandwidth = decodeFrequency(b[51:])
	drv.info.SpreadingFactor = b[55]
	drv.info.CodingRate = b[56]
	drv.info.Name = strings.TrimRight(string(b[57:]), "\x00")

	return
}

func (drv *companionDriver) sendDeviceInfo() (err error) {
	var (
		args = []byte{0x03}
		data []byte
	)
	if data, err = drv.writeCommand(companionDeviceQuery, args, companionResponseDeviceInfo); err != nil {
		return
	}

	const expect = 4 + 4 + 12 + 40 + 20
	if len(data) < expect {
		return fmt.Errorf("companion: expected %d bytes of self info, got %d", expect, len(data))
	}
	if data[0] != companionResponseDeviceInfo {
		return fmt.Errorf("companion: expected device info response, got %#02x", data[0])
	}
	data = data[1:]

	drv.info.FirmwareVersionCode = data[0]
	drv.info.MaxContacts = int(data[1]) * 2
	drv.info.MaxGroupChannels = int(data[2])
	drv.info.FirmwareBuildDate = decodeCString(data[7:19])
	drv.info.Manufacturer = decodeCString(data[19:59])
	drv.info.FirmwareVersion = decodeCString(data[59:79])

	return
}

func (drv *companionDriver) poll() {
	for {
		frame, err := drv.readFrame()
		if err != nil {
			Logger.Errorf("meshcore: unrecoverable error: %v", err)
			return
		} else if len(frame) < 1 {
			continue
		}

		response := frame[0]
		Logger.Tracef("meshcore: handle %s (%02x, %d bytes)", companionResponseName(response), response, len(frame[1:]))
		switch {
		case response == companionResponseError:
			err := CompanionError{Code: frame[1]}
			select {
			case waiting := <-drv.waiting:
				Logger.Debugf("meshcore: sending error to waiting: %v", err)
				waiting.Error(err)
			default:
				Logger.Debugf("meshcore: unexpected error: %v", err)
			}

		case response >= 0x80:
			drv.handlePushFrame(frame)

		default:
			select {
			case waiting := <-drv.waiting:
				if len(waiting.expect) == 0 {
					//Logger.Debugf("meshcore: respond %02x verbatim", response)
					waiting.Respond(frame)
				} else if slices.Contains(waiting.expect, response) {
					//Logger.Debugf("meshcore: respond %02x to expected %02x", response, waiting.expect)
					waiting.Respond(frame)
				} else {
					//Logger.Debugf("meshcore: unexpected %02x response (want %02x)", response, waiting.expect)
					waiting.Error(fmt.Errorf("unexpected %02x response", response))
				}
			default:
				Logger.Warnf("meshcore: unhandled %02x response", response)
			}
		}
	}
}

var (
	_ protocol.PacketReceiver = (*Node)(nil)
	_ nodeDriver              = (*companionDriver)(nil)
)