mirror of
https://github.com/RoganDawes/P4wnP1_aloa.git
synced 2025-03-27 01:51:45 +01:00
Changed dwc2 disconnect detection to dwc2
This commit is contained in:
MaMe82
parent
b977db4c34
commit
32147709b3
@ -1,105 +0,0 @@
|
||||
package service
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"github.com/mame82/P4wnP1_go/common_web"
|
||||
"github.com/mame82/P4wnP1_go/service/dwc2"
|
||||
)
|
||||
|
||||
/*
|
||||
Needs modified dwc2 kernel module, sending multicast netlink messages to group 24
|
||||
|
||||
The initial connection state is undefined, the event is fired based on an IRQ of the gadget core (not in host mode)
|
||||
which has the USBRST flag set if a host connection occurs or unset if a disconnection occurs.
|
||||
As these IRQs are triggered, as soon as a new gadget is deployed, the state wouldn't stay undefined, once the USB
|
||||
stack has initialized the gadget settings.
|
||||
|
||||
The mentioned IRQ fires multiple times:
|
||||
On disconnect: two times 0x00000000
|
||||
On connect: 0x00000000 followed by two times 0x00001000
|
||||
|
||||
To cope with that behavior, we only fire an event if the new state differs from the last one
|
||||
*/
|
||||
|
||||
const (
|
||||
mcast_group = 24
|
||||
host_connected = byte(0x01)
|
||||
host_disconnected = byte(0x00)
|
||||
)
|
||||
|
||||
type Dwc2ConnectWatcher struct {
|
||||
rootSvc *Service
|
||||
|
||||
isRunning bool
|
||||
connected bool
|
||||
nl *dwc2.Dwc2Netlink
|
||||
firstUpdateDone bool
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) udateNeeded(newStateConnected bool) (needed bool) {
|
||||
if !d.firstUpdateDone {
|
||||
d.firstUpdateDone = true
|
||||
return true
|
||||
}
|
||||
return newStateConnected != d.connected
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) update(newStateConnected bool) {
|
||||
d.connected = newStateConnected
|
||||
|
||||
// --> here a event could be triggered (in case the event manager is registered)
|
||||
if d.connected {
|
||||
fmt.Println("Connected to USB host")
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventTrigger(common_web.EVT_TRIGGER_TYPE_USB_GADGET_CONNECTED))
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventLog("USB watcher", 1, "Connected to USB host"))
|
||||
|
||||
} else {
|
||||
fmt.Println("Disconnected from USB host")
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventTrigger(common_web.EVT_TRIGGER_TYPE_USB_GADGET_DISCONNECTED))
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventLog("USB watcher", 1, "Disconnected from USB host"))
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
func (d * Dwc2ConnectWatcher) evt_loop() {
|
||||
for d.isRunning {
|
||||
indata := d.nl.Read()
|
||||
if len(indata) != 1 {
|
||||
continue // ignore, we want tor receive an uint32
|
||||
}
|
||||
val := indata[0]
|
||||
switch val {
|
||||
case host_connected:
|
||||
d.update(true)
|
||||
case host_disconnected:
|
||||
d.update(false)
|
||||
default:
|
||||
fmt.Println("Unknown value from DWC2: ", val)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) IsConnected() bool {
|
||||
return d.connected
|
||||
}
|
||||
|
||||
|
||||
func (d * Dwc2ConnectWatcher) Start() {
|
||||
d.nl.OpenNlKernelSock()
|
||||
d.isRunning = true
|
||||
go d.evt_loop()
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) Stop() {
|
||||
d.isRunning = false
|
||||
d.nl.Close()
|
||||
}
|
||||
|
||||
func NewDwc2ConnectWatcher(rootSvc *Service) (d *Dwc2ConnectWatcher) {
|
||||
d = &Dwc2ConnectWatcher{
|
||||
nl: dwc2.NewDwc2Nl(24),
|
||||
rootSvc: rootSvc,
|
||||
}
|
||||
return d
|
||||
}
|
||||
|
||||
@ -1,168 +0,0 @@
|
||||
package dwc2
|
||||
|
||||
import (
|
||||
"encoding/binary"
|
||||
"fmt"
|
||||
"golang.org/x/sys/unix"
|
||||
"os"
|
||||
"syscall"
|
||||
"unsafe"
|
||||
)
|
||||
|
||||
var hbo = HostByteOrder()
|
||||
|
||||
func Hbo() binary.ByteOrder {
|
||||
return hbo
|
||||
}
|
||||
|
||||
type Dwc2Netlink struct {
|
||||
sock_fd int
|
||||
nl_group int
|
||||
}
|
||||
|
||||
func NewDwc2Nl(mcast_group int) (d* Dwc2Netlink) {
|
||||
return &Dwc2Netlink{
|
||||
nl_group: mcast_group,
|
||||
}
|
||||
}
|
||||
|
||||
func (d *Dwc2Netlink) OpenNlKernelSock() (err error) {
|
||||
d.sock_fd,err = unix.Socket(unix.AF_NETLINK, unix.SOCK_RAW, unix.NETLINK_USERSOCK)
|
||||
if err != nil { return }
|
||||
|
||||
err = unix.Bind(d.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 (d *Dwc2Netlink) Close() (err error) {
|
||||
return unix.Close(d.sock_fd)
|
||||
}
|
||||
|
||||
|
||||
|
||||
func (d *Dwc2Netlink) socketReaderLoop() {
|
||||
fmt.Println("Readloop started")
|
||||
rcvBuf := make([]byte, 1024)
|
||||
for {
|
||||
fmt.Println("READER LOOP")
|
||||
|
||||
n, err := syscall.Read(d.sock_fd, rcvBuf)
|
||||
if err != nil || n == 0 {
|
||||
d.Close()
|
||||
fmt.Println("Error reading from socket")
|
||||
break
|
||||
}
|
||||
nlMsg := make([]byte, n)
|
||||
copy(nlMsg, rcvBuf) // Copy over as many bytes as readen
|
||||
|
||||
fmt.Printf("Received packet %+v\n", nlMsg)
|
||||
/*
|
||||
//fmt.Printf("Sending raw event packet to handler loop: %+v\n", nlMsg)
|
||||
select {
|
||||
case m.newRawPacket <- nlMsg:
|
||||
// do nothing
|
||||
case <-m.disposeMgmtConnection:
|
||||
// unblock and exit the loop if eventHandler is closed
|
||||
close(m.newRawPacket)
|
||||
break
|
||||
}
|
||||
*/
|
||||
}
|
||||
|
||||
fmt.Println("Socket read loop exited")
|
||||
}
|
||||
|
||||
func (d *Dwc2Netlink) SocketReaderLoop2() {
|
||||
fmt.Println("Readloop started")
|
||||
rcvBuf := make([]byte, os.Getpagesize())
|
||||
for {
|
||||
//fmt.Println("calling receive")
|
||||
|
||||
// peek into rcv to fetch bytes available
|
||||
n,_,_ := unix.Recvfrom(d.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 {
|
||||
n,_,_ = unix.Recvfrom(d.sock_fd, rcvBuf, 0)
|
||||
|
||||
nlMsgRaw := make([]byte, n)
|
||||
copy(nlMsgRaw, rcvBuf) // Copy over as many bytes as readen
|
||||
|
||||
fmt.Printf("Received packet %+v\n", nlMsgRaw)
|
||||
msg := NlMsg{}
|
||||
msg.fromWire(nlMsgRaw)
|
||||
fmt.Printf("Received message %+v\n", msg)
|
||||
}
|
||||
}
|
||||
|
||||
fmt.Println("Socket read loop exited")
|
||||
}
|
||||
|
||||
func (d *Dwc2Netlink) Read() (res []byte) {
|
||||
rcvBuf := make([]byte, os.Getpagesize())
|
||||
for {
|
||||
//fmt.Println("calling receive")
|
||||
|
||||
// peek into rcv to fetch bytes available
|
||||
n,_,_ := unix.Recvfrom(d.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(d.sock_fd, rcvBuf, 0)
|
||||
|
||||
nlMsgRaw := make([]byte, n)
|
||||
copy(nlMsgRaw, rcvBuf) // Copy over as many bytes as readen
|
||||
|
||||
msg := NlMsg{}
|
||||
msg.fromWire(nlMsgRaw)
|
||||
return msg.Payload
|
||||
}
|
||||
|
||||
|
||||
type NlMsg struct {
|
||||
Length uint32
|
||||
Type uint16
|
||||
Flags uint16
|
||||
SeqNum uint32
|
||||
PortID uint32 // == Process ID for first socket of Proc. If the message comes from kernel (like here) PortID is 0
|
||||
Payload []byte
|
||||
}
|
||||
|
||||
// Note: data is formated in host byte order, unless the attribute NLA_F_NET_BYTEORDER is specified
|
||||
func (m *NlMsg) fromWire(src []byte) {
|
||||
m.Length = hbo.Uint32(src[0:4])
|
||||
// truncate message to length
|
||||
src = src[:m.Length]
|
||||
|
||||
m.Type = hbo.Uint16(src[4:6])
|
||||
m.Flags = hbo.Uint16(src[6:8])
|
||||
m.SeqNum = hbo.Uint32(src[8:12])
|
||||
m.PortID = hbo.Uint32(src[12:16])
|
||||
m.Payload = src[16:]
|
||||
}
|
||||
|
||||
func HostByteOrder() (res binary.ByteOrder) {
|
||||
i := int(0x0100)
|
||||
ptr := unsafe.Pointer(&i)
|
||||
if 0x01 == *(*byte)(ptr) {
|
||||
return binary.BigEndian
|
||||
}
|
||||
return binary.LittleEndian
|
||||
}
|
||||
189
service/dwc2_connect_watcher.go
Normal file
189
service/dwc2_connect_watcher.go
Normal file
@ -0,0 +1,189 @@
|
||||
// +build arm
|
||||
|
||||
package service
|
||||
|
||||
import (
|
||||
"fmt"
|
||||
"errors"
|
||||
"github.com/mame82/P4wnP1_go/common_web"
|
||||
|
||||
genl "github.com/mame82/P4wnP1_go/mgenetlink"
|
||||
nl "github.com/mame82/P4wnP1_go/mnetlink"
|
||||
)
|
||||
|
||||
/*
|
||||
Needs modified dwc2 kernel module, sending multicast generic netlink messages for genl family 'p4wnp1' on multicast group 'dwc2'
|
||||
|
||||
*/
|
||||
|
||||
const (
|
||||
fam_name = "p4wnp1"
|
||||
dwc2_group_name = "dwc2"
|
||||
|
||||
// commands
|
||||
dwc2_cmd_connection_state = uint8(0)
|
||||
|
||||
// attributes
|
||||
dwc2_attr_connection_dummy = uint16(0)
|
||||
dwc2_attr_connection_state = uint16(1)
|
||||
|
||||
)
|
||||
|
||||
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 Dwc2ConnectWatcher struct {
|
||||
rootSvc *Service
|
||||
|
||||
genl *genl.Client
|
||||
fam *genl.Family
|
||||
|
||||
|
||||
isRunning bool
|
||||
connected bool
|
||||
firstUpdateDone bool
|
||||
}
|
||||
|
||||
|
||||
func (d * Dwc2ConnectWatcher) update(newStateConnected bool) {
|
||||
d.connected = newStateConnected
|
||||
|
||||
// --> here a event could be triggered (in case the event manager is registered)
|
||||
if d.connected {
|
||||
fmt.Println("Connected to USB host")
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventTrigger(common_web.EVT_TRIGGER_TYPE_USB_GADGET_CONNECTED))
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventLog("USB watcher", 1, "Connected to USB host"))
|
||||
|
||||
} else {
|
||||
fmt.Println("Disconnected from USB host")
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventTrigger(common_web.EVT_TRIGGER_TYPE_USB_GADGET_DISCONNECTED))
|
||||
d.rootSvc.SubSysEvent.Emit(ConstructEventLog("USB watcher", 1, "Disconnected from USB host"))
|
||||
}
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) parseMsg(msg nl.Message) (cmd genl.Message, err error) {
|
||||
if msg.Type != d.fam.ID {
|
||||
// Multicast message from different familiy, ignore
|
||||
err = EWrongFamily
|
||||
return
|
||||
}
|
||||
|
||||
err = cmd.UnmarshalBinary(msg.GetData())
|
||||
if err != nil { return }
|
||||
return
|
||||
}
|
||||
|
||||
|
||||
func (d * Dwc2ConnectWatcher) evt_loop() {
|
||||
d.isRunning = true
|
||||
// ToDo, make loop stoppable by non-blocking/interruptable socket read a.k.a select with timeout
|
||||
for d.isRunning {
|
||||
fmt.Println("\nWaiting for messages from P4wnP1 kernel mods...\n")
|
||||
msgs,errm := d.genl.Receive()
|
||||
if errm == nil {
|
||||
for _,msg := range msgs {
|
||||
if cmd,errp := d.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())
|
||||
switch param.GetDataUint8() {
|
||||
case 0: //disconnected
|
||||
d.update(false)
|
||||
case 1: //connected
|
||||
d.update(true)
|
||||
}
|
||||
}
|
||||
}
|
||||
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")
|
||||
|
||||
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) IsConnected() bool {
|
||||
return d.connected
|
||||
}
|
||||
|
||||
|
||||
func (d * Dwc2ConnectWatcher) Start() (err error){
|
||||
d.genl,err = genl.NewGeNl() //genl client
|
||||
if err != nil { return err }
|
||||
|
||||
err = d.genl.Open() //Connect to generic netlink
|
||||
if err != nil { return }
|
||||
|
||||
// try to find GENL family for P4wnP1
|
||||
d.fam,err = d.genl.GetFamily(fam_name)
|
||||
if err != nil {
|
||||
d.genl.Close()
|
||||
return EP4wnP1FamilyMissing
|
||||
}
|
||||
|
||||
// try to join group for dwc2
|
||||
grpId,err := d.fam.GetGroupByName(dwc2_group_name)
|
||||
if err != nil {
|
||||
d.genl.Close()
|
||||
return EDwc2GrpMissing
|
||||
}
|
||||
err = d.genl.AddGroupMembership(grpId)
|
||||
if err != nil {
|
||||
d.genl.Close()
|
||||
return EDwc2GrpMissing
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
d.isRunning = true
|
||||
go d.evt_loop()
|
||||
|
||||
return nil
|
||||
}
|
||||
|
||||
func (d * Dwc2ConnectWatcher) Stop() error {
|
||||
d.isRunning = false
|
||||
|
||||
// leave dwc2 group
|
||||
if grpId,err := d.fam.GetGroupByName(dwc2_group_name); err == nil {
|
||||
d.genl.DropGroupMembership(grpId)
|
||||
}
|
||||
// close soket
|
||||
return d.genl.Close()
|
||||
|
||||
}
|
||||
|
||||
func NewDwc2ConnectWatcher(rootSvc *Service) (d *Dwc2ConnectWatcher) {
|
||||
|
||||
d = &Dwc2ConnectWatcher{
|
||||
rootSvc: rootSvc,
|
||||
}
|
||||
return d
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user