Merge pull request #169 from achingupta/ag/tf-issues#198

Ag/tf issues#198
diff --git a/Makefile b/Makefile
index 5272474..39496fb 100644
--- a/Makefile
+++ b/Makefile
@@ -29,6 +29,12 @@
 #
 
 #
+# Trusted Firmware Version
+#
+VERSION_MAJOR		:= 0
+VERSION_MINOR		:= 4
+
+#
 # Default values for build configurations
 #
 
@@ -51,6 +57,9 @@
 RESET_TO_BL31		:= 0
 # Include FP registers in cpu context
 CTX_INCLUDE_FPREGS		:= 0
+# Determine the version of ARM GIC architecture to use for interrupt management
+# in EL3. The platform port can change this value if needed.
+ARM_GIC_ARCH		:=	2
 
 
 # Checkpatch ignores
@@ -73,8 +82,16 @@
 	BUILD_TYPE	:=	release
 endif
 
+# Default build string (git branch and commit)
+ifeq (${BUILD_STRING},)
+	BUILD_STRING	:=	$(shell git log -n 1 --pretty=format:"%h")
+endif
+
+VERSION_STRING		:=	v${VERSION_MAJOR}.${VERSION_MINOR}(${BUILD_TYPE}):${BUILD_STRING}
+
 BL_COMMON_SOURCES	:=	common/bl_common.c			\
 				common/debug.c				\
+				common/tf_printf.c			\
 				lib/aarch64/cache_helpers.S		\
 				lib/aarch64/misc_helpers.S		\
 				lib/aarch64/xlat_helpers.c		\
@@ -187,6 +204,10 @@
 $(eval $(call assert_boolean,CTX_INCLUDE_FPREGS))
 $(eval $(call add_define,CTX_INCLUDE_FPREGS))
 
+# Process ARM_GIC_ARCH flag
+$(eval $(call add_define,ARM_GIC_ARCH))
+
+
 ASFLAGS			+= 	-nostdinc -ffreestanding -Wa,--fatal-warnings	\
 				-Werror -Wmissing-include-dirs			\
 				-mgeneral-regs-only -D__ASSEMBLY__		\
@@ -369,7 +390,8 @@
 
 $(ELF) : $(OBJS) $(LINKERFILE)
 	@echo "  LD      $$@"
-	@echo 'const char build_message[] = "Built : "__TIME__", "__DATE__;' | \
+	@echo 'const char build_message[] = "Built : "__TIME__", "__DATE__; \
+	       const char version_string[] = "${VERSION_STRING}";' | \
 		$$(CC) $$(CFLAGS) -xc - -o $(BUILD_DIR)/build_message.o
 	$$(Q)$$(LD) -o $$@ $$(LDFLAGS) -Map=$(MAPFILE) --script $(LINKERFILE) \
 					$(BUILD_DIR)/build_message.o $(OBJS)
diff --git a/bl1/bl1_main.c b/bl1/bl1_main.c
index 897660c..c6f2caa 100644
--- a/bl1/bl1_main.c
+++ b/bl1/bl1_main.c
@@ -35,7 +35,6 @@
 #include <debug.h>
 #include <platform.h>
 #include <platform_def.h>
-#include <stdio.h>
 #include "bl1_private.h"
 
 /*******************************************************************************
@@ -130,8 +129,9 @@
 	bl1_platform_setup();
 
 	/* Announce our arrival */
-	printf(FIRMWARE_WELCOME_STR);
-	printf("%s\n\r", build_message);
+	tf_printf(FIRMWARE_WELCOME_STR);
+	tf_printf("%s\n", version_string);
+	tf_printf("%s\n", build_message);
 
 	SET_PARAM_HEAD(&bl2_image_info, PARAM_IMAGE_BINARY, VERSION_1, 0);
 	SET_PARAM_HEAD(&bl2_ep, PARAM_EP, VERSION_1, 0);
@@ -150,7 +150,7 @@
 		 * TODO: print failure to load BL2 but also add a tzwdog timer
 		 * which will reset the system eventually.
 		 */
-		printf("Failed to load boot loader stage 2 (BL2) firmware.\n");
+		tf_printf("Failed to load boot loader stage 2 (BL2) firmware.\n");
 		panic();
 	}
 	/*
@@ -165,12 +165,12 @@
 
 	bl1_plat_set_bl2_ep_info(&bl2_image_info, &bl2_ep);
 	bl2_ep.args.arg1 = (unsigned long)bl2_tzram_layout;
-	printf("Booting trusted firmware boot loader stage 2\n");
+	tf_printf("Booting trusted firmware boot loader stage 2\n");
 #if DEBUG
-	printf("BL2 address = 0x%llx\n",
+	tf_printf("BL2 address = 0x%llx\n",
 		(unsigned long long) bl2_ep.pc);
-	printf("BL2 cpsr = 0x%x\n", bl2_ep.spsr);
-	printf("BL2 memory layout address = 0x%llx\n",
+	tf_printf("BL2 cpsr = 0x%x\n", bl2_ep.spsr);
+	tf_printf("BL2 memory layout address = 0x%llx\n",
 	       (unsigned long long) bl2_tzram_layout);
 #endif
 	bl1_run_bl2(&bl2_ep);
@@ -184,13 +184,13 @@
  ******************************************************************************/
 void display_boot_progress(entry_point_info_t *bl31_ep_info)
 {
-	printf("Booting trusted firmware boot loader stage 3\n\r");
+	tf_printf("Booting trusted firmware boot loader stage 3\n\r");
 #if DEBUG
-	printf("BL31 address = 0x%llx\n", (unsigned long long)bl31_ep_info->pc);
-	printf("BL31 cpsr = 0x%llx\n", (unsigned long long)bl31_ep_info->spsr);
-	printf("BL31 params address = 0x%llx\n",
+	tf_printf("BL31 address = 0x%llx\n", (unsigned long long)bl31_ep_info->pc);
+	tf_printf("BL31 cpsr = 0x%llx\n", (unsigned long long)bl31_ep_info->spsr);
+	tf_printf("BL31 params address = 0x%llx\n",
 			(unsigned long long)bl31_ep_info->args.arg0);
-	printf("BL31 plat params address = 0x%llx\n",
+	tf_printf("BL31 plat params address = 0x%llx\n",
 			(unsigned long long)bl31_ep_info->args.arg1);
 #endif
 	return;
diff --git a/bl2/bl2_main.c b/bl2/bl2_main.c
index d96e199..ca83842 100644
--- a/bl2/bl2_main.c
+++ b/bl2/bl2_main.c
@@ -35,7 +35,6 @@
 #include <debug.h>
 #include <platform.h>
 #include <platform_def.h>
-#include <stdio.h>
 #include "bl2_private.h"
 
 /*******************************************************************************
@@ -196,7 +195,8 @@
 	/* Perform platform setup in BL2 */
 	bl2_platform_setup();
 
-	printf("BL2 %s\n\r", build_message);
+	tf_printf("BL2 %s\n", version_string);
+	tf_printf("BL2 %s\n", build_message);
 
 	/*
 	 * Load the subsequent bootloader images
diff --git a/bl31/bl31_main.c b/bl31/bl31_main.c
index d949a08..861b391 100644
--- a/bl31/bl31_main.c
+++ b/bl31/bl31_main.c
@@ -34,9 +34,9 @@
 #include <bl_common.h>
 #include <bl31.h>
 #include <context_mgmt.h>
+#include <debug.h>
 #include <platform.h>
 #include <runtime_svc.h>
-#include <stdio.h>
 #include <string.h>
 
 /*******************************************************************************
@@ -77,7 +77,8 @@
 	/* Perform platform setup in BL1 */
 	bl31_platform_setup();
 
-	printf("BL31 %s\n\r", build_message);
+	tf_printf("BL31 %s\n", version_string);
+	tf_printf("BL31 %s\n", build_message);
 
 	/* Initialise helper libraries */
 	bl31_lib_init();
diff --git a/bl32/tsp/tsp_interrupt.c b/bl32/tsp/tsp_interrupt.c
index ff6bdc5..c1c9aad 100644
--- a/bl32/tsp/tsp_interrupt.c
+++ b/bl32/tsp/tsp_interrupt.c
@@ -56,7 +56,7 @@
 		tsp_stats[linear_id].sync_fiq_ret_count++;
 
 	spin_lock(&console_lock);
-	printf("TSP: cpu 0x%x sync fiq request from 0x%llx \n\r",
+	tf_printf("TSP: cpu 0x%x sync fiq request from 0x%llx \n\r",
 	       mpidr, elr_el3);
 	INFO("cpu 0x%x: %d sync fiq requests, %d sync fiq returns\n",
 	     mpidr,
@@ -100,7 +100,7 @@
 	/* Update the statistics and print some messages */
 	tsp_stats[linear_id].fiq_count++;
 	spin_lock(&console_lock);
-	printf("TSP: cpu 0x%x handled fiq %d \n\r",
+	tf_printf("TSP: cpu 0x%x handled fiq %d \n\r",
 	       mpidr, id);
 	INFO("cpu 0x%x: %d fiq requests \n",
 	     mpidr, tsp_stats[linear_id].fiq_count);
@@ -116,7 +116,7 @@
 
 	tsp_stats[linear_id].irq_count++;
 	spin_lock(&console_lock);
-	printf("TSP: cpu 0x%x received irq\n\r", mpidr);
+	tf_printf("TSP: cpu 0x%x received irq\n\r", mpidr);
 	INFO("cpu 0x%x: %d irq requests \n",
 	     mpidr, tsp_stats[linear_id].irq_count);
 	spin_unlock(&console_lock);
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index eaabe61..b2850e9 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -34,7 +34,6 @@
 #include <platform.h>
 #include <platform_def.h>
 #include <spinlock.h>
-#include <stdio.h>
 #include <tsp.h>
 
 /*******************************************************************************
@@ -121,7 +120,8 @@
 	tsp_stats[linear_id].cpu_on_count++;
 
 	spin_lock(&console_lock);
-	printf("TSP %s\n\r", build_message);
+	tf_printf("TSP %s\n", version_string);
+	tf_printf("TSP %s\n", build_message);
 	INFO("Total memory base : 0x%x\n", (unsigned long)BL32_TOTAL_BASE);
 	INFO("Total memory size : 0x%x bytes\n",
 			 (unsigned long)(BL32_TOTAL_LIMIT - BL32_TOTAL_BASE));
@@ -153,7 +153,7 @@
 	tsp_stats[linear_id].cpu_on_count++;
 
 	spin_lock(&console_lock);
-	printf("SP: cpu 0x%x turned on\n\r", mpidr);
+	tf_printf("SP: cpu 0x%x turned on\n\r", mpidr);
 	INFO("cpu 0x%x: %d smcs, %d erets %d cpu on requests\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count,
@@ -193,7 +193,7 @@
 	tsp_stats[linear_id].cpu_off_count++;
 
 	spin_lock(&console_lock);
-	printf("SP: cpu 0x%x off request\n\r", mpidr);
+	tf_printf("SP: cpu 0x%x off request\n\r", mpidr);
 	INFO("cpu 0x%x: %d smcs, %d erets %d cpu off requests\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count,
@@ -235,7 +235,7 @@
 	tsp_stats[linear_id].cpu_suspend_count++;
 
 	spin_lock(&console_lock);
-	printf("SP: cpu 0x%x suspend request. power state: 0x%x\n\r",
+	tf_printf("SP: cpu 0x%x suspend request. power state: 0x%x\n\r",
 	       mpidr, power_state);
 	INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
@@ -273,7 +273,7 @@
 	tsp_stats[linear_id].cpu_resume_count++;
 
 	spin_lock(&console_lock);
-	printf("SP: cpu 0x%x resumed. suspend level %d \n\r",
+	tf_printf("SP: cpu 0x%x resumed. suspend level %d \n\r",
 	       mpidr, suspend_level);
 	INFO("cpu 0x%x: %d smcs, %d erets %d cpu suspend requests\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
@@ -312,7 +312,7 @@
 
 	smc_type = ((func >> 31) & 1) == 1 ? "fast" : "standard";
 
-	printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
+	tf_printf("SP: cpu 0x%x received %s smc 0x%x\n", read_mpidr(), smc_type, func);
 	INFO("cpu 0x%x: %d smcs, %d erets\n", mpidr,
 	     tsp_stats[linear_id].smc_count,
 	     tsp_stats[linear_id].eret_count);
diff --git a/common/bl_common.c b/common/bl_common.c
index d4bde51..60b63f1 100644
--- a/common/bl_common.c
+++ b/common/bl_common.c
@@ -148,12 +148,12 @@
 			   const meminfo_t *mem_layout)
 {
 #if DEBUG
-	printf("Trying to load image at address 0x%lx, size = 0x%lx\r\n",
+	tf_printf("Trying to load image at address 0x%lx, size = 0x%lx\r\n",
 		image_load_addr, image_size);
-	printf("Current memory layout:\r\n");
-	printf("  total region = [0x%lx, 0x%lx]\r\n", mem_layout->total_base,
+	tf_printf("Current memory layout:\r\n");
+	tf_printf("  total region = [0x%lx, 0x%lx]\r\n", mem_layout->total_base,
 			mem_layout->total_base + mem_layout->total_size);
-	printf("  free region = [0x%lx, 0x%lx]\r\n", mem_layout->free_base,
+	tf_printf("  free region = [0x%lx, 0x%lx]\r\n", mem_layout->free_base,
 			mem_layout->free_base + mem_layout->free_size);
 #endif
 }
diff --git a/common/debug.c b/common/debug.c
index 0f59ed5..be54f5d 100644
--- a/common/debug.c
+++ b/common/debug.c
@@ -29,6 +29,7 @@
  */
 #include <console.h>
 #include <debug.h>
+#include <stdarg.h>
 #include <stdio.h>
 
 /******************************************************************
@@ -73,7 +74,7 @@
 #if DEBUG
 void __dead2 do_panic(const char *file, int line)
 {
-		printf("PANIC in file: %s line: %d\n", file, line);
+		tf_printf("PANIC in file: %s line: %d\n", file, line);
 		while (1)
 			;
 }
@@ -87,7 +88,7 @@
 	/* x30 reports the next eligible instruction whereas we want the
 	 * place where panic() is invoked. Hence decrement by 4.
 	 */
-	printf("PANIC in PC location 0x%016X\n", pc_reg - 0x4);
+	tf_printf("PANIC in PC location 0x%016X\n", pc_reg - 0x4);
 	while (1)
 		;
 
diff --git a/common/tf_printf.c b/common/tf_printf.c
new file mode 100644
index 0000000..02461c0
--- /dev/null
+++ b/common/tf_printf.c
@@ -0,0 +1,141 @@
+/*
+ * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include <debug.h>
+#include <stdarg.h>
+#include <stdint.h>
+
+/***********************************************************
+ * The tf_printf implementation for all BL stages
+ ***********************************************************/
+static void unsigned_num_print(unsigned long int unum, unsigned int radix)
+{
+	/* Just need enough space to store 64 bit decimal integer */
+	unsigned char num_buf[20];
+	int i = 0 , rem;
+
+	do {
+		rem = unum % radix;
+		if (rem < 0xa)
+			num_buf[i++] = '0' + rem;
+		else
+			num_buf[i++] = 'a' + (rem - 0xa);
+	} while (unum /= radix);
+
+	while (--i >= 0)
+		putchar(num_buf[i]);
+}
+
+static void string_print(const char *str)
+{
+	while (*str)
+		putchar(*str++);
+}
+
+/*******************************************************************
+ * Reduced format print for Trusted firmware.
+ * The following formats are supported by this print
+ * %x - 32 bit hexadecimal format
+ * %llx and %lx -64 bit hexadecimal format
+ * %s - string format
+ * %d or %i - signed 32 bit decimal format
+ * %u - unsigned 32 bit decimal format
+ * %ld and %lld - signed 64 bit decimal format
+ * %lu and %llu - unsigned 64 bit decimal format
+ * Exits on all other formats.
+ *******************************************************************/
+
+void tf_printf(const char *fmt, ...)
+{
+	va_list args;
+	int bit64;
+	int64_t num;
+	uint64_t unum;
+	char *str;
+
+	va_start(args, fmt);
+	while (*fmt) {
+		bit64 = 0;
+
+		if (*fmt == '%') {
+			fmt++;
+			/* Check the format specifier */
+loop:
+			switch (*fmt) {
+			case 'i': /* Fall through to next one */
+			case 'd':
+				if (bit64)
+					num = va_arg(args, int64_t);
+				else
+					num = va_arg(args, int32_t);
+
+				if (num < 0) {
+					putchar('-');
+					unum = (unsigned long int)-num;
+				} else
+					unum = (unsigned long int)num;
+
+				unsigned_num_print(unum, 10);
+				break;
+			case 's':
+				str = va_arg(args, char *);
+				string_print(str);
+				break;
+			case 'x':
+				if (bit64)
+					unum = va_arg(args, uint64_t);
+				else
+					unum = va_arg(args, uint32_t);
+
+				unsigned_num_print(unum, 16);
+				break;
+			case 'l':
+				bit64 = 1;
+				fmt++;
+				goto loop;
+			case 'u':
+				if (bit64)
+					unum = va_arg(args, uint64_t);
+				else
+					unum = va_arg(args, uint32_t);
+
+				unsigned_num_print(unum, 10);
+				break;
+			default:
+				/* Exit on any other format specifier */
+				goto exit;
+			}
+			fmt++;
+			continue;
+		}
+		putchar(*fmt++);
+	}
+exit:
+	va_end(args);
+}
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 154c0f4..e996fd6 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -210,6 +210,7 @@
 	       image_info_t *image_data,
 	       entry_point_info_t *entry_point_info);
 extern const char build_message[];
+extern const char version_string[];
 
 void reserve_mem(uint64_t *free_base, size_t *free_size,
 		uint64_t addr, size_t size);
diff --git a/include/common/debug.h b/include/common/debug.h
index 5b496d7..c70109f 100644
--- a/include/common/debug.h
+++ b/include/common/debug.h
@@ -43,14 +43,14 @@
  * TODO : add debug levels.
  */
 #if DEBUG
- #define INFO(...)	printf("INFO: " __VA_ARGS__)
- #define WARN(...)	printf("WARN: " __VA_ARGS__)
+ #define INFO(...)	tf_printf("INFO: " __VA_ARGS__)
+ #define WARN(...)	tf_printf("WARN: " __VA_ARGS__)
 #else
  #define INFO(...)
  #define WARN(...)
 #endif
 
-#define ERROR(...)	printf("ERROR: " __VA_ARGS__)
+#define ERROR(...)	tf_printf("ERROR: " __VA_ARGS__)
 
 
 /* For the moment this Panic function is very basic, Report an error and
@@ -67,5 +67,7 @@
 #endif
 
 void print_string_value(char *s, unsigned long *mem);
+void tf_printf(const char *fmt, ...);
+
 
 #endif /* __DEBUG_H__ */
diff --git a/lib/stdlib/assert.c b/lib/stdlib/assert.c
index 8156a28..c614854 100644
--- a/lib/stdlib/assert.c
+++ b/lib/stdlib/assert.c
@@ -28,7 +28,7 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <stdio.h>
+#include <debug.h>
 
 /*
  * This is a basic implementation. This could be improved.
@@ -36,6 +36,6 @@
 void __assert (const char *function, const char *file, unsigned int line,
 		const char *assertion)
 {
-	printf("ASSERT: %s <%d> : %s\n\r", function, line, assertion);
+	tf_printf("ASSERT: %s <%d> : %s\n\r", function, line, assertion);
 	while(1);
 }
diff --git a/plat/fvp/aarch64/fvp_helpers.S b/plat/fvp/aarch64/fvp_helpers.S
index f856f46..3cd0b46 100644
--- a/plat/fvp/aarch64/fvp_helpers.S
+++ b/plat/fvp/aarch64/fvp_helpers.S
@@ -122,8 +122,11 @@
 	str	w2, [x1, #PSYSR_OFF]
 	ldr	w2, [x1, #PSYSR_OFF]
 	ubfx	w2, w2, #PSYSR_WK_SHIFT, #PSYSR_WK_MASK
-	cbnz	w2, warm_reset
-	mov	x0, x2
+	cmp	w2, #WKUP_PPONR
+	beq	warm_reset
+	cmp	w2, #WKUP_GICREQ
+	beq	warm_reset
+	mov	x0, #0
 	b	exit
 warm_reset:
 	/* ---------------------------------------------
diff --git a/plat/fvp/platform.mk b/plat/fvp/platform.mk
index 21ac1e7..b22a339 100644
--- a/plat/fvp/platform.mk
+++ b/plat/fvp/platform.mk
@@ -82,8 +82,3 @@
 				plat/fvp/aarch64/fvp_helpers.S			\
 				plat/fvp/aarch64/fvp_common.c			\
 				plat/fvp/drivers/pwrc/fvp_pwrc.c
-
-# Flag used by the platform port to determine the version of ARM GIC
-# architecture to use for interrupt management in EL3.
-ARM_GIC_ARCH		:=	2
-$(eval $(call add_define,ARM_GIC_ARCH))