1 files changed, 60 insertions, 0 deletions
diff --git a/sw/cal/can.go b/sw/cal/can.go
new file mode 100644
index 0000000..c1397b0
--- /dev/null
+++ b/sw/cal/can.go
@@ -0,0 +1,60 @@
+package main
+
+import (
+	"context"
+	"time"
+
+	"go.einride.tech/can"
+
+	"git.samanthony.xyz/can_gauge_interface/sw/cal/canbus"
+)
+
+const (
+	stdMask = 0x7FF
+	extMask = 0x1FFFFFFF
+
+	timeout    = 1 * time.Second
+	maxRetries = 8
+)
+
+func sendCtrlFrame[C can.FrameMarshaler, R can.FrameUnmarshaler](cmd C, req can.FrameMarshaler, reply R, bus canbus.Bus, isReply func(R) bool, verify func(C, R) bool) error {
+	cmdFrame, err := cmd.MarshalFrame()
+	if err != nil {
+		return err
+	}
+
+	for retry := 0; retry < maxRetries; retry++ {
+		// Write
+		ctx, _ := context.WithTimeout(context.Background(), timeout)
+		if err := bus.Send(ctx, cmdFrame); err != nil {
+			return err
+		}
+
+		// Read back
+		reqFrame, err := req.MarshalFrame()
+		if err != nil {
+			return err
+		}
+		ctx, _ = context.WithTimeout(context.Background(), timeout)
+		if err := bus.Send(ctx, reqFrame); err != nil {
+			return err
+		}
+		ctx, _ = context.WithTimeout(context.Background(), timeout)
+		replyFrame, err := bus.Receive(ctx)
+		if err != nil {
+			return err
+		}
+		err = reply.UnmarshalFrame(replyFrame)
+		if err == errWrongId || !isReply(reply) {
+			continue
+		} else if err != nil {
+			return err
+		}
+
+		// Verify
+		if verify(cmd, reply) {
+			return nil
+		}
+	}
+	return errVerifyFail
+}