forked from amkovkov/GranuSightSoftware2
813 lines
36 KiB
C#
813 lines
36 KiB
C#
using System.Runtime.InteropServices;
|
|
using Microsoft.Extensions.Logging;
|
|
using LibCameraSharp;
|
|
using Stream = LibCameraSharp.Stream;
|
|
using System.Buffers;
|
|
using GSS2.Core.Extentions;
|
|
|
|
namespace GSS2.Core.Hardware;
|
|
|
|
public class CameraService : IDisposable
|
|
{
|
|
public 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 munmap(IntPtr address, ulong length);
|
|
[DllImport("libc", SetLastError = true)] public static extern int close(int fd);
|
|
[DllImport("libc", SetLastError = true)] public static extern int memfd_create(string name, uint flags);
|
|
}
|
|
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
|
|
{
|
|
CameraId = "",
|
|
ViewFinderFrameBufferCount = 5,
|
|
ViewFinderWidth = 1920,
|
|
ViewFinderHeight = 1080,
|
|
ViewFinderFps = 30,
|
|
ImageCaptureFrameBufferCount = 2,
|
|
ImageCaptureWidth = 4056,
|
|
ImageCaptureHeight = 3080,
|
|
};
|
|
|
|
public string CameraId { get; set; } = "";
|
|
public int ViewFinderFrameBufferCount { get; set; }
|
|
public int ViewFinderWidth { get; set; }
|
|
public int ViewFinderHeight { get; set; }
|
|
public int ViewFinderFps { get; set; }
|
|
public int ImageCaptureFrameBufferCount { get; set; }
|
|
public int ImageCaptureWidth { get; set; }
|
|
public int ImageCaptureHeight { get; set; }
|
|
}
|
|
|
|
[Flags]
|
|
private enum State
|
|
{
|
|
NotInitialized = 0,
|
|
CameraManagerStarted = 1 << 1,
|
|
CameraAcquired = 1 << 2,
|
|
CameraStarted = 1 << 3,
|
|
ViewFinderFrameBufferAllocated = 1 << 4,
|
|
ImageCaptureFrameBufferAllocated = 1 << 5,
|
|
ImageCaptureFrameBuffersMapped = 1 << 6
|
|
}
|
|
|
|
private bool _disposedValue;
|
|
private readonly ILogger<CameraService> _logger;
|
|
private readonly CameraServiceConfiguration _configuration;
|
|
private State _state = State.NotInitialized;
|
|
private CameraManager? _cameraManager;
|
|
private Camera? _camera;
|
|
private CameraConfiguration? _cameraConfiguration;
|
|
private StreamConfiguration? _viewFinderStreamConfiguration;
|
|
private StreamConfiguration? _imageCaptureStreamConfiguration;
|
|
private FrameBufferAllocator? _viewFinderFrameBufferAllocator;
|
|
private FrameBufferAllocator? _imageCaptureFrameBufferAllocator;
|
|
private Stream? _viewFinderStream;
|
|
private Stream? _imageCaptureStream;
|
|
private List<MappedBuffer>? _imageCaptureMappedBuffers;
|
|
private readonly CancellationTokenSource _disposeCts = new CancellationTokenSource();
|
|
|
|
public CameraService(ILogger<CameraService> logger, CameraServiceConfiguration? configuration = null, bool autoInitialize = false)
|
|
{
|
|
_logger = logger;
|
|
|
|
_logger.LogInformation("CameraService initialization");
|
|
|
|
if (configuration is null)
|
|
{
|
|
_logger.LogWarning("CameraService configuration not set. Using default configuration.");
|
|
_configuration = CameraServiceConfiguration.Default;
|
|
}
|
|
else
|
|
{
|
|
_configuration = configuration;
|
|
}
|
|
|
|
_logger.LogDebug(@$"CameraService use following configuration:
|
|
CameraId: {_configuration.CameraId}
|
|
ViewFinderFrameBufferCount: {_configuration.ViewFinderFrameBufferCount}
|
|
ViewFinderWidth: {_configuration.ViewFinderWidth}
|
|
ViewFinderHeight: {_configuration.ViewFinderHeight}
|
|
ViewFinderFps: {_configuration.ViewFinderFps}
|
|
ImageCaptureFrameBufferCount: {_configuration.ImageCaptureFrameBufferCount}
|
|
ImageCaptureWidth: {_configuration.ImageCaptureWidth}
|
|
ImageCaptureHeight: {_configuration.ImageCaptureHeight}
|
|
");
|
|
|
|
if (autoInitialize)
|
|
Initialize(CancellationToken.None);
|
|
}
|
|
|
|
public async Task InitializeAsync(CancellationToken cancellationToken)
|
|
{
|
|
var linkedCancellationToken = CancellationTokenSource
|
|
.CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
|
|
.Token;
|
|
await Task.Run(() => Initialize(linkedCancellationToken), linkedCancellationToken);
|
|
}
|
|
public void Initialize(CancellationToken cancellationToken)
|
|
{
|
|
var effectiveCancellationToken = CancellationTokenSource
|
|
.CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
|
|
.Token;
|
|
|
|
try
|
|
{
|
|
if (_state != State.NotInitialized)
|
|
throw new InvalidOperationException("Camera already initialized");
|
|
|
|
CameraManagerCreateAndStart(effectiveCancellationToken);
|
|
CameraSelect(effectiveCancellationToken);
|
|
CameraAcquire(effectiveCancellationToken);
|
|
CameraConfigure(effectiveCancellationToken);
|
|
ViewFinderFrameBuffersAllocate(effectiveCancellationToken);
|
|
ImageCaptureFrameBuffersAllocate(effectiveCancellationToken);
|
|
CameraStart(effectiveCancellationToken);
|
|
ViewFinderFrameBuffersInitialize(effectiveCancellationToken);
|
|
ImageCaptureFrameBuffersInitialize(effectiveCancellationToken);
|
|
ImageCaptureFrameBuffersMap(effectiveCancellationToken);
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
_logger.LogCritical(e, "An exception occurred while camera initialization");
|
|
Cleanup();
|
|
throw;
|
|
}
|
|
}
|
|
private void CameraManagerCreateAndStart(CancellationToken cancellationToken)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
_cameraManager = new CameraManager();
|
|
int result = _cameraManager.Start();
|
|
if (result < 0)
|
|
{
|
|
_logger.LogCritical("Error while camera manager start {Result}", result);
|
|
throw new Exception($"Error while camera manager start {result}");
|
|
}
|
|
|
|
_state |= State.CameraManagerStarted;
|
|
}
|
|
private void CameraSelect(CancellationToken cancellationToken)
|
|
{
|
|
if (_cameraManager is null)
|
|
throw new InvalidOperationException("CameraManager not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
if (_cameraManager.Cameras.Count() == 0)
|
|
throw new Exception("No cameras found");
|
|
|
|
_camera = _cameraManager.Cameras
|
|
.FirstOrDefault(c => c.Id == _configuration.CameraId);
|
|
|
|
if (_camera is null)
|
|
{
|
|
var camerasIds = string.Join("\", \"", _cameraManager.Cameras.Select(c => c.Id));
|
|
_logger.LogError(
|
|
"Configured camera id (\"{CameraId}\") not found, using first camera from following list: [\"{Cameras}\"]",
|
|
_configuration.CameraId,
|
|
camerasIds);
|
|
|
|
_camera = _cameraManager.Cameras.First();
|
|
}
|
|
}
|
|
private void CameraAcquire(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
int result = _camera.Acquire();
|
|
if (result < 0)
|
|
{
|
|
_logger.LogCritical("Error while camera acquire {Result}", result);
|
|
throw new Exception($"Error while camera acquire {result}");
|
|
}
|
|
|
|
_state |= State.CameraAcquired;
|
|
}
|
|
private void CameraConfigure(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
_cameraConfiguration = _camera.GenerateConfiguration(
|
|
StreamRoleEnum.Viewfinder,
|
|
StreamRoleEnum.Raw);
|
|
|
|
_viewFinderStreamConfiguration = _cameraConfiguration.First();
|
|
_viewFinderStreamConfiguration.BufferCount = checked((uint)_configuration.ViewFinderFrameBufferCount);
|
|
_viewFinderStreamConfiguration.Size = new Size(
|
|
checked((uint)_configuration.ViewFinderWidth),
|
|
checked((uint)_configuration.ViewFinderHeight)
|
|
);
|
|
|
|
_imageCaptureStreamConfiguration = _cameraConfiguration.Skip(1).First();
|
|
_imageCaptureStreamConfiguration.BufferCount = checked((uint)_configuration.ImageCaptureFrameBufferCount);
|
|
_imageCaptureStreamConfiguration.Size = new Size(
|
|
checked((uint)_configuration.ImageCaptureWidth),
|
|
checked((uint)_configuration.ImageCaptureHeight)
|
|
);
|
|
|
|
if (_cameraConfiguration.Validate() == CameraConfiguration.Status.Invalid)
|
|
{
|
|
_logger.LogCritical("Error while camera configuration validation");
|
|
throw new Exception("Error while camera configuration validation");
|
|
}
|
|
|
|
_logger.LogDebug(@$"ViewFinder stream configuration:
|
|
Size: {_viewFinderStreamConfiguration.Size.ToString(true)}
|
|
FrameSize: {_viewFinderStreamConfiguration.FrameSize}
|
|
Stride: {_viewFinderStreamConfiguration.Stride}
|
|
ColorSpace: {_viewFinderStreamConfiguration.ColorSpace?.ToString(true)}
|
|
PixelFormat: {_viewFinderStreamConfiguration.PixelFormat?.ToString(true)}");
|
|
|
|
_logger.LogDebug(@$"ImageCapture stream configuration:
|
|
Size: {_imageCaptureStreamConfiguration.Size.ToString(true)}
|
|
FrameSize: {_imageCaptureStreamConfiguration.FrameSize}
|
|
Stride: {_imageCaptureStreamConfiguration.Stride}
|
|
ColorSpace: {_imageCaptureStreamConfiguration.ColorSpace?.ToString(true)}
|
|
PixelFormat: {_imageCaptureStreamConfiguration.PixelFormat?.ToString(true)}");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
int result = _camera.Configure(_cameraConfiguration);
|
|
if (result < 0)
|
|
{
|
|
_logger.LogCritical("Error while camera configuration {Result}", result);
|
|
throw new Exception($"Error while camera configuration {result}");
|
|
}
|
|
|
|
_viewFinderStream = _viewFinderStreamConfiguration.Stream();
|
|
_imageCaptureStream = _imageCaptureStreamConfiguration.Stream();
|
|
}
|
|
private void CameraStart(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
int result = _camera.Start();
|
|
if (result < 0)
|
|
{
|
|
_logger.LogCritical("Error while camera start {Result}", result);
|
|
throw new Exception($"Error while camera start {result}");
|
|
}
|
|
|
|
_state |= State.CameraStarted;
|
|
}
|
|
private void ViewFinderFrameBuffersAllocate(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
if (_viewFinderStream is null)
|
|
throw new InvalidOperationException("ViewFinderStream not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
_viewFinderFrameBufferAllocator = new FrameBufferAllocator(_camera);
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
int result = _viewFinderFrameBufferAllocator.Allocate(_viewFinderStream);
|
|
if (result < 0)
|
|
{
|
|
_logger.LogCritical("Error while view finder frame buffers allocation {Result}", result);
|
|
throw new Exception($"Error while view finder frame buffers allocation {result}");
|
|
}
|
|
|
|
_state |= State.ViewFinderFrameBufferAllocated;
|
|
}
|
|
private void ImageCaptureFrameBuffersAllocate(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
if (_imageCaptureStream is null)
|
|
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
_imageCaptureFrameBufferAllocator = new FrameBufferAllocator(_camera);
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
int result = _imageCaptureFrameBufferAllocator.Allocate(_imageCaptureStream);
|
|
if (result < 0)
|
|
{
|
|
_logger.LogCritical("Error while image capture frame buffers allocation {Result}", result);
|
|
throw new Exception($"Error while image capture frame buffers allocation {result}");
|
|
}
|
|
|
|
_state |= State.ImageCaptureFrameBufferAllocated;
|
|
}
|
|
private void ViewFinderFrameBuffersInitialize(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
if (_viewFinderStream is null)
|
|
throw new InvalidOperationException("ViewFinderStream not initialized");
|
|
if (_viewFinderFrameBufferAllocator is null)
|
|
throw new InvalidOperationException("ViewFinderFrameBufferAllocator not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var buffers = _viewFinderFrameBufferAllocator.Buffers(_viewFinderStream);
|
|
|
|
_logger.LogDebug("View finder frame buffers initialization");
|
|
foreach (var buffer in buffers)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
|
var result = request.AddBuffer(_viewFinderStream, buffer);
|
|
if (result < 0)
|
|
throw new Exception("Error while request create");
|
|
|
|
var cts = new TaskCompletionSource();
|
|
void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
|
{
|
|
if (e.Request.Handle == request.Handle)
|
|
cts.SetResult();
|
|
}
|
|
|
|
_camera.RequestCompleted += RequestCompleted;
|
|
result = _camera.QueueRequest(request);
|
|
if (result < 0)
|
|
{
|
|
_camera.RequestCompleted -= RequestCompleted;
|
|
throw new Exception("Error while request queue");
|
|
}
|
|
cts.Task.WaitAsync(cancellationToken).GetAwaiter().GetResult();
|
|
_camera.RequestCompleted -= RequestCompleted;
|
|
|
|
_logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
|
|
_logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
|
_logger.LogDebug("Request buffer planes: ");
|
|
foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
|
|
_logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
|
|
foreach (var (i, plane) in buffer.Planes.Enumerate())
|
|
_logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
|
|
}
|
|
}
|
|
private void ImageCaptureFrameBuffersInitialize(CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
if (_imageCaptureStream is null)
|
|
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
|
if (_imageCaptureFrameBufferAllocator is null)
|
|
throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
|
|
|
|
_logger.LogDebug("Image capture frame buffers initialization");
|
|
foreach (var buffer in buffers)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
|
var result = request.AddBuffer(_imageCaptureStream, buffer);
|
|
if (result < 0)
|
|
throw new Exception("Error while request create");
|
|
|
|
var cts = new TaskCompletionSource();
|
|
void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
|
{
|
|
if (e.Request.Handle == request.Handle)
|
|
cts.SetResult();
|
|
}
|
|
|
|
_camera.RequestCompleted += RequestCompleted;
|
|
result = _camera.QueueRequest(request);
|
|
if (result < 0)
|
|
{
|
|
_camera.RequestCompleted -= RequestCompleted;
|
|
throw new Exception("Error while request queue");
|
|
}
|
|
cts.Task.WaitAsync(cancellationToken).GetAwaiter().GetResult();
|
|
_camera.RequestCompleted -= RequestCompleted;
|
|
|
|
_logger.LogDebug("Request complited: \n\tResult: {} \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", result, request.Status, request.Cookie, request.Sequence, request.HasPendingBuffers);
|
|
_logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
|
foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
|
|
_logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
|
|
foreach (var (i, plane) in buffer.Planes.Enumerate())
|
|
_logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
|
|
}
|
|
}
|
|
private void ImageCaptureFrameBuffersMap(CancellationToken cancellationToken)
|
|
{
|
|
if (_imageCaptureStream is null)
|
|
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
|
if (_imageCaptureFrameBufferAllocator is null)
|
|
throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
|
|
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
_imageCaptureMappedBuffers = new List<MappedBuffer>();
|
|
|
|
var frameBuffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
|
|
|
|
foreach (var buffer in frameBuffers)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var planesByFd = buffer.Planes.GroupBy(p => p.Fd.Get());
|
|
|
|
foreach (var fdGroup in planesByFd)
|
|
{
|
|
cancellationToken.ThrowIfCancellationRequested();
|
|
|
|
int fd = fdGroup.Key;
|
|
|
|
ulong minOffset = fdGroup.Min(p => p.Offset);
|
|
ulong maxOffset = fdGroup.Max(p => p.Offset + p.Length);
|
|
|
|
ulong pageSize = (ulong)Environment.SystemPageSize;
|
|
ulong alignedOffset = minOffset & ~(pageSize - 1);
|
|
ulong alignedLength = (maxOffset - alignedOffset + pageSize - 1) & ~(pageSize - 1);
|
|
|
|
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($"Error while buffers mapping, mmap failed for fd=0x{fd:X}, error={error}");
|
|
}
|
|
|
|
_logger.LogDebug("Mapped buffer fd=0x{:X}, address=0x{:X}, length=0x{:X}", fd, address, alignedLength);
|
|
|
|
_imageCaptureMappedBuffers.Add(new MappedBuffer
|
|
{
|
|
Fd = fd,
|
|
Address = address,
|
|
Length = alignedLength,
|
|
AlignedOffset = alignedOffset
|
|
});
|
|
}
|
|
}
|
|
|
|
_state |= State.ImageCaptureFrameBuffersMapped;
|
|
}
|
|
|
|
public async Task<OpenCvSharp.Mat?> CaptureImage(CancellationToken cancellationToken, int framesCount = 1)
|
|
{
|
|
ArgumentOutOfRangeException.ThrowIfLessThan(framesCount, 1, nameof(framesCount));
|
|
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
if (_imageCaptureStream is null)
|
|
throw new InvalidOperationException("ImageCaptureStream not initialized");
|
|
if (_imageCaptureStreamConfiguration is null)
|
|
throw new InvalidOperationException("ImageCaptureStreamConfiguration not initialized");
|
|
if (_imageCaptureFrameBufferAllocator is null)
|
|
throw new InvalidOperationException("ImageCaptureFrameBufferAllocator not initialized");
|
|
if (_imageCaptureMappedBuffers is null)
|
|
throw new InvalidOperationException("ImageCaptureMappedBuffers not mapped");
|
|
|
|
var effectiveCancellationToken = CancellationTokenSource
|
|
.CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
|
|
.Token;
|
|
|
|
_logger.LogDebug("Capturing {} frame(s)", framesCount);
|
|
|
|
OpenCvSharp.Mat? image = null;
|
|
|
|
var planesData = new List<byte[]>();
|
|
var fdsData = new Dictionary<int, byte[]>();
|
|
|
|
try
|
|
{
|
|
while (planesData.Count() < framesCount)
|
|
{
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
_logger.LogDebug("Requested {} frame(s), now collected {}", framesCount, planesData.Count());
|
|
if (File.Exists("/proc/self/status"))
|
|
_logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN");
|
|
|
|
var oldPlanesDataCount = planesData.Count();
|
|
|
|
var buffers = _imageCaptureFrameBufferAllocator.Buffers(_imageCaptureStream);
|
|
if (buffers.Count() > framesCount - planesData.Count())
|
|
buffers = buffers.Take(framesCount - planesData.Count());
|
|
_logger.LogDebug("Taken {} buffer(s)", buffers.Count());
|
|
|
|
var requests = new List<Request>();
|
|
|
|
foreach (var buffer in buffers)
|
|
{
|
|
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
|
var result = request.AddBuffer(_imageCaptureStream, buffer);
|
|
if (result < 0)
|
|
throw new Exception("Error while request create");
|
|
requests.Add(request);
|
|
}
|
|
_logger.LogDebug("Request(s) created");
|
|
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var cts = new TaskCompletionSource();
|
|
void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
|
{
|
|
if (!requests.Any(r => r.Handle == e.Request.Handle))
|
|
return;
|
|
|
|
var buffer = e.Request.FindBuffer(_imageCaptureStream);
|
|
if (buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameError ||
|
|
buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup)
|
|
{
|
|
_logger.LogDebug("Request buffers status: {}", buffer.Metadata.Status);
|
|
e.Request.Reuse(Request.ReuseFlagEnum.ReuseBuffers);
|
|
var result = _camera.QueueRequest(e.Request);
|
|
if (result >= 0)
|
|
return;
|
|
}
|
|
|
|
_logger.LogDebug("Request complited: \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", e.Request.Status, e.Request.Cookie, e.Request.Sequence, e.Request.HasPendingBuffers);
|
|
_logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
|
_logger.LogDebug("Request buffer planes: ");
|
|
foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
|
|
_logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
|
|
foreach (var (i, plane) in buffer.Planes.Enumerate())
|
|
_logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
|
|
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
foreach (var plane in buffer.Planes)
|
|
{
|
|
var fd = plane.Fd.Get();
|
|
if (fdsData.ContainsKey(fd))
|
|
continue;
|
|
var mappedBuffer = _imageCaptureMappedBuffers.Find(b => b.Fd == fd);
|
|
if (mappedBuffer is null)
|
|
continue;
|
|
var fdData = new byte[mappedBuffer.Length];
|
|
Marshal.Copy(mappedBuffer.Address, fdData, 0, checked((int)mappedBuffer.Length));
|
|
fdsData.Add(fd, fdData);
|
|
}
|
|
|
|
foreach (var plane in buffer.Planes)
|
|
{
|
|
var fd = plane.Fd.Get();
|
|
if (!fdsData.ContainsKey(fd))
|
|
continue;
|
|
var fdData = fdsData[fd];
|
|
var planeData = fdData
|
|
.Skip(checked((int)plane.Offset))
|
|
.Take(checked((int)plane.Length))
|
|
.ToArray();
|
|
planesData.Add(planeData);
|
|
}
|
|
|
|
if (planesData.Count() - oldPlanesDataCount == requests.Count())
|
|
cts.SetResult();
|
|
}
|
|
|
|
_camera.RequestCompleted += RequestCompleted;
|
|
foreach (var request in requests)
|
|
{
|
|
var result = _camera.QueueRequest(request);
|
|
if (result < 0)
|
|
{
|
|
_camera.RequestCompleted -= RequestCompleted;
|
|
throw new Exception("Error while request queue");
|
|
}
|
|
}
|
|
_logger.LogDebug("Request(s) queued");
|
|
await cts.Task.WaitAsync(effectiveCancellationToken);
|
|
_camera.RequestCompleted -= RequestCompleted;
|
|
|
|
foreach (var key in fdsData.Keys)
|
|
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
|
|
fdsData[key] = null;
|
|
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
|
|
fdsData.Clear();
|
|
}
|
|
|
|
_logger.LogDebug("Frame(s) collected");
|
|
if (File.Exists("/proc/self/status"))
|
|
_logger.LogDebug("Current RAM Usage: {}", File.ReadAllLines("/proc/self/status").FirstOrDefault(l => l.StartsWith("RssAnon:"))?.Substring("RssAnon:".Length)?.Trim() ?? "UNKNOWN");
|
|
|
|
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var config = _imageCaptureStreamConfiguration;
|
|
var size = config.Size;
|
|
var fourcc = config.PixelFormat.Fourcc;
|
|
var modifier = config.PixelFormat.Modifier;
|
|
var primariesEnum = config.ColorSpace?.Primaries;
|
|
var transferFunctionEnum = config.ColorSpace?.TransferFunction;
|
|
var ycbcrEncodingEnum = config.ColorSpace?.YcbcrEncoding;
|
|
var rangeEnum = config.ColorSpace?.Range;
|
|
var stride = config.Stride;
|
|
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
var decoder = FrameDecoderHelper.FindDecoder(fourcc, modifier, primariesEnum, transferFunctionEnum, ycbcrEncodingEnum, rangeEnum);
|
|
if (decoder is null)
|
|
throw new Exception($"Cannot find decoder for fourcc=0x{fourcc:X}({new string(BitConverter.GetBytes(fourcc).Select(b => (char)b).ToArray())}) modifier=0x{modifier:X} primariesEnum={primariesEnum} transferFunctionEnum={transferFunctionEnum} ycbcrEncodingEnum={ycbcrEncodingEnum} rangeEnum={rangeEnum}");
|
|
_logger.LogDebug("Found decoder {}", decoder);
|
|
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
try
|
|
{
|
|
image = decoder.Decode(checked((int)size.Width), checked((int)size.Height), checked((int)stride), planesData);
|
|
}
|
|
catch (Exception ex)
|
|
{
|
|
_logger.LogError(ex, "Error while image decode");
|
|
throw;
|
|
}
|
|
_logger.LogDebug("Image decoded successfully");
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
_logger.LogError(e, "Error while image capture");
|
|
}
|
|
finally
|
|
{
|
|
foreach (var key in fdsData.Keys)
|
|
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
|
|
fdsData[key] = null;
|
|
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
|
|
fdsData.Clear();
|
|
for (int i = 0; i < planesData.Count(); i++)
|
|
#pragma warning disable CS8625 // Cannot convert null literal to non-nullable reference type.
|
|
planesData[i] = null;
|
|
#pragma warning restore CS8625 // Cannot convert null literal to non-nullable reference type.
|
|
planesData.Clear();
|
|
}
|
|
return image;
|
|
}
|
|
|
|
public async Task<(FrameBuffer, StreamConfiguration)?> CaptureViewFinderBuffer(int bufferId, CancellationToken cancellationToken)
|
|
{
|
|
if (_camera is null)
|
|
throw new InvalidOperationException("Camera not initialized");
|
|
if (_viewFinderStream is null)
|
|
throw new InvalidOperationException("ViewFinderStream not initialized");
|
|
if (_viewFinderStreamConfiguration is null)
|
|
throw new InvalidOperationException("ViewFinderStreamConfiguration not initialized");
|
|
if (_viewFinderFrameBufferAllocator is null)
|
|
throw new InvalidOperationException("ViewFinderFrameBufferAllocator not initialized");
|
|
|
|
var effectiveCancellationToken = CancellationTokenSource
|
|
.CreateLinkedTokenSource(_disposeCts.Token, cancellationToken)
|
|
.Token;
|
|
|
|
var buffers = _viewFinderFrameBufferAllocator.Buffers(_viewFinderStream);
|
|
var buffer = buffers.Skip(bufferId % buffers.Count()).First();
|
|
|
|
var request = _camera.CreateRequest(checked((ulong)DateTimeOffset.UtcNow.ToUnixTimeMilliseconds()));
|
|
|
|
int fd = LibC.memfd_create($"buffer_fence_{bufferId % buffers.Count()}", 0x0002U);
|
|
if (fd < 0)
|
|
{
|
|
throw new Exception($"memfd_create error: {Marshal.GetLastPInvokeError()} {Marshal.GetLastPInvokeErrorMessage()}");
|
|
}
|
|
Console.WriteLine($"Created fd {fd}");
|
|
var fence = new Fence(new UniqueFD(fd));
|
|
var result = request.AddBuffer(_viewFinderStream, buffer, fence);
|
|
if (result < 0)
|
|
{
|
|
var fenceFd = buffer.ReleaseFence()?.Fd?.Release();
|
|
Console.WriteLine($"Request queue failed, closing fd {fenceFd}");
|
|
if (fenceFd is not null)
|
|
LibC.close(fenceFd.Value);
|
|
throw new Exception("Error while request create");
|
|
}
|
|
// _logger.LogDebug("Request(s) created");
|
|
|
|
effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
// var cts = new TaskCompletionSource();
|
|
// void RequestCompleted(object? sender, Camera.RequestCompletedEventArgs e)
|
|
// {
|
|
// if (request.Handle != e.Request.Handle)
|
|
// return;
|
|
|
|
// var buffer = e.Request.FindBuffer(_viewFinderStream);
|
|
// if (buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameError ||
|
|
// buffer.Metadata.Status == FrameMetadata.StatusEnum.FrameStartup)
|
|
// {
|
|
// // _logger.LogDebug("Request buffers status: {}", buffer.Metadata.Status);
|
|
// e.Request.Reuse(Request.ReuseFlagEnum.ReuseBuffers);
|
|
// var result = _camera.QueueRequest(e.Request);
|
|
// if (result >= 0)
|
|
// return;
|
|
// }
|
|
|
|
// // _logger.LogDebug("Request complited: \n\tStatus: {}\n\tCookie: {}\n\tSequence: {}\n\tHasPendingBuffers: {}", e.Request.Status, e.Request.Cookie, e.Request.Sequence, e.Request.HasPendingBuffers);
|
|
// // _logger.LogDebug("Request buffer: \n\tStatus: {} \n\tCookie: {} \n\tSequence: {} \n\tTimestamp: {}", buffer.Metadata.Status, buffer.Cookie, buffer.Metadata.Sequence, buffer.Metadata.Timestamp);
|
|
// // _logger.LogDebug("Request buffer planes: ");
|
|
// // foreach (var (i, plane) in buffer.Metadata.Planes.Enumerate())
|
|
// // _logger.LogDebug("Plane[{}]: bytes used: 0x{:X}", i, plane.BytesUsed);
|
|
// // foreach (var (i, plane) in buffer.Planes.Enumerate())
|
|
// // _logger.LogDebug("Plane[{}]: \n\tFd: 0x{:X} \n\tOffset: 0x{:X} \n\tLength: 0x{:X} \n\tKInvalidOffset: 0x{:X}", i, plane.Fd.Get(), plane.Offset, plane.Length, plane.KInvalidOffset);
|
|
|
|
// effectiveCancellationToken.ThrowIfCancellationRequested();
|
|
|
|
// cts.SetResult();
|
|
// }
|
|
|
|
// _camera.RequestCompleted += RequestCompleted;
|
|
result = _camera.QueueRequest(request);
|
|
if (result < 0)
|
|
{
|
|
// _camera.RequestCompleted -= RequestCompleted;
|
|
throw new Exception("Error while request queue");
|
|
}
|
|
// _logger.LogDebug("Request queued");
|
|
// await cts.Task.WaitAsync(effectiveCancellationToken);
|
|
// _camera.RequestCompleted -= RequestCompleted;
|
|
// _logger.LogDebug("Request complited");
|
|
|
|
return new(buffer, _viewFinderStreamConfiguration);
|
|
}
|
|
|
|
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) && _imageCaptureMappedBuffers is not null)
|
|
{
|
|
foreach (var buffer in _imageCaptureMappedBuffers)
|
|
{
|
|
_logger.LogDebug("MUNMAP address=0x{:X}, length=0x{:X}", buffer.Address, buffer.Length);
|
|
LibC.munmap(buffer.Address, buffer.Length);
|
|
}
|
|
_imageCaptureMappedBuffers.Clear();
|
|
}
|
|
if (_state.HasFlag(State.ViewFinderFrameBufferAllocated) && _viewFinderStream is not null)
|
|
_viewFinderFrameBufferAllocator?.Free(_viewFinderStream);
|
|
if (_state.HasFlag(State.ImageCaptureFrameBufferAllocated) && _imageCaptureStream is not null)
|
|
_imageCaptureFrameBufferAllocator?.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();
|
|
// _viewFinderFrameBufferAllocator?.Dispose();
|
|
// _imageCaptureFrameBufferAllocator?.Dispose();
|
|
_cameraManager?.Dispose();
|
|
}
|
|
catch (Exception e)
|
|
{
|
|
_logger.LogError(e, "Error while CameraService cleanup");
|
|
}
|
|
finally
|
|
{
|
|
_cameraManager = null;
|
|
_camera = null;
|
|
_viewFinderFrameBufferAllocator = null;
|
|
_imageCaptureFrameBufferAllocator = null;
|
|
_viewFinderStream = null;
|
|
_imageCaptureStream = null;
|
|
_imageCaptureMappedBuffers = null;
|
|
_state = State.NotInitialized;
|
|
}
|
|
}
|
|
}
|