/* VDB debugging utility

   author: Lin Gu <lingu@cs.virginia.edu>
 */

/* If the application does not use the GenericComm/GenericCommPromiscurious 
   component, and there is no handler for UART interrupts, the macro 
   NO_COMM must be defined so that the vdb handles the UART interrupt. Note 
   that, though any multiple modules can include "vdbavr.inc", only one 
   can have NO_COMM defined. 
   To define NO_COMM, just uncomment the line below. */
// #define NO_COMM


#include "vdbavr.h"
#include <string.h>
// #include "stdarg.h"
//#include <avr/io.h>
//#include <hw.h>
//#include <stdio.h>
//#include "stdio.inc"
//#include <stdlib.h>
//#include "commontknl.h"
//#include "bl_flash.h"

//extern int	vsprintf(char *__s, const char *__fmt, va_list ap);

#define SYSCLK (14745600UL/2)

#ifndef UCSRB
#ifdef UCSR1A /* atmega 128 */
#define UCSRA UCSR1A
#define UCSRB UCSR1B
#define UBRR UBRR1L
#define UDR UDR1
#endif
#endif

char cFlag;

/* UART running with mica2 convention:  115kbps, N-8-1
 */
int initUSART() {
  outp(0,UBRR0H);   // 57.6 KBps
  outp(15, UBRR0L);
  outp((1<<U2X),UCSR0A);  // double speed
  outp(((1 << UCSZ1) | (1 << UCSZ0)) , UCSR0C);  // 8 data-bits, 1 stop-bit
  outp(((1 << RXCIE) | (1 << TXCIE) | (1 << RXEN) | (1 << TXEN)) ,UCSR0B);  // Enable recieve/transmit/interrupts

  cFlag |= 1;

  return 1;
} // uartinit

/*
Not supported now to save code size.

int vdbPrintf(char *pstrFormat, ...)
{
  va_list args;
  char strOutBuf[100];
  uint8_t i;

  if (!(cFlag & 1))
    {
      initUSART();
      doze(10);
      //vdbPrintf("vdbavr.vdbPrintf: USART initialized\n"); /////// this does not seem to be printed out
    }

  va_start(args, pstrFormat);
  vsprintf(strOutBuf, pstrFormat, args);
  va_end(args);

  for (i=0; i<strlen(strOutBuf); i++)
    {
      // printf("%c", strOutBuf[i]);
      doze(90);
      outp(strOutBuf[i], UDR0); 
      sbi(UCSR0A, TXC);
    }

  doze(100);

     outp(0x00, UCSR0A);
    outp(0x00, UCSR0B);
    outp(0x00, UCSR0C);
  return 1;
} // vdbPrintf
*/

//#define PUT_WAIT 6500 /* was 110 */
#define PUT_WAIT 1500

void usartPut(uint8_t c)
{
  doze(PUT_WAIT);
      outp(c, UDR0); 
      sbi(UCSR0A, TXC);
}

#ifdef NO_COMM
  TOSH_INTERRUPT(SIG_UART0_TRANS) {
    return;
    // signal UART.putDone();
  }
#endif

void usartPutChipHex(uint8_t cChip)
{
  if (cChip > 9)
    {
      usartPut('a'+cChip-10);
    }
  else
    {
      usartPut('0'+cChip);
    }
}

void usartPutHex(uint8_t c)
{
  usartPutChipHex(c>>4);
  usartPutChipHex(c&0xf);
}

void usartPutLong(uint32_t l)
{
  uint8_t *pcByte = ((char *)(&l)) + 3;

  if (!l)
    {
      usartPut('0');
      return;
    }

  usartPut('0');
  usartPut('x');
  usartPutHex(*pcByte);
  pcByte--;
  usartPutHex(*pcByte);
  pcByte--;
  usartPutHex(*pcByte);
  pcByte--;
  usartPutHex(*pcByte);
}

void usartPutInt(uint16_t l)
{
  uint8_t *pcByte = ((char *)(&l)) + 1;

  if (!l)
    {
      usartPut('.');
      return;
    }

  usartPut('0');
  usartPut('x');
  usartPutHex(*pcByte);
  pcByte--;
  usartPutHex(*pcByte);
}

/*int vdbPrintf(char *format, ...)
{
  return vdbPrint(format);
}

#ifndef TKNL_RESTRICTED_SIZE
uint8_t vdbPrint(char *pstr)
{
  uint8_t i;

  if (!pstr) return 0;

  if (!(cFlag & 1))
    {
      initUSART();
      doze(10);
    }

  for (i=0; i<strlen(pstr); i++)
    {
      doze(90);
      outp(pstr[i], UDR0); 
      sbi(UCSR0A, TXC);
    }

  doze(100);
  return 1;
}

#endif
*/

void vdbPrintln4(char *pstr, uint32_t l1, uint32_t l2, uint32_t l3, uint32_t l4)
{
  print(pstr, l1, l2, l3, l4);
}

uint8_t print(char *pstr, uint32_t l1, uint32_t l2, uint32_t l3, uint32_t l4)
{
  uint8_t i;
	
//cli();	

  if (!(cFlag & 1))
    {
      initUSART();
      doze(10);
    }

  if (pstr) 
    {
      for (i=0; i<strlen(pstr); i++)
	{
	  usartPut(pstr[i]);
	}
    } // if pstr

  usartPut(' ');
  usartPutLong(l1);
  usartPut(' ');
  usartPutLong(l2);
  usartPut(' ');
  usartPutLong(l3);
  usartPut(' ');
  usartPutLong(l4);
  usartPut('\n');
  doze(100);
//	sei();

  return 1;
} // vdbPrintln4

uint8_t vdbPrintln2Int(uint16_t l1, uint16_t l2)
{
  if (!(cFlag & 1))
    {
      initUSART();
      doze(10);
    }

  usartPut(' ');
  usartPut('=');
  usartPut(' ');
  usartPutInt(l1);
  usartPut(' ');
  usartPutInt(l2);
  usartPut('\n');

  doze(100);

  return 1;
} // vdbPrintln2Int

void blueScreen(uint8_t n) 
{
  blueLong(n, 0, 0, 0);
}

void blueLong(uint8_t cErrNo, uint32_t l1, uint32_t l2, uint32_t l3)
{
  for (;;)
    {
      print("t-k ctrl", cErrNo, l1, l2, l3);
      l3 = 800;
      while (l3)
	doze(l3--);
      //      longWait();
    } // for
}

void doze(int nMicroSec) {
    while (nMicroSec > 0) {
      asm volatile  ("nop" ::);
      asm volatile  ("nop" ::);
      asm volatile  ("nop" ::);
      asm volatile  ("nop" ::);
      asm volatile  ("nop" ::);
      asm volatile  ("nop" ::);
      nMicroSec--;
    }
}

/* print stack information, including the stack pointer and
   the top elements in the stack.
 */
inline void stackInfo()
{
  asm volatile (
    "in r24, 0x3d" "\n\t"
    "in r25, 0x3e" "\n\t"
    "eor r26, r26" "\n\t"
    "eor r27, r27" "\n\t"
    "call vdbPrintln2Int" "\n\t"
    :
    :
  );
} // stackInfo

uint16_t getSP()
{
  uint16_t nSP; // asm ("r24");

  nSP = 0;

  asm volatile (
    "in %A0, 0x3d" "\n\t"
    "in %B0, 0x3e" "\n\t"
    : "=r" (nSP)
    :
  );

  return nSP;
} // getSP

#define isStackBelow(x) (!(isStackAbove(x)))

char isStackAbove(uint16_t nLowerBoundary)
{
  if (getSP() >= nLowerBoundary)
	return 1;
  else
	return 0;
} // isStackAbove

/* write a byte to EEPROM */
void _eepromWriteByte(uint8_t *addr, uint8_t val)
  {
/* addr = r25:r24, val = r22 */
    asm volatile (
	"push	r22"  "\n\t"
	"push	r23"  "\n\t"
	"push	r24"  "\n\t"
	"push	r25"  "\n\t"
	"push	r0"  "\n\t"

	".global vdb_eeprom_write_byte" "\n\t"
	"vdb_eeprom_write_byte:" "\n\t"
	//"sbic	_SFR_IO_ADDR(EECR), EEWE" "\n\t"
	"sbic 0x1c, 1" "\n\t"
	"rjmp	vdb_eeprom_write_byte" "\n\t"
	"out	0x1f, %B0"  "\n\t"
	"out	0x1e, %A0" "\n\t"
	"out	0x1d, %1" "\n\t"
	"in	r0, 0x3f" "\n\t"
	"cli"  "\n\t"
	"sbi	0x1c, 2" "\n\t"
	"sbi	0x1c, 1" "\n\t"
	"out	0x3f, r0" "\n\t"
	"pop r0" "\n\t"
	"pop r25" "\n\t"
	"pop r24" "\n\t"
	"pop r23" "\n\t"
	"pop r22" "\n\t"
	:
	: "d" (addr),
	  "r" (val)
	);
    //#undef val
    return;
  }

/* read one byte from EEPROM */
uint8_t _eepromReadByte(const uint8_t *addr)
  {

/* addr = r25:r24, result = r25(=0):r24 */
    register uint16_t r asm("r24");

    asm volatile (
		  //	"lds r24, eeprom_addr_l" "\n\t"
		  //	"lds r25, eeprom_addr_h" "\n\t"
	".global vdb_eeprom_read_byte"  "\n\t"
	"vdb_eeprom_read_byte:"  "\n\t"
	"sbic	0x1c, 1"  "\n\t"
	"rjmp	vdb_eeprom_read_byte"  "\n\t"	/* make sure EEPROM is ready */
	//#ifdef EEARH
//	"ldi r22, 1" "\n\t"
	"out	0x1f, %B0"  "\n\t"
	//#endif
//	"ldi r22, 0" "\n\t"
	"out	0x1e, %A0"  "\n\t"
	"sbi	0x1c, 0"  "\n\t"
	"clr	r25"  "\n\t"		/* gcc wants result extended to "int"? */
	"in	r24,0x1d"  "\n\t"
	//	"sts r24, eeprom_data" "\n\t"
	:
	: "d" (addr)
	);

    return r;
  }

 char logEepromByte(unsigned int addr, char this_data){
//    eeprom_addr_h = 1;
//    eeprom_addr_l = 0;
//    eeprom_data = this_data;
    _eepromWriteByte((uint8_t *)addr, this_data);
    // print("written", 0,0,0,0);
    return 1;
  }

 char readEepromByte(unsigned int addr)
    {
      char c;
	c = _eepromReadByte((uint8_t *)addr);
//      print("reading", gnRead, pcRead, c, eeprom_data);
      // gnRead++;

      return c;
  }

void _showLed(char c)
{
  asm volatile (
		"push r24" "\n\t"
		"sbi 0x1a, 2" "\n\t"
		"sbi 0x1a, 1" "\n\t"
		"sbi 0x1a, 0" "\n\t"
		"sbi 0x1b, 0" "\n\t"
		"sbi 0x1b, 1" "\n\t"
		"sbi 0x1b, 2" "\n\t"
		"mov r24, %0" "\n\t"
		"sbrc r24, 0" "\n\t"
		"cbi 0x1b, 0" "\n\t"
		"sbrc r24, 1" "\n\t"
		"cbi 0x1b, 1" "\n\t"
		"sbrc r24, 2" "\n\t"
		"cbi 0x1b, 2" "\n\t"
 		"pop r24" "\n\t"
		:
		: "r" (c)
		);
}

void assert(int n)
{
  while (!n)
    {
      // cli();
      asm volatile ("cli"::);
      _showLed(7);  
    }
}

void assertNum(int n, char cLed)
{
  while (!n)
    {
      // cli();
      asm volatile ("cli"::);
      _showLed(cLed);  
    }
}

#undef NO_COMM

     /*									tab:4
      *
      *
      * "Copyright (c) 2004-2005 The the University  of Virginia.  
      * All rights reserved.
      *
      * Permission to use, copy, modify, and distribute this software and its
      * documentation for any purpose, without fee, and without written agreement is
      * hereby granted, provided that the above copyright notice, the following
      * two paragraphs and the author appear in all copies of this software.
      * 
      * IN NO EVENT SHALL THE UNIVERSITY OF VIRGINIA BE LIABLE TO ANY PARTY FOR
      * DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR CONSEQUENTIAL DAMAGES ARISING OUT
      * OF THE USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY OF
      * VIRGINIA HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
      * 
      * THE UNIVERSITY OF VIRGINIA SPECIFICALLY DISCLAIMS ANY WARRANTIES,
      * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY
      * AND FITNESS FOR A PARTICULAR PURPOSE.  THE SOFTWARE PROVIDED HEREUNDER IS
      * ON AN "AS IS" BASIS, AND THE UNIVERSITY OF VIRGINIA HAS NO OBLIGATION TO
      * PROVIDE MAINTENANCE, SUPPORT, UPDATES, ENHANCEMENTS, OR MODIFICATIONS."
      *
      */