diff --git a/genl_p4wnp1/Client.go b/genl_p4wnp1/Client.go new file mode 100644 index 0000000..f3b9ff5 --- /dev/null +++ b/genl_p4wnp1/Client.go @@ -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() + +} diff --git a/mgenetlink/client.go b/mgenetlink/client.go new file mode 100644 index 0000000..1ef6a88 --- /dev/null +++ b/mgenetlink/client.go @@ -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 +} diff --git a/mgenetlink/common.go b/mgenetlink/common.go new file mode 100644 index 0000000..c71cf3e --- /dev/null +++ b/mgenetlink/common.go @@ -0,0 +1,5 @@ +package mgenetlink + +import "github.com/mame82/P4wnP1_go/mnetlink" + +var hbo = mnetlink.Hbo() diff --git a/mgenetlink/family.go b/mgenetlink/family.go new file mode 100644 index 0000000..110bc84 --- /dev/null +++ b/mgenetlink/family.go @@ -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 +} \ No newline at end of file diff --git a/mgenetlink/mcast_group.go b/mgenetlink/mcast_group.go new file mode 100644 index 0000000..fe1fc2c --- /dev/null +++ b/mgenetlink/mcast_group.go @@ -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 +} diff --git a/mgenetlink/message.go b/mgenetlink/message.go new file mode 100644 index 0000000..2966bf6 --- /dev/null +++ b/mgenetlink/message.go @@ -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 +} diff --git a/mgenetlink/ops.go b/mgenetlink/ops.go new file mode 100644 index 0000000..d6eb62b --- /dev/null +++ b/mgenetlink/ops.go @@ -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 +} + diff --git a/mnetlink/attributes.go b/mnetlink/attributes.go new file mode 100644 index 0000000..e67eb71 --- /dev/null +++ b/mnetlink/attributes.go @@ -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 +} + diff --git a/mnetlink/client.go b/mnetlink/client.go new file mode 100644 index 0000000..43b8e0a --- /dev/null +++ b/mnetlink/client.go @@ -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 + */ +} + diff --git a/mnetlink/common.go b/mnetlink/common.go new file mode 100644 index 0000000..637644b --- /dev/null +++ b/mnetlink/common.go @@ -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})) +} diff --git a/mnetlink/message.go b/mnetlink/message.go new file mode 100644 index 0000000..0e4f6f0 --- /dev/null +++ b/mnetlink/message.go @@ -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) +} \ No newline at end of file diff --git a/ntest.go b/ntest.go index f10e775..978529b 100644 --- a/ntest.go +++ b/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() }