cal: refactor

1 files changed, 91 insertions, 68 deletions

diff --git a/sw/cal/table.go b/sw/cal/table.go
index 7d63d8d..3848bef 100644
--- a/sw/cal/table.go
+++ b/sw/cal/table.go
@@ -2,10 +2,13 @@ package main
 
 import (
 	"cmp"
-	"context"
 	bin "encoding/binary"
+	"encoding/csv"
 	"fmt"
+	"io"
+	"os"
 	"slices"
+	"strconv"
 
 	"go.einride.tech/can"
 
@@ -25,20 +28,66 @@ type Table struct {
 }
 
 type Row struct {
-	key int32
-	val uint16
+	sigIndex, rowIndex uint8
+	key                int32
+	val                uint16
 }
 
-func (t *Table) Insert(key int32, val uint16) error {
-	if len(t.rows) >= maxTabRows {
+func parseTable(filename string, sigIndex uint8) (Table, error) {
+	f, err := os.Open(filename)
+	if err != nil {
+		eprintf("%v\n", err)
+	}
+	defer f.Close()
+
+	tbl := Table{sigIndex, nil}
+	rdr := csv.NewReader(f)
+	for {
+		err := parseRow(rdr, &tbl)
+		if err == io.EOF {
+			return tbl, nil
+		} else if err != nil {
+			return Table{}, fmt.Errorf("%s:%v", filename, err)
+		}
+	}
+}
+
+func parseRow(rdr *csv.Reader, tbl *Table) error {
+	row, err := rdr.Read()
+	if err != nil {
+		return err
+	}
+	if len(row) != 2 {
+		line, _ := rdr.FieldPos(0)
+		return fmt.Errorf("%d: malformed row", line)
+	}
+	key, err := strconv.ParseInt(row[0], 10, 32)
+	if err != nil {
+		line, col := rdr.FieldPos(0)
+		return fmt.Errorf("%d:%d: %v", line, col, err)
+	}
+	val, err := strconv.ParseUint(row[1], 10, 16)
+	if err != nil {
+		line, col := rdr.FieldPos(1)
+		return fmt.Errorf("%d:%d: %v", line, col, err)
+	}
+	if err := tbl.Insert(int32(key), uint16(val)); err != nil {
+		line, col := rdr.FieldPos(0)
+		return fmt.Errorf("%d:%d: %v", line, col, err)
+	}
+	return nil
+}
+
+func (tbl *Table) Insert(key int32, val uint16) error {
+	if len(tbl.rows) >= maxTabRows {
 		return fmt.Errorf("too many rows")
 	}
 
-	i, ok := slices.BinarySearchFunc(t.rows, key, cmpRowKey)
+	i, ok := slices.BinarySearchFunc(tbl.rows, key, cmpRowKey)
 	if ok {
 		return ErrDupKey{key}
 	}
-	t.rows = slices.Insert(t.rows, i, Row{key, val})
+	tbl.rows = slices.Insert(tbl.rows, i, Row{tbl.sigIndex, uint8(i), key, val})
 
 	return nil
 }
@@ -52,7 +101,7 @@ 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 {
+		if err := tbl.rows[i].Send(bus); err != nil {
 			return err
 		}
 	}
@@ -60,7 +109,8 @@ func (tbl Table) Send(bus canbus.Bus) error {
 	// 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 {
+		lastRow.rowIndex = uint8(i)
+		if err := lastRow.Send(bus); err != nil {
 			return err
 		}
 	}
@@ -69,52 +119,22 @@ func (tbl Table) Send(bus canbus.Bus) error {
 }
 
 // Transmit a Table Control frame containing one row of a table.
-func sendRow(sigIndex, rowIndex uint8, row Row, bus canbus.Bus) error {
-	frame, err := marshalTblCtrlFrame(sigIndex, rowIndex, row)
-	if err != nil {
-		return err
+func (row Row) Send(bus canbus.Bus) error {
+	req := TableControlRequest{row.sigIndex, row.rowIndex}
+	reply := &Row{}
+	isReply := func(reply *Row) bool {
+		return reply.sigIndex == row.sigIndex && reply.rowIndex == row.rowIndex
 	}
+	return sendCtrlFrame(row, req, reply, bus, isReply, verifyTblCtrlReply)
+}
 
-	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 := tblCtrlRequest(sigIndex, rowIndex)
-		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
-		}
-		rRow, rSigIndex, rRowIndex, err := unmarshalTblCtrlFrame(reply)
-		if err == errWrongId || rSigIndex != sigIndex || rRowIndex != rowIndex {
-			continue
-		} else if err != nil {
-			return err
-		}
-
-		// Verify
-		if rRow == row {
-			fmt.Printf("Table %d row %d OK\n", sigIndex, rowIndex)
-			return nil // success
-		} else {
-			weprintf("Warning: table %d row %d verification failed; rewriting...\n", sigIndex, rowIndex)
-			continue
-		}
-	}
-	// Max retries exceeded
-	return fmt.Errorf("table %d row %d verification failed", sigIndex, rowIndex)
+// Verify that the response to a Table Control REMOTE REQUEST
+// is the same as what was commanded to be written.
+func verifyTblCtrlReply(cmd Row, reply *Row) bool {
+	return *reply == cmd
 }
 
-func marshalTblCtrlFrame(sigIndex, rowIndex uint8, row Row) (can.Frame, error) {
+func (row Row) MarshalFrame() (can.Frame, error) {
 	var data [8]byte
 	if _, err := bin.Encode(data[0:4], bin.BigEndian, row.key); err != nil {
 		return can.Frame{}, err
@@ -123,37 +143,40 @@ func marshalTblCtrlFrame(sigIndex, rowIndex uint8, row Row) (can.Frame, error) {
 		return can.Frame{}, err
 	}
 	return can.Frame{
-		ID:         uint32(tblCtrlId) | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F),
+		ID:         uint32(tblCtrlId) | uint32((row.sigIndex<<5)&0xE0) | uint32(row.rowIndex&0x1F),
 		Length:     6,
 		Data:       data,
 		IsExtended: true,
 	}, nil
 }
 
-func unmarshalTblCtrlFrame(frame can.Frame) (row Row, sigIndex, rowIndex uint8, err error) {
+func (row *Row) UnmarshalFrame(frame can.Frame) error {
 	if !frame.IsExtended || frame.ID&tblCtrlMask != tblCtrlId {
-		err = errWrongId
-		return
+		return errWrongId
 	}
 	if frame.Length != 6 {
-		err = fmt.Errorf("Table Control frame has wrong DLC: %d", frame.Length)
-		return
+		return fmt.Errorf("wrong DLC for Table Control frame: %d", frame.Length)
 	}
-	sigIndex = uint8((frame.ID & 0xE0) >> 5)
-	rowIndex = uint8(frame.ID & 0x1F)
-	_, err = bin.Decode(frame.Data[0:4], bin.BigEndian, &row.key)
-	if err != nil {
-		return
+	row.sigIndex = uint8((frame.ID & 0xE0) >> 5)
+	row.rowIndex = uint8(frame.ID & 0x1F)
+	if _, err := bin.Decode(frame.Data[0:4], bin.BigEndian, &row.key); err != nil {
+		return err
+	}
+	if _, err := bin.Decode(frame.Data[4:6], bin.BigEndian, &row.val); err != nil {
+		return err
 	}
-	_, err = bin.Decode(frame.Data[4:6], bin.BigEndian, &row.val)
-	return
+	return nil
+}
+
+// TableControlRequest is a Table Control REMOTE REQUEST frame.
+type TableControlRequest struct {
+	sigIndex, rowIndex uint8
 }
 
-// Construct a Table Control REMOTE REQUEST frame.
-func tblCtrlRequest(sigIndex, rowIndex uint8) can.Frame {
+func (r TableControlRequest) MarshalFrame() (can.Frame, error) {
 	return can.Frame{
-		ID:         tblCtrlId | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F),
+		ID:         tblCtrlId | uint32((r.sigIndex<<5)&0xE0) | uint32(r.rowIndex&0x1F),
 		IsRemote:   true,
 		IsExtended: true,
-	}
+	}, nil
 }