diff --git a/rpi3.mk b/rpi3.mk
index 8f965c44b1e27c5082c3e6e6bc3c80b4382efbad..0ad036dbc08cbf0266f08971cf5a88cec53c5feb 100644
--- a/rpi3.mk
+++ b/rpi3.mk
@@ -40,14 +40,15 @@ U-BOOT_PATH		?= $(ROOT)/u-boot
 U-BOOT_BIN		?= $(U-BOOT_PATH)/u-boot.bin
 U-BOOT_JTAG_BIN		?= $(U-BOOT_PATH)/u-boot-jtag.bin
 
-RPI3_FIRMWARE_PATH	?= $(BUILD_PATH)/rpi3/firmware
-RPI3_HEAD_BIN		?= $(ROOT)/out/head.bin
-RPI3_BOOT_CONFIG	?= $(RPI3_FIRMWARE_PATH)/config.txt
-RPI3_UBOOT_ENV		?= $(ROOT)/out/uboot.env
-RPI3_UBOOT_ENV_TXT	?= $(RPI3_FIRMWARE_PATH)/uboot.env.txt
-RPI3_STOCK_FW_PATH	?= $(ROOT)/rpi3_firmware
-
-OPTEE_OS_PAGER		?= $(OPTEE_OS_PATH)/out/arm/core/tee-pager.bin
+RPI3_FIRMWARE_PATH		?= $(BUILD_PATH)/rpi3/firmware
+RPI3_HEAD_BIN			?= $(ROOT)/out/head.bin
+RPI3_BOOT_CONFIG		?= $(RPI3_FIRMWARE_PATH)/config.txt
+RPI3_UBOOT_ENV			?= $(ROOT)/out/uboot.env
+RPI3_UBOOT_ENV_TXT		?= $(RPI3_FIRMWARE_PATH)/uboot.env.txt
+RPI3_STOCK_FW_PATH		?= $(ROOT)/rpi3_firmware
+RPI3_STOCK_FW_PATH_BOOT	?= $(RPI3_STOCK_FW_PATH)/boot
+RPI3_STOCK_ZIP_BOOT		?= firmware-$(RPI3_FIRMWARE_FILE)/boot
+OPTEE_OS_PAGER			?= $(OPTEE_OS_PATH)/out/arm/core/tee-pager.bin
 
 LINUX_IMAGE		?= $(LINUX_PATH)/arch/arm64/boot/Image
 LINUX_DTB		?= $(LINUX_PATH)/arch/arm64/boot/dts/broadcom/bcm2710-rpi-3-b.dtb
@@ -184,14 +185,41 @@ rpi3-firmware:
 ifeq ("$(wildcard $(ROOT)/out/$(RPI3_FIRMWARE_FILE).$(RPI3_FIRMWARE_FILE_EXT))","")
 	echo "Downloading Raspberry Pi 3 firmware ..."
 	mkdir -p $(ROOT)/out
+	mkdir -p $(RPI3_STOCK_FW_PATH)/boot
 	wget $(RPI3_FIRMWARE_URL)/$(RPI3_FIRMWARE_FILE).$(RPI3_FIRMWARE_FILE_EXT) -O $(ROOT)/out/$(RPI3_FIRMWARE_FILE).$(RPI3_FIRMWARE_FILE_EXT)
-	unzip -a $(ROOT)/out/$(RPI3_FIRMWARE_FILE).$(RPI3_FIRMWARE_FILE_EXT) -d $(ROOT)
-	mv $(ROOT)/firmware-$(RPI3_FIRMWARE_FILE) $(RPI3_STOCK_FW_PATH)
+	unzip -aj $(ROOT)/out/$(RPI3_FIRMWARE_FILE).$(RPI3_FIRMWARE_FILE_EXT) \
+	'$(RPI3_STOCK_ZIP_BOOT)/bootcode.bin' \
+	'$(RPI3_STOCK_ZIP_BOOT)/COPYING.linux' \
+	'$(RPI3_STOCK_ZIP_BOOT)/fixup_cd.dat' \
+	'$(RPI3_STOCK_ZIP_BOOT)/fixup.dat' \
+	'$(RPI3_STOCK_ZIP_BOOT)/fixup_db.dat' \
+	'$(RPI3_STOCK_ZIP_BOOT)/fixup_x.dat' \
+	'$(RPI3_STOCK_ZIP_BOOT)/LICENCE.broadcom' \
+	'$(RPI3_STOCK_ZIP_BOOT)/start_cd.elf' \
+	'$(RPI3_STOCK_ZIP_BOOT)/start_db.elf' \
+	'$(RPI3_STOCK_ZIP_BOOT)/start.elf' \
+	'$(RPI3_STOCK_ZIP_BOOT)/start_x.elf' -d $(RPI3_STOCK_FW_PATH)/boot
 endif
 
 .PHONY: rpi3-firmware-clean
 rpi3-firmware-clean:
 	rm -f $(ROOT)/out/$(RPI3_FIRMWARE_FILE).$(RPI3_FIRMWARE_FILE_EXT)
+	if [ -d "$(RPI3_STOCK_FW_PATH_BOOT)" ]; then \
+		rm -f '$(RPI3_STOCK_FW_PATH_BOOT)/bootcode.bin' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/COPYING.linux' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/fixup_cd.dat' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/fixup.dat' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/fixup_db.dat' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/fixup_x.dat' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/LICENCE.broadcom' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/start_cd.elf' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/start_db.elf' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/start.elf' \
+		'$(RPI3_STOCK_FW_PATH_BOOT)/start_x.elf'; \
+		rmdir $(RPI3_STOCK_FW_PATH_BOOT); \
+		rmdir $(RPI3_STOCK_FW_PATH); \
+	fi
+
 
 ################################################################################
 # xtest / optee_test
@@ -213,7 +241,7 @@ helloworld-clean: helloworld-clean-common
 # Root FS
 ################################################################################
 .PHONY: filelist-tee
-filelist-tee: linux
+filelist-tee: linux rpi3-firmware
 filelist-tee: filelist-tee-common
 	@echo "dir /usr/bin 755 0 0" >> $(GEN_ROOTFS_FILELIST)
 	@echo "dir /boot 755 0 0" >> $(GEN_ROOTFS_FILELIST)
@@ -238,7 +266,7 @@ filelist-tee: filelist-tee-common
 	@echo "file /boot/start_x.elf $(RPI3_STOCK_FW_PATH)/boot/start_x.elf 755 0 0" >> $(GEN_ROOTFS_FILELIST)
 
 .PHONY: update_rootfs
-update_rootfs: arm-tf u-boot
+update_rootfs: arm-tf u-boot rpi3-firmware
 update_rootfs: update_rootfs-common
 
 # Creating images etc, could wipe out a drive on the system, therefore we don't