static void musb_peri_ep0_stall(void)
{
	u16 csr0;

	csr0 = readw(&musbr->ep[0].ep0.csr0);
	csr0 |= MUSB_CSR0_P_SENDSTALL;
	writew(csr0, &musbr->ep[0].ep0.csr0);
	if ((debug_setup) && (debug_level > 1))
		serial_printf("INFO : %s stall\n", __PRETTY_FUNCTION__);
}

void cmd_tasks(SerialDevice *io, int argc, char *argv[])
{
    serial_printf(io, "name: priority, mode, stack: used/size\r\n");
    serial_printf(io, "static tasks:\r\n");
    // static tasks
    Task_Object *task;
    int i;
    for (i = 0; i < Task_Object_count(); i++) {
        task = Task_Object_get(NULL, i);
        task_print(io, task);
    }
    // dymanmically allocated tasks
    serial_printf(io, "dynamic tasks:\r\n");
    task = Task_Object_first();
    while (task) {
        task_print(io, task);
        task = Task_Object_next(task);
    }
}

void sim800_send_http(char * tx_buffer, int tx_size, SIM800_MIME mime_type){
	if(!sim800_initialised){
		serial_printf(cli_stdout, "sim800 not initialised",0);
	}else{
		sim800_locked = 1;
		sim800_init_http(mime_type);
		sim800_buffermessage_http(tx_buffer, tx_size);
		sim800_locked = 0;
	}
}

static void debug_urb_buffer(char *prefix, struct usb_endpoint_instance *ep)
{
#ifdef DEBUG
	int num;
	static char buf[128];

	if (!ep->tx_urb) {
		serial_printf("no tx_urb\n");
		return;
	}

	num = MIN(ep->tx_urb->actual_length - ep->sent, ep->tx_packetSize);

	memset(buf, 0, sizeof(buf));
	strncpy(buf, ep->tx_urb->buffer + ep->sent, num);

	serial_printf("%s(%d:%s)\n", prefix, num, buf);
#endif
}

int udc_endpoint_write(struct usb_endpoint_instance *endpoint)
{
	int ret = 0;

	/* Transmit only if the hardware is available */
	if (endpoint->tx_urb && endpoint->state == 0) {
		unsigned int ep = endpoint->endpoint_address &
			USB_ENDPOINT_NUMBER_MASK;

		u16 peri_txcsr = readw(&musbr->ep[ep].epN.txcsr);

		/* Error conditions */
		if (peri_txcsr & MUSB_TXCSR_P_UNDERRUN) {
			peri_txcsr &= ~MUSB_TXCSR_P_UNDERRUN;
			writew(peri_txcsr, &musbr->ep[ep].epN.txcsr);
		}

		if (debug_level > 1)
			musb_print_txcsr(peri_txcsr);

		/* Check if a packet is waiting to be sent */
		if (!(peri_txcsr & MUSB_TXCSR_TXPKTRDY)) {
			u32 length;
			u8 *data;
			struct urb *urb = endpoint->tx_urb;
			unsigned int remaining_packet = urb->actual_length -
				endpoint->sent;

			if (endpoint->tx_packetSize < remaining_packet)
				length = endpoint->tx_packetSize;
			else
				length = remaining_packet;

			data = (u8 *) urb->buffer;
			data += endpoint->sent;

			/* common musb fifo function */
			write_fifo(ep, length, data);

			musb_peri_tx_ready(ep);

			endpoint->last = length;
			/* usbd_tx_complete will take care of updating 'sent' */
			usbd_tx_complete(endpoint);
		}
	} else {
		if (debug_level > 0)
			serial_printf("ERROR : %s Problem with urb %p "
				      "or ep state %d\n",
				      __PRETTY_FUNCTION__,
				      endpoint->tx_urb, endpoint->state);
	}

	return ret;
}

void serial_print_header(char *board,char *cpu_revision)
{
	for(int i = 0; i < 8; i++)
		serial_printf("----------");
	serial_printf("\n");
	serial_printf("Beryllium %s - version %s (from %s)\n",BERYLLIUM_RELEASE,BERYLLIUM_VERSION,BERYLLIUM_SOURCE);
	serial_printf("Output Device: Serial\n");
	serial_printf("Architecture: %s (%s)\n",ARCH_STRING,board);
	serial_printf("Processor: %s\n",cpu_revision);
	for(int i = 0; i < 8; i++)
		serial_printf("----------");
	serial_printf("\n");
}

pdirectory* vmmngr_create_address_space()
{
	pdirectory* dir = (pdirectory*)pmmngr_alloc_block();
	if (!dir)
		return 0;

	serial_printf("creating addr space at: %h\n", dir);

	memset(dir, 0, sizeof(pdirectory));
	return dir;
}

irom void setup()
{
	serial_begin(115200);

	//disable the wifi
	wifi_set_opmode(NULL_MODE);

	pinMode(ledPower,OUTPUT);
	digitalWrite(ledPower,HIGH);
	serial_printf("Sharp GP2Y1010AU0F Particle Sensor Example\r\n");
}

void serial_print_header(char *board,char *cpu_revision)
{
	for(int i = 0; i < 8; i++)
		serial_printf("----------");
	serial_printf("\n");
	serial_printf("Beryllium2 Kernel Version %s-%s:%s %s\n",SYSTEM_VERSION,SYSTEM_RELEASE,__DATE__,__TIME__);
	serial_printf("Output Device: Serial\n");
	serial_printf("Architecture: %s (%s)\n",ARCH_STRING,board);
	serial_printf("Processor: %s\n",cpu_revision);
	for(int i = 0; i < 8; i++)
		serial_printf("----------");
	serial_printf("\n");
}

Uint32 vfs_run_tests()
{
    serial_string( "running VFS tests\n\r" );
    Uint32 test_failures = 0;
    test_failures += test_raw_read();

    serial_printf("==> VFS tests completed, %d failures.\n\r",
                  test_failures );

    return test_failures;
}

void navigation_task()
{
	bool logging_enabled = false;
	navigation_init();

#ifdef FLASH_ENABLED
	if (flash_init() == 0) {
		logging_enabled = true;
	}

    if (logging_enabled) {
        if (!log_init()) {
            serial_printf(cli_stdout, "log_init failed\n");
            log_reset();
        }
    }

    //We look if we have mavlink mission item logged, in case we just suffered a crash

    uint32_t time = 0;
    char name[4]; //Name is a 3 characters long string
    mission_item_list_t item_list;
    if(log_read_mavlink_item_list(&item_list, &time, &name, &pos_counter))
    	navigation_restore_mission_items(item_list);

#endif

    unsigned int loops_since_stop = 0;
	while(1){

//		navigation_update_target();
		navigation_update_current_target();
		navigation_update_position();
		navigation_update_state();
		navigation_arm_disarm();
		navigation_move();

		if(navigation_rover_moving() || loops_since_stop < 62) // trasmit messages after stop for a duration such that
																//at least one msg with speed zero is tx'd via LoRa
		{
			comm_mavlink_broadcast(navigation_pack_mavlink_hud());
			comm_mavlink_broadcast(navigation_pack_rc_channels_scaled());
		}

		if(!navigation_rover_moving())
			loops_since_stop++;
		else
			loops_since_stop = 0;


		Task_sleep(500);

	}
}

static OS_STATUS _show_msgQ_info (MSG_QUE *pMsgQ, int unused)
{
    serial_printf("ID: %8X  cur_num: %3d  max_num: %3d  wait_send_task: %d wait_recv_task: %d\n",
                  pMsgQ,
                  pMsgQ->cur_num,
                  pMsgQ->max_num,
                  dlist_count (&pMsgQ->wait_send_list),
                  dlist_count (&pMsgQ->wait_recv_list)
                 );
    return OS_STATUS_OK;
}

static void musb_peri_ep0_set_address(void)
{
	u8 faddr;
	writeb(udc_device->address, &musbr->faddr);

	/* Verify */
	faddr = readb(&musbr->faddr);
	if (udc_device->address == faddr) {
		SET_EP0_STATE(IDLE);
		usbd_device_event_irq(udc_device, DEVICE_ADDRESS_ASSIGNED, 0);
		if ((debug_setup) && (debug_level > 1))
			serial_printf("INFO : %s Address set to %d\n",
				      __PRETTY_FUNCTION__, udc_device->address);
	} else {
		if (debug_level > 0)
			serial_printf("ERROR : %s Address missmatch "
				      "sw %d vs hw %d\n",
				      __PRETTY_FUNCTION__,
				      udc_device->address, faddr);
	}
}

static void musb_peri_softconnect(void)
{
    u8 power, devctl;
    u8 intrusb;
    u16 intrrx, intrtx;

    /* Power off MUSB */
    power = readb(&musbr->power);
    power &= ~MUSB_POWER_SOFTCONN;
    writeb(power, &musbr->power);

    /* Read intr to clear */
    intrusb = readb(&musbr->intrusb);
    intrrx = readw(&musbr->intrrx);
    intrtx = readw(&musbr->intrtx);

    udelay(1000 * 1000); /* 1 sec */

    /* Power on MUSB */
    power = readb(&musbr->power);
    power |= MUSB_POWER_SOFTCONN;
    /*
     * The usb device interface is usb 1.1
     * Disable 2.0 high speed by clearring the hsenable bit.
     */
    power &= ~MUSB_POWER_HSENAB;
    writeb(power, &musbr->power);

    /* Check if device is in b-peripheral mode */
    devctl = readb(&musbr->devctl);
    if (!(devctl & MUSB_DEVCTL_BDEVICE) ||
            (devctl & MUSB_DEVCTL_HM)) {
        serial_printf("ERROR : Unsupport USB mode\n");
        serial_printf("Check that mini-B USB cable is attached "
                      "to the device\n");
    }

    if (debug_setup && (debug_level > 1))
        musb_db_regs();
}

static void musb_peri_ep0_tx(void)
{
	u16 csr0;
	int transfer_size = 0;
	unsigned int p, pm;

	csr0 = readw(&musbr->ep[0].ep0.csr0);

	/* Check for pending tx */
	if (csr0 & MUSB_CSR0_TXPKTRDY)
		goto end;

	/* Check if this is the last packet sent */
	if (ep0_endpoint->sent >= ep0_urb->actual_length) {
		SET_EP0_STATE(IDLE);
		goto end;
	}

	transfer_size = ep0_urb->actual_length - ep0_endpoint->sent;
	/* Is the transfer size negative ? */
	if (transfer_size <= 0) {
		if (debug_level > 0)
			serial_printf("ERROR : %s problem with the"
				      " transfer size %d\n",
				      __PRETTY_FUNCTION__,
				      transfer_size);
		SET_EP0_STATE(IDLE);
		goto end;
	}

	/* Truncate large transfers to the fifo size */
	if (transfer_size > ep0_endpoint->tx_packetSize)
		transfer_size = ep0_endpoint->tx_packetSize;

	write_fifo(0, transfer_size, &ep0_urb->buffer[ep0_endpoint->sent]);
	ep0_endpoint->sent += transfer_size;

	/* Done or more to send ? */
	if (ep0_endpoint->sent >= ep0_urb->actual_length)
		musb_ep0_tx_ready_and_last();
	else
		musb_ep0_tx_ready();
	
	/* Wait a bit */
	musb_peri_wait_tx_done(0);

	if ((ep0_endpoint->sent >= ep0_urb->actual_length) && (p < pm))
		SET_EP0_STATE(IDLE);

end:
	return;
}

int main(){
	int lineCounter=1;
	led_init();
	USART2_init(9600);
	lcd_init();
	
	serial_puts(USART2_Serial,"\nSystem ready\n");
	serial_puts(LCD_Serial,"\fSystem ready\n");

	while(1){
		serial_printf(USART2_Serial,"%d$ ",lineCounter);
		serial_gets(USART2_Serial,mybf,80);
		serial_printf(USART2_Serial,"%s\n",mybf);
		if(sscanf(mybf,"%s",wordBuffer) > 0){
			serial_printf(USART2_Serial,"word: %s\n",wordBuffer);
			serial_printf(LCD_Serial,"\fword: %s\n",wordBuffer);
			serial_printf(USART2_Serial,"characters: %d\n",strlen(wordBuffer));
		}
		lineCounter++;
	}
	return 0;
}

/*==============================================================================
 * - gui_job_add()
 *
 * - touch screen driver call this to post message
 */
OS_STATUS gui_job_add (GUI_COOR *pCoor, GUI_MSG_TYPE type)
{
    GUI_MSG msg = {type, *pCoor};

    if (msgQ_send(&_G_msg_queue, &msg, sizeof(msg), NO_WAIT)
            == OS_STATUS_ERROR) {
        serial_printf("gui lost message!\n");
        _G_msg_lost++;
        return OS_STATUS_ERROR;
    }

    return OS_STATUS_OK;
}

static void musb_peri_reset(void)
{
	if ((debug_setup) && (debug_level > 1))
		serial_printf("INFO : %s reset\n", __PRETTY_FUNCTION__);

	if (ep0_endpoint)
		ep0_endpoint->endpoint_address = 0xff;

	/* Sync sw and hw addresses */
	writeb(udc_device->address, &musbr->faddr);

	SET_EP0_STATE(IDLE);
}

static bool ICACHE_FLASH_ATTR twi_write_start(void)
{
	SCL_HIGH();
	SDA_HIGH();
	if (SDA_READ() == 0) {
		serial_printf("twi write start sda read false\r\n");
		return false;
	}
	twi_delay(twi_dcount);
	SDA_LOW();
	twi_delay(twi_dcount);
	return true;
}

static void musb_db_regs(void)
{
	u8 b;
	u16 w;

	b = readb(&musbr->faddr);
	serial_printf("\tfaddr   0x%2.2x\n", b);

	b = readb(&musbr->power);
	musb_print_pwr(b);

	w = readw(&musbr->ep[0].ep0.csr0);
	musb_print_csr0(w);

	b = readb(&musbr->devctl);
	musb_print_devctl(b);

	b = readb(&musbr->ep[0].ep0.configdata);
	musb_print_config(b);

	w = readw(&musbr->frame);
	serial_printf("\tframe   0x%4.4x\n", w);

	b = readb(&musbr->index);
	serial_printf("\tindex   0x%2.2x\n", b);

	w = readw(&musbr->ep[1].epN.rxmaxp);
	musb_print_rxmaxp(w);

	w = readw(&musbr->ep[1].epN.rxcsr);
	musb_print_rxcsr(w);

	w = readw(&musbr->ep[1].epN.txmaxp);
	musb_print_txmaxp(w);

	w = readw(&musbr->ep[1].epN.txcsr);
	musb_print_txcsr(w);
}