/* Watchdog write routine */
static void watchdog_write(const uint16_t pio, const uint8_t byte) {
uint8_t index = pio & 0xFF;
uint8_t bit_offset = (index & 3) << 3;
switch (index) {
case 0x004: case 0x005: case 0x006: case 0x007:
write8(watchdog.load, bit_offset, byte);
break;
case 0x008: case 0x009:
write8(watchdog.restart, bit_offset, byte);
if(watchdog.restart == 0x5AB9) {
event_set(SCHED_WATCHDOG, watchdog.load);
watchdog.count = watchdog.load;
watchdog.restart = 0;
}
break;
case 0x00C:
write8(watchdog.control, bit_offset, byte);
if(watchdog.control & 1) {
event_set(SCHED_WATCHDOG, watchdog.load);
} else {
watchdog.count = event_ticks_remaining(SCHED_WATCHDOG);
event_clear(SCHED_WATCHDOG);
}
if(watchdog.control & 16) {
sched.items[SCHED_WATCHDOG].clock = CLOCK_32K;
} else {
sched.items[SCHED_WATCHDOG].clock = CLOCK_CPU;
}
break;
case 0x014:
if(byte & 1) {
watchdog.status = 0;
}
break;
default:
break;
}
}
STORAGE_STATUS storage_verify(STORAGE* storage, unsigned long addr, unsigned long count, bool byte_compare)
{
storage->read_status = STORAGE_STATUS_OK;
int retry = STORAGE_RETRY_COUNT;
if (!storage->reading)
{
if (storage->media_descriptor)
{
if ((storage->media_descriptor->flags & STORAGE_MEDIA_FLAG_READ_PROTECTION) == 0)
{
if (addr < storage->media_descriptor->num_sectors && addr + count <= storage->media_descriptor->num_sectors)
{
if (storage->driver_cb->on_storage_prepare_read)
storage->read_status = storage->driver_cb->on_storage_prepare_read(storage->driver_param, addr, count);
if (storage->read_status == STORAGE_STATUS_OK)
{
storage->reading = true;
storage_update_state(storage);
unsigned long cur_addr = addr;
unsigned long sectors_left = count;
unsigned long sectors_cur;
char* buf;
char* verify_buf = queue_allocate_buffer_ms(storage->queue, INFINITE);
while (storage->read_status == STORAGE_STATUS_OK && sectors_left)
{
sectors_cur = storage->sectors_in_block;
if (sectors_cur > sectors_left)
sectors_cur = sectors_left;
event_clear(storage->rx_event);
storage->read_status = storage->driver_cb->on_storage_read_blocks(storage->driver_param, cur_addr, verify_buf, sectors_cur);
if (storage->read_status == STORAGE_STATUS_OK)
event_wait_ms(storage->rx_event, INFINITE);
if (storage->read_status == STORAGE_STATUS_OK)
{
if (byte_compare)
{
storage->host_io_cb->on_storage_request_buffers(storage->host_io_param, sectors_cur * storage->device_descriptor->sector_size);
buf = queue_pull_ms(storage->queue, INFINITE);
if (memcmp(verify_buf, buf, sectors_cur * storage->device_descriptor->sector_size) != 0)
storage->read_status = STORAGE_STATUS_MISCOMPARE;
queue_release_buffer(storage->queue, buf);
}
sectors_left -= sectors_cur;
cur_addr += sectors_cur;
}
else
{
while (retry && (storage->read_status == STORAGE_STATUS_TIMEOUT || storage->read_status == STORAGE_STATUS_CRC_ERROR))
{
--retry;
storage->read_status = STORAGE_STATUS_OK;
if (storage->driver_cb->on_storage_prepare_read)
storage->read_status = storage->driver_cb->on_storage_prepare_read(storage->driver_param, cur_addr, sectors_left);
}
}
}
if (sectors_left)
storage_cancel_io(storage);
else
{
storage->reading = false;
storage_update_state(storage);
}
if (storage->driver_cb->on_storage_read_done)
storage->driver_cb->on_storage_read_done(storage->driver_param);
queue_release_buffer(storage->queue, verify_buf);
}
}
else
storage->read_status = STORAGE_STATUS_INVALID_ADDRESS;
}
else
storage->read_status = STORAGE_STATUS_DATA_PROTECTED;
}
else
storage->read_status = STORAGE_STATUS_NO_MEDIA;
}
else
{
storage_cancel_io(storage);
storage->read_status = STORAGE_STATUS_OPERATION_IN_PROGRESS;
}
return storage->read_status;
}
STORAGE_STATUS storage_write(STORAGE* storage, unsigned long addr, unsigned long count)
{
storage->write_status = STORAGE_STATUS_OK;
bool write_finished;
if (!storage->writing)
{
if (storage->media_descriptor)
{
if ((storage->media_descriptor->flags & STORAGE_MEDIA_FLAG_WRITE_PROTECTION) == 0)
{
if (addr < storage->media_descriptor->num_sectors && addr + count <= storage->media_descriptor->num_sectors)
{
if (storage->driver_cb->on_storage_prepare_write)
storage->write_status = storage->driver_cb->on_storage_prepare_write(storage->driver_param, addr, count);
if (storage->write_status == STORAGE_STATUS_OK)
{
storage->writing = true;
storage_update_state(storage);
unsigned long cur_addr = addr;
unsigned long sectors_left = count;
unsigned long sectors_cur;
char* buf;
//double-buffering
storage->host_io_cb->on_storage_request_buffers(storage->host_io_param, sectors_left > storage->sectors_in_block ? storage->block_size :
sectors_left * storage->device_descriptor->sector_size);
int retry = STORAGE_RETRY_COUNT;
while (storage->write_status == STORAGE_STATUS_OK && sectors_left)
{
if (sectors_left > storage->sectors_in_block)
storage->host_io_cb->on_storage_request_buffers(storage->host_io_param, (sectors_left - storage->sectors_in_block > storage->sectors_in_block) ?
storage->block_size : (sectors_left - storage->sectors_in_block) * storage->device_descriptor->sector_size);
sectors_cur = storage->sectors_in_block;
if (sectors_cur > sectors_left)
sectors_cur = sectors_left;
buf = queue_pull_ms(storage->queue, INFINITE);
write_finished = false;
while (retry && !write_finished)
{
event_clear(storage->tx_event);
storage->write_status = storage->driver_cb->on_storage_write_blocks(storage->driver_param, cur_addr, buf, sectors_cur);
if (storage->write_status == STORAGE_STATUS_OK)
{
event_wait_ms(storage->tx_event, INFINITE);
if (storage->write_status == STORAGE_STATUS_OK)
{
queue_release_buffer(storage->queue, buf);
sectors_left -= sectors_cur;
cur_addr += sectors_cur;
write_finished = true;
}
}
if (storage->write_status != STORAGE_STATUS_OK)
{
storage_cancel_io(storage);
while (retry-- && (storage->write_status == STORAGE_STATUS_TIMEOUT || storage->write_status == STORAGE_STATUS_CRC_ERROR))
{
storage->write_status = STORAGE_STATUS_OK;
if (storage->driver_cb->on_storage_prepare_write)
storage->write_status = storage->driver_cb->on_storage_prepare_write(storage->driver_param, cur_addr, sectors_left);
}
}
}
}
storage->writing = false;
storage_update_state(storage);
if (storage->driver_cb->on_storage_write_done && sectors_left == 0)
storage->driver_cb->on_storage_write_done(storage->driver_param);
}
}
else
storage->write_status = STORAGE_STATUS_INVALID_ADDRESS;
}
else
storage->write_status = STORAGE_STATUS_DATA_PROTECTED;
}
else
storage->write_status = STORAGE_STATUS_NO_MEDIA;
}
else
{
storage_cancel_io(storage);
storage->write_status = STORAGE_STATUS_OPERATION_IN_PROGRESS;
}
return storage->write_status;
}
void usb_msc_thread(void* param)
{
USB_MSC* msc = (USB_MSC*)param;
for (;;)
{
event_wait_ms(msc->event, INFINITE);
msc->state = MSC_STATE_DATA;
//process received CBW
if (msc->cbw.dCBWSignature == CBW_MAGIC && msc->cbw.bCBWCBLength <= MAX_CB_SIZE)
{
msc->csw_status = CSW_STATUS_OK;
#if (USB_MSC_DEBUG_FLOW)
printf("USB_MSC: dCBWTag: %08x\n\r", msc->cbw.dCBWTag);
printf("USB_MSC: dCBWDataTransferLength: %08x\n\r", msc->cbw.dCBWDataTransferLength);
printf("USB_MSC: dCBWDataFlags: %02x\n\r", msc->cbw.bmCBWFlags);
printf("USB_MSC: dCBWLUN: %02x\n\r", msc->cbw.bCBWLUN);
printf("USB_MSC: dCBWCB:");
int i;
for (i = 0; i < msc->cbw.bCBWCBLength; ++i)
printf(" %02x", msc->cbw.CBWCB[i]);
printf(" (%d)\n\r", msc->cbw.bCBWCBLength);
#endif
if (!scsi_cmd(msc->scsi, (char*)msc->cbw.CBWCB, msc->cbw.bCBWCBLength))
msc->csw_status = CSW_STATUS_FAILED;
//wait for transfer completed in any case
event_wait_ms(msc->event, INFINITE);
}
//CBW invalid, phase ERROR
else
{
#if (USB_DEBUG_ERRORS)
printf("Invalid CBW\n\r");
#endif
msc->csw_status = CSW_STATUS_ERROR;
}
event_clear(msc->event);
msc->state = MSC_STATE_CSW;
//need zlp?
if ((msc->scsi_transferred % msc->ep_size) == 0 && msc->cbw.dCBWDataTransferLength != 0 &&
msc->scsi_transferred < msc->cbw.dCBWDataTransferLength)
{
if (msc->cbw.bmCBWFlags & 0x80)
usb_write(usbd_get_usb(msc->usbd), EP_IN(msc->ep_num), NULL, 0);
else
usb_read(usbd_get_usb(msc->usbd), EP_OUT(msc->ep_num), NULL, 0);
#if (USB_MSC_DEBUG_FLOW)
if (msc->cbw.bmCBWFlags & 0x80)
printf("USB_MSC: TX ZLP\n\r");
else
printf("USB_MSC: RX ZLP\n\r");
#endif
}
//send csw directly
else
{
if (msc->cbw.bmCBWFlags & 0x80)
on_msc_sent(EP_IN(msc->ep_num), msc);
else
on_msc_received(EP_OUT(msc->ep_num), msc);
}
}
}
