chademo-log/can.go

package main

import (
	"encoding/binary"
	"errors"
	"fmt"
	"io"
	"net"
	"regexp"
	"strconv"
	"time"
	"unsafe"

	"golang.org/x/sys/unix"
)

type CanFrame struct {
	canid   uint32
	payload []uint8
	date    string
}

//  (2022-07-08 16:54:15.587099)  can0  100   [8]  00 00 00 00 00 00 64 00
const expr = "\\((.*)\\)\\s+can\\d+\\s+(\\d+)\\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 length = 11

var pattern = regexp.MustCompile(expr)

func FromString(text *string) *CanFrame {
	submatch := pattern.FindStringSubmatch(*text)
	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
}

// 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")
}