From 1884a8decf2273d5c1c554c3271f6e07c5be52dd Mon Sep 17 00:00:00 2001
From: Teemu Ikonen <tpikonen@mailbox.org>
Date: Mon, 4 Sep 2023 20:41:51 +0300
Subject: [PATCH] Add modemmanager driver (MR 12)
The driver is called 'mm' in config.
Requires the dbus module.
[ci:skip-build] already built successfully in CI
---
cmd/gnss-share/main.go | 2 +
gnss-share.conf | 2 +-
go.mod | 1 +
go.sum | 2 +
internal/gnss/modemmanager.go | 298 ++++++++++++++++++++++++++++++++++
5 files changed, 304 insertions(+), 1 deletion(-)
create mode 100644 internal/gnss/modemmanager.go
diff --git a/cmd/gnss-share/main.go b/cmd/gnss-share/main.go
index e2684df..77b6ab0 100644
--- a/cmd/gnss-share/main.go
+++ b/cmd/gnss-share/main.go
@@ -57,6 +57,8 @@ func main() {
driver = gnss.NewStmGnss(conf.DevicePath, debug)
case "stm_serial":
driver = gnss.NewStmSerial(conf.DevicePath, conf.BaudRate, debug)
+ case "mm":
+ driver = gnss.NewModemManager(debug)
}
gnssDevice, err := gnss.New(driver)
if err != nil {
diff --git a/gnss-share.conf b/gnss-share.conf
index 55aa8fc..4e53e36 100644
--- a/gnss-share.conf
+++ b/gnss-share.conf
@@ -4,7 +4,7 @@ socket="/var/run/gnss-share.sock"
group="geoclue"
# GPS device driver to use
-# Supported values: stm, stm_serial
+# Supported values: stm, stm_serial, mm
device_driver="stm"
# Path to GPS device to use
diff --git a/go.mod b/go.mod
index d6684b4..8ae21b2 100644
--- a/go.mod
+++ b/go.mod
@@ -3,6 +3,7 @@ module gitlab.com/postmarketOS/gnss-share
go 1.15
require (
+ github.com/godbus/dbus/v5 v5.1.0
github.com/google/uuid v1.3.0
github.com/pelletier/go-toml v1.9.4
github.com/tarm/serial v0.0.0-20180830185346-98f6abe2eb07
diff --git a/go.sum b/go.sum
index 0d3d4d6..0fa9402 100644
--- a/go.sum
+++ b/go.sum
@@ -1,3 +1,5 @@
+github.com/godbus/dbus/v5 v5.1.0 h1:4KLkAxT3aOY8Li4FRJe/KvhoNFFxo0m6fNuFUO8QJUk=
+github.com/godbus/dbus/v5 v5.1.0/go.mod h1:xhWf0FNVPg57R7Z0UbKHbJfkEywrmjJnf7w5xrFpKfA=
github.com/google/uuid v1.3.0 h1:t6JiXgmwXMjEs8VusXIJk2BXHsn+wx8BZdTaoZ5fu7I=
github.com/google/uuid v1.3.0/go.mod h1:TIyPZe4MgqvfeYDBFedMoGGpEw/LqOeaOT+nhxU+yHo=
github.com/pelletier/go-toml v1.9.4 h1:tjENF6MfZAg8e4ZmZTeWaWiT2vXtsoO6+iuOjFhECwM=
diff --git a/internal/gnss/modemmanager.go b/internal/gnss/modemmanager.go
new file mode 100644
index 0000000..a9c8578
--- /dev/null
+++ b/internal/gnss/modemmanager.go
@@ -0,0 +1,298 @@
+// Copyright 2023 Teemu Ikonen <tpikonen@mailbox.org>
+// SPDX-License-Identifier: GPL-3.0-or-later
+
+package gnss
+
+import (
+ "bytes"
+ "context"
+ "fmt"
+ "time"
+
+ dbus "github.com/godbus/dbus/v5"
+)
+
+const (
+ // Timeout for Dbus calls
+ DbusCallTimeout = 10 * time.Second
+ // Timeout for GPS setup Dbus calls
+ // (MM GPS setup sometimes takes > 20 s, e.g. on Oneplus 6)
+ GpsSetupTimeout = 50 * time.Second
+ // MMModemLocationSource
+ mmModemLocationSourceGpsNmea uint32 = 1 << 2
+ // MMModemState
+ mmModemStateEnabled int32 = 6
+)
+
+type ModemManager struct {
+ modemObj dbus.BusObject
+ systemBus *dbus.Conn
+ debug bool
+}
+
+func NewModemManager(debug bool) *ModemManager {
+ m := ModemManager{
+ modemObj: nil,
+ systemBus: nil,
+ debug: debug,
+ }
+ return &m
+}
+
+func (m *ModemManager) ensure_bus() error {
+ if m.systemBus != nil {
+ return nil
+ }
+ var err error
+ m.systemBus, err = dbus.ConnectSystemBus()
+ if err != nil {
+ return err
+ }
+ ctx, cancel := context.WithTimeout(context.Background(), DbusCallTimeout)
+ err = m.systemBus.AddMatchSignalContext(ctx,
+ dbus.WithMatchObjectPath("/org/freedesktop/ModemManager1"),
+ dbus.WithMatchInterface("org.freedesktop.DBus.ObjectManager"),
+ dbus.WithMatchMember("InterfacesAdded"),
+ )
+ cancel() // Free context resources
+ if err != nil {
+ return err
+ }
+ ctx, cancel = context.WithTimeout(context.Background(), DbusCallTimeout)
+ err = m.systemBus.AddMatchSignalContext(ctx,
+ dbus.WithMatchObjectPath("/org/freedesktop/ModemManager1"),
+ dbus.WithMatchInterface("org.freedesktop.DBus.ObjectManager"),
+ dbus.WithMatchMember("InterfacesRemoved"),
+ )
+ cancel() // Free context resources
+ if err != nil {
+ return err
+ }
+
+ return nil
+}
+
+func wait_for_modem_enabled(modem dbus.BusObject, maxwait int) error {
+ var state int32 = -1
+
+ for i := 0; i < maxwait; i++ {
+ err := modem.StoreProperty("org.freedesktop.ModemManager1.Modem.State", &state)
+ if err == nil && state >= mmModemStateEnabled {
+ return nil
+ }
+ time.Sleep(1 * time.Second)
+ }
+
+ return fmt.Errorf("modem not enabled after waiting %d seconds", maxwait)
+}
+
+func (m *ModemManager) initialize_modem() error {
+ if err := m.ensure_bus(); err != nil {
+ return err
+ }
+
+ ctx, cancel := context.WithTimeout(context.Background(), DbusCallTimeout)
+ manager := m.systemBus.Object("org.freedesktop.ModemManager1", dbus.ObjectPath("/org/freedesktop/ModemManager1"))
+ getcall := manager.CallWithContext(ctx, "org.freedesktop.DBus.ObjectManager.GetManagedObjects", 0)
+ cancel()
+ if getcall.Err != nil {
+ return fmt.Errorf("unable to enumerate modems")
+ }
+ // The return value of GetManagedObjects is a somewhat involved map of map of map,
+ // see https://dbus.freedesktop.org/doc/dbus-specification.html#standard-interfaces-objectmanager
+ // We're only interested in the keys of the first map (the object paths) though.
+ modems := make(map[dbus.ObjectPath]map[string]map[string]dbus.Variant)
+ getcall.Store(&modems)
+ if len(modems) < 1 {
+ return fmt.Errorf("no modems found")
+ }
+ paths := make([]dbus.ObjectPath, 0, len(modems))
+ for k := range modems {
+ paths = append(paths, k)
+ }
+ if m.debug {
+ fmt.Println("Modems found:", paths)
+ }
+
+ m.modemObj = nil
+ // Find a modemObj which has a GPS NMEA location source and is enabled
+ for _, path := range paths {
+ var capabilities uint32
+
+ modem := m.systemBus.Object("org.freedesktop.ModemManager1", path)
+ err := modem.StoreProperty("org.freedesktop.ModemManager1.Modem.Location.Capabilities", &capabilities)
+ if err != nil || (capabilities&mmModemLocationSourceGpsNmea == 0) {
+ continue
+ }
+ err = wait_for_modem_enabled(modem, 5)
+ if err == nil {
+ if m.debug {
+ fmt.Printf("Modem %s is enabled and has GPS\n", path)
+ }
+ m.modemObj = modem
+ break
+ } else {
+ if m.debug {
+ fmt.Printf("Modem %s: %s\n", path, err)
+ }
+ }
+ }
+ if m.modemObj == nil {
+ return fmt.Errorf("could not find enabled modem with GPS capability")
+ }
+
+ return nil
+}
+
+func (m *ModemManager) enable_gps(refresh_interval time.Duration) (uint32, error) {
+ var enabled uint32 = 0
+ err := m.modemObj.StoreProperty("org.freedesktop.ModemManager1.Modem.Location.Enabled", &enabled)
+ if err != nil {
+ return 0, fmt.Errorf("unable get enabled location sources: %w", err)
+ }
+
+ ctx, cancel_setup := context.WithTimeout(context.Background(), GpsSetupTimeout)
+ err = m.modemObj.CallWithContext(ctx, "org.freedesktop.ModemManager1.Modem.Location.Setup", 0, enabled|mmModemLocationSourceGpsNmea, false).Err
+ cancel_setup() // Free context resources
+ if err != nil {
+ return enabled, fmt.Errorf("unable to enable GPS: %w", err)
+ }
+
+ ctx, cancel_setup = context.WithTimeout(context.Background(), DbusCallTimeout)
+ err = m.modemObj.CallWithContext(ctx, "org.freedesktop.ModemManager1.Modem.Location.SetGpsRefreshRate", 0, uint(refresh_interval.Seconds())).Err
+ cancel_setup() // Free context resources
+ if err != nil {
+ return enabled, fmt.Errorf("unable to set GPS refresh rate: %w", err)
+ }
+
+ return enabled, nil
+}
+
+func (m *ModemManager) disable_gps(enabled uint32) {
+ // Disable GPS NMEA, but keep other sources as they were
+ ctx, cancel := context.WithTimeout(context.Background(), DbusCallTimeout)
+ err := m.modemObj.CallWithContext(ctx, "org.freedesktop.ModemManager1.Modem.Location.Setup", 0, enabled&^mmModemLocationSourceGpsNmea, false).Err
+ cancel()
+ if m.debug && err != nil {
+ fmt.Println("Unable to disable GPS location on modem:", err)
+ }
+}
+
+func (m *ModemManager) Start(send func(data []byte), write <-chan []byte, stop <-chan struct{}) error {
+ if err := m.ensure_bus(); err != nil {
+ return err
+ }
+
+ refresh_interval := 1 * time.Second
+
+ go func() {
+ var enabled uint32 = 0
+ var errcount = 0
+ const max_errors = 2
+
+ signalChan := make(chan *dbus.Signal, 1)
+ m.systemBus.Signal(signalChan)
+ defer m.systemBus.RemoveSignal(signalChan)
+
+ run_loop:
+ // Repeat modem initialization and location polling until stopped.
+ for {
+
+ init_loop:
+ for {
+ if err := m.initialize_modem(); err != nil {
+ fmt.Println("Error initializing modem:", err)
+ } else {
+ if enabled, err = m.enable_gps(refresh_interval); err != nil {
+ fmt.Println("Error enabling GPS:", err)
+ } else {
+ break init_loop
+ }
+ }
+ select {
+ case <-stop:
+ return
+ case msg := <-write:
+ fmt.Println("message received, but mm driver is unable to forward:", string(msg))
+ case signal := <-signalChan:
+ if m.debug {
+ fmt.Println("Signal received:", signal.Name)
+ }
+ case <-time.After(60 * time.Second):
+ }
+ fmt.Println("Retrying modem init")
+ }
+
+ errcount = 0
+ stopped := false
+ poll_loop:
+ // This loop exits either when stopped, or when we get more than
+ // max_errors consecutive errors. In case of errors, initialize again.
+ for {
+ select {
+ case <-stop:
+ stopped = true
+ break poll_loop
+ case msg := <-write:
+ fmt.Println("message received, but mm driver is unable to forward:", string(msg))
+ case signal := <-signalChan:
+ if m.debug {
+ fmt.Println("Signal received:", signal.Name)
+ }
+ case <-time.After(refresh_interval): // TODO: make interval configurable
+ loc_ctx, cancel_loc := context.WithTimeout(context.Background(), refresh_interval)
+ loc_call := m.modemObj.CallWithContext(loc_ctx, "org.freedesktop.ModemManager1.Modem.Location.GetLocation", 0)
+ cancel_loc()
+ if loc_call.Err != nil {
+ fmt.Println("ERROR: unable to get location data from response: ", loc_call.Err)
+ errcount++
+ continue
+ }
+ loc := make(map[uint32]dbus.Variant)
+ loc_call.Store(&loc)
+ if val, ok := loc[mmModemLocationSourceGpsNmea]; !ok {
+ // MM can return locations without NMEA data, so this is not an error
+ continue
+ } else {
+ b := []byte{}
+ if err := val.Store(&b); err != nil {
+ fmt.Println("ERROR: unable to Store NMEAs: ", val.String())
+ errcount++
+ continue
+ }
+ // Remove double newlines and add one to the end
+ send(append(
+ bytes.ReplaceAll(b, []byte("\r\n\r\n"), []byte("\r\n")),
+ []byte("\r\n")...))
+ errcount = 0
+ }
+ }
+ if errcount >= max_errors {
+ // The modem probably disappeared, so go back to init_loop
+ // to find and initialize it.
+ if m.debug {
+ fmt.Println("Too many errors when requesting location, re-initializing modem")
+ }
+ break poll_loop
+ }
+ }
+ // Disable GPS on both modem error and when stopped
+ m.disable_gps(enabled)
+ m.modemObj = nil
+ // Only exit run_loop (and the goroutine) when stopped.
+ if stopped {
+ break run_loop
+ }
+ }
+ }()
+
+ return nil
+}
+
+func (m *ModemManager) Save(dir string) (err error) {
+ return
+}
+
+func (m *ModemManager) Load(dir string) (err error) {
+ return
+}
--
GitLab