ham/protocol/meshcore/node.go

package meshcore

import (
	"bytes"
	"encoding/binary"
	"fmt"
	"io"
	"log"
	"strings"
	"sync"
	"time"

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

const (
	maxCompanionFrameSize = 172
)

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()
}

type nodeDriver interface {
	radio.Device
	protocol.PacketReceiver

	Setup() error

	Packets() <-chan *Packet
}

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
	info       companionInfo
}

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,
	}
}

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

func (drv *companionDriver) Setup() (err error) {
	if err = drv.sendAppStart(); err != nil {
		return
	}
	if err = drv.sendDeviceInfo(); err != nil {
		return
	}
	go drv.poll()
	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
	firmwareDate, _ = time.Parse("02-01-2006", drv.info.FirmwareBuildDate)

	return &radio.Info{
		Name:            drv.info.Name,
		Manufacturer:    drv.info.Manufacturer,
		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) 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
		}

		//log.Printf("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)

	//log.Printf("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(wait ...byte) ([]byte, error) {
	for {
		b, err := drv.readFrame()
		if err != nil {
			return nil, err
		}
		if len(b) < 1 {
			continue
		}
		switch {
		case b[0] == companionResponseError:
			return nil, CompanionError{Code: b[1]}

		case b[0] >= 0x80:
			drv.handlePushFrame(b)
			continue

		case bytes.Contains(wait, b[:1]):
			return b, nil

		case wait == nil:
			return b, nil
		}
	}
}

func (drv *companionDriver) handlePushFrame(b []byte) {
	switch b[0] {
	case companionPushAdvert:
	case companionPushMessageWaiting:
	case companionPushLogRXData:
		drv.handleRXData(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:
			log.Printf("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:
				log.Printf("meshcore: packet channel full, dropping packet")
			}
		}
	}
}

func (drv *companionDriver) sendAppStart() (err error) {
	var b []byte
	if b, err = drv.writeCommand(companionAppStart, append(make([]byte, 8), []byte("git.maze.io/go/ham")...), 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 b []byte
	if b, err = drv.writeCommand(companionDeviceQuery, []byte{0x03}, companionResponseDeviceInfo); err != nil {
		return
	}

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

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

	return
}

func (drv *companionDriver) poll() {
	for {
		if _, err := drv.wait(); err != nil {
			log.Printf("meshcore: companion %s fatal error: %v", drv.info.Name, err)
			return
		}
	}
}

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