{"id":21516,"url":"https://patchwork.libcamera.org/api/patches/21516/?format=json","web_url":"https://patchwork.libcamera.org/patch/21516/","project":{"id":1,"url":"https://patchwork.libcamera.org/api/projects/1/?format=json","name":"libcamera","link_name":"libcamera","list_id":"libcamera_core","list_email":"libcamera-devel@lists.libcamera.org","web_url":"","scm_url":"","webscm_url":""},"msgid":"<20241004115558.9166-6-david.plowman@raspberrypi.com>","date":"2024-10-04T11:55:57","name":"[5/6] ipa: rpi: sync: Add an implementation of the camera sync algorithm","commit_ref":null,"pull_url":null,"state":"superseded","archived":false,"hash":"03d36475786a3e643ad101830f74b5ebaa1d3c6d","submitter":{"id":42,"url":"https://patchwork.libcamera.org/api/people/42/?format=json","name":"David Plowman","email":"david.plowman@raspberrypi.com"},"delegate":null,"mbox":"https://patchwork.libcamera.org/patch/21516/mbox/","series":[{"id":4658,"url":"https://patchwork.libcamera.org/api/series/4658/?format=json","web_url":"https://patchwork.libcamera.org/project/libcamera/list/?series=4658","date":"2024-10-04T11:55:52","name":"Raspberry Pi software camera sync algorithm","version":1,"mbox":"https://patchwork.libcamera.org/series/4658/mbox/"}],"comments":"https://patchwork.libcamera.org/api/patches/21516/comments/","check":"pending","checks":"https://patchwork.libcamera.org/api/patches/21516/checks/","tags":{},"headers":{"Return-Path":"","X-Original-To":"parsemail@patchwork.libcamera.org","Delivered-To":"parsemail@patchwork.libcamera.org","Received":["from lancelot.ideasonboard.com (lancelot.ideasonboard.com\n\t[92.243.16.209])\n\tby patchwork.libcamera.org (Postfix) with ESMTPS id EF1ACBD80A\n\tfor ;\n\tFri, 4 Oct 2024 11:56:21 +0000 (UTC)","from lancelot.ideasonboard.com (localhost [IPv6:::1])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTP id 272206352E;\n\tFri, 4 Oct 2024 13:56:21 +0200 (CEST)","from mail-wr1-x42e.google.com (mail-wr1-x42e.google.com\n\t[IPv6:2a00:1450:4864:20::42e])\n\tby lancelot.ideasonboard.com (Postfix) with ESMTPS id 88B8B62C92\n\tfor ;\n\tFri, 4 Oct 2024 13:56:06 +0200 (CEST)","by mail-wr1-x42e.google.com with SMTP id\n\tffacd0b85a97d-37cc84c12c2so1134770f8f.3\n\tfor ;\n\tFri, 04 Oct 2024 04:56:06 -0700 (PDT)","from raspberrypi.pitowers.org\n\t([2a00:1098:3142:1f:daa2:371b:a97:3e3e])\n\tby smtp.gmail.com with ESMTPSA id\n\tffacd0b85a97d-37d081f743esm3107147f8f.21.2024.10.04.04.56.04\n\t(version=TLS1_3 cipher=TLS_AES_256_GCM_SHA384 bits=256/256);\n\tFri, 04 Oct 2024 04:56:05 -0700 (PDT)"],"Authentication-Results":"lancelot.ideasonboard.com; dkim=pass (2048-bit key;\n\tunprotected) header.d=raspberrypi.com header.i=@raspberrypi.com\n\theader.b=\"BobiUisv\"; dkim-atps=neutral","DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=raspberrypi.com; s=google; t=1728042966; x=1728647766;\n\tdarn=lists.libcamera.org; \n\th=content-transfer-encoding:mime-version:references:in-reply-to\n\t:message-id:date:subject:cc:to:from:from:to:cc:subject:date\n\t:message-id:reply-to;\n\tbh=obcW8TllxKY4racC770ZVBJlg3ef+KFaFjujkzVUGIA=;\n\tb=BobiUisvPdaXruLLn5OdPd1qknjJMoFA+DEqITTSamFgRoygIA+YPidzeYZk1979Au\n\thr9R0kRJGgQU8cGS8LUbXd4xnnFRJhHB/GaWT1movUJ8KENQxPM+KSeB/dcyJs42Mc6N\n\t6J5UQlSWJjZnOsqZuwkyAvRW0Ulk3ta+xgEiNIyg+lz4eolHLwmByb36K4enlq01Nhku\n\t7aDAML+T2SMsuo/ouMczogSB9oWFyFVuPlxyM4j7ffUttvP7viOpvYsmty1nzdRMdlyH\n\tXmw1kO1AhqebKlJTJDBwU0ZllwiIpL/zfkd05bck8S/RFoEuLNJ2lJnMXeKY3vEK3huB\n\tRlrg==","X-Google-DKIM-Signature":"v=1; a=rsa-sha256; c=relaxed/relaxed;\n\td=1e100.net; s=20230601; t=1728042966; x=1728647766;\n\th=content-transfer-encoding:mime-version:references:in-reply-to\n\t:message-id:date:subject:cc:to:from:x-gm-message-state:from:to:cc\n\t:subject:date:message-id:reply-to;\n\tbh=obcW8TllxKY4racC770ZVBJlg3ef+KFaFjujkzVUGIA=;\n\tb=A094ZGMPlYwp8CW4bfLfScQs3c6ZvM4W3N6w76tc3ZuAIRmB4GpIR1ScTAd0QlsjLW\n\taODZua+Y7zHbCJGryk8pj/6SjeCNWK1gXc04aOtp4pm1U7VTpJEo1jRyHFSMX4R1L+X8\n\tTz6TDfCqEhAiqUdMBY7blfS1rN4RMO7cz0Jb+b1BqlPiQaKfrrmldN+H4XoA1Vu4b718\n\tmLWbWtcKjw4IOQr2iuAiHgmlt3uJrMZPp5z6Me1/ZPZ4n2ftwEfSSxVlwLguqjBccrhO\n\t9XtJL6+fKnnQZwwGmUypKGrTvppY/+uTotQKkQFYi2BcIP5vVsvj3ifXR35cXS+S8HL7\n\tOXlQ==","X-Gm-Message-State":"AOJu0YzVEyE0PKxyHa1artPw4XhFd7sNABVn2HvkT8UGvr9RtDbzLfiF\n\t+b1+kK7OGy3lt/iCJG2bi1ZkHAfPbdeeNRzGwZdIyY1qb/uGqYrY41BOOQAbwKUaaX/635mATOV\n\tW","X-Google-Smtp-Source":"AGHT+IFfVtQZ1tC5ApzCGAa1QQ6y8mfMrURmjQUNfW1wEarklQHspQn0wpXJIfvZPkAyzmTHjZ25yA==","X-Received":"by 2002:adf:ce12:0:b0:371:9121:5643 with SMTP id\n\tffacd0b85a97d-37d0e6f247cmr1557654f8f.12.1728042965359; \n\tFri, 04 Oct 2024 04:56:05 -0700 (PDT)","From":"David Plowman ","To":"libcamera-devel@lists.libcamera.org","Cc":"David Plowman ,\n\tArsen Mikovic ,\n\tNaushir Patuck ","Subject":"[PATCH 5/6] ipa: rpi: sync: Add an implementation of the camera sync\n\talgorithm","Date":"Fri, 4 Oct 2024 12:55:57 +0100","Message-Id":"<20241004115558.9166-6-david.plowman@raspberrypi.com>","X-Mailer":"git-send-email 2.39.5","In-Reply-To":"<20241004115558.9166-1-david.plowman@raspberrypi.com>","References":"<20241004115558.9166-1-david.plowman@raspberrypi.com>","MIME-Version":"1.0","Content-Transfer-Encoding":"8bit","X-BeenThere":"libcamera-devel@lists.libcamera.org","X-Mailman-Version":"2.1.29","Precedence":"list","List-Id":"","List-Unsubscribe":",\n\t","List-Archive":"","List-Post":"","List-Help":"","List-Subscribe":",\n\t","Errors-To":"libcamera-devel-bounces@lists.libcamera.org","Sender":"\"libcamera-devel\" "},"content":"In this implementation, the server sends data packets out onto the\nnetwork every 30 frames or so.\n\nClients listening for this packet will send frame length deltas back\nto the pipeline handler to match the synchronisation of the server.\n\nWe can use wallclock timestamps so that the process will actually work\nacross networked Pis, but it does really on those wallclocks being\nproperly synchronised. We de-jitter our wallclock measurements (as\nthey're made in userspace) to match the more accurate kernel\nSensorTimestamp value.\n\nWhen the server's advertised \"ready time\" is reached, both client and\nserver will signal this through metadata back to their respective\ncontrolling applications.\n\nSigned-off-by: David Plowman \nSigned-off-by: Arsen Mikovic \nSigned-off-by: Naushir Patuck \n---\n src/ipa/rpi/controller/meson.build | 2 +\n src/ipa/rpi/controller/rpi/clock_recovery.cpp | 87 ++++\n src/ipa/rpi/controller/rpi/clock_recovery.h | 55 +++\n src/ipa/rpi/controller/rpi/sync.cpp | 384 ++++++++++++++++++\n src/ipa/rpi/controller/rpi/sync.h | 71 ++++\n 5 files changed, 599 insertions(+)\n create mode 100644 src/ipa/rpi/controller/rpi/clock_recovery.cpp\n create mode 100644 src/ipa/rpi/controller/rpi/clock_recovery.h\n create mode 100644 src/ipa/rpi/controller/rpi/sync.cpp\n create mode 100644 src/ipa/rpi/controller/rpi/sync.h","diff":"diff --git a/src/ipa/rpi/controller/meson.build b/src/ipa/rpi/controller/meson.build\nindex 74b74888..8df38a0c 100644\n--- a/src/ipa/rpi/controller/meson.build\n+++ b/src/ipa/rpi/controller/meson.build\n@@ -13,6 +13,7 @@ rpi_ipa_controller_sources = files([\n 'rpi/black_level.cpp',\n 'rpi/cac.cpp',\n 'rpi/ccm.cpp',\n+ 'rpi/clock_recovery.cpp',\n 'rpi/contrast.cpp',\n 'rpi/denoise.cpp',\n 'rpi/dpc.cpp',\n@@ -23,6 +24,7 @@ rpi_ipa_controller_sources = files([\n 'rpi/saturation.cpp',\n 'rpi/sdn.cpp',\n 'rpi/sharpen.cpp',\n+ 'rpi/sync.cpp',\n 'rpi/tonemap.cpp',\n ])\n \ndiff --git a/src/ipa/rpi/controller/rpi/clock_recovery.cpp b/src/ipa/rpi/controller/rpi/clock_recovery.cpp\nnew file mode 100644\nindex 00000000..1ccbf9e9\n--- /dev/null\n+++ b/src/ipa/rpi/controller/rpi/clock_recovery.cpp\n@@ -0,0 +1,87 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2024, Raspberry Pi Ltd\n+ *\n+ * Camera sync control algorithm\n+ */\n+#include \"clock_recovery.h\"\n+\n+#include \n+\n+using namespace RPiController;\n+using namespace libcamera;\n+\n+LOG_DEFINE_CATEGORY(RPiClockRec)\n+\n+ClockRecovery::ClockRecovery()\n+{\n+\tinitialise();\n+}\n+\n+void ClockRecovery::initialise(unsigned int numPts, unsigned int maxJitter, unsigned int minPts)\n+{\n+\tnumPts_ = numPts;\n+\tmaxJitter_ = maxJitter;\n+\tminPts_ = minPts;\n+\treset();\n+}\n+\n+void ClockRecovery::reset()\n+{\n+\txAve_ = 0;\n+\tyAve_ = 0;\n+\tx2Ave_ = 0;\n+\txyAve_ = 0;\n+\tcount_ = 0;\n+\tslope_ = 0.0;\n+\toffset_ = 0.0;\n+}\n+\n+void ClockRecovery::addSample(uint64_t input, uint64_t output)\n+{\n+\tif (count_ == 0) {\n+\t\tinputBase_ = input;\n+\t\toutputBase_ = output;\n+\t}\n+\n+\t/*\n+\t * Never let the new output value be more than maxJitter_ away from what we would have expected.\n+\t * This is just to filter out any rare but really crazy values.\n+\t */\n+\tuint64_t expectedOutput = getOutput(input);\n+\toutput = std::clamp(output, expectedOutput - maxJitter_, expectedOutput + maxJitter_);\n+\tdouble x = input - inputBase_;\n+\tdouble y = output - outputBase_ - x;\n+\n+\t/*\n+\t * We use x, y, x^2 and x*y sums to calculate the best fit line. Here we update them by\n+\t * pretending we have count_ samples at the previous fit, and now one new one. Gradually\n+\t * the effect of the older values gets lost. This is a very simple way of updating the\n+\t * fit (there are much more complicated ones!), but it works well enough. Using averages\n+\t * instead of sums makes the relative effect of old values and the new sample clearer.\n+\t */\n+\tunsigned int count1 = count_ + 1;\n+\txAve_ = (count_ * xAve_ + x) / count1;\n+\tyAve_ = (count_ * yAve_ + y) / count1;\n+\tx2Ave_ = (count_ * x2Ave_ + x * x) / count1;\n+\txyAve_ = (count_ * xyAve_ + x * y) / count1;\n+\n+\t/* Don't update slope and offset until we've seen \"enough\" sample points. */\n+\tif (count_ > minPts_) {\n+\t\t/* These are the standard equations for least squares linear regressions. */\n+\t\tslope_ = (count1 * count1 * xyAve_ - count1 * xAve_ * count1 * yAve_) /\n+\t\t\t (count1 * count1 * x2Ave_ - count1 * xAve_ * count1 * xAve_);\n+\t\toffset_ = yAve_ - slope_ * xAve_;\n+\t}\n+\n+\t/* Don't increase count_ above numPts_, as this controls the long-term amount of the residual fit. */\n+\tif (count1 < numPts_)\n+\t\tcount_++;\n+}\n+\n+uint64_t ClockRecovery::getOutput(uint64_t input)\n+{\n+\tdouble x = input - inputBase_;\n+\tdouble y = slope_ * x + offset_;\n+\treturn y + x + outputBase_;\n+}\ndiff --git a/src/ipa/rpi/controller/rpi/clock_recovery.h b/src/ipa/rpi/controller/rpi/clock_recovery.h\nnew file mode 100644\nindex 00000000..dd05dd97\n--- /dev/null\n+++ b/src/ipa/rpi/controller/rpi/clock_recovery.h\n@@ -0,0 +1,55 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2024, Raspberry Pi Ltd\n+ *\n+ * Camera sync control algorithm\n+ */\n+#pragma once\n+\n+#include \n+\n+namespace RPiController {\n+\n+class ClockRecovery\n+{\n+public:\n+\tClockRecovery();\n+\n+\t/* Initialise with configuration parameters and restart the fitting process. */\n+\tvoid initialise(unsigned int numPts = 100, unsigned int maxJitter = 100000, unsigned int minPts = 10);\n+\t/* Erase all history and restart the fitting process. */\n+\tvoid reset();\n+\n+\t// Add a new input clock / output clock sample. */\n+\tvoid addSample(uint64_t input, uint64_t output);\n+\t/* Calculate the output clock value for this input. */\n+\tuint64_t getOutput(uint64_t input);\n+\n+private:\n+\tunsigned int numPts_; /* how many samples contribute to the history */\n+\tunsigned int maxJitter_; /* smooth out any jitter larger than this immediately */\n+\tunsigned int minPts_; /* number of samples below which we treat clocks as 1:1 */\n+\tunsigned int count_; /* how many samples seen (up to numPts_) */\n+\tuint64_t inputBase_; /* subtract this from all input values, just to make the numbers easier */\n+\tuint64_t outputBase_; /* as above, for the output */\n+\n+\t/*\n+\t * We do a linear regression of y against x, where:\n+\t * x is the value input - inputBase_, and\n+\t * y is the value output - outputBase_ - x.\n+\t * We additionally subtract x from y so that y \"should\" be zero, again making the numnbers easier.\n+\t */\n+\tdouble xAve_; /* average x value seen so far */\n+\tdouble yAve_; /* average y value seen so far */\n+\tdouble x2Ave_; /* average x^2 value seen so far */\n+\tdouble xyAve_; /* average x*y value seen so far */\n+\n+\t/*\n+\t * Once we've seen more than minPts_ samples, we recalculate the slope and offset according\n+\t * to the linear regression normal equations.\n+\t */\n+\tdouble slope_; /* latest slope value */\n+\tdouble offset_; /* latest offset value */\n+};\n+\n+} //namespace RPiController\ndiff --git a/src/ipa/rpi/controller/rpi/sync.cpp b/src/ipa/rpi/controller/rpi/sync.cpp\nnew file mode 100644\nindex 00000000..9e76d879\n--- /dev/null\n+++ b/src/ipa/rpi/controller/rpi/sync.cpp\n@@ -0,0 +1,384 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2024, Raspberry Pi Ltd\n+ *\n+ * sync.cpp - sync algorithm\n+ */\n+#include \"sync.h\"\n+\n+#include \n+#include \n+#include \n+#include \n+#include \n+\n+#include \n+\n+#include \n+\n+#include \"sync_status.h\"\n+\n+using namespace std;\n+using namespace std::chrono_literals;\n+using namespace RPiController;\n+using namespace libcamera;\n+\n+LOG_DEFINE_CATEGORY(RPiSync)\n+\n+#define NAME \"rpi.sync\"\n+\n+const char *kDefaultGroup = \"239.255.255.250\";\n+constexpr unsigned int kDefaultPort = 10000;\n+constexpr unsigned int kDefaultSyncPeriod = 30;\n+constexpr unsigned int kDefaultReadyFrame = 1000;\n+constexpr unsigned int kDefaultMinAdjustment = 50;\n+constexpr unsigned int kDefaultFitNumPts = 100;\n+constexpr unsigned int kDefaultFitMaxJitter = 100000;\n+constexpr unsigned int kDefaultFitMinPts = 10;\n+\n+/* Returns IP address of the device we are on. */\n+static std::string local_address_IP()\n+{\n+\tconst char *google_dns_server = \"8.8.8.8\";\n+\tint dns_port = 53;\n+\n+\tstruct sockaddr_in serv;\n+\tint sock = socket(AF_INET, SOCK_DGRAM, 0);\n+\n+\tif (sock < 0)\n+\t\tLOG(RPiSync, Error) << \"Socket error\";\n+\n+\tmemset(&serv, 0, sizeof(serv));\n+\tserv.sin_family = AF_INET;\n+\tserv.sin_addr.s_addr = inet_addr(google_dns_server);\n+\tserv.sin_port = htons(dns_port);\n+\n+\tint err = connect(sock, (const struct sockaddr *)&serv, sizeof(serv));\n+\tif (err < 0)\n+\t\tLOG(RPiSync, Error) << \"Socket connect error\";\n+\n+\tstruct sockaddr_in name;\n+\tsocklen_t namelen = sizeof(name);\n+\terr = getsockname(sock, (struct sockaddr *)&name, &namelen);\n+\n+\tchar buffer[80];\n+\t(void)inet_ntop(AF_INET, &name.sin_addr, buffer, 80);\n+\tclose(sock);\n+\treturn buffer;\n+}\n+\n+Sync::Sync(Controller *controller)\n+\t: SyncAlgorithm(controller), mode_(Mode::Off), socket_(-1), frameDuration_(0s), frameCount_(0)\n+{\n+}\n+\n+Sync::~Sync()\n+{\n+\tif (socket_ >= 0)\n+\t\tclose(socket_);\n+}\n+\n+char const *Sync::name() const\n+{\n+\treturn NAME;\n+}\n+\n+/* This reads from json file and intitiaises server and client */\n+int Sync::read(const libcamera::YamlObject ¶ms)\n+{\n+\t/* Socket on which to communicate. */\n+\tgroup_ = params[\"group\"].get(kDefaultGroup);\n+\tport_ = params[\"port\"].get(kDefaultPort);\n+\t/* Send a sync message every this many frames. */\n+\tsyncPeriod_ = params[\"sync_period\"].get(kDefaultSyncPeriod);\n+\t/* Application will be told we're ready after this many frames. */\n+\treadyFrame_ = params[\"ready_frame\"].get(kDefaultReadyFrame);\n+\t/* Don't change client frame length unless the change exceeds this amount (microseconds). */\n+\tminAdjustment_ = params[\"min_adjustment\"].get(kDefaultMinAdjustment);\n+\n+\t/* Parameters controlling the clock fitting. */\n+\tuint32_t fitNumPts = params[\"fit_num_pts\"].get(kDefaultFitNumPts);\n+\tuint32_t fitMaxJitter = params[\"fit_max_jitter\"].get(kDefaultFitMaxJitter);\n+\tuint32_t fitMinPts = params[\"fit_min_pts\"].get(kDefaultFitMinPts);\n+\tsystemToWallClock_.initialise(fitNumPts, fitMaxJitter, fitMinPts);\n+\n+\treturn 0;\n+}\n+\n+void Sync::initialiseSocket()\n+{\n+\tsocket_ = socket(AF_INET, SOCK_DGRAM, 0);\n+\tif (socket_ < 0) {\n+\t\tLOG(RPiSync, Error) << \"Unable to create socket\";\n+\t\treturn;\n+\t}\n+\n+\tmemset(&addr_, 0, sizeof(addr_));\n+\taddr_.sin_family = AF_INET;\n+\taddr_.sin_addr.s_addr = mode_ == Mode::Client ? htonl(INADDR_ANY) : inet_addr(group_.c_str());\n+\taddr_.sin_port = htons(port_);\n+\n+\tif (mode_ == Mode::Client) {\n+\t\t/* Set to non-blocking. */\n+\t\tint flags = fcntl(socket_, F_GETFL, 0);\n+\t\tfcntl(socket_, F_SETFL, flags | O_NONBLOCK);\n+\n+\t\tunsigned int en = 1;\n+\t\tif (setsockopt(socket_, SOL_SOCKET, SO_REUSEADDR, &en, sizeof(en)) < 0) {\n+\t\t\tLOG(RPiSync, Error) << \"Unable to set socket options\";\n+\t\t\tgoto err;\n+\t\t}\n+\n+\t\tstruct ip_mreq mreq {\n+\t\t};\n+\t\tmreq.imr_multiaddr.s_addr = inet_addr(group_.c_str());\n+\t\tmreq.imr_interface.s_addr = htonl(INADDR_ANY);\n+\t\tif (setsockopt(socket_, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq)) < 0) {\n+\t\t\tLOG(RPiSync, Error) << \"Unable to set socket options\";\n+\t\t\tgoto err;\n+\t\t}\n+\n+\t\tif (bind(socket_, (struct sockaddr *)&addr_, sizeof(addr_)) < 0) {\n+\t\t\tLOG(RPiSync, Error) << \"Unable to bind client socket\";\n+\t\t\tgoto err;\n+\t\t}\n+\t}\n+\n+\treturn;\n+\n+err:\n+\tclose(socket_);\n+\tsocket_ = -1;\n+}\n+\n+void Sync::switchMode([[maybe_unused]] CameraMode const &cameraMode, [[maybe_unused]] Metadata *metadata)\n+{\n+\tsyncReady_ = false;\n+\tframeCount_ = 0;\n+\tfirstFrame_ = true;\n+\tlag_ = 0;\n+\tserverFrameCountPeriod_ = 0;\n+\tclientServerReadyTime_ = 0;\n+\tclientSeenPacket_ = false;\n+}\n+\n+/*\n+ * Camera sync algorithm.\n+ * Server - there is a single server that sends framerate timing information over the network to any\n+ * clients that are listening. It also signals when it will send a \"everything is synchronised, now go\"\n+ * message back to the algorithm.\n+ * Client - there may be many clients, either on the same Pi or different ones. They match their\n+ * framerates to the server, and indicate when to \"go\" at the same instant as the server.\n+ */\n+void Sync::process([[maybe_unused]] StatisticsPtr &stats, Metadata *imageMetadata)\n+{\n+\tSyncPayload payload;\n+\tSyncParams local{};\n+\tSyncStatus status{};\n+\tbool lagKnown = true;\n+\n+\timageMetadata->get(\"sync.params\", local);\n+\n+\tif (!frameDuration_) {\n+\t\tLOG(RPiSync, Error) << \"Sync frame duration not set!\";\n+\t\treturn;\n+\t}\n+\n+\tif (mode_ == Mode::Off)\n+\t\treturn;\n+\n+\tif (socket_ < 0)\n+\t\tinitialiseSocket();\n+\n+\t/* The local wallclock for the very first frame can be a bit off, so ignore it. */\n+\tif (firstFrame_) {\n+\t\tfirstFrame_ = false;\n+\n+\t\t/*\n+\t\t * For the client, flush anything in the socket. It might be stale from a previous sync run,\n+\t\t * or we might get another packet in a frame to two before the adjustment caused by this (old)\n+\t\t * packet, although correct, had taken effect. So this keeps things simpler.\n+\t\t */\n+\t\tif (mode_ == Mode::Client) {\n+\t\t\tsocklen_t addrlen = sizeof(addr_);\n+\t\t\tint ret = 0;\n+\t\t\twhile (ret >= 0)\n+\t\t\t\tret = recvfrom(socket_, &payload, sizeof(payload), 0, (struct sockaddr *)&addr_, &addrlen);\n+\t\t}\n+\n+\t\treturn;\n+\t}\n+\n+\t/*\n+\t * It might be possible for a frame not to have a valid wallclock, in which case don't let it\n+\t * get into the clock recovery as it would totally throw it off.\n+\t */\n+\tif (local.wallClock == 0) {\n+\t\tLOG(RPiSync, Debug) << \"Zero-valued wallclock - ignoring\";\n+\t\treturn;\n+\t}\n+\n+\t/* Derive a de-jittered version of wall clock. sensorTimestamp needs converting from ns to us. */\n+\tuint64_t systemFrameTimestamp = local.sensorTimestamp / 1000;\n+\tsystemToWallClock_.addSample(systemFrameTimestamp, local.wallClock);\n+\tuint64_t wallClockFrameTimestamp = systemToWallClock_.getOutput(systemFrameTimestamp);\n+\n+\t/*\n+\t * This is the headline frame duration in microseconds as programmed into the sensor. Strictly,\n+\t * the sensor might not quite match the system clock, but this shouldn't matter for the calculations\n+\t * we'll do with it, unless it's a very very long way out!\n+\t */\n+\tuint32_t frameDuration = frameDuration_.get();\n+\n+\t/* Timestamps tell us if we've dropped any frames, but we still want to count them. */\n+\tint droppedFrames = 0;\n+\tif (frameCount_) {\n+\t\t/*\n+\t\t * Round down here, because frameCount_ gets incremented at the end of the function. Also\n+\t\t * ensure droppedFrames can't go negative. It shouldn't, but things would go badly wrong\n+\t\t * if it did.\n+\t\t */\n+\t\twallClockFrameTimestamp = std::max(wallClockFrameTimestamp, lastWallClockFrameTimestamp_ + frameDuration / 2);\n+\t\tdroppedFrames = (wallClockFrameTimestamp - lastWallClockFrameTimestamp_ - frameDuration / 2) / frameDuration;\n+\t\tframeCount_ += droppedFrames;\n+\t}\n+\n+\tif (mode_ == Mode::Server) {\n+\t\t/*\n+\t\t * Server sends a packet every syncPeriod_ frames, or as soon after as possible (if any\n+\t\t * frames were dropped).\n+\t\t */\n+\t\tserverFrameCountPeriod_ += droppedFrames;\n+\n+\t\t/*\n+\t\t * The client may want a better idea of the true frame duration. Any error would feed straight\n+\t\t * into the correction term because of how it uses it to get the \"nearest\" frame.\n+\t\t */\n+\t\tif (frameCount_ == 0)\n+\t\t\tframeDurationEstimated_ = frameDuration;\n+\t\telse {\n+\t\t\tdouble diff = (systemFrameTimestamp - lastSystemFrameTimestamp_) / (1 + droppedFrames);\n+\t\t\tint N = std::min(frameCount_, 99U);\n+\t\t\tframeDurationEstimated_ = frameCount_ == 1 ? diff : (N * frameDurationEstimated_ + diff) / (N + 1);\n+\t\t}\n+\n+\t\t/* Calculate frames remaining, and therefore \"time left until ready\". */\n+\t\tint framesRemaining = readyFrame_ - frameCount_;\n+\t\tuint64_t systemReadyTime = systemFrameTimestamp + (int64_t)framesRemaining * frameDurationEstimated_;\n+\t\tuint64_t wallClockReadyTime = systemToWallClock_.getOutput(systemReadyTime);\n+\n+\t\tif (serverFrameCountPeriod_ >= syncPeriod_) {\n+\t\t\tserverFrameCountPeriod_ = 0;\n+\n+\t\t\tpayload.frameDuration = frameDurationEstimated_ + .5; /* round to nearest */\n+\t\t\tpayload.systemFrameTimestamp = systemFrameTimestamp;\n+\t\t\tpayload.wallClockFrameTimestamp = wallClockFrameTimestamp;\n+\t\t\tpayload.systemReadyTime = systemReadyTime;\n+\t\t\tpayload.wallClockReadyTime = wallClockReadyTime;\n+\n+\t\t\tLOG(RPiSync, Debug) << \"Send packet (frameNumber \" << frameCount_ << \"):\";\n+\t\t\tLOG(RPiSync, Debug) << \" frameDuration \" << payload.frameDuration;\n+\t\t\tLOG(RPiSync, Debug) << \" systemFrameTimestamp \" << systemFrameTimestamp\n+\t\t\t\t\t << \" (\" << systemFrameTimestamp - lastSystemFrameTimestamp_ << \")\";\n+\t\t\tLOG(RPiSync, Debug) << \" wallClockFrameTimestamp \" << wallClockFrameTimestamp\n+\t\t\t\t\t << \" (\" << wallClockFrameTimestamp - lastWallClockFrameTimestamp_ << \")\";\n+\t\t\tLOG(RPiSync, Debug) << \" systemReadyTime \" << systemReadyTime;\n+\t\t\tLOG(RPiSync, Debug) << \" wallClockReadyTime \" << wallClockReadyTime;\n+\n+\t\t\tif (sendto(socket_, &payload, sizeof(payload), 0, (const sockaddr *)&addr_, sizeof(addr_)) < 0)\n+\t\t\t\tLOG(RPiSync, Error) << \"Send error! \" << strerror(errno);\n+\t\t}\n+\n+\t\tlag_ = (int64_t)wallClockFrameTimestamp - (int64_t)wallClockReadyTime;\n+\t\tif (!syncReady_ && wallClockFrameTimestamp + frameDurationEstimated_ / 2 > wallClockReadyTime) {\n+\t\t\tsyncReady_ = true;\n+\t\t\tLOG(RPiSync, Info) << \"*** Sync achieved! Lag \" << lag_;\n+\t\t}\n+\n+\t\tserverFrameCountPeriod_ += 1;\n+\n+\t} else if (mode_ == Mode::Client) {\n+\t\tuint64_t serverFrameTimestamp = 0;\n+\n+\t\tbool packetReceived = false;\n+\t\twhile (true) {\n+\t\t\tsocklen_t addrlen = sizeof(addr_);\n+\t\t\tint ret = recvfrom(socket_, &payload, sizeof(payload), 0, (struct sockaddr *)&addr_, &addrlen);\n+\n+\t\t\tif (ret < 0)\n+\t\t\t\tbreak;\n+\t\t\tpacketReceived = (ret > 0);\n+\t\t\tclientSeenPacket_ = true;\n+\n+\t\t\tif (!IPCheck_) {\n+\t\t\t\tIPCheck_ = true;\n+\t\t\t\tchar srcIP[INET_ADDRSTRLEN];\n+\t\t\t\tinet_ntop(AF_INET, &(addr_.sin_addr), srcIP, INET_ADDRSTRLEN);\n+\t\t\t\tclientSamePi_ = (local_address_IP() == srcIP);\n+\t\t\t\tLOG(RPiSync, Debug) << \"Server is \" << (clientSamePi_ ? \"same\" : \"different\");\n+\t\t\t}\n+\n+\t\t\tframeDurationEstimated_ = payload.frameDuration;\n+\t\t\tif (clientSamePi_) {\n+\t\t\t\tserverFrameTimestamp = payload.systemFrameTimestamp;\n+\t\t\t\tclientServerReadyTime_ = payload.systemReadyTime;\n+\t\t\t} else {\n+\t\t\t\tserverFrameTimestamp = payload.wallClockFrameTimestamp;\n+\t\t\t\tclientServerReadyTime_ = payload.wallClockReadyTime;\n+\t\t\t}\n+\t\t}\n+\n+\t\tif (packetReceived) {\n+\t\t\tuint64_t clientFrameTimestamp = clientSamePi_ ? systemFrameTimestamp : wallClockFrameTimestamp;\n+\t\t\tint64_t clientServerDelta = clientFrameTimestamp - serverFrameTimestamp;\n+\t\t\t/* \"A few frames ago\" may have better matched the server's frame. Calculate when it was. */\n+\t\t\tint framePeriodErrors = (clientServerDelta + frameDurationEstimated_ / 2) / frameDurationEstimated_;\n+\t\t\tint64_t clientFrameTimestampNearest = clientFrameTimestamp - framePeriodErrors * frameDurationEstimated_;\n+\t\t\t/* We must shorten a single client frame by this amount if it exceeds the minimum: */\n+\t\t\tint32_t correction = clientFrameTimestampNearest - serverFrameTimestamp;\n+\t\t\tif (std::abs(correction) < minAdjustment_)\n+\t\t\t\tcorrection = 0;\n+\n+\t\t\tLOG(RPiSync, Debug) << \"Received packet (frameNumber \" << frameCount_ << \"):\";\n+\t\t\tLOG(RPiSync, Debug) << \" serverFrameTimestamp \" << serverFrameTimestamp;\n+\t\t\tLOG(RPiSync, Debug) << \" serverReadyTime \" << clientServerReadyTime_;\n+\t\t\tLOG(RPiSync, Debug) << \" clientFrameTimestamp \" << clientFrameTimestamp;\n+\t\t\tLOG(RPiSync, Debug) << \" clientFrameTimestampNearest \" << clientFrameTimestampNearest\n+\t\t\t\t\t << \" (\" << framePeriodErrors << \")\";\n+\t\t\tLOG(RPiSync, Debug) << \" systemFrameTimestamp \" << systemFrameTimestamp\n+\t\t\t\t\t << \" (\" << systemFrameTimestamp - lastSystemFrameTimestamp_ << \")\";\n+\t\t\tLOG(RPiSync, Debug) << \" correction \" << correction;\n+\n+\t\t\tstatus.frameDurationOffset = correction * 1us;\n+\t\t}\n+\n+\t\tuint64_t clientFrameTimestamp = clientSamePi_ ? systemFrameTimestamp : wallClockFrameTimestamp;\n+\t\tlag_ = (int64_t)clientFrameTimestamp - (int64_t)clientServerReadyTime_;\n+\t\tlagKnown = clientSeenPacket_; /* client must receive a packet before the lag is correct */\n+\t\tif (clientSeenPacket_ && !syncReady_ && clientFrameTimestamp + frameDurationEstimated_ / 2 > clientServerReadyTime_) {\n+\t\t\tsyncReady_ = true;\n+\t\t\tLOG(RPiSync, Info) << \"*** Sync achieved! Lag \" << lag_;\n+\t\t}\n+\t}\n+\n+\tlastSystemFrameTimestamp_ = systemFrameTimestamp;\n+\tlastWallClockFrameTimestamp_ = wallClockFrameTimestamp;\n+\n+\tstatus.ready = syncReady_;\n+\tstatus.lag = lag_;\n+\tstatus.lagKnown = lagKnown;\n+\timageMetadata->set(\"sync.status\", status);\n+\tframeCount_++;\n+}\n+\n+void Sync::setFrameDuration(libcamera::utils::Duration frameDuration)\n+{\n+\tframeDuration_ = frameDuration;\n+};\n+\n+/* Register algorithm with the system. */\n+static Algorithm *create(Controller *controller)\n+{\n+\treturn (Algorithm *)new Sync(controller);\n+}\n+static RegisterAlgorithm reg(NAME, &create);\ndiff --git a/src/ipa/rpi/controller/rpi/sync.h b/src/ipa/rpi/controller/rpi/sync.h\nnew file mode 100644\nindex 00000000..15427adb\n--- /dev/null\n+++ b/src/ipa/rpi/controller/rpi/sync.h\n@@ -0,0 +1,71 @@\n+/* SPDX-License-Identifier: BSD-2-Clause */\n+/*\n+ * Copyright (C) 2024, Raspberry Pi Ltd\n+ *\n+ * sync.h - sync algorithm\n+ */\n+#pragma once\n+\n+#include \n+\n+#include \"../sync_algorithm.h\"\n+#include \"clock_recovery.h\"\n+\n+namespace RPiController {\n+\n+struct SyncPayload {\n+\t/* Frame duration in microseconds. */\n+\tuint32_t frameDuration;\n+\t/* Server system (kernel) frame timestamp. */\n+\tuint64_t systemFrameTimestamp;\n+\t/* Server wall clock version of the frame timestamp. */\n+\tuint64_t wallClockFrameTimestamp;\n+\t/* Server system (kernel) sync time (the time at which frames are marked ready). */\n+\tuint64_t systemReadyTime;\n+\t/* Server wall clock version of the sync time. */\n+\tuint64_t wallClockReadyTime;\n+};\n+\n+class Sync : public SyncAlgorithm\n+{\n+public:\n+\tSync(Controller *controller);\n+\t~Sync();\n+\tchar const *name() const override;\n+\tint read(const libcamera::YamlObject ¶ms) override;\n+\tvoid setMode(Mode mode) override { mode_ = mode; }\n+\tvoid initialiseSocket();\n+\tvoid switchMode(CameraMode const &cameraMode, Metadata *metadata) override;\n+\tvoid process(StatisticsPtr &stats, Metadata *imageMetadata) override;\n+\tvoid setFrameDuration(libcamera::utils::Duration frameDuration) override;\n+\n+private:\n+\tMode mode_; /* server or client */\n+\tstd::string group_; /* IP group address for sync messages */\n+\tuint16_t port_; /* port number for messages */\n+\tuint32_t syncPeriod_; /* send a sync message every this many frames */\n+\tuint32_t readyFrame_; /* tell the application we're ready after this many frames */\n+\tuint32_t minAdjustment_; /* don't adjust the client frame length by less than this */\n+\n+\tstruct sockaddr_in addr_;\n+\tint socket_ = -1;\n+\tlibcamera::utils::Duration frameDuration_;\n+\tunsigned int frameCount_;\n+\tbool syncReady_;\n+\tint64_t lag_ = 0;\n+\tbool IPCheck_ = false;\n+\tbool firstFrame_ = true;\n+\n+\tdouble frameDurationEstimated_ = 0; /* estimate the true frame duration of the sensor */\n+\tClockRecovery systemToWallClock_; /* for deriving a de-jittered wall clock time */\n+\tuint64_t lastSystemFrameTimestamp_; /* system timestamp of previous frame */\n+\tuint64_t lastWallClockFrameTimestamp_; /* wall clock timestamp of previous frame */\n+\n+\tuint32_t serverFrameCountPeriod_ = 0; /* send the next packet when this reaches syncPeriod_ */\n+\n+\tbool clientSeenPacket_ = false; /* whether the client has received a packet yet */\n+\tbool clientSamePi_ = false; /* whether server running on the same Pi as client */\n+\tuint64_t clientServerReadyTime_ = 0; /* the client's latest value for when the server will be \"ready\" */\n+};\n+\n+} /* namespace RPiController */\n","prefixes":["5/6"]}