Ubuntu Lucid 10.04 Xfce4 on TechNexion Thunder Board

Here is being presented the Thunder board of TechNexion running Ubuntu 10.04 but now with the Xfce4 window manager. As you can see from the video that is uploaded the environment is very quick. Finally, I think a little better than the LXDE that was presented preciously.
Modifications had been implemented in order to have everything functional on the Thunder board. Ethernet-internet connection via eth0, usb hub with mouse and keyboard, audio and the other peripherals are full functional using the linux kernel 2.6.32 costumized for this board.

Ubuntu Lucid 10.04 LXDE on TechNexion Thunder Board

In this project I finally ported Ubuntu 10.04 Lucid with the LXDE windows environment to the Thunder Board of TechNexion. From the beginning you can notice that is very stable and quick. Especially if you compare it with the standard distro for this board that officially TechNexion is providing.
In the other hand, the graphical environment of LXDE looks similar to the XFCE4 that someone can use in distros of linux like Xubuntu or ArchLinux. However, my personal opinion is that is better and much user friendlier than the XFCE4.
I will not provide a detailed documentation on how I managed to port finally Ubuntu 10.04 in this board. I will give only some general directions.
For those who have more questions can contact me.
Also in the end of this post are some videos and photos of this implementation.

General Guides:
1. SD card no smaller than 1 GB.
2. 2 partitions (boot:1 fat (<80MB) & rootfs:1 ext3 (the rest))
3. Boot partition:3 files: MLO(x-loader), u-boot.bin (bootloader), and uImage (image of the kernel that supports the current board, cpu architecture-arm and peripherics, precompiled kernel of TechNexion 2.6.32 (download folder of the web page of TechNexion))
4. Create your own Ubuntu/debian root file system using the rootstock command/program.
5. After the creation of the rootfs, untgz it (using tar -zxf <...>.tgz) and place it in the rootfs partition of the SD card.
6. Rename the folder that exists in the /rootfs/lib/modules as 2.6.32.
7. Apply these commands in order to use sudo su when the system is running. Initialy from the RS232 and after that in the system.

>sudo chown root:root /usr/bin/sudo
>sudo chmod 4111 /usr/bin/sudo
>sudo root:root /etc/sudoers
> chmod 0440 /etc/sudoers

8. Unmount the SD card and initialise the Thunder Board with that.

Ubuntu Karmic 9.10 on TechNexion Thunder Board

In this project I ported Karmic Koala 9.10 version of Ubuntu to the Thunder Board of TechNexion. Unfortunately, I do not have graphical environment because the package of karmic from rootstock does not have the Xorg installed and while I am running in command mode the repos of Karmic are not online, as a non LTS version of Ubuntu, so I cannot install the packages that are missing for a complete experience. However, I am working on that...
For those who want more details can leave a comment or send me an e-mail.
Video (sorry for the poor quality) and photos are following.

Angstrom on Thunder Board

Here i will describe the main steps for producing a bootable SD card for the boards of TechNexion, like Tsunami and Thunder Board that they are based on the OMAP3530 processor of TI.

1. Download Codesourcery G++ Lite from Mentor page and run the .bin file in the /bin/sh shell in your linux distro. If you follow the GUI installation, you will add the path of the g++ automatically in the system PATH. Check your path with 

>echo $PATH.

2. In the bash standard shell of Ubuntu:

>export CROSS_COMPILE=arm-none-linux-gnueabi-

>export ARCH=arm

3. Download the xukr-20120201-omap3.tar.xz package from technexion that contains the linux kernel, the u-boot, the x-loader and a sample root file system of Angstrom.

unzipping this package:

unxz package_name.tar.xz

results to package_name.tar

and then unzip this tarball using the unzipping manager.

4. Browse the folder in command line

5. Browse the folder of the x-loader

>make distclean && make tao3530_config && make -j 2  (for the tao3530)

The resulting binary is named MLO.

6. Browse the folder of the u-boot

Change the #define TN_PANEL 043 (for 4.3") or 070 (for 7")
 

> make distclean && make tao3530_config && make -j 2 tao3530 (for the tao3530)

The resulting binary is named u-boot.bin

7. Browse the linux kernel folder:

>make distclean && make tao3530_thunder_defconfig && make -j 2 uImage && make modules  (for the thunder baord)

For other board choose other board_name_defconfig.

The resulting kernel binary is in arch/arm/boot/uImage.

8. Formatting the sd card:

a)boot partition: msdos, 100MB,

b)root file system partition: ext3/ext4, >270MB

If you have a pre-formatted sd card, will be fine. Or else have to create the partitions possibly using fdisk utility of linux.

So, if it is preformatted delete the contains of the partitions and replace them with the new that have generated.

unmount the SD card and install it in the board

9. Change the rootfs of the Angstrom
find the .ko for the wireless and the powervr in the drivers folder of the linux kernel and copy them in the boot folder of the provided Angstrom rootfs.
AND
for TAO3530 the default console is ttyO2 and not ttuO0 --change this in the /etc/inittab of the provided Angstrom rootfs

10. Copy the files in the cd card.
MLO, u-boot.bin and uImage in the boot partition
and the rootfs on the rootfs folder (need sudo permissions).

11. Unmount safely the sd card.

12. Boot the system from the SD card.

And some fotos: 

Shell changing in Ubuntu 10.04 from bash to tcsh and sh

Useful commands and tips:

See the current shell:
kostas777711@kostas777711-desktop:~$ echo $0
bash








Install the tcsh shell via the ubuntu software center and change from the bash shell (default in Ubuntu), and check that you are there:
kostas777711@kostas777711-desktop:/bin$ /bin/tcsh
kostas777711-desktop:/bin> echo $0
/bin/tcsh


Change from tcsh to sh shell, and check:
kostas777711-desktop:/bin> /bin/sh
sh-4.1$ echo $0
/bin/sh

An autonomous, co-operative telerobotic agent framework for WowWee Rovio

This is my dissertation that I did for the completion of my master in The University of Birmingham. And it is a mix of robotic logic and image processing methods, including the off-the-shelf robot Rovio from WowWee.

Abstract

This project presents a telerobotic agent framework for WowWee Rovio. An advanced and

parametrical Graphical User Interface has been implemented. Through this GUI the operator can

manually command the available agents or can trigger agents to complete tasks by applying them

autonomous behaviours. For the completion of these tasks, that require object recognition

techniques, advanced image processing methods were developed. The design and the

implementation of advanced tasks were possible after developing and completing successfully

smaller and simpler tasks. The GUI allows the intervention of the user applying semi-autonomous

behaviour for specific functionalities in order to help agents to successfully fulfil autonomous co-

operating tasks. Experimental results and measurements of this framework demonstrate that the

Rovios can successfully and efficiently complete the various tasks within a reasonable amount of

time meeting all the constraints that have been set.

Download link here:

http://www.2shared.com/document/yZpup3SZ/An_autonomous_co-operative_tel.html

Load Bitmap (*.png) in Android Application

Finally it wasn't so easy.
So here are some general instructions.

import java.io.IOException;
import java.lang.String;
import org.microbridge.server.AbstractServerListener;
import org.microbridge.server.Server;
import android.content.Context;
import android.content.res.Resources;
import android.graphics.Canvas;
import android.graphics.Color;
import android.graphics.Paint;
import android.graphics.Bitmap;
import android.graphics.Rect;
import android.graphics.drawable.BitmapDrawable;
import android.graphics.drawable.Drawable;
import android.graphics.BitmapFactory;
import android.graphics.Paint.Style;
import android.util.Log;
import android.view.MotionEvent;
import android.view.View;
import android.view.View.OnTouchListener;

public class Menu extends View implements OnTouchListener
{

....

public Bitmap bitmapMenu;

private Paint borderPaint = new Paint();

.....

.....

    public Menu(Context context, Server server)
    {

     ....

        //fetch bitmap
        bitmapMenu =         BitmapFactory.decodeResource(context.getResources(),R.drawable.android);

      ...

    @Override
    public void onDraw(Canvas canvas)  //Canvas canvas
   
    {

       
        // Draw background
       
        canvas.drawRect(this.getLeft(), this.getTop(), this.getRight(), this.getBottom(), borderPaint);


       
       
        //canvas.drawBitmap(bitmapMenu, 0 , 0, textPaint);
       
        Rect src=new Rect(0,0,960,600);
        Rect dst=new Rect(0,0,730,370);
       
        canvas.drawBitmap(bitmapMenu, src, dst, textPaint);
       
        canvas.drawText("Menu", 37 , 65, textPaint3);
    }

....

....

}

SOT358-1 BreakOut of TDA8029

This breakout can be used for more robust testing.
TDA8029 is a chip card reader, for example it can read the data of the chip of the bank cards.
The Layout of the breakout (Negative print out):

And the results in photos:

Read Strings in Android via Microbridge

This project is like the previous that was described. The difference here is that the host NXP1768 is sending/generating a string ("ADB Connection!!!") to the Android system. The application for the Android was modified to read strings and display them in the screen, as the photos show below.
This project can be used as a base for many projects that want to pass information from the external PIC to the Android and be displayed from an Android application.

Application running unconnected (null input).

mbed and Android unconnected.

Host and Client unconnected.

When connected, the Host is sending successfully the string ("ADB Connection!!!") to the Client, Andoid, and the Andoid is displaying the string.

From the link below you can find the project for mbed and the application for Android (Android Application Eclipse project).
http://www.multiupload.com/XBB1MLLN0D

MCP9800 Temp.Sensor in Android via mbed MicroBridge

In this project I am reading the temperature of the Microchip MCP9800 temperature sensor via mbed Microbridge in the Thunder Board running Android 2.2 Froyo.
The mbed NXP1768 is behaving like a host. In this project we see how we can communicate from the external PIC (NXP1768) with an Android System.
We want the program (mbed project) for the communication of the NXP1768 with the MCP9800 sensor, the same project implements the MicroBridge with the Android System and finally in the side of the Android we have the application which communicates with the NXP1768 and displays the info in the screen.
The photos below present this project.

The side of mbed.

The side of Android.

The application.

The Adb  (USB) connection between Android and mbed via MicroBridge.

The desktop of Android with the MicroBridge Application.

Here are the necessary files-projects-application for this project:
http://www.multiupload.com/XV7M8J939X.
The zip file contains the application (Eclipse Project file) for the Android and the mbed NXP1768 project.
I forgot to mention that for the side of the mbed you will need the MCP9800 sensor to be connected in pin9 and pin10 (I2C communication) in the mbed. The sensor needs pull-up resistors. I used 4.1K Ohms. One in the SDA and the other in the SCL of the sensor, they are going at 5 V dc voltage. Finally I used the D+ and D- of the mbed board (NXP1768) connected with a Sparkfun mini usb breakout which then is connected with the ADB USB OTG port of the Android System (Thunder board in my project from TechNexion).
The sensor is measuring the temperature with 3 decimal digits (ex. 23.567) but I am sending integer at the Android, so it is displaying only the 23 for example.
Maybe one of the next implementations will be the reading of float numbers through the mbed MicroBridge.

Basic Files MicroBridge Mbed Project for Tsunami Board Alterations

 /* main.cpp */
#include "mbed.h"
#include "Adb.h"
#include <TextLCD.h>
//TextLCD lcd(p11, p12, p27, p28, p29, p30);

Connection * connection;

Serial pc(USBTX, USBRX);

DigitalOut greenLed1(p12);
DigitalOut greenLed2(p13);
DigitalOut greenLed3(p14);
DigitalOut greenLed4(p15);
DigitalOut greenLed5(p16);

DigitalOut redLed1(p30);
DigitalOut redLed2(p29);
DigitalOut redLed3(p28);
DigitalOut redLed4(p27);
DigitalOut redLed5(p26);
DigitalOut redLed6(p25);
DigitalOut redLed7(p24);

AnalogOut motor(p18);
DigitalIn sw1(p5);
DigitalOut led(LED1);

void adbEventHandler(Connection * connection, adb_eventType event, uint16_t length, uint8_t * data)
{
    if (event == ADB_CONNECTION_RECEIVE)
    {
        printf("[ADB RECV]:%d   %d\r\n",data[0],data[1]);
        float val = ((float)data[0] * 5.5) + 1000.0;
        printf(">>>> val1:%f\r\n",val);
       
        //this val controls the green leds
        if (val<1200)
        {
            greenLed1=1;
            motor=0.1;
        }
        else if (val<1400)
        {
            greenLed2=1;
            motor=0.3;
        }
        else if (val<1600)
        {
            greenLed3=1;
            motor=0.5;
        }   
        else if (val<1800)
        {
            greenLed4=1;
            motor=0.8;
        }   
        else
        {
            greenLed5=1;
            motor=1;
        }   
                  
       
       
       
        //servo1.pulsewidth_us(val);
        val = ((float)data[1] * 5.5) + 1000.0;
       
        //servo2.pulsewidth_us(val);
        printf(">>>>  val2:%f\r\n",val);
       
        //this val controls the red leds
        if (val<1200)
            redLed1=1;
        else if (val<1300)
            redLed2=1;
        else if (val<1400)
            redLed3=1;
        else if (val<1500)
            redLed4=1;
        else if (val<1600)
            redLed5=1;
        else if (val<1700)
            redLed6=1;
        else
            redLed7=1;


       
        //initialise the leds
         wait(0.5);
         greenLed1=greenLed2=greenLed3=greenLed4=greenLed5=0;
         redLed1=redLed2=redLed3=redLed4=redLed5=redLed6=redLed7=0;
    }
}

int main()
{
    int now=0,old;
    unsigned short data = 0;
   
    pc.baud(115200);
    //servo1.period_us(20000);
    //servo2.period_us(20000);
    //motor.period_us(200);
    sw1.mode(PullUp);
   
   
    // Initialise the ADB subsystem. 
    ADB::init();
    // Open an ADB stream to the phone's shell. Auto-reconnect
    connection = ADB::addConnection("tcp:4567", true, adbEventHandler);
   
    while(1)
    {
   
        ADB::poll();
       
        //lcd.locate(0, 0);
        //lcd.printf("MicroBridge Test");
       
        old = now;
        now = sw1;
        if((now == 1)&&(old==0))
        {
            printf("down key\r\n");
            connection->write(2, (unsigned char*)&data);
            //lcd.locate(0, 1);
            //lcd.printf("COUNT = %d",data);

            data++;
        }
        led = sw1;
    }
}



/* USBHost.cpp */

/*
Copyright (c) 2010 Peter Barrett

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
 


#include "mbed.h"
#include "USBHost.h"
#ifndef  USBHOST_LOG

#define DESC_INDEX 0

//    Config (default uses x bytes)
#define MAX_DEVICES 8                // Max number of devices
#define MAX_ENDPOINTS_TOTAL 16        // Max number of endpoints total
#define MAX_ENDPOINTS_PER_DEVICE 8    // Max number of endpoints for any one device

#define  USBLOG 0
#if USBLOG
#define  LOG(...)       printf(__VA_ARGS__)
#else
#define  LOG(...)       do {} while(0)
#endif

// USB host structures

#define USB_RAM_SIZE 16*1024    // AHB SRAM block 1 TODO MACHINE DEPENDENT
#define USB_RAM_BASE 0x2007C000

#define TOKEN_SETUP 0
#define TOKEN_IN  1
#define TOKEN_OUT 2

//    Status flags from hub
#define PORT_CONNECTION 0
#define PORT_ENABLE  1
#define PORT_SUSPEND  2
#define PORT_OVER_CURRENT 3
#define PORT_RESET 4
#define PORT_POWER 8
#define PORT_LOW_SPEED 9

#define C_PORT_CONNECTION 16
#define C_PORT_ENABLE 17
#define C_PORT_SUSPEND 18
#define C_PORT_OVER_CURRENT 19
#define C_PORT_RESET 20

typedef struct {
    u8 bm_request_type;
    u8 b_request;
    u16 w_value;
    u16 w_index;
    u16 w_length;
} Setup;


//    Hub stuff is kept private just to keep api simple
int SetPortFeature(int device, int feature, int index);
int ClearPortFeature(int device, int feature, int index);
int SetPortPower(int device, int port);
int SetPortReset(int device, int port);
int GetPortStatus(int device, int port, u32* status);

//===================================================================
//===================================================================
//    Hardware defines

//    HcControl
#define PeriodicListEnable    0x00000004
#define    IsochronousEnable    0x00000008
#define    ControlListEnable    0x00000010
#define    BulkListEnable        0x00000020
#define    OperationalMask        0x00000080
#define    HostControllerFunctionalState    0x000000C0

//    HcCommandStatus
#define HostControllerReset    0x00000001
#define ControlListFilled    0x00000002
#define BulkListFilled        0x00000004

//    HcInterruptStatus Register
#define    WritebackDoneHead        0x00000002
#define    StartofFrame            0x00000004
#define ResumeDetected            0x00000008
#define UnrecoverableError        0x00000010
#define FrameNumberOverflow        0x00000020
#define RootHubStatusChange        0x00000040
#define OwnershipChange            0x00000080
#define MasterInterruptEnable    0x80000000

//    HcRhStatus
#define SetGlobalPower            0x00010000
#define DeviceRemoteWakeupEnable    0x00008000

//    HcRhPortStatus (hub 0, port 1)
#define CurrentConnectStatus    0x00000001
#define    PortEnableStatus        0x00000002
#define PortSuspendStatus        0x00000004
#define PortOverCurrentIndicator    0x00000008
#define PortResetStatus            0x00000010

#define PortPowerStatus            0x00000100
#define LowspeedDevice            0x00000200
#define HighspeedDevice            0x00000400

#define ConnectStatusChange    (CurrentConnectStatus << 16)
#define PortResetStatusChange    (PortResetStatus << 16)


#define  TD_ROUNDING        (u32)0x00040000
#define  TD_SETUP            (u32)0x00000000
#define  TD_IN                (u32)0x00100000
#define  TD_OUT                (u32)0x00080000
#define  TD_DELAY_INT(x)    (u32)((x) << 21)
#define  TD_TOGGLE_0        (u32)0x02000000
#define  TD_TOGGLE_1        (u32)0x03000000
#define  TD_CC                (u32)0xF0000000

//    HostController EndPoint Descriptor
typedef struct {
    volatile u32    Control;
    volatile u32    TailTd;
    volatile u32    HeadTd;
    volatile u32    Next;
} HCED;

// HostController Transfer Descriptor
typedef struct {
    volatile u32    Control;
    volatile u32    CurrBufPtr;
    volatile u32    Next;
    volatile u32    BufEnd;
} HCTD;

// Host Controller Communication Area
typedef struct {
    volatile u32    InterruptTable[32];
    volatile u16    FrameNumber;
    volatile u16    FrameNumberPad;
    volatile u32    DoneHead;
    volatile u8        Reserved[120];
} HCCA;

//====================================================================================
//====================================================================================

class HostController;
class Endpoint;
class Device;

//      must be 3*16 bytes long
class Endpoint
{
public:
    HCED    EndpointDescriptor;    // Pointer to EndpointDescriptor == Pointer to Endpoint
    HCTD    TDHead;

    enum State
    {
        Free,
        NotQueued,
        Idle,
        SetupQueued,
        DataQueued,
        StatusQueued,
        CallbackPending
    };
   
    volatile u8 CurrentState;
    u8        Flags;            // 0x80 In, 0x03 mask endpoint type

    u16        Length;
    u8*        Data;
    USBCallback Callback;     // Must be a multiple of 16 bytes long
    void*  UserData;
 
    int Address()
    {
        int ep = (EndpointDescriptor.Control >> 7) & 0xF;
        if (ep)
            ep |= Flags & 0x80;
        return ep;
    }
   
    int Device()
    {
        return EndpointDescriptor.Control & 0x7F;
    }

    int Status()
    {
        return (TDHead.Control >> 28) & 0xF;
    }

    u32 Enqueue(u32 head)
    {
        if (CurrentState == NotQueued)
        {
            EndpointDescriptor.Next = head;
            head = (u32)&EndpointDescriptor;
            CurrentState = Idle;
        }
        return head;
    }
};

class Device
{
public:
    u8    _endpointMap[MAX_ENDPOINTS_PER_DEVICE*2];
    u8    Hub;
    u8    Port;
    u8    Addr;
    u8    Pad;

    //    Only if this device is a hub
    u8    HubPortCount;    // nonzero if this is a hub
    u8    HubInterruptData;
    u8    HubMap;
    u8    HubMask;

    int Flags;        // 1 = Disconnected

    Setup    SetupBuffer;

    // Allocate endpoint zero
    int Init(DeviceDescriptor* d, int hub, int port, int addr, int lowSpeed)
    {
        Hub = hub;
        Port = port;
        Addr = addr;
        Flags = lowSpeed;
        memset(_endpointMap,0xFF,sizeof(_endpointMap));
        return 0;
    }

    int SetEndpointIndex(int ep, int endpointIndex)
    {
        for (int i = 0; i < MAX_ENDPOINTS_PER_DEVICE*2; i += 2)
        {
            if (_endpointMap[i] == 0xFF)    // Add endpoint to map
            {
                _endpointMap[i] = ep;
                _endpointMap[i+1] = endpointIndex;
                return 0;
            }
        }
        return ERR_ENDPOINT_NONE_LEFT;
    }

    int GetEndpointIndex(int ep)
    {
        for (int i = 0; i < MAX_ENDPOINTS_PER_DEVICE*2; i += 2)
        {
            if (_endpointMap[i] == ep)
                return _endpointMap[i+1];
            if (_endpointMap[i] == 0xFF)
                break;
        }
        return -1;
    }
};

class HostController
{
public:
    HCCA        CommunicationArea;
    Endpoint    Endpoints[MAX_ENDPOINTS_TOTAL];    // Multiple of 16
   
    Endpoint    EndpointZero;                        // For device enumeration
    HCTD        _commonTail;
    Setup        _setupZero;
   
    Device    Devices[MAX_DEVICES];
    u32    _frameNumber;            // 32 bit ms counter

    u8    _callbacksPending;        //    Endpoints with callbacks are pending, set from ISR via ProcessDoneQueue
    u8    _rootHubStatusChange;    //    Root hub status has changed, set from ISR
    u8    _unused0;
    u8    _unused1;

    u8    _connectPending;    //    Reset has initiated a connect
    u8    _connectCountdown;    //    Number of ms left after reset before we can connect
    u8    _connectHub;        //    Will connect on this hub
    u8    _connectPort;        //    ... and this port

    u8    SRAM[0];            // Start of free SRAM

    void Loop()
    {
        u16 elapsed = CommunicationArea.FrameNumber - (u16)_frameNumber;    // extend to 32 bits
        _frameNumber += elapsed;

        // Do callbacks, if any
        while (_callbacksPending)
        {
            for (int i = 0; i < MAX_ENDPOINTS_TOTAL; i++)
            {
                Endpoint* endpoint = Endpoints + i;
                if (endpoint->CurrentState == Endpoint::CallbackPending)
                {
                    LOG("Sorting Callbacks %i\r\n\r",endpoint->CurrentState);
                    _callbacksPending--;
                    endpoint->CurrentState = Endpoint::Idle;
                    LOG("SatusChanged %i\r\n\r",endpoint->CurrentState);
                    LOG("CallBack DataSize:%d",endpoint->Length);
                    endpoint->Callback(endpoint->Device(),endpoint->Address(),endpoint->Status(),endpoint->Data,endpoint->Length,endpoint->UserData);
                }
            }
        }

        //    Deal with changes on the root hub
        if (_rootHubStatusChange)
        {
            u32 status = LPC_USB->HcRhPortStatus1;
            _rootHubStatusChange = 0;
            if (status >> 16)
            {
                HubStatusChange(0,1,status);
                LPC_USB->HcRhPortStatus1 = status & 0xFFFF0000;    // clear status changes
            }
        }

        //    Connect after reset timeout
        if (_connectCountdown)
        {
            if (elapsed >= _connectCountdown)
            {
                _connectCountdown = 0;
                Connect(_connectHub,_connectPort & 0x7F,_connectPort & 0x80);
            } else
                _connectCountdown -= elapsed;
        }
    }

    //    HubInterrupt - bitmap in dev->HubInterruptData
    void HubInterrupt(int device)
    {
        Device* dev = &Devices[device-1];
        for (int i = 0; i < dev->HubPortCount; i++)
        {
            int port = i+1;
            if (dev->HubInterruptData & (1 << port))
            {
                u32 status = 0;
                GetPortStatus(device,port,&status);
                if (status >> 16)
                {
                    if (_connectPending && (status & ConnectStatusChange))
                        continue;    // Don't connect again until previous device has been added and addressed

                    HubStatusChange(device,port,status);
                    if (status & ConnectStatusChange)
                        ClearPortFeature(device,C_PORT_CONNECTION,port);
                    if (status & PortResetStatusChange)
                        ClearPortFeature(device,C_PORT_RESET,port);
                }
            }
        }
    }

    static void HubInterruptCallback(int device, int endpoint, int status, u8* data, int len, void* userData)
    {
        HostController* controller = (HostController*)userData;
        if (status == 0)
            controller->HubInterrupt(device);
        USBInterruptTransfer(device,endpoint,data,1,HubInterruptCallback,userData);
    }

    int InitHub(int device)
    {
        u8 buf[16];
        int r= USBControlTransfer(device,DEVICE_TO_HOST | REQUEST_TYPE_CLASS | RECIPIENT_DEVICE,GET_DESCRIPTOR,(DESCRIPTOR_TYPE_HUB << 8),0,buf,sizeof(buf));
        if (r < 0)
            return ERR_HUB_INIT_FAILED;
       
        //    turn on power on the hubs ports
        Device* dev = &Devices[device-1];
        int ports = buf[2];
        dev->HubPortCount = ports;
        for (int i = 0; i < ports; i++)
            SetPortPower(device,i+1);
       
        // Enable hub change interrupts
        return USBInterruptTransfer(device,0x81,&dev->HubInterruptData,1,HubInterruptCallback,this);
    }
   
    int AddEndpoint(int device, int ep, int attributes, int maxPacketSize, int interval)
    {
        LOG("AddEndpoint D:%02X A:%02X T:%02X P:%04X I:%02X\r\r\n",device,ep,attributes,maxPacketSize,interval);
        Device* dev = &Devices[device-1];
        Endpoint* endpoint = AllocateEndpoint(device,ep,attributes,maxPacketSize);
        if (!endpoint)
            return ERR_ENDPOINT_NONE_LEFT;
        dev->SetEndpointIndex(ep,endpoint - Endpoints);
        endpoint->EndpointDescriptor.Control |= dev->Flags; // Map in slow speed
        return 0;  // TODO ed->bInterval
    }
   
    int AddEndpoint(int device, EndpointDescriptor* ed)
    {
        return AddEndpoint(device,ed->bEndpointAddress,ed->bmAttributes,ed->wMaxPacketSize,ed->bInterval);
    }

    //      allocate a endpoint
    Endpoint* AllocateEndpoint(int device, int endpointAddress, int type, int maxPacketSize)
    {
        for (int i = 0; i < MAX_ENDPOINTS_TOTAL; i++)
        {
            Endpoint* ep = &Endpoints[i];
            if (ep->CurrentState == 0)
            {
                //LOG("Allocated endpoint %d to %02X:%02X\r\r\n",i,device,endpointAddress);
                ep->Flags = (endpointAddress & 0x80) | (type & 3);
                ep->CurrentState = Endpoint::NotQueued;
                ep->EndpointDescriptor.Control = (maxPacketSize << 16) | ((endpointAddress & 0x7F) << 7) | device;
                return ep;
            }
        }
        return 0;
    }

    Endpoint* GetEndpoint(int device, int ep)
    {
        if (device == 0)
        {
            //printf("WARNING: USING DEVICE 0\r\n");
            return &EndpointZero;
        }
        if (device > MAX_DEVICES)
            return 0;
        int i = Devices[device-1].GetEndpointIndex(ep);
        if (i == -1)
            return 0;
        return Endpoints + i;
    }

    int Transfer(Endpoint* endpoint, int token, u8* data, int len, int state)
    {
        //LOG("Transfer %02X T:%d Len:%d S:%d\r\r\n",endpoint->Address(),token,len,state);
   
        int toggle = 0;
        if (endpoint->Address() == 0)
            toggle = (token == TOKEN_SETUP) ? TD_TOGGLE_0 : TD_TOGGLE_1;

        if (token != TOKEN_SETUP)
            token = (token == TOKEN_IN ? TD_IN : TD_OUT);

        HCTD* head = &endpoint->TDHead;
        HCTD* tail = &_commonTail;

        head->Control = TD_ROUNDING | token | TD_DELAY_INT(0) | toggle | TD_CC;
        head->CurrBufPtr = (u32)data;
        head->BufEnd = (u32)(data + len - 1);
        head->Next = (u32)tail;

        HCED* ed = &endpoint->EndpointDescriptor;
        ed->HeadTd = (u32)head | (ed->HeadTd & 0x00000002);    // carry toggle
        ed->TailTd = (u32)tail;
       
        //HCTD* td = head;
        //LOG("%04X TD %08X %08X %08X Next:%08X\r\r\n",CommunicationArea.FrameNumber,td->Control,td->CurrBufPtr,td->BufEnd,td->Next);
        //LOG("%04X ED %08X %08X %08X\r\r\n",CommunicationArea.FrameNumber,ed->Control,ed->HeadTd,ed->TailTd);
       
        switch (endpoint->Flags & 3)
        {
            case ENDPOINT_CONTROL:
                LPC_USB->HcControlHeadED = endpoint->Enqueue(LPC_USB->HcControlHeadED);    // May change state NotQueued->Idle
                endpoint->CurrentState = state;                                               // Get in before an int
                LPC_USB->HcCommandStatus = LPC_USB->HcCommandStatus | ControlListFilled;
                LPC_USB->HcControl = LPC_USB->HcControl | ControlListEnable;
                break;

            case ENDPOINT_BULK:
                LPC_USB->HcBulkHeadED = endpoint->Enqueue(LPC_USB->HcBulkHeadED);
                endpoint->CurrentState = state;
                LPC_USB->HcCommandStatus = LPC_USB->HcCommandStatus | BulkListFilled;
                LPC_USB->HcControl = LPC_USB->HcControl | BulkListEnable;
                break;

            case ENDPOINT_INTERRUPT:
                CommunicationArea.InterruptTable[0] = endpoint->Enqueue(CommunicationArea.InterruptTable[0]);
                endpoint->CurrentState = state;
                LPC_USB->HcControl |= PeriodicListEnable;
                break;
        }
        return 0;
    }
   
    //    Remove an endpoint from an active queue
    bool Remove(HCED* ed, volatile HCED** queue)
    {
        if (*queue == 0)
            return false;
        if (*queue == (volatile HCED*)ed)
        {
            *queue = (volatile HCED*)ed->Next;    // At head of queue
            return true;
        }

        volatile HCED* head = *queue;
        while (head)
        {
            if (head->Next == (u32)ed)
            {
                head->Next = ed->Next;
                return true;
            }
            head = (volatile HCED*)head->Next;
        }
        return false;
    }

    void Release(Endpoint* endpoint)
    {
        if (endpoint->CurrentState == Endpoint::NotQueued)
        {
            // Never event used it, nothing to do
        }
        else
        {
            HCED* ed = (HCED*)endpoint;
            ed->Control |= 0x4000;    // SKIP
            switch (endpoint->Flags & 0x03)
            {
                case ENDPOINT_CONTROL:
                    Remove(ed,(volatile HCED**)&LPC_USB->HcControlHeadED);
                    break;
                case ENDPOINT_BULK:
                    Remove(ed,(volatile HCED**)&LPC_USB->HcBulkHeadED);
                    break;
                case ENDPOINT_INTERRUPT:
                    for (int i = 0; i < 32; i++)
                        Remove(ed,(volatile HCED**)&CommunicationArea.InterruptTable[i]);
                    break;
            }

            u16 fn = CommunicationArea.FrameNumber;
            while (fn == CommunicationArea.FrameNumber)
                ;    // Wait for next frame

        }

        //    In theory, the endpoint is now dead.
        //    TODO: Will Callbacks ever be pending? BUGBUG
        memset(endpoint,0,sizeof(Endpoint));
    }

    //      Pop the last TD from the list
    HCTD* Reverse(HCTD* current)
    {
        HCTD *result = NULL,*temp;
        while (current)
        {
            temp = (HCTD*)current->Next;
            current->Next = (u32)result;
            result = current;
            current = temp;
        }
        return result;
    }

    //      Called from interrupt...
    //      Control endpoints use a state machine to progress through the transfers
    void ProcessDoneQueue(u32 tdList)
    {
        HCTD* list = Reverse((HCTD*)tdList);
        while (list)
        {
            Endpoint* endpoint = (Endpoint*)(list-1);
            list = (HCTD*)list->Next;
            int ep = endpoint->Address();
            bool in = endpoint->Flags & 0x80;
            int status = (endpoint->TDHead.Control >> 28) & 0xF;

            //LOG("ProcessDoneQueue %02X %08X\r\r\n",ep,endpoint->TDHead.Control);

            if (status != 0)
            {
                LOG("ProcessDoneQueue status %02X %d\r\r\n",ep,status);
                endpoint->CurrentState = Endpoint::Idle;
            } else {
                switch (endpoint->CurrentState)
                {
                    case Endpoint::SetupQueued:
                        if (endpoint->Length == 0)
                            Transfer(endpoint,in ? TOKEN_OUT : TOKEN_IN,0,0,Endpoint::StatusQueued);    // Skip Data Phase
                        else
                            Transfer(endpoint,in ? TOKEN_IN : TOKEN_OUT,endpoint->Data,endpoint->Length, Endpoint::DataQueued);    // Setup is done, now Data
                        break;

                    case Endpoint::DataQueued:
                        if (endpoint->TDHead.CurrBufPtr)
                            endpoint->Length = endpoint->TDHead.CurrBufPtr - (u32)endpoint->Data;

                        if (ep == 0)
                            Transfer(endpoint,in ? TOKEN_OUT : TOKEN_IN,0,0,Endpoint::StatusQueued);    // Data is done, now Status, Control only
                        else
                            endpoint->CurrentState = Endpoint::Idle;
                        break;

                    case Endpoint::StatusQueued:    // Transaction is done
                        endpoint->CurrentState = Endpoint::Idle;
                        break;
                }
            }

            //      Complete, flag if we need a callback
            if (endpoint->Callback && endpoint->CurrentState == Endpoint::Idle)
            {
                endpoint->CurrentState = Endpoint::CallbackPending;
                _callbacksPending++;
            }
        }
    }

    //    Hack to reset devices that don't want to connect
    int AddDevice(int hub, int port, bool isLowSpeed)
    {
        int device = AddDeviceCore(hub,port,isLowSpeed);
        if (device < 0)
        {
            LOG("========RETRY ADD DEVICE========\r\r\n");    // This will go for ever.. TODO power cycle root?
            Disconnect(hub,port);    // Could not read descriptor at assigned address, reset this port and try again
            ResetPort(hub,port);    // Cheap bluetooth dongles often need this on a hotplug
            return -1;
        }
        return device;
    }

    int AddDeviceCore(int hub, int port, bool isLowSpeed)
    {
        int lowSpeed = isLowSpeed ? 0x2000 : 0;
        DeviceDescriptor desc;
        EndpointZero.EndpointDescriptor.Control = (8 << 16) | lowSpeed;               // MaxPacketSize == 8
        int r = GetDescriptor(0,DESCRIPTOR_TYPE_DEVICE,0,(u8*)&desc,8);
        if (r < 0)
        {
            LOG("FAILED TO LOAD DESCRIPTOR FOR DEVICE 0\r\r\n");
            return r;
        }

        EndpointZero.EndpointDescriptor.Control = (desc.bMaxPacketSize << 16) | lowSpeed;     // Actual MaxPacketSize
        r = GetDescriptor(0,DESCRIPTOR_TYPE_DEVICE,0,(u8*)&desc,sizeof(desc));
        if (r < 0)
            return r;

        LOG("\r\nClass %02X found %04X:%04X\r\r\n",desc.bDeviceClass,desc.idVendor,desc.idProduct);

        //      Now assign the device an address, move off EndpointZero
        int device = 0;
        for (int i = 0; i < MAX_DEVICES; i++)
        {
            if (Devices[i].Port == 0)
            {
                device = i+1;
                break;
            }
        }
        if (!device)
            return ERR_DEVICE_NONE_LEFT;

        r = SetAddress(0,device);
        if (r)
            return r;
        DelayMS(2);
       
        // Now at a nonzero address, create control endpoint
        Device* dev = &Devices[device-1];
        dev->Init(&desc,hub,port,device,lowSpeed);
        AddEndpoint(device,0,ENDPOINT_CONTROL,desc.bMaxPacketSize,0);
        _connectPending = 0;

        //    Verify this all works
        r = GetDescriptor(device,DESCRIPTOR_TYPE_DEVICE,0,(u8*)&desc,sizeof(desc));
        if (r < 0)
            return r;

        //    Set to interface 0 by default
        //    Calls LoadDevice if interface is found
        r = SetConfigurationAndInterface(device,1,0,&desc);
        //r = SetConfigurationAndInterface(device,1,1,&desc);
        //r = SetConfigurationAndInterface(device,1,0,&desc);

        if (desc.bDeviceClass == CLASS_HUB)
            InitHub(device);            // Handle hubs in this code

        return device;
    }

    // Walk descriptors and create endpoints for a given device
    // TODO configuration !=1, alternate settings etc.
    int SetConfigurationAndInterface(int device, int configuration, int interfaceNumber, DeviceDescriptor* desc)
    {
        u8 buffer[255];
        int err = GetDescriptor(device,DESCRIPTOR_TYPE_CONFIGURATION,DESC_INDEX,buffer,sizeof(buffer));
        if (err < 0)
            return err;

        err = SetConfiguration(device,configuration);
        if (err < 0)
            return err;

        //    Add the endpoints for this interface
        int len = buffer[2] | (buffer[3] << 8);
        u8* d = buffer;
        u8* end = d + len;
        InterfaceDescriptor* found = 0;
        while (d < end)
        {
            if (d[1] == DESCRIPTOR_TYPE_INTERFACE)
            {
                InterfaceDescriptor* id = (InterfaceDescriptor*)d;
                if (id->bInterfaceNumber == interfaceNumber)
                {
                    found = id;
                    d += d[0];
                    while (d < end && d[1] != DESCRIPTOR_TYPE_INTERFACE)
                    {
                        switch (d[1])
                        {
                            case DESCRIPTOR_TYPE_ENDPOINT:
                                AddEndpoint(device,(EndpointDescriptor*)d);
                                break;
                            default:
                                LOG("Skipping descriptor %02X (%d bytes)\r\r\n",d[1],d[0]);
                        }
                        d += d[0];
                    }
                }
            }
            d += d[0];
        }

        if (!found)
            return ERR_INTERFACE_NOT_FOUND;
        OnLoadDevice(device,desc,found);
        return 0;
    }

    void Init()
    {
        LOG("USB INIT (Controller is %d bytes)\r\r\n",sizeof(*this));
        memset(this,0,sizeof(HostController));
        EndpointZero.CurrentState = Endpoint::NotQueued;
        HWInit(&CommunicationArea);
        DelayMS(10);
    }

    void ResetPort(int hub, int port)
    {       
        LOG("ResetPort Hub:%d Port:%d\r\r\n",hub,port);
        _connectPending++;            // Only reset/add 1 device at a time
        if (hub == 0)
            LPC_USB->HcRhPortStatus1 = PortResetStatus;    // Reset Root Hub, port 1
        else
            SetPortReset(hub,port);    // or reset other hub
    }

    void Disconnect(int hub, int port)
    {
        LOG("Disconnect Hub:%d Port:%d\r\r\n",hub,port);    // Mark a device for destruction
        for (int i = 0; i < MAX_DEVICES; i++)
        {
            Device* dev = Devices + i;
            if (dev->Port == port && dev->Hub == hub)
            {
                //    Disconnect everything that is attached to this device if it is a hub
                for (int p = 0; p < dev->HubPortCount; p++)
                    Disconnect(i+1,p+1);

                //    Now release endpoints
                for (int j = 1; j < MAX_ENDPOINTS_PER_DEVICE*2; j += 2)
                {
                    u8 endpointIndex = dev->_endpointMap[j];
                    if (endpointIndex != 0xFF)
                        Release(Endpoints + endpointIndex);
                }
                dev->Port = 0;    // Device is now free
                dev->Flags = 0;
                return;
            }
        }
    }

    // called after reset
    void Connect(int hub, int port, bool lowspeed)
    {
        LOG("Connect Hub:%d Port:%d %s\r\r\n",hub,port,lowspeed ? "slow" : "full");
        AddDevice(hub,port,lowspeed);
    }

    // Called from interrupt
    void HubStatusChange(int hub, int port, u32 status)
    {
        LOG("HubStatusChange Hub:%d Port:%d %08X\r\r\n",hub,port,status);
        if (status & ConnectStatusChange)
        {
            if (status & CurrentConnectStatus)    // Connecting
                ResetPort(hub,port);            // Reset to initiate connect (state machine?)
            else
                Disconnect(hub,port);
        }

        if (status & PortResetStatusChange)
        {
            if (!(status & PortResetStatus))
            {
                _connectCountdown = 200;        // Schedule a connection in 200ms
                if (status & LowspeedDevice)
                    port |= 0x80;
                _connectHub = hub;
                _connectPort = port;
            }
        }
    }

    #define HOST_CLK_EN        (1<<0)
    #define PORTSEL_CLK_EN    (1<<3)
    #define AHB_CLK_EN        (1<<4)
    #define CLOCK_MASK        (HOST_CLK_EN | PORTSEL_CLK_EN | AHB_CLK_EN)

    #define  FRAMEINTERVAL        (12000-1)    // 1ms
    #define  DEFAULT_FMINTERVAL    ((((6 * (FRAMEINTERVAL - 210)) / 7) << 16) | FRAMEINTERVAL)

    void DelayMS(int ms)
    {
        u16 f = ms + CommunicationArea.FrameNumber;
        while (f != CommunicationArea.FrameNumber)
            ;
    }

    static void HWInit(HCCA* cca)
    {
        NVIC_DisableIRQ(USB_IRQn);
       
        // turn on power for USB
        LPC_SC->PCONP        |= (1UL<<31);
        // Enable USB host clock, port selection and AHB clock
        LPC_USB->USBClkCtrl |= CLOCK_MASK;
        // Wait for clocks to become available
        while ((LPC_USB->USBClkSt & CLOCK_MASK) != CLOCK_MASK)
            ;
       
        //    We are a Host
        LPC_USB->OTGStCtrl |= 1;
        LPC_USB->USBClkCtrl &= ~PORTSEL_CLK_EN;                // we don't need port selection clock until we do OTG
       
        // configure USB pins
        LPC_PINCON->PINSEL1 &= ~((3<<26)|(3<<28));   
        LPC_PINCON->PINSEL1 |=    ((1<<26)|(1<<28));            // USB D+/D-
           
        LPC_PINCON->PINSEL3 &= ~((3 << 6) | (3 << 22));        // USB_PPWR, USB_OVRCR
        LPC_PINCON->PINSEL3 |= ((2 << 6) | (2 << 22));
       
        LPC_PINCON->PINSEL4 &= ~(3 << 18);                    // USB_CONNECT
        LPC_PINCON->PINSEL4 |= (1 << 18);

        //    Reset OHCI block
        LPC_USB->HcControl         = 0;
        LPC_USB->HcControlHeadED = 0;
        LPC_USB->HcBulkHeadED     = 0;
       
        LPC_USB->HcCommandStatus = HostControllerReset;
        LPC_USB->HcFmInterval     = DEFAULT_FMINTERVAL;
        LPC_USB->HcPeriodicStart = FRAMEINTERVAL*90/100;

        LPC_USB->HcControl    = (LPC_USB->HcControl & (~HostControllerFunctionalState)) | OperationalMask;
        LPC_USB->HcRhStatus = SetGlobalPower;
       
        LPC_USB->HcHCCA = (u32)cca;
        LPC_USB->HcInterruptStatus |= LPC_USB->HcInterruptStatus;
        LPC_USB->HcInterruptEnable = MasterInterruptEnable | WritebackDoneHead | RootHubStatusChange | FrameNumberOverflow;

        NVIC_SetPriority(USB_IRQn, 0);
        NVIC_EnableIRQ(USB_IRQn);
        while (cca->FrameNumber < 10)
            ;    // 10ms delay before diving in
    }
};

//====================================================================================
//====================================================================================
//      Host controller instance and Interrupt handler

static HostController _controller __attribute__((at(USB_RAM_BASE)));

extern "C" void USB_IRQHandler(void) __irq;
void USB_IRQHandler (void) __irq
{
    u32 int_status = LPC_USB->HcInterruptStatus;

    if(int_status & UnrecoverableError) //Error
    {
      LOG("USB_IRQHandler UnrecoverableError Please reset\r\r\n");
    }


    if (int_status & RootHubStatusChange)    //    Root hub status change
        _controller._rootHubStatusChange++;    //    Just flag the controller, will be processed in USBLoop

    u32 head = 0;
    if (int_status & WritebackDoneHead)
    {
        head = _controller.CommunicationArea.DoneHead;        // Writeback Done
        _controller.CommunicationArea.DoneHead = 0;
    }            
    LPC_USB->HcInterruptStatus = int_status;

    if (head)
       _controller.ProcessDoneQueue(head);     // TODO - low bit can be set BUGBUG
}

//====================================================================================
//====================================================================================
//      API Methods

void USBInit()
{
    return _controller.Init();
}

void USBLoop()
{
   
    return _controller.Loop();
   
}

u8* USBGetBuffer(u32* len)
{
    *len = USB_RAM_SIZE - sizeof(HostController);
    return _controller.SRAM;
}

static Setup* GetSetup(int device)
{
    if (device == 0)
        return &_controller._setupZero;
   
    if (device < 1 || device > MAX_DEVICES)
        return 0;
    return &_controller.Devices[device-1].SetupBuffer;
}

//    Loop until IO on endpoint is complete
static int WaitIODone(Endpoint* endpoint)
{
    LOG("Waiting\r\n\r");
    if (endpoint->CurrentState == Endpoint::NotQueued)
        return 0;
    while (endpoint->CurrentState != Endpoint::Idle)
        //LOG("Loopz");
        USBLoop();    // May generate callbacks, mount or unmount devices etc
    int status = endpoint->Status();
    if (status == 0)
        return endpoint->Length;
        LOG("DATA SENT\r\n\r");
    return -status;
}

int USBTransfer(int device, int ep, u8 flags, u8* data, int length, USBCallback callback, void* userData)
{
    Endpoint* endpoint = _controller.GetEndpoint(device,ep);
    if (!endpoint)
        return ERR_ENDPOINT_NOT_FOUND;
       
    WaitIODone(endpoint);
    LOG("before ep=%x,callback=%p\r\r\n",endpoint,endpoint->Callback);
    endpoint->Flags = flags;
    endpoint->Data = data;
    endpoint->Length = length;
    endpoint->Callback = callback;
    endpoint->UserData = userData;
    LOG("ep=%x,callback=%p\r\r\n",ep,callback);
    if (ep == 0)
        _controller.Transfer(endpoint,TOKEN_SETUP,(u8*)GetSetup(device),8,Endpoint::SetupQueued);
    else
        _controller.Transfer(endpoint,flags & 0x80 ? TOKEN_IN : TOKEN_OUT,data,length,Endpoint::DataQueued);
    if (callback)
        return IO_PENDING;
    return WaitIODone(endpoint);
}

int USBControlTransfer(int device, int request_type, int request, int value, int index, u8* data, int length, USBCallback callback, void * userData)
{
    Setup* setup = GetSetup(device);
    if (!setup)
        return ERR_DEVICE_NOT_FOUND;
       
    // Async control calls may overwrite setup buffer of previous call, so we need to wait before setting up next call
    WaitIODone(_controller.GetEndpoint(device,0));
   
    setup->bm_request_type = request_type;
    setup->b_request = request;
    setup->w_value = value;
    setup->w_index = index;
    setup->w_length = length;
    return USBTransfer(device,0,request_type & DEVICE_TO_HOST,data,length,callback,userData);
}

int  USBInterruptTransfer(int device, int ep, u8* data, int length, USBCallback callback, void* userData)
{
    return USBTransfer(device,ep,(ep & 0x80) | ENDPOINT_INTERRUPT,data,length,callback,userData);
}

int  USBBulkTransfer(int device, int ep, u8* data, int length, USBCallback callback, void* userData)
{
    return USBTransfer(device,ep,(ep & 0x80) | ENDPOINT_BULK,data,length,callback,userData);
}

int GetDescriptor(int device, int descType,int descIndex, u8* data, int length)
{
    return USBControlTransfer(device,DEVICE_TO_HOST | RECIPIENT_DEVICE, GET_DESCRIPTOR,(descType << 8)|(descIndex), 0, data, length, 0);
}

int GetString(int device, int index, char* dst, int length)
{
    u8 buffer[255];
    int le = GetDescriptor(device,DESCRIPTOR_TYPE_STRING,index,buffer,sizeof(buffer));
    if (le < 0)
        return le;
    if (length < 1)
        return -1;
    length <<= 1;
    if (le > length)
        le = length;
    for (int j = 2; j < le; j += 2)
        *dst++ = buffer[j];
    *dst = 0;
    return (le>>1)-1;
}

int SetAddress(int device, int new_addr)
{
    return USBControlTransfer(device,HOST_TO_DEVICE | RECIPIENT_DEVICE, SET_ADDRESS, new_addr, 0, 0, 0, 0);
}

int SetConfiguration(int device, int configNum)
{
    return USBControlTransfer(device,HOST_TO_DEVICE | RECIPIENT_DEVICE, SET_CONFIGURATION, configNum, 0, 0, 0, 0);
}

int SetInterface(int device, int ifNum, int altNum)
{
    return USBControlTransfer(device,HOST_TO_DEVICE | RECIPIENT_INTERFACE, SET_INTERFACE, altNum, ifNum, 0, 0, 0);
}

//    HUB stuff
int SetPortFeature(int device, int feature, int index)
{
    return USBControlTransfer(device,HOST_TO_DEVICE | REQUEST_TYPE_CLASS | RECIPIENT_OTHER,SET_FEATURE,feature,index,0,0);
}

int ClearPortFeature(int device, int feature, int index)
{
    return USBControlTransfer(device,HOST_TO_DEVICE | REQUEST_TYPE_CLASS | RECIPIENT_OTHER,CLEAR_FEATURE,feature,index,0,0);
}

int SetPortPower(int device, int port)
{
    int r = SetPortFeature(device,PORT_POWER,port);
    _controller.DelayMS(20);    // 80ms to turn on a hubs power... DESCRIPTOR? todo
    return r;
}

int SetPortReset(int device, int port)
{
    return SetPortFeature(device,PORT_RESET,port);
}

int GetPortStatus(int device, int port, u32* status)
{
    return USBControlTransfer(device,DEVICE_TO_HOST | REQUEST_TYPE_CLASS | RECIPIENT_OTHER,GET_STATUS,0,port,(u8*)status,4);
}

#endif




/* Adb.cpp */
/*
    Copyright 2011 Niels Brouwers

    Licensed under the Apache License, Version 2.0 (the "License");
    you may not use this file except in compliance with the License.
    You may obtain a copy of the License at

       http://www.apache.org/licenses/LICENSE-2.0

    Unless required by applicable law or agreed to in writing, software
    distributed under the License is distributed on an "AS IS" BASIS,
    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
    See the License for the specific language governing permissions and
    limitations under the License.
*/

/* Changed by Junichi Katsu */

#include <string.h>
#include <Adb.h>

#define DEBUG

#define MAX_BUF_SIZE 128

#ifdef DEBUG
#define  log(...)       printf(__VA_ARGS__)
#else
#define  log(...)       do {} while(0)
#endif

int input_ep;
int output_ep;
int _device;
int _configuration;
int _interfaceNumber;

static Connection * firstConnection;
static boolean connected;
static int connectionLocalId = 1;
unsigned char readbuff[MAX_BUF_SIZE];

// Event handler callback function.
adb_eventHandler * eventHandler;

ADB* _adb;
PacketBuffer recv_packet_buf(100,MAX_BUF_SIZE);
Ticker timer;

int time_ms = 0;


void attime(void)
{
    time_ms++;
}

int millis()
{
    return(time_ms);
}

char *strdup(const char *src)
{
    char *p;
   
    if(src == NULL){
        return NULL;
    }
   
    p = (char *)malloc(strlen(src) + 1);
    if(p != NULL) strcpy(p, src);
    return p;

}

/**
 * Initialises the ADB protocol. This function initialises the USB layer underneath so no further setup is required.
 */
void ADB::init()
{  
    recv_packet_buf.clear();
   
    // Signal that we are not connected.
    _device = 0;
    connected = false;
   

    // Initialise Usb host.
    USBInit();
   
    timer.attach_us(&attime, 1000);
}

/**
 * Sets the ADB event handler function. This function will be called by the ADB layer
 * when interesting events occur, such as ADB connect/disconnect, connection open/close, and
 * connection writes from the ADB device.
 *
 * @param handler event handler function.
 */
void ADB::setEventHandler(adb_eventHandler * handler)
{
    eventHandler = handler;
}

/**
 * Fires an ADB event.
 * @param connection ADB connection. May be NULL in case of global connect/disconnect events.
 * @param type event type.
 * @param length payload length or zero if no payload.
 * @param data payload data if relevant or NULL otherwise.
 */
void ADB::fireEvent(Connection * connection, adb_eventType type, uint16_t length, uint8_t * data)
{
    // Fire the global event handler, if set.
    if (eventHandler!=NULL)
        eventHandler(connection, type, length, data);

    // Fire the event handler of the connection in question, if relevant
    if (connection!=NULL && connection->eventHandler!=NULL)
        connection->eventHandler(connection, type, length, data);
}

/**
 * Adds a new ADB connection. The connection string is per ADB specs, for example "tcp:1234" opens a
 * connection to tcp port 1234, and "shell:ls" outputs a listing of the phone root filesystem. Connections
 * can be made persistent by setting reconnect to true. Persistent connections will be automatically
 * reconnected when the USB cable is re-plugged in. Non-persistent connections will connect only once,
 * and should never be used after they are closed.
 *
 * The connection string is copied into the Connection record and may not exceed ADB_CONNECTIONSTRING_LENGTH-1
 * characters.
 *
 * @param connectionString ADB connectionstring. I.e. "tcp:1234" or "shell:ls".
 * @param reconnect true for automatic reconnect (persistent connections).
 * @param handler event handler.
 * @return an ADB connection record or NULL on failure (not enough slots or connection string too long).
 */
Connection * ADB::addConnection(const char * connectionString, boolean reconnect, adb_eventHandler * handler)
{

    // Allocate a new ADB connection object
    Connection * connection = (Connection*)malloc(sizeof(Connection));
    if (connection == NULL) return NULL;

    // Allocate memory for the connection string
    connection->connectionString = (char*)strdup(connectionString);
    if (connection->connectionString==NULL)
    {
        // Free the connection object and return null
        free(connection);
        return NULL;
    }

    // Initialise the newly created object.
    connection->localID = connectionLocalId ++;
    connection->status = ADB_CLOSED;
    connection->lastConnectionAttempt = 0;
    connection->reconnect = reconnect;
    connection->eventHandler = handler;

    // Add the connection to the linked list. Note that it's easier to just insert
    // at position 0 because you don't have to traverse the list :)
    connection->next = firstConnection;
    firstConnection = connection;

    // Unable to find an empty spot, all connection slots in use.
    return connection;
}

/**
 * Prints an ADB_message, for debugging purposes.
 * @param message ADB message to print.
 */
#ifdef DEBUG
static void adb_printMessage(adb_message * message)
{
    switch(message->command)
    {
    case A_OKAY:
        printf("OKAY message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_CLSE:
        printf("CLSE message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_WRTE:
        printf("WRTE message [%lx] %ld %ld, %ld bytes\r\n", message->command, message->arg0, message->arg1, message->data_length);
        break;
    case A_CNXN:
        printf("CNXN message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_SYNC:
        printf("SYNC message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    case A_OPEN:
        printf("OPEN message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    default:
        printf("WTF message [%lx] %ld %ld\r\n", message->command, message->arg0, message->arg1);
        break;
    }
}
#endif

/**
 * Writes an empty message (without payload) to the ADB device.
 *
 * @param device USB device handle.
 * @param command ADB command.
 * @param arg0 first ADB argument (command dependent).
 * @param arg0 second ADB argument (command dependent).
 * @return error code or 0 for success.
 */
int ADB::writeEmptyMessage(int device, uint32_t command, uint32_t arg0, uint32_t arg1)
{
    adb_message message;

    message.command = command;
    message.arg0 = arg0;
    message.arg1 = arg1;
    message.data_length = 0;
    message.data_check = 0;
    message.magic = command ^ 0xffffffff;

#ifdef DEBUG
    printf("OUT << "); adb_printMessage(&message);
#endif

    int r = USBBulkTransfer( device , output_ep , (uint8_t*)&message , sizeof(adb_message) );

#ifdef DEBUG
    log("[writeMessage1] size:%d\r\n",r);
   
    int ii,jj;
    uint8_t* buf = (uint8_t*)&message;
    for(ii = 0 ; ii < r ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log("\r\n");
        if((ii+jj) > r) break;
    }
#endif

    return r;
}

/**
 * Writes an ADB message with payload to the ADB device.
 *
 * @param device USB device handle.
 * @param command ADB command.
 * @param arg0 first ADB argument (command dependent).
 * @param arg0 second ADB argument (command dependent).
 * @param length payload length.
 * @param data command payload.
 * @return error code or 0 for success.
 */
int ADB::writeMessage(int device, uint32_t command, uint32_t arg0, uint32_t arg1, uint32_t length, uint8_t * data)
{
    adb_message message;
    uint8_t msg[256];
    uint32_t count, sum = 0;
    uint8_t * x;

    // Calculate data checksum
    count = length;
    x = data;
    while(count-- > 0) sum += *x++;

    // Fill out the message record.
    message.command = command;
    message.arg0 = arg0;
    message.arg1 = arg1;
    message.data_length = length;
    message.data_check = (sum);
    message.magic = command ^ 0xffffffff;
   
#ifdef DEBUG
    printf("OUT << "); adb_printMessage(&message);
#endif

    int r = USBBulkTransfer( device , output_ep , (uint8_t*)&message , sizeof(adb_message) );
   
    if (r<0) return r;

#ifdef DEBUG
    log("[writeMessage1] size:%d\r\n",r);
   
    int ii,jj;
    uint8_t* buf = (uint8_t*)&message;
    for(ii = 0 ; ii < r ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log("\r\n");
        if((ii+jj) > r) break;
    }
#endif
   
    memcpy( msg , data , length );
   
    r = USBBulkTransfer( device , output_ep , msg , length );
    log("USB SEND RET2:%d\r\n",r);
   
    if (r<0) return r;
   
#ifdef DEBUG
    log("[writeMessage2] size:%d\r\n",r);
   
    buf = msg;
    for(ii = 0 ; ii < r ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > r) break;
        }
        log("\r\n");
        if((ii+jj) > r) break;
    }
#endif
   
    r = 0;
   
    return r;
}

/**
 * Writes an ADB command with a string as payload.
 *
 * @param device USB device handle.
 * @param command ADB command.
 * @param arg0 first ADB argument (command dependent).
 * @param arg0 second ADB argument (command dependent).
 * @param str payload string.
 * @return error code or 0 for success.
 */
int ADB::writeStringMessage(int device, uint32_t command, uint32_t arg0, uint32_t arg1, char * str)
{
    return ADB::writeMessage(device, command, arg0, arg1, strlen(str) + 1, (uint8_t*)str);
}

/**
 * Poll an ADB message.
 * @param message on success, the ADB message will be returned in this struct.
 * @param poll true to poll for a packet on the input endpoint, false to wait for a packet. Use false here when a packet is expected (i.e. OKAY in response to WRTE)
 * @return true iff a packet was successfully received, false otherwise.
 */
boolean ADB::pollMessage(adb_message * message, boolean poll)
{
    int bytesRead = 0;
    uint8_t buf[ADB_USB_PACKETSIZE];

    // Poll a packet from the USB
    bytesRead = recv_packet_buf.GetPacket((char*)buf);

    // Check if the USB in transfer was successful.
    if (bytesRead<=0) return false;

    log("[pollMessage] byteRead size:%d\r\n",bytesRead);
   
    // Check if the buffer contains a valid message
    memcpy((void*)message, (void*)buf, sizeof(adb_message));

    // If the message is corrupt, return.
#if 1
    if (message->magic != (message->command ^ 0xffffffff))
    {
#ifdef DEBUG
        printf("Broken message, magic mismatch, %d bytes\r\n", bytesRead);
        return false;
#endif
    }
#endif
    // Check if the received number of bytes matches our expected 24 bytes of ADB message header.
    if (bytesRead != sizeof(adb_message)) return false;

    return true;
}

/**
 * Sends an ADB OPEN message for any connections that are currently in the CLOSED state.
 */
void ADB::openClosedConnections()
{
    //printf("Open Closed Connections\r\n");
    uint32_t timeSinceLastConnect;
    Connection * connection;

    // Iterate over the connection list and send "OPEN" for the ones that are currently closed.
    for (connection = firstConnection; connection!=NULL; connection = connection->next)
    {
        timeSinceLastConnect = millis() - connection->lastConnectionAttempt;
        if (connection->status==ADB_CLOSED && timeSinceLastConnect>ADB_CONNECTION_RETRY_TIME)
        {
            // Issue open command.
            ADB::writeStringMessage(_device, A_OPEN, connection->localID, 0, connection->connectionString);

            // Record the last attempt time
            connection->lastConnectionAttempt = millis();
            connection->status = ADB_OPENING;

        }
    }

}

/**
 * Handles and ADB OKAY message, which represents a transition in the connection state machine.
 *
 * @param connection ADB connection
 * @param message ADB message struct.
 */
void ADB::handleOkay(Connection * connection, adb_message * message)
{
    // Check if the OKAY message was a response to a CONNECT message.
    if (connection->status==ADB_OPENING)
    {
        connection->status = ADB_OPEN;
        connection->remoteID = message->arg0;

        ADB::fireEvent(connection, ADB_CONNECTION_OPEN, 0, NULL);
    }

    // Check if the OKAY message was a response to a WRITE message.
    if (connection->status == ADB_WRITING)
        connection->status = ADB_OPEN;

}

/**
 * Handles an ADB CLOSE message, and fires an ADB event accordingly.
 *
 * @param connection ADB connection
 */
void ADB::handleClose(Connection * connection)
{
    // Check if the CLOSE message was a response to a CONNECT message.
    if (connection->status==ADB_OPENING)
        ADB::fireEvent(connection, ADB_CONNECTION_FAILED, 0, NULL);
    else
        ADB::fireEvent(connection, ADB_CONNECTION_CLOSE, 0, NULL);

    // Connection failed
    if (connection->reconnect)
        connection->status = ADB_CLOSED;
    else
        connection->status = ADB_UNUSED;

}

/**
 * Handles an ADB WRITE message.
 *
 * @param connection ADB connection
 * @param message ADB message struct.
 */
void ADB::handleWrite(Connection * connection, adb_message * message)
{
    uint32_t bytesLeft = message->data_length;
    uint8_t buf[ADB_USB_PACKETSIZE];
    ConnectionStatus previousStatus;
    int bytesRead;

    previousStatus = connection->status;

    connection->status = ADB_RECEIVING;
    connection->dataRead = 0;
    connection->dataSize = message->data_length;

    while (bytesLeft>0)
    {
        int len = bytesLeft < ADB_USB_PACKETSIZE ? bytesLeft : ADB_USB_PACKETSIZE;

        // Read payload
        bytesRead = recv_packet_buf.GetPacket((char*)buf);
       

        // Poll the USB layer.
        USBLoop();
       
        log("[handleWrite] byteRead size:%d\r\n",bytesRead);

//        if (len != bytesRead)
//            printf("bytes read mismatch: %d expected, %d read, %ld left\r\n", len, bytesRead, bytesLeft);

        // Break out of the read loop if there's no data to read :(
        if (bytesRead==-1) break;
        else if(bytesRead!=0)
        {
            connection->dataRead += len;
            ADB::fireEvent(connection, ADB_CONNECTION_RECEIVE, len, buf);

            bytesLeft -= bytesRead;
        }
    }

    // Send OKAY message in reply.
    bytesRead = ADB::writeEmptyMessage(_device, A_OKAY, message->arg1, message->arg0);

    connection->status = previousStatus;
   
}

/**
 * Close all ADB connections.
 *
 * @param connection ADB connection
 * @param message ADB message struct.
 */
void ADB::closeAll()
{
    Connection * connection;

    // Iterate over all connections and close the ones that are currently open.
    for (connection = firstConnection; connection != NULL; connection = connection->next)
        if (!(connection->status==ADB_UNUSED || connection->status==ADB_CLOSED))
            ADB::handleClose(connection);

}

/**
 * Handles an ADB connect message. This is a response to a connect message sent from our side.
 * @param message ADB message.
 */
void ADB::handleConnect(adb_message * message)
{
    unsigned int bytesRead;
    uint8_t buf[MAX_BUF_SIZE];
    uint16_t len;

    // Read payload (remote ADB device ID)
    len = message->data_length < MAX_BUF_SIZE ? message->data_length : MAX_BUF_SIZE;
    bytesRead = recv_packet_buf.GetPacket((char*)buf);
   
    log("[handleConnect] byteRead size:%d\r\n",bytesRead);

    // Signal that we are now connected to an Android device (yay!)
    connected = true;

    // Fire event.
    ADB::fireEvent(NULL, ADB_CONNECT, len, buf);

}

/**
 * This method is called periodically to check for new messages on the USB bus and process them.
 */
void ADB::poll()
{

    //printf("Polling!\r\n");

    Connection * connection;
    adb_message message;

    // Poll the USB layer.
    USBLoop();

   

    // If no USB device, there's no work for us to be done, so just return.
    if (_device==0) printf("No Device\r\n");
    if (_device==0) return;

   
   
    // If not connected, send a connection string to the device.
    if (!connected)
    {
        ADB::writeStringMessage(_device, A_CNXN, 0x01000000, 4096, (char*)"host::microbridge");
        for(int ii=0;ii<2000;ii++)
        {
            USBLoop();
            wait_ms(1);
        }
        wait_ms(500); // Give the device some time to respond.
    }

   
    // If we are connected, check if there are connections that need to be opened
    if (connected)
        ADB::openClosedConnections();

    // Check for an incoming ADB message.
    if (!ADB::pollMessage(&message, true))
        return;

    // Handle a response from the ADB device to our CONNECT message.
    if (message.command == A_CNXN)
        ADB::handleConnect(&message);

    // Handle messages for specific connections
    for (connection = firstConnection; connection != NULL; connection = connection->next)
    {
        if(connection->status!=ADB_UNUSED && connection->localID==message.arg1)
        {
            switch(message.command)
            {
            case A_OKAY:
                printf("HANDLE OKEY\r\n");
                ADB::handleOkay(connection, &message);
                break;
            case A_CLSE:
                printf("HANDLE CLOSE\r\n");
                ADB::handleClose(connection);
                break;
            case A_WRTE:
                printf("HANDLE WRITE\r\n");
                ADB::handleWrite(connection, &message);
                break;
            default:
                break;
            }
        }
    }

}

void ADB::AdbreadCallback(int device, int endpoint, int status, u8* buf, int len, void* userData) {
   
    recv_packet_buf.PutPacket((char*)buf,len);
   
#ifdef DEBUG
    log("[AdbreadCallback] size:%d\r\n",len);
   
    int ii,jj;
    for(ii = 0 ; ii < len ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > len) break;
        }
        log(" : ");
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%c%c",buf[ii+jj],buf[ii+jj+1]);
            if((ii+jj) > len) break;
        }
        log("\r\n");
        if((ii+jj) > len) break;
    }
#endif
    USBBulkTransfer(device, endpoint ,readbuff,sizeof(readbuff), AdbreadCallback, userData);
  //  wait_ms(4);
}

/**
 * Checks whether the a connected USB device is an ADB device and populates a configuration record if it is.
 *
 * @param device USB device.
 * @param handle pointer to a configuration record. The endpoint device address, configuration, and endpoint information will be stored here.
 * @return true if the device is an ADB device.
 */
boolean ADB::isAdbDevice(int device, int configuration, int interfaceNumber)
{
    boolean ret = false;
   
    log("connecting Android \r\n");
   
    _device = device;
    _configuration = configuration;
    _interfaceNumber = interfaceNumber;
   
    log("device = %d configuration = %d interfaceNumber = %d\r\n", device, configuration, interfaceNumber);
   
    int err;
   
    u8 buffer[255];
    err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,4);
   
    if (err < 0) {
        log("Failed to get descriptor\r\n");
        return(ret);
    }
   
    int len = buffer[2] | (buffer[3] << 8);
    if (len > sizeof(buffer)) {
        log("config descriptor too large\r\n");
        /* might want to truncate here */
        return(ret);
    }
    err = GetDescriptor(_device,DESCRIPTOR_TYPE_CONFIGURATION,0,buffer,len);
    u8* p = buffer;
    input_ep=0;
    output_ep=0;
    EndpointDescriptor *epDesc;
   
    log("Descriptor size:%d\r\n",len);
    int ii,jj;
    for(ii = 0 ; ii < len ; ii+=16)
    {
        for(jj = 0 ; jj < 16 ; jj+=2 )
        {
            log("%02X%02X ",buffer[ii+jj],buffer[ii+jj+1]);
            if((ii+jj) > len) break;
        }
        log("\r\n");
        if((ii+jj) > len) break;
    }
    u8 interface_num = 0;
   
    while (p<(buffer+len)) {
        u8 descLen  = p[0];
        u8 descType = p[1];
        log("descLen=%d,descType=%d\r\n",descLen,descType);
        switch (descType) {
            case DESCRIPTOR_TYPE_CONFIGURATION:
                log("config desc\r\n");
                break;
            case DESCRIPTOR_TYPE_INTERFACE:
                interface_num = p[2];
                log("interface desc num[%d]\r\n",interface_num);
                break;
            case DESCRIPTOR_TYPE_ENDPOINT:
                epDesc=(EndpointDescriptor*)p;
                if( interface_num == 1 )
                {
                    if (!input_ep && (epDesc->bEndpointAddress& 0x80)) {
                        input_ep=epDesc->bEndpointAddress& 0x7f;
                        printf(" >>> input_ep=%d\r\n",input_ep);
                        //PacketSize drop
                        log("input Endpoint address=%d,wMaxPacketSize=%d,bmAttributes=%d\r\n",input_ep,epDesc->wMaxPacketSize,epDesc->bmAttributes);

                    } else if (!output_ep) {
                        output_ep=epDesc->bEndpointAddress& 0x7f;
                        //PacketSize drop
                        log("output Endpoint address=%d,wMaxPacketSize=%d,bmAttributes=%d\r\n",output_ep,epDesc->wMaxPacketSize,epDesc->bmAttributes);
                    } else {
                        //other
                        log("non input,output Endpoint address=%d,wMaxPacketSize=%d,bmAttributes=%d\r\n",input_ep,epDesc->wMaxPacketSize,epDesc->bmAttributes);
                    }
                }
                break;
            default:
                log("unkown desc type(%d) \r\n",descType);
        }
        p+=descLen;
    }
   
    input_ep=129;
    output_ep=1;
    
   
    if (!(input_ep && output_ep)) {
        log("can't find accessory endpoints\r\n");
        return(false);
    }
   
    log("SetConfiguration\r\n");
    err = SetConfiguration(device,configuration);
    if (err < 0) {
        log("SetConfiguration error\r\n");
        return(false);
    }
   
    log("interrupt setup\r\n");
    //interrupt setup
    if (IO_PENDING!=USBBulkTransfer(_device,input_ep|0x80,readbuff,sizeof(readbuff),AdbreadCallback,NULL))    return(ret);
   
    log("ADB Standby\r\n");
    ret = true;
   
    return(ret);
}


void desconnect(void)
{
    ADB::closeAll();
    _device = 0;
    connected = false;
}

/**
 * Write a set of bytes to an open ADB connection.
 *
 * @param connection ADB connection to write the data to.
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int ADB::write(Connection * connection, uint16_t length, uint8_t * data)
{
    int ret;

    // First check if we have a working ADB connection
    if (_device==0 || !connected) return -1;

    // Check if the connection is open for writing.
    if (connection->status != ADB_OPEN) return -2;

    // Write payload
    ret = ADB::writeMessage(_device, A_WRTE, connection->localID, connection->remoteID, length, data);
    if (ret==0)
        connection->status = ADB_WRITING;

    return ret;
}

/**
 * Write a string to an open ADB connection. The trailing zero is not transmitted.
 *
 * @param connection ADB connection to write the data to.
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int ADB::writeString(Connection * connection, char * str)
{
    int ret;

    // First check if we have a working ADB connection
    if (_device==0 || !connected) return -1;

    // Check if the connection is open for writing.
    if (connection->status != ADB_OPEN) return -2;

    // Write payload
    ret = ADB::writeStringMessage(_device, A_WRTE, connection->localID, connection->remoteID, str);
    if (ret==0)
        connection->status = ADB_WRITING;

    return ret;
}

/**
 * Write a set of bytes to this ADB connection.
 *
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int Connection::write(uint16_t length, uint8_t * data)
{
    return ADB::write(this, length, data);
}

/**
 * Write a string to this connection.
 *
 * @param length number of bytes to transmit.
 * @param data data to send.
 * @return number of transmitted bytes, or -1 on failure.
 */
int Connection::writeString(char * str)
{
    return ADB::writeString(this, str);
}

/**
 * Checks if the connection is open for writing.
 * @return true iff the connection is open and ready to accept write commands.
 */
bool Connection::isOpen()
{
    return this->status == ADB_OPEN;
}


/** from USBHost load function. initialize Android device**/
void OnLoadDevice(int device, DeviceDescriptor* deviceDesc, InterfaceDescriptor* interfaceDesc) {
    char s[128];
   
    log("LoadDevice %d %02X:%02X:%02X\r\n",device,interfaceDesc->bInterfaceClass,interfaceDesc->bInterfaceSubClass,interfaceDesc->bInterfaceProtocol);
   
    for (int i = 1; i < 4; i++) {
        if (GetString(device,i,s,sizeof(s)) < 0)
            break;
        printf("%d: %s\r\n",i,s);
    }
   
    // Adb?
    if(1)
    {
        ADB::isAdbDevice(device,1,2);
    }
   
}