[BROKEN] playback works, taking a pic doesn't and it's 15fps

Makefile

@@ -58,7 +58,7 @@ CXXFLAGS	:= $(CFLAGS) -fno-rtti -fno-exceptions -std=gnu++14
 ASFLAGS	:=	-g $(ARCH)
 LDFLAGS	=	-specs=3dsx.specs -g $(ARCH) -Wl,-Map,$(notdir $*.map)
 
-LIBS	:= -lbox2d -lcitro2d -lcitro3d -ljpeg -lctru -lm
+LIBS	:= -lbox2d -lcitro2d -lcitro3d  -lctru -lm -ljpeg -lcurl
 
 #---------------------------------------------------------------------------------
 # list of directories containing libraries, this must be the top level containing

flake.lock

@@ -55,22 +55,6 @@
         "type": "github"
       }
     },
-    "makerom": {
-      "flake": false,
-      "locked": {
-        "lastModified": 1756011173,
-        "narHash": "sha256-nPRUwJwHA6cwGb0nocdCde7vfgxoRJF9T90mgEOMYu4=",
-        "owner": "3DSGuy",
-        "repo": "Project_CTR",
-        "rev": "f55e0fbc00f12ffb77f7af869f93472bed320ac6",
-        "type": "github"
-      },
-      "original": {
-        "owner": "3DSGuy",
-        "repo": "Project_CTR",
-        "type": "github"
-      }
-    },
     "nixpkgs": {
       "locked": {
         "lastModified": 1751949589,
@@ -107,7 +91,6 @@
       "inputs": {
         "devkitNix": "devkitNix",
         "flake-utils": "flake-utils_2",
-        "makerom": "makerom",
         "nixpkgs": "nixpkgs_2"
       }
     },

flake.nix

@@ -3,10 +3,6 @@
     nixpkgs.url = "github:NixOS/nixpkgs/nixpkgs-unstable";
     flake-utils.url = "github:numtide/flake-utils";
     devkitNix.url = "github:bandithedoge/devkitNix";
-    makerom = {
-      flake = false;
-      url = "github:/3DSGuy/Project_CTR";
-    };
   };
 
   outputs = {
@@ -14,7 +10,6 @@
     nixpkgs,
     flake-utils,
     devkitNix,
-    makerom,
     ...
   }:
     flake-utils.lib.eachDefaultSystem (
@@ -28,11 +23,10 @@
           pkgs.mkShell.override {
             stdenv = pkgs.devkitNix.stdenvARM;
           } {
-            packages = [self.packages.${pkgs.system}.makerom];
+            packages = with pkgs; [imagemagick];
           };
-        packages = {
+        packages.default = pkgs.devkitNix.stdenvARM.mkDerivation {
-          default = pkgs.devkitNix.stdenvARM.mkDerivation {
+          name = "somding";
-            name = "print3d";
           src = ./.;
           buildInputs = [pkgs.imagemagick pkgs.which];
 
@@ -42,34 +36,6 @@
             cp 3ds.3dsx $out
           '';
         };
-          makerom =
-            pkgs.stdenv.mkDerivation
-            {
-              name = "makerom";
-              src = "${makerom}/makerom";
-
-              buildInputs = let
-                pkgDep = name:
-                  pkgs.stdenv.mkDerivation {
-                    inherit name;
-                    src = "${makerom}/makerom/deps/${name}";
-
-                    installPhase = ''
-                      mkdir -p $out/lib
-                      cp bin/${name}.a $out/lib
-                    '';
-                  };
-              in [
-                (pkgDep "libblz")
-                (pkgDep "libmbedtls")
-                (pkgDep "libyaml")
-              ];
-              installPhase = ''
-                mkdir -p $out/bin
-                cp bin/makerom $out/bin
-              '';
-            };
-        };
       }
     );
 }

src/main.c

@@ -1,386 +1,247 @@
-#include <stdio.h>
-
 #include <3ds.h>
-#include <errno.h>
-#include <fcntl.h>
-#include <inttypes.h>
-#include <jpeglib.h>
 #include <malloc.h>
+#include <setjmp.h>
+#include <stdbool.h>
+#include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
-#include <sys/stat.h>
+#include <sys/dirent.h>
-#include <time.h>
+#include <sys/errno.h>
-#include <unistd.h>
+#include <sys/unistd.h>
 
-#define W 400
+#define WAIT_TIMEOUT 1000000000ULL
-#define H 240
-#define FB_BYTES (W * H * 3)
-#define DIR "sdmc:/printcam"
-#define EP_PATH DIR "/endpoint.txt"
-#define EP_DEFAULT "https://example.com/upload"
-#define JPG_QUALITY 100
-#define CAM_TIMEOUT_NS (5ULL * 1000ULL * 1000ULL * 1000ULL)
 
-static volatile bool quit = false;
+#define WIDTH 400
-static Thread camThread = NULL;
+#define HEIGHT 240
-static Handle readyEv[2] = {0, 0};
-static u8 *camBuf[2] = {NULL, NULL};
-static u8 *dispBuf = NULL;
-static volatile u8 writeIdx = 0;
-static volatile u8 dispIdx = 0;
-static volatile bool reinitCam = false;
-static volatile int camIndex = 0;
-static char endpoint[512] = EP_DEFAULT;
-static PrintConsole bottom;
 
-static int init_endpoint_config(void) {
+#define PIC_WIDTH 640
-  if (mkdir(DIR, 0777) != 0 && errno != EEXIST)
+#define PIC_HEIGHT 480
-    return -1;
+
-  FILE *f = fopen(EP_PATH, "r");
+#define SCREEN_SIZE WIDTH *HEIGHT * 2
-  if (f != NULL) {
+#define PIC_SIZE PIC_WIDTH *PIC_HEIGHT * 2
-    if (fgets(endpoint, sizeof(endpoint), f) == NULL) {
+#define BUF_SIZE SCREEN_SIZE * 2
-      fclose(f);
+
-      return -1;
+#define CAMS SELECT_IN1_OUT1
-    }
+#define PORT PORT_CAM1
-    fclose(f);
+
-    size_t n = strlen(endpoint);
+static jmp_buf exitJmp;
-    while (n && (endpoint[n - 1] == '\n' || endpoint[n - 1] == '\r')) {
+bool activeCam = false;
-      endpoint[--n] = '\0';
+
-    }
+Result getActiveSelect() { return activeCam ? SELECT_IN1 : SELECT_OUT1; };
-    if (n == 0)
+
-      return -1;
+inline void clearScreen(void) {
-    return 1;
+  u8 *frame = gfxGetFramebuffer(GFX_BOTTOM, GFX_LEFT, NULL, NULL);
-  }
+  memset(frame, 0, 320 * 240 * 3);
-  if (errno != ENOENT)
-    return -1;
-  FILE *w = fopen(EP_PATH, "w");
-  if (!w)
-    return -1;
-  if (fprintf(w, "%s\n", EP_DEFAULT) < 0) {
-    fclose(w);
-    unlink(EP_PATH);
-    return -1;
-  }
-  fflush(w);
-  fsync(fileno(w));
-  fclose(w);
-  strncpy(endpoint, EP_DEFAULT, sizeof(endpoint) - 1);
-  endpoint[sizeof(endpoint) - 1] = '\0';
-  return 0;
 }
 
-static inline void put_pixel(u8 *d, int x, int y, u8 r, u8 g, u8 b) {
+void hang(char *message) {
-  int off = (x * H + (H - 1 - y)) * 3;
+  clearScreen();
-  d[off + 0] = b;
+  printf("%s", message);
-  d[off + 1] = g;
+  printf("Press start to exit");
-  d[off + 2] = r;
-}
 
-static void yuyv_to_display(const u8 *s, u8 *d) {
+  while (aptMainLoop()) {
-  for (int y = 0; y < H; ++y) {
+    hidScanInput();
-    for (int x = 0; x < W; x += 2) {
+
-      u8 y0 = s[0];
+    u32 kHeld = hidKeysHeld();
-      u8 u = s[1];
+    if (kHeld & KEY_START)
-      u8 y1 = s[2];
+      longjmp(exitJmp, 1);
-      u8 v = s[3];
-      s += 4;
-      int8_t u2 = (int8_t)(u - 128);
-      int8_t v2 = (int8_t)(v - 128);
-      int rcoef = (int)(1.402f * v2);
-      int gcoef = (int)(-0.344f * u2 - 0.714f * v2);
-      int bcoef = (int)(1.772f * u2);
-      int R = y0 + rcoef;
-      int G = y0 + gcoef;
-      int B = y0 + bcoef;
-      R = R < 0 ? 0 : (R > 255 ? 255 : R);
-      G = G < 0 ? 0 : (G > 255 ? 255 : G);
-      B = B < 0 ? 0 : (B > 255 ? 255 : B);
-      put_pixel(d, x, y, (u8)R, (u8)G, (u8)B);
-      R = y1 + rcoef;
-      G = y1 + gcoef;
-      B = y1 + bcoef;
-      R = R < 0 ? 0 : (R > 255 ? 255 : R);
-      G = G < 0 ? 0 : (G > 255 ? 255 : G);
-      B = B < 0 ? 0 : (B > 255 ? 255 : B);
-      put_pixel(d, x + 1, y, (u8)R, (u8)G, (u8)B);
-    }
   }
 }
 
-static void display_to_row_major(const u8 *in, u8 *out) {
+void cleanup() {
-  for (int y = 0; y < H; ++y) {
-    for (int x = 0; x < W; ++x) {
-      int di = (x * H + (H - 1 - y)) * 3;
-      int oi = (y * W + x) * 3;
-      out[oi + 0] = in[di + 0];
-      out[oi + 1] = in[di + 1];
-      out[oi + 2] = in[di + 2];
-    }
-  }
-}
-
-static int write_jpeg(const u8 *bgr, const char *path) {
-  struct jpeg_compress_struct cinfo;
-  struct jpeg_error_mgr jerr;
-  FILE *f = fopen(path, "wb");
-  if (!f)
-    return -1;
-  cinfo.err = jpeg_std_error(&jerr);
-  jpeg_create_compress(&cinfo);
-  jpeg_stdio_dest(&cinfo, f);
-  cinfo.image_width = W;
-  cinfo.image_height = H;
-  cinfo.input_components = 3;
-  cinfo.in_color_space = JCS_RGB;
-  jpeg_set_defaults(&cinfo);
-  jpeg_set_quality(&cinfo, JPG_QUALITY, TRUE);
-  jpeg_start_compress(&cinfo, TRUE);
-  int rowstride = W * 3;
-  u8 *row = malloc(rowstride);
-  if (!row) {
-    jpeg_destroy_compress(&cinfo);
-    fclose(f);
-    return -1;
-  }
-  while (cinfo.next_scanline < cinfo.image_height) {
-    const u8 *src = bgr + (size_t)cinfo.next_scanline * rowstride;
-    for (int x = 0; x < W; ++x) {
-      row[x * 3 + 0] = src[x * 3 + 2];
-      row[x * 3 + 1] = src[x * 3 + 1];
-      row[x * 3 + 2] = src[x * 3 + 0];
-    }
-    JSAMPROW rp[1];
-    rp[0] = row;
-    jpeg_write_scanlines(&cinfo, rp, 1);
-  }
-  free(row);
-  jpeg_finish_compress(&cinfo);
-  jpeg_destroy_compress(&cinfo);
-  fclose(f);
-  return 0;
-}
-
-static Result post_multipart(const char *url, const char *filepath) {
-  FILE *f = fopen(filepath, "rb");
-  if (!f)
-    return -1;
-  if (fseek(f, 0, SEEK_END) != 0) {
-    fclose(f);
-    return -1;
-  }
-  long sz = ftell(f);
-  if (sz < 0) {
-    fclose(f);
-    return -1;
-  }
-  rewind(f);
-  u8 *filebuf = malloc((size_t)sz);
-  if (!filebuf) {
-    fclose(f);
-    return -1;
-  }
-  if (fread(filebuf, 1, (size_t)sz, f) != (size_t)sz) {
-    free(filebuf);
-    fclose(f);
-    return -1;
-  }
-  fclose(f);
-  char boundary[48];
-  snprintf(boundary, sizeof(boundary), "----printcam%08x", (unsigned)rand());
-  const char *fn = strrchr(filepath, '/');
-  if (fn)
-    fn++;
-  else
-    fn = filepath;
-  char head[256];
-  int headlen = snprintf(
-      head, sizeof(head),
-      "--%s\r\n"
-      "Content-Disposition: form-data; name=\"file\"; filename=\"%s\"\r\n"
-      "Content-Type: image/jpeg\r\n\r\n",
-      boundary, fn);
-  char tail[64];
-  int taillen = snprintf(tail, sizeof(tail), "\r\n--%s--\r\n", boundary);
-  size_t total = (size_t)headlen + (size_t)sz + (size_t)taillen;
-  u8 *buf = malloc(total);
-  if (!buf) {
-    free(filebuf);
-    return -1;
-  }
-  memcpy(buf, head, (size_t)headlen);
-  memcpy(buf + headlen, filebuf, (size_t)sz);
-  memcpy(buf + headlen + sz, tail, (size_t)taillen);
-  free(filebuf);
-  httpcContext ctx;
-  Result r = httpcOpenContext(&ctx, HTTPC_METHOD_POST, url, 0);
-  if (r != 0) {
-    free(buf);
-    return r;
-  }
-  httpcSetSSLOpt(&ctx, SSLCOPT_DisableVerify);
-  httpcSetKeepAlive(&ctx, HTTPC_KEEPALIVE_ENABLED);
-  char ct[128];
-  snprintf(ct, sizeof(ct), "multipart/form-data; boundary=%s", boundary);
-  httpcAddRequestHeaderField(&ctx, "User-Agent", "printcam/1.0");
-  httpcAddRequestHeaderField(&ctx, "Content-Type", ct);
-  r = httpcAddPostDataRaw(&ctx, (u32 *)buf, (u32)total);
-  if (r != 0) {
-    httpcCloseContext(&ctx);
-    free(buf);
-    return r;
-  }
-  r = httpcBeginRequest(&ctx);
-  if (r == 0) {
-    u32 sc = 0;
-    httpcGetResponseStatusCode(&ctx, &sc);
-    u32 downloaded = 0;
-    u32 contentsize = 0;
-    while (httpcGetDownloadSizeState(&ctx, &downloaded, &contentsize) == 0 &&
-           contentsize != 0 && downloaded < contentsize) {
-      u32 rem = contentsize - downloaded;
-      u32 toread = rem > 4096 ? 4096 : rem;
-      u8 tmp[4096];
-      if (httpcReceiveData(&ctx, tmp, toread) != 0)
-        break;
-    }
-  }
-  httpcCloseContext(&ctx);
-  free(buf);
-  return r;
-}
-
-static void setup_camera(u32 max) {
-  u32 sel = camIndex ? SELECT_IN1 : SELECT_OUT1;
-  CAMU_SetSize(sel, SIZE_CTR_TOP_LCD, CONTEXT_A);
-  CAMU_SetOutputFormat(sel, OUTPUT_YUV_422, CONTEXT_A);
-  CAMU_SetFrameRate(sel, FRAME_RATE_30);
-  CAMU_SetNoiseFilter(sel, true);
-  CAMU_SetAutoExposure(sel, true);
-  CAMU_SetAutoWhiteBalance(sel, true);
-  CAMU_SetTransferBytes(PORT_BOTH, max, W, H);
-  CAMU_Activate(sel);
-}
-
-static void cam_thread(void *arg) {
-  acInit();
-  camInit();
-  u32 max;
-  CAMU_GetMaxBytes(&max, W, H);
-
-  setup_camera(max); // Initial setup
-  CAMU_ClearBuffer(PORT_BOTH);
-  CAMU_SynchronizeVsyncTiming(SELECT_OUT1, SELECT_OUT2);
-  CAMU_StartCapture(PORT_BOTH);
-
-  while (!quit) {
-    if (reinitCam) {
-      CAMU_StopCapture(PORT_BOTH);
-      CAMU_ClearBuffer(PORT_BOTH);
-      setup_camera(max); // Reuse same setup logic
-      CAMU_StartCapture(PORT_BOTH);
-      reinitCam = false;
-    }
-
-    Handle ev = 0;
-    int recv = camIndex ? PORT_CAM2 : PORT_CAM1;
-    CAMU_SetReceiving(&ev, camBuf[writeIdx], recv, W * H * 2, max);
-    svcWaitSynchronization(ev, CAM_TIMEOUT_NS);
-    svcCloseHandle(ev);
-    svcSignalEvent(readyEv[writeIdx]);
-    writeIdx ^= 1;
-  }
-
-  CAMU_StopCapture(PORT_BOTH);
-  CAMU_Activate(SELECT_NONE);
   camExit();
+  gfxExit();
   acExit();
 }
 
-int main(int argc, char **argv) {
+void writePictureToFramebufferRGB565(void *fb, void *img, u16 x, u16 y,
-  gfxInitDefault();
+                                     u16 width, u16 height) {
-  httpcInit(4 * 1024 * 1024);
+  u8 *fb_8 = (u8 *)fb;
-  consoleInit(GFX_BOTTOM, &bottom);
+  u16 *img_16 = (u16 *)img;
-  consoleSelect(&bottom);
+  int i, j, draw_x, draw_y;
-  printf("printcam\nL/R=take X=switch Y=reload START=exit\n");
+  for (j = 0; j < height; j++) {
-  int rc = init_endpoint_config();
+    for (i = 0; i < width; i++) {
-  if (rc == 1)
+      draw_y = y + height - j;
-    printf("Endpoint loaded: %s\n", endpoint);
+      draw_x = x + i;
-  else if (rc == 0)
+      u32 v = (draw_y + draw_x * height) * 3;
-    printf("Endpoint created: %s\n", endpoint);
+      u16 data = img_16[j * width + i];
-  else
+      uint8_t b = ((data >> 11) & 0x1F) << 3;
-    printf("Endpoint init failed errno=%d %s\n", errno, strerror(errno));
+      uint8_t g = ((data >> 5) & 0x3F) << 2;
-  camBuf[0] = memalign(0x40, W * H * 2);
+      uint8_t r = (data & 0x1F) << 3;
-  camBuf[1] = memalign(0x40, W * H * 2);
+      fb_8[v] = r;
-  dispBuf = memalign(0x40, FB_BYTES);
+      fb_8[v + 1] = g;
-  u8 *linear = malloc(FB_BYTES);
+      fb_8[v + 2] = b;
-  if (!camBuf[0] || !camBuf[1] || !dispBuf || !linear) {
-    printf("OOM\n");
-    goto end;
-  }
-  svcCreateEvent(&readyEv[0], RESET_ONESHOT);
-  svcCreateEvent(&readyEv[1], RESET_ONESHOT);
-  gfxSetDoubleBuffering(GFX_TOP, true);
-  camThread = threadCreate(cam_thread, NULL, 0x1000, 0x2F, -2, false);
-  while (aptMainLoop()) {
-    hidScanInput();
-    u32 k = hidKeysDown();
-    if (k & KEY_START)
-      break;
-    if (k & KEY_X) {
-      camIndex ^= 1;
-      reinitCam = true;
-      printf("Switched to %s\n", camIndex ? "front" : "back");
-    }
-    if (k & KEY_Y) {
-      int r = init_endpoint_config();
-      if (r >= 0)
-        printf("Reload=%d endpoint=%s\n", r, endpoint);
-      else
-        printf("Reload failed errno=%d %s\n", errno, strerror(errno));
-    }
-    if (k & (KEY_L | KEY_R)) {
-      consoleSelect(&bottom);
-      printf("Snapshot\n");
-      display_to_row_major(dispBuf, linear);
-      char path[128];
-      time_t t = time(NULL);
-      strftime(path, sizeof(path), DIR "/%Y%m%d_%H%M%S.jpg", localtime(&t));
-      if (write_jpeg(linear, path) == 0) {
-        printf("Wrote %s\n", path);
-        Result r = post_multipart(endpoint, path);
-        printf("upload: 0x%" PRIx32 "\n", (u32)r);
-      } else {
-        printf("JPEG failed\n");
     }
   }
+}
+
+void takePicture() {
+  u8 *buf = malloc(BUF_SIZE);
+  if (!buf) {
+    hang("Failed to allocate memory!");
+  }
+  u32 bufSize;
+
+  CAMU_GetMaxBytes(&bufSize, PIC_WIDTH, PIC_HEIGHT);
+  CAMU_SetTransferBytes(PORT, bufSize, PIC_WIDTH, PIC_HEIGHT);
+  CAMU_SwitchContext(getActiveSelect(), CONTEXT_B);
+
+  Handle camReceiveEvent = 0;
+  CAMU_Activate(getActiveSelect());
+  CAMU_ClearBuffer(PORT);
+  CAMU_StartCapture(PORT);
+  CAMU_SetReceiving(&camReceiveEvent, buf, PORT, PIC_SIZE, (s16)bufSize);
+  svcWaitSynchronization(camReceiveEvent, WAIT_TIMEOUT);
+
+  CAMU_StopCapture(PORT);
+  CAMU_Activate(PORT_NONE);
+  svcCloseHandle(camReceiveEvent);
+  CAMU_SwitchContext(getActiveSelect(), CONTEXT_A);
+
+  for (int i = 0; i < 5; i++) {
+    printf("%02x", buf[i]);
+  }
+
+  free(buf);
+}
+
+void setupCam(u32 *bufSize) {
+  camInit();
+
+  CAMU_SetSize(CAMS, SIZE_CTR_TOP_LCD, CONTEXT_A);
+  CAMU_SetSize(CAMS, SIZE_VGA, CONTEXT_B);
+  CAMU_SetOutputFormat(CAMS, OUTPUT_RGB_565, CONTEXT_BOTH);
+  CAMU_SetFrameRate(CAMS, FRAME_RATE_30);
+  CAMU_SetNoiseFilter(CAMS, true);
+  CAMU_SetAutoExposure(CAMS, true);
+  CAMU_SetAutoWhiteBalance(CAMS, true);
+  CAMU_SetTrimming(PORT_BOTH, false);
+  CAMU_PlayShutterSound(SHUTTER_SOUND_TYPE_MOVIE);
+
+  CAMU_GetMaxBytes(bufSize, WIDTH, HEIGHT);
+  CAMU_SetTransferBytes(PORT_BOTH, *bufSize, WIDTH, HEIGHT);
+}
+void activateCam(Handle camReceiveEvent[2]) {
+  CAMU_Activate(getActiveSelect());
+  CAMU_GetBufferErrorInterruptEvent(&camReceiveEvent[0], PORT);
+  CAMU_ClearBuffer(PORT);
+  CAMU_StartCapture(PORT);
+
+  gfxFlushBuffers();
   gspWaitForVBlank();
-    if (svcWaitSynchronization(readyEv[dispIdx], 0) == 0) {
+  gfxSwapBuffers();
-      yuyv_to_display(camBuf[dispIdx], dispBuf);
-      dispIdx ^= 1;
 }
-    u8 *fb = gfxGetFramebuffer(GFX_TOP, GFX_LEFT, NULL, NULL);
+
-    memcpy(fb, dispBuf, FB_BYTES);
+int main() {
-    gfxSwapBuffersGpu();
+  acInit();
-  }
+  gfxInitDefault();
-end:
+  consoleInit(GFX_BOTTOM, NULL);
-  quit = true;
+
-  if (camThread)
+  gfxSetDoubleBuffering(GFX_TOP, true);
-    threadJoin(camThread, U64_MAX);
+  gfxSetDoubleBuffering(GFX_BOTTOM, false);
-  if (camThread)
+
-    threadFree(camThread);
+  if (setjmp(exitJmp)) {
-  if (readyEv[0])
+    cleanup();
-    svcCloseHandle(readyEv[0]);
+    return 0;
-  if (readyEv[1])
+  }
-    svcCloseHandle(readyEv[1]);
+
-  if (camBuf[0])
+  u32 kDown;
-    free(camBuf[0]);
+  u8 *buf = malloc(BUF_SIZE);
-  if (camBuf[1])
+  u32 bufSize;
-    free(camBuf[1]);
+  Handle camReceiveEvent[2] = {0};
-  if (dispBuf)
+  bool captureInterrupted = false;
-    free(dispBuf);
+  s32 index = 0;
-  if (linear)
+
-    free(linear);
+  if (!buf) {
-  httpcExit();
+    hang("Failed to allocate memory!");
-  gfxExit();
+  }
+
+  printf("Initializing camera\n");
+
+  setupCam(&bufSize);
+  activateCam(camReceiveEvent);
+
+  printf("Press Start to exit to Homebrew Launcher\n");
+
+  while (aptMainLoop()) {
+    if (!captureInterrupted) {
+      hidScanInput();
+      kDown = hidKeysDown();
+
+      if (kDown & KEY_START) {
+        break;
+      }
+      if (kDown & KEY_A) {
+        printf("Switching cameras...\n");
+
+        CAMU_StopCapture(PORT);
+        svcCloseHandle(camReceiveEvent[0]);
+        svcCloseHandle(camReceiveEvent[1]);
+
+        activeCam ^= true;
+        activateCam(camReceiveEvent);
+      }
+
+      if (kDown & KEY_R) {
+        printf("Taking pic...\n");
+
+        CAMU_StopCapture(PORT);
+        svcCloseHandle(camReceiveEvent[0]);
+        svcCloseHandle(camReceiveEvent[1]);
+
+        takePicture();
+        // setupCam();
+        activateCam(camReceiveEvent);
+      }
+    }
+
+    if (camReceiveEvent[1] == 0) {
+      CAMU_SetReceiving(&camReceiveEvent[1], buf, PORT, SCREEN_SIZE,
+                        (s16)bufSize);
+    }
+    if (captureInterrupted) {
+      CAMU_StartCapture(PORT);
+      captureInterrupted = false;
+    }
+
+    svcWaitSynchronizationN(&index, camReceiveEvent, 2, false, WAIT_TIMEOUT);
+    switch (index) {
+    case 0:
+      svcCloseHandle(camReceiveEvent[1]);
+      camReceiveEvent[1] = 0;
+
+      captureInterrupted = true;
+      continue; // skip screen update
+      break;
+    case 1:
+      svcCloseHandle(camReceiveEvent[1]);
+      camReceiveEvent[1] = 0;
+      break;
+    default:
+      break;
+    }
+
+    writePictureToFramebufferRGB565(
+        gfxGetFramebuffer(GFX_TOP, GFX_LEFT, NULL, NULL), buf, 0, 0, WIDTH,
+        HEIGHT);
+
+    gfxFlushBuffers();
+    gspWaitForVBlank();
+    gfxSwapBuffers();
+  }
+
+  CAMU_StopCapture(PORT);
+  for (int i = 0; i < 2; i++) {
+    if (camReceiveEvent[i] != 0) {
+      svcCloseHandle(camReceiveEvent[i]);
+    }
+  }
+  CAMU_Activate(PORT_NONE);
+
+  // Exit
+  free(buf);
+  cleanup();
+
+  // Return to hbmenu
   return 0;
 }