Merge pull request #870 from iabdalkader/camera_fb_align

pennam · web-flow · commit 4930554bd364 · 2024-04-19T08:46:24.000+02:00
Camera: Fix framebuffer malloc alignment.
diff --git a/libraries/Camera/src/camera.cpp b/libraries/Camera/src/camera.cpp
@@ -27,6 +27,12 @@
 
 #define ALIGN_PTR(p,a)   ((p & (a-1)) ?(((uintptr_t)p + a) & ~(uintptr_t)(a-1)) : p)
 
+#ifdef __SCB_DCACHE_LINE_SIZE
+#define FB_ALIGNMENT    __SCB_DCACHE_LINE_SIZE
+#else
+#define FB_ALIGNMENT    32
+#endif
+
 // Include all image sensor drivers here.
 #if defined (ARDUINO_PORTENTA_H7_M7)
 
@@ -337,15 +343,16 @@ FrameBuffer::FrameBuffer(int32_t x, int32_t y, int32_t bpp) :
     _fb_size(x*y*bpp),
     _isAllocated(true)
 {
-    uint8_t *buffer = (uint8_t *)malloc(x*y*bpp);
-    _fb = (uint8_t *)ALIGN_PTR((uintptr_t)buffer, 32);
+    uint8_t *buffer = (uint8_t *) malloc(x * y * bpp + FB_ALIGNMENT);
+    _fb = (uint8_t *) ALIGN_PTR((uintptr_t) buffer, FB_ALIGNMENT);
 }
 
 FrameBuffer::FrameBuffer(int32_t address) : 
     _fb_size(0),
-    _isAllocated(true)
+    _isAllocated(true),
+    _fb((uint8_t *) address)
 {
-    _fb = (uint8_t *)ALIGN_PTR((uintptr_t)address, 32);
+    // Assume that `address` is aligned, this will be verified later in grabFrame.
 }
 
 FrameBuffer::FrameBuffer() : 
@@ -688,17 +695,17 @@ int Camera::grabFrame(FrameBuffer &fb, uint32_t timeout)
             }
         }
     } else {
-        uint8_t *buffer = (uint8_t *)malloc(framesize+32);
-        uint8_t *alignedBuff = (uint8_t *)ALIGN_PTR((uintptr_t)buffer, 32);
-        fb.setBuffer(alignedBuff);
+        uint8_t *buffer = (uint8_t *) malloc(framesize + FB_ALIGNMENT);
+        uint8_t *aligned_buffer = (uint8_t *) ALIGN_PTR((uintptr_t) buffer, FB_ALIGNMENT);
+        fb.setBuffer(aligned_buffer);
     }
 
     uint8_t *framebuffer = fb.getBuffer();
 
-    // Ensure FB is aligned to 32 bytes cache lines.
-    if ((uint32_t) framebuffer & 0x1F) {
+    // Ensure that the framebuffer is aligned.
+    if ((uint32_t) framebuffer & (FB_ALIGNMENT - 1)) {
         if (_debug) {
-            _debug->println("Framebuffer not aligned to 32 bytes cache lines");
+            _debug->println("The framebuffer memory is not aligned!");
         }
         return -1;
     }

Original file line number	Diff line number	Diff line change
`@@ -27,6 +27,12 @@`
`27`	`27`
`28`	`28`	`#define ALIGN_PTR(p,a) ((p & (a-1)) ?(((uintptr_t)p + a) & ~(uintptr_t)(a-1)) : p)`
`29`	`29`
	`30`	`+#ifdef __SCB_DCACHE_LINE_SIZE`
	`31`	`+#define FB_ALIGNMENT __SCB_DCACHE_LINE_SIZE`
	`32`	`+#else`
	`33`	`+#define FB_ALIGNMENT 32`
	`34`	`+#endif`
	`35`	`+`
`30`	`36`	`// Include all image sensor drivers here.`
`31`	`37`	`#if defined (ARDUINO_PORTENTA_H7_M7)`
`32`	`38`
`@@ -337,15 +343,16 @@ FrameBuffer::FrameBuffer(int32_t x, int32_t y, int32_t bpp) :`
`337`	`343`	`_fb_size(xybpp),`
`338`	`344`	`_isAllocated(true)`
`339`	`345`	`{`
`340`		`- uint8_t buffer = (uint8_t )malloc(xybpp);`
`341`		`- _fb = (uint8_t *)ALIGN_PTR((uintptr_t)buffer, 32);`
	`346`	`+ uint8_t buffer = (uint8_t ) malloc(x * y * bpp + FB_ALIGNMENT);`
	`347`	`+ _fb = (uint8_t *) ALIGN_PTR((uintptr_t) buffer, FB_ALIGNMENT);`
`342`	`348`	`}`
`343`	`349`
`344`	`350`	`FrameBuffer::FrameBuffer(int32_t address) :`
`345`	`351`	`_fb_size(0),`
`346`		`- _isAllocated(true)`
	`352`	`+ _isAllocated(true),`
	`353`	`+ _fb((uint8_t *) address)`
`347`	`354`	`{`
`348`		`- _fb = (uint8_t *)ALIGN_PTR((uintptr_t)address, 32);`
	`355`	+ // Assume that `address` is aligned, this will be verified later in grabFrame.
`349`	`356`	`}`
`350`	`357`
`351`	`358`	`FrameBuffer::FrameBuffer() :`
`@@ -688,17 +695,17 @@ int Camera::grabFrame(FrameBuffer &fb, uint32_t timeout)`
`688`	`695`	`}`
`689`	`696`	`}`
`690`	`697`	`} else {`
`691`		`- uint8_t buffer = (uint8_t )malloc(framesize+32);`
`692`		`- uint8_t alignedBuff = (uint8_t )ALIGN_PTR((uintptr_t)buffer, 32);`
`693`		`- fb.setBuffer(alignedBuff);`
	`698`	`+ uint8_t buffer = (uint8_t ) malloc(framesize + FB_ALIGNMENT);`
	`699`	`+ uint8_t aligned_buffer = (uint8_t ) ALIGN_PTR((uintptr_t) buffer, FB_ALIGNMENT);`
	`700`	`+ fb.setBuffer(aligned_buffer);`
`694`	`701`	`}`
`695`	`702`
`696`	`703`	`uint8_t *framebuffer = fb.getBuffer();`
`697`	`704`
`698`		`- // Ensure FB is aligned to 32 bytes cache lines.`
`699`		`- if ((uint32_t) framebuffer & 0x1F) {`
	`705`	`+ // Ensure that the framebuffer is aligned.`
	`706`	`+ if ((uint32_t) framebuffer & (FB_ALIGNMENT - 1)) {`
`700`	`707`	`if (_debug) {`
`701`		`- _debug->println("Framebuffer not aligned to 32 bytes cache lines");`
	`708`	`+ _debug->println("The framebuffer memory is not aligned!");`
`702`	`709`	`}`
`703`	`710`	`return -1;`
`704`	`711`	`}`