diff --git a/GSS2.Core/Hardware/CameraService.cs b/GSS2.Core/Hardware/CameraService.cs index 3ec4ad2..80ebfd7 100644 --- a/GSS2.Core/Hardware/CameraService.cs +++ b/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? _mappedBuffers; private readonly CancellationTokenSource _disposeCts = new CancellationTokenSource(); public CameraService(ILogger 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(); - - _frameBufferAllocator = new FrameBufferAllocator(_camera!); + if (_camera is null) + throw new InvalidOperationException("Camera not initialized"); + 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(); + + 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); - } }