/* Invokes a SCSI LOG SELECT command. Return of 0 -> success,
* various SG_LIB_CAT_* positive values or -1 -> other errors */
int
sg_ll_log_select(int sg_fd, int pcr, int sp, int pc, int pg_code,
int subpg_code, unsigned char * paramp, int param_len,
int noisy, int verbose)
{
static const char * const cdb_name_s = "log select";
int res, ret, k, sense_cat;
unsigned char logs_cdb[LOG_SELECT_CMDLEN] =
{LOG_SELECT_CMD, 0, 0, 0, 0, 0, 0, 0, 0, 0};
unsigned char sense_b[SENSE_BUFF_LEN];
struct sg_pt_base * ptvp;
if (param_len > 0xffff) {
pr2ws("%s: param_len too big\n", cdb_name_s);
return -1;
}
logs_cdb[1] = (unsigned char)((pcr ? 2 : 0) | (sp ? 1 : 0));
logs_cdb[2] = (unsigned char)(((pc << 6) & 0xc0) | (pg_code & 0x3f));
logs_cdb[3] = (unsigned char)(subpg_code & 0xff);
sg_put_unaligned_be16((int16_t)param_len, logs_cdb + 7);
if (verbose) {
pr2ws(" %s cdb: ", cdb_name_s);
for (k = 0; k < LOG_SELECT_CMDLEN; ++k)
pr2ws("%02x ", logs_cdb[k]);
pr2ws("\n");
}
if ((verbose > 1) && (param_len > 0)) {
pr2ws(" %s parameter list\n", cdb_name_s);
dStrHexErr((const char *)paramp, param_len, -1);
}
if (NULL == ((ptvp = create_pt_obj(cdb_name_s))))
return -1;
set_scsi_pt_cdb(ptvp, logs_cdb, sizeof(logs_cdb));
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, paramp, param_len);
res = do_scsi_pt(ptvp, sg_fd, DEF_PT_TIMEOUT, verbose);
ret = sg_cmds_process_resp(ptvp, cdb_name_s, res, 0, sense_b, noisy,
verbose, &sense_cat);
if (-1 == ret)
;
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
/* Invokes a SCSI MODE SELECT (10) command. Return of 0 -> success,
* various SG_LIB_CAT_* positive values or -1 -> other errors,
* v2 adds rtd (revert to defaults) bit (spc5r11). */
int
sg_ll_mode_select10_v2(int sg_fd, bool pf, bool rtd, bool sp, void * paramp,
int param_len, bool noisy, int verbose)
{
static const char * const cdb_s = "mode select(10)";
int res, ret, k, sense_cat;
uint8_t modes_cdb[MODE_SELECT10_CMDLEN] =
{MODE_SELECT10_CMD, 0, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t sense_b[SENSE_BUFF_LEN];
struct sg_pt_base * ptvp;
modes_cdb[1] = (uint8_t)((pf ? 0x10 : 0x0) | (sp ? 0x1 : 0x0));
if (rtd)
modes_cdb[1] |= 0x2;
sg_put_unaligned_be16((int16_t)param_len, modes_cdb + 7);
if (param_len > 0xffff) {
pr2ws("%s: param_len too big\n", cdb_s);
return -1;
}
if (verbose) {
pr2ws(" %s cdb: ", cdb_s);
for (k = 0; k < MODE_SELECT10_CMDLEN; ++k)
pr2ws("%02x ", modes_cdb[k]);
pr2ws("\n");
}
if (verbose > 1) {
pr2ws(" %s parameter list\n", cdb_s);
hex2stderr((const uint8_t *)paramp, param_len, -1);
}
if (NULL == ((ptvp = create_pt_obj(cdb_s))))
return -1;
set_scsi_pt_cdb(ptvp, modes_cdb, sizeof(modes_cdb));
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, (uint8_t *)paramp, param_len);
res = do_scsi_pt(ptvp, sg_fd, DEF_PT_TIMEOUT, verbose);
ret = sg_cmds_process_resp(ptvp, cdb_s, res, noisy, verbose, &sense_cat);
if (-1 == ret)
ret = sg_convert_errno(get_scsi_pt_os_err(ptvp));
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
/* Invokes a SCSI MODE SELECT (6) command. Return of 0 -> success,
* various SG_LIB_CAT_* positive values or -1 -> other errors */
int
sg_ll_mode_select6(int sg_fd, int pf, int sp, void * paramp, int param_len,
int noisy, int verbose)
{
static const char * const cdb_name_s = "mode select(6)";
int res, ret, k, sense_cat;
unsigned char modes_cdb[MODE_SELECT6_CMDLEN] =
{MODE_SELECT6_CMD, 0, 0, 0, 0, 0};
unsigned char sense_b[SENSE_BUFF_LEN];
struct sg_pt_base * ptvp;
modes_cdb[1] = (unsigned char)(((pf << 4) & 0x10) | (sp & 0x1));
modes_cdb[4] = (unsigned char)(param_len & 0xff);
if (param_len > 0xff) {
pr2ws("%s: param_len too big\n", cdb_name_s);
return -1;
}
if (verbose) {
pr2ws(" %s cdb: ", cdb_name_s);
for (k = 0; k < MODE_SELECT6_CMDLEN; ++k)
pr2ws("%02x ", modes_cdb[k]);
pr2ws("\n");
}
if (verbose > 1) {
pr2ws(" %s parameter list\n", cdb_name_s);
dStrHexErr((const char *)paramp, param_len, -1);
}
if (NULL == ((ptvp = create_pt_obj(cdb_name_s))))
return -1;
set_scsi_pt_cdb(ptvp, modes_cdb, sizeof(modes_cdb));
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, (unsigned char *)paramp, param_len);
res = do_scsi_pt(ptvp, sg_fd, DEF_PT_TIMEOUT, verbose);
ret = sg_cmds_process_resp(ptvp, cdb_name_s, res, 0, sense_b, noisy,
verbose, &sense_cat);
if (-1 == ret)
;
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
/* Invokes the SET TIMESTAMP command. Return of 0 -> success, various
* SG_LIB_CAT_* positive values or -1 -> other errors */
static int
sg_ll_set_timestamp(int sg_fd, void * paramp, int param_len, int noisy,
int verbose)
{
int k, ret, res, sense_cat;
unsigned char stCmdBlk[SET_TIMESTAMP_CMDLEN] =
{SG_MAINTENANCE_OUT, SET_TIMESTAMP_SA, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0};
unsigned char sense_b[SENSE_BUFF_LEN];
struct sg_pt_base * ptvp;
sg_put_unaligned_be32(param_len, stCmdBlk + 6);
if (verbose) {
pr2serr(" Set timestamp cdb: ");
for (k = 0; k < SET_TIMESTAMP_CMDLEN; ++k)
pr2serr("%02x ", stCmdBlk[k]);
pr2serr("\n");
if ((verbose > 1) && paramp && param_len) {
pr2serr(" set timestamp parameter list:\n");
dStrHexErr((const char *)paramp, param_len, -1);
}
}
ptvp = construct_scsi_pt_obj();
if (NULL == ptvp) {
pr2serr("%s: out of memory\n", __func__);
return -1;
}
set_scsi_pt_cdb(ptvp, stCmdBlk, sizeof(stCmdBlk));
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, (unsigned char *)paramp, param_len);
res = do_scsi_pt(ptvp, sg_fd, DEF_PT_TIMEOUT, verbose);
ret = sg_cmds_process_resp(ptvp, "set timestamp", res, 0, sense_b, noisy,
verbose, &sense_cat);
if (-1 == ret)
;
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
static int
do_write_same(int sg_fd, const struct opts_t * op, const void * dataoutp,
int * act_cdb_lenp)
{
int k, ret, res, sense_cat, cdb_len;
uint64_t llba;
uint8_t ws_cdb[WRITE_SAME32_LEN];
uint8_t sense_b[SENSE_BUFF_LEN];
struct sg_pt_base * ptvp;
cdb_len = op->pref_cdb_size;
if (WRITE_SAME10_LEN == cdb_len) {
llba = op->lba + op->numblocks;
if ((op->numblocks > 0xffff) || (llba > UINT32_MAX) ||
op->ndob || (op->unmap && (! op->want_ws10))) {
cdb_len = WRITE_SAME16_LEN;
if (op->verbose) {
const char * cp = "use WRITE SAME(16) instead of 10 byte "
"cdb";
if (op->numblocks > 0xffff)
pr2serr("%s since blocks exceed 65535\n", cp);
else if (llba > UINT32_MAX)
pr2serr("%s since LBA may exceed 32 bits\n", cp);
else
pr2serr("%s due to ndob or unmap settings\n", cp);
}
}
}
if (act_cdb_lenp)
*act_cdb_lenp = cdb_len;
memset(ws_cdb, 0, sizeof(ws_cdb));
switch (cdb_len) {
case WRITE_SAME10_LEN:
ws_cdb[0] = WRITE_SAME10_OP;
ws_cdb[1] = ((op->wrprotect & 0x7) << 5);
/* ANCHOR + UNMAP not allowed for WRITE_SAME10 in sbc3r24+r25 but
* a proposal has been made to allow it. Anticipate approval. */
if (op->anchor)
ws_cdb[1] |= 0x10;
if (op->unmap)
ws_cdb[1] |= 0x8;
if (op->pbdata)
ws_cdb[1] |= 0x4;
if (op->lbdata)
ws_cdb[1] |= 0x2;
sg_put_unaligned_be32((uint32_t)op->lba, ws_cdb + 2);
ws_cdb[6] = (op->grpnum & 0x1f);
sg_put_unaligned_be16((uint16_t)op->numblocks, ws_cdb + 7);
break;
case WRITE_SAME16_LEN:
ws_cdb[0] = WRITE_SAME16_OP;
ws_cdb[1] = ((op->wrprotect & 0x7) << 5);
if (op->anchor)
ws_cdb[1] |= 0x10;
if (op->unmap)
ws_cdb[1] |= 0x8;
if (op->pbdata)
ws_cdb[1] |= 0x4;
if (op->lbdata)
ws_cdb[1] |= 0x2;
if (op->ndob)
ws_cdb[1] |= 0x1;
sg_put_unaligned_be64(op->lba, ws_cdb + 2);
sg_put_unaligned_be32((uint32_t)op->numblocks, ws_cdb + 10);
ws_cdb[14] = (op->grpnum & 0x1f);
break;
case WRITE_SAME32_LEN:
ws_cdb[0] = VARIABLE_LEN_OP;
ws_cdb[6] = (op->grpnum & 0x1f);
ws_cdb[7] = WRITE_SAME32_ADD;
sg_put_unaligned_be16((uint16_t)WRITE_SAME32_SA, ws_cdb + 8);
ws_cdb[10] = ((op->wrprotect & 0x7) << 5);
if (op->anchor)
ws_cdb[10] |= 0x10;
if (op->unmap)
ws_cdb[10] |= 0x8;
if (op->pbdata)
ws_cdb[10] |= 0x4;
if (op->lbdata)
ws_cdb[10] |= 0x2;
if (op->ndob)
ws_cdb[10] |= 0x1;
sg_put_unaligned_be64(op->lba, ws_cdb + 12);
sg_put_unaligned_be32((uint32_t)op->numblocks, ws_cdb + 28);
break;
default:
pr2serr("do_write_same: bad cdb length %d\n", cdb_len);
return -1;
}
if (op->verbose > 1) {
pr2serr(" Write same(%d) cdb: ", cdb_len);
for (k = 0; k < cdb_len; ++k)
pr2serr("%02x ", ws_cdb[k]);
pr2serr("\n Data-out buffer length=%d\n",
op->xfer_len);
}
if ((op->verbose > 3) && (op->xfer_len > 0)) {
pr2serr(" Data-out buffer contents:\n");
hex2stderr((const uint8_t *)dataoutp, op->xfer_len, 1);
}
ptvp = construct_scsi_pt_obj();
if (NULL == ptvp) {
pr2serr("Write same(%d): out of memory\n", cdb_len);
return -1;
}
set_scsi_pt_cdb(ptvp, ws_cdb, cdb_len);
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, (uint8_t *)dataoutp, op->xfer_len);
res = do_scsi_pt(ptvp, sg_fd, op->timeout, op->verbose);
ret = sg_cmds_process_resp(ptvp, "Write same", res, SG_NO_DATA_IN,
sense_b, true /*noisy */, op->verbose,
&sense_cat);
if (-1 == ret)
get_scsi_pt_os_err(ptvp);
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
case SG_LIB_CAT_MEDIUM_HARD:
{
bool valid;
int slen;
uint64_t ull = 0;
slen = get_scsi_pt_sense_len(ptvp);
valid = sg_get_sense_info_fld(sense_b, slen, &ull);
if (valid)
pr2serr("Medium or hardware error starting at lba=%"
PRIu64 " [0x%" PRIx64 "]\n", ull, ull);
}
ret = sense_cat;
break;
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
/* Invokes a SCSI WRITE AND VERIFY according with CDB. Returns 0 -> success,
* various SG_LIB_CAT_* positive values or -1 -> other errors */
static int
run_scsi_transaction(int sg_fd, const uint8_t *cdbp, int cdb_len,
uint8_t *dop, int do_len, int timeout,
bool noisy, int verbose)
{
int res, k, sense_cat, ret;
struct sg_pt_base * ptvp;
uint8_t sense_b[SENSE_BUFF_LEN];
char b[32];
snprintf(b, sizeof(b), "Write and verify(%d)", cdb_len);
if (verbose) {
pr2serr(" %s cdb: ", b);
for (k = 0; k < cdb_len; ++k)
pr2serr("%02x ", cdbp[k]);
pr2serr("\n");
if ((verbose > 2) && dop && do_len) {
pr2serr(" Data out buffer [%d bytes]:\n", do_len);
hex2stderr(dop, do_len, -1);
}
}
ptvp = construct_scsi_pt_obj();
if (NULL == ptvp) {
pr2serr("%s: out of memory\n", b);
return -1;
}
set_scsi_pt_cdb(ptvp, cdbp, cdb_len);
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, dop, do_len);
res = do_scsi_pt(ptvp, sg_fd, timeout, verbose);
ret = sg_cmds_process_resp(ptvp, b, res, noisy, verbose, &sense_cat);
if (-1 == ret)
ret = sg_convert_errno(get_scsi_pt_os_err(ptvp));
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
case SG_LIB_CAT_MEDIUM_HARD: /* write or verify failed */
{
bool valid;
int slen;
uint64_t ull = 0;
slen = get_scsi_pt_sense_len(ptvp);
valid = sg_get_sense_info_fld(sense_b, slen, &ull);
if (valid)
pr2serr("Medium or hardware error starting at lba=%"
PRIu64 " [0x%" PRIx64 "]\n", ull, ull);
}
ret = sense_cat;
break;
case SG_LIB_CAT_PROTECTION: /* PI failure */
case SG_LIB_CAT_MISCOMPARE: /* only in bytchk=1 case */
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
int
main(int argc, char *argv[])
{
int ret = 0;
int res_cat, status, slen, k, ret2;
int sg_fd = -1;
struct sg_pt_base *ptvp = NULL;
unsigned char sense_buffer[32];
unsigned char * dxfer_buffer_in = NULL;
unsigned char * dxfer_buffer_out = NULL;
unsigned char *wrkBuf = NULL;
struct opts_t opts;
struct opts_t * op;
char b[128];
op = &opts;
memset(op, 0, sizeof(opts));
op->timeout = DEFAULT_TIMEOUT;
ret = process_cl(op, argc, argv);
if (ret != 0) {
usage();
goto done;
} else if (op->do_help) {
usage();
goto done;
} else if (op->do_version) {
version();
goto done;
}
sg_fd = scsi_pt_open_device(op->device_name, op->readonly,
op->do_verbose);
if (sg_fd < 0) {
fprintf(stderr, "%s: %s\n", op->device_name, safe_strerror(-sg_fd));
ret = SG_LIB_FILE_ERROR;
goto done;
}
ptvp = construct_scsi_pt_obj();
if (ptvp == NULL) {
fprintf(stderr, "out of memory\n");
ret = SG_LIB_CAT_OTHER;
goto done;
}
if (op->do_verbose) {
fprintf(stderr, " cdb to send: ");
for (k = 0; k < op->cdb_length; ++k)
fprintf(stderr, "%02x ", op->cdb[k]);
fprintf(stderr, "\n");
if (op->do_verbose > 2) {
sg_get_command_name(op->cdb, 0, sizeof(b) - 1, b);
b[sizeof(b) - 1] = '\0';
fprintf(stderr, " Command name: %s\n", b);
}
}
set_scsi_pt_cdb(ptvp, op->cdb, op->cdb_length);
set_scsi_pt_sense(ptvp, sense_buffer, sizeof(sense_buffer));
if (op->do_dataout) {
dxfer_buffer_out = fetch_dataout(op);
if (dxfer_buffer_out == NULL) {
ret = SG_LIB_CAT_OTHER;
goto done;
}
set_scsi_pt_data_out(ptvp, dxfer_buffer_out, op->dataout_len);
}
if (op->do_datain) {
dxfer_buffer_in = my_memalign(op->datain_len, &wrkBuf);
if (dxfer_buffer_in == NULL) {
perror("malloc");
ret = SG_LIB_CAT_OTHER;
goto done;
}
set_scsi_pt_data_in(ptvp, dxfer_buffer_in, op->datain_len);
}
ret = do_scsi_pt(ptvp, sg_fd, op->timeout, op->do_verbose);
if (ret > 0) {
if (SCSI_PT_DO_BAD_PARAMS == ret) {
fprintf(stderr, "do_scsi_pt: bad pass through setup\n");
ret = SG_LIB_CAT_OTHER;
} else if (SCSI_PT_DO_TIMEOUT == ret) {
fprintf(stderr, "do_scsi_pt: timeout\n");
ret = SG_LIB_CAT_TIMEOUT;
} else
ret = SG_LIB_CAT_OTHER;
goto done;
} else if (ret < 0) {
fprintf(stderr, "do_scsi_pt: %s\n", safe_strerror(-ret));
ret = SG_LIB_CAT_OTHER;
goto done;
}
slen = 0;
res_cat = get_scsi_pt_result_category(ptvp);
switch (res_cat) {
case SCSI_PT_RESULT_GOOD:
ret = 0;
break;
case SCSI_PT_RESULT_SENSE:
slen = get_scsi_pt_sense_len(ptvp);
ret = sg_err_category_sense(sense_buffer, slen);
break;
case SCSI_PT_RESULT_TRANSPORT_ERR:
get_scsi_pt_transport_err_str(ptvp, sizeof(b), b);
fprintf(stderr, ">>> transport error: %s\n", b);
ret = SG_LIB_CAT_OTHER;
break;
case SCSI_PT_RESULT_OS_ERR:
get_scsi_pt_os_err_str(ptvp, sizeof(b), b);
fprintf(stderr, ">>> os error: %s\n", b);
ret = SG_LIB_CAT_OTHER;
break;
default:
fprintf(stderr, ">>> unknown pass through result category (%d)\n",
res_cat);
ret = SG_LIB_CAT_OTHER;
break;
}
status = get_scsi_pt_status_response(ptvp);
fprintf(stderr, "SCSI Status: ");
sg_print_scsi_status(status);
fprintf(stderr, "\n\n");
if ((SAM_STAT_CHECK_CONDITION == status) && (! op->no_sense)) {
if (SCSI_PT_RESULT_SENSE != res_cat)
slen = get_scsi_pt_sense_len(ptvp);
if (0 == slen)
fprintf(stderr, ">>> Strange: status is CHECK CONDITION but no "
"Sense Information\n");
else {
fprintf(stderr, "Sense Information:\n");
sg_print_sense(NULL, sense_buffer, slen, (op->do_verbose > 0));
fprintf(stderr, "\n");
}
}
if (SAM_STAT_RESERVATION_CONFLICT == status)
ret = SG_LIB_CAT_RES_CONFLICT;
if (op->do_datain) {
int data_len = op->datain_len - get_scsi_pt_resid(ptvp);
if (ret && !(SG_LIB_CAT_RECOVERED == ret ||
SG_LIB_CAT_NO_SENSE == ret))
fprintf(stderr, "Error %d occurred, no data received\n", ret);
else if (data_len == 0) {
fprintf(stderr, "No data received\n");
} else {
if (op->datain_file == NULL && !op->datain_binary) {
fprintf(stderr, "Received %d bytes of data:\n", data_len);
dStrHexErr((const char *)dxfer_buffer_in, data_len, 0);
} else {
const char * cp = "stdout";
if (op->datain_file &&
! ((1 == strlen(op->datain_file)) &&
('-' == op->datain_file[0])))
cp = op->datain_file;
fprintf(stderr, "Writing %d bytes of data to %s\n", data_len,
cp);
ret2 = write_dataout(op->datain_file, dxfer_buffer_in,
data_len);
if (0 != ret2) {
if (0 == ret)
ret = ret2;
goto done;
}
}
}
}
done:
if (op->do_verbose) {
sg_get_category_sense_str(ret, sizeof(b), b, op->do_verbose - 1);
fprintf(stderr, "%s\n", b);
}
if (wrkBuf)
free(wrkBuf);
if (ptvp)
destruct_scsi_pt_obj(ptvp);
if (sg_fd >= 0)
scsi_pt_close_device(sg_fd);
return ret;
}
/* Invokes a SCSI WRITE BUFFER command (SPC). Return of 0 ->
* success, SG_LIB_CAT_INVALID_OP -> invalid opcode,
* SG_LIB_CAT_ILLEGAL_REQ -> bad field in cdb, SG_LIB_CAT_UNIT_ATTENTION,
* SG_LIB_CAT_NOT_READY -> device not ready, SG_LIB_CAT_ABORTED_COMMAND,
* -1 -> other failure */
static int
sg_ll_write_buffer_v2(int sg_fd, int mode, int m_specific, int buffer_id,
uint32_t buffer_offset, void * paramp,
uint32_t param_len, int to_secs, int noisy, int verbose)
{
int k, res, ret, sense_cat;
uint8_t wbuf_cdb[WRITE_BUFFER_CMDLEN] =
{WRITE_BUFFER_CMD, 0, 0, 0, 0, 0, 0, 0, 0, 0};
uint8_t sense_b[SENSE_BUFF_LEN];
struct sg_pt_base * ptvp;
if (buffer_offset > 0xffffff) {
pr2serr("%s: buffer_offset value too large for 24 bits\n", __func__);
return -1;
}
if (param_len > 0xffffff) {
pr2serr("%s: param_len value too large for 24 bits\n", __func__);
return -1;
}
wbuf_cdb[1] = (uint8_t)(mode & 0x1f);
wbuf_cdb[1] |= (uint8_t)((m_specific & 0x7) << 5);
wbuf_cdb[2] = (uint8_t)(buffer_id & 0xff);
sg_put_unaligned_be24(buffer_offset, wbuf_cdb + 3);
sg_put_unaligned_be24(param_len, wbuf_cdb + 6);
if (verbose) {
pr2serr(" Write buffer cdb: ");
for (k = 0; k < WRITE_BUFFER_CMDLEN; ++k)
pr2serr("%02x ", wbuf_cdb[k]);
pr2serr("\n");
if ((verbose > 1) && paramp && param_len) {
pr2serr(" Write buffer parameter list%s:\n",
((param_len > 256) ? " (first 256 bytes)" : ""));
dStrHexErr((const char *)paramp,
((param_len > 256) ? 256 : param_len), -1);
}
}
ptvp = construct_scsi_pt_obj();
if (NULL == ptvp) {
pr2serr("%s: out of memory\n", __func__);
return -1;
}
set_scsi_pt_cdb(ptvp, wbuf_cdb, sizeof(wbuf_cdb));
set_scsi_pt_sense(ptvp, sense_b, sizeof(sense_b));
set_scsi_pt_data_out(ptvp, (uint8_t *)paramp, param_len);
res = do_scsi_pt(ptvp, sg_fd, to_secs, verbose);
ret = sg_cmds_process_resp(ptvp, "Write buffer", res, 0, sense_b,
noisy, verbose, &sense_cat);
if (-1 == ret)
;
else if (-2 == ret) {
switch (sense_cat) {
case SG_LIB_CAT_RECOVERED:
case SG_LIB_CAT_NO_SENSE:
ret = 0;
break;
default:
ret = sense_cat;
break;
}
} else
ret = 0;
destruct_scsi_pt_obj(ptvp);
return ret;
}
/**
* 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;
}
