Insteon RF decoding in Go

insteon_decode.go

package main

import (
	"bufio"
	"bytes"
	"fmt"
	"os"
)

var frameStartN = []byte(`0011000101010101`)
var frameStartP = []byte(`1100111010101010`)
var preamble = []byte(`10101010`)
var byteIntro = []byte(`11`)

func main() {
	// This program is expecting the "ASCII binary" format produced by the
	// reciever tools in https://github.com/evilpete/insteonrf
	sc := bufio.NewScanner(os.Stdin)
	for sc.Scan() {
		line := sc.Bytes()
		if len(line) == 0 || line[0] == '#' {
			continue
		}

		fmt.Printf("packet {\n")
		framesRaw := findRawFrames(line)
		for _, frameRaw := range framesRaw {
			bytesRaw, err := tokenizeFrame(frameRaw)
			if err != nil {
				fmt.Printf("        unable to tokenize frame: %s\n", err)
				continue
			}

			bytes, err := decodeBytes(bytesRaw)
			if err != nil {
				fmt.Printf("        unable to decode bytes: %s\n", err)
				continue
			}

			msg, err := ParseMessage(bytes)
			if msg != nil {
				var suffix string
				if err != nil {
					suffix = fmt.Sprintf(" # ERROR: %s", err)
				}
				fmt.Printf("  message {\n    from      %s\n    to        %s\n    command   %#v\n    user data %x\n    hops left %d/%d\n  }%s\n", msg.From, msg.To, msg.Command, msg.UserData, msg.HopsRemaining, msg.MaxHops-1, suffix)
			} else {
				fmt.Printf("  # ERROR: %s", err)
			}
		}
		fmt.Printf("}\n\n")
	}
}

func findRawFrames(line []byte) [][]byte {
	remain := line
	var ret [][]byte

	// Does it contain the positive start sequence?
	prev := bytes.Index(remain, frameStartP)
	if prev < 0 {
		// Does it contain the inverted start sequence?
		prev = bytes.Index(remain, frameStartN)
		if prev < 0 {
			// No frames at all then, I guess.
			return ret
		}
		// If we found the negative start sequence then we'll invert
		// our whole payload so that everything is positive for the
		// rest of our work here.
		invertASCIIBin(line)
	}
	remain = remain[prev:] // skip to the beginning of the first frame

	markerLen := len(frameStartP)
	for {
		next := bytes.Index(remain[markerLen:], frameStartP)
		if next < 0 {
			ret = append(ret, remain)
			break
		}
		next += markerLen
		ret = append(ret, remain[:next])
		remain = remain[next:]
	}

	return ret
}

// tokenizeFrame splits a given frame into its constituent tokens:
//
//    checks for but skips the packet start header
//    checks for by skips the preamble: 10101010
//    returns elements representing individual bytes, still in full 28-bit representation each
func tokenizeFrame(frame []byte) ([][]byte, error) {
	if !bytes.HasPrefix(frame, frameStartP) {
		return nil, fmt.Errorf("frame does not start with the frame start marker")
	}

	remain := frame
	if len(remain) >= 7 && remain[5] == '1' && remain[6] == '1' {
		remain = remain[5:]
	}

	var ret [][]byte

	i := 0
	for len(remain) > 0 {
		if !bytes.HasPrefix(remain, byteIntro) {
			// Some trailing garbage, then
			break
		}
		if len(remain) < 28 {
			return ret, fmt.Errorf("not enough bits left for byte %d", i)
		}
		ret = append(ret, remain[:28])
		remain = remain[28:]
		i++
	}

	return ret, nil
}

func decodeBytes(tokens [][]byte) ([]byte, error) {
	ret := make([]byte, len(tokens))

	for i, token := range tokens {
		if len(token) != 28 {
			return ret, fmt.Errorf("token %d has incorrect length %d (should be 28 bits)", i, len(token))
		}
		if !bytes.HasPrefix(token, byteIntro) {
			return ret, fmt.Errorf("token %d is missing introducer", i)
		}

		dataBitsManch := token[2+10:]                // skip the introducer and the index value
		dataBits, err := demanchester(dataBitsManch) // skipping the 11 introducer
		if err != nil {
			return ret, fmt.Errorf("token %d invalid: %s", i, err)
		}

		for shift, asciiBit := range dataBits {
			if asciiBit == '1' {
				ret[i] |= 1 << uint(shift)
			}
		}
	}

	return ret, nil
}

func demanchester(manch []byte) ([]byte, error) {
	ret := make([]byte, len(manch)/2)

	for i := range ret {
		manchOfs := i * 2
		pair := manch[manchOfs : manchOfs+2]
		if pair[0] == pair[1] {
			return nil, fmt.Errorf("invalid Manchester sequence %#v %#v", pair[0], pair[1])
		}
		ret[i] = pair[1]
	}

	return ret, nil
}

// in-place inversion of ASCII binary
func invertASCIIBin(bin []byte) {
	for i, c := range bin {
		switch c {
		case '0':
			bin[i] = '1'
		case '1':
			bin[i] = '0'
		}
	}
}

func decodeASCIIBin(bin []byte) []byte {
	ret := make([]byte, len(bin)/8)

	for i := range ret {
		bitOffset := i * 8
		bits := bin[bitOffset : bitOffset+8]
		for shift, bit := range bits {
			switch bit {
			case '1':
				//ret[i] |= 0x80 >> uint(shift)
				ret[i] |= 1 << uint(shift)
			}
		}
	}

	return ret
}

type Message struct {
	From, To      DeviceID
	Command       Command
	HopsRemaining int
	MaxHops       int

	// For extended messages only. nil for standard messages.
	UserData []byte
}

func ParseMessage(bytes []byte) (*Message, error) {
	if len(bytes) < 10 {
		return nil, fmt.Errorf("too short to be an Insteon message")
	}

	msg := &Message{}
	flags := bytes[0]
	mt := MessageType(flags >> 5)
	extended := (flags & 16) != 0
	cmd1 := bytes[7]
	cmd2 := bytes[8]

	msg.MaxHops = int(flags & 0x3)
	msg.HopsRemaining = int((flags >> 2) & 0x3)
	msg.Command = NewCommand(mt, extended, cmd1, cmd2)
	msg.To = DecodeDeviceID(bytes[1:4])
	msg.From = DecodeDeviceID(bytes[4:7])

	if mt.IsBroadcast() {
		// Broadcast messages have the addresses inverted so that a
		// battery-powered device can see the from address first and more
		// quickly go back to sleep if they can see the message is not addressed
		// to a broadcast group they participate in.
		msg.To, msg.From = msg.From, msg.To
	}

	var wantCRC byte
	if extended && !mt.IsAcknowledgement() {
		if len(bytes) < 25 {
			return nil, fmt.Errorf("extended flag is set but too short to be extended (got %d bytes, but need at least 25)", len(bytes))
		}
		msg.UserData = bytes[9:22]
		wantCRC = bytes[23]
	} else {
		wantCRC = bytes[9]
	}

	if got, want := messageMainCRC(bytes), wantCRC; got != want {
		return msg, fmt.Errorf("invalid message CRC (got %d, but want %d)", got, want)
	}

	return msg, nil
}

func messageMainCRC(bytes []byte) byte {
	if len(bytes) < 1 {
		return 0 // invalid; flags byte isn't present
	}

	extended := (bytes[0] & 16) != 0
	if extended {
		if len(bytes) > 23 {
			bytes = bytes[:23]
		}
	} else {
		if len(bytes) > 9 {
			bytes = bytes[:9]
		}
	}

	var r byte
	for _, c := range bytes {
		r ^= c // xor
		x := (r ^ (r << 1))
		x = x & 0x0F
		r ^= (x << 4)
	}
	return r
}

type MessageType byte

const (
	DirectMessage     MessageType = 0x0
	DirectMessageACK  MessageType = 0x1
	DirectMessageNACK MessageType = 0x5

	BroadcastMessage MessageType = 0x4

	AllLinkBroadcastMessage   MessageType = 0x6
	AllLinkCleanupMessage     MessageType = 0x2
	AllLinkCleanupMessageACK  MessageType = 0x3
	AllLinkCleanupMessageNACK MessageType = 0x7
)

func (t MessageType) IsBroadcast() bool {
	switch t {
	case BroadcastMessage, AllLinkBroadcastMessage:
		return true
	default:
		return false
	}
}

func (t MessageType) IsAcknowledgement() bool {
	switch t {
	case DirectMessageACK, DirectMessageNACK, AllLinkCleanupMessageACK, AllLinkCleanupMessageNACK:
		return true
	default:
		return false
	}
}

func (t MessageType) Acknowledgement() MessageType {
	switch t {
	case DirectMessage:
		return DirectMessageACK
	case AllLinkCleanupMessage:
		return AllLinkCleanupMessageACK
	default:
		panic(fmt.Sprintf("cannot ACK %#v", t))
	}
}

func (t MessageType) AcknowledgementOf() MessageType {
	switch t {
	case DirectMessageACK, DirectMessageNACK:
		return DirectMessage
	case AllLinkCleanupMessageACK, AllLinkCleanupMessageNACK:
		return AllLinkCleanupMessage
	default:
		panic(fmt.Sprintf("%#v is not an acknowledgement message type", t))
	}
}

func (t MessageType) NonAcknowledgement() MessageType {
	switch t {
	case DirectMessage:
		return DirectMessageNACK
	case AllLinkCleanupMessage:
		return AllLinkCleanupMessageNACK
	default:
		panic(fmt.Sprintf("cannot NACK %#v", t))
	}
}

func (t MessageType) Cleanup() MessageType {
	switch t {
	case AllLinkBroadcastMessage:
		return AllLinkCleanupMessage
	default:
		panic(fmt.Sprintf("no cleanup message type for %#v", t))
	}
}

func (t MessageType) IsCleanup() bool {
	switch t {
	case AllLinkCleanupMessage:
		return true
	default:
		return false
	}
}

func (t MessageType) CleanupOf() MessageType {
	switch t {
	case AllLinkCleanupMessage:
		return AllLinkBroadcastMessage
	default:
		panic(fmt.Sprintf("%#v is not a cleanup message type", t))
	}
}

func (t MessageType) GoString() string {
	switch t {
	case DirectMessage:
		return "DirectMessage"
	case DirectMessageACK:
		return "DirectMessageACK"
	case DirectMessageNACK:
		return "DirectMessageNACK"
	case BroadcastMessage:
		return "BroadcastMessage"
	case AllLinkBroadcastMessage:
		return "AllLinkBroadcastMessage"
	case AllLinkCleanupMessage:
		return "AllLinkCleanupMessage"
	case AllLinkCleanupMessageACK:
		return "AllLinkCleanupMessageACK"
	case AllLinkCleanupMessageNACK:
		return "AllLinkCleanupMessageNACK"
	default:
		// Should never get here, because the above cases cover all possible
		// 3-bit combinations.
		return fmt.Sprintf("MessageType(0x%02x)", byte(t))
	}
}

// Command is a packed combination of MessageType and two command bytes, which
// uniquely identify one of the commands in the Insteon command table.
//
// Only 20 bits of the value are actually used; the most significant 12 bits
// are always zero.
type Command uint32

var (
	ProductDataRequest             = NewCommand(DirectMessage, false, 0x02, 0x00)
	FXUsernameRequest              = NewCommand(DirectMessage, false, 0x02, 0x01)
	DeviceTextStringRequest        = NewCommand(DirectMessage, false, 0x02, 0x02)
	GetInsteonEngineVersionRequest = NewCommand(DirectMessage, false, 0x0d, 0x00)
	Ping                           = NewCommand(DirectMessage, false, 0x0f, 0x00)
	Beep                           = NewCommand(DirectMessage, false, 0x30, 0x00)
	LightStatusRequest             = NewCommand(DirectMessage, false, 0x19, 0x00)

	AllLinkRecall     = NewCommand(AllLinkBroadcastMessage, false, 0x11, 0x00)
	AllLinkAlias2High = NewCommand(AllLinkBroadcastMessage, false, 0x12, 0x00)
	AllLinkAlias1Low  = NewCommand(AllLinkBroadcastMessage, false, 0x13, 0x00)
	AllLinkAlias2Low  = NewCommand(AllLinkBroadcastMessage, false, 0x14, 0x00)
	AllLinkAlias3High = NewCommand(AllLinkBroadcastMessage, false, 0x15, 0x00)
	AllLinkAlias3Low  = NewCommand(AllLinkBroadcastMessage, false, 0x16, 0x00)
	AllLinkAlias4High = NewCommand(AllLinkBroadcastMessage, false, 0x17, 0x00)
	AllLinkAlias4Low  = NewCommand(AllLinkBroadcastMessage, false, 0x18, 0x00)
)

func AssignToAllLinkGroup(groupNumber byte) Command {
	return NewCommand(DirectMessage, false, 0x01, groupNumber)
}

func RemoveFromAllLinkGroup(groupNumber byte) Command {
	return NewCommand(DirectMessage, false, 0x02, groupNumber)
}

func EnterLinkingMode(groupNumber byte) Command {
	return NewCommand(DirectMessage, false, 0x09, groupNumber)
}

func EnterUnlinkingMode(groupNumber byte) Command {
	return NewCommand(DirectMessage, false, 0x10, groupNumber)
}

func GetInsteonEngineVersionResponse(version byte) Command {
	return NewCommand(DirectMessage.Acknowledgement(), false, 0x0d, version)
}

func NewCommand(mt MessageType, ext bool, cmd1, cmd2 byte) Command {
	var extBit Command
	if ext {
		extBit = 1 << 16
	}
	return Command(Command(mt)<<17 | extBit | Command(cmd1) | Command(cmd2)<<8)
}

func (c Command) MessageType() MessageType {
	return MessageType(c >> 17)
}

func (c Command) Extended() bool {
	return (c & 0x100) != 0
}

func (c Command) Cmd1() byte {
	return byte(c)
}

func (c Command) Cmd2() byte {
	return byte(c >> 8)
}

func (c Command) GoString() string {
	switch c.MessageType() {
	case AllLinkCleanupMessageACK, DirectMessageACK:
		return c.AcknowledgementOf().GoString() + ".Acknowledgement()"
	case AllLinkCleanupMessageNACK, DirectMessageNACK:
		return c.AcknowledgementOf().GoString() + ".NonAcknowledgement()"
	case AllLinkCleanupMessage:
		return c.CleanupOf().GoString() + ".Cleanup()"
	}

	switch c {
	case ProductDataRequest:
		return "ProductDataRequest"
	case FXUsernameRequest:
		return "FXUsernameRequest"
	case DeviceTextStringRequest:
		return "DeviceTextStringRequest"
	case Beep:
		return "Beep"
	case Ping:
		return "Ping"
	}

	switch c & 0xf00ff { // Zero out the cmd2 byte
	case AssignToAllLinkGroup(0):
		return fmt.Sprintf("AssignToAllLinkGroup(0x%02x)", c.Cmd2())
	case RemoveFromAllLinkGroup(0):
		return fmt.Sprintf("RemoveFromAllLinkGroup(0x%02x)", c.Cmd2())
	case EnterLinkingMode(0):
		return fmt.Sprintf("EnterLinkingMode(0x%02x)", c.Cmd2())
	case EnterUnlinkingMode(0):
		return fmt.Sprintf("EnterUnlinkingMode(0x%02x)", c.Cmd2())
	case LightStatusRequest:
		return "LightStatusRequest"
	case AllLinkRecall:
		return "AllLinkRecall"
	case AllLinkAlias1Low:
		return "AllLinkAlias1Low"
	case AllLinkAlias2Low:
		return "AllLinkAlias2Low"
	case AllLinkAlias2High:
		return "AllLinkAlias2High"
	case AllLinkAlias3Low:
		return "AllLinkAlias3Low"
	case AllLinkAlias3High:
		return "AllLinkAlias3High"
	case AllLinkAlias4Low:
		return "AllLinkAlias4Low"
	case AllLinkAlias4High:
		return "AllLinkAlias4High"
	default:
		return fmt.Sprintf("NewCommand(%#v, 0x%02x, 0x%02x)", c.MessageType(), c.Cmd1(), c.Cmd2())
	}
}

// IsAcknowledgement returns true if the reciever is an acknowledgement of
// some other command.
func (c Command) IsAcknowledgement() bool {
	return c.MessageType().IsAcknowledgement()
}

// Acknowledgement returns the acknowledgement command corresponding to the
// reciever, which must not already be an acknowledgement command and must
// not be a general broadcast command, or this method will panic.
func (c Command) Acknowledgement() Command {
	mt := c.MessageType().Acknowledgement()
	return c.newMessageType(mt, false)
}

// AcknowledgementOf returns the command that the given command is acknowleding.
// Will panic if the command is not an acknowledgement command.
func (c Command) AcknowledgementOf() Command {
	mt := c.MessageType().AcknowledgementOf()
	return c.newMessageType(mt, false)
}

// NonAcknowledgement returns the non-acknowledgement command corresponding to
// the reciever, which must not already be an acknowledgement command and must
// not be a general broadcast command, or this method will panic.
func (c Command) NonAcknowledgement() Command {
	mt := c.MessageType().NonAcknowledgement()
	return c.newMessageType(mt, false)
}

// IsCleanup returns true if the reciever is a cleanup of some other command.
func (c Command) IsCleanup() bool {
	return c.MessageType().IsCleanup()
}

// Cleanup returns the cleanup command corresponding to the reciever, which
// must be an ALL-Link broadcast command or this method will panic.
func (c Command) Cleanup() Command {
	mt := c.MessageType().Cleanup()
	return c.newMessageType(mt, c.Extended())
}

// CleanupOf returns the command that the given command is cleaning up.
// Will panic if the command is not a cleanup command.
func (c Command) CleanupOf() Command {
	mt := c.MessageType().CleanupOf()
	return c.newMessageType(mt, false)
}

func (c Command) newMessageType(mt MessageType, ext bool) Command {
	return NewCommand(mt, ext, c.Cmd1(), c.Cmd2())
}

type DeviceID [3]byte

func DecodeDeviceID(bytes []byte) DeviceID {
	if len(bytes) != 3 {
		panic("incorrect device id slice length")
	}
	var ret DeviceID
	for i, v := range bytes {
		ret[i] = v
	}
	return ret
}

func (id DeviceID) String() string {
	return fmt.Sprintf("%02X.%02X.%02X", id[2], id[1], id[0])
}

func (id DeviceID) GoString() string {
	return fmt.Sprintf("ParseDeviceID(%q)", id.String())
}