Adds camera serviceHEADmaster

6 files changed, 254 insertions, 101 deletions

diff --git a/hsm-cam/FFI/Cam.cpp b/hsm-cam/FFI/Cam.cpp
index 05fd1a8..9e371c1 100644
--- a/hsm-cam/FFI/Cam.cpp
+++ b/hsm-cam/FFI/Cam.cpp
@@ -25,8 +25,9 @@ logMsg(Severity severity, const format_string<Args...> fmt, const Args &...args)
 void
 request_complete(Request *request)
 {
-    logMsg(Trace, "Completed request");
-    g_callback(request->buffers().begin()->second->planes()[0].fd.get());
+    int sequence = request->buffers().begin()->second->metadata().sequence;
+    logMsg(Trace, "Completed request #{}", sequence);
+    g_callback();
 }
 
 extern "C" void
@@ -44,56 +45,101 @@ register_callback(HsCallback hs_callback)
 }
 
 extern "C" void
-initialize_ffi()
+start_camera_manager()
 {
     logMsg(Info, "Starting camera manager");
     g_manager = make_unique<CameraManager>();
     g_manager->start();
+}
+
+extern "C" void
+stop_camera_manager()
+{
+    logMsg(Info, "Stopping camera manager");
+    g_manager->stop();
+}
 
+extern "C" void
+acquire_camera()
+{
     logMsg(Info, "Acquiring camera");
     g_camera = g_manager->cameras()[0];
     g_camera->acquire();
+    logMsg(Info, "Acquired camera: {}", g_camera->id());
+}
 
-    logMsg(Info, "Generating still capture configuration");
+extern "C" void
+release_camera()
+{
+    logMsg(Info, "Releasing camera");
+    g_camera->release();
+    g_camera.reset();
+}
+
+extern "C" void
+allocate_frame_buffer()
+{
+    logMsg(Info, "Generating camera configuration");
     g_config = g_camera->generateConfiguration({ StreamRole::StillCapture });
+    g_config->at(0).size.width = FRAME_WIDTH;
+    g_config->at(0).size.height = FRAME_HEIGHT;
+    g_config->at(0).pixelFormat = formats::RGB888;
+    logMsg(Info, "Generated camera configuration: {}", g_config->at(0).toString());
     g_camera->configure(g_config.get());
 
-    logMsg(Info, "Allocating buffer");
+    logMsg(Info, "Generating frame buffer allocator");
     g_allocator = make_unique<FrameBufferAllocator>(g_camera);
-    g_allocator->allocate((*g_config)[0].stream());
+    g_allocator->allocate(g_config->at(0).stream());
 
     logMsg(Info, "Registering request complete callback");
     g_camera->requestCompleted.connect(request_complete);
+}
 
+extern "C" void
+free_frame_buffer()
+{
+    logMsg(Info, "Freeing frame buffer allocator");
+    g_allocator->free(g_config->at(0).stream());
+    g_allocator.reset();
+}
+
+extern "C" void
+start_camera()
+{
     logMsg(Info, "Starting camera");
     g_camera->start();
 }
 
 extern "C" void
-shutdown_ffi()
+stop_camera()
 {
     logMsg(Info, "Stopping camera");
     g_camera->stop();
+}
 
-    logMsg(Info, "Freeing frame buffer allocator");
-    g_allocator->free((*g_config)[0].stream());
-    g_allocator.reset();
+extern "C" void
+create_request()
+{
+    logMsg(Info, "Creating request");
+    g_request = g_camera->createRequest();
 
-    logMsg(Info, "Releasing camera");
-    g_camera->release();
-    g_camera.reset();
+    logMsg(Info, "Setting buffer for request");
+    Stream *stream = g_config->at(0).stream();
+    g_request->addBuffer(stream, g_allocator->buffers(stream)[0].get());
+}
 
-    logMsg(Info, "Stopping camera manager");
-    g_manager->stop();
+extern "C" int
+get_dma_buffer_fd()
+{
+    int fd = g_request->buffers().begin()->second->planes()[0].fd.get();
+    logMsg(Info, "DMA buffer available in FD {}", fd);
+    return fd;
 }
 
 extern "C" void
-request_capture()
+request_frame()
 {
-    logMsg(Trace, "Requesting still capture");
-
-    Stream *stream = (*g_config)[0].stream();
-    g_request = g_camera->createRequest();
-    g_request->addBuffer(stream, g_allocator->buffers(stream)[0].get());
+    logMsg(Trace, "Requested frame");
+    g_request->reuse(Request::ReuseBuffers);
     g_camera->queueRequest(g_request.get());
 }
diff --git a/hsm-cam/FFI/Cam.hpp b/hsm-cam/FFI/Cam.hpp
index c2cd4ed..374e16a 100644
--- a/hsm-cam/FFI/Cam.hpp
+++ b/hsm-cam/FFI/Cam.hpp
@@ -1,6 +1,16 @@
 #ifndef CAM_HPP
 #define CAM_HPP
 
+// RGB888 configuration for ov5647 sensor (Raspberry Pi Camera Module)
+// Must be updated if either:
+// - Pixel format changes (e.g., to BGR, YUV, etc.)
+// - Camera module is replaced
+#define FRAME_WIDTH (800)
+#define FRAME_HEIGHT (600)
+#define FRAME_LINE (FRAME_WIDTH * 3)
+#define FRAME_STRIDE (FRAME_LINE + 32)
+#define FRAME_BUFFER_LENGTH (FRAME_STRIDE * FRAME_HEIGHT + 3072)
+
 enum Severity
 {
     Attention = 0,
@@ -9,7 +19,7 @@ enum Severity
 };
 
 typedef void (*HsLogger)(enum Severity, const char *);
-typedef void (*HsCallback)(int fd);
+typedef void (*HsCallback)();
 
 #ifdef __cplusplus
 extern "C"
@@ -17,9 +27,18 @@ extern "C"
 #endif
     void register_logger(HsLogger hs_logger);
     void register_callback(HsCallback hs_callback);
-    void initialize_ffi();
-    void shutdown_ffi();
-    void request_capture();
+    void start_camera_manager();
+    void stop_camera_manager();
+    void acquire_camera();
+    void release_camera();
+    void allocate_frame_buffer();
+    void free_frame_buffer();
+    void start_camera();
+    void stop_camera();
+    void create_request();
+
+    int get_dma_buffer_fd();
+    void request_frame();
 #ifdef __cplusplus
 }
 #endif
diff --git a/hsm-cam/Hsm/Cam.hs b/hsm-cam/Hsm/Cam.hs
index 78a3e25..8300ae7 100644
--- a/hsm-cam/Hsm/Cam.hs
+++ b/hsm-cam/Hsm/Cam.hs
@@ -1,61 +1,86 @@
 {-# LANGUAGE AllowAmbiguousTypes #-}
+{-# LANGUAGE RecordWildCards #-}
 {-# LANGUAGE TypeFamilies #-}
 
 module Hsm.Cam
   ( Cam
-  , stillCapture
+  , capturePng
   , runCam
-  )
-where
+  ) where
 
+import Codec.Picture (Image(Image), encodePng)
+import Codec.Picture.Types (PixelRGB8)
 import Control.Concurrent (MVar, newEmptyMVar, putMVar, takeMVar)
-import Control.Monad (void)
-import Effectful (Dispatch (Static), DispatchOf, Eff, IOE, liftIO, (:>))
-import Effectful.Dispatch.Static
-  ( SideEffects (WithSideEffects)
-  , StaticRep
-  , evalStaticRep
-  , getStaticRep
-  , unsafeEff_
-  )
-import Effectful.Resource (Resource, allocateEff, allocateEff_)
+import Control.Exception (mask_)
+import Data.ByteString.Lazy (ByteString)
+import Data.Primitive.Ptr (readOffPtr)
+import Data.Vector.Storable (generateM)
+import Effectful (Dispatch(Static), DispatchOf, Eff, IOE, (:>), liftIO)
+import Effectful.Dispatch.Static (SideEffects(WithSideEffects), StaticRep, evalStaticRep, getStaticRep, unsafeEff_)
+import Effectful.Exception (bracket, bracket_)
 import Foreign.C.String (peekCString)
-import Foreign.Ptr (freeHaskellFunPtr)
+import Foreign.C.Types (CSize(CSize))
+import Foreign.Ptr (Ptr, castPtr, freeHaskellFunPtr, nullPtr)
 import Hsm.Cam.FFI
-  ( initializeFFI
+  ( acquireCamera
+  , allocateFrameBuffer
+  , createRequest
+  , frameBufferLength
+  , frameHeight
+  , frameLine
+  , frameStride
+  , frameWidth
+  , freeFrameBuffer
+  , getDmaBufferFd
   , makeCallback
   , makeLogger
   , registerCallback
   , registerLogger
-  , requestCapture
-  , shutdownFFI
+  , releaseCamera
+  , requestFrame
+  , startCamera
+  , startCameraManager
+  , stopCamera
+  , stopCameraManager
   )
-import Hsm.Log (Log, Severity (Info, Trace), getLoggerIO, logMsg)
+import Hsm.Log (Log, Severity(Info, Trace), getLoggerIO, logMsg)
+import MMAP (mapShared, mkMmapFlags, mmap, munmap, protRead)
 
 data Cam (a :: * -> *) (b :: *)
 
 type instance DispatchOf Cam = Static WithSideEffects
 
-newtype instance StaticRep Cam
-  = Cam (MVar Int)
+data Rep = Rep
+  { callbackMVar :: MVar ()
+  , dmaBuffer :: Ptr ()
+  }
+
+newtype instance StaticRep Cam =
+  Cam Rep
 
-stillCapture :: (Log "cam" :> es, Cam :> es) => Eff es ()
-stillCapture = do
-  Cam fdVar <- getStaticRep
-  logMsg Trace "Requesting still capture"
-  fd <- unsafeEff_ $ requestCapture >> takeMVar fdVar
-  logMsg Trace $ "Still capture data available in FD " <> show fd
+capturePng :: (Log "cam" :> es, Cam :> es) => Eff es ByteString
+capturePng = do
+  Cam Rep {..} <- getStaticRep
+  logMsg Trace "Requesting frame"
+  unsafeEff_ . mask_ $ requestFrame >> takeMVar callbackMVar
+  logMsg Trace "Processing frame data"
+  pixelVector <- unsafeEff_ . generateM (frameLine * frameHeight) $ mapPixel dmaBuffer
+  logMsg Trace "Encoding PNG"
+  return . encodePng $ Image @PixelRGB8 frameWidth frameHeight pixelVector
+  where
+    mapPixel dmaBuffer index = readOffPtr (castPtr dmaBuffer) offset
+      where
+        yIndex = index `div` frameLine
+        xIndex = index `mod` frameLine
+        offset = yIndex * frameStride + xIndex
 
-runCam
-  :: (IOE :> es, Log "cam" :> es, Resource :> es) => Eff (Cam : es) a -> Eff es a
+runCam :: (IOE :> es, Log "cam" :> es) => Eff (Cam : es) a -> Eff es a
 runCam action = do
-  fdVar <- liftIO newEmptyMVar
-  void loggerBracket
-  void $ requestCallbackBracket fdVar
-  void ffiBracket
-  evalStaticRep (Cam fdVar) action
+  callbackMVar <- liftIO newEmptyMVar
+  loggerBracket . callbackBracket callbackMVar . cameraManagerBracket . cameraAcquireBracket . frameBufferBracket . cameraStartBracket . createRequestBracket . mmapBracket $ \dmaBuffer -> do
+    evalStaticRep (Cam Rep {..}) action
   where
-    loggerBracket = allocateEff loggerAlloc loggerDealloc
+    loggerBracket = bracket loggerAlloc loggerDealloc . const
       where
         loggerAlloc = do
           logMsg Info "Registering FFI logger"
@@ -66,17 +91,31 @@ runCam action = do
         loggerDealloc loggerFFI = do
           logMsg Info "Unregistering FFI logger"
           liftIO $ freeHaskellFunPtr loggerFFI
-    requestCallbackBracket fdVar = allocateEff requestCallbackAlloc requestCallbackDealloc
+    callbackBracket callbackMVar = bracket callbackAlloc callbackDealloc . const
       where
-        requestCallbackAlloc = do
+        callbackAlloc = do
           logMsg Info "Registering FFI callback"
-          requestCallbackFFI <- liftIO . makeCallback $ putMVar fdVar
-          liftIO $ registerCallback requestCallbackFFI
-          return requestCallbackFFI
-        requestCallbackDealloc requestCallbackFFI = do
+          callbackFFI <- liftIO . makeCallback $ putMVar callbackMVar ()
+          liftIO $ registerCallback callbackFFI
+          return callbackFFI
+        callbackDealloc callbackFFI = do
           logMsg Info "Unregistering FFI callback"
-          liftIO $ freeHaskellFunPtr requestCallbackFFI
-    ffiBracket = allocateEff_ ffiAlloc ffiDealloc
+          liftIO $ freeHaskellFunPtr callbackFFI
+    -- Convenience wrappers for specific libcamera resources
+    bracketFFI alloc dealloc = bracket_ (liftIO alloc) (liftIO dealloc)
+    cameraManagerBracket = bracketFFI startCameraManager stopCameraManager
+    cameraAcquireBracket = bracketFFI acquireCamera releaseCamera
+    frameBufferBracket = bracketFFI allocateFrameBuffer freeFrameBuffer
+    cameraStartBracket = bracketFFI startCamera stopCamera
+    createRequestBracket = bracketFFI createRequest $ return ()
+    -- Memory maps the camera's DMA buffer for frame access
+    mmapBracket = bracket mmapDmaBuffer munmapDmaBuffer
       where
-        ffiAlloc = liftIO initializeFFI
-        ffiDealloc = liftIO shutdownFFI
+        mmapSize = CSize $ toEnum frameBufferLength
+        mmapFlags = mkMmapFlags mapShared mempty
+        mmapDmaBuffer = do
+          logMsg Info "Mapping DMA buffer"
+          liftIO $ getDmaBufferFd >>= \dmaBufferFd -> mmap nullPtr mmapSize protRead mmapFlags dmaBufferFd 0
+        munmapDmaBuffer dmaBuffer = do
+          logMsg Info "Unmapping DMA buffer"
+          liftIO $ munmap dmaBuffer mmapSize
diff --git a/hsm-cam/Hsm/Cam/FFI.hs b/hsm-cam/Hsm/Cam/FFI.hs
index 93d2f57..1c37fac 100644
--- a/hsm-cam/Hsm/Cam/FFI.hs
+++ b/hsm-cam/Hsm/Cam/FFI.hs
@@ -1,36 +1,73 @@
 {-# LANGUAGE CApiFFI #-}
 
 module Hsm.Cam.FFI
-  ( makeLogger
+  ( frameStride
+  , frameBufferLength
+  , frameWidth
+  , frameHeight
+  , frameLine
+  , makeLogger
   , registerLogger
   , makeCallback
   , registerCallback
-  , initializeFFI
-  , shutdownFFI
-  , requestCapture
-  )
-where
+  , startCameraManager
+  , stopCameraManager
+  , acquireCamera
+  , releaseCamera
+  , allocateFrameBuffer
+  , freeFrameBuffer
+  , startCamera
+  , stopCamera
+  , createRequest
+  , getDmaBufferFd
+  , requestFrame
+  ) where
 
 import Foreign.C.String (CString)
+import Foreign.C.Types (CInt(CInt))
 import Foreign.Ptr (FunPtr)
+import System.Posix.Types (Fd(Fd))
 
 type Logger = Int -> CString -> IO ()
 
-type Callback = Int -> IO ()
+type Callback = IO ()
 
-foreign import ccall safe "wrapper" makeLogger :: Logger -> IO (FunPtr Logger)
+foreign import capi unsafe "Cam.hpp value FRAME_WIDTH" frameWidth :: Int
 
-foreign import capi safe "Cam.hpp register_logger"
-  registerLogger :: FunPtr Logger -> IO ()
+foreign import capi unsafe "Cam.hpp value FRAME_HEIGHT" frameHeight :: Int
 
-foreign import ccall safe "wrapper"
-  makeCallback :: Callback -> IO (FunPtr Callback)
+foreign import capi unsafe "Cam.hpp value FRAME_LINE" frameLine :: Int
 
-foreign import capi safe "Cam.hpp register_callback"
-  registerCallback :: FunPtr Callback -> IO ()
+foreign import capi unsafe "Cam.hpp value FRAME_STRIDE" frameStride :: Int
 
-foreign import capi safe "Cam.hpp initialize_ffi" initializeFFI :: IO ()
+foreign import capi unsafe "Cam.hpp value FRAME_BUFFER_LENGTH" frameBufferLength :: Int
 
-foreign import capi safe "Cam.hpp shutdown_ffi" shutdownFFI :: IO ()
+foreign import ccall unsafe "wrapper" makeLogger :: Logger -> IO (FunPtr Logger)
 
-foreign import capi safe "Cam.hpp request_capture" requestCapture :: IO ()
+foreign import ccall safe "Cam.hpp register_logger" registerLogger :: FunPtr Logger -> IO ()
+
+foreign import ccall unsafe "wrapper" makeCallback :: Callback -> IO (FunPtr Callback)
+
+foreign import ccall safe "Cam.hpp register_callback" registerCallback :: FunPtr Callback -> IO ()
+
+foreign import ccall safe "Cam.hpp start_camera_manager" startCameraManager :: IO Int
+
+foreign import ccall safe "Cam.hpp stop_camera_manager" stopCameraManager :: IO ()
+
+foreign import ccall safe "Cam.hpp acquire_camera" acquireCamera :: IO Int
+
+foreign import ccall safe "Cam.hpp release_camera" releaseCamera :: IO ()
+
+foreign import ccall safe "Cam.hpp allocate_frame_buffer" allocateFrameBuffer :: IO Int
+
+foreign import ccall safe "Cam.hpp free_frame_buffer" freeFrameBuffer :: IO ()
+
+foreign import ccall safe "Cam.hpp start_camera" startCamera :: IO Int
+
+foreign import ccall safe "Cam.hpp stop_camera" stopCamera :: IO ()
+
+foreign import ccall safe "Cam.hpp create_request" createRequest :: IO Int
+
+foreign import ccall safe "Cam.hpp get_dma_buffer_fd" getDmaBufferFd :: IO Fd
+
+foreign import ccall safe "Cam.hpp request_frame" requestFrame :: IO ()
diff --git a/hsm-cam/Test/Cam.hs b/hsm-cam/Test/Cam.hs
index 4cf9e7f..9b06785 100644
--- a/hsm-cam/Test/Cam.hs
+++ b/hsm-cam/Test/Cam.hs
@@ -1,8 +1,16 @@
+import Control.Monad (forM_)
+import Data.ByteString.Lazy (writeFile)
 import Data.Function ((&))
-import Effectful (runEff)
-import Effectful.Resource (runResource)
-import Hsm.Cam (runCam, stillCapture)
-import Hsm.Log (Severity (Trace), runLog)
+import Effectful (liftIO, runEff)
+import Hsm.Cam (capturePng, runCam)
+import Hsm.Log (Severity(Info, Trace), logMsg, runLog)
+import Prelude hiding (writeFile)
 
 main :: IO ()
-main = stillCapture & runCam & runLog @"cam" Trace & runResource & runEff
+main = forM_ [0 .. 31] savePng & runCam & runLog @"cam" Trace & runEff
+  where
+    savePng index = do
+      logMsg Info $ "Saving image to file: " <> path
+      capturePng >>= liftIO . writeFile path
+      where
+        path = "/tmp/hsm-cam-test" <> show @Int index <> ".png"
diff --git a/hsm-cam/hsm-cam.cabal b/hsm-cam/hsm-cam.cabal
index a4aa467..ade45d9 100644
--- a/hsm-cam/hsm-cam.cabal
+++ b/hsm-cam/hsm-cam.cabal
@@ -9,14 +9,16 @@ extra-source-files:
 library
   build-depends:
     , base
+    , bytestring
     , effectful-core
     , effectful-plugin
     , hsm-log
-    , resourcet-effectful
-
-  cxx-options:
-    -O3 -Wall -Wextra -Werror -std=c++20 -I/usr/include/libcamera
+    , JuicyPixels
+    , primitive
+    , shared-memory
+    , vector
 
+  cxx-options:      -O3 -Wall -Wextra -Werror -std=c++20
   cxx-sources:      FFI/Cam.cpp
   default-language: GHC2024
   exposed-modules:  Hsm.Cam
@@ -29,20 +31,22 @@ library
     -O2 -Wall -Werror -Wno-star-is-type -Wunused-packages
     -fplugin=Effectful.Plugin
 
-  include-dirs:     FFI Hsm/Cam
+  include-dirs:     FFI Hsm/Cam /usr/include/libcamera
   other-modules:    Hsm.Cam.FFI
 
 executable test-cam
   build-depends:
     , base
+    , bytestring
     , effectful-core
     , effectful-plugin
     , hsm-log
-    , resourcet-effectful
-
-  cxx-options:
-    -O3 -Wall -Wextra -Werror -std=c++20 -I/usr/include/libcamera
+    , JuicyPixels
+    , primitive
+    , shared-memory
+    , vector
 
+  cxx-options:      -O3 -Wall -Wextra -Werror -std=c++20
   cxx-sources:      FFI/Cam.cpp
   default-language: GHC2024
   extra-libraries:
@@ -57,7 +61,7 @@ executable test-cam
   if !arch(x86_64)
     ghc-options: -optl=-mno-fix-cortex-a53-835769
 
-  include-dirs:     FFI Hsm/Cam
+  include-dirs:     FFI Hsm/Cam /usr/include/libcamera
   main-is:          Test/Cam.hs
   other-modules:
     Hsm.Cam