feat: CameraService features and buffer mapping

Use cleanup method in dispose
Add initialization methods null fields check
Add image capture stream buffer memory mapping

GSS2.Core/Hardware/CameraService.cs

@@ -1,3 +1,4 @@
+using System.Runtime.InteropServices;
 using Microsoft.Extensions.Logging;
 using LibCameraSharp;
 using Stream = LibCameraSharp.Stream;
@@ -6,6 +7,22 @@ namespace GSS2.Core.Hardware;
 
 public class CameraService : IDisposable
 {
+    private static class LibC
+    {
+        public const int PROT_READ = 0x01;
+        public const int MAP_SHARED = 0x0001;
+
+        [DllImport("libc", SetLastError = true)] public static extern IntPtr mmap(IntPtr address, ulong length, int prot, int flags, int fd, ulong offset);
+        [DllImport("libc", SetLastError = true)] public static extern int mumap(IntPtr address, ulong length);
+    }
+    private sealed class MappedBuffer
+    {
+        public int Fd { get; init; }
+        public IntPtr Address { get; init; }
+        public ulong Length { get; init; }
+        public ulong AlignedOffset { get; init; }
+    }
+
     public class CameraServiceConfiguration
     {
         public static CameraServiceConfiguration Default => new CameraServiceConfiguration
@@ -36,7 +53,8 @@ public class CameraService : IDisposable
         CameraAcquired = 1 << 2,
         CameraStarted = 1 << 3,
         ViewFinderFrameBufferAllocated = 1 << 4,
-        ImageCaptureFrameBufferAllocated = 1 << 5
+        ImageCaptureFrameBufferAllocated = 1 << 5,
+        ImageCaptureFrameBuffersMapped = 1 << 6
     }
 
     private bool _disposedValue;
@@ -49,6 +67,7 @@ public class CameraService : IDisposable
     private FrameBufferAllocator? _frameBufferAllocator;
     private Stream? _viewFinderStream;
     private Stream? _imageCaptureStream;
+    private List<MappedBuffer>? _mappedBuffers;
     private readonly CancellationTokenSource _disposeCts = new CancellationTokenSource();
 
     public CameraService(ILogger<CameraService> logger, CameraServiceConfiguration? configuration = null, bool autoInitialize = false)
@@ -98,11 +117,12 @@ public class CameraService : IDisposable
             CameraConfigure(effectiveCancellationToken);
             CameraStart(effectiveCancellationToken);
             FrameBuffersAllocate(effectiveCancellationToken);
+            ImageCaptureFrameBuffersMap(effectiveCancellationToken);
         }
         catch (Exception e)
         {
             _logger.LogCritical(e, "An exception occurred while camera initialization");
-            CleanupAfterFailedInitialization();
+            Cleanup();
             throw;
         }
     }
@@ -122,9 +142,12 @@ public class CameraService : IDisposable
     }
     private void CameraSelect(CancellationToken cancellationToken)
     {
+        if (_cameraManager is null)
+            throw new InvalidOperationException("CameraManager not initialized");
+
         cancellationToken.ThrowIfCancellationRequested();
 
-        if (_cameraManager!.Cameras.Count() == 0)
+        if (_cameraManager.Cameras.Count() == 0)
             throw new Exception("No cameras found");
 
         _camera = _cameraManager.Cameras
@@ -143,9 +166,12 @@ public class CameraService : IDisposable
     }
     private void CameraAcquire(CancellationToken cancellationToken)
     {
+        if (_camera is null)
+            throw new InvalidOperationException("Camera not initialized");
+
         cancellationToken.ThrowIfCancellationRequested();
 
-        int result = _camera!.Acquire();
+        int result = _camera.Acquire();
         if (result < 0)
         {
             _logger.LogCritical("Error while camera acquire {Result}", result);
@@ -156,9 +182,12 @@ public class CameraService : IDisposable
     }
     private void CameraConfigure(CancellationToken cancellationToken)
     {
+        if (_camera is null)
+            throw new InvalidOperationException("Camera not initialized");
+
         cancellationToken.ThrowIfCancellationRequested();
 
-        _cameraConfiguration = _camera!.GenerateConfiguration(
+        _cameraConfiguration = _camera.GenerateConfiguration(
             StreamRoleEnum.Viewfinder,
             StreamRoleEnum.StillCapture);
 
@@ -188,9 +217,12 @@ public class CameraService : IDisposable
     }
     private void CameraStart(CancellationToken cancellationToken)
     {
+        if (_camera is null)
+            throw new InvalidOperationException("Camera not initialized");
+
         cancellationToken.ThrowIfCancellationRequested();
 
-        int result = _camera!.Start();
+        int result = _camera.Start();
         if (result < 0)
         {
             _logger.LogCritical("Error while camera start {Result}", result);
@@ -201,13 +233,21 @@ public class CameraService : IDisposable
     }
     private void FrameBuffersAllocate(CancellationToken cancellationToken)
     {
-        cancellationToken.ThrowIfCancellationRequested();
+        if (_camera is null)
-
+            throw new InvalidOperationException("Camera not initialized");
-        _frameBufferAllocator = new FrameBufferAllocator(_camera!);
+        if (_viewFinderStream is null)
+            throw new InvalidOperationException("ViewFinderStream not initialized");
+        if (_imageCaptureStream is null)
+            throw new InvalidOperationException("ImageCaptureStream not initialized");
 
         cancellationToken.ThrowIfCancellationRequested();
 
-        int result = _frameBufferAllocator.Allocate(_viewFinderStream!);
+        _frameBufferAllocator = new FrameBufferAllocator(_camera);
+
+
+        cancellationToken.ThrowIfCancellationRequested();
+
+        int result = _frameBufferAllocator.Allocate(_viewFinderStream);
         if (result < 0)
         {
             _logger.LogCritical("Error while view finder frame buffers allocation {Result}", result);
@@ -218,7 +258,7 @@ public class CameraService : IDisposable
 
         cancellationToken.ThrowIfCancellationRequested();
 
-        result = _frameBufferAllocator.Allocate(_imageCaptureStream!);
+        result = _frameBufferAllocator.Allocate(_imageCaptureStream);
         if (result < 0)
         {
             _logger.LogCritical("Error while image capture frame buffers allocation {Result}", result);
@@ -227,10 +267,96 @@ public class CameraService : IDisposable
 
         _state |= State.ImageCaptureFrameBufferAllocated;
     }
-    private void CleanupAfterFailedInitialization()
+    private void ImageCaptureFrameBuffersMap(CancellationToken cancellationToken)
+    {
+        if (_frameBufferAllocator is null)
+            throw new InvalidOperationException("FrameBufferAllocator not initialized");
+        if (_imageCaptureStream is null)
+            throw new InvalidOperationException("ImageCaptureStream not initialized");
+
+        cancellationToken.ThrowIfCancellationRequested();
+
+        _mappedBuffers = new List<MappedBuffer>();
+
+        var frameBuffers = _frameBufferAllocator.Buffers(_imageCaptureStream);
+
+        foreach (var buffer in frameBuffers)
+        {
+            cancellationToken.ThrowIfCancellationRequested();
+
+            var planesByFd = buffer.Planes.GroupBy(p => p.Fd);
+
+            foreach (var fdGroup in planesByFd)
+            {
+                cancellationToken.ThrowIfCancellationRequested();
+
+                int fd = fdGroup.Key.Get();
+
+                ulong minOffset = fdGroup.Min(p => p.Offset);
+                ulong maxEnd = fdGroup.Max(p => p.Offset + p.Length);
+
+                ulong pageSize = (ulong)Environment.SystemPageSize;
+                ulong alignedOffset = minOffset & ~(pageSize - 1);
+                ulong alignedLength = maxEnd - alignedOffset;
+
+                IntPtr address = LibC.mmap(IntPtr.Zero, alignedLength, LibC.PROT_READ, LibC.MAP_SHARED, fd, alignedOffset);
+
+                if (address == new IntPtr(-1))
+                {
+                    int error = Marshal.GetLastWin32Error();
+                    throw new InvalidOperationException($"mmap failed for fd={fd}, error={error}");
+                }
+
+                _mappedBuffers.Add(new MappedBuffer
+                {
+                    Fd = fd,
+                    Address = address,
+                    Length = alignedLength,
+                    AlignedOffset = alignedOffset
+                });
+            }
+        }
+
+        _state |= State.ImageCaptureFrameBuffersMapped;
+    }
+
+    private IntPtr GetPlanePointer(FrameBuffer.Plane plane)
+    {
+        if (_mappedBuffers is null)
+            throw new InvalidOperationException("Buffers not mapped");
+
+        var mapped = _mappedBuffers.First(m => m.Fd == plane.Fd.Get());
+        ulong offsetInMapping = plane.Offset - mapped.AlignedOffset;
+        return IntPtr.Add(mapped.Address, checked((int)offsetInMapping));
+    }
+
+    protected virtual void Dispose(bool disposing)
+    {
+        if (!_disposedValue)
+        {
+            if (disposing)
+            {
+                _disposeCts.Cancel();
+                Cleanup();
+            }
+            _disposedValue = true;
+        }
+    }
+    void IDisposable.Dispose()
+    {
+        Dispose(disposing: true);
+        GC.SuppressFinalize(this);
+    }
+    private void Cleanup()
     {
         try
         {
+            if (_state.HasFlag(State.ImageCaptureFrameBuffersMapped) && _mappedBuffers is not null)
+            {
+                foreach (var buffer in _mappedBuffers)
+                    LibC.mumap(buffer.Address, buffer.Length);
+                _mappedBuffers.Clear();
+            }
             if (_state.HasFlag(State.ViewFinderFrameBufferAllocated) && _viewFinderStream is not null)
                 _frameBufferAllocator?.Free(_viewFinderStream);
             if (_state.HasFlag(State.ImageCaptureFrameBufferAllocated) && _imageCaptureStream is not null)
@@ -242,10 +368,10 @@ public class CameraService : IDisposable
             if (_state.HasFlag(State.CameraManagerStarted))
                 _cameraManager?.Stop();
 
-            _frameBufferAllocator?.Dispose();
-            _cameraManager?.Dispose();
             _viewFinderStream?.Dispose();
             _imageCaptureStream?.Dispose();
+            _frameBufferAllocator?.Dispose();
+            _cameraManager?.Dispose();
         }
         catch
         { }
@@ -256,52 +382,9 @@ public class CameraService : IDisposable
             _frameBufferAllocator = null;
             _viewFinderStream = null;
             _imageCaptureStream = null;
+            _mappedBuffers = null;
             _state = State.NotInitialized;
         }
     }
 
-    
-
-    protected virtual void Dispose(bool disposing)
-    {
-        if (!_disposedValue)
-        {
-            if (disposing)
-            {
-                _disposeCts.Cancel();
-
-                if (_state.HasFlag(State.ViewFinderFrameBufferAllocated) && _viewFinderStream is not null)
-                    _frameBufferAllocator?.Free(_viewFinderStream);
-                if (_state.HasFlag(State.ImageCaptureFrameBufferAllocated) && _imageCaptureStream is not null)
-                    _frameBufferAllocator?.Free(_imageCaptureStream);
-                if (_state.HasFlag(State.CameraStarted))
-                    _camera?.Stop();
-                if (_state.HasFlag(State.CameraAcquired))
-                    _camera?.Release();
-                if (_state.HasFlag(State.CameraManagerStarted))
-                    _cameraManager?.Stop();
-
-                _viewFinderStream?.Dispose();
-                _imageCaptureStream?.Dispose();
-                _frameBufferAllocator?.Dispose();
-                _cameraConfiguration?.Dispose();
-                _cameraManager?.Dispose();
-
-                _cameraManager = null;
-                _frameBufferAllocator = null;
-                _camera = null;
-                _cameraConfiguration = null;
-                _frameBufferAllocator = null;
-                _viewFinderStream = null;
-                _imageCaptureStream = null;
-            }
-
-            _disposedValue = true;
-        }
-    }
-    void IDisposable.Dispose()
-    {
-        Dispose(disposing: true);
-        GC.SuppressFinalize(this);
-    }
 }