diff --git a/c/src/lib/libbsp/m68k/mcf5225x/make/custom/mcf5225x.cfg b/c/src/lib/libbsp/m68k/mcf5225x/make/custom/mcf5225x.cfg
index 90e76e2709..b53b214199 100644
--- a/c/src/lib/libbsp/m68k/mcf5225x/make/custom/mcf5225x.cfg
+++ b/c/src/lib/libbsp/m68k/mcf5225x/make/custom/mcf5225x.cfg
@@ -16,7 +16,9 @@ CPU_CFLAGS = -mcpu=52235 $(PROJECT_FLAGS)
 
 # optimize flag: typically -O2
 CFLAGS_OPTIMIZE_V = -O2 -fomit-frame-pointer
-#CFLAGS_OPTIMIZE_V = -g
+CFLAGS_OPTIMIZE_V += -ffunction-sections -fdata-sections
+
+LDFLAGS = -Wl,--gc-sections
 
 # This defines the operations performed on the linked executable.
 # is currently required.