trimtools/monitor/receiver.go

package monitor

import (
	"encoding/binary"
	"fmt"
	"github.com/jacobsa/go-serial/serial"
	"io"
	"os"
)

const (
	MAXPACKETLLEN = 255 + 6
)

type Receiver struct {
	port       io.ReadWriteCloser
	device     string
	baud       uint
	dataBits   uint
	stopBits   uint
	parity     serial.ParityMode
	portConfig serial.OpenOptions

	receiverType ReceiverType

	Debug bool
	Dummy bool

	inMonitor bool

	seekPos uint32
}

func Connect(device string, baud int, dataBits int, stopBits int, parity string, debug bool) (*Receiver, error) {
	r := &Receiver{
		device:   device,
		baud:     uint(baud),
		dataBits: uint(dataBits),
		stopBits: uint(stopBits),
		Debug:    debug,
	}

	r.portConfig = serial.OpenOptions{
		PortName:              r.device,
		BaudRate:              r.baud,
		DataBits:              r.dataBits,
		StopBits:              r.stopBits,
		MinimumReadSize:       0,
		InterCharacterTimeout: 1000, // a non-zero timeout is required for waitForMonitor to work
	}

	switch parity {
	case "none":
		r.parity = serial.PARITY_NONE
		r.portConfig.ParityMode = serial.PARITY_NONE
	case "even":
		r.parity = serial.PARITY_EVEN
		r.portConfig.ParityMode = serial.PARITY_EVEN
	case "odd":
		r.parity = serial.PARITY_ODD
		r.portConfig.ParityMode = serial.PARITY_ODD
	default:
		return nil, fmt.Errorf("unrecognized parity setting")
	}
	var err error
	r.port, err = serial.Open(r.portConfig)
	if err != nil {
		return nil, err
	}

	// figure out baud and parity
	err = r.ProbePortConfig()
	if err != nil {
		return nil, err
	}

	return r, nil
}

// ProbePortConfig probes different serial port settings to find a responding receiver
func (r *Receiver) ProbePortConfig() error {

	var err error

	// First figure out what baud rate the receiver is currently set up for
	// easiest way to test this is by sending an ENQ
	for attempts := 0; attempts < 3; attempts++ {

		switch attempts {
		case 0:
			err = r.useUserBaudRate()
		case 1:
			err = r.useFastBaudRate()
		case 2:
			err = r.useSlowBaudRate()
		}
		if err != nil {
			return err
		}

		err = r.ClearComm()
		if err != nil {
			return err
		}

		ok, err := r.EnqAckTest()
		if err != nil {
			return err
		}

		if !ok {
			continue
		}

		if ok {
			return nil
		}
	}
	return fmt.Errorf("can't find receiver")

}

func (r *Receiver) reconfigurePort(baudRate uint, dataBits uint, stopBits uint, parity serial.ParityMode) error {
	var err error
	r.port.Close()
	r.portConfig.BaudRate = baudRate
	r.portConfig.DataBits = dataBits
	r.portConfig.StopBits = stopBits
	r.portConfig.ParityMode = parity
	r.port, err = serial.Open(r.portConfig)
	return err
}

func (r *Receiver) useSlowBaudRate() error {
	fmt.Println("Reconfiguring port to slow monitor baud rate: 9600 n-8-1")
	return r.reconfigurePort(9600, 8, 1, serial.PARITY_NONE)
}

func (r *Receiver) useFastBaudRate() error {
	fmt.Println("Reconfiguring port to fast baud rate: 38400 n-8-1")
	//	return r.reconfigurePort(38400, 8, 1, serial.PARITY_NONE)
	return r.reconfigurePort(38400, 8, 1, serial.PARITY_ODD)
}

func (r *Receiver) useUserBaudRate() error {
	fmt.Printf("Reconfiguring port to user baud rate: %d %s-%d-%d\n", r.baud, parityToString(r.parity), r.dataBits, r.stopBits)
	return r.reconfigurePort(r.baud, r.dataBits, r.stopBits, r.parity)
}

func parityToString(p serial.ParityMode) string {
	s := []string{"none", "odd", "even"}
	return s[p]
}

// ClearComm sents 249 nulls on the serial port, which is something trimble likes to
// do when starting the monitor. I think the point of this was to flush out the FIFO
// buffers.
func (r *Receiver) ClearComm() error {
	nulls := make([]byte, 249)
	_, err := r.Write(nulls)
	if err != nil {
		return err
	}
	// 	b := make([]byte, 12000)
	// 	n, _ := m.Read(b)
	// 	fmt.Printf("%d %02X \n",n,b)
	x := r.port.(*os.File) // TODO: verify if this is actually needed
	x.Sync()
	return nil
}

func (r *Receiver) FlushInput() {
	b := make([]byte, 12000)
	for {
		n, _ := r.Read(b)
		fmt.Printf("FLUSH read %d bytes: % 02X\n", n, b[:n])
		if n == 0 {
			return
		}
	}
}

// Write is wrapped so that debug messages can be printed
func (r *Receiver) Write(b []byte) (int, error) {
	r.debugLog("Port Write: % 02X", b)
	//fmt.Printf("%02X••\n",b)
	if r.Dummy {
		return len(b), nil
	}
	n, err := r.port.Write(b)
	return n, err
}

// Read is wrapped so that debug messages can be printed
func (r *Receiver) Read(b []byte) (int, error) {
	n, err := r.port.Read(b)
	if err == nil {
		r.debugLog("Port Read: % 02X", b)
	}
	return n, err
}

// ReadFull is wrapped so that debug messages can be printed
func (r *Receiver) ReadFull(b []byte) (int, error) {
	n, err := io.ReadFull(r.port, b)
	if err == nil {
		r.debugLog("Port ReadFull: % 02X", b)
	}
	return n, err
}

func (r *Receiver) debugLog(format string, a ...interface{}) {
	if r.Debug {
		fmt.Printf("    "+format+"\n", a...)
	}
}

// waitForMonitor blocks until the receiver has entered monitor mode.
// It can take a while for the receiver to boot into monitor mode, this
// function tests the state of the receiver by repeatedly sending an ENQ
// command. When the monitor is ready, it will send an ACK.
func (r *Receiver) waitForMonitor() error {
	fmt.Println("Waiting for monitor to become ready...")
	for i := 0; i < 30; i++ {
		if ok, err := r.EnqAckTest(); ok {
			return nil
		} else if err != nil {
			return err
		}
	}
	return fmt.Errorf("timed out waiting for monitor")
}

// ProbeReceiverState checks if the receiver is in normal mode or monitor mode.
// this is done by running command 0x82, which in normal mode is the screen dump
// command, which replies with a packet of type 0x82. In monitor mode, the
// command replies with a packet of type 0x92. If the receiver responds with
// a 0x92 packet, we know it's in monitor mode.
func (r *Receiver) ProbeReceiverState() (ReceiverState, error) {
	fmt.Println("Preforming dummy read to determine monitor state...")

	b := make([]byte, 5)

	binary.BigEndian.PutUint32(b[0:], 0x80000) // 'safe' dummy location to read from, I think it's the RAM base address
	b[4] = 2                                   // read 2 bytes

	reply, err := r.DoStxEtxCommand(MonCmdReadMem, b)
	if err != nil {
		return ReceiverUnresponsive, err
	}

	if reply.Command == MonCmdReadMemAck && reply.Status == 0 {
		fmt.Println("Receiver appears to be in monitor mode.")
		return ReceiverInMonitor, nil
	}

	fmt.Println("Receiver appears to be in normal mode.")
	return ReceiverNormal, nil
}

type ProtocolError struct {
	err string
}

func newProtocolError(format string, a ...interface{}) error {
	return &ProtocolError{err: fmt.Sprintf(format, a...)}
}

func (e *ProtocolError) Error() string {
	return e.err
}

func (r *Receiver) EnqAckTest() (bool, error) {
	buf := make([]byte, 1)
	r.Write([]byte{0x05}) // enq
	n, err := r.Read(buf)
	if err == nil && n > 0 && buf[0] == 0x06 { // ack
		return true, nil
	} else {
		if err != io.EOF {
			return false, err
		}
	}
	return false, nil
}

// DoAckCommand sends an STX/ETX framed command that expects a single byte reply. Sometimes
// it's not an ACK that's sent, so it's up to the caller to tell this function what it
// response it expects.
func (r *Receiver) DoAckCommand(command byte, data []byte) error {
	err := r.SendStxEtxRequest(command, data)
	if err != nil {
		return err
	}

	if r.Dummy {
		return nil
	}

	return r.ReadAckReply()
}

// ReadAckReply reads a single byte reply
func (r *Receiver) ReadAckReply() error {
	reply := make([]byte, 1)
	n, err := r.Read(reply)
	if err != nil {
		return err
	}

	if n != 1 || reply[0] != 0x06 { // ack
		return newProtocolError("ack not received")
	}

	return nil
}

type CommandReply struct {
	Status  byte
	Command byte
	Data    []byte
}

// DoStxEtxCommand sends an STX/ETX framed command that expects an STX/ETX framed reply
func (r *Receiver) DoStxEtxCommand(command byte, data []byte) (*CommandReply, error) {

	err := r.SendStxEtxRequest(command, data)
	if err != nil {
		return nil, err
	}

	reply, err := r.ReadStxEtxReply()
	if err != nil {
		return nil, err
	}

	return reply, nil
}

// SendStxEtxRequest sends an STX/ETX framed request
func (r *Receiver) SendStxEtxRequest(command byte, data []byte) error {

	payload := []byte{0, command, byte(len(data))}
	payload = append(payload, data...)

	var checksum byte
	for _, b := range payload {
		checksum += b
	}

	packet := append([]byte{0x02}, payload...)         // stx
	packet = append(packet, []byte{checksum, 0x03}...) // etx

	_, err := r.Write(packet)

	return err
}

// ReadStxEtxReply reads a STX/ETX framed reply
func (r *Receiver) ReadStxEtxReply() (*CommandReply, error) {
	packet := make([]byte, MAXPACKETLLEN) // maximum packet length

	_, err := r.ReadFull(packet[:4])
	if err != nil {
		return nil, err
	}

	if packet[0] != 0x02 { // stx
		fmt.Printf("expected STX received: %02X\n", packet[0])
		return nil, newProtocolError("expected STX received: %02X", packet[0])
	}

	status := packet[1]
	command := packet[2]
	length := int(packet[3])

	_, err = r.ReadFull(packet[4 : length+6]) // also read checksum and ETX

	packet = packet[:length+6] // trim to length

	checksum := packet[len(packet)-2]

	var computedChecksum byte
	for _, b := range packet[1 : len(packet)-2] {
		computedChecksum += b
	}

	if checksum != computedChecksum {
		return nil, newProtocolError("bad checksum. received: %02X expected: %02X", checksum, computedChecksum)
	}

	if packet[len(packet)-1] != 0x03 { // etx
		return nil, newProtocolError("expected ETX received: %02X", packet[len(packet)-1])
	}

	return &CommandReply{
		Status:  status,
		Command: command,
		Data:    packet[4 : len(packet)-2],
	}, nil

}