From ba288f29591dc0cd510e5d435d4a94ff20589ed7 Mon Sep 17 00:00:00 2001 From: saji Date: Sun, 16 Apr 2023 02:46:41 -0500 Subject: [PATCH] finish socketcan rework, make xbee helpers --- can/frame.go | 20 ++- socketcan/socketcan.go | 198 +++++++++++----------- socketcan/socketcan_test.go | 55 +++++- xbee/api_frame.go | 71 ++++++++ xbee/{frame_test.go => api_frame_test.go} | 0 xbee/frame.go | 19 --- 6 files changed, 238 insertions(+), 125 deletions(-) create mode 100644 xbee/api_frame.go rename xbee/{frame_test.go => api_frame_test.go} (100%) delete mode 100644 xbee/frame.go diff --git a/can/frame.go b/can/frame.go index 7135811..1cf3698 100644 --- a/can/frame.go +++ b/can/frame.go @@ -1,8 +1,8 @@ package can type Frame struct { - ID uint32 - Data []uint8 + Id uint32 + Data []byte Kind Kind } @@ -10,16 +10,20 @@ type Frame struct { type Kind uint8 const ( - SFF Kind = iota // Standard Frame Format - EFF // Extended Frame - RTR // remote transmission requests - ERR // Error frame. + SFF Kind = iota // Standard ID Frame + EFF // Extended ID Frame + RTR // Remote Transmission Request Frame + ERR // Error Frame ) -// for routing flexibility +type CanFilter struct { + Id uint32 + Mask uint32 + Inverted bool +} type CanSink interface { - Send(Frame) error + Send(*Frame) error } type CanSource interface { diff --git a/socketcan/socketcan.go b/socketcan/socketcan.go index 342380f..19a739a 100644 --- a/socketcan/socketcan.go +++ b/socketcan/socketcan.go @@ -1,7 +1,6 @@ package socketcan import ( - "bytes" "encoding/binary" "errors" "fmt" @@ -11,86 +10,20 @@ import ( "golang.org/x/sys/unix" ) -// this file implements a simple wrapper around linux socketCAN +// A CanSocket is a CAN device that uses the socketCAN linux drivers to write to real +// CAN hardware. type CanSocket struct { iface *net.Interface addr *unix.SockaddrCAN fd int } -//internal frame structure for socketcan with padding - -type stdFrame struct { - ID uint32 - Len uint8 - //lint:ignore U1000 these are to make serialization easier - _pad, _res1 uint8 // padding - Dlc uint8 - Data [8]uint8 -} - -func socketCanMarshal(f can.Frame) (*bytes.Buffer, error) { - - if len(f.Data) > 8 && f.Kind == can.SFF { - return nil, errors.New("data too large for std frame") - } - if len(f.Data) > 64 && f.Kind == can.EFF { - return nil, errors.New("data too large for extended frame") - } - - var idflags uint32 = f.ID - if f.Kind == can.EFF { - idflags = idflags | unix.CAN_EFF_FLAG - } else if f.Kind == can.RTR { - idflags = idflags | unix.CAN_RTR_FLAG - } else if f.Kind == can.ERR { - idflags = idflags | unix.CAN_ERR_FLAG - } - - var d [8]uint8 - - for i := 0; i < len(f.Data); i++ { - d[i] = f.Data[i] - } - - var unixFrame stdFrame = stdFrame{ - ID: idflags, Len: uint8(len(f.Data)), - Data: d, - } - - // finally, write our bytes buffer. - buf := new(bytes.Buffer) - err := binary.Write(buf, binary.LittleEndian, unixFrame) - if err != nil { - return nil, err - } - - return buf, nil -} - -func Unmarshal(f *Frame, buf *bytes.Buffer) error { - - return nil -} - -// helper function to make a filter. -// id and mask are straightforward, if inverted is true, the filter -// will reject anything that matches. -func MakeFilter(id, mask uint32, inverted bool) *unix.CanFilter { - f := &unix.CanFilter{Id: id, Mask: mask} - - if inverted { - f.Id = f.Id | unix.CAN_INV_FILTER - } - return f -} - const standardFrameSize = unix.CAN_MTU -// the hack. -const extendedFrameSize = unix.CAN_MTU + 56 +// we use the base CAN_MTU since the FD MTU is not in sys/unix. but we know it's +64-8 bytes +const fdFrameSize = unix.CAN_MTU + 56 -// Constructs a new CanSocket and creates a file descriptor for it. +// Constructs a new CanSocket and binds it to the interface given by ifname func NewCanSocket(ifname string) (*CanSocket, error) { var sck CanSocket @@ -119,12 +52,12 @@ func NewCanSocket(ifname string) (*CanSocket, error) { return &sck, nil } -// close the socket file descriptor, freeing it from the system. +// Closes the socket. func (sck *CanSocket) Close() error { return unix.Close(sck.fd) } -// get the name of the socket, or nil if it hasn't been bound yet. +// get the name of the socket. func (sck *CanSocket) Name() string { return sck.iface.Name } @@ -139,33 +72,86 @@ func (sck *CanSocket) SetErrFilter(shouldFilter bool) error { } err = unix.SetsockoptInt(sck.fd, unix.SOL_CAN_RAW, unix.CAN_RAW_ERR_FILTER, errmask) - if err != nil { - return err - } - return nil + + return err } -// set the filters for the can device. -func (sck *CanSocket) SetFilters(filters []unix.CanFilter) error { - return unix.SetsockoptCanRawFilter(sck.fd, unix.SOL_CAN_RAW, unix.CAN_RAW_FILTER, filters) +// SetFDMode enables or disables the transmission of CAN FD packets. +func (sck *CanSocket) SetFDMode(enable bool) error { + var val int + if enable { + val = 1 + } else { + val = 0 + } + + err := unix.SetsockoptInt(sck.fd, unix.SOL_CAN_RAW, unix.CAN_RAW_FD_FRAMES, val) + + return err } -func (sck *CanSocket) Send(msg can.Frame) error { - // convert our abstract frame into a real unix frame and then push it. - // check return value to raise errors. - buf, err := socketCanMarshal(msg) +// SetFilters will set the socketCAN filters based on a standard CAN filter list. +func (sck *CanSocket) SetFilters(filters []can.CanFilter) error { - if err != nil { - return fmt.Errorf("error sending frame: %w", err) + // helper function to make a filter. + // id and mask are straightforward, if inverted is true, the filter + // will reject anything that matches. + makeFilter := func(filter can.CanFilter) unix.CanFilter { + f := unix.CanFilter{Id: filter.Id, Mask: filter.Mask} + + if filter.Inverted { + f.Id = f.Id | unix.CAN_INV_FILTER + } + return f } - if buf.Len() != unix.CAN_MTU { - return fmt.Errorf("socket send: buffer size mismatch %d", buf.Len()) + convertedFilters := make([]unix.CanFilter, len(filters)) + for i, filt := range filters { + convertedFilters[i] = makeFilter(filt) } + return unix.SetsockoptCanRawFilter(sck.fd, unix.SOL_CAN_RAW, unix.CAN_RAW_FILTER, convertedFilters) + +} + +func (sck *CanSocket) Send(msg *can.Frame) error { + + buf := make([]byte, fdFrameSize) + + idToWrite := msg.Id + + switch msg.Kind { + case can.SFF: + idToWrite &= unix.CAN_SFF_MASK + case can.EFF: + idToWrite &= unix.CAN_EFF_MASK + idToWrite |= unix.CAN_EFF_FLAG + case can.RTR: + idToWrite |= unix.CAN_RTR_FLAG + default: + return errors.New("you can't send error frames") + } + + binary.LittleEndian.PutUint32(buf[:4], idToWrite) + + // write the length, it's one byte, so do it directly. + payloadLength := len(msg.Data) + buf[4] = byte(payloadLength) + + if payloadLength > 64 { + return fmt.Errorf("payload too large: %d", payloadLength) + } + + // copy in the data now. + copy(buf[8:], msg.Data) + // send the buffer using unix syscalls! - - err = unix.Send(sck.fd, buf.Bytes(), 0) + var err error + if payloadLength > 8 { + err = unix.Send(sck.fd, buf, 0) + } else { + err = unix.Send(sck.fd, buf[:standardFrameSize], 0) + } if err != nil { return fmt.Errorf("error sending frame: %w", err) } @@ -176,10 +162,30 @@ func (sck *CanSocket) Send(msg can.Frame) error { func (sck *CanSocket) Recv() (*can.Frame, error) { // todo: support extended frames. - buf := make([]byte, standardFrameSize) - unix.Read(sck.fd, buf) + buf := make([]byte, fdFrameSize) + _, err := unix.Read(sck.fd, buf) + if err != nil { + return nil, err + } + + id := binary.LittleEndian.Uint32(buf[0:4]) + + var k can.Kind + if id&unix.CAN_EFF_FLAG != 0 { + // extended id frame + k = can.EFF + } else { + // it's a normal can frame + k = can.SFF + } + + dataLength := uint8(buf[4]) + + result := &can.Frame{ + Id: id & unix.CAN_EFF_MASK, + Kind: k, + Data: buf[8 : dataLength+8], + } + return result, nil - stdF := &stdFrame{} - binary.Read(bytes.NewBuffer(buf), binary.LittleEndian, stdF) - return nil, errors.New("not implemented") } diff --git a/socketcan/socketcan_test.go b/socketcan/socketcan_test.go index 46cc066..c381ac6 100644 --- a/socketcan/socketcan_test.go +++ b/socketcan/socketcan_test.go @@ -1,8 +1,11 @@ package socketcan import ( + "bytes" + "net" "testing" + "github.com/kschamplin/gotelem/can" "golang.org/x/sys/unix" ) @@ -10,7 +13,7 @@ func TestMakeFilter(t *testing.T) { t.Run("non-invert", func(t *testing.T) { - filter := MakeFilter(0x123, 0x11, false) + filter := can.CanFilter{Id: 0x123, Mask: 0x11, Inverted: true} if filter.Id != 0x123 { t.Errorf("expected %d, got %d", 0x123, filter.Id) @@ -21,7 +24,7 @@ func TestMakeFilter(t *testing.T) { }) t.Run("invert", func(t *testing.T) { - filter := MakeFilter(0x123, 0x11, true) + filter := can.CanFilter{Id: 0x123, Mask: 0x11, Inverted: true} if filter.Id != 0x123|unix.CAN_INV_FILTER { t.Errorf("expected %d, got %d", 0x123|unix.CAN_INV_FILTER, filter.Id) @@ -34,6 +37,10 @@ func TestMakeFilter(t *testing.T) { func TestCanSocket(t *testing.T) { + if _, err := net.InterfaceByName("vcan0"); err != nil { + t.Skipf("missing vcan0, skipping socket tests: %v", err) + } + t.Run("test construction and destruction", func(t *testing.T) { sock, err := NewCanSocket("vcan0") if err != nil { @@ -49,10 +56,54 @@ func TestCanSocket(t *testing.T) { t.Run("test name", func(t *testing.T) { sock, _ := NewCanSocket("vcan0") + defer sock.Close() if sock.Name() != "vcan0" { t.Errorf("incorrect interface name: got %s, expected %s", sock.Name(), "vcan0") } }) + t.Run("test sending can 2.0 packet", func(t *testing.T) { + sock, _ := NewCanSocket("vcan0") + defer sock.Close() + + // make a packet. + testFrame := &can.Frame{ + Id: 0x123, + Kind: can.SFF, + Data: []byte{0, 1, 2, 3, 4, 5, 6, 7}, + } + err := sock.Send(testFrame) + + if err != nil { + t.Error(err) + } + }) + + t.Run("test receiving a can 2.0 packet", func(t *testing.T) { + sock, _ := NewCanSocket("vcan0") + rsock, _ := NewCanSocket("vcan0") + defer sock.Close() + defer rsock.Close() + + testFrame := &can.Frame{ + Id: 0x234, + Kind: can.SFF, + Data: []byte{0, 1, 2, 3, 4, 5, 6, 7}, + } + _ = sock.Send(testFrame) + + rpkt, err := rsock.Recv() + if err != nil { + t.Error(err) + } + if len(rpkt.Data) != 8 { + t.Errorf("length mismatch: got %d expected 8", len(rpkt.Data)) + } + if !bytes.Equal(testFrame.Data, rpkt.Data) { + t.Error("data corrupted") + } + + }) + } diff --git a/xbee/api_frame.go b/xbee/api_frame.go new file mode 100644 index 0000000..8d6d27a --- /dev/null +++ b/xbee/api_frame.go @@ -0,0 +1,71 @@ +package xbee + +import "encoding/binary" + +// the frames have an outer shell - we will make a function that takes +// an inner frame element and wraps it in the appropriate headers. + +// first, we should make it take the frame directly, so we make an interface +// that represents "framable" things. note that bytes.Buffer also fulfils this. + +type Frameable interface { + Bytes() []byte +} + +// now we can describe our function that takes a framable and contains it + calculates checksums. +func calculateChecksum(data []byte) byte { + var sum byte + for _, v := range data { + sum += v + } + return 0xFF - sum +} +func makeXbeeApiFrame(cmd Frameable) ([]byte, error) { + dataBuf := cmd.Bytes() + frameBuf := make([]byte, len(dataBuf)+4) + + // move data and construct the frame + + frameBuf[0] = 0x7E // start delimiter + + // length + // todo: check endiannes (0x7e, msb lsb) + binary.LittleEndian.PutUint16(frameBuf[1:3], uint16(len(dataBuf))) + + copy(frameBuf[3:], dataBuf) + + chksum := calculateChecksum(dataBuf) + + frameBuf[len(frameBuf)-1] = chksum + + return frameBuf, nil +} + +// now we can describe frames in other files that implement Frameable. this makes trasmission complete. +// the remaining challenge is reception and actual API frames. +// xbee uses the first byte of the "frame data" as the API identifier or command. + +//go:generate stringer -output=api_frame_cmd.go -type xbeeCmd +type xbeeCmd byte + +const ( + // commands sent to the xbee s3b + + ATCmd xbeeCmd = 0x08 // AT Command + ATCmdQueuePVal xbeeCmd = 0x09 // AT Command - Queue Parameter Value + TxReq xbeeCmd = 0x10 // TX Request + TxReqExpl xbeeCmd = 0x11 // Explicit TX Request + RemoteCmdReq xbeeCmd = 0x17 // Remote Command Request + // commands recieved from the xbee + + ATCmdResponse xbeeCmd = 0x88 // AT Command Response + ModemStatus xbeeCmd = 0x8A // Modem Status + TxStatus xbeeCmd = 0x8B // Transmit Status + RouteInfoPkt xbeeCmd = 0x8D // Route information packet + AddrUpdate xbeeCmd = 0x8E // Aggregate Addressing Update + RxPkt xbeeCmd = 0x90 // RX Indicator (AO=0) + RxPktExpl xbeeCmd = 0x91 // Explicit RX Indicator (AO=1) + IOSample xbeeCmd = 0x92 // Data Sample RX Indicator + NodeId xbeeCmd = 0x95 // Note Identification Indicator + RemoteCmdResp xbeeCmd = 0x97 // Remote Command Response +) diff --git a/xbee/frame_test.go b/xbee/api_frame_test.go similarity index 100% rename from xbee/frame_test.go rename to xbee/api_frame_test.go diff --git a/xbee/frame.go b/xbee/frame.go deleted file mode 100644 index 4204e4d..0000000 --- a/xbee/frame.go +++ /dev/null @@ -1,19 +0,0 @@ -package xbee - -const ( - // commands sent to the xbee s3b - ATCmd = 0x08 - ATCmdQueueParameterValue = 0x09 - TxReq = 0x10 - TxReqExpl = 0x11 - RemoteCmdReq = 0x17 - // commands recieved from the xbee - ATCmdResponse = 0x88 - ModemStatus = 0x8A - TxStatus = 0x8B - RouteInfoPkt = 0x8D - AddrUpdate = 0x8E - RxPkt = 0x90 - RxPktExpl = 0x91 - IOSample = 0x92 -)