fm-rds-tx/internal/platform/plutosdr/pluto_linux.go

//go:build pluto && linux

package plutosdr

/*
#cgo pkg-config: libiio
#include <iio.h>
#include <stdlib.h>
#include <stdint.h>
*/
import "C"

import (
	"context"
	"fmt"
	"log"
	"sync"
	"sync/atomic"
	"time"
	"unsafe"

	"github.com/jan/fm-rds-tx/internal/output"
	"github.com/jan/fm-rds-tx/internal/platform"
)

func updateMaxDuration(dst *atomic.Uint64, d time.Duration) {
	v := uint64(d)
	for {
		cur := dst.Load()
		if v <= cur {
			return
		}
		if dst.CompareAndSwap(cur, v) {
			return
		}
	}
}

func durationMs(ns uint64) float64 {
	return float64(ns) / float64(time.Millisecond)
}

type PlutoDriver struct {
	mu  sync.Mutex
	cfg platform.SoapyConfig

	ctx     *C.struct_iio_context
	txDev   *C.struct_iio_device
	phyDev  *C.struct_iio_device
	chanI   *C.struct_iio_channel
	chanQ   *C.struct_iio_channel
	chanLO  *C.struct_iio_channel
	buf     *C.struct_iio_buffer
	bufSize int

	started        bool
	configured     bool
	framesWritten  atomic.Uint64
	samplesWritten atomic.Uint64
	underruns      atomic.Uint64
	slowWrites     atomic.Uint64
	slowFills      atomic.Uint64
	slowPushes     atomic.Uint64
	maxWriteNs     atomic.Uint64
	maxFillNs      atomic.Uint64
	maxPushNs      atomic.Uint64
	lastError      string
	lastErrorAt    string
	layoutLogged   bool
}

func NewPlutoDriver() platform.SoapyDriver {
	return &PlutoDriver{}
}

func (d *PlutoDriver) Name() string { return "pluto-iio" }

func (d *PlutoDriver) Configure(_ context.Context, cfg platform.SoapyConfig) error {
	d.mu.Lock()
	defer d.mu.Unlock()

	d.cleanup()
	d.cfg = cfg

	uri := "usb:"
	if cfg.Device != "" && cfg.Device != "plutosdr" {
		uri = cfg.Device
	}
	if v, ok := cfg.DeviceArgs["uri"]; ok && v != "" {
		uri = v
	}

	cURI := C.CString(uri)
	defer C.free(unsafe.Pointer(cURI))
	ctx := C.iio_create_context_from_uri(cURI)
	if ctx == nil {
		return fmt.Errorf("pluto: failed to create IIO context (uri=%s)", uri)
	}
	d.ctx = ctx

	txDev := d.findDevice("cf-ad9361-dds-core-lpc")
	if txDev == nil {
		return fmt.Errorf("pluto: TX device 'cf-ad9361-dds-core-lpc' not found")
	}
	d.txDev = txDev

	phyDev := d.findDevice("ad9361-phy")
	if phyDev == nil {
		return fmt.Errorf("pluto: PHY device 'ad9361-phy' not found")
	}
	d.phyDev = phyDev

	phyChanTX := d.findChannel(phyDev, "voltage3", true)
	if phyChanTX == nil {
		phyChanTX = d.findChannel(phyDev, "voltage0", true)
	}
	if phyChanTX == nil {
		return fmt.Errorf("pluto: PHY TX channel not found (tried voltage3, voltage0)")
	}

	rate := int64(cfg.SampleRateHz)
	if rate < 2084000 {
		rate = 2084000
	}
	d.cfg.SampleRateHz = float64(rate)
	if err := d.writeChanAttrLL(phyChanTX, "sampling_frequency", rate); err != nil {
		return err
	}

	bw := rate
	if bw > 2000000 {
		bw = 2000000
	}
	if err := d.writeChanAttrLL(phyChanTX, "rf_bandwidth", bw); err != nil {
		return err
	}

	phyChanLO := d.findChannel(phyDev, "altvoltage1", true)
	d.chanLO = phyChanLO
	if phyChanLO != nil {
		freqHz := int64(cfg.CenterFreqHz)
		if freqHz <= 0 {
			freqHz = 100000000
		}
		if err := d.writeChanAttrLL(phyChanLO, "frequency", freqHz); err != nil {
			return err
		}
	}

	attenDB := int64(0)
	if cfg.GainDB > 0 {
		attenDB = -int64(89 - cfg.GainDB)
		if attenDB > 0 {
			attenDB = 0
		}
		if attenDB < -89 {
			attenDB = -89
		}
	}
	_ = d.writeChanAttrLL(phyChanTX, "hardwaregain", attenDB*1000)

	chanI := d.findChannel(txDev, "voltage0", true)
	chanQ := d.findChannel(txDev, "voltage1", true)
	if chanI == nil || chanQ == nil {
		return fmt.Errorf("pluto: TX I/Q channels not found on streaming device")
	}
	C.iio_channel_enable(chanI)
	C.iio_channel_enable(chanQ)
	d.chanI = chanI
	d.chanQ = chanQ

	d.bufSize = int(rate) / 20
	if d.bufSize < 4096 {
		d.bufSize = 4096
	}
	buf := C.iio_device_create_buffer(txDev, C.size_t(d.bufSize), C.bool(false))
	if buf == nil {
		return fmt.Errorf("pluto: failed to create TX buffer (size=%d)", d.bufSize)
	}
	d.buf = buf
	d.configured = true
	return nil
}

func (d *PlutoDriver) Capabilities(_ context.Context) (platform.DeviceCaps, error) {
	return platform.DeviceCaps{
		MinSampleRate: 521e3,
		MaxSampleRate: 61.44e6,
		HasGain:       true,
		GainMinDB:     -89,
		GainMaxDB:     0,
		Channels:      []int{0},
	}, nil
}

func (d *PlutoDriver) Start(_ context.Context) error {
	d.mu.Lock()
	defer d.mu.Unlock()
	if !d.configured {
		return fmt.Errorf("pluto: not configured")
	}
	if d.started {
		return fmt.Errorf("pluto: already started")
	}
	d.started = true
	return nil
}

func (d *PlutoDriver) Write(_ context.Context, frame *output.CompositeFrame) (int, error) {
	d.mu.Lock()
	buf := d.buf
	chanI := d.chanI
	chanQ := d.chanQ
	started := d.started
	bufSize := d.bufSize
	d.mu.Unlock()

	if !started || buf == nil {
		return 0, fmt.Errorf("pluto: not active")
	}
	if frame == nil || len(frame.Samples) == 0 {
		return 0, nil
	}

	written := 0
	total := len(frame.Samples)

	for written < total {
		chunk := total - written
		if chunk > bufSize {
			chunk = bufSize
		}

		step := uintptr(C.iio_buffer_step(buf))
		if step == 0 {
			return written, fmt.Errorf("pluto: buffer step is 0")
		}

		ptrI := uintptr(C.iio_buffer_first(buf, chanI))
		ptrQ := uintptr(C.iio_buffer_first(buf, chanQ))
		if ptrI == 0 || ptrQ == 0 {
			return written, fmt.Errorf("pluto: buffer_first returned null")
		}

		end := uintptr(C.iio_buffer_end(buf))
		d.mu.Lock()
		if !d.layoutLogged {
			delta := int64(ptrQ) - int64(ptrI)
			span := int64(0)
			if end > ptrI {
				span = int64(end - ptrI)
			}
			log.Printf("pluto-linux: buffer layout step=%d ptrI=%#x ptrQ=%#x delta=%d end=%#x span=%d bufSize=%d", step, ptrI, ptrQ, delta, end, span, bufSize)
			d.layoutLogged = true
		}
		d.mu.Unlock()
		if end > 0 {
			bufSamples := int((end - ptrI) / step)
			if bufSamples > 0 && chunk > bufSamples {
				chunk = bufSamples
			}
		}

		fillStart := time.Now()
		for i := 0; i < chunk; i++ {
			s := frame.Samples[written+i]
			*(*int16)(unsafe.Pointer(ptrI)) = int16(s.I * 32767)
			*(*int16)(unsafe.Pointer(ptrQ)) = int16(s.Q * 32767)
			ptrI += step
			ptrQ += step
		}
		fillDur := time.Since(fillStart)

		pushStart := time.Now()
		pushed := int(C.iio_buffer_push(buf))
		pushDur := time.Since(pushStart)

		chunkBudget := time.Duration(0)
		if d.cfg.SampleRateHz > 0 {
			chunkBudget = time.Duration(float64(chunk) / d.cfg.SampleRateHz * float64(time.Second))
		}
		writeDur := fillDur + pushDur

		updateMaxDuration(&d.maxFillNs, fillDur)
		updateMaxDuration(&d.maxPushNs, pushDur)
		updateMaxDuration(&d.maxWriteNs, writeDur)

		slow := false
		if chunkBudget > 0 {
			if fillDur > chunkBudget/4 {
				d.slowFills.Add(1)
				slow = true
			}
			if pushDur > chunkBudget/2 {
				d.slowPushes.Add(1)
				slow = true
			}
			if writeDur > chunkBudget {
				d.slowWrites.Add(1)
				slow = true
			}
		}
		if slow {
			sw := d.slowWrites.Load()
			sf := d.slowFills.Load()
			sp := d.slowPushes.Load()
			if sw <= 5 || sf <= 5 || sp <= 5 || sw%20 == 0 || sf%20 == 0 || sp%20 == 0 {
				log.Printf("pluto-linux slow write: chunk=%d budget=%s fill=%s push=%s total=%s",
					chunk, chunkBudget, fillDur, pushDur, writeDur)
			}
		}

		if pushed < 0 {
			d.mu.Lock()
			d.lastError = fmt.Sprintf("buffer_push: %d", pushed)
			d.lastErrorAt = time.Now().UTC().Format(time.RFC3339)
			d.underruns.Add(1)
			d.mu.Unlock()
			return written, fmt.Errorf("pluto: buffer_push returned %d", pushed)
		}

		written += chunk
	}

	d.framesWritten.Add(1)
	d.samplesWritten.Add(uint64(written))
	return written, nil
}

func (d *PlutoDriver) Stop(_ context.Context) error {
	d.mu.Lock()
	defer d.mu.Unlock()
	d.started = false
	return nil
}

func (d *PlutoDriver) Flush(_ context.Context) error { return nil }

func (d *PlutoDriver) Tune(_ context.Context, freqHz float64) error {
	d.mu.Lock()
	defer d.mu.Unlock()
	if !d.configured || d.chanLO == nil {
		return fmt.Errorf("pluto: not configured or LO channel not available")
	}
	return d.writeChanAttrLL(d.chanLO, "frequency", int64(freqHz))
}

func (d *PlutoDriver) Close(_ context.Context) error {
	d.mu.Lock()
	defer d.mu.Unlock()
	d.started = false
	d.cleanup()
	return nil
}

func (d *PlutoDriver) Stats() platform.RuntimeStats {
	d.mu.Lock()
	defer d.mu.Unlock()
	return platform.RuntimeStats{
		TXEnabled:      d.started,
		StreamActive:   d.started && d.buf != nil,
		FramesWritten:  d.framesWritten.Load(),
		SamplesWritten: d.samplesWritten.Load(),
		Underruns:      d.underruns.Load(),
		SlowWrites:     d.slowWrites.Load(),
		SlowFills:      d.slowFills.Load(),
		SlowPushes:     d.slowPushes.Load(),
		LastError:      d.lastError,
		LastErrorAt:    d.lastErrorAt,
		EffectiveRate:  d.cfg.SampleRateHz,
		MaxWriteMs:     durationMs(d.maxWriteNs.Load()),
		MaxFillMs:      durationMs(d.maxFillNs.Load()),
		MaxPushMs:      durationMs(d.maxPushNs.Load()),
	}
}

func (d *PlutoDriver) cleanup() {
	if d.buf != nil {
		C.iio_buffer_destroy(d.buf)
		d.buf = nil
	}
	if d.chanI != nil {
		C.iio_channel_disable(d.chanI)
		d.chanI = nil
	}
	if d.chanQ != nil {
		C.iio_channel_disable(d.chanQ)
		d.chanQ = nil
	}
	d.chanLO = nil
	if d.ctx != nil {
		C.iio_context_destroy(d.ctx)
		d.ctx = nil
	}
	d.txDev = nil
	d.phyDev = nil
	d.configured = false
	d.layoutLogged = false
}

func (d *PlutoDriver) findDevice(name string) *C.struct_iio_device {
	if d.ctx == nil {
		return nil
	}
	cName := C.CString(name)
	defer C.free(unsafe.Pointer(cName))
	return C.iio_context_find_device(d.ctx, cName)
}

func (d *PlutoDriver) findChannel(dev *C.struct_iio_device, name string, isOutput bool) *C.struct_iio_channel {
	if dev == nil {
		return nil
	}
	cName := C.CString(name)
	defer C.free(unsafe.Pointer(cName))
	if isOutput {
		return C.iio_device_find_channel(dev, cName, C.bool(true))
	}
	return C.iio_device_find_channel(dev, cName, C.bool(false))
}

func (d *PlutoDriver) writeChanAttrLL(ch *C.struct_iio_channel, attr string, val int64) error {
	if ch == nil {
		return fmt.Errorf("pluto: channel missing for attr %s", attr)
	}
	cAttr := C.CString(attr)
	defer C.free(unsafe.Pointer(cAttr))
	ret := C.iio_channel_attr_write_longlong(ch, cAttr, C.longlong(val))
	if ret < 0 {
		return fmt.Errorf("pluto: write attr %s failed (rc=%d)", attr, int(ret))
	}
	return nil
}