Bootloader: small refactoring

bootloaders/zero/sam_ba_monitor.c

@@ -174,161 +174,159 @@ void sam_ba_monitor_run(void)
 {
 	ptr_data = NULL;
 	command = 'z';
-	
-	// Start waiting some cmd
-	while (1)
+	while (1) {
+		sam_ba_monitor_loop();
+	}
+}
+
+void sam_ba_monitor_loop(void)
+{
+	length = ptr_monitor_if->getdata(data, SIZEBUFMAX);
+	ptr = data;
+	for (i = 0; i < length; i++, ptr++)
 	{
-		length = ptr_monitor_if->getdata(data, SIZEBUFMAX);
-		ptr = data;
-		for (i = 0; i < length; i++)
+		if (*ptr == 0xff) continue;
+
+		if (*ptr == '#')
 		{
-			if (*ptr != 0xff)
+			if (b_terminal_mode)
+			{
+				ptr_monitor_if->putdata("\n\r", 2);
+			}
+			if (command == 'S')
 			{
-				if (*ptr == '#')
+				//Check if some data are remaining in the "data" buffer
+				if(length>i)
 				{
-					if (b_terminal_mode)
-					{
-						ptr_monitor_if->putdata("\n\r", 2);
-					}
-					if (command == 'S')
-					{
-						//Check if some data are remaining in the "data" buffer
-						if(length>i)
-						{
-							//Move current indexes to next avail data (currently ptr points to "#")
-							ptr++;
-							i++;
-							//We need to add first the remaining data of the current buffer already read from usb
-							//read a maximum of "current_number" bytes
-							u32tmp=min((length-i),current_number);
-							memcpy(ptr_data, ptr, u32tmp);
-							i += u32tmp;
-							ptr += u32tmp;
-							j = u32tmp;
-						}
-						//update i with the data read from the buffer
-						i--;
-						ptr--;
-						//Do we expect more data ?
-						if(j<current_number)
-							ptr_monitor_if->getdata_xmd(ptr_data, current_number-j);
-						
-						__asm("nop");
-					}
-					else if (command == 'R')
-					{
-						ptr_monitor_if->putdata_xmd(ptr_data, current_number);
-					}
-					else if (command == 'O')
-					{
-						*ptr_data = (char) current_number;
-					}
-					else if (command == 'H')
-					{
-						*((uint16_t *) ptr_data) = (uint16_t) current_number;
-					}
-					else if (command == 'W')
-					{
-						*((int *) ptr_data) = current_number;
-					}
-					else if (command == 'o')
-					{
-						sam_ba_putdata_term(ptr_data, 1);
-					}
-					else if (command == 'h')
-					{
-						current_number = *((uint16_t *) ptr_data);
-						sam_ba_putdata_term((uint8_t*) &current_number, 2);
-					}
-					else if (command == 'w')
-					{
-						current_number = *((uint32_t *) ptr_data);
-						sam_ba_putdata_term((uint8_t*) &current_number, 4);
-					}
-					else if (command == 'G')
-					{
-						call_applet(current_number);
-						/* Rebase the Stack Pointer */
-						__set_MSP(sp);
-						cpu_irq_enable();
-						if (b_sam_ba_interface_usart) {
-							ptr_monitor_if->put_c(0x6);
-						}
-					}
-					else if (command == 'T')
-					{
-						b_terminal_mode = 1;
-						ptr_monitor_if->putdata("\n\r", 2);
-					}
-					else if (command == 'N')
-					{
-						if (b_terminal_mode == 0)
-						{
-							ptr_monitor_if->putdata("\n\r", 2);
-						}
-						b_terminal_mode = 0;
-					}
-					else if (command == 'V')
-					{
-						ptr_monitor_if->putdata("v", 1);
-						ptr_monitor_if->putdata((uint8_t *) RomBOOT_Version,
-								strlen(RomBOOT_Version));
-						ptr_monitor_if->putdata(" ", 1);
-						ptr = (uint8_t*) &(__DATE__);
-						i = 0;
-						while (*ptr++ != '\0')
-							i++;
-						ptr_monitor_if->putdata((uint8_t *) &(__DATE__), i);
-						ptr_monitor_if->putdata(" ", 1);
-						i = 0;
-						ptr = (uint8_t*) &(__TIME__);
-						while (*ptr++ != '\0')
-							i++;
-						ptr_monitor_if->putdata((uint8_t *) &(__TIME__), i);
-						ptr_monitor_if->putdata("\n\r", 2);
-					}
-
-					command = 'z';
-					current_number = 0;
-
-					if (b_terminal_mode)
-					{
-						ptr_monitor_if->putdata(">", 1);
-					}
+					//Move current indexes to next avail data (currently ptr points to "#")
+					ptr++;
+					i++;
+					//We need to add first the remaining data of the current buffer already read from usb
+					//read a maximum of "current_number" bytes
+					u32tmp=min((length-i),current_number);
+					memcpy(ptr_data, ptr, u32tmp);
+					i += u32tmp;
+					ptr += u32tmp;
+					j = u32tmp;
+				}
+				//update i with the data read from the buffer
+				i--;
+				ptr--;
+				//Do we expect more data ?
+				if(j<current_number)
+					ptr_monitor_if->getdata_xmd(ptr_data, current_number-j);
+				
+				__asm("nop");
+			}
+			else if (command == 'R')
+			{
+				ptr_monitor_if->putdata_xmd(ptr_data, current_number);
+			}
+			else if (command == 'O')
+			{
+				*ptr_data = (char) current_number;
+			}
+			else if (command == 'H')
+			{
+				*((uint16_t *) ptr_data) = (uint16_t) current_number;
+			}
+			else if (command == 'W')
+			{
+				*((int *) ptr_data) = current_number;
+			}
+			else if (command == 'o')
+			{
+				sam_ba_putdata_term(ptr_data, 1);
+			}
+			else if (command == 'h')
+			{
+				current_number = *((uint16_t *) ptr_data);
+				sam_ba_putdata_term((uint8_t*) &current_number, 2);
+			}
+			else if (command == 'w')
+			{
+				current_number = *((uint32_t *) ptr_data);
+				sam_ba_putdata_term((uint8_t*) &current_number, 4);
+			}
+			else if (command == 'G')
+			{
+				call_applet(current_number);
+				/* Rebase the Stack Pointer */
+				__set_MSP(sp);
+				cpu_irq_enable();
+				if (b_sam_ba_interface_usart) {
+					ptr_monitor_if->put_c(0x6);
 				}
-				else
+			}
+			else if (command == 'T')
+			{
+				b_terminal_mode = 1;
+				ptr_monitor_if->putdata("\n\r", 2);
+			}
+			else if (command == 'N')
+			{
+				if (b_terminal_mode == 0)
 				{
-					if (('0' <= *ptr) && (*ptr <= '9'))
-					{
-						current_number = (current_number << 4) | (*ptr - '0');
-
-					}
-					else if (('A' <= *ptr) && (*ptr <= 'F'))
-					{
-						current_number = (current_number << 4)
-								| (*ptr - 'A' + 0xa);
-
-					}
-					else if (('a' <= *ptr) && (*ptr <= 'f'))
-					{
-						current_number = (current_number << 4)
-								| (*ptr - 'a' + 0xa);
-
-					}
-					else if (*ptr == ',')
-					{
-						ptr_data = (uint8_t *) current_number;
-						current_number = 0;
-
-					}
-					else
-					{
-						command = *ptr;
-						current_number = 0;
-					}
+					ptr_monitor_if->putdata("\n\r", 2);
 				}
-				ptr++;
+				b_terminal_mode = 0;
+			}
+			else if (command == 'V')
+			{
+				ptr_monitor_if->putdata("v", 1);
+				ptr_monitor_if->putdata((uint8_t *) RomBOOT_Version,
+						strlen(RomBOOT_Version));
+				ptr_monitor_if->putdata(" ", 1);
+				ptr = (uint8_t*) &(__DATE__);
+				i = 0;
+				while (*ptr++ != '\0')
+					i++;
+				ptr_monitor_if->putdata((uint8_t *) &(__DATE__), i);
+				ptr_monitor_if->putdata(" ", 1);
+				i = 0;
+				ptr = (uint8_t*) &(__TIME__);
+				while (*ptr++ != '\0')
+					i++;
+				ptr_monitor_if->putdata((uint8_t *) &(__TIME__), i);
+				ptr_monitor_if->putdata("\n\r", 2);
+			}
+
+			command = 'z';
+			current_number = 0;
+
+			if (b_terminal_mode)
+			{
+				ptr_monitor_if->putdata(">", 1);
+			}
+		}
+		else
+		{
+			if (('0' <= *ptr) && (*ptr <= '9'))
+			{
+				current_number = (current_number << 4) | (*ptr - '0');
+			}
+			else if (('A' <= *ptr) && (*ptr <= 'F'))
+			{
+				current_number = (current_number << 4) | (*ptr - 'A' + 0xa);
+			}
+			else if (('a' <= *ptr) && (*ptr <= 'f'))
+			{
+				current_number = (current_number << 4) | (*ptr - 'a' + 0xa);
+			}
+			else if (*ptr == ',')
+			{
+				ptr_data = (uint8_t *) current_number;
+				current_number = 0;
+			}
+			else
+			{
+				command = *ptr;
+				current_number = 0;
 			}
 		}
 	}
 }
+
+
+
+