Add CAN implementation

Signed-off-by: didinst <9f6129a2ddff0d0c6d03169eaa75e3fea1c1770f>

can.go

@@ -1,13 +1,21 @@
 package main
 
 import (
+	"encoding/binary"
+	"errors"
 	"fmt"
+	"io"
+	"net"
 	"regexp"
 	"strconv"
+	"time"
+	"unsafe"
+
+	"golang.org/x/sys/unix"
 )
 
 type CanFrame struct {
-	canid   uint16
+	canid   uint32
 	payload []uint8
 	date    string
 }
@@ -21,7 +29,7 @@ var pattern = regexp.MustCompile(expr)
 func FromString(text *string) *CanFrame {
 	submatch := pattern.FindStringSubmatch(*text)
 	if len(submatch) == length {
-		canid, err := strconv.Atoi(submatch[2])
+		canid, err := strconv.ParseUint(submatch[2], 16, 0)
 		if err != nil {
 			return nil
 		}
@@ -39,10 +47,128 @@ func FromString(text *string) *CanFrame {
 
 		return &CanFrame{
 			date:    submatch[1],
-			canid:   uint16(canid),
+			canid:   uint32(canid),
 			payload: payload,
 		}
 	}
 
 	return nil
 }
+
+// New returns a new CAN bus socket.
+func NewCan() (*Socket, error) {
+	fd, err := unix.Socket(unix.AF_CAN, unix.SOCK_RAW, unix.CAN_RAW)
+	if err != nil {
+		return nil, err
+	}
+
+	return &Socket{dev: device{fd}}, nil
+}
+
+// Socket is a high-level representation of a CANBus socket.
+type Socket struct {
+	iface *net.Interface
+	addr  *unix.SockaddrCAN
+	dev   device
+}
+
+// Close closes the CAN bus socket.
+func (sck *Socket) Close() error {
+	return unix.Close(sck.dev.fd)
+}
+
+func (sck *Socket) Bind(addr string, filter []unix.CanFilter) error {
+	iface, err := net.InterfaceByName(addr)
+	if err != nil {
+		return err
+	}
+
+	sck.iface = iface
+	sck.addr = &unix.SockaddrCAN{Ifindex: sck.iface.Index}
+
+	//add filter
+	if filter != nil {
+		unix.SetsockoptCanRawFilter(sck.dev.fd, unix.SOL_CAN_RAW, unix.CAN_RAW_FILTER, filter)
+	}
+
+	return unix.Bind(sck.dev.fd, sck.addr)
+}
+
+// Recv receives data from the CAN socket.
+// id is the CAN_frame id the data was originated from.
+func (sck *Socket) Recv() (id uint32, data []byte, err error) {
+	var buf [frameSize]byte
+	n, err := io.ReadFull(sck.dev, buf[:])
+	if err != nil {
+		return id, data, err
+	}
+
+	if n != len(buf) {
+		return id, data, io.ErrUnexpectedEOF
+	}
+
+	id = binary.LittleEndian.Uint32(buf[:4])
+	id &= unix.CAN_SFF_MASK
+	data = make([]byte, buf[4])
+	copy(data, buf[8:])
+	return id, data, nil
+}
+
+type device struct {
+	fd int
+}
+
+func (d device) Read(data []byte) (int, error) {
+	return unix.Read(d.fd, data)
+}
+
+func (d device) Write(data []byte) (int, error) {
+	return unix.Write(d.fd, data)
+}
+
+const frameSize = unsafe.Sizeof(canframe{})
+
+// frame is a can_frame.
+type canframe struct {
+	ID   uint32
+	Len  byte
+	_    [3]byte
+	Data [8]byte
+}
+
+func StartCan(canbus string) (<-chan *CanFrame, error) {
+
+	var filter []unix.CanFilter = []unix.CanFilter{
+		{Id: CAN_ID_100, Mask: 0xFFF},
+		{Id: CAN_ID_101, Mask: 0xFFF},
+		{Id: CAN_ID_102, Mask: 0xFFF},
+	}
+
+	if socket, err := NewCan(); err == nil {
+
+		if cerr := socket.Bind(canbus, filter); cerr == nil {
+
+			canevents := make(chan *CanFrame)
+
+			go func() {
+				defer close(canevents)
+				defer socket.Close()
+
+				for { //TODO implement stop
+					if id, data, serr := socket.Recv(); serr == nil {
+						//log.Println("debug: data", data)
+
+						canevents <- &CanFrame{
+							date:    time.Now().String(),
+							canid:   id,
+							payload: data,
+						}
+					}
+				}
+			}()
+
+			return canevents, nil
+		}
+	}
+	return nil, errors.New("can't bind can socket")
+}

candump.go

@@ -10,6 +10,8 @@ import (
 
 func main() {
 	filename := flag.String("f", "", "Candump filename")
+	canbus := flag.String("c", "", "CAN bus interface")
+	all := flag.Bool("a", false, "Show all packets")
 	flag.Parse()
 
 	var frames <-chan *CanFrame
@@ -17,8 +19,17 @@ func main() {
 
 	switch {
 	case len(*filename) > 0:
-		fmt.Printf("Open %v\n", *filename)
+		fmt.Printf("Open file %v\n", *filename)
 		frames = readfile(filename)
+	case len(*canbus) > 0:
+		fmt.Printf("Open device %v\n", canbus)
+		var err error
+		frames, err = StartCan(*canbus)
+		if err != nil {
+			fmt.Println(err)
+			return
+		}
+
 	default:
 		flag.Usage()
 	}
@@ -37,7 +48,7 @@ func main() {
 		lastValue := last[eventType]
 		value, date := event.GetValue()
 
-		if lastValue != value {
+		if lastValue != value || *all {
 			fmt.Printf("%v | %s = %v\n", *date, eventType, value)
 			last[eventType] = value
 		}

chademo.go

@@ -5,9 +5,9 @@ import (
 )
 
 const (
-	CAN_ID_100 = 100
-	CAN_ID_101 = 101
-	CAN_ID_102 = 102
+	CAN_ID_100 = 256
+	CAN_ID_101 = 257
+	CAN_ID_102 = 258
 
 	DISABLED VehicleChargingEnabled = iota
 	ENABLED

go.mod

@@ -1,3 +1,5 @@
 module chademo-log
 
 go 1.18
+
+require golang.org/x/sys v0.0.0-20220722155257-8c9f86f7a55f