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 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/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 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), +} 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 6b7edd7..53b55e8 100644 --- a/pkg/node/local.go +++ b/pkg/node/local.go @@ -335,10 +335,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 @@ -357,6 +357,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..0eee0af --- /dev/null +++ b/pkg/od/base.go @@ -0,0 +1,15 @@ +package od + +import _ "embed" + +//go:embed base.eds +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..5dbf295 100644 --- a/pkg/od/od.go +++ b/pkg/od/od.go @@ -1,12 +1,10 @@ package od import ( + "bytes" "fmt" "io" "log/slog" - "sync" - - "gopkg.in/ini.v1" ) var _logger = slog.Default() @@ -14,13 +12,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] @@ -174,118 +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 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_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.go b/pkg/od/parser_v1.go similarity index 64% rename from pkg/od/parser.go rename to pkg/od/parser_v1.go index 8175715..91c4361 100644 --- a/pkg/od/parser.go +++ b/pkg/od/parser_v1.go @@ -3,28 +3,16 @@ package od import ( "archive/zip" "bytes" - "embed" "fmt" "io" "regexp" "strconv" + "strings" "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 +32,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() @@ -183,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/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) +} diff --git a/pkg/od/variable.go b/pkg/od/variable.go index d4a3cbd..ada67b9 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)} +}