mirror of
https://github.com/RoganDawes/P4wnP1_aloa.git
synced 2025-03-27 01:51:45 +01:00
Icnluded USB host connect/disconnect trigger based on modded dwc2 kernel module
This commit is contained in:
MaMe82
parent
073e96de12
commit
eeb127c290
@ -3,12 +3,9 @@
|
||||
package main
|
||||
|
||||
import (
|
||||
"log"
|
||||
"strconv"
|
||||
"time"
|
||||
|
||||
"fmt"
|
||||
"github.com/mame82/P4wnP1_go/service"
|
||||
"log"
|
||||
"os"
|
||||
"os/signal"
|
||||
"syscall"
|
||||
@ -37,7 +34,7 @@ func main() {
|
||||
}
|
||||
svc.Start()
|
||||
|
||||
|
||||
/*
|
||||
// ToDo: Remove this (testing only)
|
||||
//Send some log messages for testing
|
||||
textfill := "Lorem ipsum dolor sit amet, consetetur sadipscing elitr, sed diam nonumy eirmod tempor invidunt ut labore et dolore magna aliquyam erat, sed diam voluptua. At vero eos et accusam et justo duo dolores et ea"
|
||||
@ -50,7 +47,7 @@ func main() {
|
||||
i++
|
||||
}
|
||||
}()
|
||||
|
||||
*/
|
||||
|
||||
//use a channel to wait for SIGTERM or SIGINT
|
||||
fmt.Println("P4wnP1 service initialized, stop with SIGTERM or SIGINT")
|
||||
|
||||
109
service/connect_watcher.go
Normal file
109
service/connect_watcher.go
Normal file
@ -0,0 +1,109 @@
|
||||
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 = uint32(0x00001000)
|
||||
host_disconnected = uint32(0x00000000)
|
||||
)
|
||||
|
||||
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) != 4 {
|
||||
continue // ignore, we want tor receive an uint32
|
||||
}
|
||||
val := dwc2.Hbo().Uint32(indata)
|
||||
switch val {
|
||||
case host_connected:
|
||||
if d.udateNeeded(true) {
|
||||
d.update(true)
|
||||
}
|
||||
case host_disconnected:
|
||||
if d.udateNeeded(false) {
|
||||
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
|
||||
}
|
||||
|
||||
168
service/dwc2/netlink_mcast.go
Normal file
168
service/dwc2/netlink_mcast.go
Normal file
@ -0,0 +1,168 @@
|
||||
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
|
||||
}
|
||||
@ -98,11 +98,10 @@ func RegisterDefaultTriggerActions(tam *TriggerActionManager) {
|
||||
tam.AddTriggerAction(logSSHLogin)
|
||||
}
|
||||
|
||||
|
||||
type Service struct {
|
||||
SubSysDataStore *datastore.Store // very first service
|
||||
// SubSysState interface{}
|
||||
// SubSysLogging interface{}
|
||||
SubSysDataStore *datastore.Store // very first service
|
||||
// SubSysState interface{}
|
||||
// SubSysLogging interface{}
|
||||
SubSysNetwork *NetworkManager
|
||||
|
||||
SubSysEvent *EventManager
|
||||
@ -112,35 +111,46 @@ type Service struct {
|
||||
SubSysBluetooth *BtService
|
||||
SubSysRPC *server
|
||||
SubSysTriggerActions *TriggerActionManager
|
||||
|
||||
SubSysDwc2ConnectWatcher *Dwc2ConnectWatcher
|
||||
}
|
||||
|
||||
func NewService() (svc *Service, err error) {
|
||||
svc = &Service{}
|
||||
|
||||
svc.SubSysDataStore,err = datastore.Open(pPATH_DATA_STORE)
|
||||
if err != nil { return nil,err}
|
||||
svc.SubSysDataStore, err = datastore.Open(pPATH_DATA_STORE)
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
svc.SubSysEvent = NewEventManager(20)
|
||||
|
||||
svc.SubSysLed = NewLedService()
|
||||
svc.SubSysNetwork, err = NewNetworkManager()
|
||||
if err != nil { return nil,err}
|
||||
svc.SubSysUSB,err = NewUSBGadgetManager(svc) //Depends on NetworkSubSys, EvenSubSys
|
||||
// if err == ErrUsbNotUsable { err = nil } //ToDo: delete this
|
||||
if err != nil { return nil,err}
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
svc.SubSysUSB, err = NewUSBGadgetManager(svc) //Depends on NetworkSubSys, EvenSubSys
|
||||
// if err == ErrUsbNotUsable { err = nil } //ToDo: delete this
|
||||
if err != nil {
|
||||
return nil, err
|
||||
}
|
||||
|
||||
svc.SubSysWifi = NewWifiService(svc) //Depends on NetworkSubSys
|
||||
|
||||
svc.SubSysBluetooth = NewBtService(svc) //Depends on NetworkSubSys
|
||||
|
||||
svc.SubSysRPC = NewRpcServerService(svc) //Depends on all other
|
||||
svc.SubSysRPC = NewRpcServerService(svc) //Depends on all other
|
||||
|
||||
svc.SubSysTriggerActions = NewTriggerActionManager(svc) //Depends on EventManager, UsbGadgetManager (to trigger HID scripts)
|
||||
|
||||
svc.SubSysDwc2ConnectWatcher = NewDwc2ConnectWatcher(svc) // Depends on EventManager, should be started before USB gadget settings are deployed (to avoid missing initial state change)
|
||||
return
|
||||
}
|
||||
|
||||
func (s *Service) Start() {
|
||||
s.SubSysEvent.Start()
|
||||
s.SubSysDwc2ConnectWatcher.Start()
|
||||
s.SubSysLed.Start()
|
||||
s.SubSysRPC.StartRpcServerAndWeb("0.0.0.0", "50051", "8000", "/usr/local/P4wnP1/www") //start gRPC service
|
||||
s.SubSysTriggerActions.Start()
|
||||
@ -160,6 +170,6 @@ func (s *Service) Stop() {
|
||||
s.SubSysLed.Stop()
|
||||
|
||||
s.SubSysBluetooth.StopNAP()
|
||||
|
||||
s.SubSysDwc2ConnectWatcher.Stop()
|
||||
s.SubSysEvent.Stop()
|
||||
}
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user