static int uPD98402_ioctl(struct atm_dev *dev,unsigned int cmd,void __user *arg)
{
switch (cmd) {
case SONET_GETSTATZ:
case SONET_GETSTAT:
return fetch_stats(dev,arg, cmd == SONET_GETSTATZ);
case SONET_SETFRAMING:
return set_framing(dev, (int)(unsigned long)arg);
case SONET_GETFRAMING:
return put_user(PRIV(dev)->framing,(int __user *)arg) ?
-EFAULT : 0;
case SONET_GETFRSENSE:
return get_sense(dev,arg);
case ATM_SETLOOP:
return set_loopback(dev, (int)(unsigned long)arg);
case ATM_GETLOOP:
return put_user(PRIV(dev)->loop_mode,(int __user *)arg) ?
-EFAULT : 0;
case ATM_QUERYLOOP:
return put_user(ATM_LM_LOC_PHY | ATM_LM_LOC_ATM |
ATM_LM_RMT_PHY,(int __user *)arg) ? -EFAULT : 0;
default:
return -ENOIOCTLCMD;
}
}
// Mass Storage device to test bulk transfers (non destructive test)
static int test_mass_storage(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
{
int r, size;
uint8_t lun;
uint32_t expected_tag;
uint32_t i, max_lba, block_size;
double device_size;
uint8_t cdb[16]; // SCSI Command Descriptor Block
uint8_t buffer[64];
char vid[9], pid[9], rev[5];
unsigned char *data;
FILE *fd;
printf("Reading Max LUN:\n");
r = libusb_control_transfer(handle, LIBUSB_ENDPOINT_IN|LIBUSB_REQUEST_TYPE_CLASS|LIBUSB_RECIPIENT_INTERFACE,
BOMS_GET_MAX_LUN, 0, 0, &lun, 1, 1000);
// Some devices send a STALL instead of the actual value.
// In such cases we should set lun to 0.
if (r == 0) {
lun = 0;
} else if (r < 0) {
perr(" Failed: %s", libusb_strerror((enum libusb_error)r));
}
printf(" Max LUN = %d\n", lun);
// Send Inquiry
printf("Sending Inquiry:\n");
memset(buffer, 0, sizeof(buffer));
memset(cdb, 0, sizeof(cdb));
cdb[0] = 0x12; // Inquiry
cdb[4] = INQUIRY_LENGTH;
send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, INQUIRY_LENGTH, &expected_tag);
CALL_CHECK(libusb_bulk_transfer(handle, endpoint_in, (unsigned char*)&buffer, INQUIRY_LENGTH, &size, 1000));
printf(" received %d bytes\n", size);
// The following strings are not zero terminated
for (i=0; i<8; i++) {
vid[i] = buffer[8+i];
pid[i] = buffer[16+i];
rev[i/2] = buffer[32+i/2]; // instead of another loop
}
vid[8] = 0;
pid[8] = 0;
rev[4] = 0;
printf(" VID:PID:REV \"%8s\":\"%8s\":\"%4s\"\n", vid, pid, rev);
if (get_mass_storage_status(handle, endpoint_in, expected_tag) == -2) {
get_sense(handle, endpoint_in, endpoint_out);
}
// Read capacity
printf("Reading Capacity:\n");
memset(buffer, 0, sizeof(buffer));
memset(cdb, 0, sizeof(cdb));
cdb[0] = 0x25; // Read Capacity
send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, READ_CAPACITY_LENGTH, &expected_tag);
CALL_CHECK(libusb_bulk_transfer(handle, endpoint_in, (unsigned char*)&buffer, READ_CAPACITY_LENGTH, &size, 1000));
printf(" received %d bytes\n", size);
max_lba = be_to_int32(&buffer[0]);
block_size = be_to_int32(&buffer[4]);
device_size = ((double)(max_lba+1))*block_size/(1024*1024*1024);
printf(" Max LBA: %08X, Block Size: %08X (%.2f GB)\n", max_lba, block_size, device_size);
if (get_mass_storage_status(handle, endpoint_in, expected_tag) == -2) {
get_sense(handle, endpoint_in, endpoint_out);
}
// coverity[tainted_data]
data = (unsigned char*) calloc(1, block_size);
if (data == NULL) {
perr(" unable to allocate data buffer\n");
return -1;
}
// Send Read
printf("Attempting to read %d bytes:\n", block_size);
memset(cdb, 0, sizeof(cdb));
cdb[0] = 0x28; // Read(10)
cdb[8] = 0x01; // 1 block
send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, block_size, &expected_tag);
libusb_bulk_transfer(handle, endpoint_in, data, block_size, &size, 5000);
printf(" READ: received %d bytes\n", size);
if (get_mass_storage_status(handle, endpoint_in, expected_tag) == -2) {
get_sense(handle, endpoint_in, endpoint_out);
} else {
display_buffer_hex(data, size);
if ((binary_dump) && ((fd = fopen(binary_name, "w")) != NULL)) {
if (fwrite(data, 1, (size_t)size, fd) != (unsigned int)size) {
perr(" unable to write binary data\n");
}
fclose(fd);
}
}
free(data);
return 0;
}
static void detection_work(struct work_struct *work)
{
struct cpcap_usb_det_data *data =
container_of(work, struct cpcap_usb_det_data, work.work);
#ifdef CONFIG_TTA_CHARGER
static unsigned char first_time;
#endif
printk(KERN_ERR "detection_work:data->state = %d\n", data->state);
switch (data->state) {
case CONFIG:
vusb_enable(data);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_CHRG_DET);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_IDGND);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_VBUSVLD);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_DPI);
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_DMI);
configure_hardware(data, CPCAP_ACCY_UNKNOWN);
data->state = SAMPLE_1;
schedule_delayed_work(&data->work, msecs_to_jiffies(11));
break;
case SAMPLE_1:
get_sense(data);
#ifdef CONFIG_TTA_CHARGER
if (!(data->sense_tta.gpio_val) &&
(data->sense & CPCAP_BIT_SESSVLD_S)) {
disable_tta();
enable_tta();
}
#endif
data->state = SAMPLE_2;
schedule_delayed_work(&data->work, msecs_to_jiffies(100));
break;
case SAMPLE_2:
data->prev_sense = data->sense;
get_sense(data);
if (data->prev_sense != data->sense) {
/* Stay in this state */
data->state = SAMPLE_2;
schedule_delayed_work(&data->work,
msecs_to_jiffies(100));
} else if (!(data->sense & CPCAP_BIT_SE1_S) &&
(data->sense & CPCAP_BIT_ID_FLOAT_S) &&
!(data->sense & CPCAP_BIT_ID_GROUND_S) &&
!(data->sense & CPCAP_BIT_SESSVLD_S)) {
data->state = IDENTIFY;
#ifdef CONFIG_TTA_CHARGER
if (!(data->sense_tta.gpio_val))
data->state = IDENTIFY_TTA;
#endif
schedule_delayed_work(&data->work,
msecs_to_jiffies(100));
} else {
data->state = IDENTIFY;
#ifdef CONFIG_TTA_CHARGER
if (!(data->sense & CPCAP_BIT_SESSVLD_S) &&
!(data->sense_tta.gpio_val)) {
data->state = IDENTIFY_TTA;
}
#endif
schedule_delayed_work(&data->work, 0);
}
break;
#ifdef CONFIG_TTA_CHARGER
case IDENTIFY_TTA:
configure_hardware(data, CPCAP_ACCY_TTA_CHARGER);
data->state = IDENTIFY;
schedule_delayed_work(&data->work, 0);
break;
#endif
case IDENTIFY:
get_sense(data);
data->state = CONFIG;
#ifdef CONFIG_TTA_CHARGER
if ((data->sense_tta.dplus == data->sense_tta.dminus) &&
!(data->sense_tta.gpio_val) &&
!(data->sense & CPCAP_BIT_SESSVLD_S)) {
notify_accy(data, CPCAP_ACCY_TTA_CHARGER);
cpcap_irq_clear(data->cpcap, CPCAP_IRQ_DMI);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_DMI);
data->state = TTA;
disable_musb_int();
} else if ((data->sense == SENSE_USB) ||
(data->sense == SENSE_USB_FLASH)) {
notify_accy(data, CPCAP_ACCY_USB);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_DET);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_IDGND);
/* Special handling of USB cable undetect. */
data->state = USB;
}
#else
if ((data->sense == SENSE_USB) ||
(data->sense == SENSE_USB_FLASH)) {
notify_accy(data, CPCAP_ACCY_USB);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_DET);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_IDGND);
/* Special handling of USB cable undetect. */
data->state = USB;
}
#endif
else if (data->sense == SENSE_FACTORY) {
notify_accy(data, CPCAP_ACCY_FACTORY);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
#ifdef CONFIG_TTA_CHARGER
disable_tta();
#endif
/* Special handling of factory cable undetect. */
data->state = FACTORY;
} else if ((data->sense == SENSE_CHARGER_FLOAT) ||
(data->sense == SENSE_CHARGER)) {
notify_accy(data, CPCAP_ACCY_CHARGER);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_IDGND);
/* Special handling of charger undetect. */
data->state = CHARGER;
} else if ((data->sense & CPCAP_BIT_VBUSVLD_S) &&
(data->usb_accy == CPCAP_ACCY_NONE)) {
data->state = CONFIG;
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_DET);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_DPI);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_DMI);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_IDGND);
} else {
notify_accy(data, CPCAP_ACCY_NONE);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_DET);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
/* When a charger is unpowered by unplugging from the
* wall, VBUS voltage will drop below CHRG_DET (3.5V)
* until the ICHRG bits are cleared. Once ICHRG is
* cleared, VBUS will rise above CHRG_DET, but below
* VBUSVLD (4.4V) briefly as it decays. If the charger
* is re-powered while VBUS is within this window, the
* VBUSVLD interrupt is needed to trigger charger
* detection.
*
* VBUSVLD must be masked before going into suspend.
* See cpcap_usb_det_suspend() for details.
*/
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_VBUSVLD);
#ifdef CONFIG_TTA_CHARGER
disable_tta();
enable_tta();
#endif
}
break;
#ifdef CONFIG_TTA_CHARGER
case TTA:
get_sense(data);
if ((data->sense_tta.dplus != data->sense_tta.dminus) ||
(data->sense_tta.gpio_val)) {
cpcap_irq_mask(data->cpcap, CPCAP_IRQ_DMI);
disable_tta();
enable_tta();
data->state = CONFIG;
enable_musb_int();
schedule_delayed_work(&data->work, 0);
} else {
data->state = TTA;
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_DMI);
}
break;
#endif
case USB:
get_sense(data);
if ((data->sense & CPCAP_BIT_SE1_S) ||
(data->sense & CPCAP_BIT_ID_GROUND_S) ||
(!(data->sense & CPCAP_BIT_VBUSVLD_S))) {
data->state = CONFIG;
schedule_delayed_work(&data->work, 0);
} else {
data->state = USB;
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_DET);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_IDGND);
}
break;
case FACTORY:
get_sense(data);
/* The removal of a factory cable can only be detected if a
* charger is attached.
*/
if (data->sense & CPCAP_BIT_SE1_S) {
#ifdef CONFIG_TTA_CHARGER
enable_tta();
#endif
data->state = CONFIG;
schedule_delayed_work(&data->work, 0);
} else {
data->state = FACTORY;
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
}
break;
case CHARGER:
get_sense(data);
/* If the battery voltage is above the set charge voltage in
* CPCAP and ICHRG is set, CHRGCURR1 will be 0. Do not undetect
* charger in this case. */
if (!(data->sense & CPCAP_BIT_SE1_S) ||
(!(data->sense & CPCAP_BIT_VBUSVLD_S) &&
!(data->sense & CPCAP_BIT_CHRGCURR1_S))) {
data->state = CONFIG;
schedule_delayed_work(&data->work, 0);
} else {
data->state = CHARGER;
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_CHRG_CURR1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_SE1);
cpcap_irq_unmask(data->cpcap, CPCAP_IRQ_IDGND);
}
break;
default:
/* This shouldn't happen. Need to reset state machine. */
vusb_disable(data);
data->state = CONFIG;
schedule_delayed_work(&data->work, 0);
break;
}
#ifdef CONFIG_TTA_CHARGER
temp_data = data;
if (!first_time) {
enable_musb_int();
first_time = 1;
}
#endif
}
static int configure_hardware(struct cpcap_usb_det_data *data,
enum cpcap_accy accy)
{
int retval;
/* Take control of pull up from ULPI. */
retval = cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC3,
CPCAP_BIT_PU_SPI,
CPCAP_BIT_PU_SPI);
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC1,
CPCAP_BIT_DP150KPU,
(CPCAP_BIT_DP150KPU | CPCAP_BIT_DP1K5PU |
CPCAP_BIT_DM1K5PU | CPCAP_BIT_DPPD |
CPCAP_BIT_DMPD));
#ifdef CONFIG_TTA_CHARGER
get_sense(data);
if (!(data->sense_tta.dplus)) {
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC3,
CPCAP_BIT_PU_SPI |
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI,
CPCAP_BIT_PU_SPI |
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI);
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC1,
CPCAP_BIT_DP150KPU,
(CPCAP_BIT_DP150KPU | CPCAP_BIT_DP1K5PU |
CPCAP_BIT_DM1K5PU | CPCAP_BIT_DPPD |
CPCAP_BIT_DMPD));
}
#endif
switch (accy) {
case CPCAP_ACCY_USB:
case CPCAP_ACCY_FACTORY:
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC1, 0,
CPCAP_BIT_VBUSPD);
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC2,
CPCAP_BIT_USBXCVREN,
CPCAP_BIT_USBXCVREN);
/* Give USB driver control of pull up via ULPI. */
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC3,
0,
CPCAP_BIT_PU_SPI |
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI |
CPCAP_BIT_SUSPEND_SPI |
CPCAP_BIT_ULPI_SPI_SEL);
if ((data->cpcap->vendor == CPCAP_VENDOR_ST) &&
(data->cpcap->revision == CPCAP_REVISION_2_0))
vusb_enable(data);
break;
case CPCAP_ACCY_CHARGER:
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC1,
CPCAP_BIT_VBUSPD,
CPCAP_BIT_VBUSPD);
break;
#ifdef CONFIG_TTA_CHARGER
case CPCAP_ACCY_TTA_CHARGER:
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC3,
CPCAP_BIT_PU_SPI |
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI,
CPCAP_BIT_PU_SPI |
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI);
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC2,
CPCAP_BIT_USBXCVREN,
CPCAP_BIT_USBXCVREN);
break;
#endif
case CPCAP_ACCY_UNKNOWN:
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC1, 0,
CPCAP_BIT_VBUSPD);
break;
case CPCAP_ACCY_NONE:
default:
#ifdef CONFIG_TTA_CHARGER
retval |= cpcap_regacc_write(data->cpcap,
CPCAP_REG_USBC3,
CPCAP_BIT_PU_SPI,
CPCAP_BIT_PU_SPI |
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI);
#endif
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC1,
CPCAP_BIT_VBUSPD,
CPCAP_BIT_VBUSPD);
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC2, 0,
CPCAP_BIT_USBXCVREN);
retval |= cpcap_regacc_write(data->cpcap, CPCAP_REG_USBC3,
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI |
CPCAP_BIT_SUSPEND_SPI |
CPCAP_BIT_ULPI_SPI_SEL,
CPCAP_BIT_DMPD_SPI |
CPCAP_BIT_DPPD_SPI |
CPCAP_BIT_SUSPEND_SPI |
CPCAP_BIT_ULPI_SPI_SEL);
break;
}
if (retval != 0)
retval = -EFAULT;
return retval;
}
/**
* 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;
// send....
do {
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;
}
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]);
}
}
/**
* 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 = 0;
do {
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;
}
// 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 {
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;
}
// 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);
}
}
int _stlink_sg_version(stlink_t *stl) {
struct stlink_libsg *sl = stl->backend_data;
clear_cdb(sl);
sl->cdb_cmd_blk[0] = STLINK_GET_VERSION;
stl->q_len = 6;
sl->q_addr = 0;
return stlink_q(stl);
}
// 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;
if (stlink_q(stl))
return -1;
return stl->q_buf[0];
}
// Exit the mass mode and enter the swd debug mode.
int _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
return stlink_q(sl);
}
// Exit the mass mode and enter the jtag debug mode.
// (jtag is disabled in the discovery's stlink firmware)
int _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;
return stlink_q(sl);
}
// XXX kernel driver performs reset, the device temporally disappears
// Suspect this is no longer the case when we have ignore on? RECHECK
int _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; // ??
return 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
*/
}
int _stlink_sg_core_id(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_READCOREID;
sl->q_len = 4;
sg->q_addr = 0;
ret = stlink_q(sl);
if (ret)
return ret;
sl->core_id = read_uint32(sl->q_buf, 0);
return 0;
}
// Arm-core reset -> halted state.
int _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;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "core reset");
return 0;
}
// Arm-core reset -> halted state.
int _stlink_sg_jtag_reset(stlink_t *sl, int value) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_DRIVE_NRST;
sg->cdb_cmd_blk[2] = (value)?0:1;
sl->q_len = 3;
sg->q_addr = 2;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "core reset");
return 0;
}
// Arm-core status: halted or running.
int _stlink_sg_status(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_GETSTATUS;
sl->q_len = 2;
sg->q_addr = 0;
return stlink_q(sl);
}
// Force the core into the debug mode -> halted state.
int _stlink_sg_force_debug(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_FORCEDEBUG;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "force debug");
return 0;
}
// Read all arm-core registers.
int _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;
if (stlink_q(sl))
return -1;
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 0;
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);
return 0;
}
// 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
int _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;
if (stlink_q(sl))
return -1;
// 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;
}
return 0;
}
// Write an arm-core register. Index:
// 0 | 1 | ... | 15 | 16 | 17 | 18 | 19 | 20
// r0 | r1 | ... | r15 | xpsr | main_sp | process_sp | rw | rw2
int _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;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "write reg");
return 0;
}
// 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.
int _stlink_sg_run(stlink_t *sl) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_DEBUG_RUNCORE;
sl->q_len = 2;
sg->q_addr = 0;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "run core");
return 0;
}
// Step the arm-core.
int _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;
if (stlink_q(sl))
return -1;
stlink_stat(sl, "step core");
return 0;
}
// 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)
int _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;
if (stlink_q(sl))
return -1;
stlink_print_data(sl);
return 0;
}
// Write a "len" bytes from the sl->q_buf to the memory, max 64 Bytes.
int _stlink_sg_write_mem8(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
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...
ret = send_usb_mass_storage_command(sg->usb_handle,
sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
if (ret == -1)
return ret;
// This sends the data...
ret = send_usb_data_only(sg->usb_handle,
sg->ep_req, sg->ep_rep, sl->q_buf, len);
if (ret == -1)
return ret;
stlink_print_data(sl);
return 0;
}
// Write a "len" bytes from the sl->q_buf to the memory, max Q_BUF_LEN bytes.
int _stlink_sg_write_mem32(stlink_t *sl, uint32_t addr, uint16_t len) {
struct stlink_libsg *sg = sl->backend_data;
int ret;
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...
ret = send_usb_mass_storage_command(sg->usb_handle,
sg->ep_req, sg->cdb_cmd_blk, CDB_SL, 0, 0, 0);
if (ret == -1)
return ret;
// This sends the data...
ret = send_usb_data_only(sg->usb_handle,
sg->ep_req, sg->ep_rep, sl->q_buf, len);
if (ret == -1)
return ret;
stlink_print_data(sl);
return 0;
}
// Write one DWORD data to memory
int _stlink_sg_write_debug32(stlink_t *sl, uint32_t addr, uint32_t data) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_WRITEDEBUG_32BIT;
// 2-5: addr
write_uint32(sg->cdb_cmd_blk + 2, addr);
write_uint32(sg->cdb_cmd_blk + 6, data);
sl->q_len = 2;
return stlink_q(sl);
}
// Read one DWORD data from memory
int _stlink_sg_read_debug32(stlink_t *sl, uint32_t addr, uint32_t *data) {
struct stlink_libsg *sg = sl->backend_data;
clear_cdb(sg);
sg->cdb_cmd_blk[1] = STLINK_JTAG_READDEBUG_32BIT;
// 2-5: addr
write_uint32(sg->cdb_cmd_blk + 2, addr);
sl->q_len = 8;
if (stlink_q(sl))
return -1;
*data = read_uint32(sl->q_buf, 4);
return 0;
}
// Exit the jtag or swd mode and enter the mass mode.
int _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
return stlink_q(stl);
}
return 0;
}
// 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_jtag_reset,
_stlink_sg_run,
_stlink_sg_status,
_stlink_sg_version,
_stlink_sg_read_debug32,
_stlink_sg_read_mem32,
_stlink_sg_write_debug32,
_stlink_sg_write_mem32,
_stlink_sg_write_mem8,
_stlink_sg_read_all_regs,
_stlink_sg_read_reg,
NULL, /* read_all_unsupported_regs */
NULL, /* read_unsupported_regs */
NULL, /* write_unsupported_regs */
_stlink_sg_write_reg,
_stlink_sg_step,
_stlink_sg_current_mode,
_stlink_sg_force_debug,
NULL
};
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);
libusb_exit(slsg->libusb_ctx);
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);
libusb_exit(slsg->libusb_ctx);
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);
libusb_exit(slsg->libusb_ctx);
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);
libusb_exit(slsg->libusb_ctx);
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);
libusb_exit(slsg->libusb_ctx);
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;
return sl;
}
// Mass Storage device to test bulk transfers (non destructive test)
static int test_mass_storage(libusb_device_handle *handle, uint8_t endpoint_in, uint8_t endpoint_out)
{
int r, size;
uint8_t lun;
uint32_t expected_tag;
uint32_t i, max_lba, block_size;
double device_size;
uint8_t cdb[16]; // SCSI Command Descriptor Block
uint8_t buffer[64];
char vid[9], pid[9], rev[5];
debug("Reading Max LUN:\n");
r = sceUsbdControlTransfer(handle, LIBUSB_ENDPOINT_IN|LIBUSB_REQUEST_TYPE_CLASS|LIBUSB_RECIPIENT_INTERFACE,
BOMS_GET_MAX_LUN, 0, 0, &lun, 1, 1000);
// Some devices send a STALL instead of the actual value.
// In such cases we should set lun to 0.
if (r == 0) {
lun = 0;
} else if (r < 0) {
debug(" Failed\n");
}
debug(" Max LUN = %d\n", lun);
// Send Inquiry
debug("Sending Inquiry:\n");
memset(buffer, 0, sizeof(buffer));
memset(cdb, 0, sizeof(cdb));
cdb[0] = 0x12; // Inquiry
cdb[4] = INQUIRY_LENGTH;
send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, INQUIRY_LENGTH, &expected_tag);
CALL_CHECK(sceUsbdBulkTransfer(handle, endpoint_in, (unsigned char*)&buffer, INQUIRY_LENGTH, &size, 1000));
debug(" received %d bytes\n", size);
// The following strings are not zero terminated
for (i=0; i<8; i++) {
vid[i] = buffer[8+i];
pid[i] = buffer[16+i];
rev[i/2] = buffer[32+i/2]; // instead of another loop
}
vid[8] = 0;
pid[8] = 0;
rev[4] = 0;
debug(" VID:PID:REV \"%8s\":\"%8s\":\"%4s\"\n", vid, pid, rev);
if (get_mass_storage_status(handle, endpoint_in, expected_tag) == -2) {
get_sense(handle, endpoint_in, endpoint_out);
}
// Read capacity
debug("Reading Capacity:\n");
memset(buffer, 0, sizeof(buffer));
memset(cdb, 0, sizeof(cdb));
cdb[0] = 0x25; // Read Capacity
send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, READ_CAPACITY_LENGTH, &expected_tag);
CALL_CHECK(sceUsbdBulkTransfer(handle, endpoint_in, (unsigned char*)&buffer, READ_CAPACITY_LENGTH, &size, 1000));
debug(" received %d bytes\n", size);
max_lba = be_to_int32(&buffer[0]);
block_size = be_to_int32(&buffer[4]);
device_size = ((double)(max_lba+1))*block_size/(1024*1024*1024);
debug(" Max LBA: %08X, Block Size: %08X (%.2f GB)\n", max_lba, block_size, device_size);
if (get_mass_storage_status(handle, endpoint_in, expected_tag) == -2) {
get_sense(handle, endpoint_in, endpoint_out);
}
// coverity[tainted_data]
//data = (unsigned char*) calloc(1, block_size);
//if (data == NULL) {
// debug(" unable to allocate data buffer\n");
// return -1;
//}
// Send Read
debug("Attempting to read %d bytes:\n", block_size);
memset(cdb, 0, sizeof(cdb));
cdb[0] = 0x28; // Read(10)
//cdb[8] = 0x01; // 1 block
cdb[8] = sizeof(cart) / block_size; // blocks
//send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, block_size, &expected_tag);
send_mass_storage_command(handle, endpoint_out, lun, cdb, LIBUSB_ENDPOINT_IN, sizeof(cart), &expected_tag);
//sceUsbdBulkTransfer(handle, endpoint_in, cart, block_size, &size, 5000);
sceUsbdBulkTransfer(handle, endpoint_in, cart, sizeof(cart), &size, 5000);
debug(" READ: received %d bytes\n", size);
if (get_mass_storage_status(handle, endpoint_in, expected_tag) == -2) {
get_sense(handle, endpoint_in, endpoint_out);
}// else {
//display_buffer_hex(data, size);
//}
//free(data);
return 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;
}
