char char_to_hex(char value) {
   if (value > 9)
      return '7' + value;
   return '0' + value;
}
void DumpText(char *text) {
   while(*text) {
      USART3->TDR = *text;
      text++;
      while (!(USART3->ISR & USART_ISR_TC)) {};
   }
}
void DumpHexDWord(uint32_t Data) {
   char txt[9];
   txt[0] = char_to_hex((Data >> 28) & 0x0F);
   txt[1] = char_to_hex((Data >> 24) & 0x0F);
   txt[2] = char_to_hex((Data >> 20) & 0x0F);
   txt[3] = char_to_hex((Data >> 16) & 0x0F);
   txt[4] = char_to_hex((Data >> 12) & 0x0F);
   txt[5] = char_to_hex((Data >> 8) & 0x0F);
   txt[6] = char_to_hex((Data >> 4) & 0x0F);
   txt[7] = char_to_hex((Data) & 0x0F);
   txt[8] = 0;
   DumpText(txt);
}
void BusFault_Handler_c(unsigned int * hardfault_args) {
   unsigned int stacked_r0;
   unsigned int stacked_r1;
   unsigned int stacked_r2;
   unsigned int stacked_r3;
   unsigned int stacked_r12;
   unsigned int stacked_lr;
   unsigned int stacked_pc;
   unsigned int stacked_psr;

   stacked_r0 = ((unsigned long) hardfault_args[0]);
   stacked_r1 = ((unsigned long) hardfault_args[1]);
   stacked_r2 = ((unsigned long) hardfault_args[2]);
   stacked_r3 = ((unsigned long) hardfault_args[3]);

   stacked_r12 = ((unsigned long) hardfault_args[4]);
   stacked_lr = ((unsigned long) hardfault_args[5]);
   stacked_pc = ((unsigned long) hardfault_args[6]);
   stacked_psr = ((unsigned long) hardfault_args[7]);

   DumpText ("\r\n[Bus fault handler - all numbers in hex]\r\n");
   DumpText ("R0 = ");
   DumpHexDWord(stacked_r0);
   DumpText ("\r\nR1 = ");
   DumpHexDWord(stacked_r1);
   DumpText ("\r\nR2 = ");
   DumpHexDWord(stacked_r2);
   DumpText ("\r\nR3 = ");
   DumpHexDWord(stacked_r3);
   DumpText ("\r\nR12 = ");
   DumpHexDWord(stacked_r12);
   DumpText ("\r\nLR [R14] (subroutine call return address) = ");
   DumpHexDWord(stacked_lr);
   DumpText ("\r\nPC [R15] (program counter) = ");
   DumpHexDWord(stacked_pc);
   DumpText ("\r\nPSR = ");
   DumpHexDWord(stacked_psr);
//   DumpText ("\r\nBFAR = ");
//   DumpHexDWord((*((volatile unsigned long *)(0xE000ED38))));
//   DumpText ("\r\nCFSR = ");
//   DumpHexDWord((*((volatile unsigned long *)(0xE000ED28))));
//   DumpText ("\r\nHFSR = ");
//   DumpHexDWord((*((volatile unsigned long *)(0xE000ED2C))));
//   DumpText ("\r\nDFSR = ");
//   DumpHexDWord((*((volatile unsigned long *)(0xE000ED30))));
//   DumpText ("\r\nAFSR = ");
//   DumpHexDWord((*((volatile unsigned long *)(0xE000ED3C))));
   DumpText ("\r\nSCB_SHCSR = ");
   DumpHexDWord(SCB->SHCSR);
   DumpText("\r\n");
   while (1) { };
}

void HardFault_Handler(void) {
   console_put("SHIT! - HardFault\r\n");
   while(1) {}
}
void MemManage_Handler(void) {
   console_put("SHIT! - MemManageFault\r\n");
   while(1) {}
}
void UsageFault_Handler(void) {
   console_put("SHIT! - UsageFault\r\n");
   while(1) {}
}