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.
160 lines
3.1 KiB
160 lines
3.1 KiB
package can
|
|
|
|
import (
|
|
"encoding/binary"
|
|
"errors"
|
|
"io"
|
|
"net"
|
|
"time"
|
|
"unsafe"
|
|
|
|
"golang.org/x/sys/unix"
|
|
)
|
|
|
|
const bufferSize = 100
|
|
|
|
type CanFrame struct {
|
|
CanId uint32
|
|
Payload *[]uint8
|
|
Date *time.Time
|
|
}
|
|
|
|
// 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)
|
|
}
|
|
|
|
// Receive receives data from the CAN socket.
|
|
// id is the CAN_frame id the data was originated from.
|
|
func (sck *Socket) Receive() (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
|
|
payload := make([]byte, buf[4])
|
|
copy(payload, buf[8:])
|
|
return id, &payload, nil
|
|
}
|
|
|
|
// Send sends the provided frame on the CAN bus.
|
|
func (sck *Socket) Send(msg *CanFrame) (int, error) {
|
|
var payload = *msg.Payload
|
|
if len(payload) > 8 {
|
|
return 0, errors.New("payload too big")
|
|
}
|
|
|
|
//TODO make correct switch betwin EFF and SFF
|
|
var id = msg.CanId
|
|
id &= unix.CAN_EFF_MASK
|
|
id |= unix.CAN_EFF_FLAG
|
|
|
|
var frame [frameSize]byte
|
|
binary.LittleEndian.PutUint32(frame[:4], id)
|
|
frame[4] = byte(len(payload))
|
|
copy(frame[8:], payload)
|
|
|
|
return sck.dev.Write(frame[:])
|
|
}
|
|
|
|
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(fromBus chan<- *CanFrame, toBus <-chan *CanFrame, canbus string, filter []unix.CanFilter) error {
|
|
|
|
if socket, err := NewCan(); err == nil {
|
|
|
|
if cerr := socket.Bind(canbus, filter); cerr == nil {
|
|
|
|
//canevents := make(chan *CanFrame, bufferSize)
|
|
|
|
go func() {
|
|
//defer close(fromBus)
|
|
defer socket.Close()
|
|
|
|
for { //TODO implement stop
|
|
if id, data, serr := socket.Receive(); serr == nil {
|
|
var now = time.Now()
|
|
fromBus <- &CanFrame{
|
|
Date: &now,
|
|
CanId: id,
|
|
Payload: data,
|
|
}
|
|
}
|
|
}
|
|
}()
|
|
|
|
go func() {
|
|
for frame := range toBus {
|
|
if _, err = socket.Send(frame); err != nil {
|
|
return
|
|
}
|
|
}
|
|
}()
|
|
|
|
return err
|
|
}
|
|
}
|
|
return errors.New("can't bind can socket")
|
|
}
|
|
|