mirror of
https://github.com/RoganDawes/P4wnP1_aloa.git
synced 2025-03-17 13:21:50 +01:00
Changed dwc2 callbacks from NL to GENL, ntest.go has testlistener
This commit is contained in:
MaMe82
parent
cd965a90fd
commit
e73deaa173
133
genl_p4wnp1/Client.go
Normal file
133
genl_p4wnp1/Client.go
Normal file
@ -0,0 +1,133 @@
|
||||
package genl_p4wnp1
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
genl "github.com/mame82/P4wnP1_go/mgenetlink"
|
||||
nl "github.com/mame82/P4wnP1_go/mnetlink"
|
||||
"errors"
|
||||
)
|
||||
|
||||
const (
|
||||
fam_name = "p4wnp1"
|
||||
dwc2_group_name = "p4wnp1_dwc2_mc"
|
||||
)
|
||||
|
||||
var (
|
||||
EP4wnP1FamilyMissing = errors.New("Couldn't find generic netlink family for P4wnP1")
|
||||
EDwc2GrpMissing = errors.New("Couldn't find generic netlink mcast group for P4wnP1 dwc2")
|
||||
EDwc2GrpJoin = errors.New("Couldn't join generic netlink mcast group for P4wnP1 dwc2")
|
||||
EWrongFamily = errors.New("Message not from generic netlink family P4wnP1")
|
||||
)
|
||||
|
||||
type Client struct {
|
||||
genl *genl.Client
|
||||
fam *genl.Family
|
||||
running bool
|
||||
}
|
||||
|
||||
func NewClient() (c *Client, err error) {
|
||||
res := &Client{}
|
||||
res.genl,err = genl.NewGeNl()
|
||||
if err != nil { return c,err }
|
||||
return res,nil
|
||||
}
|
||||
|
||||
func (c *Client) Open() (err error) {
|
||||
err = c.genl.Open()
|
||||
if err != nil { return }
|
||||
|
||||
// try to find GENL family for P4wnP1
|
||||
c.fam,err = c.genl.GetFamily(fam_name)
|
||||
if err != nil {
|
||||
c.genl.Close()
|
||||
return EP4wnP1FamilyMissing
|
||||
}
|
||||
|
||||
// try to join group for dwc2
|
||||
grpId,err := c.fam.GetGroupByName(dwc2_group_name)
|
||||
if err != nil {
|
||||
c.genl.Close()
|
||||
return EDwc2GrpMissing
|
||||
}
|
||||
err = c.genl.AddGroupMembership(grpId)
|
||||
if err != nil {
|
||||
c.genl.Close()
|
||||
return EDwc2GrpMissing
|
||||
}
|
||||
|
||||
go c.rcv_loop()
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) rcv_loop() {
|
||||
c.running = true
|
||||
// ToDo, make loop stoppable by non-blocking/interruptable socket read a.k.a select with timeout
|
||||
for c.running {
|
||||
fmt.Println("\nWaiting for messages from P4wnP1 kernel mods...\n")
|
||||
msgs,errm := c.genl.Receive()
|
||||
if errm == nil {
|
||||
for _,msg := range msgs {
|
||||
if cmd,errp := c.parseMsg(msg); errp == nil {
|
||||
switch cmd.Cmd {
|
||||
case dwc2_cmd_connection_state:
|
||||
fmt.Println("COMMAND_CONNECTION_STATE")
|
||||
params,perr := cmd.AttributesFromData()
|
||||
if perr != nil {
|
||||
fmt.Println("Couldn't parse params for COMMAND_CONNECTION_STATE")
|
||||
continue
|
||||
}
|
||||
// find
|
||||
for _,param := range params {
|
||||
if param.Type == dwc2_attr_connection_state {
|
||||
fmt.Println("Connection State: ", param.GetDataUint8())
|
||||
}
|
||||
}
|
||||
default:
|
||||
fmt.Printf("Unknown command:\n%+v\n", cmd)
|
||||
}
|
||||
} else {
|
||||
fmt.Printf("Message ignored:\n%+v\n", msg)
|
||||
continue
|
||||
}
|
||||
|
||||
}
|
||||
} else {
|
||||
fmt.Println("Receive error: ", errm)
|
||||
}
|
||||
}
|
||||
|
||||
fmt.Println("GenNl rcv loop ended")
|
||||
}
|
||||
|
||||
const (
|
||||
dwc2_cmd_connection_state = uint8(0)
|
||||
|
||||
dwc2_attr_connection_dummy = uint16(0)
|
||||
dwc2_attr_connection_state = uint16(1)
|
||||
|
||||
|
||||
)
|
||||
|
||||
func (c *Client) parseMsg(msg nl.Message) (cmd genl.Message, err error) {
|
||||
if msg.Type != c.fam.ID {
|
||||
// Multicast message from different familiy, ignore
|
||||
err = EWrongFamily
|
||||
return
|
||||
}
|
||||
|
||||
err = cmd.UnmarshalBinary(msg.GetData())
|
||||
if err != nil { return }
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) Close() (err error) {
|
||||
c.running = false
|
||||
|
||||
// leave dwc2 group
|
||||
if grpId,err := c.fam.GetGroupByName(dwc2_group_name); err == nil {
|
||||
c.genl.DropGroupMembership(grpId)
|
||||
}
|
||||
// close soket
|
||||
return c.genl.Close()
|
||||
|
||||
}
|
||||
159
mgenetlink/client.go
Normal file
159
mgenetlink/client.go
Normal file
@ -0,0 +1,159 @@
|
||||
package mgenetlink
|
||||
|
||||
import (
|
||||
nl "github.com/mame82/P4wnP1_go/mnetlink"
|
||||
"github.com/pkg/errors"
|
||||
"golang.org/x/sys/unix"
|
||||
"log"
|
||||
)
|
||||
|
||||
var (
|
||||
ENlClient = errors.New("Netlink client not connected, maybe not created with NewGeNl")
|
||||
)
|
||||
|
||||
func NewGeNl() (res *Client, err error) {
|
||||
nlcl,err := nl.NewNl(unix.NETLINK_GENERIC)
|
||||
if err != nil { return nil, err }
|
||||
res = &Client{
|
||||
nlclient: nlcl,
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) Open() (err error) {
|
||||
if c.nlclient == nil { return ENlClient }
|
||||
return c.nlclient.Open()
|
||||
}
|
||||
|
||||
func (c *Client) Close() (err error) {
|
||||
if c.nlclient == nil { return ENlClient }
|
||||
return c.nlclient.Close()
|
||||
}
|
||||
|
||||
func (c *Client) AddGroupMembership(groupid uint32) (err error) {
|
||||
return c.nlclient.AddGroupMembership(int(groupid))
|
||||
}
|
||||
|
||||
func (c *Client) DropGroupMembership(groupid uint32) (err error) {
|
||||
return c.nlclient.DropGroupMembership(int(groupid))
|
||||
}
|
||||
|
||||
func (c *Client) Receive() (msgs []nl.Message, err error) {
|
||||
return c.nlclient.Receive()
|
||||
}
|
||||
|
||||
|
||||
// This refers to generic netlink families, not netlink families (netlink family is always NETLINK_GENERIC)
|
||||
func (c *Client) GetFamilies() (families []Family, err error) {
|
||||
nlmsg := nl.Message{
|
||||
Flags: unix.NLM_F_REQUEST | unix.NLM_F_DUMP, // + dump request to get all
|
||||
Type: unix.GENL_ID_CTRL,
|
||||
}
|
||||
genlmsg := Message{
|
||||
Cmd: unix.CTRL_CMD_GETFAMILY,
|
||||
Version: 1, // control version 1 ??
|
||||
}
|
||||
|
||||
genlmsg.Data = []byte{}
|
||||
if err != nil { return }
|
||||
|
||||
nlmsg_data,err := genlmsg.MarshalBinary()
|
||||
if err != nil { return }
|
||||
nlmsg.SetData(nlmsg_data)
|
||||
|
||||
|
||||
|
||||
err = c.nlclient.Send(nlmsg)
|
||||
if err != nil { return }
|
||||
|
||||
// read answer (or sth else)
|
||||
msgs,err := c.nlclient.Receive()
|
||||
if err != nil { return }
|
||||
|
||||
//fmt.Printf("Answer: %+v\n", msgs)
|
||||
|
||||
for _,msg := range msgs {
|
||||
|
||||
genl_msg := Message{} //genl message
|
||||
genl_msg.UnmarshalBinary(msg.GetData()) //parse NL message payload as genl message
|
||||
attrs,err := genl_msg.AttributesFromData() // parse genl_msg payload as attr array
|
||||
if err != nil {
|
||||
log.Println("Error parsing message data as attributes")
|
||||
continue
|
||||
}
|
||||
//fmt.Printf("Msg %d attributes: %+v\n", idx, attrs)
|
||||
curFamily,err := ParseAttrsToFamily(attrs) // parse attr array as data for a single family
|
||||
if err != nil {
|
||||
log.Println("Error parsing attributes as family")
|
||||
continue
|
||||
}
|
||||
families = append(families, curFamily)
|
||||
|
||||
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) GetFamily(familyName string) (family *Family, err error) {
|
||||
nlmsg := nl.Message{
|
||||
Flags: unix.NLM_F_REQUEST,
|
||||
Type: unix.GENL_ID_CTRL,
|
||||
//Seq: seq,
|
||||
//Pid: pid,
|
||||
}
|
||||
genlmsg := Message{
|
||||
Cmd: unix.CTRL_CMD_GETFAMILY,
|
||||
Version: 1, // control version 1 ??
|
||||
}
|
||||
genlmsg_attr := nl.Attr{
|
||||
Type: unix.CTRL_ATTR_FAMILY_NAME,
|
||||
}
|
||||
genlmsg_attr.SetData(nl.Str2Bytes(familyName))
|
||||
|
||||
genlmsg.Data,err = genlmsg_attr.MarshalBinary()
|
||||
if err != nil { return }
|
||||
|
||||
nlmsg_data,err := genlmsg.MarshalBinary()
|
||||
if err != nil { return }
|
||||
nlmsg.SetData(nlmsg_data)
|
||||
|
||||
|
||||
|
||||
err = c.nlclient.Send(nlmsg)
|
||||
if err != nil { return }
|
||||
|
||||
// read answer (or sth else)
|
||||
msgs,err := c.nlclient.Receive()
|
||||
if err != nil { return }
|
||||
|
||||
//fmt.Printf("Answer: %+v\n", msgs)
|
||||
|
||||
for _,msg := range msgs {
|
||||
if family != nil {
|
||||
// multiple valid families in response
|
||||
return nil,errors.New("Multiple valid families in resposne to GETFAMILY command")
|
||||
}
|
||||
|
||||
genl_msg := Message{} //genl message
|
||||
genl_msg.UnmarshalBinary(msg.GetData()) //parse NL message payload as genl message
|
||||
attrs,err := genl_msg.AttributesFromData() // parse genl_msg payload as attr array
|
||||
if err != nil {
|
||||
log.Println("Error parsing message data as attributes")
|
||||
continue
|
||||
}
|
||||
//fmt.Printf("Msg %d attributes: %+v\n", idx, attrs)
|
||||
curFamily,err := ParseAttrsToFamily(attrs) // parse attr array as data for a single family
|
||||
if err != nil {
|
||||
log.Println("Error parsing attributes as family")
|
||||
continue
|
||||
}
|
||||
family = &curFamily
|
||||
|
||||
|
||||
}
|
||||
return
|
||||
}
|
||||
|
||||
type Client struct {
|
||||
nlclient *nl.Client
|
||||
}
|
||||
5
mgenetlink/common.go
Normal file
5
mgenetlink/common.go
Normal file
@ -0,0 +1,5 @@
|
||||
package mgenetlink
|
||||
|
||||
import "github.com/mame82/P4wnP1_go/mnetlink"
|
||||
|
||||
var hbo = mnetlink.Hbo()
|
||||
69
mgenetlink/family.go
Normal file
69
mgenetlink/family.go
Normal file
@ -0,0 +1,69 @@
|
||||
package mgenetlink
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"fmt"
|
||||
"github.com/mame82/P4wnP1_go/mnetlink"
|
||||
"golang.org/x/sys/unix"
|
||||
)
|
||||
|
||||
var (
|
||||
EGroupNotFound = errors.New("Group not found")
|
||||
)
|
||||
|
||||
type Family struct {
|
||||
ID uint16
|
||||
Version uint8
|
||||
Name string
|
||||
Groups []McastGroup
|
||||
Ops []Op //ToDo: struct and parsing
|
||||
}
|
||||
|
||||
func ParseAttrsToFamily(attrs []mnetlink.Attr) (family Family, err error) {
|
||||
for _,attr := range attrs {
|
||||
|
||||
switch attr.Type {
|
||||
case unix.CTRL_ATTR_FAMILY_ID:
|
||||
//fmt.Printf("Family ID: %d\n", attr.GetDataUint16())
|
||||
family.ID = attr.GetDataUint16()
|
||||
case unix.CTRL_ATTR_FAMILY_NAME:
|
||||
//fmt.Printf("Family Name: %s\n", attr.GetDataString())
|
||||
family.Name = attr.GetDataString()
|
||||
case unix.CTRL_ATTR_VERSION:
|
||||
//fmt.Printf("Family Version: %d\n", attr.GetDataUint8())
|
||||
family.Version = attr.GetDataUint8()
|
||||
case unix.CTRL_ATTR_MCAST_GROUPS:
|
||||
mcast_attr,err := attr.GetDataAttrs()
|
||||
if err != nil { return family,err }
|
||||
//fmt.Printf("Family Mcast Groups: \n%+v\n", mcast_attr)
|
||||
family.Groups,err = ParseAttrsToMcastGroups(mcast_attr)
|
||||
if err != nil { return family,err }
|
||||
case unix.CTRL_ATTR_HDRSIZE:
|
||||
//fmt.Printf("ATTR HDRSIZE: \n%v\n", attr.GetDataDump())
|
||||
case unix.CTRL_ATTR_MAXATTR:
|
||||
//fmt.Printf("ATTR MAXATTR: \n%v\n", attr.GetDataDump())
|
||||
case unix.CTRL_ATTR_OPS:
|
||||
//fmt.Printf("Family Ops: \n%v\n", attr.GetDataDump())
|
||||
ops_attr,err := attr.GetDataAttrs()
|
||||
if err != nil { return family,err }
|
||||
//fmt.Printf("Family Mcast Groups: \n%+v\n", mcast_attr)
|
||||
family.Ops,err = ParseAttrsToOps(ops_attr)
|
||||
if err != nil { return family,err }
|
||||
|
||||
|
||||
default:
|
||||
fmt.Printf("Unknown attr %d: \n%v\n", attr.Type, attr.GetDataDump())
|
||||
}
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func (fam *Family) GetGroupByName(name string) (id uint32, err error) {
|
||||
for _,grp := range fam.Groups {
|
||||
if grp.Name == name {
|
||||
return grp.Id,nil
|
||||
}
|
||||
}
|
||||
return id,EGroupNotFound
|
||||
}
|
||||
55
mgenetlink/mcast_group.go
Normal file
55
mgenetlink/mcast_group.go
Normal file
@ -0,0 +1,55 @@
|
||||
package mgenetlink
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"github.com/mame82/P4wnP1_go/mnetlink"
|
||||
"github.com/pkg/errors"
|
||||
"golang.org/x/sys/unix"
|
||||
)
|
||||
|
||||
var (
|
||||
EParseGroup = errors.New("Error parsing multicast group data")
|
||||
)
|
||||
|
||||
type McastGroup struct {
|
||||
Id uint32
|
||||
Name string
|
||||
}
|
||||
|
||||
|
||||
|
||||
func ParseAttrsToMcastGroups(attrs []mnetlink.Attr) (groups []McastGroup, err error) {
|
||||
for _,attr := range attrs {
|
||||
//attr type == mcast group index
|
||||
// attr.data == []attr describing group
|
||||
grp_attrs,err := attr.GetDataAttrs()
|
||||
if err != nil { return nil,err }
|
||||
mcats_grp,err := ParseAttrsToMcastGroup(grp_attrs)
|
||||
if err != nil { return nil,err }
|
||||
|
||||
groups = append(groups, mcats_grp)
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func ParseAttrsToMcastGroup(attrs []mnetlink.Attr) (group McastGroup, err error) {
|
||||
for _,attr := range attrs {
|
||||
switch attr.Type {
|
||||
case unix.CTRL_ATTR_MCAST_GRP_ID:
|
||||
//fmt.Printf("Multicast Group ID: %d\n", attr.GetDataUint32())
|
||||
group.Id = attr.GetDataUint32()
|
||||
case unix.CTRL_ATTR_MCAST_GRP_NAME:
|
||||
//fmt.Printf("Multicast Group Name: %s\n", attr.GetDataString())
|
||||
group.Name = attr.GetDataString()
|
||||
case unix.CTRL_ATTR_MCAST_GRP_UNSPEC:
|
||||
//fmt.Printf("Multicast Group Unspec: \n%v\n", attr.GetDataDump())
|
||||
return group,EParseGroup
|
||||
default:
|
||||
fmt.Printf("Unknown McastGrp attr %d: \n%v\n", attr.Type, attr.GetDataDump())
|
||||
return group,EParseGroup
|
||||
}
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
53
mgenetlink/message.go
Normal file
53
mgenetlink/message.go
Normal file
@ -0,0 +1,53 @@
|
||||
package mgenetlink
|
||||
|
||||
import (
|
||||
"github.com/mame82/P4wnP1_go/mnetlink"
|
||||
"golang.org/x/sys/unix"
|
||||
"errors"
|
||||
)
|
||||
|
||||
var (
|
||||
EInvalidMessage = errors.New("Invalid generic netlink message")
|
||||
)
|
||||
|
||||
|
||||
type Message struct {
|
||||
Cmd uint8
|
||||
Version uint8
|
||||
Reserved uint16
|
||||
Data []byte
|
||||
}
|
||||
|
||||
func (m *Message) MarshalBinary() (data []byte, err error) {
|
||||
data = make([]byte, unix.GENL_HDRLEN + len(m.Data))
|
||||
|
||||
data[0] = m.Cmd
|
||||
data[1] = m.Version
|
||||
copy(data[unix.GENL_HDRLEN:], m.Data)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func (m *Message) UnmarshalBinary(src []byte) (err error) {
|
||||
if len(src) < unix.GENL_HDRLEN {
|
||||
return EInvalidMessage
|
||||
}
|
||||
|
||||
m.Cmd = src[0]
|
||||
m.Version = src[1]
|
||||
|
||||
m.Data = src[unix.GENL_HDRLEN:]
|
||||
return
|
||||
}
|
||||
|
||||
func (m Message) AttributesFromData() (attrs []mnetlink.Attr, err error) {
|
||||
for offs := 0; len(m.Data[offs:]) >= unix.NLA_HDRLEN; {
|
||||
attr := mnetlink.Attr{}
|
||||
err = attr.UnmarshalBinary(m.Data[offs:])
|
||||
if err != nil { return nil, err }
|
||||
attrs = append(attrs, attr)
|
||||
offs += mnetlink.AlignAttr(int(attr.Len))
|
||||
}
|
||||
|
||||
return attrs, nil
|
||||
}
|
||||
44
mgenetlink/ops.go
Normal file
44
mgenetlink/ops.go
Normal file
@ -0,0 +1,44 @@
|
||||
package mgenetlink
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"github.com/mame82/P4wnP1_go/mnetlink"
|
||||
"golang.org/x/sys/unix"
|
||||
)
|
||||
|
||||
type Op struct {
|
||||
Id uint32
|
||||
Flags uint32
|
||||
}
|
||||
|
||||
func ParseAttrsToOps(attrs []mnetlink.Attr) (ops []Op, err error) {
|
||||
for _,attr := range attrs {
|
||||
//attr type == mcast group index
|
||||
// attr.data == []attr describing group
|
||||
op_attrs,err := attr.GetDataAttrs()
|
||||
if err != nil { return nil,err }
|
||||
op,err := ParseAttrsToOp(op_attrs)
|
||||
if err != nil { return nil,err }
|
||||
|
||||
ops = append(ops, op)
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func ParseAttrsToOp(attrs []mnetlink.Attr) (op Op, err error) {
|
||||
for _,attr := range attrs {
|
||||
switch attr.Type {
|
||||
case unix.CTRL_ATTR_OP_ID:
|
||||
op.Id = attr.GetDataUint32()
|
||||
case unix.CTRL_ATTR_OP_FLAGS:
|
||||
op.Flags = attr.GetDataUint32()
|
||||
default:
|
||||
fmt.Printf("Unknown Op attr %d: \n%v\n", attr.Type, attr.GetDataDump())
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
102
mnetlink/attributes.go
Normal file
102
mnetlink/attributes.go
Normal file
@ -0,0 +1,102 @@
|
||||
package mnetlink
|
||||
|
||||
import (
|
||||
"encoding/hex"
|
||||
"errors"
|
||||
"golang.org/x/sys/unix"
|
||||
)
|
||||
|
||||
|
||||
|
||||
var (
|
||||
EInvalidAttribute = errors.New("Invalid netlink attribute")
|
||||
)
|
||||
|
||||
func AlignAttr(len int) int {
|
||||
return ((len) + unix.NLA_ALIGNTO - 1) & ^( unix.NLA_ALIGNTO - 1)
|
||||
}
|
||||
|
||||
type Attr struct {
|
||||
Type uint16
|
||||
Len uint16
|
||||
data []byte
|
||||
}
|
||||
|
||||
func (a *Attr) UnmarshalBinary(data []byte) (err error) {
|
||||
if len(data) < unix.NLA_HDRLEN {
|
||||
// less data than attribute header length
|
||||
err = EInvalidAttribute
|
||||
return
|
||||
}
|
||||
|
||||
a.Len = hbo.Uint16(data[0:2])
|
||||
a.Type = hbo.Uint16(data[2:4])
|
||||
|
||||
if AlignAttr(int(a.Len)) > len(data) {
|
||||
return EInvalidAttribute
|
||||
}
|
||||
|
||||
a.data = make([]byte, a.Len - unix.NLA_HDRLEN)
|
||||
copy(a.data, data[unix.NLA_HDRLEN:])
|
||||
return
|
||||
}
|
||||
|
||||
func (a *Attr) MarshalBinary() (data []byte, err error) {
|
||||
if a.Len < unix.NLA_HDRLEN {
|
||||
// less data than attribute header length
|
||||
err = EInvalidAttribute
|
||||
return
|
||||
}
|
||||
|
||||
data = make([]byte, unix.NLA_HDRLEN + AlignAttr(len(a.data)))
|
||||
hbo.PutUint16(data[0:2], a.Len)
|
||||
hbo.PutUint16(data[2:4], a.Type)
|
||||
copy(data[unix.NLA_HDRLEN:], a.data)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
|
||||
|
||||
func (a *Attr) SetData(data []byte) (err error) {
|
||||
a.data = data
|
||||
a.Len = uint16(len(data) + unix.NLA_HDRLEN)
|
||||
return
|
||||
}
|
||||
|
||||
func (a Attr) GetData() []byte {
|
||||
return a.data
|
||||
}
|
||||
|
||||
func (a Attr) GetDataString() string {
|
||||
return Bytes2Str(a.data)
|
||||
}
|
||||
|
||||
func (a Attr) GetDataUint32() uint32 {
|
||||
return hbo.Uint32(a.data[0:4])
|
||||
}
|
||||
|
||||
func (a Attr) GetDataUint16() uint16 {
|
||||
return hbo.Uint16(a.data[0:2])
|
||||
}
|
||||
|
||||
func (a Attr) GetDataUint8() uint8 {
|
||||
return uint8(a.data[0])
|
||||
}
|
||||
|
||||
func (a Attr) GetDataDump() string {
|
||||
return hex.Dump(a.data)
|
||||
}
|
||||
|
||||
func (a Attr) GetDataAttrs() (attrs []Attr, err error) {
|
||||
for offs := 0; len(a.data[offs:]) >= unix.NLA_HDRLEN; {
|
||||
attr := Attr{}
|
||||
err = attr.UnmarshalBinary(a.data[offs:])
|
||||
if err != nil { return nil, err }
|
||||
attrs = append(attrs, attr)
|
||||
offs += AlignAttr(int(attr.Len))
|
||||
}
|
||||
|
||||
return attrs, nil
|
||||
}
|
||||
|
||||
178
mnetlink/client.go
Normal file
178
mnetlink/client.go
Normal file
@ -0,0 +1,178 @@
|
||||
package mnetlink
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"github.com/pkg/errors"
|
||||
"golang.org/x/sys/unix"
|
||||
"math/rand"
|
||||
"os"
|
||||
"sync/atomic"
|
||||
"syscall"
|
||||
"time"
|
||||
)
|
||||
|
||||
type Client struct {
|
||||
Family int
|
||||
sock_fd int
|
||||
seq uint32
|
||||
pid uint32
|
||||
}
|
||||
|
||||
func NewNl(family int) (res *Client, err error) {
|
||||
res = &Client{
|
||||
Family: family,
|
||||
}
|
||||
|
||||
// random start seq
|
||||
r := rand.New(rand.NewSource(time.Now().UnixNano()))
|
||||
res.seq = r.Uint32()
|
||||
// assign current PID as PortID
|
||||
res.pid = uint32(os.Getpid())
|
||||
|
||||
|
||||
return res,nil
|
||||
}
|
||||
|
||||
func (c *Client) incSeq() {
|
||||
atomic.AddUint32(&c.seq,1)
|
||||
}
|
||||
|
||||
func (c *Client) Open() (err error) {
|
||||
// if family is 0, choose NETLINK_USERSOCK by default
|
||||
if c.Family == 0 { c.Family = unix.NETLINK_USERSOCK }
|
||||
|
||||
c.sock_fd,err = unix.Socket(unix.AF_NETLINK, unix.SOCK_RAW, c.Family)
|
||||
if err != nil { return }
|
||||
|
||||
|
||||
// bind socket
|
||||
err = unix.Bind(c.sock_fd, &unix.SockaddrNetlink{
|
||||
Family: unix.AF_NETLINK,
|
||||
Groups: 0,
|
||||
Pid: uint32(os.Getpid()),
|
||||
})
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) Close() (err error) {
|
||||
unix.Close(c.sock_fd)
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) AddGroupMembership(groupid int) (err error) {
|
||||
err = syscall.SetsockoptInt(c.sock_fd, unix.SOL_NETLINK, unix.NETLINK_ADD_MEMBERSHIP, groupid)
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) DropGroupMembership(groupid int) (err error) {
|
||||
err = syscall.SetsockoptInt(c.sock_fd, unix.SOL_NETLINK, unix.NETLINK_DROP_MEMBERSHIP, groupid)
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) Sendmsg(p, oob []byte, to unix.Sockaddr, flags int) (err error) {
|
||||
err = unix.Sendmsg(c.sock_fd, p, oob, to, flags)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func (c *Client) Send(msg Message) (err error) {
|
||||
// adjust seq
|
||||
msg.Seq = c.seq
|
||||
msg.Pid = c.pid
|
||||
|
||||
|
||||
raw,err := msg.MarshalBinary()
|
||||
if err != nil { return }
|
||||
|
||||
addr := &unix.SockaddrNetlink{
|
||||
Family: unix.AF_NETLINK,
|
||||
}
|
||||
|
||||
//fmt.Printf("Sending raw2:\n%+v\n", hex.Dump(raw))
|
||||
err = c.Sendmsg(raw, nil, addr, 0)
|
||||
if err == nil { c.incSeq() }
|
||||
return err
|
||||
}
|
||||
|
||||
func (c *Client) Receive() (msgs []Message, err error) {
|
||||
for {
|
||||
//fmt.Println("Reading")
|
||||
raw_in := c.Read()
|
||||
for len(raw_in) > unix.NLMSG_HDRLEN {
|
||||
msg := Message{}
|
||||
msg.UnmarshalBinary(raw_in)
|
||||
|
||||
if msg.IsTypeError() {
|
||||
return nil,errors.New(fmt.Sprintf("Error response: %+v %+v", msg.GetData(), msg.GetErrNo()))
|
||||
}
|
||||
|
||||
msgs = append(msgs, msg)
|
||||
//fmt.Printf("Received raw: \n%v\n", hex.Dump(raw_in))
|
||||
//fmt.Printf("Received %+v\nData:\n%+v\n", msg, hex.Dump(msg.data))
|
||||
|
||||
// check if last message
|
||||
if msg.IsTypeDone() || !msg.HasFlagMulti() {
|
||||
return
|
||||
}
|
||||
|
||||
raw_in = raw_in[AlignMsg(int(msg.Len)):]
|
||||
}
|
||||
|
||||
}
|
||||
return
|
||||
}
|
||||
/*
|
||||
func (c *Client) Receive2() (msgs []Message, err error) {
|
||||
for {
|
||||
//fmt.Println("Reading")
|
||||
raw_in := c.Read()
|
||||
msg := Message{}
|
||||
msg.UnmarshalBinary(raw_in)
|
||||
|
||||
if msg.IsTypeError() {
|
||||
return nil,errors.New(fmt.Sprintf("Error response: %+v %+v", msg.GetData(), msg.GetErrNo()))
|
||||
|
||||
}
|
||||
|
||||
msgs = append(msgs, msg)
|
||||
fmt.Printf("Received raw: \n%v\n", hex.Dump(raw_in))
|
||||
fmt.Printf("Received %+v\nData:\n%+v\n", msg, hex.Dump(msg.data))
|
||||
|
||||
// check if last message
|
||||
if msg.IsTypeDone() || !msg.HasFlagMulti() {
|
||||
break
|
||||
}
|
||||
}
|
||||
return
|
||||
}
|
||||
*/
|
||||
func (c *Client) Read() (res []byte) {
|
||||
rcvBuf := make([]byte, os.Getpagesize())
|
||||
for {
|
||||
//fmt.Println("calling receive")
|
||||
|
||||
// peek into rcv to fetch bytes available
|
||||
n,_,_ := unix.Recvfrom(c.sock_fd, rcvBuf, unix.MSG_PEEK)
|
||||
//fmt.Println("Bytes received: ", n)
|
||||
|
||||
if len(rcvBuf) < n {
|
||||
fmt.Println("Receive buffer too small, increasing...")
|
||||
rcvBuf = make([]byte, len(rcvBuf)*2)
|
||||
} else {
|
||||
break
|
||||
}
|
||||
}
|
||||
n,_,_ := unix.Recvfrom(c.sock_fd, rcvBuf, 0)
|
||||
|
||||
nlMsgRaw := make([]byte, n)
|
||||
copy(nlMsgRaw, rcvBuf) // Copy over as many bytes as readen
|
||||
|
||||
return nlMsgRaw
|
||||
/*
|
||||
msg := NlMsg{}
|
||||
msg.fromWire(nlMsgRaw)
|
||||
return msg.Payload
|
||||
*/
|
||||
}
|
||||
|
||||
32
mnetlink/common.go
Normal file
32
mnetlink/common.go
Normal file
@ -0,0 +1,32 @@
|
||||
package mnetlink
|
||||
|
||||
import (
|
||||
"bytes"
|
||||
"encoding/binary"
|
||||
"unsafe"
|
||||
)
|
||||
|
||||
var hbo = HostByteOrder()
|
||||
|
||||
func Hbo() binary.ByteOrder {
|
||||
return hbo
|
||||
}
|
||||
|
||||
// Detect host byteorder (used by netlink)
|
||||
func HostByteOrder() (res binary.ByteOrder) {
|
||||
i := int(0x0100)
|
||||
ptr := unsafe.Pointer(&i)
|
||||
if 0x01 == *(*byte)(ptr) {
|
||||
return binary.BigEndian
|
||||
}
|
||||
return binary.LittleEndian
|
||||
}
|
||||
|
||||
|
||||
func Str2Bytes(s string) []byte {
|
||||
return append([]byte(s), 0x00)
|
||||
}
|
||||
|
||||
func Bytes2Str(b []byte) string {
|
||||
return string(bytes.TrimSuffix(b, []byte{0x00}))
|
||||
}
|
||||
98
mnetlink/message.go
Normal file
98
mnetlink/message.go
Normal file
@ -0,0 +1,98 @@
|
||||
package mnetlink
|
||||
|
||||
import (
|
||||
"errors"
|
||||
"golang.org/x/sys/unix"
|
||||
"syscall"
|
||||
"unsafe"
|
||||
)
|
||||
|
||||
var (
|
||||
EInvalidMessage = errors.New("Invalid netlink message")
|
||||
)
|
||||
|
||||
|
||||
func AlignMsg(len int) int {
|
||||
return ((len) + unix.NLMSG_ALIGNTO - 1) & ^( unix.NLMSG_ALIGNTO - 1)
|
||||
}
|
||||
|
||||
type Message struct {
|
||||
Len uint32
|
||||
Type uint16
|
||||
Flags uint16
|
||||
Seq uint32
|
||||
Pid uint32
|
||||
data []byte
|
||||
}
|
||||
|
||||
func (m *Message) MarshalBinary() (data []byte, err error) {
|
||||
msgLen := AlignMsg(int(m.Len))
|
||||
if msgLen < unix.NLMSG_HDRLEN {
|
||||
return nil, EInvalidMessage
|
||||
}
|
||||
|
||||
data = make([]byte, msgLen)
|
||||
|
||||
hbo.PutUint32(data[0:4], m.Len)
|
||||
hbo.PutUint16(data[4:6], uint16(m.Type))
|
||||
hbo.PutUint16(data[6:8], uint16(m.Flags))
|
||||
hbo.PutUint32(data[8:12], m.Seq)
|
||||
hbo.PutUint32(data[12:16], m.Pid)
|
||||
copy(data[unix.NLMSG_HDRLEN:], m.data)
|
||||
|
||||
return
|
||||
}
|
||||
|
||||
func (m *Message) UnmarshalBinary(src []byte) (err error) {
|
||||
m.Len = hbo.Uint32(src[0:4])
|
||||
// truncate message to length
|
||||
src = src[:m.Len]
|
||||
|
||||
m.Type = hbo.Uint16(src[4:6])
|
||||
m.Flags = hbo.Uint16(src[6:8])
|
||||
m.Seq = hbo.Uint32(src[8:12])
|
||||
m.Pid = hbo.Uint32(src[12:16])
|
||||
m.data = src[16:]
|
||||
return
|
||||
}
|
||||
|
||||
func (m *Message) SetData(data []byte) (err error) {
|
||||
m.data = data
|
||||
m.Len = uint32(len(data) + unix.NLMSG_HDRLEN)
|
||||
return
|
||||
}
|
||||
|
||||
func (m Message) GetData() []byte {
|
||||
return m.data
|
||||
}
|
||||
|
||||
func (m Message) HasFlagMulti() bool {
|
||||
return (m.Flags & unix.NLM_F_MULTI) > 0
|
||||
}
|
||||
|
||||
func (m Message) HasFlagDump() bool {
|
||||
return (m.Flags & unix.NLM_F_DUMP) > 0
|
||||
}
|
||||
|
||||
func (m Message) HasFlagAck() bool {
|
||||
return (m.Flags & unix.NLM_F_ACK) > 0
|
||||
}
|
||||
|
||||
func (m Message) IsTypeDone() bool {
|
||||
return m.Type == unix.NLMSG_DONE
|
||||
}
|
||||
|
||||
func (m Message) IsTypeError() bool {
|
||||
return m.Type == unix.NLMSG_ERROR
|
||||
}
|
||||
|
||||
func (m Message) IsTypeNoop() bool {
|
||||
return m.Type == unix.NLMSG_NOOP
|
||||
}
|
||||
|
||||
func (m Message) GetErrNo() error {
|
||||
//neg_err := int(hbo.Uint32(m.data[0:4]))
|
||||
neg_err := *(*int32)(unsafe.Pointer(&m.data[0]))
|
||||
|
||||
return syscall.Errno(neg_err * -1)
|
||||
}
|
||||
71
ntest.go
71
ntest.go
@ -5,7 +5,8 @@ package main
|
||||
import (
|
||||
"errors"
|
||||
"fmt"
|
||||
"github.com/mame82/P4wnP1_go/service/peripheral"
|
||||
"github.com/mame82/P4wnP1_go/genl_p4wnp1"
|
||||
"golang.org/x/sys/unix"
|
||||
"io/ioutil"
|
||||
"os"
|
||||
"os/signal"
|
||||
@ -205,9 +206,70 @@ func ReadState() (regval *HprtData, err error) {
|
||||
|
||||
|
||||
|
||||
|
||||
func init_nl() (sock_fd int, err error) {
|
||||
sock_fd,err = unix.Socket(unix.AF_NETLINK, unix.SOCK_RAW, unix.NETLINK_GENERIC) // Socket to netlink generic family
|
||||
if err != nil { return }
|
||||
|
||||
err = unix.Bind(sock_fd, &unix.SockaddrNetlink {
|
||||
Family: unix.AF_NETLINK,
|
||||
Groups: 0,
|
||||
Pid: uint32(os.Getpid()),
|
||||
})
|
||||
if err != nil { return }
|
||||
|
||||
//err = syscall.SetsockoptInt(d.sock_fd, unix.SOL_NETLINK, unix.NETLINK_ADD_MEMBERSHIP, d.nl_group)
|
||||
|
||||
return
|
||||
|
||||
}
|
||||
|
||||
func deinit_nl() {
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
func main() {
|
||||
wo := peripheral.WaveshareOled{}
|
||||
wo.Start()
|
||||
|
||||
/*
|
||||
genl,err := mgennetlink.NewGeNl()
|
||||
if err != nil { panic(err) }
|
||||
genl.Open()
|
||||
defer genl.Close()
|
||||
|
||||
fam_name := "p4wnp1"
|
||||
dwc2_group_name := "p4wnp1_dwc2_mc"
|
||||
|
||||
fam,err := genl.GetFamily(fam_name)
|
||||
if err != nil { panic(err) }
|
||||
fmt.Printf("FAMILY\n=====\n%+v\n", fam)
|
||||
|
||||
dwc2grpId,err := fam.GetGroupByName(dwc2_group_name)
|
||||
if err != nil { panic(err) }
|
||||
|
||||
fmt.Println("Join Group: ", dwc2_group_name)
|
||||
genl.AddGroupMembership(dwc2grpId)
|
||||
|
||||
|
||||
for {
|
||||
fmt.Println("Recieve....")
|
||||
msgs,errm := genl.Receive()
|
||||
if errm == nil {
|
||||
fmt.Printf("Messages:\n%+v\n", msgs)
|
||||
} else {
|
||||
fmt.Println("Receive error: ", errm)
|
||||
}
|
||||
}
|
||||
*/
|
||||
|
||||
c,err := genl_p4wnp1.NewClient()
|
||||
if err != nil { panic(err) }
|
||||
c.Open()
|
||||
|
||||
//wo := peripheral.WaveshareOled{}
|
||||
//wo.Start()
|
||||
/*
|
||||
dwc := dwc2.NewDwc2Nl(24)
|
||||
err := dwc.OpenNlKernelSock()
|
||||
@ -245,7 +307,8 @@ func main() {
|
||||
signal.Notify(sig, syscall.SIGINT, syscall.SIGTERM)
|
||||
si := <-sig
|
||||
fmt.Printf("Signal (%v) received, ending P4wnP1_service ...\n", si)
|
||||
wo.Stop()
|
||||
//wo.Stop()
|
||||
//w.Stop()
|
||||
|
||||
c.Close()
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user