You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
141 lines
2.7 KiB
141 lines
2.7 KiB
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")
|
|
}
|
|
|