/* 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); } } }