Initial commit

can/can.go

@@ -0,0 +1,141 @@
+package can
+
+import (
+	"encoding/binary"
+	"errors"
+	"io"
+	"net"
+	"time"
+	"unsafe"
+
+	"golang.org/x/sys/unix"
+)
+
+const CAN_ID_071 = 0x071
+const CAN_ID_073 = 0x073
+const CAN_ID_021 = 0x021
+const CAN_ID_022 = 0x022
+
+type CanFrame struct {
+	CanId   uint32
+	Payload []uint8
+	Date    string
+}
+
+// NewCan 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])
+	//TODO make correct switch betwin EFF and SFF
+	id &= unix.CAN_EFF_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_071, Mask: 0xFFF},
+		{Id: CAN_ID_073, Mask: 0xFFF},
+		{Id: CAN_ID_021, Mask: 0xFFF},
+		{Id: CAN_ID_022, 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 {
+						canevents <- &CanFrame{
+							Date:    time.Now().String(),
+							CanId:   id,
+							Payload: data,
+						}
+					}
+				}
+			}()
+
+			return canevents, nil
+		}
+	}
+	return nil, errors.New("can't bind can socket")
+}

can/input.go

@@ -0,0 +1,96 @@
+package can
+
+import (
+	"bufio"
+	"fmt"
+	"os"
+	"regexp"
+	"strconv"
+)
+
+// (2022-07-08 16:54:15.587099)  can0  100   [8]  00 00 00 00 00 00 64 00
+// (1670578900.771868) can0 00004021#00000000FCFFFFFF
+const EXPR_HR = "\\((.*)\\)\\s+can\\d+\\s+(\\S+)\\s+\\[\\d\\]\\s+([A-F0-9]+)\\s+([A-F0-9]+)\\s+([A-F0-9]+)\\s+([A-F0-9]+)\\s+([A-F0-9]+)\\s+([A-F0-9]+)\\s+([A-F0-9]+)\\s+([A-F0-9]+)"
+const EXPR_M = "\\((.*)\\)\\s+can\\d+\\s+(\\S+)#([A-F0-9]{2})([A-F0-9]{2})([A-F0-9]{2})([A-F0-9]{2})([A-F0-9]{2})([A-F0-9]{2})([A-F0-9]{2})([A-F0-9]{2})"
+const length = 11
+
+var patternHr = regexp.MustCompile(EXPR_HR)
+var patternM = regexp.MustCompile(EXPR_M)
+
+func Readfile(filename *string) <-chan *CanFrame {
+	c := make(chan *CanFrame)
+
+	go func() {
+		defer close(c)
+
+		file, err := os.Open(*filename)
+		if err != nil {
+			fmt.Println(err)
+			os.Exit(1)
+		}
+		defer file.Close()
+
+		scanner := bufio.NewScanner(file)
+		process(scanner, c)
+	}()
+
+	return c
+}
+
+func process(scanner *bufio.Scanner, ch chan<- *CanFrame) {
+	for scanner.Scan() {
+		text := scanner.Text()
+		if frame := fromString(&text); frame != nil {
+			ch <- frame
+		}
+	}
+
+	if err := scanner.Err(); err != nil {
+		fmt.Println(err)
+	}
+}
+
+func ReadStdin() <-chan *CanFrame {
+	c := make(chan *CanFrame)
+
+	go func() {
+		scanner := bufio.NewScanner(os.Stdin)
+		process(scanner, c)
+	}()
+	return c
+}
+
+func fromString(text *string) *CanFrame {
+	var submatch []string
+	if submatch = patternHr.FindStringSubmatch(*text); submatch == nil {
+		if submatch = patternM.FindStringSubmatch(*text); submatch == nil {
+			return nil
+		}
+	}
+
+	if len(submatch) == length {
+		canid, err := strconv.ParseUint(submatch[2], 16, 0)
+		if err != nil {
+			return nil
+		}
+
+		payload := make([]uint8, 8)
+		for idx, octet := range submatch[3:] {
+			val, err := strconv.ParseUint(octet, 16, 0)
+			if err != nil {
+				fmt.Println(err)
+				continue
+			}
+
+			payload[idx] = uint8(val)
+		}
+
+		return &CanFrame{
+			Date:    submatch[1],
+			CanId:   uint32(canid),
+			Payload: payload,
+		}
+	}
+
+	return nil
+}