From e1b8a2ba5b4c017da191367f68b6dacb267f4123 Mon Sep 17 00:00:00 2001 From: Samuel Lee <54152208+samsamfire@users.noreply.github.com> Date: Mon, 9 Dec 2024 23:36:11 +0100 Subject: [PATCH 1/6] Finish removing logrus (#18) * Changed : renamed to NewNodeProcessor for consistency with type * Refactored : continue changes to remove logrus Changed : removed MainCallback for now * Refactored : add slog to fileobject * Changed : removed logrus from http gateway & added slog Changed : minor coherence refactor for naming * Fixed : error in gateway example, should now work + slog * Changed : removed logrus from CAN drivers, replaced by slog * Changed : removed logrus from SDO client * Fixed : forgot to add logger to virtual can * Removed : logrus in commented server test http * Changed : bus manager doesn't use logrus Changed : .gitignore remove created file during testing Changed : removed last references to logrus & run log tidy --- .gitignore | 3 +- README.md | Bin 8452 -> 8468 bytes bus_manager.go | 9 +- examples/http/main.go | 25 +- examples/master/main.go | 12 +- examples/test/main.go | 8 +- go.mod | 1 - go.sum | 6 - pkg/can/kvaser/kvaser.go | 7 +- pkg/can/virtual/virtual.go | 11 +- pkg/gateway/gateway.go | 14 +- pkg/gateway/http/client.go | 18 +- pkg/gateway/http/handlers.go | 103 ++- pkg/gateway/http/parser.go | 54 -- pkg/gateway/http/server.go | 67 +- pkg/gateway/http/server_test.go | 5 - pkg/network/network.go | 4 +- pkg/node/local.go | 2 +- pkg/node/node.go | 2 +- pkg/node/remote.go | 2 +- pkg/sdo/client.go | 1038 +++++++++++++++++-------------- 21 files changed, 765 insertions(+), 626 deletions(-) diff --git a/.gitignore b/.gitignore index 5d6195b..ef06423 100644 --- a/.gitignore +++ b/.gitignore @@ -4,4 +4,5 @@ __pycache__/ *.vscode node_modules *.code-workspace -site \ No newline at end of file +site +examples/test/example.bin diff --git a/README.md b/README.md index 84b5694e393f2695e0184072b030bdf6b46a27e7..e517a98d23cb2b451760475e505e9c5f9763a438 100644 GIT binary patch delta 401 zcmZXQ%}T>i5QWcLaMe{;&8oCT@&G|MB?SvX7y1AyNuxAPBtIq~l8tYm58`X}E&T2c zF%2?Y=FXfs=bpKr>+kiCH){(e$`s4dj+~RbI@XI)`mWA(ri~gs>Y!K9=wv3cjDlXN z3e-^zuhcWQ72$(CfyA~l+L7Kk3AksJzoyQl?O*KFJY9kIzFgffUu*HNb5FnFt!td6 z3axg|T*joUJCKljMZQ39e!?F!ZhefG*v^nC^seB~q&G4$^oO-Rv?ReGq*R-^r8IYC({Y zgq(Z%^4;6;I=m08p2QX&Y=ns6^FCsM74|%RozLEt1Sg2>SuwM&(MSaXsN@L2t_W_q z3mw#8V62IP%oS-7@1-U#u*Oc&Ao*T1zmTRpY`y|)xIEGoS28QiLMCVKkRA`KWd=C# z|CpWSE+?y3o9RD0N82gGoZQERR7u~0-~Loa%$9WZOVxZjmopb`KZFjxh$)9pILa5F I1Ljcc1yW>55&!@I diff --git a/bus_manager.go b/bus_manager.go index e84ef33..0c7ee90 100644 --- a/bus_manager.go +++ b/bus_manager.go @@ -1,14 +1,14 @@ package canopen import ( + "log/slog" "sync" - - log "github.com/sirupsen/logrus" ) // Bus manager is a wrapper around the CAN bus interface // Used by the CANopen stack to control errors, callbacks for specific IDs, etc. type BusManager struct { + logger *slog.Logger mu sync.Mutex bus Bus // Bus interface that can be adapted frameListeners map[uint32][]FrameListener @@ -48,7 +48,7 @@ func (bm *BusManager) Bus() Bus { func (bm *BusManager) Send(frame Frame) error { err := bm.bus.Send(frame) if err != nil { - log.Warnf("[CAN] %v", err) + bm.logger.Warn("error sending frame", "err", err) } return err } @@ -78,7 +78,7 @@ func (bm *BusManager) Subscribe(ident uint32, mask uint32, rtr bool, callback Fr // Iterate over all callbacks and verify that we are not adding the same one twice for _, cb := range bm.frameListeners[ident] { if cb == callback { - log.Warnf("[CAN] callback for frame id %x already added", ident) + bm.logger.Warn("callback for frame already present", "id", ident) return nil } } @@ -96,6 +96,7 @@ func (bm *BusManager) Error() uint16 { func NewBusManager(bus Bus) *BusManager { bm := &BusManager{ bus: bus, + logger: slog.Default(), frameListeners: make(map[uint32][]FrameListener), canError: 0, } diff --git a/examples/http/main.go b/examples/http/main.go index 77f51b6..d1733a8 100644 --- a/examples/http/main.go +++ b/examples/http/main.go @@ -3,34 +3,33 @@ package main import ( "flag" "fmt" + "log/slog" + "os" "github.com/samsamfire/gocanopen/pkg/gateway/http" "github.com/samsamfire/gocanopen/pkg/network" - log "github.com/sirupsen/logrus" ) -var DEFAULT_NODE_ID = 0x20 -var DEFAULT_CAN_INTERFACE = "vcan0" -var DEFAULT_HTTP_PORT = 8090 - const ( - INIT = 0 - RUNNING = 1 - RESETING = 2 + NodeId = 0x20 + Interface = "vcan0" + Port = 8090 ) func main() { - log.SetLevel(log.DebugLevel) + + logger := slog.New(slog.NewTextHandler(os.Stdout, &slog.HandlerOptions{Level: slog.LevelDebug})) // Command line arguments - channel := flag.String("i", DEFAULT_CAN_INTERFACE, "socketcan channel e.g. can0,vcan0") + channel := flag.String("i", Interface, "socketcan channel e.g. can0,vcan0") flag.Parse() network := network.NewNetwork(nil) - e := network.Connect("", *channel, 500000) + network.SetLogger(logger) + e := network.Connect("socketcan", *channel, 500000) if e != nil { panic(e) } - gateway := http.NewGatewayServer(&network, 1, 1, 1000) - gateway.ListenAndServe(fmt.Sprintf(":%d", DEFAULT_HTTP_PORT)) + gateway := http.NewGatewayServer(&network, logger, 1, 1, 1000) + gateway.ListenAndServe(fmt.Sprintf(":%d", Port)) } diff --git a/examples/master/main.go b/examples/master/main.go index ddacfa8..a961325 100644 --- a/examples/master/main.go +++ b/examples/master/main.go @@ -2,8 +2,10 @@ package main import ( + "log/slog" + "os" + "github.com/samsamfire/gocanopen/pkg/network" - log "github.com/sirupsen/logrus" ) var DEFAULT_NODE_ID = uint8(0x20) @@ -12,9 +14,11 @@ var DEFAULT_CAN_BITRATE = 500_000 var EDS_PATH = "../../testdata/base.eds" func main() { - log.SetLevel(log.DebugLevel) + logger := slog.New(slog.NewTextHandler(os.Stdout, &slog.HandlerOptions{Level: slog.LevelDebug})) network := network.NewNetwork(nil) + network.SetLogger(logger) + err := network.Connect("socketcan", DEFAULT_CAN_INTERFACE, DEFAULT_CAN_BITRATE) if err != nil { panic(err) @@ -30,11 +34,11 @@ func main() { // Read values via SDO val, err := node.ReadUint("UNSIGNED32 value", "") if err == nil { - log.Infof("read : %v", val) + logger.Info("read", "value", val) } // Or write values via SDO err = node.Write("UNSIGNED64 value", "", uint64(10)) if err != nil { - log.Info("failed to write", err) + logger.Info("failed to write", "err", err) } } diff --git a/examples/test/main.go b/examples/test/main.go index 78ab057..39d40d0 100644 --- a/examples/test/main.go +++ b/examples/test/main.go @@ -11,7 +11,6 @@ import ( "github.com/samsamfire/gocanopen/pkg/network" "github.com/samsamfire/gocanopen/pkg/od" - log "github.com/sirupsen/logrus" ) var DEFAULT_NODE_ID = 0x10 @@ -24,19 +23,20 @@ const ( ) func main() { - log.SetLevel(log.DebugLevel) go func() { http.ListenAndServe("localhost:6060", nil) }() + logger := slog.New(slog.NewTextHandler(os.Stdout, &slog.HandlerOptions{Level: slog.LevelDebug})) network := network.NewNetwork(nil) + network.SetLogger(logger) + err := network.Connect("virtualcan", "127.0.0.1:18889", 500000) if err != nil { panic(err) } - logger := slog.New(slog.NewTextHandler(os.Stdout, &slog.HandlerOptions{Level: slog.LevelDebug})) - network.SetLogger(logger) + // Load node EDS, this will be used to generate all the CANopen objects // Basic template can be found in the current directory node, err := network.CreateLocalNode(uint8(DEFAULT_NODE_ID), od.Default()) diff --git a/go.mod b/go.mod index 50f5b82..a0d7c4c 100644 --- a/go.mod +++ b/go.mod @@ -3,7 +3,6 @@ module github.com/samsamfire/gocanopen go 1.22 require ( - github.com/sirupsen/logrus v1.9.0 github.com/stretchr/testify v1.8.4 golang.org/x/sys v0.0.0-20220715151400-c0bba94af5f8 gopkg.in/ini.v1 v1.67.0 diff --git a/go.sum b/go.sum index 0338b80..e7c289b 100644 --- a/go.sum +++ b/go.sum @@ -1,12 +1,7 @@ -github.com/davecgh/go-spew v1.1.0/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c= github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= -github.com/sirupsen/logrus v1.9.0 h1:trlNQbNUG3OdDrDil03MCb1H2o9nJ1x4/5LYw7byDE0= -github.com/sirupsen/logrus v1.9.0/go.mod h1:naHLuLoDiP4jHNo9R0sCBMtWGeIprob74mVsIT4qYEQ= -github.com/stretchr/objx v0.1.0/go.mod h1:HFkY916IF+rwdDfMAkV7OtwuqBVzrE8GR6GFx+wExME= -github.com/stretchr/testify v1.7.0/go.mod h1:6Fq8oRcR53rry900zMqJjRRixrwX3KX962/h/Wwjteg= github.com/stretchr/testify v1.8.4 h1:CcVxjf3Q8PM0mHUKJCdn+eZZtm5yQwehR5yeSVQQcUk= github.com/stretchr/testify v1.8.4/go.mod h1:sz/lmYIOXD/1dqDmKjjqLyZ2RngseejIcXlSw2iwfAo= golang.org/x/sys v0.0.0-20220715151400-c0bba94af5f8 h1:0A+M6Uqn+Eje4kHMK80dtF3JCXC4ykBgQG4Fe06QRhQ= @@ -15,6 +10,5 @@ gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405 h1:yhCVgyC4o1eVCa2tZl7eS0r+ gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0= gopkg.in/ini.v1 v1.67.0 h1:Dgnx+6+nfE+IfzjUEISNeydPJh9AXNNsWbGP9KzCsOA= gopkg.in/ini.v1 v1.67.0/go.mod h1:pNLf8WUiyNEtQjuu5G5vTm06TEv9tsIgeAvK8hOrP4k= -gopkg.in/yaml.v3 v3.0.0-20200313102051-9f266ea9e77c/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= gopkg.in/yaml.v3 v3.0.1 h1:fxVm/GzAzEWqLHuvctI91KS9hhNmmWOoWu0XTYJS7CA= gopkg.in/yaml.v3 v3.0.1/go.mod h1:K4uyk7z7BCEPqu6E+C64Yfv1cQ7kz7rIZviUmN+EgEM= diff --git a/pkg/can/kvaser/kvaser.go b/pkg/can/kvaser/kvaser.go index a8e0e81..d0690b1 100644 --- a/pkg/can/kvaser/kvaser.go +++ b/pkg/can/kvaser/kvaser.go @@ -11,12 +11,11 @@ import "C" import ( "errors" "fmt" + "log/slog" "unsafe" canopen "github.com/samsamfire/gocanopen" "github.com/samsamfire/gocanopen/pkg/can" - - log "github.com/sirupsen/logrus" ) const ( @@ -52,6 +51,7 @@ func init() { type KvaserBus struct { handle C.canHandle + logger *slog.Logger rxCallback canopen.FrameListener timeoutRead int timeoutWrite int @@ -83,6 +83,7 @@ func NewKvaserBus(name string) (canopen.Bus, error) { bus := &KvaserBus{} bus.timeoutRead = defaultReadTimeoutMs bus.timeoutWrite = defaultWriteTimeoutMs + bus.logger = slog.Default() bus.exit = make(chan bool) // Call lib init, any error here is silent // and will happen when trying to open port @@ -166,7 +167,7 @@ func (k *KvaserBus) handleReception() { default: frame, err := k.Recv() if err != nil && err.Error() != ErrNoMsg.Error() { - log.Errorf("[KVASER DRIVER] listening routine has closed because : %v", err) + k.logger.Error("listening routine has closed because", "err", err) return } if k.rxCallback != nil { diff --git a/pkg/can/virtual/virtual.go b/pkg/can/virtual/virtual.go index 0d2a80a..cccb07e 100644 --- a/pkg/can/virtual/virtual.go +++ b/pkg/can/virtual/virtual.go @@ -5,13 +5,13 @@ import ( "encoding/binary" "errors" "fmt" + "log/slog" "net" "sync" "time" canopen "github.com/samsamfire/gocanopen" can "github.com/samsamfire/gocanopen/pkg/can" - log "github.com/sirupsen/logrus" ) // Virtual CAN bus implementation with TCP primarily used for testing @@ -24,6 +24,7 @@ func init() { } type Bus struct { + logger *slog.Logger mu sync.Mutex channel string conn net.Conn @@ -36,7 +37,11 @@ type Bus struct { } func NewVirtualCanBus(channel string) (canopen.Bus, error) { - return &Bus{channel: channel, stopChan: make(chan bool), isRunning: false}, nil + return &Bus{ + channel: channel, + logger: slog.Default(), + stopChan: make(chan bool), + isRunning: false}, nil } // Helper function for serializing a CAN frame into the expected binary format @@ -181,7 +186,7 @@ func (client *Bus) handleReception() { if netErr, ok := err.(net.Error); ok && netErr.Timeout() { // No message received, this is OK } else if err != nil { - log.Errorf("[VIRTUAL DRIVER] listening routine has closed because : %v", err) + client.logger.Error("listening routine has closed because", "err", err) client.errSubscriber = true client.mu.Unlock() return diff --git a/pkg/gateway/gateway.go b/pkg/gateway/gateway.go index 5423149..45411a4 100644 --- a/pkg/gateway/gateway.go +++ b/pkg/gateway/gateway.go @@ -1,11 +1,12 @@ package gateway import ( + "log/slog" + "github.com/samsamfire/gocanopen/pkg/network" "github.com/samsamfire/gocanopen/pkg/nmt" "github.com/samsamfire/gocanopen/pkg/od" "github.com/samsamfire/gocanopen/pkg/sdo" - log "github.com/sirupsen/logrus" ) // BaseGateway implements the basic gateway features defined by CiA 309 @@ -16,14 +17,21 @@ import ( // CiA 309-5 : HTTP / Websocket // Each gateway maps its own parsing logic to this base gateway type BaseGateway struct { + logger *slog.Logger network *network.Network defaultNetwork uint16 defaultNodeId uint8 sdoBuffer []byte } -func NewBaseGateway(network *network.Network, defaultNetwork uint16, defaultNodeId uint8, sdoUploadBufferSize int) *BaseGateway { +func NewBaseGateway(network *network.Network, logger *slog.Logger, defaultNetwork uint16, defaultNodeId uint8, sdoUploadBufferSize int) *BaseGateway { + + if logger == nil { + logger = slog.Default() + } + return &BaseGateway{ + logger: logger, network: network, defaultNetwork: defaultNetwork, defaultNodeId: defaultNodeId, @@ -87,7 +95,7 @@ func (gw *BaseGateway) SetSDOTimeout(timeoutMs uint32) error { // TODO : maybe add mutex in case ongoing transfer gw.network.SDOClient.SetTimeout(timeoutMs) gw.network.SDOClient.SetTimeoutBlockTransfer(timeoutMs) - log.Debugf("[HTTP][SERVER] changing sdo client timeout to %vms", timeoutMs) + gw.logger.Debug("changing sdo client timeout", "timeoutMs", timeoutMs) return nil } diff --git a/pkg/gateway/http/client.go b/pkg/gateway/http/client.go index c4c3687..8f00188 100644 --- a/pkg/gateway/http/client.go +++ b/pkg/gateway/http/client.go @@ -5,23 +5,29 @@ import ( "encoding/json" "fmt" "io" + "log/slog" "net/http" "strconv" "github.com/samsamfire/gocanopen/pkg/gateway" - log "github.com/sirupsen/logrus" ) type GatewayClient struct { http.Client + logger *slog.Logger baseURL string apiVersion string currentSequenceNb int networkId int } -func NewGatewayClient(baseURL string, apiVersion string, networkId int) *GatewayClient { +func NewGatewayClient(baseURL string, apiVersion string, networkId int, logger *slog.Logger) *GatewayClient { + + if logger == nil { + logger = slog.Default() + } return &GatewayClient{ + logger: logger.With("service", "[HTTP client]"), Client: http.Client{}, baseURL: baseURL, networkId: networkId, @@ -37,19 +43,19 @@ func (client *GatewayClient) Do(method string, uri string, body io.Reader, respo baseUri := client.baseURL + "/cia309-5" + fmt.Sprintf("/%s/%d/%d", client.apiVersion, client.currentSequenceNb, client.networkId) req, err := http.NewRequest(method, baseUri+uri, body) if err != nil { - log.Errorf("[HTTP][CLIENT] http error : %v", err) + client.logger.Error("failed to create request", "err", err) return err } // HTTP request httpResp, err := client.Client.Do(req) if err != nil { - log.Errorf("[HTTP][CLIENT] http error : %v", err) + client.logger.Error("failed request", "err", err) return err } // Decode JSON "generic" response err = json.NewDecoder(httpResp.Body).Decode(response) if err != nil { - log.Errorf("[HTTP][CLIENT] error decoding json response : %v", err) + client.logger.Error("failed to decode response", "err", err) return err } // Check for gateway errors @@ -60,7 +66,7 @@ func (client *GatewayClient) Do(method string, uri string, body io.Reader, respo // Check for sequence nb mismatch sequence := response.GetSequenceNb() if client.currentSequenceNb != sequence { - log.Errorf("[HTTP][CLIENT][SEQ:%v] sequence number does not match expected value (%v)", sequence, client.currentSequenceNb) + client.logger.Error("wrong sequence number", "sequence", sequence, "expected", client.currentSequenceNb) return fmt.Errorf("error in sequence number") } return nil diff --git a/pkg/gateway/http/handlers.go b/pkg/gateway/http/handlers.go index 631be7f..a92a4b1 100644 --- a/pkg/gateway/http/handlers.go +++ b/pkg/gateway/http/handlers.go @@ -3,6 +3,7 @@ package http import ( "encoding/hex" "encoding/json" + "io" "net/http" "slices" "strconv" @@ -10,7 +11,6 @@ import ( "github.com/samsamfire/gocanopen/pkg/gateway" "github.com/samsamfire/gocanopen/pkg/nmt" - log "github.com/sirupsen/logrus" ) // Wrapper around [http.ResponseWriter] but keeps track of any writes already done @@ -33,11 +33,60 @@ func (w *doneWriter) Write(b []byte) (int, error) { return w.ResponseWriter.Write(b) } +// Create a new sanitized api request object from raw http request +// This function also checks that values are within bounds etc. +func (g *GatewayServer) newRequestFromRaw(r *http.Request) (*GatewayRequest, error) { + // Global expression match + match := regURI.FindStringSubmatch(r.URL.Path) + if len(match) != 6 { + g.logger.Error("request does not match a known API pattern") + return nil, ErrGwSyntaxError + } + // Check differents components of API route : api, sequence number, network and node + apiVersion := match[1] + if apiVersion != API_VERSION { + g.logger.Error("api version is not supported", "version", apiVersion) + return nil, ErrGwRequestNotSupported + } + sequence, err := strconv.Atoi(match[2]) + if err != nil || sequence > MAX_SEQUENCE_NB { + g.logger.Error("error processing sequence number", "sequence", match[2]) + return nil, ErrGwSyntaxError + } + netStr := match[3] + netInt, err := parseNodeOrNetworkParam(netStr) + if err != nil || netInt == 0 || netInt > 0xFFFF { + g.logger.Error("error processing network param", "param", netStr) + return nil, ErrGwUnsupportedNet + } + nodeStr := match[4] + nodeInt, err := parseNodeOrNetworkParam(nodeStr) + if err != nil || nodeInt == 0 || nodeInt > 127 { + g.logger.Error("error processing node param", "param", nodeStr) + } + + // Unmarshall request body + var parameters json.RawMessage + err = json.NewDecoder(r.Body).Decode(¶meters) + if err != nil && err != io.EOF { + g.logger.Warn("failed to unmarshal request body", "err", err) + return nil, ErrGwSyntaxError + } + request := &GatewayRequest{ + nodeId: nodeInt, + networkId: netInt, + command: match[5], // Contains rest of URL after node + sequence: uint32(sequence), + parameters: parameters, + } + return request, nil +} + // Default handler of any HTTP gateway request // This parses a typical request and forwards it to the correct handler -func (gateway *GatewayServer) handleRequest(w http.ResponseWriter, raw *http.Request) { - log.Debugf("[HTTP][SERVER] new request : %v", raw.URL) - req, err := NewGatewayRequestFromRaw(raw) +func (g *GatewayServer) handleRequest(w http.ResponseWriter, raw *http.Request) { + g.logger.Debug("handle incoming request", "endpoint", raw.URL) + req, err := g.newRequestFromRaw(raw) if err != nil { w.Write(NewResponseError(0, err)) return @@ -50,7 +99,7 @@ func (gateway *GatewayServer) handleRequest(w http.ResponseWriter, raw *http.Req // e.g. '/reset/node' exists and is handled straight away // '/read/0x2000/0x0' does not exist in map, so we then check 'read' which does exist var route GatewayRequestHandler - route, ok := gateway.routes[req.command] + route, ok := g.routes[req.command] if !ok { indexFirstSep := strings.Index(req.command, "/") var firstCommand string @@ -59,9 +108,9 @@ func (gateway *GatewayServer) handleRequest(w http.ResponseWriter, raw *http.Req } else { firstCommand = req.command } - route, ok = gateway.routes[firstCommand] + route, ok = g.routes[firstCommand] if !ok { - log.Debugf("[HTTP][SERVER] no handler found for : '%v' or '%v'", req.command, firstCommand) + g.logger.Debug("no handler found", "command", req.command, "firstCommand", firstCommand) w.Write(NewResponseError(int(req.sequence), ErrGwRequestNotSupported)) return } @@ -102,10 +151,10 @@ func handlerNotSupported(w doneWriter, req *GatewayRequest) error { // Handle a read // This includes different type of handlers : SDO, PDO, ... -func (gw *GatewayServer) handlerRead(w doneWriter, req *GatewayRequest) error { +func (g *GatewayServer) handlerRead(w doneWriter, req *GatewayRequest) error { matchSDO := regSDO.FindStringSubmatch(req.command) if len(matchSDO) >= 2 { - return gw.handlerSDORead(w, req, matchSDO) + return g.handlerSDORead(w, req, matchSDO) } matchPDO := regPDO.FindStringSubmatch(req.command) if len(matchPDO) >= 2 { @@ -114,19 +163,19 @@ func (gw *GatewayServer) handlerRead(w doneWriter, req *GatewayRequest) error { return ErrGwSyntaxError } -func (gw *GatewayServer) handlerSDORead(w doneWriter, req *GatewayRequest, commands []string) error { +func (g *GatewayServer) handlerSDORead(w doneWriter, req *GatewayRequest, commands []string) error { index, subindex, err := parseSdoCommand(commands[1:]) if err != nil { - log.Errorf("[HTTP][SERVER] unable to parse SDO command : %v", err) + g.logger.Error("unable to parse SDO command", "err", err) return err } - n, err := gw.ReadSDO(uint8(req.nodeId), uint16(index), uint8(subindex)) + n, err := g.ReadSDO(uint8(req.nodeId), uint16(index), uint8(subindex)) if err != nil { w.Write(NewResponseError(int(req.sequence), err)) return nil } - buf := gw.Buffer()[:n] + buf := g.Buffer()[:n] slices.Reverse(buf) resp := SDOReadResponse{ GatewayResponseBase: NewResponseBase(int(req.sequence), "OK"), @@ -143,10 +192,10 @@ func (gw *GatewayServer) handlerSDORead(w doneWriter, req *GatewayRequest, comma // Handle a write // This includes different type of handlers : SDO, PDO, ... -func (gw *GatewayServer) handleWrite(w doneWriter, req *GatewayRequest) error { +func (g *GatewayServer) handleWrite(w doneWriter, req *GatewayRequest) error { matchSDO := regSDO.FindStringSubmatch(req.command) if len(matchSDO) >= 2 { - return gw.handlerSDOWrite(w, req, matchSDO) + return g.handlerSDOWrite(w, req, matchSDO) } matchPDO := regPDO.FindStringSubmatch(req.command) if len(matchPDO) >= 2 { @@ -155,10 +204,10 @@ func (gw *GatewayServer) handleWrite(w doneWriter, req *GatewayRequest) error { return ErrGwSyntaxError } -func (gw *GatewayServer) handlerSDOWrite(w doneWriter, req *GatewayRequest, commands []string) error { +func (g *GatewayServer) handlerSDOWrite(w doneWriter, req *GatewayRequest, commands []string) error { index, subindex, err := parseSdoCommand(commands[1:]) if err != nil { - log.Errorf("[HTTP][SERVER] unable to parse SDO command : %v", err) + g.logger.Error("unable to parse SDO command", "err", err) return err } @@ -169,10 +218,10 @@ func (gw *GatewayServer) handlerSDOWrite(w doneWriter, req *GatewayRequest, comm } datatype, ok := DATATYPE_MAP[sdoWrite.Datatype] if !ok { - log.Errorf("[HTTP][SERVER] requested datatype is either wrong or unsupported : %v", sdoWrite.Datatype) + g.logger.Error("requested datatype is wrong or unsupported", "dataType", sdoWrite.Datatype) return ErrGwRequestNotSupported } - err = gw.WriteSDO(uint8(req.nodeId), uint16(index), uint8(subindex), sdoWrite.Value, datatype) + err = g.WriteSDO(uint8(req.nodeId), uint16(index), uint8(subindex), sdoWrite.Value, datatype) if err != nil { w.Write(NewResponseError(int(req.sequence), err)) return nil @@ -181,7 +230,7 @@ func (gw *GatewayServer) handlerSDOWrite(w doneWriter, req *GatewayRequest, comm } // Update SDO client timeout -func (gw *GatewayServer) handleSDOTimeout(w doneWriter, req *GatewayRequest) error { +func (g *GatewayServer) handleSDOTimeout(w doneWriter, req *GatewayRequest) error { var sdoTimeout SDOSetTimeoutRequest err := json.Unmarshal(req.parameters, &sdoTimeout) @@ -192,11 +241,11 @@ func (gw *GatewayServer) handleSDOTimeout(w doneWriter, req *GatewayRequest) err if err != nil || sdoTimeoutInt > 0xFFFF { return ErrGwSyntaxError } - return gw.SetSDOTimeout(uint32(sdoTimeoutInt)) + return g.SetSDOTimeout(uint32(sdoTimeoutInt)) } -func (gw *GatewayServer) handleGetVersion(w doneWriter, req *GatewayRequest) error { - version, err := gw.GetVersion() +func (g *GatewayServer) handleGetVersion(w doneWriter, req *GatewayRequest) error { + version, err := g.GetVersion() if err != nil { return ErrGwRequestNotProcessed } @@ -212,7 +261,7 @@ func (gw *GatewayServer) handleGetVersion(w doneWriter, req *GatewayRequest) err return nil } -func (gw *GatewayServer) handleSetDefaultNetwork(w doneWriter, req *GatewayRequest) error { +func (g *GatewayServer) handleSetDefaultNetwork(w doneWriter, req *GatewayRequest) error { var defaultNetwork SetDefaultNetOrNode err := json.Unmarshal(req.parameters, &defaultNetwork) if err != nil { @@ -222,13 +271,13 @@ func (gw *GatewayServer) handleSetDefaultNetwork(w doneWriter, req *GatewayReque if err != nil || networkId > 0xFFFF || networkId == 0 { return ErrGwSyntaxError } - gw.SetDefaultNetworkId(uint16(networkId)) + g.SetDefaultNetworkId(uint16(networkId)) respRaw := NewResponseSuccess(int(req.sequence)) w.Write(respRaw) return nil } -func (gw *GatewayServer) handleSetDefaultNode(w doneWriter, req *GatewayRequest) error { +func (g *GatewayServer) handleSetDefaultNode(w doneWriter, req *GatewayRequest) error { var defaultNode SetDefaultNetOrNode err := json.Unmarshal(req.parameters, &defaultNode) if err != nil { @@ -238,7 +287,7 @@ func (gw *GatewayServer) handleSetDefaultNode(w doneWriter, req *GatewayRequest) if err != nil || nodeId > 0xFF || nodeId == 0 { return ErrGwSyntaxError } - gw.SetDefaultNodeId(uint8(nodeId)) + g.SetDefaultNodeId(uint8(nodeId)) respRaw := NewResponseSuccess(int(req.sequence)) w.Write(respRaw) return nil diff --git a/pkg/gateway/http/parser.go b/pkg/gateway/http/parser.go index 5197bb0..298a354 100644 --- a/pkg/gateway/http/parser.go +++ b/pkg/gateway/http/parser.go @@ -1,12 +1,7 @@ package http import ( - "encoding/json" - "io" - "net/http" "strconv" - - log "github.com/sirupsen/logrus" ) const TOKEN_NONE = -3 @@ -58,52 +53,3 @@ func parseNodeOrNetworkParam(param string) (int, error) { } return int(paramUint), nil } - -// Create a new sanitized api request object from raw http request -// This function also checks that values are within bounds etc. -func NewGatewayRequestFromRaw(r *http.Request) (*GatewayRequest, error) { - // Global expression match - match := regURI.FindStringSubmatch(r.URL.Path) - if len(match) != 6 { - log.Error("[HTTP][SERVER] request does not match a known API pattern") - return nil, ErrGwSyntaxError - } - // Check differents components of API route : api, sequence number, network and node - apiVersion := match[1] - if apiVersion != API_VERSION { - log.Errorf("[HTTP][SERVER] api version %v is not supported", apiVersion) - return nil, ErrGwRequestNotSupported - } - sequence, err := strconv.Atoi(match[2]) - if err != nil || sequence > MAX_SEQUENCE_NB { - log.Errorf("[HTTP][SERVER] error processing sequence number %v", match[2]) - return nil, ErrGwSyntaxError - } - netStr := match[3] - netInt, err := parseNodeOrNetworkParam(netStr) - if err != nil || netInt == 0 || netInt > 0xFFFF { - log.Errorf("[HTTP][SERVER] error processing network param %v", netStr) - return nil, ErrGwUnsupportedNet - } - nodeStr := match[4] - nodeInt, err := parseNodeOrNetworkParam(nodeStr) - if err != nil || nodeInt == 0 || nodeInt > 127 { - log.Errorf("[HTTP][SERVER] error processing node param %v", nodeStr) - } - - // Unmarshall request body - var parameters json.RawMessage - err = json.NewDecoder(r.Body).Decode(¶meters) - if err != nil && err != io.EOF { - log.Warnf("[HTTP][SERVER] failed to unmarshal request body : %v", err) - return nil, ErrGwSyntaxError - } - request := &GatewayRequest{ - nodeId: nodeInt, - networkId: netInt, - command: match[5], // Contains rest of URL after node - sequence: uint32(sequence), - parameters: parameters, - } - return request, nil -} diff --git a/pkg/gateway/http/server.go b/pkg/gateway/http/server.go index 80d8bc4..27626ab 100644 --- a/pkg/gateway/http/server.go +++ b/pkg/gateway/http/server.go @@ -1,6 +1,7 @@ package http import ( + "log/slog" "net/http" "regexp" @@ -37,52 +38,62 @@ var DATATYPE_MAP = map[string]uint8{ type GatewayServer struct { *gateway.BaseGateway + logger *slog.Logger serveMux *http.ServeMux routes map[string]GatewayRequestHandler } // Create a new gateway -func NewGatewayServer(network *network.Network, defaultNetworkId uint16, defaultNodeId uint8, sdoUploadBufferSize int) *GatewayServer { - base := gateway.NewBaseGateway(network, defaultNetworkId, defaultNodeId, sdoUploadBufferSize) - gw := &GatewayServer{BaseGateway: base} - gw.serveMux = http.NewServeMux() - gw.serveMux.HandleFunc("/", gw.handleRequest) // This base route handles all the requests - gw.routes = make(map[string]GatewayRequestHandler) +func NewGatewayServer(network *network.Network, logger *slog.Logger, defaultNetworkId uint16, defaultNodeId uint8, sdoUploadBufferSize int) *GatewayServer { + if logger == nil { + logger = slog.Default() + } + logger = logger.With("service", "[HTTP]") + base := gateway.NewBaseGateway(network, logger, defaultNetworkId, defaultNodeId, sdoUploadBufferSize) + g := &GatewayServer{BaseGateway: base, logger: logger} + g.serveMux = http.NewServeMux() + g.serveMux.HandleFunc("/", g.handleRequest) // This base route handles all the requests + g.routes = make(map[string]GatewayRequestHandler) + + g.logger.Info("initializing http gateway (CiA 309-5) endpoints") // CiA 309-5 | 4.1 - gw.addRoute("r", gw.handlerRead) - gw.addRoute("read", gw.handlerRead) - gw.addRoute("w", gw.handleWrite) - gw.addRoute("write", gw.handleWrite) - gw.addRoute("set/sdo-timeout", gw.handleSDOTimeout) + g.addRoute("r", g.handlerRead) + g.addRoute("read", g.handlerRead) + g.addRoute("w", g.handleWrite) + g.addRoute("write", g.handleWrite) + g.addRoute("set/sdo-timeout", g.handleSDOTimeout) // CiA 309-5 | 4.3 - gw.addRoute("start", createNmtHandler(base, nmt.CommandEnterOperational)) - gw.addRoute("stop", createNmtHandler(base, nmt.CommandEnterStopped)) - gw.addRoute("preop", createNmtHandler(base, nmt.CommandEnterPreOperational)) - gw.addRoute("preoperational", createNmtHandler(base, nmt.CommandEnterPreOperational)) - gw.addRoute("reset/node", createNmtHandler(base, nmt.CommandResetNode)) - gw.addRoute("reset/comm", createNmtHandler(base, nmt.CommandResetCommunication)) - gw.addRoute("reset/communication", createNmtHandler(base, nmt.CommandResetCommunication)) - gw.addRoute("enable/guarding", handlerNotSupported) - gw.addRoute("disable/guarding", handlerNotSupported) - gw.addRoute("enable/heartbeat", handlerNotSupported) - gw.addRoute("disable/heartbeat", handlerNotSupported) + g.addRoute("start", createNmtHandler(base, nmt.CommandEnterOperational)) + g.addRoute("stop", createNmtHandler(base, nmt.CommandEnterStopped)) + g.addRoute("preop", createNmtHandler(base, nmt.CommandEnterPreOperational)) + g.addRoute("preoperational", createNmtHandler(base, nmt.CommandEnterPreOperational)) + g.addRoute("reset/node", createNmtHandler(base, nmt.CommandResetNode)) + g.addRoute("reset/comm", createNmtHandler(base, nmt.CommandResetCommunication)) + g.addRoute("reset/communication", createNmtHandler(base, nmt.CommandResetCommunication)) + g.addRoute("enable/guarding", handlerNotSupported) + g.addRoute("disable/guarding", handlerNotSupported) + g.addRoute("enable/heartbeat", handlerNotSupported) + g.addRoute("disable/heartbeat", handlerNotSupported) // CiA 309-5 | 4.6 - gw.addRoute("set/network", gw.handleSetDefaultNetwork) - gw.addRoute("set/node", gw.handleSetDefaultNode) - gw.addRoute("info/version", gw.handleGetVersion) + g.addRoute("set/network", g.handleSetDefaultNetwork) + g.addRoute("set/node", g.handleSetDefaultNode) + g.addRoute("info/version", g.handleGetVersion) + + g.logger.Info("finished initializing") - return gw + return g } // Process server, blocking -func (gateway *GatewayServer) ListenAndServe(addr string) error { - return http.ListenAndServe(addr, gateway.serveMux) +func (g *GatewayServer) ListenAndServe(addr string) error { + return http.ListenAndServe(addr, g.serveMux) } // Add a route to the server for handling a specific command func (g *GatewayServer) addRoute(command string, handler GatewayRequestHandler) { + g.logger.Debug("registering route", "command", command) g.routes[command] = handler } diff --git a/pkg/gateway/http/server_test.go b/pkg/gateway/http/server_test.go index d6388f8..eb98fd0 100644 --- a/pkg/gateway/http/server_test.go +++ b/pkg/gateway/http/server_test.go @@ -9,14 +9,9 @@ package http // "github.com/samsamfire/gocanopen/pkg/can/virtual" // "github.com/samsamfire/gocanopen/pkg/network" // "github.com/samsamfire/gocanopen/pkg/od" -// log "github.com/sirupsen/logrus" // "github.com/stretchr/testify/assert" // ) -// func init() { -// log.SetLevel(log.DebugLevel) -// } - // const NODE_ID_TEST = uint8(0x66) // func createNetworkEmpty() *network.Network { diff --git a/pkg/network/network.go b/pkg/network/network.go index 99f5813..d581843 100644 --- a/pkg/network/network.go +++ b/pkg/network/network.go @@ -116,7 +116,7 @@ func (network *Network) Connect(args ...any) error { return err } // Add SDO client to network by default - client, err := sdo.NewSDOClient(network.BusManager, nil, 0, sdo.DefaultClientTimeout, nil) + client, err := sdo.NewSDOClient(network.BusManager, network.logger, nil, 0, sdo.DefaultClientTimeout, nil) network.SDOClient = client return err } @@ -378,7 +378,7 @@ func (network *Network) Scan(timeoutMs uint32) (map[uint8]NodeInformation, error // Create multiple sdo clients to speed up discovery clients := make([]*sdo.SDOClient, 0) for i := nodeIdMin; i <= nodeIdMax; i++ { - client, err := sdo.NewSDOClient(network.BusManager, nil, i, timeoutMs, nil) + client, err := sdo.NewSDOClient(network.BusManager, network.logger, nil, i, timeoutMs, nil) if err != nil { return nil, err } diff --git a/pkg/node/local.go b/pkg/node/local.go index 68208b2..4e7c8fb 100644 --- a/pkg/node/local.go +++ b/pkg/node/local.go @@ -290,7 +290,7 @@ func NewLocalNode( logger.Warn("no [SDOClient] initialized") } else { - client, err := sdo.NewSDOClient(bm, odict, nodeId, sdoClientTimeoutMs, entry1280) + client, err := sdo.NewSDOClient(bm, logger, odict, nodeId, sdoClientTimeoutMs, entry1280) if err != nil { logger.Error("init failed [SDOClient]", "error", err) } else { diff --git a/pkg/node/node.go b/pkg/node/node.go index afec3f2..3255b3d 100644 --- a/pkg/node/node.go +++ b/pkg/node/node.go @@ -54,7 +54,7 @@ func newBaseNode( od: odict, id: nodeId, } - sdoClient, err := sdo.NewSDOClient(bm, odict, nodeId, sdo.DefaultClientTimeout, nil) + sdoClient, err := sdo.NewSDOClient(bm, logger, odict, nodeId, sdo.DefaultClientTimeout, nil) if err != nil { return nil, err } diff --git a/pkg/node/remote.go b/pkg/node/remote.go index 7610e29..7db5599 100644 --- a/pkg/node/remote.go +++ b/pkg/node/remote.go @@ -102,7 +102,7 @@ func NewRemoteNode( node.remoteOd = remoteOd // Create a new SDO client for the remote node & for local access - client, err := sdo.NewSDOClient(bm, remoteOd, 0, sdo.DefaultClientTimeout, nil) + client, err := sdo.NewSDOClient(bm, logger, remoteOd, 0, sdo.DefaultClientTimeout, nil) if err != nil { logger.Error("error when initializing SDO client object", "error", err) return nil, err diff --git a/pkg/sdo/client.go b/pkg/sdo/client.go index f65ea0e..2d70434 100644 --- a/pkg/sdo/client.go +++ b/pkg/sdo/client.go @@ -2,13 +2,14 @@ package sdo import ( "encoding/binary" + "fmt" + "log/slog" "sync" canopen "github.com/samsamfire/gocanopen" "github.com/samsamfire/gocanopen/internal/crc" "github.com/samsamfire/gocanopen/internal/fifo" "github.com/samsamfire/gocanopen/pkg/od" - log "github.com/sirupsen/logrus" ) const ( @@ -23,6 +24,7 @@ const ( type SDOClient struct { *canopen.BusManager + logger *slog.Logger mu sync.Mutex od *od.ObjectDictionary streamer *od.Streamer @@ -59,46 +61,51 @@ type SDOClient struct { } // Handle [SDOClient] related RX CAN frames -func (client *SDOClient) Handle(frame canopen.Frame) { - client.mu.Lock() - defer client.mu.Unlock() +func (c *SDOClient) Handle(frame canopen.Frame) { + c.mu.Lock() + defer c.mu.Unlock() - if client.state != stateIdle && frame.DLC == 8 && (!client.rxNew || frame.Data[0] == 0x80) { - if frame.Data[0] == 0x80 || (client.state != stateUploadBlkSubblockSreq && client.state != stateUploadBlkSubblockCrsp) { + if c.state != stateIdle && frame.DLC == 8 && (!c.rxNew || frame.Data[0] == 0x80) { + if frame.Data[0] == 0x80 || (c.state != stateUploadBlkSubblockSreq && c.state != stateUploadBlkSubblockCrsp) { // Copy data in response - client.response.raw = frame.Data - client.rxNew = true - } else if client.state == stateUploadBlkSubblockSreq { + c.response.raw = frame.Data + c.rxNew = true + } else if c.state == stateUploadBlkSubblockSreq { state := stateUploadBlkSubblockSreq seqno := frame.Data[0] & 0x7F - client.timeoutTimer = 0 - client.timeoutTimerBlock = 0 + c.timeoutTimer = 0 + c.timeoutTimerBlock = 0 // Checks on the Sequence number switch { - case seqno <= client.blockSize && seqno == (client.blockSequenceNb+1): - client.blockSequenceNb = seqno + case seqno <= c.blockSize && seqno == (c.blockSequenceNb+1): + c.blockSequenceNb = seqno // Is it last segment if (frame.Data[0] & 0x80) != 0 { - copy(client.blockDataUploadLast[:], frame.Data[1:]) - client.finished = true + copy(c.blockDataUploadLast[:], frame.Data[1:]) + c.finished = true state = stateUploadBlkSubblockCrsp } else { - client.fifo.Write(frame.Data[1:], &client.blockCRC) - client.sizeTransferred += BlockSeqSize - if seqno == client.blockSize { - log.Debugf("[CLIENT][RX][x%x] BLOCK UPLOAD END SUB-BLOCK | x%x:x%x | %v", client.nodeIdServer, client.index, client.subindex, frame.Data) + c.fifo.Write(frame.Data[1:], &c.blockCRC) + c.sizeTransferred += BlockSeqSize + if seqno == c.blockSize { + c.logger.Debug("[RX] block upload end segment", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "data", frame.Data, + ) state = stateUploadBlkSubblockCrsp } } - case seqno != client.blockSequenceNb && client.blockSequenceNb != 0: + case seqno != c.blockSequenceNb && c.blockSequenceNb != 0: state = stateUploadBlkSubblockCrsp - log.Warnf("Wrong sequence number in rx sub-block. seqno %x, previous %x", seqno, client.blockSequenceNb) + c.logger.Warn("wrong sequence number in rx sub-block", "seqno", seqno, "prevSeqno", c.blockSequenceNb) default: - log.Warnf("Wrong sequence number in rx ignored. seqno %x, expected %x", seqno, client.blockSequenceNb+1) + c.logger.Warn("wrong sequence number in rx sub-block,ignored", "seqno", seqno, "expected", c.blockSequenceNb+1) } if state != stateUploadBlkSubblockSreq { - client.rxNew = false - client.state = state + c.rxNew = false + c.state = state } } } @@ -106,18 +113,18 @@ func (client *SDOClient) Handle(frame canopen.Frame) { } // Setup the client for communication with an SDO server -func (client *SDOClient) setupServer(cobIdClientToServer uint32, cobIdServerToClient uint32, nodeIdServer uint8) error { - client.mu.Lock() - defer client.mu.Unlock() - client.state = stateIdle - client.rxNew = false - client.nodeIdServer = nodeIdServer +func (c *SDOClient) setupServer(cobIdClientToServer uint32, cobIdServerToClient uint32, nodeIdServer uint8) error { + c.mu.Lock() + defer c.mu.Unlock() + c.state = stateIdle + c.rxNew = false + c.nodeIdServer = nodeIdServer // If server is the same don't re-initialize the buffers - if client.cobIdClientToServer == cobIdClientToServer && client.cobIdServerToClient == cobIdServerToClient { + if c.cobIdClientToServer == cobIdClientToServer && c.cobIdServerToClient == cobIdServerToClient { return nil } - client.cobIdClientToServer = cobIdClientToServer - client.cobIdServerToClient = cobIdServerToClient + c.cobIdClientToServer = cobIdClientToServer + c.cobIdServerToClient = cobIdServerToClient // Check the valid bit var CanIdC2S, CanIdS2C uint16 if cobIdClientToServer&0x80000000 == 0 { @@ -131,51 +138,51 @@ func (client *SDOClient) setupServer(cobIdClientToServer uint32, cobIdServerToCl CanIdS2C = 0 } if CanIdC2S != 0 && CanIdS2C != 0 { - client.valid = true + c.valid = true } else { CanIdC2S = 0 CanIdS2C = 0 - client.valid = false + c.valid = false } - err := client.Subscribe(uint32(CanIdS2C), 0x7FF, false, client) + err := c.Subscribe(uint32(CanIdS2C), 0x7FF, false, c) if err != nil { return err } - client.txBuffer = canopen.NewFrame(uint32(CanIdC2S), 0, 8) + c.txBuffer = canopen.NewFrame(uint32(CanIdC2S), 0, 8) return nil } // Start a new download sequence -func (client *SDOClient) downloadSetup(index uint16, subindex uint8, sizeIndicated uint32, blockEnabled bool) error { - if !client.valid { +func (c *SDOClient) downloadSetup(index uint16, subindex uint8, sizeIndicated uint32, blockEnabled bool) error { + if !c.valid { return ErrInvalidArgs } - client.index = index - client.subindex = subindex - client.sizeIndicated = sizeIndicated - client.sizeTransferred = 0 - client.finished = false - client.timeoutTimer = 0 - client.fifo.Reset() + c.index = index + c.subindex = subindex + c.sizeIndicated = sizeIndicated + c.sizeTransferred = 0 + c.finished = false + c.timeoutTimer = 0 + c.fifo.Reset() // Select transfer type switch { - case client.od != nil && client.nodeIdServer == client.nodeId: - client.streamer.SetWriter(nil) + case c.od != nil && c.nodeIdServer == c.nodeId: + c.streamer.SetWriter(nil) // Local transfer - client.state = stateDownloadLocalTransfer + c.state = stateDownloadLocalTransfer case blockEnabled && (sizeIndicated == 0 || sizeIndicated > ClientProtocolSwitchThreshold): // Block download - client.state = stateDownloadBlkInitiateReq + c.state = stateDownloadBlkInitiateReq default: // Segmented / expedited download - client.state = stateDownloadInitiateReq + c.state = stateDownloadInitiateReq } - client.rxNew = false + c.rxNew = false return nil } -func (client *SDOClient) downloadMain( +func (c *SDOClient) downloadMain( timeDifferenceUs uint32, abort bool, bufferPartial bool, @@ -183,151 +190,178 @@ func (client *SDOClient) downloadMain( timerNextUs *uint32, forceSegmented bool, ) (uint8, error) { - client.mu.Lock() - defer client.mu.Unlock() + c.mu.Lock() + defer c.mu.Unlock() ret := waitingResponse var err error var abortCode error - if !client.valid { + if !c.valid { abortCode = AbortDeviceIncompat err = ErrInvalidArgs - } else if client.state == stateIdle { + } else if c.state == stateIdle { ret = success - } else if client.state == stateDownloadLocalTransfer && !abort { - ret, err = client.downloadLocal(bufferPartial) + } else if c.state == stateDownloadLocalTransfer && !abort { + ret, err = c.downloadLocal(bufferPartial) if ret != waitingLocalTransfer { - client.state = stateIdle + c.state = stateIdle } else if timerNextUs != nil { *timerNextUs = 0 } - } else if client.rxNew { - response := client.response + } else if c.rxNew { + response := c.response if response.IsAbort() { abortCode = response.GetAbortCode() - log.Debugf("[CLIENT][RX][x%x] SERVER ABORT | x%x:x%x | %v (x%x)", client.nodeIdServer, client.index, client.subindex, abortCode, uint32(response.GetAbortCode())) - client.state = stateIdle + c.logger.Info("[RX] server abort", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "code", uint32(response.GetAbortCode()), + "description", abortCode, + ) + c.state = stateIdle err = abortCode // Abort from the client } else if abort { abortCode = AbortDeviceIncompat - client.state = stateAbort + c.state = stateAbort - } else if !response.isResponseCommandValid(client.state) { - log.Warnf("Unexpected response code from server : %x", response.raw[0]) - client.state = stateAbort + } else if !response.isResponseCommandValid(c.state) { + c.logger.Warn("[RX] unexpected response code from server", "code", response.raw[0]) + c.state = stateAbort abortCode = AbortCmd } else { - switch client.state { + switch c.state { case stateDownloadInitiateRsp: index := response.GetIndex() subIndex := response.GetSubindex() - if index != client.index || subIndex != client.subindex { + if index != c.index || subIndex != c.subindex { abortCode = AbortParamIncompat - client.state = stateAbort + c.state = stateAbort break } // Expedited transfer - if client.finished { - client.state = stateIdle + if c.finished { + c.state = stateIdle ret = success - log.Debugf("[CLIENT][RX][x%x] DOWNLOAD EXPEDITED | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.logger.Debug("[RX] download expedited", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) // Segmented transfer } else { - client.toggle = 0x00 - client.state = stateDownloadSegmentReq - log.Debugf("[CLIENT][RX][x%x] DOWNLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.toggle = 0x00 + c.state = stateDownloadSegmentReq + c.logger.Debug("[RX] download segment", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) } case stateDownloadSegmentRsp: // Verify and alternate toggle bit toggle := response.GetToggle() - if toggle != client.toggle { + if toggle != c.toggle { abortCode = AbortToggleBit - client.state = stateAbort + c.state = stateAbort break } - client.toggle ^= 0x10 - if client.finished { - client.state = stateIdle + c.toggle ^= 0x10 + if c.finished { + c.state = stateIdle ret = success } else { - client.state = stateDownloadSegmentReq + c.state = stateDownloadSegmentReq } - log.Debugf("[CLIENT][RX][x%x] DOWNLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.logger.Debug("[RX] download segment", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) case stateDownloadBlkInitiateRsp: index := response.GetIndex() subIndex := response.GetSubindex() - if index != client.index || subIndex != client.subindex { + if index != c.index || subIndex != c.subindex { abortCode = AbortParamIncompat - client.state = stateAbort + c.state = stateAbort break } - client.blockCRC = crc.CRC16(0) - client.blockSize = response.GetBlockSize() - if client.blockSize < 1 || client.blockSize > BlockMaxSize { - client.blockSize = BlockMaxSize + c.blockCRC = crc.CRC16(0) + c.blockSize = response.GetBlockSize() + if c.blockSize < 1 || c.blockSize > BlockMaxSize { + c.blockSize = BlockMaxSize } - client.blockSequenceNb = 0 - client.fifo.AltBegin(0) - client.state = stateDownloadBlkSubblockReq - log.Debugf("[CLIENT][RX][x%x] DOWNLOAD BLOCK | x%x:x%x %v | blksize %v", client.nodeIdServer, client.index, client.subindex, response.raw, client.blockSize) + c.blockSequenceNb = 0 + c.fifo.AltBegin(0) + c.state = stateDownloadBlkSubblockReq + c.logger.Debug("[RX] download block", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "blksize", c.blockSize, + "raw", response.raw, + ) case stateDownloadBlkSubblockReq, stateDownloadBlkSubblockRsp: - if response.GetNumberOfSegments() < client.blockSequenceNb { - log.Error("Not all segments transferred successfully") - client.fifo.AltBegin(int(response.raw[1]) * BlockSeqSize) - client.finished = false + if response.GetNumberOfSegments() < c.blockSequenceNb { + c.logger.Error("not all segments transferred successfully") + c.fifo.AltBegin(int(response.raw[1]) * BlockSeqSize) + c.finished = false - } else if response.GetNumberOfSegments() > client.blockSequenceNb { + } else if response.GetNumberOfSegments() > c.blockSequenceNb { abortCode = AbortCmd - client.state = stateAbort + c.state = stateAbort break } - client.fifo.AltFinish(&client.blockCRC) - if client.finished { - client.state = stateDownloadBlkEndReq + c.fifo.AltFinish(&c.blockCRC) + if c.finished { + c.state = stateDownloadBlkEndReq } else { - client.blockSize = response.raw[2] - client.blockSequenceNb = 0 - client.fifo.AltBegin(0) - client.state = stateDownloadBlkSubblockReq + c.blockSize = response.raw[2] + c.blockSequenceNb = 0 + c.fifo.AltBegin(0) + c.state = stateDownloadBlkSubblockReq } case stateDownloadBlkEndRsp: - client.state = stateIdle + c.state = stateIdle ret = success } - client.timeoutTimer = 0 + c.timeoutTimer = 0 timeDifferenceUs = 0 - client.rxNew = false + c.rxNew = false } } else if abort { abortCode = AbortDeviceIncompat - client.state = stateAbort + c.state = stateAbort } if ret == waitingResponse { - if client.timeoutTimer < client.timeoutTimeUs { - client.timeoutTimer += timeDifferenceUs + if c.timeoutTimer < c.timeoutTimeUs { + c.timeoutTimer += timeDifferenceUs } - if client.timeoutTimer >= client.timeoutTimeUs { + if c.timeoutTimer >= c.timeoutTimeUs { abortCode = AbortTimeout - client.state = stateAbort + c.state = stateAbort } else if timerNextUs != nil { - diff := client.timeoutTimeUs - client.timeoutTimer + diff := c.timeoutTimeUs - c.timeoutTimer if *timerNextUs > diff { *timerNextUs = diff } @@ -335,39 +369,39 @@ func (client *SDOClient) downloadMain( } if ret == waitingResponse { - client.txBuffer.Data = [8]byte{0} - switch client.state { + c.txBuffer.Data = [8]byte{0} + switch c.state { case stateDownloadInitiateReq: - abortCode = client.downloadInitiate(forceSegmented) + abortCode = c.downloadInitiate(forceSegmented) if abortCode != nil { - client.state = stateIdle + c.state = stateIdle err = abortCode break } - client.state = stateDownloadInitiateRsp + c.state = stateDownloadInitiateRsp case stateDownloadSegmentReq: - abortCode = client.downloadSegment(bufferPartial) + abortCode = c.downloadSegment(bufferPartial) if abortCode != nil { - client.state = stateAbort + c.state = stateAbort err = abortCode break } - client.state = stateDownloadSegmentRsp + c.state = stateDownloadSegmentRsp case stateDownloadBlkInitiateReq: - _ = client.downloadBlockInitiate() - client.state = stateDownloadBlkInitiateRsp + _ = c.downloadBlockInitiate() + c.state = stateDownloadBlkInitiateRsp case stateDownloadBlkSubblockReq: - abortCode = client.downloadBlock(bufferPartial, timerNextUs) + abortCode = c.downloadBlock(bufferPartial, timerNextUs) if abortCode != nil { - client.state = stateAbort + c.state = stateAbort } case stateDownloadBlkEndReq: - client.downloadBlockEnd() - client.state = stateDownloadBlkEndRsp + c.downloadBlockEnd() + c.state = stateDownloadBlkEndRsp default: break @@ -378,70 +412,83 @@ func (client *SDOClient) downloadMain( if ret == waitingResponse { - switch client.state { + switch c.state { case stateAbort: - client.abort(abortCode.(Abort)) + c.abort(abortCode.(Abort)) err = abortCode - client.state = stateIdle + c.state = stateIdle case stateDownloadBlkSubblockReq: ret = blockDownloadInProgress } } if sizeTransferred != nil { - *sizeTransferred = client.sizeTransferred + *sizeTransferred = c.sizeTransferred } return ret, err } // Helper function for starting download // Valid for expedited or segmented transfer -func (client *SDOClient) downloadInitiate(forceSegmented bool) error { +func (c *SDOClient) downloadInitiate(forceSegmented bool) error { - client.txBuffer.Data[0] = 0x20 - client.txBuffer.Data[1] = byte(client.index) - client.txBuffer.Data[2] = byte(client.index >> 8) - client.txBuffer.Data[3] = client.subindex + c.txBuffer.Data[0] = 0x20 + c.txBuffer.Data[1] = byte(c.index) + c.txBuffer.Data[2] = byte(c.index >> 8) + c.txBuffer.Data[3] = c.subindex - count := uint32(client.fifo.GetOccupied()) - if (client.sizeIndicated == 0 && count <= 4) || (client.sizeIndicated > 0 && client.sizeIndicated <= 4) && !forceSegmented { - client.txBuffer.Data[0] |= 0x02 + count := uint32(c.fifo.GetOccupied()) + if (c.sizeIndicated == 0 && count <= 4) || (c.sizeIndicated > 0 && c.sizeIndicated <= 4) && !forceSegmented { + c.txBuffer.Data[0] |= 0x02 // Check length - if count == 0 || (client.sizeIndicated > 0 && client.sizeIndicated != count) { - client.state = stateIdle + if count == 0 || (c.sizeIndicated > 0 && c.sizeIndicated != count) { + c.state = stateIdle return AbortTypeMismatch } - if client.sizeIndicated > 0 { - client.txBuffer.Data[0] |= byte(0x01 | ((4 - count) << 2)) + if c.sizeIndicated > 0 { + c.txBuffer.Data[0] |= byte(0x01 | ((4 - count) << 2)) } // Copy the data in queue and add the count - count = uint32(client.fifo.Read(client.txBuffer.Data[4:], nil)) - client.sizeTransferred = count - client.finished = true - log.Debugf("[CLIENT][TX][x%x] DOWNLOAD EXPEDITED | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, client.txBuffer.Data) - + count = uint32(c.fifo.Read(c.txBuffer.Data[4:], nil)) + c.sizeTransferred = count + c.finished = true + c.logger.Debug("[TX] download expedited", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", c.txBuffer.Data, + ) } else { /* segmented transfer, indicate data size */ - if client.sizeIndicated > 0 { - size := client.sizeIndicated - client.txBuffer.Data[0] |= 0x01 - binary.LittleEndian.PutUint32(client.txBuffer.Data[4:], size) + if c.sizeIndicated > 0 { + size := c.sizeIndicated + c.txBuffer.Data[0] |= 0x01 + binary.LittleEndian.PutUint32(c.txBuffer.Data[4:], size) } - log.Debugf("[CLIENT][TX][x%x] DOWNLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, client.txBuffer.Data) + c.logger.Debug("[TX] download segment", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", c.txBuffer.Data, + ) } - client.timeoutTimer = 0 - return client.Send(client.txBuffer) + c.timeoutTimer = 0 + return c.Send(c.txBuffer) } // Write value to OD locally -func (client *SDOClient) downloadLocal(bufferPartial bool) (ret uint8, abortCode error) { +func (c *SDOClient) downloadLocal(bufferPartial bool) (ret uint8, abortCode error) { var err error - if client.streamer.Writer() == nil { - log.Debugf("[CLIENT][TX][x%x] LOCAL TRANSFER WRITE | x%x:x%x", client.nodeId, client.index, client.subindex) - streamer, err := client.od.Streamer(client.index, client.subindex, false) + if c.streamer.Writer() == nil { + c.logger.Debug("[TX] local transfer write", + "nodeId", fmt.Sprintf("x%x", c.nodeId), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + ) + streamer, err := c.od.Streamer(c.index, c.subindex, false) if err == nil { - client.streamer = streamer + c.streamer = streamer } odErr, ok := err.(od.ODR) @@ -450,54 +497,54 @@ func (client *SDOClient) downloadLocal(bufferPartial bool) (ret uint8, abortCode return 0, AbortGeneral } return 0, ConvertOdToSdoAbort(odErr) - } else if !client.streamer.HasAttribute(od.AttributeSdoRw) { + } else if !c.streamer.HasAttribute(od.AttributeSdoRw) { return 0, AbortUnsupportedAccess - } else if !client.streamer.HasAttribute(od.AttributeSdoW) { + } else if !c.streamer.HasAttribute(od.AttributeSdoW) { return 0, AbortReadOnly - } else if client.streamer.Writer() == nil { + } else if c.streamer.Writer() == nil { return 0, AbortDeviceIncompat } } // If still nil, return - if client.streamer.Writer() == nil { + if c.streamer.Writer() == nil { return } - count := client.fifo.Read(client.localBuffer, nil) - client.sizeTransferred += uint32(count) + count := c.fifo.Read(c.localBuffer, nil) + c.sizeTransferred += uint32(count) // No data error if count == 0 { abortCode = AbortDeviceIncompat // Size transferred is too large - } else if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { - client.sizeTransferred -= uint32(count) + } else if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { + c.sizeTransferred -= uint32(count) abortCode = AbortDataLong // Size transferred is too small (check on last call) - } else if !bufferPartial && client.sizeIndicated > 0 && client.sizeTransferred < client.sizeIndicated { + } else if !bufferPartial && c.sizeIndicated > 0 && c.sizeTransferred < c.sizeIndicated { abortCode = AbortDataShort // Last part of data ! } else if !bufferPartial { - odVarSize := client.streamer.DataLength + odVarSize := c.streamer.DataLength // Special case for strings where the downloaded data may be shorter (nul character can be omitted) - if client.streamer.HasAttribute(od.AttributeStr) && odVarSize == 0 || client.sizeTransferred < uint32(odVarSize) { + if c.streamer.HasAttribute(od.AttributeStr) && odVarSize == 0 || c.sizeTransferred < uint32(odVarSize) { count += 1 - client.localBuffer[count] = 0 - client.sizeTransferred += 1 - if odVarSize == 0 || odVarSize > client.sizeTransferred { + c.localBuffer[count] = 0 + c.sizeTransferred += 1 + if odVarSize == 0 || odVarSize > c.sizeTransferred { count += 1 - client.localBuffer[count] = 0 - client.sizeTransferred += 1 + c.localBuffer[count] = 0 + c.sizeTransferred += 1 } - client.streamer.DataLength = client.sizeTransferred + c.streamer.DataLength = c.sizeTransferred } else if odVarSize == 0 { - client.streamer.DataLength = client.sizeTransferred - } else if client.sizeTransferred > uint32(odVarSize) { + c.streamer.DataLength = c.sizeTransferred + } else if c.sizeTransferred > uint32(odVarSize) { abortCode = AbortDataLong - } else if client.sizeTransferred < uint32(odVarSize) { + } else if c.sizeTransferred < uint32(odVarSize) { abortCode = AbortDataShort } } if abortCode == nil { - _, err = client.streamer.Write(client.localBuffer[:count]) + _, err = c.streamer.Write(c.localBuffer[:count]) odErr, ok := err.(od.ODR) if err != nil && odErr != od.ErrPartial { if !ok { @@ -522,129 +569,144 @@ func (client *SDOClient) downloadLocal(bufferPartial bool) (ret uint8, abortCode } // Helper function for downloading a segement of segmented transfer -func (client *SDOClient) downloadSegment(bufferPartial bool) error { +func (c *SDOClient) downloadSegment(bufferPartial bool) error { // Fill data part - count := uint32(client.fifo.Read(client.txBuffer.Data[1:], nil)) - client.sizeTransferred += count - if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { - client.sizeTransferred -= count + count := uint32(c.fifo.Read(c.txBuffer.Data[1:], nil)) + c.sizeTransferred += count + if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { + c.sizeTransferred -= count return AbortDataLong } // Command specifier - client.txBuffer.Data[0] = uint8(uint32(client.toggle) | ((BlockSeqSize - count) << 1)) - if client.fifo.GetOccupied() == 0 && !bufferPartial { - if client.sizeIndicated > 0 && client.sizeTransferred < client.sizeIndicated { + c.txBuffer.Data[0] = uint8(uint32(c.toggle) | ((BlockSeqSize - count) << 1)) + if c.fifo.GetOccupied() == 0 && !bufferPartial { + if c.sizeIndicated > 0 && c.sizeTransferred < c.sizeIndicated { return AbortDataShort } - client.txBuffer.Data[0] |= 0x01 - client.finished = true + c.txBuffer.Data[0] |= 0x01 + c.finished = true } - client.timeoutTimer = 0 - log.Debugf("[CLIENT][TX][x%x] DOWNLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, client.txBuffer.Data) - return client.Send(client.txBuffer) + c.timeoutTimer = 0 + c.logger.Debug("[TX] download segment", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", c.txBuffer.Data, + ) + return c.Send(c.txBuffer) } // Helper function for initiating a block download -func (client *SDOClient) downloadBlockInitiate() error { - client.txBuffer.Data[0] = 0xC4 - client.txBuffer.Data[1] = byte(client.index) - client.txBuffer.Data[2] = byte(client.index >> 8) - client.txBuffer.Data[3] = client.subindex - if client.sizeIndicated > 0 { - client.txBuffer.Data[0] |= 0x02 - binary.LittleEndian.PutUint32(client.txBuffer.Data[4:], client.sizeIndicated) +func (c *SDOClient) downloadBlockInitiate() error { + c.txBuffer.Data[0] = 0xC4 + c.txBuffer.Data[1] = byte(c.index) + c.txBuffer.Data[2] = byte(c.index >> 8) + c.txBuffer.Data[3] = c.subindex + if c.sizeIndicated > 0 { + c.txBuffer.Data[0] |= 0x02 + binary.LittleEndian.PutUint32(c.txBuffer.Data[4:], c.sizeIndicated) } - client.timeoutTimer = 0 - return client.Send(client.txBuffer) + c.timeoutTimer = 0 + return c.Send(c.txBuffer) } // Helper function for downloading a sub-block -func (client *SDOClient) downloadBlock(bufferPartial bool, timerNext *uint32) error { - if client.fifo.AltGetOccupied() < BlockSeqSize && bufferPartial { +func (c *SDOClient) downloadBlock(bufferPartial bool, timerNext *uint32) error { + if c.fifo.AltGetOccupied() < BlockSeqSize && bufferPartial { // No data yet return nil } - client.blockSequenceNb++ - client.txBuffer.Data[0] = client.blockSequenceNb - count := uint32(client.fifo.AltRead(client.txBuffer.Data[1:])) - client.blockNoData = uint8(BlockSeqSize - count) - client.sizeTransferred += count - if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { - client.sizeTransferred -= count + c.blockSequenceNb++ + c.txBuffer.Data[0] = c.blockSequenceNb + count := uint32(c.fifo.AltRead(c.txBuffer.Data[1:])) + c.blockNoData = uint8(BlockSeqSize - count) + c.sizeTransferred += count + if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { + c.sizeTransferred -= count return AbortDataLong } - if client.fifo.AltGetOccupied() == 0 && !bufferPartial { - if client.sizeIndicated > 0 && client.sizeTransferred < client.sizeIndicated { + if c.fifo.AltGetOccupied() == 0 && !bufferPartial { + if c.sizeIndicated > 0 && c.sizeTransferred < c.sizeIndicated { return AbortDataShort } - client.txBuffer.Data[0] |= 0x80 - client.finished = true - client.state = stateDownloadBlkSubblockRsp - } else if client.blockSequenceNb >= client.blockSize { - client.state = stateDownloadBlkSubblockRsp + c.txBuffer.Data[0] |= 0x80 + c.finished = true + c.state = stateDownloadBlkSubblockRsp + } else if c.blockSequenceNb >= c.blockSize { + c.state = stateDownloadBlkSubblockRsp } else if timerNext != nil { *timerNext = 0 } - client.timeoutTimer = 0 - return client.Send(client.txBuffer) + c.timeoutTimer = 0 + return c.Send(c.txBuffer) } // Helper function for end of block -func (client *SDOClient) downloadBlockEnd() { - client.txBuffer.Data[0] = 0xC1 | (client.blockNoData << 2) - client.txBuffer.Data[1] = byte(client.blockCRC) - client.txBuffer.Data[2] = byte(client.blockCRC >> 8) - client.timeoutTimer = 0 - _ = client.Send(client.txBuffer) +func (c *SDOClient) downloadBlockEnd() { + c.txBuffer.Data[0] = 0xC1 | (c.blockNoData << 2) + c.txBuffer.Data[1] = byte(c.blockCRC) + c.txBuffer.Data[2] = byte(c.blockCRC >> 8) + c.timeoutTimer = 0 + _ = c.Send(c.txBuffer) } // Create & send abort on bus -func (client *SDOClient) abort(abortCode Abort) { +func (c *SDOClient) abort(abortCode Abort) { code := uint32(abortCode) - client.txBuffer.Data[0] = 0x80 - client.txBuffer.Data[1] = uint8(client.index) - client.txBuffer.Data[2] = uint8(client.index >> 8) - client.txBuffer.Data[3] = client.subindex - binary.LittleEndian.PutUint32(client.txBuffer.Data[4:], code) - log.Warnf("[CLIENT][TX][x%x] CLIENT ABORT | x%x:x%x | %v (x%x)", client.nodeIdServer, client.index, client.subindex, abortCode, code) - _ = client.Send(client.txBuffer) + c.txBuffer.Data[0] = 0x80 + c.txBuffer.Data[1] = uint8(c.index) + c.txBuffer.Data[2] = uint8(c.index >> 8) + c.txBuffer.Data[3] = c.subindex + binary.LittleEndian.PutUint32(c.txBuffer.Data[4:], code) + c.logger.Warn("[TX] client abort", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "code", code, + "description", abortCode, + ) + _ = c.Send(c.txBuffer) } ///////////////////////////////////// ////////////SDO UPLOAD/////////////// ///////////////////////////////////// -func (client *SDOClient) uploadSetup(index uint16, subindex uint8, blockEnabled bool) error { - if !client.valid { +func (c *SDOClient) uploadSetup(index uint16, subindex uint8, blockEnabled bool) error { + if !c.valid { return ErrInvalidArgs } - client.index = index - client.subindex = subindex - client.sizeIndicated = 0 - client.sizeTransferred = 0 - client.finished = false - client.fifo.Reset() - if client.od != nil && client.nodeIdServer == client.nodeId { - client.streamer.SetReader(nil) - client.state = stateUploadLocalTransfer + c.index = index + c.subindex = subindex + c.sizeIndicated = 0 + c.sizeTransferred = 0 + c.finished = false + c.fifo.Reset() + if c.od != nil && c.nodeIdServer == c.nodeId { + c.streamer.SetReader(nil) + c.state = stateUploadLocalTransfer } else if blockEnabled { - client.state = stateUploadBlkInitiateReq + c.state = stateUploadBlkInitiateReq } else { - client.state = stateUploadInitiateReq + c.state = stateUploadInitiateReq } - client.rxNew = false + c.rxNew = false return nil } -func (client *SDOClient) uploadLocal() (ret uint8, err error) { +func (c *SDOClient) uploadLocal() (ret uint8, err error) { - if client.streamer.Reader() == nil { - log.Debugf("[CLIENT][RX][x%x] LOCAL TRANSFER READ | x%x:x%x", client.nodeId, client.index, client.subindex) - streamer, err := client.od.Streamer(client.index, client.subindex, false) + if c.streamer.Reader() == nil { + c.logger.Debug("[RX] local transfer read", + "nodeId", fmt.Sprintf("x%x", c.nodeId), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + ) + streamer, err := c.od.Streamer(c.index, c.subindex, false) if err == nil { - client.streamer = streamer + c.streamer = streamer } odErr, ok := err.(od.ODR) @@ -653,19 +715,19 @@ func (client *SDOClient) uploadLocal() (ret uint8, err error) { return 0, AbortGeneral } return 0, ConvertOdToSdoAbort(odErr) - } else if !client.streamer.HasAttribute(od.AttributeSdoRw) { + } else if !c.streamer.HasAttribute(od.AttributeSdoRw) { return 0, AbortUnsupportedAccess - } else if !client.streamer.HasAttribute(od.AttributeSdoR) { + } else if !c.streamer.HasAttribute(od.AttributeSdoR) { return 0, AbortWriteOnly - } else if client.streamer.Reader() == nil { + } else if c.streamer.Reader() == nil { return 0, AbortDeviceIncompat } } - countFifo := client.fifo.GetSpace() + countFifo := c.fifo.GetSpace() if countFifo == 0 { ret = uploadDataFull - } else if client.streamer.Reader() != nil { - countData := client.streamer.DataLength + } else if c.streamer.Reader() != nil { + countData := c.streamer.DataLength countBuffer := uint32(0) countRead := 0 if countData > 0 && countData <= uint32(countFifo) { @@ -673,7 +735,7 @@ func (client *SDOClient) uploadLocal() (ret uint8, err error) { } else { countBuffer = uint32(countFifo) } - countRead, err = client.streamer.Read(client.localBuffer[:countBuffer]) + countRead, err = c.streamer.Read(c.localBuffer[:countBuffer]) odErr, ok := err.(od.ODR) if err != nil && err != od.ErrPartial { if !ok { @@ -681,10 +743,10 @@ func (client *SDOClient) uploadLocal() (ret uint8, err error) { } return 0, ConvertOdToSdoAbort(odErr) } else { - if countRead > 0 && client.streamer.HasAttribute(od.AttributeStr) { - client.localBuffer[countRead] = 0 + if countRead > 0 && c.streamer.HasAttribute(od.AttributeStr) { + c.localBuffer[countRead] = 0 countStr := 0 - for i, v := range client.localBuffer { + for i, v := range c.localBuffer { if v == 0 { countStr = i break @@ -696,16 +758,16 @@ func (client *SDOClient) uploadLocal() (ret uint8, err error) { if countStr < countRead { countRead = countStr odErr = od.ErrNo - client.streamer.DataLength = client.sizeTransferred + uint32(countRead) + c.streamer.DataLength = c.sizeTransferred + uint32(countRead) } } - client.fifo.Write(client.localBuffer[:countRead], nil) - client.sizeTransferred += uint32(countRead) - client.sizeIndicated = client.streamer.DataLength - if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { + c.fifo.Write(c.localBuffer[:countRead], nil) + c.sizeTransferred += uint32(countRead) + c.sizeIndicated = c.streamer.DataLength + if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { err = AbortDataLong } else if odErr == od.ErrNo { - if client.sizeIndicated > 0 && client.sizeTransferred < client.sizeIndicated { + if c.sizeIndicated > 0 && c.sizeTransferred < c.sizeIndicated { err = AbortDataShort } } else { @@ -718,56 +780,63 @@ func (client *SDOClient) uploadLocal() (ret uint8, err error) { } // Main state machine -func (client *SDOClient) upload( +func (c *SDOClient) upload( timeDifferenceUs uint32, abort bool, sizeIndicated *uint32, sizeTransferred *uint32, timerNextUs *uint32, ) (uint8, error) { - client.mu.Lock() - defer client.mu.Unlock() + c.mu.Lock() + defer c.mu.Unlock() ret := waitingResponse var err error var abortCode error - if !client.valid { + if !c.valid { abortCode = AbortDeviceIncompat err = ErrInvalidArgs - } else if client.state == stateIdle { + } else if c.state == stateIdle { ret = success - } else if client.state == stateUploadLocalTransfer && !abort { - ret, err = client.uploadLocal() + } else if c.state == stateUploadLocalTransfer && !abort { + ret, err = c.uploadLocal() if ret != uploadDataFull && ret != waitingLocalTransfer { - client.state = stateIdle + c.state = stateIdle } else if timerNextUs != nil { *timerNextUs = 0 } - } else if client.rxNew { - response := client.response + } else if c.rxNew { + response := c.response if response.IsAbort() { abortCode = response.GetAbortCode() - log.Debugf("[CLIENT][RX][x%x] SERVER ABORT | x%x:x%x | %v (x%x)", client.nodeIdServer, client.index, client.subindex, abortCode, uint32(response.GetAbortCode())) - client.state = stateIdle + c.logger.Info("[RX] server abort", + "server", fmt.Sprintf("x%x", c.nodeIdServer), + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "code", uint32(response.GetAbortCode()), + "description", abortCode, + ) + c.state = stateIdle err = abortCode + } else if abort { abortCode = AbortDeviceIncompat - client.state = stateAbort + c.state = stateAbort - } else if !response.isResponseCommandValid(client.state) { - log.Warnf("Unexpected response code from server : %x", response.raw[0]) - client.state = stateAbort + } else if !response.isResponseCommandValid(c.state) { + c.logger.Warn("unexpected response code from server", "code", response.raw[0]) + c.state = stateAbort abortCode = AbortCmd } else { - switch client.state { + switch c.state { case stateUploadInitiateRsp: index := response.GetIndex() subIndex := response.GetSubindex() - if index != client.index || subIndex != client.subindex { + if index != c.index || subIndex != c.subindex { abortCode = AbortParamIncompat - client.state = stateAbort + c.state = stateAbort break } if (response.raw[0] & 0x02) != 0 { @@ -777,87 +846,101 @@ func (client *SDOClient) upload( if (response.raw[0] & 0x01) != 0 { count -= uint32((response.raw[0] >> 2) & 0x03) } - client.fifo.Write(response.raw[4:4+count], nil) - client.sizeTransferred = count - client.state = stateIdle + c.fifo.Write(response.raw[4:4+count], nil) + c.sizeTransferred = count + c.state = stateIdle ret = success - log.Debugf("[CLIENT][RX][x%x] UPLOAD EXPEDITED | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.logger.Debug("[RX] upload expedited", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) // Segmented } else { // Size indicated ? if (response.raw[0] & 0x01) != 0 { - client.sizeIndicated = binary.LittleEndian.Uint32(response.raw[4:]) + c.sizeIndicated = binary.LittleEndian.Uint32(response.raw[4:]) } - client.toggle = 0 - client.state = stateUploadSegmentReq - log.Debugf("[CLIENT][RX][x%x] UPLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) - + c.toggle = 0 + c.state = stateUploadSegmentReq + c.logger.Debug("[RX] upload segment", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) } case stateUploadSegmentRsp: // Verify and alternate toggle bit - log.Debugf("[CLIENT][RX][x%x] UPLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.logger.Debug("[RX] upload segment", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) toggle := response.GetToggle() - if toggle != client.toggle { + if toggle != c.toggle { abortCode = AbortToggleBit - client.state = stateAbort + c.state = stateAbort break } - client.toggle ^= 0x10 + c.toggle ^= 0x10 count := BlockSeqSize - (response.raw[0]>>1)&0x07 - countWr := client.fifo.Write(response.raw[1:1+count], nil) - client.sizeTransferred += uint32(countWr) + countWr := c.fifo.Write(response.raw[1:1+count], nil) + c.sizeTransferred += uint32(countWr) // Check enough space if fifo if countWr != int(count) { abortCode = AbortOutOfMem - client.state = stateAbort + c.state = stateAbort break } // Check size uploaded - if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { + if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { abortCode = AbortDataLong - client.state = stateAbort + c.state = stateAbort break } // No more segments ? if (response.raw[0] & 0x01) != 0 { // Check size uploaded - if client.sizeIndicated > 0 && client.sizeTransferred < client.sizeIndicated { + if c.sizeIndicated > 0 && c.sizeTransferred < c.sizeIndicated { abortCode = AbortDataLong - client.state = stateAbort + c.state = stateAbort } else { - client.state = stateIdle + c.state = stateIdle ret = success } } else { - client.state = stateUploadSegmentReq + c.state = stateUploadSegmentReq } case stateUploadBlkInitiateRsp: index := response.GetIndex() subindex := response.GetSubindex() - if index != client.index || subindex != client.subindex { + if index != c.index || subindex != c.subindex { abortCode = AbortParamIncompat - client.state = stateAbort + c.state = stateAbort break } // Block is supported if (response.raw[0] & 0xF9) == 0xC0 { - client.blockCRCEnabled = response.IsCRCEnabled() + c.blockCRCEnabled = response.IsCRCEnabled() if (response.raw[0] & 0x02) != 0 { - client.sizeIndicated = uint32(response.GetBlockSize()) + c.sizeIndicated = uint32(response.GetBlockSize()) } - client.state = stateUploadBlkInitiateReq2 - log.Debugf("[CLIENT][RX][x%x] BLOCK UPLOAD INIT | x%x:x%x | crc enabled : %v expected size : %v | %v", - client.nodeIdServer, - client.index, - client.subindex, - response.IsCRCEnabled(), - client.sizeIndicated, - response.raw, + c.state = stateUploadBlkInitiateReq2 + c.logger.Debug("[RX] block upload init", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "crc", response.IsCRCEnabled(), + "sizeIndicated", c.sizeIndicated, + "raw", response.raw, ) // Switch to normal transfer @@ -868,21 +951,29 @@ func (client *SDOClient) upload( if (response.raw[0] & 0x01) != 0 { count -= (int(response.raw[0]>>2) & 0x03) } - client.fifo.Write(response.raw[4:4+count], nil) - client.sizeTransferred = uint32(count) - client.state = stateIdle + c.fifo.Write(response.raw[4:4+count], nil) + c.sizeTransferred = uint32(count) + c.state = stateIdle ret = success - log.Debugf("[CLIENT][RX][x%x] BLOCK UPLOAD SWITCHING EXPEDITED | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) - + c.logger.Debug("[RX] block upload switching expedited", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) } else { if (response.raw[0] & 0x01) != 0 { - client.sizeIndicated = uint32(response.GetBlockSize()) + c.sizeIndicated = uint32(response.GetBlockSize()) } - client.toggle = 0x00 - client.state = stateUploadSegmentReq - log.Debugf("[CLIENT][RX][x%x] BLOCK UPLOAD SWITCHING SEGMENTED | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.toggle = 0x00 + c.state = stateUploadSegmentReq + c.logger.Debug("[RX] block upload switching segmented", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) } - } case stateUploadBlkSubblockSreq: // Handled directly in Rx callback @@ -892,71 +983,76 @@ func (client *SDOClient) upload( // Get number of data bytes in last segment, that do not // contain data. Then copy remaining data into fifo noData := (response.raw[0] >> 2) & 0x07 - client.fifo.Write(client.blockDataUploadLast[:BlockSeqSize-noData], &client.blockCRC) - client.sizeTransferred += uint32(BlockSeqSize - noData) + c.fifo.Write(c.blockDataUploadLast[:BlockSeqSize-noData], &c.blockCRC) + c.sizeTransferred += uint32(BlockSeqSize - noData) - if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { + if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { abortCode = AbortDataLong - client.state = stateAbort + c.state = stateAbort break - } else if client.sizeIndicated > 0 && client.sizeTransferred < client.sizeIndicated { + } else if c.sizeIndicated > 0 && c.sizeTransferred < c.sizeIndicated { abortCode = AbortDataShort - client.state = stateAbort + c.state = stateAbort break } - if client.blockCRCEnabled { + if c.blockCRCEnabled { crcServer := crc.CRC16(binary.LittleEndian.Uint16(response.raw[1:3])) - if crcServer != client.blockCRC { + if crcServer != c.blockCRC { abortCode = AbortCRC - client.state = stateAbort + c.state = stateAbort break } } - client.state = stateUploadBlkEndCrsp - log.Debugf("[CLIENT][RX][x%x] BLOCK UPLOAD END | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, response.raw) + c.state = stateUploadBlkEndCrsp + c.logger.Debug("[RX] block upload end", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", response.raw, + ) default: abortCode = AbortCmd - client.state = stateAbort + c.state = stateAbort } } - client.timeoutTimer = 0 + c.timeoutTimer = 0 timeDifferenceUs = 0 - client.rxNew = false + c.rxNew = false } else if abort { abortCode = AbortDeviceIncompat - client.state = stateAbort + c.state = stateAbort } if ret == waitingResponse { - if client.timeoutTimer < client.timeoutTimeUs { - client.timeoutTimer += timeDifferenceUs + if c.timeoutTimer < c.timeoutTimeUs { + c.timeoutTimer += timeDifferenceUs } - if client.timeoutTimer >= client.timeoutTimeUs { - if client.state == stateUploadSegmentReq || client.state == stateUploadBlkSubblockCrsp { + if c.timeoutTimer >= c.timeoutTimeUs { + if c.state == stateUploadSegmentReq || c.state == stateUploadBlkSubblockCrsp { abortCode = AbortGeneral } else { abortCode = AbortTimeout } - client.state = stateAbort + c.state = stateAbort } else if timerNextUs != nil { - diff := client.timeoutTimeUs - client.timeoutTimer + diff := c.timeoutTimeUs - c.timeoutTimer if *timerNextUs > diff { *timerNextUs = diff } } // Timeout for subblocks - if client.state == stateUploadBlkSubblockSreq { - if client.timeoutTimerBlock < client.timeoutTimeBlockTransferUs { - client.timeoutTimerBlock += timeDifferenceUs + if c.state == stateUploadBlkSubblockSreq { + if c.timeoutTimerBlock < c.timeoutTimeBlockTransferUs { + c.timeoutTimerBlock += timeDifferenceUs } - if client.timeoutTimerBlock >= client.timeoutTimeBlockTransferUs { - client.state = stateUploadBlkSubblockCrsp - client.rxNew = false + if c.timeoutTimerBlock >= c.timeoutTimeBlockTransferUs { + c.state = stateUploadBlkSubblockCrsp + c.rxNew = false } else if timerNextUs != nil { - diff := client.timeoutTimeBlockTransferUs - client.timeoutTimerBlock + diff := c.timeoutTimeBlockTransferUs - c.timeoutTimerBlock if *timerNextUs > diff { *timerNextUs = diff } @@ -965,105 +1061,121 @@ func (client *SDOClient) upload( } if ret == waitingResponse { - client.txBuffer.Data = [8]byte{0} - switch client.state { + c.txBuffer.Data = [8]byte{0} + switch c.state { case stateUploadInitiateReq: - client.txBuffer.Data[0] = 0x40 - client.txBuffer.Data[1] = byte(client.index) - client.txBuffer.Data[2] = byte(client.index >> 8) - client.txBuffer.Data[3] = client.subindex - client.timeoutTimer = 0 - _ = client.Send(client.txBuffer) - client.state = stateUploadInitiateRsp - log.Debugf("[CLIENT][TX][x%x] UPLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, client.txBuffer.Data) + c.txBuffer.Data[0] = 0x40 + c.txBuffer.Data[1] = byte(c.index) + c.txBuffer.Data[2] = byte(c.index >> 8) + c.txBuffer.Data[3] = c.subindex + c.timeoutTimer = 0 + _ = c.Send(c.txBuffer) + c.state = stateUploadInitiateRsp + c.logger.Debug("[TX] upload segment", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", c.txBuffer.Data, + ) case stateUploadSegmentReq: - if client.fifo.GetSpace() < BlockSeqSize { + if c.fifo.GetSpace() < BlockSeqSize { ret = uploadDataFull break } - client.txBuffer.Data[0] = 0x60 | client.toggle - client.timeoutTimer = 0 - _ = client.Send(client.txBuffer) - client.state = stateUploadSegmentRsp - log.Debugf("[CLIENT][TX][x%x] UPLOAD SEGMENT | x%x:x%x %v", client.nodeIdServer, client.index, client.subindex, client.txBuffer.Data) + c.txBuffer.Data[0] = 0x60 | c.toggle + c.timeoutTimer = 0 + _ = c.Send(c.txBuffer) + c.state = stateUploadSegmentRsp + c.logger.Debug("[TX] upload segment", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "raw", c.txBuffer.Data, + ) case stateUploadBlkInitiateReq: - client.txBuffer.Data[0] = 0xA4 - client.txBuffer.Data[1] = byte(client.index) - client.txBuffer.Data[2] = byte(client.index >> 8) - client.txBuffer.Data[3] = client.subindex + c.txBuffer.Data[0] = 0xA4 + c.txBuffer.Data[1] = byte(c.index) + c.txBuffer.Data[2] = byte(c.index >> 8) + c.txBuffer.Data[3] = c.subindex // Calculate number of block segments from free space - count := client.fifo.GetSpace() / BlockSeqSize + count := c.fifo.GetSpace() / BlockSeqSize if count >= BlockMaxSize { count = BlockMaxSize } else if count == 0 { abortCode = AbortOutOfMem - client.state = stateAbort + c.state = stateAbort break } - client.blockSize = uint8(count) - client.txBuffer.Data[4] = client.blockSize - client.txBuffer.Data[5] = ClientProtocolSwitchThreshold - client.timeoutTimer = 0 - _ = client.Send(client.txBuffer) - client.state = stateUploadBlkInitiateRsp - log.Debugf("[CLIENT][TX][x%x] BLOCK UPLOAD INITIATE | x%x:x%x %v blksize : %v", client.nodeIdServer, client.index, client.subindex, client.txBuffer.Data, client.blockSize) + c.blockSize = uint8(count) + c.txBuffer.Data[4] = c.blockSize + c.txBuffer.Data[5] = ClientProtocolSwitchThreshold + c.timeoutTimer = 0 + _ = c.Send(c.txBuffer) + c.state = stateUploadBlkInitiateRsp + c.logger.Debug("[TX] block upload initiate", + "server", c.nodeIdServer, + "index", fmt.Sprintf("x%x", c.index), + "subindex", fmt.Sprintf("x%x", c.subindex), + "blksize", c.blockSize, + "raw", c.txBuffer.Data, + ) case stateUploadBlkInitiateReq2: - client.txBuffer.Data[0] = 0xA3 - client.timeoutTimer = 0 - client.timeoutTimerBlock = 0 - client.blockSequenceNb = 0 - client.blockCRC = crc.CRC16(0) - client.state = stateUploadBlkSubblockSreq - client.rxNew = false - _ = client.Send(client.txBuffer) + c.txBuffer.Data[0] = 0xA3 + c.timeoutTimer = 0 + c.timeoutTimerBlock = 0 + c.blockSequenceNb = 0 + c.blockCRC = crc.CRC16(0) + c.state = stateUploadBlkSubblockSreq + c.rxNew = false + _ = c.Send(c.txBuffer) case stateUploadBlkSubblockCrsp: - client.txBuffer.Data[0] = 0xA2 - client.txBuffer.Data[1] = client.blockSequenceNb - transferShort := client.blockSequenceNb != client.blockSize - seqnoStart := client.blockSequenceNb - if client.finished { - client.state = stateUploadBlkEndSreq + c.txBuffer.Data[0] = 0xA2 + c.txBuffer.Data[1] = c.blockSequenceNb + transferShort := c.blockSequenceNb != c.blockSize + seqnoStart := c.blockSequenceNb + if c.finished { + c.state = stateUploadBlkEndSreq } else { // Check size too large - if client.sizeIndicated > 0 && client.sizeTransferred > client.sizeIndicated { + if c.sizeIndicated > 0 && c.sizeTransferred > c.sizeIndicated { abortCode = AbortDataLong - client.state = stateAbort + c.state = stateAbort break } // Calculate number of block segments from remaining space - count := client.fifo.GetSpace() / BlockSeqSize + count := c.fifo.GetSpace() / BlockSeqSize if count >= BlockMaxSize { count = BlockMaxSize - } else if client.fifo.GetOccupied() > 0 { + } else if c.fifo.GetOccupied() > 0 { ret = uploadDataFull if transferShort { - log.Warnf("sub-block , upload data is full seqno=%v", seqnoStart) + c.logger.Warn("upload data is full", "seqno", seqnoStart) } if timerNextUs != nil { *timerNextUs = 0 } break } - client.blockSize = uint8(count) - client.blockSequenceNb = 0 - client.state = stateUploadBlkSubblockSreq - client.rxNew = false + c.blockSize = uint8(count) + c.blockSequenceNb = 0 + c.state = stateUploadBlkSubblockSreq + c.rxNew = false } - client.txBuffer.Data[2] = client.blockSize - client.timeoutTimerBlock = 0 - _ = client.Send(client.txBuffer) - if transferShort && !client.finished { - log.Warnf("sub-block restarted: seqnoPrev=%v, blksize=%v", seqnoStart, client.blockSize) + c.txBuffer.Data[2] = c.blockSize + c.timeoutTimerBlock = 0 + _ = c.Send(c.txBuffer) + if transferShort && !c.finished { + c.logger.Warn("sub-block restarted", "seqnoPrev", seqnoStart, "blksize", c.blockSize) } case stateUploadBlkEndCrsp: - client.txBuffer.Data[0] = 0xA1 - _ = client.Send(client.txBuffer) - client.state = stateIdle + c.txBuffer.Data[0] = 0xA1 + _ = c.Send(c.txBuffer) + c.state = stateIdle ret = success default: @@ -1073,21 +1185,21 @@ func (client *SDOClient) upload( } if ret == waitingResponse { - switch client.state { + switch c.state { case stateAbort: - client.abort(abortCode.(Abort)) + c.abort(abortCode.(Abort)) err = abortCode - client.state = stateIdle + c.state = stateIdle case stateUploadBlkSubblockSreq: ret = blockUploadInProgress } } if sizeIndicated != nil { - *sizeIndicated = client.sizeIndicated + *sizeIndicated = c.sizeIndicated } if sizeTransferred != nil { - *sizeTransferred = client.sizeTransferred + *sizeTransferred = c.sizeTransferred } return ret, err @@ -1096,6 +1208,7 @@ func (client *SDOClient) upload( func NewSDOClient( bm *canopen.BusManager, + logger *slog.Logger, odict *od.ObjectDictionary, nodeId uint8, timeoutMs uint32, @@ -1105,24 +1218,31 @@ func NewSDOClient( if bm == nil { return nil, canopen.ErrIllegalArgument } + if logger == nil { + logger = slog.Default() + } + + logger = logger.With("service", "[CLIENT]") + if entry1280 != nil && (entry1280.Index < 0x1280 || entry1280.Index > (0x1280+0x7F)) { - log.Errorf("[SDO CLIENT] invalid index for sdo client : x%v", entry1280.Index) + logger.Error("invalid index for sdo client", "index", fmt.Sprintf("x%x", entry1280.Index)) return nil, canopen.ErrIllegalArgument } - client := &SDOClient{BusManager: bm} - client.od = odict - client.nodeId = nodeId - client.streamer = &od.Streamer{} - client.fifo = fifo.NewFifo(BlockMaxSize * BlockSeqSize) - client.localBuffer = make([]byte, DefaultClientBufferSize+2) - client.SetTimeout(DefaultClientTimeout) - client.SetTimeoutBlockTransfer(DefaultClientTimeout) - client.SetBlockMaxSize(BlockMaxSize) - client.SetProcessingPeriod(DefaultClientProcessPeriodUs) + + c := &SDOClient{BusManager: bm, logger: logger} + c.od = odict + c.nodeId = nodeId + c.streamer = &od.Streamer{} + c.fifo = fifo.NewFifo(BlockMaxSize * BlockSeqSize) + c.localBuffer = make([]byte, DefaultClientBufferSize+2) + c.SetTimeout(DefaultClientTimeout) + c.SetTimeoutBlockTransfer(DefaultClientTimeout) + c.SetBlockMaxSize(BlockMaxSize) + c.SetProcessingPeriod(DefaultClientProcessPeriodUs) rw := &sdoRawReadWriter{ - client: client, + client: c, } - client.rw = rw + c.rw = rw var nodeIdServer uint8 var CobIdClientToServer, CobIdServerToClient uint32 @@ -1133,50 +1253,50 @@ func NewSDOClient( CobIdServerToClient, err3 = entry1280.Uint32(2) nodeIdServer, err4 = entry1280.Uint8(3) if err1 != nil || err2 != nil || err3 != nil || err4 != nil || maxSubindex != 3 { - log.Errorf("[SDO CLIENT] error when reading SDO client parameters in OD 0:%v,1:%v,2:%v,3:%v,max sub-index(should be 3) : %v", err1, err2, err3, err4, maxSubindex) + logger.Error("error reading SDO client params") return nil, canopen.ErrOdParameters } } else { nodeIdServer = 0 } if entry1280 != nil { - entry1280.AddExtension(client, od.ReadEntryDefault, writeEntry1280) + entry1280.AddExtension(c, od.ReadEntryDefault, writeEntry1280) } - client.cobIdClientToServer = 0 - client.cobIdServerToClient = 0 + c.cobIdClientToServer = 0 + c.cobIdServerToClient = 0 - err := client.setupServer(CobIdClientToServer, CobIdServerToClient, nodeIdServer) + err := c.setupServer(CobIdClientToServer, CobIdServerToClient, nodeIdServer) if err != nil { return nil, canopen.ErrIllegalArgument } - return client, nil + return c, nil } // Set read / write to local OD // This is equivalent as reading with a node id set to 0 -func (client *SDOClient) SetNoId() { - client.nodeId = 0 +func (c *SDOClient) SetNoId() { + c.nodeId = 0 } // Set timeout for SDO non block transfers -func (client *SDOClient) SetTimeout(timeoutMs uint32) { - client.timeoutTimeUs = timeoutMs * 1000 +func (c *SDOClient) SetTimeout(timeoutMs uint32) { + c.timeoutTimeUs = timeoutMs * 1000 } // Set timeout for SDO block transfers -func (client *SDOClient) SetTimeoutBlockTransfer(timeoutMs uint32) { - client.timeoutTimeBlockTransferUs = timeoutMs * 1000 +func (c *SDOClient) SetTimeoutBlockTransfer(timeoutMs uint32) { + c.timeoutTimeBlockTransferUs = timeoutMs * 1000 } // Set the processing period for SDO client // lower number can increase transfer speeds at the cost // of more CPU usage -func (client *SDOClient) SetProcessingPeriod(periodUs int) { - client.processingPeriodUs = periodUs +func (c *SDOClient) SetProcessingPeriod(periodUs int) { + c.processingPeriodUs = periodUs } // Set maximum block size to use during block transfers // Some devices may not support big block sizes as it can use a lot of RAM. -func (client *SDOClient) SetBlockMaxSize(size int) { - client.blockMaxSize = max(min(size, BlockMaxSize), BlockMinSize) +func (c *SDOClient) SetBlockMaxSize(size int) { + c.blockMaxSize = max(min(size, BlockMaxSize), BlockMinSize) } From 3114e062cc5dcdc0db2ec2ae07fe735dee4f06a5 Mon Sep 17 00:00:00 2001 From: samsam Date: Wed, 11 Dec 2024 22:21:27 +0100 Subject: [PATCH 2/6] Added : repo link in docs --- mkdocs.yml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mkdocs.yml b/mkdocs.yml index 56a0378..5f7526b 100644 --- a/mkdocs.yml +++ b/mkdocs.yml @@ -1,5 +1,5 @@ site_name: Gocanopen - +repo_url: https://github.com/samsamfire/gocanopen nav: - Getting started: index.md - CAN driver: can.md From c25023c33e7da46fdefb7a9539e5887901fc4bd5 Mon Sep 17 00:00:00 2001 From: Samuel Lee <54152208+samsamfire@users.noreply.github.com> Date: Mon, 13 Jan 2025 00:57:59 +0100 Subject: [PATCH 3/6] Improve od parsing speed (#20) * Added : improving loadup speed of EDS by a significant margin * Changed : create a seperate file for v2 parser. Changed : other small improvements : use io.Copy, scan lines on our own, reduce regexp lookups by elimitating index search for subindexes Added : record with predefined size to reduce append(..) calls * Changed : reverted to use bufio.Scanner, otherwise we can have subtleties with new lines depending on OS type... Changed : reverted change on record as we truly do not know the length of the underlying array Changed : od.Default() will use this second implementation, not all tests are passing yet * Added : create a read seeker for OD for creating in memory zip * Changed : getting rid of intermediate .ini which isn't very useful * Tests : adding benchmark for OD parser, new v2 parser is ~15x faster. Changed : removed unused variables * Added : od.Parser type for parsing ODs Added : SetParser method to network for updating the way ODs are parsed. For now, we will stay with v1. * Changed : some comments on parse v2 Changed : seperate file "base.go" for embedded OD. * Changed : bump go.mod&go.sum * Added : improving loadup speed of EDS by a significant margin * Changed : create a seperate file for v2 parser. Changed : other small improvements : use io.Copy, scan lines on our own, reduce regexp lookups by elimitating index search for subindexes Added : record with predefined size to reduce append(..) calls * Changed : reverted to use bufio.Scanner, otherwise we can have subtleties with new lines depending on OS type... Changed : reverted change on record as we truly do not know the length of the underlying array Changed : od.Default() will use this second implementation, not all tests are passing yet * Added : create a read seeker for OD for creating in memory zip * Changed : getting rid of intermediate .ini which isn't very useful * Tests : adding benchmark for OD parser, new v2 parser is ~15x faster. Changed : removed unused variables * Added : od.Parser type for parsing ODs Added : SetParser method to network for updating the way ODs are parsed. For now, we will stay with v1. * Changed : some comments on parse v2 Changed : seperate file "base.go" for embedded OD. * Changed : bump go.mod&go.sum * Added : od.Parser type for parsing ODs Added : SetParser method to network for updating the way ODs are parsed. For now, we will stay with v1. * Changed : bump go.mod&go.sum --- go.mod | 4 +- go.sum | 8 +- pkg/network/network.go | 14 +- pkg/node/local.go | 8 +- pkg/od/base.go | 17 ++ pkg/od/export.go | 12 +- pkg/od/od.go | 16 +- pkg/od/parser.go | 19 +- pkg/od/parser_test.go | 17 ++ pkg/od/parser_v2.go | 491 +++++++++++++++++++++++++++++++++++++++++ 10 files changed, 572 insertions(+), 34 deletions(-) create mode 100644 pkg/od/base.go create mode 100644 pkg/od/parser_v2.go diff --git a/go.mod b/go.mod index a0d7c4c..61d029a 100644 --- a/go.mod +++ b/go.mod @@ -3,8 +3,8 @@ module github.com/samsamfire/gocanopen go 1.22 require ( - github.com/stretchr/testify v1.8.4 - golang.org/x/sys v0.0.0-20220715151400-c0bba94af5f8 + github.com/stretchr/testify v1.9.0 + golang.org/x/sys v0.21.0 gopkg.in/ini.v1 v1.67.0 ) diff --git a/go.sum b/go.sum index e7c289b..39eb5da 100644 --- a/go.sum +++ b/go.sum @@ -2,10 +2,10 @@ github.com/davecgh/go-spew v1.1.1 h1:vj9j/u1bqnvCEfJOwUhtlOARqs3+rkHYY13jYWTU97c github.com/davecgh/go-spew v1.1.1/go.mod h1:J7Y8YcW2NihsgmVo/mv3lAwl/skON4iLHjSsI+c5H38= github.com/pmezard/go-difflib v1.0.0 h1:4DBwDE0NGyQoBHbLQYPwSUPoCMWR5BEzIk/f1lZbAQM= github.com/pmezard/go-difflib v1.0.0/go.mod h1:iKH77koFhYxTK1pcRnkKkqfTogsbg7gZNVY4sRDYZ/4= -github.com/stretchr/testify v1.8.4 h1:CcVxjf3Q8PM0mHUKJCdn+eZZtm5yQwehR5yeSVQQcUk= -github.com/stretchr/testify v1.8.4/go.mod h1:sz/lmYIOXD/1dqDmKjjqLyZ2RngseejIcXlSw2iwfAo= -golang.org/x/sys v0.0.0-20220715151400-c0bba94af5f8 h1:0A+M6Uqn+Eje4kHMK80dtF3JCXC4ykBgQG4Fe06QRhQ= -golang.org/x/sys v0.0.0-20220715151400-c0bba94af5f8/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg= +github.com/stretchr/testify v1.9.0 h1:HtqpIVDClZ4nwg75+f6Lvsy/wHu+3BoSGCbBAcpTsTg= +github.com/stretchr/testify v1.9.0/go.mod h1:r2ic/lqez/lEtzL7wO/rwa5dbSLXVDPFyf8C91i36aY= +golang.org/x/sys v0.21.0 h1:rF+pYz3DAGSQAxAu1CbC7catZg4ebC4UIeIhKxBZvws= +golang.org/x/sys v0.21.0/go.mod h1:/VUhepiaJMQUp4+oa/7Zr1D23ma6VTLIYjOOTFZPUcA= gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405 h1:yhCVgyC4o1eVCa2tZl7eS0r+SDo693bJlVdllGtEeKM= gopkg.in/check.v1 v0.0.0-20161208181325-20d25e280405/go.mod h1:Co6ibVJAznAaIkqp8huTwlJQCZ016jof/cbN4VW5Yz0= gopkg.in/ini.v1 v1.67.0 h1:Dgnx+6+nfE+IfzjUEISNeydPJh9AXNNsWbGP9KzCsOA= diff --git a/pkg/network/network.go b/pkg/network/network.go index d581843..049e09c 100644 --- a/pkg/network/network.go +++ b/pkg/network/network.go @@ -42,8 +42,9 @@ type Network struct { *sdo.SDOClient controllers map[uint8]*n.NodeProcessor // Network has an its own SDOClient - odMap map[uint8]*ObjectDictionaryInformation - logger *slog.Logger + odMap map[uint8]*ObjectDictionaryInformation + odParser od.Parser + logger *slog.Logger } type ObjectDictionaryInformation struct { @@ -71,6 +72,7 @@ func NewNetwork(bus canopen.Bus) Network { controllers: map[uint8]*n.NodeProcessor{}, BusManager: canopen.NewBusManager(bus), odMap: map[uint8]*ObjectDictionaryInformation{}, + odParser: od.Parse, logger: slog.Default(), } } @@ -210,7 +212,7 @@ func (network *Network) CreateLocalNode(nodeId uint8, odict any) (*n.LocalNode, switch odType := odict.(type) { case string: - odNode, err = od.Parse(odType, nodeId) + odNode, err = network.odParser(odType, nodeId) if err != nil { return nil, err } @@ -268,7 +270,7 @@ func (network *Network) AddRemoteNode(nodeId uint8, odict any) (*n.RemoteNode, e switch odType := odict.(type) { case string: - odNode, err = od.Parse(odType, nodeId) + odNode, err = network.odParser(odType, nodeId) if err != nil { return nil, err } @@ -419,3 +421,7 @@ func (network *Network) Scan(timeoutMs uint32) (map[uint8]NodeInformation, error func (network *Network) SetLogger(logger *slog.Logger) { network.logger = logger } + +func (network *Network) SetParser(parser od.Parser) { + network.odParser = parser +} diff --git a/pkg/node/local.go b/pkg/node/local.go index 4e7c8fb..27abc17 100644 --- a/pkg/node/local.go +++ b/pkg/node/local.go @@ -342,10 +342,10 @@ func NewLocalNode( switch format { case od.FormatEDSAscii: node.logger.Info("EDS is downloadable via object 0x1021 in ASCII format") - odict.AddReader(edsStore.Index, edsStore.Name, odict.Reader) + odict.AddReader(edsStore.Index, edsStore.Name, odict.NewReaderSeeker()) case od.FormatEDSZipped: node.logger.Info("EDS is downloadable via object 0x1021 in Zipped format") - compressed, err := createInMemoryZip("compressed.eds", odict.Reader) + compressed, err := createInMemoryZip("compressed.eds", odict.NewReaderSeeker()) if err != nil { node.logger.Error("failed to compress EDS", "error", err) return nil, err @@ -364,6 +364,10 @@ func NewLocalNode( // for example. func createInMemoryZip(filename string, r io.ReadSeeker) ([]byte, error) { + if r == nil { + return nil, fmt.Errorf("expecting a reader %v", r) + } + buffer := new(bytes.Buffer) zipWriter := zip.NewWriter(buffer) // Create a file inside the zip diff --git a/pkg/od/base.go b/pkg/od/base.go new file mode 100644 index 0000000..d026886 --- /dev/null +++ b/pkg/od/base.go @@ -0,0 +1,17 @@ +package od + +import "embed" + +//go:embed base.eds + +var f embed.FS +var rawDefaultOd []byte + +// Return embeded default object dictionary +func Default() *ObjectDictionary { + defaultOd, err := ParseV2(rawDefaultOd, 0) + if err != nil { + panic(err) + } + return defaultOd +} diff --git a/pkg/od/export.go b/pkg/od/export.go index a59c7bb..75f1761 100644 --- a/pkg/od/export.go +++ b/pkg/od/export.go @@ -2,6 +2,7 @@ package od import ( "fmt" + "io" "sort" "strconv" @@ -15,7 +16,16 @@ import ( // work for this library. func ExportEDS(odict *ObjectDictionary, defaultValues bool, filename string) error { if defaultValues { - return odict.iniFile.SaveTo(filename) + r := odict.NewReaderSeeker() + buffer, err := io.ReadAll(r) + if err != nil { + return fmt.Errorf("failed to read OD raw data %v", err) + } + i, err := ini.Load(buffer) + if err != nil { + return fmt.Errorf("failed to load .INI %v", err) + } + return i.SaveTo(filename) } eds := ini.Empty() diff --git a/pkg/od/od.go b/pkg/od/od.go index 34b0f14..3dce139 100644 --- a/pkg/od/od.go +++ b/pkg/od/od.go @@ -1,12 +1,11 @@ package od import ( + "bytes" "fmt" "io" "log/slog" "sync" - - "gopkg.in/ini.v1" ) var _logger = slog.Default() @@ -14,13 +13,18 @@ var _logger = slog.Default() // ObjectDictionary is used for storing all entries of a CANopen node // according to CiA 301. This is the internal representation of an EDS file type ObjectDictionary struct { - Reader io.ReadSeeker logger *slog.Logger - iniFile *ini.File + rawOd []byte entriesByIndexValue map[uint16]*Entry entriesByIndexName map[string]*Entry } +// Create a new reader object for reading +// raw OD file. +func (od *ObjectDictionary) NewReaderSeeker() io.ReadSeeker { + return bytes.NewReader(od.rawOd) +} + // Add an entry to OD, any existing entry will be replaced func (od *ObjectDictionary) addEntry(entry *Entry) { _, entryIndexValueExists := od.entriesByIndexValue[entry.Index] @@ -286,6 +290,10 @@ func NewRecord() *VariableList { return &VariableList{objectType: ObjectTypeRECORD, Variables: make([]*Variable, 0)} } +func NewRecordWithLength(length uint8) *VariableList { + return &VariableList{objectType: ObjectTypeRECORD, Variables: make([]*Variable, length)} +} + func NewArray(length uint8) *VariableList { return &VariableList{objectType: ObjectTypeARRAY, Variables: make([]*Variable, length)} } diff --git a/pkg/od/parser.go b/pkg/od/parser.go index 8175715..3b8dfd0 100644 --- a/pkg/od/parser.go +++ b/pkg/od/parser.go @@ -3,7 +3,6 @@ package od import ( "archive/zip" "bytes" - "embed" "fmt" "io" "regexp" @@ -12,19 +11,7 @@ import ( "gopkg.in/ini.v1" ) -//go:embed base.eds - -var f embed.FS -var rawDefaultOd []byte - -// Return embeded default object dictionary -func Default() *ObjectDictionary { - defaultOd, err := Parse(rawDefaultOd, 0) - if err != nil { - panic(err) - } - return defaultOd -} +type Parser func(file any, nodeId uint8) (*ObjectDictionary, error) // Parse an EDS file // file can be either a path or an *os.File or []byte @@ -44,9 +31,7 @@ func Parse(file any, nodeId uint8) (*ObjectDictionary, error) { // Write data from edsFile to the buffer // Don't care if fails _, _ = edsFile.WriteTo(&buf) - reader := bytes.NewReader(buf.Bytes()) - od.Reader = reader - od.iniFile = edsFile + od.rawOd = buf.Bytes() // Get all the sections in the file sections := edsFile.Sections() diff --git a/pkg/od/parser_test.go b/pkg/od/parser_test.go index 0b1f87d..903a1d4 100644 --- a/pkg/od/parser_test.go +++ b/pkg/od/parser_test.go @@ -11,3 +11,20 @@ func TestParseDefault(t *testing.T) { od := Default() assert.NotNil(t, od) } + +func BenchmarkParser(b *testing.B) { + b.Run("od default parse", func(b *testing.B) { + for n := 0; n < b.N; n++ { + _, err := Parse(rawDefaultOd, 0x10) + assert.Nil(b, err) + } + }) + + b.Run("od default parse v2", func(b *testing.B) { + for n := 0; n < b.N; n++ { + _, err := ParseV2(rawDefaultOd, 0x10) + assert.Nil(b, err) + } + }) + +} diff --git a/pkg/od/parser_v2.go b/pkg/od/parser_v2.go new file mode 100644 index 0000000..5f8ffe0 --- /dev/null +++ b/pkg/od/parser_v2.go @@ -0,0 +1,491 @@ +package od + +import ( + "bufio" + "bytes" + "fmt" + "io" + "os" + "strconv" + "strings" +) + +// v2 of OD parser, this implementation is ~15x faster +// than the previous one but has some caveats : +// +// - it expects OD definitions to be "in order" i.e. +// for example this is not possible : +// [1000] +// ... +// [1000sub0] +// ... +// [1001sub0] +// ... +// [1000sub1] +// ... +// [1001] +// +// With the current OD architecture, only minor other +// optimizations could be done. +// The remaining bottlenecks are the following : +// +// - bytes to string conversions for values create a lot of unnecessary allocation. +// As values are mostly stored in bytes anyway, we could remove this step. +// - bufio.Scanner() ==> more performant implementation ? +func ParseV2(file any, nodeId uint8) (*ObjectDictionary, error) { + + var err error + bu := &bytes.Buffer{} + + switch fType := file.(type) { + case string: + f, err := os.Open(fType) + if err != nil { + return nil, err + } + defer f.Close() + bu = &bytes.Buffer{} + io.Copy(bu, f) + + case []byte: + bu = bytes.NewBuffer(fType) + default: + return nil, fmt.Errorf("unsupported type") + } + + od := NewOD() + od.rawOd = bu.Bytes() + entry := &Entry{} + vList := &VariableList{} + isEntry := false + isSubEntry := false + subindex := uint8(0) + + var defaultValue string + var parameterName string + var objectType string + var pdoMapping string + var subNumber string + var accessType string + var dataType string + + scanner := bufio.NewScanner(bu) + + for scanner.Scan() { + + // New line detected + lineRaw := scanner.Bytes() + + // Skip if less than 2 chars + if len(lineRaw) < 2 { + continue + } + + line := trimSpaces(lineRaw) + + // Skip empty lines and comments + if len(line) == 0 || line[0] == ';' || line[0] == '#' { + continue + } + + // Handle section headers: [section] + if line[0] == '[' && line[len(line)-1] == ']' { + // A section should be of length 4 at least + if len(line) < 4 { + continue + } + + // New section, this means we have finished building + // Previous one, so take all the values and update the section + if parameterName != "" { + if isEntry { + entry.Name = parameterName + od.entriesByIndexName[parameterName] = entry + vList, err = populateEntry( + entry, + nodeId, + parameterName, + defaultValue, + objectType, + pdoMapping, + accessType, + dataType, + subNumber, + ) + + if err != nil { + return nil, fmt.Errorf("failed to create new entry %v", err) + } + } else if isSubEntry { + err = populateSubEntry( + entry, + vList, + nodeId, + parameterName, + defaultValue, + pdoMapping, + accessType, + dataType, + subindex, + ) + + if err != nil { + return nil, fmt.Errorf("failed to create sub entry %v", err) + } + } + } + + isEntry = false + isSubEntry = false + sectionBytes := line[1 : len(line)-1] + + // Check if a sub entry or the actual entry + // A subentry should be more than 4 bytes long + if isValidHex4(sectionBytes) { + + idx, err := hexAsciiToUint(sectionBytes) + if err != nil { + return nil, err + } + isEntry = true + entry = &Entry{} + entry.Index = uint16(idx) + entry.subEntriesNameMap = map[string]uint8{} + entry.logger = od.logger + od.entriesByIndexValue[uint16(idx)] = entry + + } else if isValidSubIndexFormat(sectionBytes) { + + sidx, err := hexAsciiToUint(sectionBytes[7:]) + if err != nil { + return nil, err + } + // TODO we could get entry to double check if ever something is out of order + isSubEntry = true + subindex = uint8(sidx) + } + + // Reset all values + defaultValue = "" + parameterName = "" + objectType = "" + pdoMapping = "" + subNumber = "" + accessType = "" + dataType = "" + + continue + } + + // We are in a section so we need to populate the given entry + // Parse key-value pairs: key = value + // We will create variables for storing intermediate values + // Once we are at the end of the section + + if equalsIdx := bytes.IndexByte(line, '='); equalsIdx != -1 { + key := string(trimSpaces(line[:equalsIdx])) + value := string(trimSpaces(line[equalsIdx+1:])) + + // We will get the different elements of the entry + switch key { + case "ParameterName": + parameterName = string(value) + case "ObjectType": + objectType = string(value) + case "SubNumber": + subNumber = string(value) + case "AccessType": + accessType = string(value) + case "DataType": + dataType = string(value) + case "DefaultValue": + defaultValue = string(value) + case "PDOMapping": + pdoMapping = string(value) + } + } + } + + // Last index or subindex part + // New section, this means we have finished building + // Previous one, so take all the values and update the section + if parameterName != "" { + if isEntry { + entry.Name = parameterName + od.entriesByIndexName[parameterName] = entry + _, err = populateEntry( + entry, + nodeId, + parameterName, + defaultValue, + objectType, + pdoMapping, + accessType, + dataType, + subNumber, + ) + + if err != nil { + return nil, fmt.Errorf("failed to create new entry %v", err) + } + } else if isSubEntry { + err = populateSubEntry( + entry, + vList, + nodeId, + parameterName, + defaultValue, + pdoMapping, + accessType, + dataType, + subindex, + ) + + if err != nil { + return nil, fmt.Errorf("failed to create sub entry %v", err) + } + } + } + + return od, nil +} + +func populateEntry( + entry *Entry, + nodeId uint8, + parameterName string, + defaultValue string, + objectType string, + pdoMapping string, + accessType string, + dataType string, + subNumber string, +) (*VariableList, error) { + + oType := uint8(0) + // Determine object type + // If no object type, default to 7 (CiA spec) + if objectType == "" { + oType = 7 + } else { + oTypeUint, err := strconv.ParseUint(objectType, 0, 8) + if err != nil { + return nil, fmt.Errorf("failed to parse object type %v", err) + } + oType = uint8(oTypeUint) + } + entry.ObjectType = oType + + // Add necessary stuff depending on oType + switch oType { + + case ObjectTypeVAR, ObjectTypeDOMAIN: + variable := &Variable{} + if dataType == "" { + return nil, fmt.Errorf("need data type") + } + dataTypeUint, err := strconv.ParseUint(dataType, 0, 8) + if err != nil { + return nil, fmt.Errorf("failed to parse object type %v", err) + } + + // Get Attribute + dType := uint8(dataTypeUint) + attribute := EncodeAttribute(accessType, pdoMapping == "1", dType) + + variable.Name = parameterName + variable.DataType = dType + variable.Attribute = attribute + variable.SubIndex = 0 + + if strings.Index(defaultValue, "$NODEID") != -1 { + defaultValue = fastRemoveNodeID(defaultValue) + } else { + nodeId = 0 + } + variable.valueDefault, err = EncodeFromString(defaultValue, variable.DataType, nodeId) + if err != nil { + return nil, fmt.Errorf("failed to parse 'DefaultValue' for x%x|x%x, because %v (datatype :x%x)", "", 0, err, variable.DataType) + } + variable.value = make([]byte, len(variable.valueDefault)) + copy(variable.value, variable.valueDefault) + entry.object = variable + return nil, nil + + case ObjectTypeARRAY: + // Array objects do not allow holes in subindex numbers + // So pre-init slice up to subnumber + sub, err := strconv.ParseUint(subNumber, 0, 8) + if err != nil { + return nil, fmt.Errorf("failed to parse subnumber %v", err) + } + vList := NewArray(uint8(sub)) + entry.object = vList + return vList, nil + + case ObjectTypeRECORD: + // Record objects allow holes in mapping + // Sub-objects will be added with "append" + vList := NewRecord() + entry.object = vList + return vList, nil + + default: + return nil, fmt.Errorf("unknown object type %v", oType) + } +} + +func populateSubEntry( + entry *Entry, + vlist *VariableList, + nodeId uint8, + parameterName string, + defaultValue string, + pdoMapping string, + accessType string, + dataType string, + subIndex uint8, +) error { + + if dataType == "" { + return fmt.Errorf("need data type") + } + dataTypeUint, err := strconv.ParseUint(dataType, 0, 8) + if err != nil { + return fmt.Errorf("failed to parse object type %v", err) + } + + // Get Attribute + dType := uint8(dataTypeUint) + attribute := EncodeAttribute(accessType, pdoMapping == "1", dType) + + variable := &Variable{ + Name: parameterName, + DataType: byte(dataTypeUint), + Attribute: attribute, + SubIndex: subIndex, + } + if strings.Index(defaultValue, "$NODEID") != -1 { + defaultValue = fastRemoveNodeID(defaultValue) + } else { + nodeId = 0 + } + variable.valueDefault, err = EncodeFromString(defaultValue, variable.DataType, nodeId) + if err != nil { + return fmt.Errorf("failed to parse 'DefaultValue' %v %v %v", err, defaultValue, variable.DataType) + } + variable.value = make([]byte, len(variable.valueDefault)) + copy(variable.value, variable.valueDefault) + + switch entry.ObjectType { + case ObjectTypeARRAY: + vlist.Variables[subIndex] = variable + entry.subEntriesNameMap[parameterName] = subIndex + case ObjectTypeRECORD: + vlist.Variables = append(vlist.Variables, variable) + entry.subEntriesNameMap[parameterName] = subIndex + default: + return fmt.Errorf("add member not supported for ObjectType : %v", entry.ObjectType) + } + + return nil +} + +// Remove '\t' and ' ' characters at beginning +// and beginning of line +func trimSpaces(b []byte) []byte { + start, end := 0, len(b) + + for start < end && (b[start] == ' ' || b[start] == '\t') { + start++ + } + for end > start && (b[end-1] == ' ' || b[end-1] == '\t') { + end-- + } + return b[start:end] +} + +func hexAsciiToUint(bytes []byte) (uint64, error) { + var num uint64 + + for _, b := range bytes { + var digit uint64 + + switch { + case b >= '0' && b <= '9': + digit = uint64(b - '0') // Convert '0'-'9' to 0-9 + case b >= 'A' && b <= 'F': + digit = uint64(b - 'A' + 10) // Convert 'A'-'F' to 10-15 + case b >= 'a' && b <= 'f': + digit = uint64(b - 'a' + 10) // Convert 'a'-'f' to 10-15 + default: + return 0, fmt.Errorf("invalid hex character: %c", b) + } + + num = (num << 4) | digit // Left shift by 4 (multiply by 16) and add new digit + } + + return num, nil +} + +// Check if exactly 4 hex digits (no regex) +func isValidHex4(b []byte) bool { + if len(b) != 4 { + return false + } + for _, c := range b { + if !((c >= '0' && c <= '9') || (c >= 'A' && c <= 'F') || (c >= 'a' && c <= 'f')) { + return false + } + } + return true +} + +// Check if format is "XXXXsubYY" (without regex) +func isValidSubIndexFormat(b []byte) bool { + + // Must be at least "XXXXsubY" (4+3+1 chars) + if len(b) < 8 { + return false + } + // Check first 4 chars are hex + if !isValidHex4(b[:4]) { + return false + } + // Check "sub" part (fixed position) + if string(b[4:7]) != "sub" { + return false + } + // Check remaining are hex + for _, c := range b[7:] { + if !((c >= '0' && c <= '9') || (c >= 'A' && c <= 'F') || (c >= 'a' && c <= 'f')) { + return false + } + } + + return true +} + +// Remove "$NODEID" from given string +func fastRemoveNodeID(s string) string { + b := make([]byte, 0, len(s)) // Preallocate same capacity as input string + + i := 0 + for i < len(s) { + if s[i] == '$' && len(s) > i+6 && s[i:i+7] == "$NODEID" { + i += 7 // Skip "$NODEID" + // Skip optional '+' after "$NODEID" + if i < len(s) && s[i] == '+' { + i++ + } + // Skip optional '+' before "$NODEID" + if len(b) > 0 && b[len(b)-1] == '+' { + b = b[:len(b)-1] + } + continue + } + b = append(b, s[i]) + i++ + } + return string(b) +} From 40bedfed32b5f23e12bc4cf354db50dc8c0b0b8c Mon Sep 17 00:00:00 2001 From: Samuel Lee Date: Mon, 13 Jan 2025 14:43:07 +0100 Subject: [PATCH 4/6] Fixed : issue with Timeval size depending on arch --- pkg/can/socketcanv2/socketcanv2.go | 12 ++++-------- pkg/can/socketcanv2/socketcanv2_amd64.go | 12 ++++++++++++ pkg/can/socketcanv2/socketcanv2_arm.go | 12 ++++++++++++ 3 files changed, 28 insertions(+), 8 deletions(-) create mode 100644 pkg/can/socketcanv2/socketcanv2_amd64.go create mode 100644 pkg/can/socketcanv2/socketcanv2_arm.go diff --git a/pkg/can/socketcanv2/socketcanv2.go b/pkg/can/socketcanv2/socketcanv2.go index a94fdfe..56ca854 100644 --- a/pkg/can/socketcanv2/socketcanv2.go +++ b/pkg/can/socketcanv2/socketcanv2.go @@ -17,8 +17,7 @@ import ( ) const ( - SocketCANFrameSize = 16 - DefaultRcvTimeoutUs = 100000 + SocketCANFrameSize = 16 ) func init() { @@ -52,15 +51,12 @@ func NewBus(channel string) (canopen.Bus, error) { return nil, err } - fd, err := syscall.Socket(syscall.AF_CAN, syscall.SOCK_RAW, unix.CAN_RAW) + fd, err := unix.Socket(unix.AF_CAN, unix.SOCK_RAW, unix.CAN_RAW) + //fd, err := syscall.Socket(syscall.AF_CAN, syscall.SOCK_RAW, unix.CAN_RAW) if err != nil { return nil, fmt.Errorf("failed to create CAN socket : %v", err) } - tv := syscall.Timeval{ - Sec: 0, - Usec: int64(DefaultRcvTimeoutUs), - } - err = syscall.SetsockoptTimeval(fd, syscall.SOL_SOCKET, syscall.SO_RCVTIMEO, &tv) + err = unix.SetsockoptTimeval(fd, unix.SOL_SOCKET, unix.SO_RCVTIMEO, &DefaultTimeVal) if err != nil { return nil, fmt.Errorf("failed to set read timeout %v", err) } diff --git a/pkg/can/socketcanv2/socketcanv2_amd64.go b/pkg/can/socketcanv2/socketcanv2_amd64.go new file mode 100644 index 0000000..9931164 --- /dev/null +++ b/pkg/can/socketcanv2/socketcanv2_amd64.go @@ -0,0 +1,12 @@ +//go:build !arm + +package socketcanv2 + +import ( + "golang.org/x/sys/unix" +) + +var DefaultTimeVal = unix.Timeval{ + Sec: int64(0), + Usec: int64(100_000), +} diff --git a/pkg/can/socketcanv2/socketcanv2_arm.go b/pkg/can/socketcanv2/socketcanv2_arm.go new file mode 100644 index 0000000..1dbd44b --- /dev/null +++ b/pkg/can/socketcanv2/socketcanv2_arm.go @@ -0,0 +1,12 @@ +//go:build arm + +package socketcanv2 + +import ( + "golang.org/x/sys/unix" +) + +var DefaultTimeVal = unix.Timeval{ + Sec: int32(0), + Usec: int32(100_000), +} From 47edb48c18604586f2b875cce0879918d9fd8ff7 Mon Sep 17 00:00:00 2001 From: Samuel Lee Date: Mon, 13 Jan 2025 21:26:55 +0100 Subject: [PATCH 5/6] Changes : renaming & moving files for better coherence --- pkg/od/base.go | 4 +- pkg/od/od.go | 120 --------------------- pkg/od/{interface_test.go => od_test.go} | 0 pkg/od/{parser.go => parser_v1.go} | 82 +++++++++++++- pkg/od/variable.go | 129 ++++++----------------- pkg/od/variable_list.go | 75 +++++++++++++ 6 files changed, 191 insertions(+), 219 deletions(-) rename pkg/od/{interface_test.go => od_test.go} (100%) rename pkg/od/{parser.go => parser_v1.go} (65%) create mode 100644 pkg/od/variable_list.go diff --git a/pkg/od/base.go b/pkg/od/base.go index d026886..0eee0af 100644 --- a/pkg/od/base.go +++ b/pkg/od/base.go @@ -1,10 +1,8 @@ package od -import "embed" +import _ "embed" //go:embed base.eds - -var f embed.FS var rawDefaultOd []byte // Return embeded default object dictionary diff --git a/pkg/od/od.go b/pkg/od/od.go index 3dce139..5dbf295 100644 --- a/pkg/od/od.go +++ b/pkg/od/od.go @@ -5,7 +5,6 @@ import ( "fmt" "io" "log/slog" - "sync" ) var _logger = slog.Default() @@ -178,122 +177,3 @@ func (od *ObjectDictionary) Streamer(index uint16, subindex uint8, origin bool) func (od *ObjectDictionary) Entries() map[uint16]*Entry { return od.entriesByIndexValue } - -// type FileInfo struct { -// FileName string -// FileVersion string -// FileRevision string -// LastEDS string -// EDSVersion string -// Description string -// CreationTime string -// CreationDate string -// CreatedBy string -// ModificationTime string -// ModificationDate string -// ModifiedBy string -// } - -// Variable is the main data representation for a value stored inside of OD -// It is used to store a "VAR" or "DOMAIN" object type as well as -// any sub entry of a "RECORD" or "ARRAY" object type -type Variable struct { - mu sync.RWMutex - valueDefault []byte - value []byte - // Name of this variable - Name string - // The CiA 301 data type of this variable - DataType byte - // Attribute contains the access type as well as the mapping - // information. e.g. AttributeSdoRw | AttributeRpdo - Attribute uint8 - // StorageLocation has information on which medium is the data - // stored. Currently this is unused, everything is stored in RAM - StorageLocation string - // The minimum value for this variable - lowLimit []byte - // The maximum value for this variable - highLimit []byte - // The subindex for this variable if part of an ARRAY or RECORD - SubIndex uint8 -} - -// VariableList is the data representation for -// storing a "RECORD" or "ARRAY" object type -type VariableList struct { - objectType uint8 // either "RECORD" or "ARRAY" - Variables []*Variable -} - -// GetSubObject returns the [Variable] corresponding to -// a given subindex if not found, it errors with -// ODR_SUB_NOT_EXIST -func (rec *VariableList) GetSubObject(subindex uint8) (*Variable, error) { - if rec.objectType == ObjectTypeARRAY { - subEntriesCount := len(rec.Variables) - if subindex >= uint8(subEntriesCount) { - return nil, ErrSubNotExist - } - return rec.Variables[subindex], nil - } - for i, variable := range rec.Variables { - if variable.SubIndex == subindex { - return rec.Variables[i], nil - } - } - return nil, ErrSubNotExist -} - -// AddSubObject adds a [Variable] to the VariableList -// If the VariableList is an ARRAY then the subindex should be -// identical to the actual placement inside of the array. -// Otherwise it can be any valid subindex value, and the VariableList -// will grow accordingly -func (rec *VariableList) AddSubObject( - subindex uint8, - name string, - datatype uint8, - attribute uint8, - value string, -) (*Variable, error) { - encoded, err := EncodeFromString(value, datatype, 0) - encodedCopy := make([]byte, len(encoded)) - copy(encodedCopy, encoded) - if err != nil { - return nil, err - } - if rec.objectType == ObjectTypeARRAY { - if int(subindex) >= len(rec.Variables) { - _logger.Error("trying to add a sub-object to array but ouf of bounds", - "subindex", subindex, - "length", len(rec.Variables), - ) - return nil, ErrSubNotExist - } - variable, err := NewVariable(subindex, name, datatype, attribute, value) - if err != nil { - return nil, err - } - rec.Variables[subindex] = variable - return rec.Variables[subindex], nil - } - variable, err := NewVariable(subindex, name, datatype, attribute, value) - if err != nil { - return nil, err - } - rec.Variables = append(rec.Variables, variable) - return rec.Variables[len(rec.Variables)-1], nil -} - -func NewRecord() *VariableList { - return &VariableList{objectType: ObjectTypeRECORD, Variables: make([]*Variable, 0)} -} - -func NewRecordWithLength(length uint8) *VariableList { - return &VariableList{objectType: ObjectTypeRECORD, Variables: make([]*Variable, length)} -} - -func NewArray(length uint8) *VariableList { - return &VariableList{objectType: ObjectTypeARRAY, Variables: make([]*Variable, length)} -} diff --git a/pkg/od/interface_test.go b/pkg/od/od_test.go similarity index 100% rename from pkg/od/interface_test.go rename to pkg/od/od_test.go diff --git a/pkg/od/parser.go b/pkg/od/parser_v1.go similarity index 65% rename from pkg/od/parser.go rename to pkg/od/parser_v1.go index 3b8dfd0..91c4361 100644 --- a/pkg/od/parser.go +++ b/pkg/od/parser_v1.go @@ -7,6 +7,7 @@ import ( "io" "regexp" "strconv" + "strings" "gopkg.in/ini.v1" ) @@ -168,6 +169,83 @@ func NewOD() *ObjectDictionary { } } -func init() { - rawDefaultOd, _ = f.ReadFile("base.eds") +// Create variable from section entry +func NewVariableFromSection( + section *ini.Section, + name string, + nodeId uint8, + index uint16, + subindex uint8, +) (*Variable, error) { + + variable := &Variable{ + Name: name, + SubIndex: subindex, + } + + // Get AccessType + accessType, err := section.GetKey("AccessType") + if err != nil { + return nil, fmt.Errorf("failed to get 'AccessType' for %x : %x", index, subindex) + } + + // Get PDOMapping to know if pdo mappable + var pdoMapping bool + if pM, err := section.GetKey("PDOMapping"); err == nil { + pdoMapping, err = pM.Bool() + if err != nil { + return nil, err + } + } else { + pdoMapping = true + } + + // TODO maybe add support for datatype particularities (>1B) + dataType, err := strconv.ParseInt(section.Key("DataType").Value(), 0, 8) + if err != nil { + return nil, fmt.Errorf("failed to parse 'DataType' for %x : %x, because %v", index, subindex, err) + } + variable.DataType = byte(dataType) + variable.Attribute = EncodeAttribute(accessType.String(), pdoMapping, variable.DataType) + + if highLimit, err := section.GetKey("HighLimit"); err == nil { + variable.highLimit, err = EncodeFromString(highLimit.Value(), variable.DataType, 0) + if err != nil { + _logger.Warn("error parsing HighLimit", + "index", fmt.Sprintf("x%x", index), + "subindex", fmt.Sprintf("x%x", subindex), + "error", err, + ) + } + } + + if lowLimit, err := section.GetKey("LowLimit"); err == nil { + variable.lowLimit, err = EncodeFromString(lowLimit.Value(), variable.DataType, 0) + if err != nil { + _logger.Warn("error parsing LowLimit", + "index", fmt.Sprintf("x%x", index), + "subindex", fmt.Sprintf("x%x", subindex), + "error", err, + ) + } + } + + if defaultValue, err := section.GetKey("DefaultValue"); err == nil { + defaultValueStr := defaultValue.Value() + // If $NODEID is in default value then remove it, and add it afterwards + if strings.Contains(defaultValueStr, "$NODEID") { + re := regexp.MustCompile(`\+?\$NODEID\+?`) + defaultValueStr = re.ReplaceAllString(defaultValueStr, "") + } else { + nodeId = 0 + } + variable.valueDefault, err = EncodeFromString(defaultValueStr, variable.DataType, nodeId) + if err != nil { + return nil, fmt.Errorf("failed to parse 'DefaultValue' for x%x|x%x, because %v (datatype :x%x)", index, subindex, err, variable.DataType) + } + variable.value = make([]byte, len(variable.valueDefault)) + copy(variable.value, variable.valueDefault) + } + + return variable, nil } diff --git a/pkg/od/variable.go b/pkg/od/variable.go index 312872b..19de02b 100644 --- a/pkg/od/variable.go +++ b/pkg/od/variable.go @@ -2,104 +2,34 @@ package od import ( "encoding/binary" - "fmt" "math" - "regexp" "strconv" - "strings" - - "gopkg.in/ini.v1" + "sync" ) -// Return number of bytes -func (variable *Variable) DataLength() uint32 { - return uint32(len(variable.value)) -} - -// Return default value as byte slice -func (variable *Variable) DefaultValue() []byte { - return variable.valueDefault -} - -// Create variable from section entry -func NewVariableFromSection( - section *ini.Section, - name string, - nodeId uint8, - index uint16, - subindex uint8, -) (*Variable, error) { - - variable := &Variable{ - Name: name, - SubIndex: subindex, - } - - // Get AccessType - accessType, err := section.GetKey("AccessType") - if err != nil { - return nil, fmt.Errorf("failed to get 'AccessType' for %x : %x", index, subindex) - } - - // Get PDOMapping to know if pdo mappable - var pdoMapping bool - if pM, err := section.GetKey("PDOMapping"); err == nil { - pdoMapping, err = pM.Bool() - if err != nil { - return nil, err - } - } else { - pdoMapping = true - } - - // TODO maybe add support for datatype particularities (>1B) - dataType, err := strconv.ParseInt(section.Key("DataType").Value(), 0, 8) - if err != nil { - return nil, fmt.Errorf("failed to parse 'DataType' for %x : %x, because %v", index, subindex, err) - } - variable.DataType = byte(dataType) - variable.Attribute = EncodeAttribute(accessType.String(), pdoMapping, variable.DataType) - - if highLimit, err := section.GetKey("HighLimit"); err == nil { - variable.highLimit, err = EncodeFromString(highLimit.Value(), variable.DataType, 0) - if err != nil { - _logger.Warn("error parsing HighLimit", - "index", fmt.Sprintf("x%x", index), - "subindex", fmt.Sprintf("x%x", subindex), - "error", err, - ) - } - } - - if lowLimit, err := section.GetKey("LowLimit"); err == nil { - variable.lowLimit, err = EncodeFromString(lowLimit.Value(), variable.DataType, 0) - if err != nil { - _logger.Warn("error parsing LowLimit", - "index", fmt.Sprintf("x%x", index), - "subindex", fmt.Sprintf("x%x", subindex), - "error", err, - ) - } - } - - if defaultValue, err := section.GetKey("DefaultValue"); err == nil { - defaultValueStr := defaultValue.Value() - // If $NODEID is in default value then remove it, and add it afterwards - if strings.Contains(defaultValueStr, "$NODEID") { - re := regexp.MustCompile(`\+?\$NODEID\+?`) - defaultValueStr = re.ReplaceAllString(defaultValueStr, "") - } else { - nodeId = 0 - } - variable.valueDefault, err = EncodeFromString(defaultValueStr, variable.DataType, nodeId) - if err != nil { - return nil, fmt.Errorf("failed to parse 'DefaultValue' for x%x|x%x, because %v (datatype :x%x)", index, subindex, err, variable.DataType) - } - variable.value = make([]byte, len(variable.valueDefault)) - copy(variable.value, variable.valueDefault) - } - - return variable, nil +// Variable is the main data representation for a value stored inside of OD +// It is used to store a "VAR" or "DOMAIN" object type as well as +// any sub entry of a "RECORD" or "ARRAY" object type +type Variable struct { + mu sync.RWMutex + valueDefault []byte + value []byte + // Name of this variable + Name string + // The CiA 301 data type of this variable + DataType byte + // Attribute contains the access type as well as the mapping + // information. e.g. AttributeSdoRw | AttributeRpdo + Attribute uint8 + // StorageLocation has information on which medium is the data + // stored. Currently this is unused, everything is stored in RAM + StorageLocation string + // The minimum value for this variable + lowLimit []byte + // The maximum value for this variable + highLimit []byte + // The subindex for this variable if part of an ARRAY or RECORD + SubIndex uint8 } // Create a new variable @@ -110,6 +40,7 @@ func NewVariable( attribute uint8, value string, ) (*Variable, error) { + encoded, err := EncodeFromString(value, datatype, 0) encodedCopy := make([]byte, len(encoded)) copy(encodedCopy, encoded) @@ -127,6 +58,16 @@ func NewVariable( return variable, nil } +// Return number of bytes +func (variable *Variable) DataLength() uint32 { + return uint32(len(variable.value)) +} + +// Return default value as byte slice +func (variable *Variable) DefaultValue() []byte { + return variable.valueDefault +} + // EncodeFromString value from EDS into bytes respecting canopen datatype func EncodeFromString(value string, datatype uint8, offset uint8) ([]byte, error) { diff --git a/pkg/od/variable_list.go b/pkg/od/variable_list.go new file mode 100644 index 0000000..c0e14e2 --- /dev/null +++ b/pkg/od/variable_list.go @@ -0,0 +1,75 @@ +package od + +// VariableList is the data representation for +// storing a "RECORD" or "ARRAY" object type +type VariableList struct { + objectType uint8 // either "RECORD" or "ARRAY" + Variables []*Variable +} + +// GetSubObject returns the [Variable] corresponding to a given +// subindex. +func (rec *VariableList) GetSubObject(subindex uint8) (*Variable, error) { + if rec.objectType == ObjectTypeARRAY { + subEntriesCount := len(rec.Variables) + if subindex >= uint8(subEntriesCount) { + return nil, ErrSubNotExist + } + return rec.Variables[subindex], nil + } + for i, variable := range rec.Variables { + if variable.SubIndex == subindex { + return rec.Variables[i], nil + } + } + return nil, ErrSubNotExist +} + +// AddSubObject adds a [Variable] to the VariableList +// If the VariableList is an ARRAY then the subindex should be +// identical to the actual placement inside of the array. +// Otherwise it can be any valid subindex value, and the VariableList +// will grow accordingly +func (rec *VariableList) AddSubObject( + subindex uint8, + name string, + datatype uint8, + attribute uint8, + value string, +) (*Variable, error) { + encoded, err := EncodeFromString(value, datatype, 0) + encodedCopy := make([]byte, len(encoded)) + copy(encodedCopy, encoded) + if err != nil { + return nil, err + } + if rec.objectType == ObjectTypeARRAY { + if int(subindex) >= len(rec.Variables) { + _logger.Error("trying to add a sub-object to array but ouf of bounds", + "subindex", subindex, + "length", len(rec.Variables), + ) + return nil, ErrSubNotExist + } + variable, err := NewVariable(subindex, name, datatype, attribute, value) + if err != nil { + return nil, err + } + rec.Variables[subindex] = variable + return rec.Variables[subindex], nil + } + variable, err := NewVariable(subindex, name, datatype, attribute, value) + if err != nil { + return nil, err + } + rec.Variables = append(rec.Variables, variable) + return rec.Variables[len(rec.Variables)-1], nil +} + +func NewRecord() *VariableList { + return &VariableList{objectType: ObjectTypeRECORD, Variables: make([]*Variable, 0)} +} + +func NewArray(length uint8) *VariableList { + return &VariableList{objectType: ObjectTypeARRAY, Variables: make([]*Variable, length)} +} From b08c01b285765b0f769d6f373ee6a5de2f857889 Mon Sep 17 00:00:00 2001 From: Samuel Lee Date: Mon, 13 Jan 2025 21:41:04 +0100 Subject: [PATCH 6/6] Docs : update documentation on OD parser + NodeProcessor --- docs/network.md | 42 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/docs/network.md b/docs/network.md index 651ff61..d73b46a 100644 --- a/docs/network.md +++ b/docs/network.md @@ -72,4 +72,46 @@ A node can be created with the following commands: node,err := network.CreateLocalNode(0x10,od.Default()) ``` +# Custom OD parsing + +The network can be configured to use a different OD parser +when creating local nodes. + +```golang +// Change default OD parser +network.SetParsev2(od.ParserV2) +``` + +# Custom node processing + +Nodes can also be added to network and controlled locally +with a **NodeProcessor**. e.g. + + +```golang + +// Create a local node +node, err := network.NewLocalNode( + network.BusManager, + slog.Default(), + odNode, // OD object ==> Should be created + nil, // Use definition from OD + nil, // Use definition from OD + nodeId, + nmt.StartupToOperational, + 500, + sdo.DefaultClientTimeout, + sdo.DefaultServerTimeout, + true, + nil, + ) + + +// Add a custom node to network and control it independently +proc,err := network.AddNode(node) + +// Start node processing +err = proc.Start(context.Background()) +``` + More information on local nodes [here](local.md) \ No newline at end of file