Map IPC Compatibility

2026-11-02 20:13:53 +08:00 · 2026-10-04 17:23:25 -05:00
parent 3b305b12b4
commit da4bceb5ac
12 changed files with 607 additions and 1 deletions
@@ -43,3 +43,5 @@ require (
 	golang.org/x/text v0.3.8 // indirect
 	google.golang.org/protobuf v1.27.1 // indirect
 )
+
+replace github.com/pfeiferj/gomsgq v0.1.11 => ./third_party/gomsgq
@@ -0,0 +1,47 @@
+package main
+
+import (
+	"os"
+	"regexp"
+	"strconv"
+	"testing"
+	"unsafe"
+
+	"github.com/edsrzf/mmap-go"
+	"github.com/pfeiferj/gomsgq"
+)
+
+func TestMsgqNativeLayout(t *testing.T) {
+	source, err := os.ReadFile("../msgq_repo/msgq/msgq.h")
+	if err != nil {
+		t.Fatal(err)
+	}
+	match := regexp.MustCompile(`(?m)^#define NUM_READERS ([0-9]+)$`).FindSubmatch(source)
+	if len(match) != 2 {
+		t.Fatal("native reader layout missing")
+	}
+	count, err := strconv.Atoi(string(match[1]))
+	if err != nil {
+		t.Fatal(err)
+	}
+	if gomsgq.NUM_READERS != count || gomsgq.HEADER_SIZE != int64(24+24*count) {
+		t.Fatalf("Go/native msgq ABI differs: Go readers=%d bytes=%d native readers=%d", gomsgq.NUM_READERS, gomsgq.HEADER_SIZE, count)
+	}
+	mem := make(mmap.MMap, gomsgq.HEADER_SIZE+8)
+	var header gomsgq.Header
+	header.Init(mem)
+	for index := range count {
+		header.ReadPointers[index] = uint64(index + 1)
+		header.ReadValids[index] = uint64(index + 101)
+		header.ReadUids[index] = uint64(index + 201)
+		for column, expected := range []uint64{uint64(index + 1), uint64(index + 101), uint64(index + 201)} {
+			actual := *(*uint64)(unsafe.Pointer(&mem[24+8*(column*count+index)]))
+			if actual != expected {
+				t.Fatalf("reader %d column %d wrong offset", index, column)
+			}
+		}
+	}
+	if *(*uint64)(unsafe.Pointer(&mem[gomsgq.HEADER_SIZE])) != 0 {
+		t.Fatal("reader metadata overlaps payload")
+	}
+}
@@ -10,6 +10,7 @@ import (
 	"os/exec"
 	"path/filepath"
 	"strings"
+	"sync"
 	"testing"
 	"time"

@@ -95,7 +96,7 @@ source.close()
 	if err != nil {
 		t.Fatal(err)
 	}
-	var stderr strings.Builder
+	var stderr lockedIPCOutput
 	cmd.Stderr = &stderr
 	if err := cmd.Start(); err != nil {
 		t.Fatal(err)
@@ -257,3 +258,19 @@ func publishIgnoredService(t *testing.T, service string) {
 	publisher.Send(raw)
 	queue.Close()
 }
+
+type lockedIPCOutput struct {
+	mu    sync.Mutex
+	value strings.Builder
+}
+
+func (b *lockedIPCOutput) Write(p []byte) (int, error) {
+	b.mu.Lock()
+	defer b.mu.Unlock()
+	return b.value.Write(p)
+}
+func (b *lockedIPCOutput) String() string {
+	b.mu.Lock()
+	defer b.mu.Unlock()
+	return b.value.String()
+}
@@ -0,0 +1,21 @@
+MIT License
+
+Copyright (c) 2025 Jacob Pfeifer
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
@@ -0,0 +1,21 @@
+package gomsgq
+
+// #include <stdint.h>
+// #include <signal.h>
+//
+// static void thread_signal(unsigned int tid) {
+//   #ifdef __APPLE__
+//     // macOS doesn't have tkill, rely on polling instead
+//     (void)tid;
+//   #elif !defined(SYS_tkill)
+//     // fallback for systems without tkill
+//     kill(tid, SIGUSR2);
+//   #else
+//     syscall(SYS_tkill, tid, SIGUSR2);
+//   #endif
+// }
+import "C"
+
+func ThreadSignal(tid uint32) {
+	C.thread_signal(C.uint(tid))
+}
@@ -0,0 +1,7 @@
+module github.com/pfeiferj/gomsgq
+
+go 1.23.0
+
+require github.com/edsrzf/mmap-go v1.2.0
+
+require golang.org/x/sys v0.0.0-20211216021012-1d35b9e2eb4e
@@ -0,0 +1,4 @@
+github.com/edsrzf/mmap-go v1.2.0 h1:hXLYlkbaPzt1SaQk+anYwKSRNhufIDCchSPkUD6dD84=
+github.com/edsrzf/mmap-go v1.2.0/go.mod h1:19H/e8pUPLicwkyNgOykDXkJ9F0MHE+Z52B8EIth78Q=
+golang.org/x/sys v0.0.0-20211216021012-1d35b9e2eb4e h1:fLOSk5Q00efkSvAm+4xcoXD+RRmLmmulPn5I3Y9F2EM=
+golang.org/x/sys v0.0.0-20211216021012-1d35b9e2eb4e/go.mod h1:oPkhp1MJrh7nUepCBck5+mAzfO9JrbApNNgaTdGDITg=
@@ -0,0 +1,28 @@
+package gomsgq
+
+import (
+	"unsafe"
+
+	"github.com/edsrzf/mmap-go"
+)
+
+type Header struct {
+	NumReaders   *uint64
+	WritePointer *uint64
+	WriteUid     *uint64
+	ReadPointers []uint64
+	ReadValids   []uint64
+	ReadUids     []uint64
+}
+
+func (h *Header) Init(m mmap.MMap) {
+	h.NumReaders = (*uint64)(unsafe.Pointer(&m[0]))
+	h.WritePointer = (*uint64)(unsafe.Pointer(&m[8]))
+	h.WriteUid = (*uint64)(unsafe.Pointer(&m[16]))
+	readPointers := unsafe.Slice((*uint64)(unsafe.Pointer(&m[24])), NUM_READERS)
+	h.ReadPointers = readPointers
+	readValids := unsafe.Slice((*uint64)(unsafe.Pointer(&m[24+NUM_READERS*8])), NUM_READERS)
+	h.ReadValids = readValids
+	readUids := unsafe.Slice((*uint64)(unsafe.Pointer(&m[24+2*NUM_READERS*8])), NUM_READERS)
+	h.ReadUids = readUids
+}
@@ -0,0 +1,130 @@
+package gomsgq
+
+import (
+	"errors"
+	"os"
+	"unsafe"
+
+	"github.com/edsrzf/mmap-go"
+)
+
+var OPENPILOT_PREFIX = os.Getenv("OPENPILOT_PREFIX")
+var USE_MSGQ_PREFIX = os.Getenv("USE_MSGQ_PREFIX")
+
+const PATH_PREFIX = "/dev/shm/"
+const ALT_PATH_PREFIX = "/tmp/"
+const MSGQ_PREFIXED_TEST_NAME = "msgq_logMessage"
+const MSGQ_PREFIX = "msgq_"
+const NUM_READERS = 64
+
+var HEADER_SIZE = (3*8 + 3*NUM_READERS*8) + align(3*8+3*NUM_READERS*8)
+
+type Msgq struct {
+	Size   int64
+	Path   string
+	File   *os.File
+	Mem    mmap.MMap
+	Data   []uint8
+	Header Header
+}
+
+func (m *Msgq) WraparoundPosition() uint64 {
+	position := uint64(0)
+	for position+8 < uint64(len(m.Data)) {
+		size := *(*int64)(unsafe.Pointer(&m.Data[position]))
+		if size == -1 {
+			break
+		}
+		position += 8 + uint64(size) + uint64(align(size))
+
+	}
+	return position
+}
+
+func pathPrefix() string {
+	if _, err := os.Stat(PATH_PREFIX); err == nil {
+		return PATH_PREFIX
+	}
+	return ALT_PATH_PREFIX
+}
+
+func IsPrefixedMsgq() bool {
+	hasMsgqPrefix := USE_MSGQ_PREFIX == "true"
+
+	if USE_MSGQ_PREFIX == "" {
+		if _, err := os.Stat(pathPrefix() + MSGQ_PREFIXED_TEST_NAME); err == nil {
+			hasMsgqPrefix = true
+		}
+	}
+	return hasMsgqPrefix
+}
+
+func align(length int64) int64 {
+	remainder := length % 8
+	if remainder == 0 {
+		return 0
+	}
+	return 8 - remainder
+}
+
+func (m *Msgq) Close() (error, error) {
+	var memErr error = nil
+	var fileErr error = nil
+	if m.Mem != nil {
+		memErr = m.Mem.Unmap()
+	}
+	if m.File != nil {
+		fileErr = m.File.Close()
+	}
+	return memErr, fileErr
+}
+
+func (m *Msgq) Init(path string, size int64) error {
+	if size >= 0xFFFFFFFF {
+		return errors.New("buffer must be smaller than 2^32 bytes")
+	}
+	m.Path = path
+	m.Size = size
+
+	fullPath := pathPrefix()
+
+	if IsPrefixedMsgq() {
+		fullPath = fullPath + MSGQ_PREFIX
+	}
+	if OPENPILOT_PREFIX != "" {
+		fullPath = fullPath + OPENPILOT_PREFIX + "/"
+	}
+	fullPath = fullPath + path
+	f, err := os.OpenFile(fullPath, os.O_RDWR|os.O_CREATE, 0664)
+	if err != nil {
+		return err
+	}
+	err = f.Truncate(size + int64(HEADER_SIZE))
+	if err != nil {
+		return err
+	}
+	err = f.Sync()
+	if err != nil {
+		return err
+	}
+	mem, err := mmap.Map(f, mmap.RDWR, 0)
+	if err != nil {
+		return err
+	}
+	m.Mem = mem
+	m.Header = Header{}
+	m.Header.Init(mem)
+	data := unsafe.Slice((*byte)(unsafe.Pointer(&mem[HEADER_SIZE])), size)
+	m.Data = data
+
+	return nil
+}
+
+func (m *Msgq) WaitForSubscriber() {
+	for *m.Header.NumReaders == 0 {
+		err := m.Mem.Flush()
+		if err != nil {
+			panic("Msgq failed to flush")
+		}
+	}
+}
@@ -0,0 +1,30 @@
+package gomsgq
+
+type Pointer struct {
+	Position uint64
+	Cycles   uint64
+}
+
+func (p *Pointer) Marshal() uint64 {
+	return p.Position | (p.Cycles << 32)
+}
+
+func (p *Pointer) Cycle() {
+	p.Position = 0
+	p.Cycles++
+}
+
+func (p *Pointer) Next(size int64) Pointer {
+	newPosition := p.Position + uint64(size) + uint64(align(size)) + 8
+	return Pointer{
+		Position: newPosition,
+		Cycles:   p.Cycles,
+	}
+}
+
+func NewPointer(value uint64) Pointer {
+	return Pointer{
+		Position: value & 0xFFFFFFFF,
+		Cycles:   value >> 32,
+	}
+}
@@ -0,0 +1,81 @@
+package gomsgq
+
+import (
+	"unsafe"
+)
+
+type MsgqPublisher struct {
+	Msgq Msgq
+	Uid  uint64
+	Id   uint64
+}
+
+func (p *MsgqPublisher) Init(msgq Msgq) {
+	p.Msgq = msgq
+	p.Uid = generateUid()
+
+	*p.Msgq.Header.NumReaders = 0
+	*p.Msgq.Header.WriteUid = p.Uid
+	*p.Msgq.Header.WritePointer = 0
+
+	for i := range NUM_READERS {
+		p.Msgq.Header.ReadValids[i] = 0
+		p.Msgq.Header.ReadUids[i] = 0
+	}
+}
+
+func (p *MsgqPublisher) Send(data []byte) {
+	if p.Uid != *p.Msgq.Header.WriteUid {
+		panic("We are not the active Msgq publisher, panic")
+	}
+	totalSize := int64(len(data)+8) + align(int64(len(data)))
+	if totalSize*3 >= p.Msgq.Size {
+		panic("Msgq size too small, panic")
+	}
+	numReaders := *p.Msgq.Header.NumReaders
+	writePointer := NewPointer(*p.Msgq.Header.WritePointer)
+	remainingSpace := p.Msgq.Size - int64(writePointer.Position) - totalSize - 8
+
+	// Invalidate all readers that are beyond the write pointer
+	if remainingSpace <= 0 {
+		// write -1 size tag indicating wraparound
+		*(*int64)(unsafe.Pointer(&p.Msgq.Data[writePointer.Position])) = int64(-1)
+		for i := range numReaders {
+			readPointer := NewPointer(p.Msgq.Header.ReadPointers[i])
+			if readPointer.Position > writePointer.Position && readPointer.Cycles != writePointer.Cycles {
+				p.Msgq.Header.ReadValids[i] = 0 //false
+			}
+		}
+		writePointer.Position = 0
+		writePointer.Cycles += 1
+		*p.Msgq.Header.WritePointer = writePointer.Marshal()
+	}
+
+	// Invalidate readers that are in the area that will be written
+	end := writePointer.Position + uint64(totalSize)
+	for i := range numReaders {
+		readPointer := NewPointer(p.Msgq.Header.ReadPointers[i])
+
+		if readPointer.Position >= writePointer.Position && readPointer.Position < end && readPointer.Cycles != writePointer.Cycles {
+			p.Msgq.Header.ReadValids[i] = 0 //false
+		}
+	}
+
+	// Write size tag
+	*(*int64)(unsafe.Pointer(&p.Msgq.Data[writePointer.Position])) = int64(len(data))
+
+	// Copy data
+	for i, b := range data {
+		p.Msgq.Data[int(writePointer.Position)+8+i] = b
+	}
+
+	// Update write pointer
+	writePointer.Position += uint64(totalSize)
+	*p.Msgq.Header.WritePointer = writePointer.Marshal()
+
+	// Notify readers
+	for i := range numReaders {
+		rUid := p.Msgq.Header.ReadUids[i]
+		ThreadSignal(uint32(rUid))
+	}
+}
@@ -0,0 +1,218 @@
+package gomsgq
+
+import (
+	"math/rand/v2"
+	"os"
+	"unsafe"
+
+	"sync/atomic"
+)
+
+type MsgqSubscriber struct {
+	Shadow                bool
+	Msgq                  Msgq
+	Uid                   uint64
+	Id                    uint64
+	Conflate              bool
+	shadowPointer         uint64
+	previousShadowPointer uint64
+	shadowReset           bool
+}
+
+func generateUid() uint64 {
+	return uint64(rand.Uint32())<<32 | uint64(os.Getpid())
+}
+
+func (s *MsgqSubscriber) Init(msgq Msgq) {
+	s.Msgq = msgq
+	s.Uid = generateUid()
+	if !s.Shadow {
+		for {
+			curNumReaders := *s.Msgq.Header.NumReaders
+			newNumReaders := curNumReaders + 1
+			if newNumReaders > NUM_READERS {
+				*s.Msgq.Header.NumReaders = 0
+
+				for i := range NUM_READERS {
+					s.Msgq.Header.ReadValids[i] = 0
+
+					old_uid := s.Msgq.Header.ReadUids[i]
+					s.Msgq.Header.ReadUids[i] = 0
+
+					ThreadSignal(uint32(old_uid & 0xFFFFFFFF))
+				}
+				continue
+			}
+			if atomic.CompareAndSwapUint64(s.Msgq.Header.NumReaders, curNumReaders, newNumReaders) {
+				s.Id = curNumReaders
+				s.Msgq.Header.ReadValids[curNumReaders] = 0
+				s.Msgq.Header.ReadPointers[curNumReaders] = 0
+				s.Msgq.Header.ReadUids[curNumReaders] = s.Uid
+				break
+			}
+		}
+	}
+	s.Reset()
+}
+
+func (s *MsgqSubscriber) ShadowValid(writePointer Pointer) bool {
+	readPointer := NewPointer(s.shadowPointer)
+
+	if !s.shadowReset {
+		previousReadPointer := NewPointer(s.previousShadowPointer)
+		previousSize := *(*int64)(unsafe.Pointer(&s.Msgq.Data[previousReadPointer.Position]))
+
+		if previousSize == -1 && readPointer.Position != 0 {
+			return false
+		}
+
+		if previousSize != -1 {
+			calculatedCurrentReadPointer := previousReadPointer.Next(previousSize)
+
+			if readPointer.Position != calculatedCurrentReadPointer.Position {
+				return false
+			}
+		}
+	}
+
+	if readPointer.Cycles != writePointer.Cycles && readPointer.Position <= writePointer.Position {
+		return false
+	}
+
+	if readPointer.Cycles != writePointer.Cycles && readPointer.Position > s.Msgq.WraparoundPosition() {
+		return false
+	}
+
+	if readPointer.Cycles == writePointer.Cycles && readPointer.Position > writePointer.Position {
+		return false
+	}
+
+	return true
+}
+
+func (s *MsgqSubscriber) Reset() {
+	if !s.Shadow {
+		s.Msgq.Header.ReadValids[s.Id] = 1
+		s.Msgq.Header.ReadPointers[s.Id] = *s.Msgq.Header.WritePointer
+	} else {
+		s.shadowPointer = *s.Msgq.Header.WritePointer
+		s.shadowReset = true
+	}
+}
+
+func (s *MsgqSubscriber) Ready() bool {
+	if !s.Shadow {
+		for s.Uid != s.Msgq.Header.ReadUids[s.Id] {
+			s.Init(s.Msgq)
+		}
+
+		for s.Msgq.Header.ReadValids[s.Id] == 0 {
+			s.Reset()
+		}
+
+		readPointer := NewPointer(s.Msgq.Header.ReadPointers[s.Id])
+
+		writePointer := NewPointer(*s.Msgq.Header.WritePointer)
+
+		return readPointer.Position != writePointer.Position
+	} else {
+		for {
+			readPointer := NewPointer(s.shadowPointer)
+			writePointer := NewPointer(*s.Msgq.Header.WritePointer)
+
+			if !s.ShadowValid(writePointer) {
+				s.Reset()
+				continue
+			}
+
+			return readPointer.Position != writePointer.Position
+		}
+
+	}
+}
+
+func (s *MsgqSubscriber) Read() []byte {
+	if !s.Ready() {
+		return nil
+	}
+
+	for {
+		var readPointer Pointer
+		if s.Shadow {
+			readPointer = NewPointer(s.shadowPointer)
+		} else {
+			readPointer = NewPointer(s.Msgq.Header.ReadPointers[s.Id])
+		}
+		if s.Shadow {
+			writePointer := NewPointer(*s.Msgq.Header.WritePointer)
+			if !s.ShadowValid(writePointer) {
+				s.Reset()
+				continue
+			}
+		}
+		size := *(*int64)(unsafe.Pointer(&s.Msgq.Data[readPointer.Position]))
+
+		if size == -1 {
+			readPointer.Cycle()
+			if s.Shadow {
+				s.previousShadowPointer = s.shadowPointer
+				s.shadowReset = false
+				s.shadowPointer = readPointer.Marshal()
+			} else {
+				s.Msgq.Header.ReadPointers[s.Id] = readPointer.Marshal()
+			}
+			continue
+		}
+
+		if size >= s.Msgq.Size || size <= 0 {
+			if s.Shadow || s.Conflate {
+				s.Reset()
+				continue
+			}
+
+			panic("Invalid Msgq message size")
+		}
+
+		nextReadPointer := readPointer.Next(size)
+		if s.Conflate {
+			writePointer := NewPointer(*s.Msgq.Header.WritePointer)
+			if nextReadPointer.Position != writePointer.Position {
+				if s.Shadow {
+					s.previousShadowPointer = s.shadowPointer
+					s.shadowReset = false
+					s.shadowPointer = nextReadPointer.Marshal()
+				} else {
+					s.Msgq.Header.ReadPointers[s.Id] = nextReadPointer.Marshal()
+				}
+				continue
+			}
+		}
+
+		err := s.Msgq.Mem.Flush()
+		if err != nil {
+			panic("Msgq Flush Error")
+		}
+		result := make([]byte, size)
+		for i := range size {
+			result[i] = s.Msgq.Data[int64(readPointer.Position)+8+i]
+		}
+		err = s.Msgq.Mem.Flush()
+		if err != nil {
+			panic("Msgq Flush Error")
+		}
+
+		if s.Shadow {
+			s.previousShadowPointer = s.shadowPointer
+			s.shadowPointer = nextReadPointer.Marshal()
+		} else {
+			s.Msgq.Header.ReadPointers[s.Id] = nextReadPointer.Marshal()
+		}
+
+		if !s.Shadow && s.Msgq.Header.ReadValids[s.Id] == 0 {
+			s.Reset()
+			continue
+		}
+
+		return result
+	}
+}