FMS-read-GO.md

↓

fms-read-recvfrom-filt.go

package main

import (
	"fmt"
	"math"
	"net"
	"syscall"
	"time"
	"unsafe"

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

//goland:noinspection GoSnakeCaseUsage
const (
	SOL_CAN_J1939   = unix.SOL_CAN_BASE + unix.CAN_J1939
	SO_J1939_FILTER = 1
)

type J1939Filter struct {
	Name     uint64
	NameMask uint64
	PGN      uint32
	PGNMask  uint32
	Addr     uint8
	AddrMask uint8
}

func SetsockoptJ1939Filter(fd, level, opt int, filter []J1939Filter) error {
	var p unsafe.Pointer
	if len(filter) > 0 {
		p = unsafe.Pointer(&filter[0])
	}
	_, _, err := unix.Syscall6(unix.SYS_SETSOCKOPT, uintptr(fd), uintptr(level), uintptr(opt), uintptr(p), uintptr(len(filter)*32), 0)
	switch err {
	case 0:
		return nil
	case unix.EAGAIN:
		return syscall.EAGAIN
	case unix.EINVAL:
		return syscall.EINVAL
	case unix.ENOENT:
		return syscall.ENOENT
	}
	return err
}

func main() {
	sock := must(unix.Socket(unix.AF_CAN, unix.SOCK_DGRAM, unix.CAN_J1939))
	throw(unix.SetsockoptInt(sock, unix.SOL_SOCKET, unix.SO_BROADCAST, 1))
	throw(SetsockoptJ1939Filter(sock, SOL_CAN_J1939, SO_J1939_FILTER, []J1939Filter{
		{PGN: 0x0FE6C, PGNMask: 0x3FFFF},
	}))

	iface := must(net.InterfaceByName("can0"))
	addr := &unix.SockaddrCANJ1939{
		Ifindex: iface.Index,
		Name:    0x0,     // J1939_NO_NAME
		PGN:     0x40000, // J1939_NO_PGN
		Addr:    0xff,    // J1939_NO_ADDR
	}
	throw(unix.Bind(sock, addr))

	buf := make([]byte, 1024)
	for {
		n, from, err := unix.Recvfrom(sock, buf, 0)
		if err == unix.EINTR {
			continue
		}
		throw(err)
		handle(from.(*unix.SockaddrCANJ1939).PGN, buf[:n])
	}
}

func handle(pgn uint32, data []byte) {
	fmt.Printf("%#v %#v\n", pgn, data)
	//log("Horas", fms.EngineTotalHoursOfOperation(pgn, data), "hr")
	//log("Peso", fms.GrossCombinationVehicleWeight(pgn, data), "kg")
	//log("Combustible", fms.FuelLevel1(pgn, data), "%")
	//log("Velocidad", fms.TachographVehicleSpeed(pgn, data), "km/h")
}

func throw(err error) {
	if err != nil {
		panic(err)
	}
}

func must[T any](val T, err error) T {
	throw(err)
	return val
}

func log(name string, value float64, unit string) {
	if !math.IsNaN(value) {
		fmt.Println(time.Now().Format(time.RFC3339), name, value, unit)
	}
}

fms-read-recvfrom.go

package main

import (
	"fmt"
	"net"

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

func main() {
	sock := must(unix.Socket(unix.AF_CAN, unix.SOCK_DGRAM, unix.CAN_J1939))
	throw(unix.SetsockoptInt(sock, unix.SOL_SOCKET, unix.SO_BROADCAST, 1))

	iface := must(net.InterfaceByName("can0"))
	addr := &unix.SockaddrCANJ1939{
		Ifindex: iface.Index,
		Name:    0x0,     // J1939_NO_NAME
		PGN:     0x40000, // J1939_NO_PGN
		Addr:    0xff,    // J1939_NO_ADDR
	}
	throw(unix.Bind(sock, addr))

	buf := make([]byte, 1024)
	for {
		n, from, err := unix.Recvfrom(sock, buf, 0)
		if err == unix.EINTR {
			continue
		}
		throw(err)
		handle(from.(*unix.SockaddrCANJ1939).PGN, buf[:n])
	}
}

func handle(pgn uint32, data []byte) {
	fmt.Printf("@%04x #%02x\n", pgn, data)
}

func throw(err error) {
	if err != nil {
		panic(err)
	}
}

func must[T any](val T, err error) T {
	throw(err)
	return val
}

fms-read-recvmsg.go

package main

import (
	"bytes"
	"encoding/binary"
	"fmt"
	"net"
	"time"
	"unsafe"

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

func main() {
	sock := must(unix.Socket(unix.AF_CAN, unix.SOCK_DGRAM, unix.CAN_J1939))
	throw(unix.SetsockoptInt(sock, unix.SOL_SOCKET, unix.SO_BROADCAST, 1))
	throw(unix.SetsockoptInt(sock, unix.SOL_SOCKET, unix.SO_TIMESTAMP, 1))
	throw(unix.SetsockoptInt(sock, unix.SOL_SOCKET, unix.SO_RCVBUF, 1024))

	iface := must(net.InterfaceByName("can0"))
	addr := &unix.SockaddrCANJ1939{
		Ifindex: iface.Index,
		Name:    0x0,     // J1939_NO_NAME
		PGN:     0x40000, // J1939_NO_PGN
		Addr:    0xff,    // J1939_NO_ADDR
	}
	throw(unix.Bind(sock, addr))

	var ooblen = 0 +
		unix.CmsgSpace(int(unsafe.Sizeof(unix.Timeval{}))) + // timestamp
		unix.CmsgSpace(int(unsafe.Sizeof(uint8(0)))) + // dest addr
		unix.CmsgSpace(int(unsafe.Sizeof(uint64(0)))) + // dest name
		unix.CmsgSpace(int(unsafe.Sizeof(uint8(0)))) // priority

	p := make([]byte, 1024)
	oob := make([]byte, ooblen)
	for {
		n, oobn, _, from, err := unix.Recvmsg(sock, p, oob, 0)
		if err == unix.EINTR {
			continue
		}
		throw(err)

		var recvflags int
		var dst = &unix.SockaddrCANJ1939{}
		var priority uint8
		var tdut time.Time
		msgs := must(unix.ParseSocketControlMessage(oob[:oobn]))
		for _, msg := range msgs {
			switch msg.Header.Level {
			case unix.SOL_SOCKET:
				if msg.Header.Type == unix.SCM_TIMESTAMP {
					tv := unix.Timeval{}
					throw(binary.Read(bytes.NewReader(msg.Data), binary.LittleEndian, &tv))
					tdut = time.Unix(tv.Unix())
					recvflags |= 1 << msg.Header.Type
				}
			case unix.SOL_CAN_BASE + unix.CAN_J1939: // SOL_CAN_J1939
				recvflags |= 1 << msg.Header.Type
				switch msg.Header.Type {
				case 1: // SCM_J1939_DEST_ADDR
					dst.Addr = msg.Data[0]
				case 2: // SCM_J1939_DEST_NAME
					panic("NAMEEEEEEEEEEEEEE")
					// dst.Name = binary.BigEndian.Uint64(msg.Data)
					// TODO: memcpy(&dst_name, CMSG_DATA(cmsg), cmsg->cmsg_len - CMSG_LEN(0));
				case 3: // SCM_J1939_PRIO
					priority = msg.Data[0]
				}
			}
		}

		src := from.(*unix.SockaddrCANJ1939)
		handle(src.Addr, dst.Addr, priority, src.PGN, tdut, p[:n])
	}
}

func handle(src, dst, pri uint8, pgn uint32, when time.Time, data []byte) {
	if dst == 0 {
		dst = 0xFF
	}
	fmt.Printf("+%02x >%02x !%d @%04x #%02x\n", src, dst, pri, pgn, data)
}

func throw(err error) {
	if err != nil {
		panic(err)
	}
}

func must[T any](val T, err error) T {
	throw(err)
	return val
}