File content as of revision 1:4676e8b9b357:

#include "mbed.h"
#include "USBHost.h"
#include "ftlibclasstxcbt.h"
#include "ftErrCode.h"
#include "Utils.h"

ftusbdevtxbt::ftusbdevtxbt(BTDevice *bd): ftusbdevtx() {
    init = idle;
    message_pending = 0;
    parseState = 0;
    if (bd) {
        memcpy((char*)&btaddr, (char*)bd->GetAddress(), sizeof(BD_ADDR));
        name = bd->_name;
    }
    set_baudrate(38400);//no effect
    //further com not possible because device is not open
//    send_msg(1); //ping
//    GetFtSerialNr();
}

unsigned ftusbdevtxbt::InitFtUsbDeviceList() {
    ftusbdev::InitFtUsbDeviceList();
    BTDevice* btdevs[8];
    int count = Bluetooth.GetDevices(btdevs,8);
    for (int i = 0; i < count; i++)
        //if (memcmp(devs[i]->_info.dev_class, FtDevClass, 3)==0)
    {
        devs.push_back(new ftusbdevtxbt(btdevs[i]));
        printf("%d: %s\n", i, btdevs[i]->_name);
    }
    return 0;
}

int ftusbdevtxbt::read_data(unsigned char *data, int size, USBCallback callback, void* userData) {
    return 0;
}

int ftusbdevtxbt::write_data(unsigned char *data, int size, USBCallback callback, void* userData) {
    return Socket_Send(device, data, size);
}

unsigned ftusbdevtxbt::GetFtSerialNr() {
    return sn;
}

void ftusbdevtxbt::parse (const unsigned char *buf, unsigned len) {
    unsigned i = 0;
    //printf("start parse, state = %d, len = %d\n", parseState, len);
    while (i < len) {
        char c = buf[i++];
        switch (parseState) {
            case 0: //ascii state
                if (c==2)
                    parseState = 1;
                else {
                    putc(c, stdout);
                    console.put(c);
                }
                break;
            case 1:
                if (c==0x55)
                    parseState = 2;
                else {
                    parseState = 0;
                    printf("expected 0x55 in X1 header, found %02X\n", c);
                }
                break;
            case 2:
                X1_len = c<<8;
                X1_crc = c;
                parseState= 3;
                break;
            case 3:
                X1_len += c;
                X1_crc += c;
                parseState= 4;
                X1_pkt = new unsigned char[X1_len];
                //printf("net length = %d\n", X1_len);
                X1_pos = 0;
                break;
            case 4:
                if  (X1_pos < X1_len) {
                    X1_pkt[X1_pos++] = c;
                    X1_crc += c;
                } else { //c is first checksum byte (MSB!)
                    parseState = 6;
                    X1_crc += c<<8;
                }
                break;
            case 5:
                //X1_crc = c<<8;
                X1_crc += c;
                parseState= 6;
                break;
            case 6:
                X1_crc += c;
                parseState= 7;
                break;
            case 7:
                if (c == 3 && X1_crc == 0) {
                    //printHex(X1_pkt, X1_len);
                  if (transferAktiv == FTX1SYNC)
                    FtThreadEnd();
                  else
                    X1_cmd = rec_msg2((headerbody*)X1_pkt, X1_len);
                  delete[] X1_pkt;
                } else
                    printf("framing or checksum error, end char = %02X, sum=%04X\n", c, X1_crc);

                parseState = 0;
                break;
        }
    }
    //printf("Leaving parse, state = %d, i = %d (last char = %02X)\n", parseState, i, buf[i-1]);
}

void ftusbdevtxbt::receive(int socket, SocketState state, const u8* data, int len) {
    //printf("ftusbdevtxbt::receive was called: socket %d, state=%d, length=%d\n", socket, state, len);
    static const unsigned char req[] = "\xdget_ser_num\xd";
    char line[80];
    if (len==0) {
        switch (state) {
            case SocketState_Opening:
                break;
            case SocketState_Open:
                console.flush();
                send_msg(1, 0, false); //ping
                init = ping;
                break;
            case SocketState_Closing:
            case SocketState_Closed:
                return;
        }
    } else {
        //printHex(data, len);
        parse(data, len);
        switch (init) {
            case idle:
                break;
            case ping:
                if (X1_cmd == 1) {
                    printf("sending sernum request \n%s\n", req);
                    Socket_Send(socket, req, strlen((char*)req));
                    init = serial;
                }
                break;
            case serial:
                while (console.gets(line, 80)==0) {
                    printf("Line=%s\n", line);
                    if (sscanf(line, "%d", &sn) == 1) {
                        printf("Got S/N=%d\n", sn);
                        send_msg(6, 1, false); //get INFO for master
                        init = info;
                    }
                }
                break;
            case info:
                if (X1_cmd == 6) {
                    printf("Master=%s\n", ta[0]->info.device_name);
                    send_msg(7, 1, false); //get state for master
                    init = slaves;
                }
                break;
            case slaves:
                if (X1_cmd == 7) {/*
                    for (int i = 1; i <= N_EXT_DEV; i++)
                        if (ta[0]->state.ext_dev_connect_state[i-1]) {
                            if (ta[i] == 0) { //new slave
                                ta[i] = new TA;
                                memset(ta[i], 0, sizeof(TA));
                                active |= 1<<i;
                                printf("New Extension %d = %s\n", i, ta[i]->info.device_name);
                                if (cbRoboExtState) cbRoboExtState(i, 1);
                            }
                        } else {
                            if (ta[i]) {//should not happen during initialisation
                                delete ta[i];
                                ta[i] = 0;
                                active &= ~(1<<i);
                                printf("Extension %d went offline\n", i);
                                if (cbRoboExtState) cbRoboExtState(i, 0);
                            }
                        }
                    if (active != 1) {
                        send_msg(6, active & ~1, false); //get info for slaves;
                        init = slaves_info;
                    } else*/
                        init = ready;
                }
                break;
            case slaves_info:
                if (X1_cmd == 6) {
                    init = ready;
                }
                break;
            case ready://normal message processing
                break;
        }
    }
}

unsigned ftusbdevtxbt::OpenFtUsbDevice() {
    ta[0] = new TA;
    memset(ta[0], 0, sizeof(TA));
    active = 1;
    L2CAPAddr s;
    s.bdaddr = btaddr;
    s.psm = 1;//abuse the psm for the channelID
    device = Socket_Open(SOCKET_RFCOM, &s.hdr, recv, this);//Open the serial connection via RFCOMM
    if (device<=0) {
        printf("Opening of RFCOMM socket for ftdevice failed (%d)\n", device);
        delete ta[0];
        ta[0] = 0;
        return FTLIB_ERR_FAILED;
    }
    return 0;
}

unsigned ftusbdevtxbt::CloseFtDevice() {
    if (ta[0]==0)
        return FTLIB_ERR_DEVICE_NOT_OPEN;
    while (transferAktiv != FTX1STOP) {
        fprintf(stderr, "Transfer ta still active\n");
        wait(1);
    }
    for (int i = 0; i < N_EXT_DEV; i++)
        if (ta[i]!=0) {
            delete ta[i];
            ta[i] = 0;
        }
    init = idle;
    unsigned retval = Socket_Close(device);
    if (retval) {
      printf("Socket_Close(%d) returned %d\n", device, retval);
      return FTLIB_ERR_FAILED; //or whatever
    }
    return 0;
}



void ftusbdevtxbt::FtThreadBegin() {
    if (init != ready) {//still initialising
      busy = false;
      return;
    }
    ftusbdevtx::FtThreadBegin();
}

void ftusbdevtxbt::FtThreadEnd() {
    if (!test_and_set()) {//skip when busy
        busy = false;
        printf("TA busy: skip end slot\n");
        return;
    }
    X1_cmd = rec_msg2((headerbody*)X1_pkt, X1_len); //parse the message
    increment(); //release the TA mutex
    if (ne.NotificationCallback) {
        ne.NotificationCallback(ne.Context);
    }
    transferAktiv = FTX1RUN;//TA update complete
    if (tid%100 == 0) { //about every 1 seconds
//        getSlaveInfo(); //synchronous! TODO: change to non-blocking
    }
    busy = false;  //ready for the next tick
//    printf("Busy released\n");
}
#if 0

UINT16 ftusbdevtxbt::checksum(unsigned char *p, unsigned n) {
    UINT16 sum = 0;
    do {
        sum -= *p++;
    } while (--n);
    return (sum<<8)|(sum>>8);
}

void ftusbdevtxbt::dump_buffer(int size) {
    if (buffer==0) {
        printf("No buffer\n");
        return;
    }
    for (int i = 0; i < size; i++) {
        printf("%02X ", buffer[i]);
        if (i%16 == 15) printf("\n");
    }
    printf("----------------------\n");
}

unsigned ftusbdevtxbt::send_msg(unsigned cmd, unsigned set, bool sync) {
    const unsigned sizes[] = {0, 0, //cmd 1
                              sizeof(TA_OUTPUT)+sizeof(UINT32), //cmd 2
                              0, //unknown
                              0, //unknown
                              sizeof(TA_CONFIG)+sizeof(UINT32), //cmd 5
                              sizeof(UINT32), //cmd 6
                              sizeof(UINT32), //cmd 7
                              sizeof(DISPLAY_MSG)+sizeof(UINT32), //cmd 8
                              DEV_NAME_LEN + 1+3 + sizeof(UINT32) //cmd 9
                             };
    int ret = 0;
    int n = 0;
    for (int i = TA_LOCAL; i < TA_N_PARTS; i++) {
        if (set & (1<<i)) {
            n++;
        }
    }
    unsigned size = 0;
    if (cmd < sizeof(sizes)/sizeof(unsigned))
        size = sizes[cmd];
    unsigned payload_size = n*size;
    num_read = sizeof(header)+sizeof(trailer);
    unsigned total_size = sizeof(header)+payload_size+sizeof(trailer);
    buffer = new unsigned char[total_size]; //size must be known in advance

    unsigned net_size = total_size-sizeof(UINT32)-sizeof(trailer);//exclude start,len,crc,etx
    header *hdr = (header*)buffer;
    hdr->start = 0x5502;
    hdr->bytesH = net_size>>8;
    hdr->bytesL = net_size&0xff;
    hdr->snd = 2;
    hdr->rec = 1;
    hdr->trans = ++tid;
    hdr->session = sid;
    hdr->cmd = cmd;
    hdr->structs = n;
    unsigned char* payload = buffer+sizeof(header);
    for (int i = TA_LOCAL; i < TA_N_PARTS; i++) {
        if (ta[i]==0 || !(set & (1<<i))) {
            if (set & (1<<i)) printf("TX %d is not open yet!\n", i);
            continue;
        }
        unsigned *id = (unsigned*)payload;
        *id = i;
        payload = (unsigned char*)(id+1);
        num_read += sizeof(UINT32);
        switch (cmd) {
            case 1:
                break;
            case 2:
                memcpy(payload, &ta[i]->output, sizeof(TA_OUTPUT));
                payload += sizeof(TA_OUTPUT);
                num_read += sizeof(TA_INPUT);
                break;
            case 5:
                memcpy(payload, &ta[i]->config, sizeof(TA_CONFIG));
                payload += sizeof(TA_OUTPUT);
                break;
            case 6:
                num_read += sizeof(TA_INFO);
                break;
            case 7:
                num_read += sizeof(TA_STATE);
                break;
            case 8:
                memcpy(payload, &ta[i]->display.display_msg, sizeof(DISPLAY_MSG));
                payload += sizeof(DISPLAY_MSG);
                break;
            case 9:
                memcpy(payload, ta[i]->info.device_name, DEV_NAME_LEN + 1+3);
                payload += sizeof(DEV_NAME_LEN + 1+3);
                break;
            default:
                printf("Unknown message type %d\n", cmd);
                break;
        }
    }
    trailer *trl = (trailer*)payload;
    trl->chksum = checksum(buffer+2, net_size+2);
    trl->etx = '\x03';
    num_read--;
    //dump_buffer(total_size);
    //printf("Expecting %d bytes for transaction %d\n", num_read, tid);
    if (sync) {
        ret = USBBulkTransfer(device, 3, buffer, total_size-1);//send 1 less because trailer is 1 too long due to alignment
        delete[] buffer;
        if (ret < 0)
            printf("synchronous send_message failed (%d)\n", ret);
        else {
            buffer = new unsigned char[num_read];
            ret = USBInterruptTransfer(device, 0x82, buffer, num_read);
            if (ret < 0) {
                printf("synchronous read failed (%d)\n", ret);
                delete[] buffer;
            } else {
                //dump_buffer(num_read);
                rec_msg();
            }
        }
    } else
        USBBulkTransfer(device, 3, buffer, total_size-1, &ftusbdevtxbt::write_finished_cb, this);
    return ret;
}

unsigned ftusbdevtxbt::rec_msg() {
    header *hdr = (header*)buffer;
    //dump_buffer(num_read);
    if (hdr->start != 0x5502) printf("Invalid packet\n");
    unsigned net_size = (hdr->bytesH<<8) + hdr->bytesL;
    if (net_size+7 != num_read) printf("message has %d bytes, was expecting %d bytes\n", net_size+7, num_read);
    //printf("message %d from %d\n", hdr->cmd, hdr->snd);
    if (hdr->rec != 2) printf("Wrong destination (%d)\n", hdr->rec);
    if (hdr->trans != tid) printf("Response to request %d, expected %d\n", hdr->trans, tid);
    if (hdr->session != sid) {
        printf("Session number has changed from %d to %d\n", sid, hdr->session);
        sid = hdr->session;
        tid = 1; //restart transaction sequence
    }
    unsigned cmd = hdr->cmd - 100;
    unsigned n = hdr->structs;
    unsigned char* payload = buffer+sizeof(header);
    trailer *trl = (trailer*)(buffer+net_size+4);
    for (int i = 0; i < n && payload < (unsigned char*)trl; i++) {
        unsigned *id = (unsigned*)payload;
        if (*id >= TA_N_PARTS) {
            printf("Illegal extension nr %d\n", *id);
            continue;
        }
        if (ta[*id]==0) {
            printf("Message for new device %d\n", *id);
            continue;//skip the copy to avoid assignment to null ta but the payload pointer is not advanced! stopping further copying
        }
        payload = (unsigned char*)(id+1);
        switch (cmd) {
            case 1:
                break;
            case 2:
                memcpy(&ta[*id]->input, payload, sizeof(TA_INPUT));
                payload += sizeof(TA_INPUT);
                break;
            case 5:
                break;
            case 6:
                memcpy(&ta[*id]->info, payload, sizeof(TA_INFO));
                payload += sizeof(TA_INFO);
                break;
            case 7:
                memcpy(&ta[*id]->state, payload, sizeof(TA_STATE));
                payload += sizeof(TA_STATE);
                break;
            case 8:
                break;
            case 9:
                break;
            default:
                printf("Unknown message type %d\n", cmd);
                break;
        }
    }
    if (payload != (unsigned char*)trl) printf("expected %d sections with in total %d bytes; got %d bytes\n", n, payload-buffer-sizeof(header), net_size-sizeof(header));
    if (trl->chksum != checksum(buffer+2, net_size+2)) printf("Checksum error\n");
    if (trl->etx != '\x03') printf("Expected ETX(03), got %02X\n", trl->etx);
    delete[] buffer;
    return 0;
}

void ftusbdevtxbt::read_finished_cb(int device, int endpoint, int status, u8* data, int len, void* userData) {
//end of reply transfer
//printf("read_finished_cb: %d bytes\n", len);
    ftusbdevtx *fth = (ftusbdevtx*)userData;
    if (fth->transferAktiv == FTX1SYNC)
        fth->FtThreadEnd();
    else
        fth->rec_msg();
}

void ftusbdevtxbt::write_finished_cb(int device, int endpoint, int status, u8* data, int len, void* userData) { //end of request transfer, issue, reply transfer
    ftusbdevtx *fth = (ftusbdevtx*)userData;
//printf("write_finished_cb: wrote %d bytes, reading %d bytes\n", len, fth->num_read);
    delete[] fth->buffer;
    fth->buffer = new unsigned char[fth->num_read];
    USBInterruptTransfer(fth->device, 0x82, fth->buffer, fth->num_read, read_finished_cb, fth);
}
#endif