예제 #1
0
static int
sg_cmds_process_helper(const char * leadin, int mx_di_len, int resid,
                       const uint8_t * sbp, int slen, bool noisy,
                       int verbose, int * o_sense_cat)
{
    int scat, got;
    bool n = false;
    bool check_data_in = false;
    char b[512];

    scat = sg_err_category_sense(sbp, slen);
    switch (scat) {
    case SG_LIB_CAT_NOT_READY:
    case SG_LIB_CAT_INVALID_OP:
    case SG_LIB_CAT_ILLEGAL_REQ:
    case SG_LIB_LBA_OUT_OF_RANGE:
    case SG_LIB_CAT_ABORTED_COMMAND:
    case SG_LIB_CAT_COPY_ABORTED:
    case SG_LIB_CAT_DATA_PROTECT:
    case SG_LIB_CAT_PROTECTION:
    case SG_LIB_CAT_NO_SENSE:
    case SG_LIB_CAT_MISCOMPARE:
        n = false;
        break;
    case SG_LIB_CAT_RECOVERED:
    case SG_LIB_CAT_MEDIUM_HARD:
        check_data_in = true;
#if defined(__GNUC__)
#if (__GNUC__ >= 7)
        __attribute__((fallthrough));
        /* FALL THROUGH */
#endif
#endif
    case SG_LIB_CAT_UNIT_ATTENTION:
    case SG_LIB_CAT_SENSE:
    default:
        n = noisy;
        break;
    }
    if (verbose || n) {
        if (leadin && (strlen(leadin) > 0))
            pr2ws("%s:\n", leadin);
        sg_get_sense_str(NULL, sbp, slen, (verbose > 1),
                         sizeof(b), b);
        pr2ws("%s", b);
        if ((mx_di_len > 0) && (resid > 0)) {
            got = mx_di_len - resid;
            if ((verbose > 2) || check_data_in || (got > 0))
                pr2ws("    %s requested %d bytes (data-in) but got %d "
                      "bytes\n", pass_through_s, mx_di_len, got);
        }
    }
    if (o_sense_cat)
        *o_sense_cat = scat;
    return -2;
}
예제 #2
0
int main(int argc, char * argv[])
{
    int sg_fd, k, ok, dsize, res, duration, resid, cat, got, slen;
    unsigned char inq_cdb [INQ_CMD_LEN] =
    {0x12, 0, 0, 0, INQ_REPLY_LEN, 0};
    unsigned char tur_cdb [TUR_CMD_LEN] =
    {0x00, 0, 0, 0, 0, 0};
    unsigned char inqBuff[INQ_REPLY_LEN];
    char * file_name = 0;
    char b[512];
    unsigned char sense_b[32];
    int verbose = 0;
    struct sg_pt_base * ptvp;

    for (k = 1; k < argc; ++k) {
        if (0 == strcmp("-v", argv[k]))
            verbose = 1;
        else if (0 == strcmp("-vv", argv[k]))
            verbose = 2;
        else if (0 == strcmp("-vvv", argv[k]))
            verbose = 3;
        else if (*argv[k] == '-') {
            printf("Unrecognized switch: %s\n", argv[k]);
            file_name = 0;
            break;
        }
        else if (0 == file_name)
            file_name = argv[k];
        else {
            printf("too many arguments\n");
            file_name = 0;
            break;
        }
    }
    if (0 == file_name) {
        printf("Usage: 'sg_simple5 [-v|-vv|-vvv] <device>'\n");
        return 1;
    }

    sg_fd = scsi_pt_open_device(file_name, 1 /* ro */, 0);
    /* N.B. An access mode of O_RDWR is required for some SCSI commands */
    if (sg_fd < 0) {
        fprintf(stderr, "error opening file: %s: %s\n",
                file_name, safe_strerror(-sg_fd));
        return 1;
    }

    dsize = sizeof(inqBuff);
    ok = 0;

    ptvp = construct_scsi_pt_obj();     /* one object per command */
    if (NULL == ptvp) {
        fprintf(stderr, "sg_simple5: out of memory\n");
        return -1;
    }
    set_scsi_pt_cdb(ptvp, inq_cdb, sizeof(inq_cdb));
    set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
    set_scsi_pt_data_in(ptvp, inqBuff, dsize);
    res = do_scsi_pt(ptvp, sg_fd, CMD_TIMEOUT_SECS, verbose);
    if (res < 0) {
        fprintf(stderr, "  pass through os error: %s\n",
                safe_strerror(-res));
        goto finish_inq;
    } else if (SCSI_PT_DO_BAD_PARAMS == res) {
        fprintf(stderr, "  bad pass through setup\n");
        goto finish_inq;
    } else if (SCSI_PT_DO_TIMEOUT == res) {
        fprintf(stderr, "  pass through timeout\n");
        goto finish_inq;
    }
    if ((verbose > 1) && ((duration = get_scsi_pt_duration_ms(ptvp)) >= 0))
        fprintf(stderr, "      duration=%d ms\n", duration);
    resid = get_scsi_pt_resid(ptvp);
    switch ((cat = get_scsi_pt_result_category(ptvp))) {
    case SCSI_PT_RESULT_GOOD:
        got = dsize - resid;
        if (verbose && (resid > 0))
            fprintf(stderr, "    requested %d bytes but "
                    "got %d bytes)\n", dsize, got);
        break;
    case SCSI_PT_RESULT_STATUS: /* other than GOOD and CHECK CONDITION */
        if (verbose) {
            sg_get_scsi_status_str(get_scsi_pt_status_response(ptvp),
                                   sizeof(b), b);
            fprintf(stderr, "  scsi status: %s\n", b);
        }
        goto finish_inq;
    case SCSI_PT_RESULT_SENSE:
        slen = get_scsi_pt_sense_len(ptvp);
        if (verbose) {
            sg_get_sense_str("", sense_b, slen, (verbose > 1),
                             sizeof(b), b);
            fprintf(stderr, "%s", b);
        }
        if (verbose && (resid > 0)) {
            got = dsize - resid;
            if ((verbose) || (got > 0))
                fprintf(stderr, "    requested %d bytes but "
                        "got %d bytes\n", dsize, got);
        }
        goto finish_inq;
    case SCSI_PT_RESULT_TRANSPORT_ERR:
        if (verbose) {
            get_scsi_pt_transport_err_str(ptvp, sizeof(b), b);
            fprintf(stderr, "  transport: %s", b);
        }
        goto finish_inq;
    case SCSI_PT_RESULT_OS_ERR:
        if (verbose) {
            get_scsi_pt_os_err_str(ptvp, sizeof(b), b);
            fprintf(stderr, "  os: %s", b);
        }
        goto finish_inq;
    default:
        fprintf(stderr, "  unknown pass through result "
                "category (%d)\n", cat);
        goto finish_inq;
    }

    ok = 1;
finish_inq:
    destruct_scsi_pt_obj(ptvp);

    if (ok) { /* output result if it is available */
        char * p = (char *)inqBuff;

        printf("Some of the INQUIRY command's results:\n");
        printf("    %.8s  %.16s  %.4s\n", p + 8, p + 16, p + 32);
    }
    ok = 0;


    /* Now prepare TEST UNIT READY command */
    ptvp = construct_scsi_pt_obj();     /* one object per command */
    if (NULL == ptvp) {
        fprintf(stderr, "sg_simple5: out of memory\n");
        return -1;
    }
    set_scsi_pt_cdb(ptvp, tur_cdb, sizeof(tur_cdb));
    set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
    /* no data in or out */
    res = do_scsi_pt(ptvp, sg_fd, CMD_TIMEOUT_SECS, verbose);
    if (res < 0) {
        fprintf(stderr, "  pass through os error: %s\n",
                safe_strerror(-res));
        goto finish_inq;
    } else if (SCSI_PT_DO_BAD_PARAMS == res) {
        fprintf(stderr, "  bad pass through setup\n");
        goto finish_inq;
    } else if (SCSI_PT_DO_TIMEOUT == res) {
        fprintf(stderr, "  pass through timeout\n");
        goto finish_inq;
    }
    if ((verbose > 1) && ((duration = get_scsi_pt_duration_ms(ptvp)) >= 0))
        fprintf(stderr, "      duration=%d ms\n", duration);
    resid = get_scsi_pt_resid(ptvp);
    switch ((cat = get_scsi_pt_result_category(ptvp))) {
    case SCSI_PT_RESULT_GOOD:
        break;
    case SCSI_PT_RESULT_STATUS: /* other than GOOD and CHECK CONDITION */
        if (verbose) {
            sg_get_scsi_status_str(get_scsi_pt_status_response(ptvp),
                                   sizeof(b), b);
            fprintf(stderr, "  scsi status: %s\n", b);
        }
        goto finish_tur;
    case SCSI_PT_RESULT_SENSE:
        slen = get_scsi_pt_sense_len(ptvp);
        if (verbose) {
            sg_get_sense_str("", sense_b, slen, (verbose > 1),
                             sizeof(b), b);
            fprintf(stderr, "%s", b);
        }
        goto finish_tur;
    case SCSI_PT_RESULT_TRANSPORT_ERR:
        if (verbose) {
            get_scsi_pt_transport_err_str(ptvp, sizeof(b), b);
            fprintf(stderr, "  transport: %s", b);
        }
        goto finish_tur;
    case SCSI_PT_RESULT_OS_ERR:
        if (verbose) {
            get_scsi_pt_os_err_str(ptvp, sizeof(b), b);
            fprintf(stderr, "  os: %s", b);
        }
        goto finish_tur;
    default:
        fprintf(stderr, "  unknown pass through result "
                "category (%d)\n", cat);
        goto finish_tur;
    }

    ok = 1;
finish_tur:
    destruct_scsi_pt_obj(ptvp);

    if (ok)
        printf("Test Unit Ready successful so unit is ready!\n");
    else
        printf("Test Unit Ready failed so unit may _not_ be ready!\n");

    scsi_pt_close_device(sg_fd);
    return 0;
}
예제 #3
0
int
main(int argc, char *argv[])
{
    int k;
    int ret = 0;
    unsigned int ui;
    size_t s;
    struct opts_t opts;
    char b[2048];
    FILE * fp = NULL;
    const char * cp;

    memset(&opts, 0, sizeof(opts));
    memset(b, 0, sizeof(b));
    ret = process_cl(&opts, argc, argv);
    if (ret != 0) {
        usage();
        return ret;
    } else if (opts.do_help) {
        usage();
        return 0;
    } else if (opts.do_version) {
        pr2serr("version: %s\n", version_str);
        return 0;
    }


    if (opts.do_status) {
        sg_get_scsi_status_str(opts.sstatus, sizeof(b) - 1, b);
        printf("SCSI status: %s\n", b);
    }

    if ((0 == opts.sense_len) && opts.no_space_str) {
        if (opts.do_verbose > 2)
            pr2serr("no_space str: %s\n", opts.no_space_str);
        cp = opts.no_space_str;
        for (k = 0; isxdigit(cp[k]) && isxdigit(cp[k + 1]); k += 2) {
            if (1 != sscanf(cp + k, "%2x", &ui)) {
                pr2serr("bad no_space hex string: %s\n", cp);
                return SG_LIB_SYNTAX_ERROR;
            }
            opts.sense[opts.sense_len++] = (unsigned char)ui;
        }
    }

    if ((0 == opts.sense_len) && (! opts.do_binary) && (! opts.do_file)) {
        if (opts.do_status)
            return 0;
        pr2serr(">> Need sense data on the command line or in a file\n\n");
        usage();
        return SG_LIB_SYNTAX_ERROR;
    }
    if (opts.sense_len && (opts.do_binary || opts.do_file)) {
        pr2serr(">> Need sense data on command line or in a file, not "
                "both\n\n");
        return SG_LIB_SYNTAX_ERROR;
    }
    if (opts.do_binary && opts.do_file) {
        pr2serr(">> Either a binary file or a ASCII hexadecimal, file not "
                "both\n\n");
        return SG_LIB_SYNTAX_ERROR;
    }

    if (opts.do_binary) {
        fp = fopen(opts.fname, "r");
        if (NULL == fp) {
            pr2serr("unable to open file: %s\n", opts.fname);
            return SG_LIB_SYNTAX_ERROR;
        }
        s = fread(opts.sense, 1, MAX_SENSE_LEN, fp);
        fclose(fp);
        if (0 == s) {
            pr2serr("read nothing from file: %s\n", opts.fname);
            return SG_LIB_SYNTAX_ERROR;
        }
        opts.sense_len = s;
    } else if (opts.do_file) {
        ret = f2hex_arr(opts.fname, opts.no_space, opts.sense,
                        &opts.sense_len, MAX_SENSE_LEN);
        if (ret) {
            pr2serr("unable to decode ASCII hex from file: %s\n", opts.fname);
            return SG_LIB_SYNTAX_ERROR;
        }
    }

    if (opts.sense_len) {
        if (opts.wfname) {
            if ((fp = fopen(opts.wfname, "w"))) {
                write2wfn(fp, &opts);
                fclose(fp);
            } else {
                perror("open");
                pr2serr("trying to write to %s\n", opts.wfname);
            }
        }
        sg_get_sense_str(NULL, opts.sense, opts.sense_len, opts.do_verbose,
                         sizeof(b) - 1, b);
        printf("%s\n", b);
    }

    return 0;
}
예제 #4
0
int main(int argc, char * argv[])
{
    int c, n, len;
    int verbose = 0;
    int ret = 1;
    char b[2048];

    while (1) {
        int option_index = 0;

        c = getopt_long(argc, argv, "hvV", long_options,
                        &option_index);
        if (c == -1)
            break;

        switch (c) {
        case 'h':
        case '?':
            usage();
            return 0;
        case 'v':
            ++verbose;
            break;
        case 'V':
            fprintf(stderr, "version: %s\n", version_str);
            return 0;
        default:
            fprintf(stderr, "unrecognised switch code 0x%x ??\n", c);
            usage();
            return 1;
        }
    }
    if (optind < argc) {
        if (optind < argc) {
            for (; optind < argc; ++optind)
                fprintf(stderr, "Unexpected extra argument: %s\n",
                        argv[optind]);
            usage();
            return 1;
        }
    }

    printf("Testing my_snprintf():\n");
    sg_print_sense("desc_sense_data test", desc_sense_data, (int)sizeof(desc_sense_data), 1);
    printf("\n");

#if 1
    sg_get_sense_str("sg_get_sense_str", desc_sense_data, sizeof(desc_sense_data), 1, sizeof(b), b);
    printf("sg_get_sense_str: strlen(b)->%zd\n", strlen(b));
    printf("%s", b);
    printf("\n");
#endif

    b[0] = '\0';
    len = sizeof(b);
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = -1;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 0;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 1;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 2;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 3;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 4;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 5;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 6;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    b[0] = '\0';
    len = 7;
    n = my_snprintf(b, len, "%s", "test");
    printf("my_snprintf(,%d,,\"test\") -> %d; strlen(b) -> %zd\n",
           len, n, strlen(b));
    if (strlen(b) > 0)
        printf("Resulting string: %s\n", b);

    return ret;
}
예제 #5
0
int main(/* int argc, char * argv[] */)
{
    unsigned char err1[] = {0x72, 0x5, 0x4, 0x1, 0, 0, 0, 32,
                            0x2, 0x6, 0, 0, 0xc8, 0x0, 0x3, 0,
                            0, 0xa, 0x80, 0, 1, 2, 3, 4,
                            0xaa, 0xbb, 0xcc, 0xdd,
                            1, 0xa, 0, 0, 1, 2, 3, 4,
                            0xaa, 0xbb, 0xee, 0xff};
    unsigned char err2[] = {0x72, SPC_SK_MEDIUM_ERROR, 0x4, 0x1, 0x80, 0, 0,
                            32,
                            0x2, 0x6, 0, 0, 0xc8, 0x0, 0x3, 0,
                            0, 0xa, 0x80, 0, 1, 2, 3, 4,
                            0xaa, 0xbb, 0xcc, 0xdd,
                            1, 0xa, 0, 0, 1, 2, 3, 4,
                            0xaa, 0xbb, 0xee, 0xff};
                           /* Set SDAT_OVFL */
    unsigned char err3[] = {0x72, SPC_SK_NO_SENSE, 0x4, 0x1, 0, 0, 0, 8,
                            0x2, 0x6, 0, 0, 0xc8, 0x0, 0x3, 0};
    unsigned char err4[] = {0x73, SPC_SK_COPY_ABORTED, 0x4, 0x1, 0, 0, 0, 22,
                            0x2, 0x6, 0, 0, 0xc8, 0x0, 0x3, 0,
                            0x3, 0x2, 0, 0x55,
                            0x5, 0x2, 0, 0x20,
                            0x85, 0x4, 0, 0x20, 0x33, 0x44};
                           /* Set Filemark, EOM, ILI and SDAT_OVFL */
    unsigned char err5[] = {0xf1, 0, (0xf0 | SPC_SK_ILLEGAL_REQUEST), 0x11,
                            0x22, 0x33, 0x44, 0xa,
                            0x0, 0x0, 0, 0, 0x4, 0x1, 0, 0xcf, 0, 5,};
    unsigned char err6[] = {0x72, SPC_SK_NO_SENSE, 0x4, 0x1, 0, 0, 0, 14,
                            0x9, 0xc, 1, 0, 0x11, 0x22, 0x66, 0x33,
                            0x77, 0x44, 0x88, 0x55, 0x1, 0x2};
    unsigned char err7[] = {0xf1, 0, 0xe5, 0x11, 0x22, 0x33, 0x44, 0xa,
                            0x0, 0x0, 0x0, 0x0, 0x24, 0x1, 0xbb,
                            0xc9, 0x0, 0x2};
    char leadin[128];
    char b[2048];
    int k, prev_len;

    sg_print_sense("err1 test", err1, sizeof(err1), 1 /* raw_info */);
    sg_print_sense("\nerr2 test", err2, sizeof(err2), 1);
    sg_print_sense("\nerr3 test", err3, sizeof(err3), 1);
    sg_print_sense("\nerr4 test", err4, sizeof(err4), 1);
    sg_print_sense("\nerr5 test", err5, sizeof(err5), 1);
    sg_print_sense("\nerr6 test", err6, sizeof(err6), 1);
    sg_print_sense("\nerr7 test", err7, sizeof(err7), 1);

    printf("\n\nTry different output string sizes with "
           "sg_get_sense_str(err2):\n");
    for (k = 1, prev_len = -1; k < 512; ++k) {
        snprintf(leadin, sizeof(leadin), "blen=%d", k);
        sg_get_sense_str(leadin, err2, sizeof(err2), 0, k, b);
        printf("%s\n", b);
        if (prev_len == (int)strlen(b))
            break;
        else
            prev_len = strlen(b);
    }

    printf("\n\nTry different output string sizes with "
           "sg_get_sense_str(err4):\n");
    for (k = 1, prev_len = -1; k < 512; ++k) {
        snprintf(leadin, sizeof(leadin), "blen=%d", k);
        sg_get_sense_str(leadin, err4, sizeof(err4), 0, k, b);
        printf("%s\n", b);
        if (prev_len == (int)strlen(b))
            break;
        else
            prev_len = strlen(b);
    }
    return 0;
}
예제 #6
0
/**
 * Wraps a CDB mass storage command in the appropriate gunk to get it down
 * @param handle
 * @param endpoint
 * @param cdb
 * @param cdb_length
 * @param lun
 * @param flags
 * @param expected_rx_size
 * @return 
 */
int send_usb_mass_storage_command(libusb_device_handle *handle, uint8_t endpoint_out,
                              uint8_t *cdb, uint8_t cdb_length,
                              uint8_t lun, uint8_t flags, uint32_t expected_rx_size) {
    DLOG("Sending usb m-s cmd: cdblen:%d, rxsize=%d\n", cdb_length, expected_rx_size);
    dump_CDB_command(cdb, cdb_length);

    static uint32_t tag;
    if (tag == 0) {
        tag = 1;
    }

    int try = 0;
    int ret = 0;
    int real_transferred;
    int i = 0;

    uint8_t c_buf[STLINK_SG_SIZE];
    // tag is allegedly ignored... TODO - verify
    c_buf[i++] = 'U';
    c_buf[i++] = 'S';
    c_buf[i++] = 'B';
    c_buf[i++] = 'C';
    write_uint32(&c_buf[i], tag);
    uint32_t this_tag = tag++;
    write_uint32(&c_buf[i+4], expected_rx_size);
    i+= 8;
    c_buf[i++] = flags;
    c_buf[i++] = lun;

    c_buf[i++] = cdb_length;

    // Now the actual CDB request
    assert(cdb_length <= CDB_SL);
    memcpy(&(c_buf[i]), cdb, cdb_length);
    
    int sending_length = STLINK_SG_SIZE;
    DLOG("sending length set to: %d\n", sending_length);
    
    // send....
    do {
        DLOG("attempting tx...\n");
        ret = libusb_bulk_transfer(handle, endpoint_out, c_buf, sending_length,
                                   &real_transferred, SG_TIMEOUT_MSEC);
        if (ret == LIBUSB_ERROR_PIPE) {
            libusb_clear_halt(handle, endpoint_out);
        }
        try++;
    } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
    if (ret != LIBUSB_SUCCESS) {
        WLOG("sending failed: %d\n", ret);
        return -1;
    }
    DLOG("Actually sent: %d, returning tag: %d\n", real_transferred, tag);
    return this_tag;
}


/**
 * Straight from stm8 stlink code...
 * @param handle
 * @param endpoint_in
 * @param endpoint_out
 */
static void
get_sense(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
{
    DLOG("Fetching sense...\n");
    uint8_t cdb[16];
    memset(cdb, 0, sizeof(cdb));
#define REQUEST_SENSE 0x03
#define REQUEST_SENSE_LENGTH 18
    cdb[0] = REQUEST_SENSE;
    cdb[4] = REQUEST_SENSE_LENGTH;
    uint32_t tag = send_usb_mass_storage_command(handle, endpoint_out, cdb, sizeof(cdb), 0,
                                                 LIBUSB_ENDPOINT_IN, REQUEST_SENSE_LENGTH);
    if (tag == 0) {
        WLOG("refusing to send request sense with tag 0\n");
        return;
    }
    unsigned char sense[REQUEST_SENSE_LENGTH];
    int transferred;
    int ret;
    int try = 0;
    do {
        ret = libusb_bulk_transfer(handle, endpoint_in, sense, sizeof(sense),
                                   &transferred, SG_TIMEOUT_MSEC);
        if (ret == LIBUSB_ERROR_PIPE) {
            libusb_clear_halt(handle, endpoint_in);
        }
        try++;
    } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
    if (ret != LIBUSB_SUCCESS) {
        WLOG("receiving sense failed: %d\n", ret);
        return;
    }
    if (transferred != sizeof(sense)) {
        WLOG("received unexpected amount of sense: %d != %d\n", transferred, sizeof(sense));
    }
    uint32_t received_tag;
    int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
    if (status != 0) {
        WLOG("receiving sense failed with status: %02x\n", status);
        return;
    }
    if (sense[0] != 0x70 && sense[0] != 0x71) {
        WLOG("No sense data\n");
    } else {
        WLOG("Sense KCQ: %02X %02X %02X\n", sense[2] & 0x0f, sense[12], sense[13]);
    }
}


//TODO rewrite/cleanup, save the error in sl

#if FINISHED_WITH_SG
static void stlink_confirm_inq(stlink_t *stl, struct sg_pt_base *ptvp) {
    struct stlink_libsg *sl = stl->backend_data;
    const int e = sl->do_scsi_pt_err;
    if (e < 0) {
        fprintf(stderr, "scsi_pt error: pass through os error: %s\n",
                safe_strerror(-e));
        return;
    } else if (e == SCSI_PT_DO_BAD_PARAMS) {
        fprintf(stderr, "scsi_pt error: bad pass through setup\n");
        return;
    } else if (e == SCSI_PT_DO_TIMEOUT) {
        fprintf(stderr, "  pass through timeout\n");
        return;
    }
    const int duration = get_scsi_pt_duration_ms(ptvp);
    if ((stl->verbose > 1) && (duration >= 0))
        DLOG("      duration=%d ms\n", duration);

    // XXX stlink fw sends broken residue, so ignore it and use the known q_len
    // "usb-storage quirks=483:3744:r"
    // forces residue to be ignored and calculated, but this causes aboard if
    // data_len = 0 and by some other data_len values.

    const int resid = get_scsi_pt_resid(ptvp);
    const int dsize = stl->q_len - resid;

    const int cat = get_scsi_pt_result_category(ptvp);
    char buf[512];
    unsigned int slen;

    switch (cat) {
        case SCSI_PT_RESULT_GOOD:
            if (stl->verbose && (resid > 0))
                DLOG("      notice: requested %d bytes but "
                    "got %d bytes, ignore [broken] residue = %d\n",
                    stl->q_len, dsize, resid);
            break;
        case SCSI_PT_RESULT_STATUS:
            if (stl->verbose) {
                sg_get_scsi_status_str(
                        get_scsi_pt_status_response(ptvp), sizeof (buf),
                        buf);
                DLOG("  scsi status: %s\n", buf);
            }
            return;
        case SCSI_PT_RESULT_SENSE:
            slen = get_scsi_pt_sense_len(ptvp);
            if (stl->verbose) {
                sg_get_sense_str("", sl->sense_buf, slen, (stl->verbose
                        > 1), sizeof (buf), buf);
                DLOG("%s", buf);
            }
            if (stl->verbose && (resid > 0)) {
                if ((stl->verbose) || (stl->q_len > 0))
                    DLOG("    requested %d bytes but "
                        "got %d bytes\n", stl->q_len, dsize);
            }
            return;
        case SCSI_PT_RESULT_TRANSPORT_ERR:
            if (stl->verbose) {
                get_scsi_pt_transport_err_str(ptvp, sizeof (buf), buf);
                // http://tldp.org/HOWTO/SCSI-Generic-HOWTO/x291.html
                // These codes potentially come from the firmware on a host adapter
                // or from one of several hosts that an adapter driver controls.
                // The 'host_status' field has the following values:
                //	[0x07] Internal error detected in the host adapter.
                // This may not be fatal (and the command may have succeeded).
                DLOG("  transport: %s", buf);
            }
            return;
        case SCSI_PT_RESULT_OS_ERR:
            if (stl->verbose) {
                get_scsi_pt_os_err_str(ptvp, sizeof (buf), buf);
                DLOG("  os: %s", buf);
            }
            return;
        default:
            fprintf(stderr, "  unknown pass through result "
                    "category (%d)\n", cat);
    }
}
#endif

/**
 * Just send a buffer on an endpoint, no questions asked.
 * Handles repeats, and time outs.  Also handles reading status reports and sense
 * @param handle libusb device *
 * @param endpoint_out sends 
 * @param endpoint_in used to read status reports back in 
 * @param cbuf  what to send
 * @param length how much to send
 * @return number of bytes actually sent, or -1 for failures.
 */
int send_usb_data_only(libusb_device_handle *handle, unsigned char endpoint_out,
    unsigned char endpoint_in, unsigned char *cbuf, unsigned int length) {
    int ret;
    int real_transferred;
    int try;
    do {
        DLOG("attempting tx...\n");
        ret = libusb_bulk_transfer(handle, endpoint_out, cbuf, length,
                                   &real_transferred, SG_TIMEOUT_MSEC);
        if (ret == LIBUSB_ERROR_PIPE) {
            libusb_clear_halt(handle, endpoint_out);
        }
        try++;
    } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));
    if (ret != LIBUSB_SUCCESS) {
        WLOG("sending failed: %d\n", ret);
        return -1;
    }
    DLOG("Actually sent: %d\n", real_transferred);
    
    // now, swallow up the status, so that things behave nicely...
    uint32_t received_tag;
    // -ve is for my errors, 0 is good, +ve is libusb sense status bytes
    int status = get_usb_mass_storage_status(handle, endpoint_in, &received_tag);
    if (status < 0) {
        WLOG("receiving status failed: %d\n", status);
        return -1;
    }
    if (status != 0) {
        WLOG("receiving status not passed :(: %02x\n", status);
    }
    if (status == 1) {
        get_sense(handle, endpoint_in, endpoint_out);
        return -1;
    }
    
    return real_transferred;
}


int stlink_q(stlink_t *sl) {
    struct stlink_libsg* sg = sl->backend_data;
    //uint8_t cdb_len = 6;  // FIXME varies!!!
    uint8_t cdb_len = 10;  // FIXME varies!!!
    uint8_t lun = 0;  // always zero...
    uint32_t tag = send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, 
        sg->cdb_cmd_blk, cdb_len, lun, LIBUSB_ENDPOINT_IN, sl->q_len);
    
    
    // now wait for our response...
    // length copied from stlink-usb...
    int rx_length = sl->q_len;
    int try = 0;
    int real_transferred;
    int ret;
    if (rx_length > 0) {
        do {
            DLOG("attempting rx\n");
            ret = libusb_bulk_transfer(sg->usb_handle, sg->ep_rep, sl->q_buf, rx_length, 
                &real_transferred, SG_TIMEOUT_MSEC);
            if (ret == LIBUSB_ERROR_PIPE) {
                libusb_clear_halt(sg->usb_handle, sg->ep_req);
            }
            try++;
        } while ((ret == LIBUSB_ERROR_PIPE) && (try < 3));

        if (ret != LIBUSB_SUCCESS) {
            WLOG("Receiving failed: %d\n", ret);
            return -1;
        }

        if (real_transferred != rx_length) {
            WLOG("received unexpected amount: %d != %d\n", real_transferred, rx_length);
        }
    }

    uint32_t received_tag;
    // -ve is for my errors, 0 is good, +ve is libusb sense status bytes
    int status = get_usb_mass_storage_status(sg->usb_handle, sg->ep_rep, &received_tag);
    if (status < 0) {
        WLOG("receiving status failed: %d\n", status);
        return -1;
    }
    if (status != 0) {
        WLOG("receiving status not passed :(: %02x\n", status);
    }
    if (status == 1) {
        get_sense(sg->usb_handle, sg->ep_rep, sg->ep_req);
        return -1;
    }
    if (received_tag != tag) {
        WLOG("received tag %d but expected %d\n", received_tag, tag);
        //return -1;
    }
    if (rx_length > 0 && real_transferred != rx_length) {
        return -1;
    }
    return 0;

        
    DLOG("Actually received: %d\n", real_transferred);

#if FINISHED_WITH_SG
    // Get control command descriptor of scsi structure,
    // (one object per command!!)
    struct sg_pt_base *ptvp = construct_scsi_pt_obj();
    if (NULL == ptvp) {
        fprintf(stderr, "construct_scsi_pt_obj: out of memory\n");
        return;
    }

    set_scsi_pt_cdb(ptvp, sg->cdb_cmd_blk, sizeof (sg->cdb_cmd_blk));

    // set buffer for sense (error information) data
    set_scsi_pt_sense(ptvp, sg->sense_buf, sizeof (sg->sense_buf));

    // Set a buffer to be used for data transferred from device
    if (sg->q_data_dir == Q_DATA_IN) {
        //clear_buf(sl);
        set_scsi_pt_data_in(ptvp, sl->q_buf, sl->q_len);
    } else {
        set_scsi_pt_data_out(ptvp, sl->q_buf, sl->q_len);
    }
    // Executes SCSI command (or at least forwards it to lower layers).
    sg->do_scsi_pt_err = do_scsi_pt(ptvp, sg->sg_fd, SG_TIMEOUT_SEC,
            sl->verbose);

    // check for scsi errors
    stlink_confirm_inq(sl, ptvp);
    // TODO recycle: clear_scsi_pt_obj(struct sg_pt_base * objp);
    destruct_scsi_pt_obj(ptvp);
#endif
}

// TODO thinking, cleanup

void stlink_stat(stlink_t *stl, char *txt) {
    if (stl->q_len <= 0)
        return;

    stlink_print_data(stl);

    switch (stl->q_buf[0]) {
        case STLINK_OK:
            DLOG("  %s: ok\n", txt);
            return;
        case STLINK_FALSE:
            DLOG("  %s: false\n", txt);
            return;
        default:
            DLOG("  %s: unknown\n", txt);
    }
}


void _stlink_sg_version(stlink_t *stl) {
    struct stlink_libsg *sl = stl->backend_data;
    DLOG("\n*** stlink_version ***\n");
    clear_cdb(sl);
    sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
    stl->q_len = 6;
    sl->q_addr = 0;
    stlink_q(stl);
    // HACK use my own private version right now...
    
}

// Get stlink mode:
// STLINK_DEV_DFU_MODE || STLINK_DEV_MASS_MODE || STLINK_DEV_DEBUG_MODE
// usb dfu             || usb mass             || jtag or swd

int _stlink_sg_current_mode(stlink_t *stl) {
    struct stlink_libsg *sl = stl->backend_data;
    clear_cdb(sl);
    sl->cdb_cmd_blk[0] = STLINK_GET_CURRENT_MODE;
    stl->q_len = 2;
    sl->q_addr = 0;
    stlink_q(stl);
    return stl->q_buf[0];
}

// Exit the mass mode and enter the swd debug mode.

void _stlink_sg_enter_swd_mode(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
    sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_SWD;
    sl->q_len = 0; // >0 -> aboard
    stlink_q(sl);
}

// Exit the mass mode and enter the jtag debug mode.
// (jtag is disabled in the discovery's stlink firmware)

void _stlink_sg_enter_jtag_mode(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_enter_jtag_mode ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_ENTER;
    sg->cdb_cmd_blk[2] = STLINK_DEBUG_ENTER_JTAG;
    sl->q_len = 0;
    stlink_q(sl);
}

// XXX kernel driver performs reset, the device temporally disappears

void _stlink_sg_exit_dfu_mode(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_exit_dfu_mode ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[0] = STLINK_DFU_COMMAND;
    sg->cdb_cmd_blk[1] = STLINK_DFU_EXIT;
    sl->q_len = 0; // ??
    stlink_q(sl);
    /*
     [135121.844564] sd 19:0:0:0: [sdb] Unhandled error code
     [135121.844569] sd 19:0:0:0: [sdb] Result: hostbyte=DID_ERROR driverbyte=DRIVER_OK
     [135121.844574] sd 19:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 10 00 00 00 08 00
     [135121.844584] end_request: I/O error, dev sdb, sector 4096
     [135121.844590] Buffer I/O error on device sdb, logical block 512
     [135130.122567] usb 6-1: reset full speed USB device using uhci_hcd and address 7
     [135130.274551] usb 6-1: device firmware changed
     [135130.274618] usb 6-1: USB disconnect, address 7
     [135130.275186] VFS: busy inodes on changed media or resized disk sdb
     [135130.275424] VFS: busy inodes on changed media or resized disk sdb
     [135130.286758] VFS: busy inodes on changed media or resized disk sdb
     [135130.292796] VFS: busy inodes on changed media or resized disk sdb
     [135130.301481] VFS: busy inodes on changed media or resized disk sdb
     [135130.304316] VFS: busy inodes on changed media or resized disk sdb
     [135130.431113] usb 6-1: new full speed USB device using uhci_hcd and address 8
     [135130.629444] usb-storage 6-1:1.0: Quirks match for vid 0483 pid 3744: 102a1
     [135130.629492] scsi20 : usb-storage 6-1:1.0
     [135131.625600] scsi 20:0:0:0: Direct-Access     STM32                          PQ: 0 ANSI: 0
     [135131.627010] sd 20:0:0:0: Attached scsi generic sg2 type 0
     [135131.633603] sd 20:0:0:0: [sdb] 64000 512-byte logical blocks: (32.7 MB/31.2 MiB)
     [135131.633613] sd 20:0:0:0: [sdb] Assuming Write Enabled
     [135131.633620] sd 20:0:0:0: [sdb] Assuming drive cache: write through
     [135131.640584] sd 20:0:0:0: [sdb] Assuming Write Enabled
     [135131.640592] sd 20:0:0:0: [sdb] Assuming drive cache: write through
     [135131.640609]  sdb:
     [135131.652634] sd 20:0:0:0: [sdb] Assuming Write Enabled
     [135131.652639] sd 20:0:0:0: [sdb] Assuming drive cache: write through
     [135131.652645] sd 20:0:0:0: [sdb] Attached SCSI removable disk
     [135131.671536] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
     [135131.671548] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
     [135131.671553] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
     [135131.671560] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
     [135131.671570] end_request: I/O error, dev sdb, sector 63872
     [135131.671575] Buffer I/O error on device sdb, logical block 7984
     [135131.678527] sd 20:0:0:0: [sdb] Result: hostbyte=DID_OK driverbyte=DRIVER_SENSE
     [135131.678532] sd 20:0:0:0: [sdb] Sense Key : Illegal Request [current]
     [135131.678537] sd 20:0:0:0: [sdb] Add. Sense: Logical block address out of range
     [135131.678542] sd 20:0:0:0: [sdb] CDB: Read(10): 28 00 00 00 f9 80 00 00 08 00
     [135131.678551] end_request: I/O error, dev sdb, sector 63872
     ...
     [135131.853565] end_request: I/O error, dev sdb, sector 4096
     */
}

void _stlink_sg_core_id(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
    sl->q_len = 4;
    sg->q_addr = 0;
    stlink_q(sl);
    sl->core_id = read_uint32(sl->q_buf, 0);
}

// Arm-core reset -> halted state.

void _stlink_sg_reset(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_RESETSYS;
    sl->q_len = 2;
    sg->q_addr = 0;
    stlink_q(sl);
    stlink_stat(sl, "core reset");
}

// Arm-core status: halted or running.

void _stlink_sg_status(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_status ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
    sl->q_len = 2;
    sg->q_addr = 0;
    stlink_q(sl);
}

// Force the core into the debug mode -> halted state.

void _stlink_sg_force_debug(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_force_debug ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
    sl->q_len = 2;
    sg->q_addr = 0;
    stlink_q(sl);
    stlink_stat(sl, "force debug");
}

// Read all arm-core registers.

void _stlink_sg_read_all_regs(stlink_t *sl, reg *regp) {
    struct stlink_libsg *sg = sl->backend_data;

    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_READALLREGS;
    sl->q_len = 84;
    sg->q_addr = 0;
    stlink_q(sl);
    stlink_print_data(sl);

    // TODO - most of this should be re-extracted up....
    
    // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
    // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
    for (int i = 0; i < 16; i++) {
        regp->r[i] = read_uint32(sl->q_buf, 4 * i);
        if (sl->verbose > 1)
            DLOG("r%2d = 0x%08x\n", i, regp->r[i]);
    }
    regp->xpsr = read_uint32(sl->q_buf, 64);
    regp->main_sp = read_uint32(sl->q_buf, 68);
    regp->process_sp = read_uint32(sl->q_buf, 72);
    regp->rw = read_uint32(sl->q_buf, 76);
    regp->rw2 = read_uint32(sl->q_buf, 80);
    if (sl->verbose < 2)
        return;

    DLOG("xpsr       = 0x%08x\n", regp->xpsr);
    DLOG("main_sp    = 0x%08x\n", regp->main_sp);
    DLOG("process_sp = 0x%08x\n", regp->process_sp);
    DLOG("rw         = 0x%08x\n", regp->rw);
    DLOG("rw2        = 0x%08x\n", regp->rw2);
}

// Read an arm-core register, the index must be in the range 0..20.
//  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
// r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2

void _stlink_sg_read_reg(stlink_t *sl, int r_idx, reg *regp) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_READREG;
    sg->cdb_cmd_blk[2] = r_idx;
    sl->q_len = 4;
    sg->q_addr = 0;
    stlink_q(sl);
    //  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
    // 0-3 | 4-7 | ... | 60-63 | 64-67 | 68-71   | 72-75      | 76-79 | 80-83
    // r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2
    stlink_print_data(sl);

    uint32_t r = read_uint32(sl->q_buf, 0);
    DLOG("r_idx (%2d) = 0x%08x\n", r_idx, r);

    switch (r_idx) {
        case 16:
            regp->xpsr = r;
            break;
        case 17:
            regp->main_sp = r;
            break;
        case 18:
            regp->process_sp = r;
            break;
        case 19:
            regp->rw = r; //XXX ?(primask, basemask etc.)
            break;
        case 20:
            regp->rw2 = r; //XXX ?(primask, basemask etc.)
            break;
        default:
            regp->r[r_idx] = r;
    }
}

// Write an arm-core register. Index:
//  0  |  1  | ... |  15   |  16   |   17    |   18       |  19   |  20
// r0  | r1  | ... | r15   | xpsr  | main_sp | process_sp | rw    | rw2

void _stlink_sg_write_reg(stlink_t *sl, uint32_t reg, int idx) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEREG;
    //   2: reg index
    // 3-6: reg content
    sg->cdb_cmd_blk[2] = idx;
    write_uint32(sg->cdb_cmd_blk + 3, reg);
    sl->q_len = 2;
    sg->q_addr = 0;
    stlink_q(sl);
    stlink_stat(sl, "write reg");
}

// Write a register of the debug module of the core.
// XXX ?(atomic writes)
// TODO test

void stlink_write_dreg(stlink_t *sl, uint32_t reg, uint32_t addr) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_write_dreg ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEDEBUGREG;
    // 2-5: address of reg of the debug module
    // 6-9: reg content
    write_uint32(sg->cdb_cmd_blk + 2, addr);
    write_uint32(sg->cdb_cmd_blk + 6, reg);
    sl->q_len = 2;
    sg->q_addr = addr;
    stlink_q(sl);
    stlink_stat(sl, "write debug reg");
}

// Force the core exit the debug mode.

void _stlink_sg_run(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_run ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
    sl->q_len = 2;
    sg->q_addr = 0;
    stlink_q(sl);
    stlink_stat(sl, "run core");
}

// Step the arm-core.

void _stlink_sg_step(stlink_t *sl) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_STEPCORE;
    sl->q_len = 2;
    sg->q_addr = 0;
    stlink_q(sl);
    stlink_stat(sl, "step core");
}

// TODO test
// see Cortex-M3 Technical Reference Manual
// TODO make delegate!
void stlink_set_hw_bp(stlink_t *sl, int fp_nr, uint32_t addr, int fp) {
    DLOG("\n*** stlink_set_hw_bp ***\n");
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_SETFP;
    // 2:The number of the flash patch used to set the breakpoint
    // 3-6: Address of the breakpoint (LSB)
    // 7: FP_ALL (0x02) / FP_UPPER (0x01) / FP_LOWER (0x00)
    sl->q_buf[2] = fp_nr;
    write_uint32(sl->q_buf, addr);
    sl->q_buf[7] = fp;

    sl->q_len = 2;
    stlink_q(sl);
    stlink_stat(sl, "set flash breakpoint");
}

// TODO test

// TODO make delegate!
void stlink_clr_hw_bp(stlink_t *sl, int fp_nr) {
    struct stlink_libsg *sg = sl->backend_data;
    DLOG("\n*** stlink_clr_hw_bp ***\n");
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_CLEARFP;
    sg->cdb_cmd_blk[2] = fp_nr;

    sl->q_len = 2;
    stlink_q(sl);
    stlink_stat(sl, "clear flash breakpoint");
}

// Read a "len" bytes to the sl->q_buf from the memory, max 6kB (6144 bytes)

void _stlink_sg_read_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_READMEM_32BIT;
    // 2-5: addr
    // 6-7: len
    write_uint32(sg->cdb_cmd_blk + 2, addr);
    write_uint16(sg->cdb_cmd_blk + 6, len);

    // data_in 0-0x40-len
    // !!! len _and_ q_len must be max 6k,
    //     i.e. >1024 * 6 = 6144 -> aboard)
    // !!! if len < q_len: 64*k, 1024*n, n=1..5  -> aboard
    //     (broken residue issue)
    sl->q_len = len;
    sg->q_addr = addr;
    stlink_q(sl);
    stlink_print_data(sl);
}

// Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.

void _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_8BIT;
    // 2-5: addr
    // 6-7: len (>0x40 (64) -> aboard)
    write_uint32(sg->cdb_cmd_blk + 2, addr);
    write_uint16(sg->cdb_cmd_blk + 6, len);

    // this sends the command...
    send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
    // This sends the data...
    send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);
    stlink_print_data(sl);
}

// Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.

void _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
    struct stlink_libsg *sg = sl->backend_data;
    clear_cdb(sg);
    sg->cdb_cmd_blk[1] = STLINK_DEBUG_WRITEMEM_32BIT;
    // 2-5: addr
    // 6-7: len "unlimited"
    write_uint32(sg->cdb_cmd_blk + 2, addr);
    write_uint16(sg->cdb_cmd_blk + 6, len);

    // this sends the command...
    send_usb_mass_storage_command(sg->usb_handle, sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
    // This sends the data...
    send_usb_data_only(sg->usb_handle, sg->ep_req, sg->ep_rep, sl->q_buf, len);

    stlink_print_data(sl);
}

#if 0 /* not working */

static int write_flash_mem16
(struct stlink* sl, uint32_t addr, uint16_t val) {
    /* half word writes */
    if (addr % 2) return -1;

    /* unlock if locked */
    unlock_flash_if(sl);

    /* set flash programming chosen bit */
    set_flash_cr_pg(sl);

    write_uint16(sl->q_buf, val);
    stlink_write_mem16(sl, addr, 2);

    /* wait for non business */
    wait_flash_busy(sl);

    lock_flash(sl);

    /* check the programmed value back */
    stlink_read_mem16(sl, addr, 2);
    if (*(const uint16_t*) sl->q_buf != val) {
        /* values differ at i * sizeof(uint16_t) */
        return -1;
    }

    /* success */
    return 0;
}
#endif /* not working */

// Exit the jtag or swd mode and enter the mass mode.

void _stlink_sg_exit_debug_mode(stlink_t *stl) {

    if (stl) {
        struct stlink_libsg* sl = stl->backend_data;
        clear_cdb(sl);
        sl->cdb_cmd_blk[1] = STLINK_DEBUG_EXIT;
        stl->q_len = 0; // >0 -> aboard
        stlink_q(stl);
    }
}


// 1) open a sg device, switch the stlink from dfu to mass mode
// 2) wait 5s until the kernel driver stops reseting the broken device
// 3) reopen the device
// 4) the device driver is now ready for a switch to jtag/swd mode
// TODO thinking, better error handling, wait until the kernel driver stops reseting the plugged-in device

stlink_backend_t _stlink_sg_backend = {
    _stlink_sg_close,
    _stlink_sg_exit_debug_mode,
    _stlink_sg_enter_swd_mode,
    _stlink_sg_enter_jtag_mode,
    _stlink_sg_exit_dfu_mode,
    _stlink_sg_core_id,
    _stlink_sg_reset,
    _stlink_sg_run,
    _stlink_sg_status,
    _stlink_sg_version,
    _stlink_sg_read_mem32,
    _stlink_sg_write_mem32,
    _stlink_sg_write_mem8,
    _stlink_sg_read_all_regs,
    _stlink_sg_read_reg,
    _stlink_sg_write_reg,
    _stlink_sg_step,
    _stlink_sg_current_mode,
    _stlink_sg_force_debug
};

static stlink_t* stlink_open(const int verbose) {
    
    stlink_t *sl = malloc(sizeof (stlink_t));
    memset(sl, 0, sizeof(stlink_t));
    struct stlink_libsg *slsg = malloc(sizeof (struct stlink_libsg));
    if (sl == NULL || slsg == NULL) {
        WLOG("Couldn't malloc stlink and stlink_sg structures out of memory!\n");
        return NULL;
    }
    
    if (libusb_init(&(slsg->libusb_ctx))) {
        WLOG("failed to init libusb context, wrong version of libraries?\n");
        free(sl);
        free(slsg);
        return NULL;
    }
    
    libusb_set_debug(slsg->libusb_ctx, 3);
    
    slsg->usb_handle = libusb_open_device_with_vid_pid(slsg->libusb_ctx, USB_ST_VID, USB_STLINK_PID);
    if (slsg->usb_handle == NULL) {
        WLOG("Failed to find an stlink v1 by VID:PID\n");
        libusb_close(slsg->usb_handle);
        free(sl);
        free(slsg);
        return NULL;
    }
    
    // TODO 
    // Could read the interface config descriptor, and assert lots of the assumptions
    
    // assumption: numInterfaces is always 1...
    if (libusb_kernel_driver_active(slsg->usb_handle, 0) == 1) {
        int r = libusb_detach_kernel_driver(slsg->usb_handle, 0);
        if (r < 0) {
            WLOG("libusb_detach_kernel_driver(() error %s\n", strerror(-r));
            libusb_close(slsg->usb_handle);
            free(sl);
            free(slsg);
            return NULL;
        }
        DLOG("Kernel driver was successfully detached\n");
    }

    int config;
    if (libusb_get_configuration(slsg->usb_handle, &config)) {
        /* this may fail for a previous configured device */
        WLOG("libusb_get_configuration()\n");
        libusb_close(slsg->usb_handle);
        free(sl);
        free(slsg);
        return NULL;

    }
    
    // assumption: bConfigurationValue is always 1
    if (config != 1) {
        WLOG("Your stlink got into a real weird configuration, trying to fix it!\n");
        DLOG("setting new configuration (%d -> 1)\n", config);
        if (libusb_set_configuration(slsg->usb_handle, 1)) {
            /* this may fail for a previous configured device */
            WLOG("libusb_set_configuration() failed\n");
            libusb_close(slsg->usb_handle);
            free(sl);
            free(slsg);
            return NULL;
        }
    }

    if (libusb_claim_interface(slsg->usb_handle, 0)) {
        WLOG("libusb_claim_interface() failed\n");
        libusb_close(slsg->usb_handle);
        free(sl);
        free(slsg);
        return NULL;
    }

    // assumption: endpoint config is fixed mang. really.
    slsg->ep_rep = 1 /* ep rep */ | LIBUSB_ENDPOINT_IN;
    slsg->ep_req = 2 /* ep req */ | LIBUSB_ENDPOINT_OUT;
    
    DLOG("Successfully opened stlinkv1 by libusb :)\n");
    
    sl->verbose = verbose;
    sl->backend_data = slsg;
    sl->backend = &_stlink_sg_backend;

    sl->core_stat = STLINK_CORE_STAT_UNKNOWN;
    slsg->q_addr = 0;

    /* flash memory settings */
    sl->flash_base = STM32_FLASH_BASE;
    sl->flash_size = STM32_FLASH_SIZE;
    sl->flash_pgsz = STM32_FLASH_PGSZ;

    /* system memory */
    sl->sys_base = STM32_SYSTEM_BASE;
    sl->sys_size = STM32_SYSTEM_SIZE;

    /* sram memory settings */
    sl->sram_base = STM32_SRAM_BASE;
    sl->sram_size = STM32_SRAM_SIZE;

    return sl;
}



stlink_t* stlink_v1_open(const int verbose) {
    ugly_init(verbose);
    stlink_t *sl = stlink_open(verbose);
    if (sl == NULL) {
        fputs("Error: could not open stlink device\n", stderr);
        return NULL;
    }

    stlink_version(sl);

    if ((sl->version.st_vid != USB_ST_VID) || (sl->version.stlink_pid != USB_STLINK_PID)) {
        ugly_log(UERROR, LOG_TAG, 
            "WTF? successfully opened, but unable to read version details. BROKEN!\n");
        return NULL;
    }

    DLOG("Reading current mode...\n");
    switch (stlink_current_mode(sl)) {
        case STLINK_DEV_MASS_MODE:
            return sl;
        case STLINK_DEV_DEBUG_MODE:
            // TODO go to mass?
            return sl;
    }

    DLOG("Attempting to exit DFU mode\n");
    _stlink_sg_exit_dfu_mode(sl);
    
    // exit the dfu mode -> the device is gone
    DLOG("\n*** reopen the stlink device ***\n");
    delay(1000);
    stlink_close(sl);
    delay(5000);

    DLOG("Attempting to reopen the stlink...\n");
    sl = stlink_open(verbose);
    if (sl == NULL) {
        fputs("Error: could not open stlink device\n", stderr);
        return NULL;
    }
    // re-query device info
    stlink_version(sl);
    return sl;
}