cal: verify signal encoding after writing

6 files changed, 412 insertions, 53 deletions

diff --git a/sw/cal/canbus/canbus.go b/sw/cal/canbus/canbus.go
new file mode 100644
index 0000000..031c456
--- /dev/null
+++ b/sw/cal/canbus/canbus.go
@@ -0,0 +1,140 @@
+package canbus
+
+import (
+	"context"
+	"net"
+	"strings"
+	"time"
+
+	"go.einride.tech/can"
+	"go.einride.tech/can/pkg/socketcan"
+)
+
+const timeout = 1 * time.Second
+
+type Bus struct {
+	conn        net.Conn
+	receiver    *socketcan.Receiver
+	transmitter *socketcan.Transmitter
+
+	tx <-chan txChan
+
+	rx    <-chan can.Frame
+	rxErr <-chan error
+
+	cancel context.CancelFunc
+}
+
+type txChan struct {
+	request chan<- transmission
+	err     <-chan error
+}
+
+type transmission struct {
+	context.Context
+	can.Frame
+}
+
+func Connect(dev string) (Bus, error) {
+	conn, err := socketcan.Dial("can", dev)
+	if err != nil {
+		return Bus{}, err
+	}
+
+	receiver := socketcan.NewReceiver(conn)
+	transmitter := socketcan.NewTransmitter(conn)
+	tx := make(chan txChan)
+	rx := make(chan can.Frame)
+	rxErr := make(chan error)
+	ctx, cancel := context.WithCancel(context.Background())
+
+	go transmit(ctx, transmitter, tx)
+	go receive(ctx, receiver, rx, rxErr)
+
+	return Bus{conn, receiver, transmitter, tx, rx, rxErr, cancel}, nil
+}
+
+func transmit(ctx context.Context, transmitter *socketcan.Transmitter, tx chan<- txChan) {
+	defer close(tx)
+
+	for {
+		reqc := make(chan transmission)
+		errc := make(chan error)
+		select {
+		case tx <- txChan{reqc, errc}:
+			req := <-reqc
+			errc <- req.transmit(transmitter)
+			close(reqc)
+			close(errc)
+		case <-ctx.Done():
+			close(reqc)
+			close(errc)
+			return
+		}
+	}
+}
+
+func (t transmission) transmit(transmitter *socketcan.Transmitter) error {
+	ctx, _ := context.WithTimeout(context.Background(), timeout)
+	for {
+		select {
+		case <-ctx.Done():
+			return ctx.Err()
+		default:
+		}
+		err := transmitter.TransmitFrame(t.Context, t.Frame)
+		if err == nil {
+			return nil // success
+		} else if strings.HasSuffix(err.Error(), "no buffer space available") {
+			continue // retry
+		} else {
+			return err
+		}
+	}
+}
+
+func receive(ctx context.Context, receiver *socketcan.Receiver, rx chan<- can.Frame, rxErr chan<- error) {
+	defer close(rx)
+	defer close(rxErr)
+
+	for receiver.Receive() {
+		frame := receiver.Frame()
+		select {
+		case rx <- frame:
+		case <-ctx.Done():
+			return
+		}
+	}
+	rxErr <- receiver.Err()
+}
+
+func (b Bus) Close() {
+	b.cancel()
+	b.transmitter.Close()
+	b.receiver.Close()
+	b.conn.Close()
+}
+
+func (b Bus) Send(ctx context.Context, frame can.Frame) error {
+	c := <-b.tx
+	c.request <- transmission{ctx, frame}
+	return <-c.err
+}
+
+func (b Bus) Receive(ctx context.Context) (can.Frame, error) {
+	select {
+	case frame := <-b.rx:
+		return frame, nil
+	case <-ctx.Done():
+		return can.Frame{}, ctx.Err()
+	}
+}
+
+func (b Bus) TryReceive() (can.Frame, bool) {
+	select {
+	case frame := <-b.rx:
+		return frame, true
+	default:
+		return can.Frame{}, false
+	}
+}
diff --git a/sw/cal/dbc.go b/sw/cal/dbc.go
index f746504..0a22a14 100644
--- a/sw/cal/dbc.go
+++ b/sw/cal/dbc.go
@@ -8,27 +8,8 @@ import (
 	"go.einride.tech/can/pkg/dbc"
 )
 
-type SignalDef struct {
-	id          dbc.MessageID
-	name        string
-	start, size uint64
-	isBigEndian bool
-	isSigned    bool
-}
-
-func newSignalDef(msg *dbc.MessageDef, sig dbc.SignalDef) SignalDef {
-	return SignalDef{
-		msg.MessageID,
-		string(sig.Name),
-		sig.StartBit,
-		sig.Size,
-		sig.IsBigEndian,
-		sig.IsSigned,
-	}
-}
-
 // Extract signals from the DBC file.
-func parseSignals(filename string, names map[int]string) (map[int]SignalDef, error) {
+func parseSignals(filename string, names map[uint8]string) ([]SignalDef, error) {
 	// Parse DBC file
 	msgDefs, err := parseDbcFile(filename)
 	if err != nil {
@@ -36,26 +17,27 @@ func parseSignals(filename string, names map[int]string) (map[int]SignalDef, err
 	}
 
 	// Search for signals
-	signals := make(map[int]SignalDef)
+	signals := make([]SignalDef, 0, len(names))
 	for _, msg := range msgDefs {
 		for k, name := range names {
 			i := slices.IndexFunc(msg.Signals, func(sig dbc.SignalDef) bool { return sig.Name == dbc.Identifier(name) })
 			if i < 0 {
 				continue
 			}
-			if _, ok := signals[k]; ok {
-				return nil, ErrDupSig{msg.Signals[i]}
+			fmt.Printf("Found signal %v\n", msg.Signals[i])
+			sig, err := NewSignalDef(k, msg, msg.Signals[i])
+			if err != nil {
+				return nil, err
 			}
-			fmt.Println(msg.Signals[i])
-			signals[k] = newSignalDef(msg, msg.Signals[i])
+			signals = append(signals, sig)
 		}
 	}
 
 	// Check all signals are present
 	if len(signals) != len(names) {
-		for k, name := range names {
-			if _, ok := signals[k]; !ok {
-				return nil, ErrNoSig{filename, name}
+		for i := 0; i < len(signals); i++ {
+			if names[uint8(i)] != signals[i].name {
+				return nil, ErrNoSig{filename, names[uint8(i)]}
 			}
 		}
 	}
diff --git a/sw/cal/err.go b/sw/cal/err.go
index 26ef5df..4ceb1cc 100644
--- a/sw/cal/err.go
+++ b/sw/cal/err.go
@@ -1,12 +1,17 @@
 package main
 
 import (
+	"errors"
 	"fmt"
 	"os"
 
 	"go.einride.tech/can/pkg/dbc"
 )
 
+var (
+	errWrongId = errors.New("wrong ID")
+)
+
 func eprintf(format string, a ...any) {
 	weprintf(format, a...)
 	os.Exit(1)
diff --git a/sw/cal/main.go b/sw/cal/main.go
index bfd0f11..503324f 100644
--- a/sw/cal/main.go
+++ b/sw/cal/main.go
@@ -3,10 +3,22 @@ package main
 import (
 	"flag"
 	"fmt"
+	"math"
 	"os"
+	"time"
 
 	"go.einride.tech/can/pkg/dbc"
-	"go.einride.tech/can/pkg/socketcan"
+
+	"git.samanthony.xyz/can_gauge_interface/sw/cal/canbus"
+)
+
+const (
+	stdMask = 0x7FF
+	extMask = 0x1FFFFFFF
+
+	timeout          = 1 * time.Second
+	eepromWriteDelay = 5 * time.Millisecond
+	maxRetries       = 8
 )
 
 type Signals struct {
@@ -68,31 +80,33 @@ func main() {
 
 	// Open CAN connection
 	fmt.Println("Opening connection to", *canDev)
-	conn, err := socketcan.Dial("can", *canDev)
+	bus, err := canbus.Connect(*canDev)
 	if err != nil {
 		eprintf("%v\n", err)
 	}
-	defer conn.Close()
-	tx := socketcan.NewTransmitter(conn)
-	defer tx.Close()
+	defer bus.Close()
 
 	// Parse DBC file and transmit encoding of each signal
-	if err := sendEncodings(*dbcFilename, sigNames, tx); err != nil {
+	if err := sendEncodings(*dbcFilename, sigNames, bus); err != nil {
 		eprintf("%v\n", err)
 	}
 
 	// Parse tables and transmit them
-	if err := sendTables(tblFilenames, tx); err != nil {
+	if err := sendTables(tblFilenames, bus); err != nil {
 		eprintf("%v\n", err)
 	}
 }
 
 // Return a map of non-empty strings keyed by their index in the given list.
-func nonEmpty(ss ...string) map[int]string {
-	m := make(map[int]string)
+func nonEmpty(ss ...string) map[uint8]string {
+	if len(ss) > math.MaxUint8 {
+		panic(len(ss))
+	}
+
+	m := make(map[uint8]string)
 	for i := range ss {
 		if ss[i] != "" {
-			m[i] = ss[i]
+			m[uint8(i)] = ss[i]
 		}
 	}
 	return m
@@ -113,19 +127,18 @@ func checkTablesProvided() error {
 }
 
 // Parse DBC file and transmit encoding of each signal using Signal Control frames.
-func sendEncodings(dbcFilename string, sigNames map[int]string, tx *socketcan.Transmitter) error {
+func sendEncodings(dbcFilename string, sigNames map[uint8]string, bus canbus.Bus) error {
 	// Parse DBC file
 	fmt.Println("Parsing", dbcFilename)
-	sigDefs, err := parseSignals(dbcFilename, sigNames)
+	sigs, err := parseSignals(dbcFilename, sigNames)
 	if err != nil {
 		return err
 	}
 
 	// Transmit Signal Control frames
-	for k, sigDef := range sigDefs {
-		fmt.Printf("Transmitting encoding of signal %d: %s\n", k, sigDef.name)
-		fmt.Println(sigDef)
-		if err := sendEncoding(sigDef, k, tx); err != nil {
+	for _, sig := range sigs {
+		fmt.Println("Sending signal encoding", sig)
+		if err := sig.SendEncoding(bus); err != nil {
 			return err
 		}
 	}
@@ -134,7 +147,7 @@ func sendEncodings(dbcFilename string, sigNames map[int]string, tx *socketcan.Tr
 }
 
 // Parse each table and transmit them using Table Control frames.
-func sendTables(tblFilenames map[int]string, tx *socketcan.Transmitter) error {
+func sendTables(tblFilenames map[uint8]string, bus canbus.Bus) error {
 	for k, filename := range tblFilenames {
 		fmt.Println("Parsing", filename)
 		tbl, err := parseTable(filename)
@@ -143,7 +156,7 @@ func sendTables(tblFilenames map[int]string, tx *socketcan.Transmitter) error {
 		}
 
 		fmt.Printf("Transmitting table %d: %s\n", k, filename)
-		if err := sendTable(tx, tbl, k); err != nil {
+		if err := tbl.Send(bus); err != nil {
 			return err
 		}
 	}
diff --git a/sw/cal/signal.go b/sw/cal/signal.go
new file mode 100644
index 0000000..6591c50
--- /dev/null
+++ b/sw/cal/signal.go
@@ -0,0 +1,155 @@
+package main
+
+import (
+	"context"
+	bin "encoding/binary"
+	"fmt"
+	"math"
+
+	"go.einride.tech/can"
+	"go.einride.tech/can/pkg/dbc"
+
+	"git.samanthony.xyz/can_gauge_interface/sw/cal/canbus"
+)
+
+const (
+	sigCtrlId   = 0x1272100
+	sigCtrlMask = 0x1FFFF00
+
+	exide = 1 << 31
+)
+
+type SignalDef struct {
+	index       uint8
+	id          uint32
+	isExtended  bool
+	name        string
+	start, size uint8
+	isBigEndian bool
+	isSigned    bool
+}
+
+func NewSignalDef(index uint8, msg *dbc.MessageDef, sig dbc.SignalDef) (SignalDef, error) {
+	if sig.StartBit > math.MaxUint8 {
+		return SignalDef{}, fmt.Errorf("%s: start bit out of range: %d", sig.Name, sig.StartBit)
+	}
+	if sig.Size > math.MaxUint8 {
+		return SignalDef{}, fmt.Errorf("%s: size out of range: %d", sig.Name, sig.Size)
+	}
+
+	return SignalDef{
+		index,
+		uint32(msg.MessageID) & extMask,
+		msg.MessageID.IsExtended(),
+		string(sig.Name),
+		uint8(sig.StartBit),
+		uint8(sig.Size),
+		sig.IsBigEndian,
+		sig.IsSigned,
+	}, nil
+}
+
+// Transmit a signal's encoding in a Signal Control frame so the Interface can store it in its EEPROM.
+func (sig SignalDef) SendEncoding(bus canbus.Bus) error {
+	frame, err := sig.MarshalFrame()
+	if err != nil {
+		return err
+	}
+
+	var retry int
+	for retry = 0; retry < maxRetries; retry++ {
+		// Write to EEPROM
+		ctx, _ := context.WithTimeout(context.Background(), timeout)
+		if err := bus.Send(ctx, frame); err != nil {
+			return err
+		}
+
+		// Read back
+		request := sigCtrlRequest(sig.index)
+		ctx, _ = context.WithTimeout(context.Background(), timeout)
+		if err := bus.Send(ctx, request); err != nil {
+			return err
+		}
+		ctx, _ = context.WithTimeout(context.Background(), timeout)
+		reply, err := bus.Receive(ctx)
+		if err != nil {
+			return err
+		}
+		var rsig SignalDef
+		err = rsig.UnmarshalFrame(reply)
+		if err == errWrongId {
+			fmt.Println("Wrong ID", reply, "; expected", sigCtrlId)
+			continue
+		} else if err != nil {
+			return err
+		}
+
+		// Verify
+		rsig.index = sig.index
+		rsig.name = sig.name
+		fmt.Println("Received signal encoding", rsig)
+		if rsig == sig {
+			fmt.Printf("Signal %d encoding OK\n", sig.index)
+			return nil // success
+		} else {
+			weprintf("Warning: signal %d verification failed; rewriting...\n", sig.index)
+			continue
+		}
+	}
+	// Max retries exceeded
+	return fmt.Errorf("signal %d verification failed", sig.index)
+}
+
+// Construct a Signal Control REMOTE REQUEST frame.
+func sigCtrlRequest(index uint8) can.Frame {
+	return can.Frame{
+		ID:         sigCtrlId | uint32(index&0xF),
+		IsRemote:   true,
+		IsExtended: true,
+	}
+}
+
+func (sig SignalDef) MarshalFrame() (can.Frame, error) {
+	var data [8]byte
+	if _, err := bin.Encode(data[0:4], bin.BigEndian, uint32(sig.id)&extMask); err != nil {
+		return can.Frame{}, err
+	}
+	if sig.isExtended {
+		data[0] |= 0x80 // EXIDE
+	}
+	data[4] = uint8(sig.start)
+	data[5] = uint8(sig.size)
+	if !sig.isBigEndian {
+		data[6] |= 0x80
+	}
+	if sig.isSigned {
+		data[6] |= 0x40
+	}
+	return can.Frame{
+		ID:         sigCtrlId | uint32(sig.index&0xF),
+		Length:     7,
+		Data:       data,
+		IsExtended: true,
+	}, nil
+}
+
+func (sig *SignalDef) UnmarshalFrame(frame can.Frame) error {
+	if !frame.IsExtended || frame.ID&sigCtrlMask != sigCtrlId {
+		return errWrongId
+	}
+	if frame.Length != 7 {
+		return fmt.Errorf("Signal Control frame has wrong DLC: %d", frame.Length)
+	}
+	sig.index = uint8(frame.ID & 0xF)
+	var id uint32
+	if _, err := bin.Decode(frame.Data[0:4], bin.BigEndian, &id); err != nil {
+		return err
+	}
+	sig.id = id & extMask
+	sig.isExtended = id&exide != 0
+	sig.start = frame.Data[4]
+	sig.size = frame.Data[5]
+	sig.isBigEndian = frame.Data[6]&0x80 == 0
+	sig.isSigned = frame.Data[6]&0x40 != 0
+	return nil
+}
diff --git a/sw/cal/table.go b/sw/cal/table.go
index 20d21d0..610448a 100644
--- a/sw/cal/table.go
+++ b/sw/cal/table.go
@@ -1,28 +1,92 @@
 package main
 
 import (
+	"cmp"
+	"context"
+	bin "encoding/binary"
 	"fmt"
 	"slices"
+	"time"
+
+	"go.einride.tech/can"
+
+	"git.samanthony.xyz/can_gauge_interface/sw/cal/canbus"
 )
 
-const tabRows = 32
+const (
+	tblCtrlId  uint32 = 0x1272000
+	maxTabRows        = 32
+)
 
 type Table struct {
-	keys []int32
-	vals []uint16
+	sigIndex uint8
+	rows     []Row
+}
+
+type Row struct {
+	key int32
+	val uint16
 }
 
 func (t *Table) Insert(key int32, val uint16) error {
-	if len(t.keys) >= tabRows {
+	if len(t.rows) >= maxTabRows {
 		return fmt.Errorf("too many rows")
 	}
 
-	i, ok := slices.BinarySearch(t.keys, key)
+	i, ok := slices.BinarySearchFunc(t.rows, key, cmpRowKey)
 	if ok {
 		return ErrDupKey{key}
 	}
-	t.keys = slices.Insert(t.keys, i, key)
-	t.vals = slices.Insert(t.vals, i, val)
+	t.rows = slices.Insert(t.rows, i, Row{key, val})
+
+	return nil
+}
+
+func cmpRowKey(row Row, key int32) int {
+	return cmp.Compare(row.key, key)
+}
+
+// Transmit a table in Table Control frames so the Interface can store it in its EEPROM.
+func (tbl Table) Send(bus canbus.Bus) error {
+	// Send populated rows
+	var i int
+	for i = 0; i < len(tbl.rows); i++ {
+		if err := sendRow(tbl.sigIndex, uint8(i), tbl.rows[i], bus); err != nil {
+			return err
+		}
+		time.Sleep(eepromWriteDelay)
+	}
+
+	// Fill rest of table with last row
+	lastRow := tbl.rows[len(tbl.rows)-1]
+	for ; i < maxTabRows; i++ {
+		if err := sendRow(tbl.sigIndex, uint8(i), lastRow, bus); err != nil {
+			return err
+		}
+		time.Sleep(eepromWriteDelay)
+	}
 
 	return nil
 }
+
+// Transmit a Table Control frame containing one row of a table.
+func sendRow(sigIndex, rowIndex uint8, row Row, bus canbus.Bus) error {
+	// Serialize DATA FIELD
+	var data [8]byte
+	if _, err := bin.Encode(data[0:4], bin.BigEndian, row.key); err != nil {
+		return err
+	}
+	if _, err := bin.Encode(data[4:6], bin.BigEndian, row.val); err != nil {
+		return err
+	}
+
+	// Construct ID and send frame
+	frame := can.Frame{
+		ID:         uint32(tblCtrlId) | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F),
+		Length:     6,
+		Data:       data,
+		IsExtended: true,
+	}
+	ctx, _ := context.WithTimeout(context.Background(), timeout)
+	return bus.Send(ctx, frame)
+}