1 files changed, 80 insertions, 13 deletions

diff --git a/sw/cal/table.go b/sw/cal/table.go
index 610448a..7d63d8d 100644
--- a/sw/cal/table.go
+++ b/sw/cal/table.go
@@ -6,7 +6,6 @@ import (
 	bin "encoding/binary"
 	"fmt"
 	"slices"
-	"time"
 
 	"go.einride.tech/can"
 
@@ -14,8 +13,10 @@ import (
 )
 
 const (
-	tblCtrlId  uint32 = 0x1272000
-	maxTabRows        = 32
+	tblCtrlId   uint32 = 0x1272000
+	tblCtrlMask uint32 = 0x1FFFF00
+
+	maxTabRows = 32
 )
 
 type Table struct {
@@ -54,7 +55,6 @@ func (tbl Table) Send(bus canbus.Bus) error {
 		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
@@ -63,7 +63,6 @@ func (tbl Table) Send(bus canbus.Bus) error {
 		if err := sendRow(tbl.sigIndex, uint8(i), lastRow, bus); err != nil {
 			return err
 		}
-		time.Sleep(eepromWriteDelay)
 	}
 
 	return nil
@@ -71,22 +70,90 @@ 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 {
-	// Serialize DATA FIELD
+	frame, err := marshalTblCtrlFrame(sigIndex, rowIndex, row)
+	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 := 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)
+}
+
+func marshalTblCtrlFrame(sigIndex, rowIndex uint8, row Row) (can.Frame, error) {
 	var data [8]byte
 	if _, err := bin.Encode(data[0:4], bin.BigEndian, row.key); err != nil {
-		return err
+		return can.Frame{}, err
 	}
 	if _, err := bin.Encode(data[4:6], bin.BigEndian, row.val); err != nil {
-		return err
+		return can.Frame{}, err
 	}
-
-	// Construct ID and send frame
-	frame := can.Frame{
+	return can.Frame{
 		ID:         uint32(tblCtrlId) | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F),
 		Length:     6,
 		Data:       data,
 		IsExtended: true,
+	}, nil
+}
+
+func unmarshalTblCtrlFrame(frame can.Frame) (row Row, sigIndex, rowIndex uint8, err error) {
+	if !frame.IsExtended || frame.ID&tblCtrlMask != tblCtrlId {
+		err = errWrongId
+		return
+	}
+	if frame.Length != 6 {
+		err = fmt.Errorf("Table Control frame has wrong DLC: %d", frame.Length)
+		return
+	}
+	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
+	}
+	_, err = bin.Decode(frame.Data[4:6], bin.BigEndian, &row.val)
+	return
+}
+
+// Construct a Table Control REMOTE REQUEST frame.
+func tblCtrlRequest(sigIndex, rowIndex uint8) can.Frame {
+	return can.Frame{
+		ID:         tblCtrlId | uint32((sigIndex<<5)&0xE0) | uint32(rowIndex&0x1F),
+		IsRemote:   true,
+		IsExtended: true,
 	}
-	ctx, _ := context.WithTimeout(context.Background(), timeout)
-	return bus.Send(ctx, frame)
 }