diff --git a/mapd_repo/go.mod b/mapd_repo/go.mod index 5b178c268d..5d12fd5867 100644 --- a/mapd_repo/go.mod +++ b/mapd_repo/go.mod @@ -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 diff --git a/mapd_repo/msgq_layout_test.go b/mapd_repo/msgq_layout_test.go new file mode 100644 index 0000000000..28a37a6607 --- /dev/null +++ b/mapd_repo/msgq_layout_test.go @@ -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") + } +} diff --git a/mapd_repo/shadow_ipc_test.go b/mapd_repo/shadow_ipc_test.go index 58a4cbb546..265472ac3c 100644 --- a/mapd_repo/shadow_ipc_test.go +++ b/mapd_repo/shadow_ipc_test.go @@ -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() +} diff --git a/mapd_repo/third_party/gomsgq/LICENSE b/mapd_repo/third_party/gomsgq/LICENSE new file mode 100644 index 0000000000..c98898659b --- /dev/null +++ b/mapd_repo/third_party/gomsgq/LICENSE @@ -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. diff --git a/mapd_repo/third_party/gomsgq/c.go b/mapd_repo/third_party/gomsgq/c.go new file mode 100644 index 0000000000..6d3dc5c536 --- /dev/null +++ b/mapd_repo/third_party/gomsgq/c.go @@ -0,0 +1,21 @@ +package gomsgq + +// #include +// #include +// +// 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)) +} diff --git a/mapd_repo/third_party/gomsgq/go.mod b/mapd_repo/third_party/gomsgq/go.mod new file mode 100644 index 0000000000..2ee2ae9c7e --- /dev/null +++ b/mapd_repo/third_party/gomsgq/go.mod @@ -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 diff --git a/mapd_repo/third_party/gomsgq/go.sum b/mapd_repo/third_party/gomsgq/go.sum new file mode 100644 index 0000000000..bdd6b1a424 --- /dev/null +++ b/mapd_repo/third_party/gomsgq/go.sum @@ -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= diff --git a/mapd_repo/third_party/gomsgq/header.go b/mapd_repo/third_party/gomsgq/header.go new file mode 100644 index 0000000000..81f132d69e --- /dev/null +++ b/mapd_repo/third_party/gomsgq/header.go @@ -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 +} diff --git a/mapd_repo/third_party/gomsgq/msgq.go b/mapd_repo/third_party/gomsgq/msgq.go new file mode 100644 index 0000000000..9599ec0fb3 --- /dev/null +++ b/mapd_repo/third_party/gomsgq/msgq.go @@ -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") + } + } +} diff --git a/mapd_repo/third_party/gomsgq/pointer.go b/mapd_repo/third_party/gomsgq/pointer.go new file mode 100644 index 0000000000..f1f3e3650c --- /dev/null +++ b/mapd_repo/third_party/gomsgq/pointer.go @@ -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, + } +} diff --git a/mapd_repo/third_party/gomsgq/publisher.go b/mapd_repo/third_party/gomsgq/publisher.go new file mode 100644 index 0000000000..517e2ed670 --- /dev/null +++ b/mapd_repo/third_party/gomsgq/publisher.go @@ -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)) + } +} diff --git a/mapd_repo/third_party/gomsgq/subscriber.go b/mapd_repo/third_party/gomsgq/subscriber.go new file mode 100644 index 0000000000..f735aab0a1 --- /dev/null +++ b/mapd_repo/third_party/gomsgq/subscriber.go @@ -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 + } +}