From 665eca30c55cb410e8b6c5a1942118c89b6bae54 Mon Sep 17 00:00:00 2001 From: Vegard Storheil Eriksen Date: Tue, 1 Oct 2013 17:15:56 +0200 Subject: Read ISO15693 UID and emulate mass storage device. --- .gdbinit | 22 +++ .gitignore | 5 + .gitmodules | 3 + SConstruct | 11 ++ laks | 1 + main.cpp | 525 ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 6 files changed, 567 insertions(+) create mode 100644 .gdbinit create mode 100644 .gitignore create mode 100644 .gitmodules create mode 100644 SConstruct create mode 160000 laks create mode 100644 main.cpp diff --git a/.gdbinit b/.gdbinit new file mode 100644 index 0000000..99eae86 --- /dev/null +++ b/.gdbinit @@ -0,0 +1,22 @@ +define flash +file demo.elf +load +end + +define restart +run +end + +define attach_swd +mon swdp_scan +attach 1 +end + +define attach_jtag +mon jtag_scan +attach 1 +end + +file demo.elf + +set mem inaccessible-by-default off diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..b137a4a --- /dev/null +++ b/.gitignore @@ -0,0 +1,5 @@ +*.o +*.a +*.elf +*.swp +.sconsign.dblite diff --git a/.gitmodules b/.gitmodules new file mode 100644 index 0000000..cd4f3c2 --- /dev/null +++ b/.gitmodules @@ -0,0 +1,3 @@ +[submodule "laks"] + path = laks + url = git://git.jvnv.net/laks.git diff --git a/SConstruct b/SConstruct new file mode 100644 index 0000000..3d2c473 --- /dev/null +++ b/SConstruct @@ -0,0 +1,11 @@ +import os + +env = Environment( + ENV = os.environ, +) + +SConscript('laks/build_rules') + +env.SelectMCU('stm32f103cb') + +env.Firmware('demo.elf', Glob('*.cpp')) diff --git a/laks b/laks new file mode 160000 index 0000000..ef34f43 --- /dev/null +++ b/laks @@ -0,0 +1 @@ +Subproject commit ef34f43a98f03e11c08819dfaa77b8f7f5705b2e diff --git a/main.cpp b/main.cpp new file mode 100644 index 0000000..3722105 --- /dev/null +++ b/main.cpp @@ -0,0 +1,525 @@ +#include +#include +#include +#include +#include +#include + +auto dev_desc = device_desc(0x200, 0, 0, 0, 64, 0x1234, 0x56e3, 0, 0, 0, 0, 1); +auto conf_desc = configuration_desc(1, 1, 0, 0xc0, 0, + // MSC BBB + interface_desc(0, 0, 2, 0x08, 0x06, 0x50, 0, + endpoint_desc(0x81, 0x02, 64, 0), // IN + endpoint_desc(0x01, 0x02, 64, 0) // OUT + ) +); + +desc_t dev_desc_p = {sizeof(dev_desc), (void*)&dev_desc}; +desc_t conf_desc_p = {sizeof(conf_desc), (void*)&conf_desc}; + +Pin usb_dm = GPIOA[11]; +Pin usb_dp = GPIOA[12]; +Pin usb_disc = GPIOB[0]; + +USB_f1 usb(USB, dev_desc_p, conf_desc_p); + +Pin nfc_irq_in = GPIOA[2]; +Pin nfc_irq_out = GPIOA[3]; + +Pin nfc_ss = GPIOA[4]; +Pin nfc_sck = GPIOA[5]; +Pin nfc_miso = GPIOA[6]; +Pin nfc_mosi = GPIOA[7]; + +class CR95HF { + private: + SPI_t& spi; + Pin& ss; + + public: + CR95HF(SPI_t& spi_bus, Pin& p) : spi(spi_bus), ss(p) {} + + void send_cmd(uint8_t cmd, uint32_t len, uint8_t* buf) { + ss.off(); + + spi.transfer_byte(0); + spi.transfer_byte(cmd); + spi.transfer_byte(len); + + while(len--) { + spi.transfer_byte(*buf++); + } + + ss.on(); + } + + uint32_t get_response(uint32_t maxlen, uint8_t* buf) { + uint32_t len = 0; + + while(nfc_irq_out.get()); + + ss.off(); + + spi.transfer_byte(2); + *buf++ = spi.transfer_byte(); + len++; + + uint32_t datalen = spi.transfer_byte(); + *buf++ = datalen; + len++; + + while(len < datalen + 2) { + *buf++ = spi.transfer_byte(); + len++; + } + + ss.on(); + + return len; + } +}; + +CR95HF cr95hf(SPI1, nfc_ss); +/* +class USB_CR95HF : public USB_class_driver { + private: + USB_generic& usb; + + uint8_t pend_reg; + uint32_t pend_len; + uint8_t buf[256]; + + bool write(uint16_t wValue, uint16_t wIndex, uint16_t wLength) { + if(wLength > sizeof(buf)) { + return false; + } + + if(wLength == 0) { + cr95hf.send_cmd(wValue, 0, nullptr); + + usb.write(0, nullptr, 0); + + return true; + } + + pend_reg = wValue; + pend_len = wLength; + // Awaiting OUT packet. + + return true; + } + + bool read(uint16_t wValue, uint16_t wIndex, uint16_t wLength) { + if(wLength > sizeof(buf)) { + return false; + } + + wLength = cr95hf.get_response(wLength, buf); + + // TODO: wLength >= 64 + + usb.write(0, (uint32_t*)buf, wLength); + return true; + } + + public: + USB_CR95HF(USB_generic& usbd) : usb(usbd) { + usb.register_driver(this); + } + + protected: + virtual SetupStatus handle_setup(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength) { + if(bmRequestType == 0x40 && bRequest == 0xf2) { + return write(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall; + } + + if(bmRequestType == 0xc0 && bRequest == 0xf3) { + return read(wValue, wIndex, wLength) ? SetupStatus::Ok : SetupStatus::Stall; + } + + return SetupStatus::Unhandled; + } + + virtual void handle_out(uint8_t ep, uint32_t len) { + if(ep != 0 || len == 0) { + return; + } + + if(len != pend_len) { + return; + } + + usb.read(0, (uint32_t*)buf, len); + + cr95hf.send_cmd(pend_reg, len, buf); + + usb.write(0, nullptr, 0); + } +}; + +USB_CR95HF usb_cr95hf(usb); +*/ +struct CBW { + uint32_t dCBWSignature; + uint32_t dCBWTag; + uint32_t dCBWDataTransferLength; + uint8_t bmCBWFlags; + uint8_t bCBWLUN; + uint8_t bCBWCBLength; + uint8_t CBWCB[16]; +} __attribute__((packed)); + +struct CSW { + uint32_t dCSWSignature; + uint32_t dCSWTag; + uint32_t dCSWDataResidue; + uint8_t bCSWStatus; +} __attribute__((packed)); + +uint32_t nullbuf[16]; +uint32_t capbuf[] = { + 0x00010000, + 0x00020000 +}; +uint32_t inquirybuf[] = { + 0x02068020, + 32, + + 0x20202020, 0x20202020, + + 0x20202020, 0x20202020, 0x20202020, 0x20202020, + + 0x34333231, +}; + +uint32_t sensebuf[] = { + 0x00050070, + 0x0a000000, + 0, + 0x00000020, + 0 +}; + +uint32_t modesensebuf[] = { + 0x00800004 +}; + +//uint32_t sd_buf[512]; + +uint8_t sectors[][512] { + {}, // NULL + { // MBR + 0xeb, 0x3c, 0x90, 0x6d, 0x6b, 0x64, 0x6f, 0x73, 0x66, 0x73, 0x00, 0x00, 0x02, 0x04, 0x01, 0x00, + 0x02, 0x00, 0x02, 0x00, 0x01, 0xf8, 0x01, 0x00, 0x20, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x29, 0xc9, 0x6a, 0x10, 0xc4, 0x20, 0x20, 0x20, 0x20, 0x20, + 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x46, 0x41, 0x54, 0x31, 0x32, 0x20, 0x20, 0x20, 0x0e, 0x1f, + 0xbe, 0x5b, 0x7c, 0xac, 0x22, 0xc0, 0x74, 0x0b, 0x56, 0xb4, 0x0e, 0xbb, 0x07, 0x00, 0xcd, 0x10, + 0x5e, 0xeb, 0xf0, 0x32, 0xe4, 0xcd, 0x16, 0xcd, 0x19, 0xeb, 0xfe, 0x54, 0x68, 0x69, 0x73, 0x20, + 0x69, 0x73, 0x20, 0x6e, 0x6f, 0x74, 0x20, 0x61, 0x20, 0x62, 0x6f, 0x6f, 0x74, 0x61, 0x62, 0x6c, + 0x65, 0x20, 0x64, 0x69, 0x73, 0x6b, 0x2e, 0x20, 0x20, 0x50, 0x6c, 0x65, 0x61, 0x73, 0x65, 0x20, + 0x69, 0x6e, 0x73, 0x65, 0x72, 0x74, 0x20, 0x61, 0x20, 0x62, 0x6f, 0x6f, 0x74, 0x61, 0x62, 0x6c, + 0x65, 0x20, 0x66, 0x6c, 0x6f, 0x70, 0x70, 0x79, 0x20, 0x61, 0x6e, 0x64, 0x0d, 0x0a, 0x70, 0x72, + 0x65, 0x73, 0x73, 0x20, 0x61, 0x6e, 0x79, 0x20, 0x6b, 0x65, 0x79, 0x20, 0x74, 0x6f, 0x20, 0x74, + 0x72, 0x79, 0x20, 0x61, 0x67, 0x61, 0x69, 0x6e, 0x20, 0x2e, 0x2e, 0x2e, 0x20, 0x0d, 0x0a, 0x00, + }, + { // FAT + 0xf8, 0xff, 0xff, 0x00, 0xf0, 0xff, + }, + { // File table + 0x41, 0x63, 0x00, 0x61, 0x00, 0x72, 0x00, 0x64, 0x00, 0x30, 0x00, 0x0f, 0x00, 0xa7, 0x2e, 0x00, + 0x74, 0x00, 0x78, 0x00, 0x74, 0x00, 0x00, 0x00, 0xff, 0xff, 0x00, 0x00, 0xff, 0xff, 0xff, 0xff, + 0x43, 0x41, 0x52, 0x44, 0x30, 0x20, 0x20, 0x20, 0x54, 0x58, 0x54, 0x20, 0x00, 0x64, 0x53, 0x95, + 0x03, 0x43, 0x03, 0x43, 0x00, 0x00, 0x53, 0x95, 0x03, 0x43, 0x03, 0x00, 0x10, 0x00, 0x00, 0x00, + }, + "E004123456789AB", +}; + +uint8_t* get_sector(uint32_t sector) { + switch(sector) { + case 0: + return sectors[1]; + + case 1: + case 2: + return sectors[2]; + + case 3: + return sectors[3]; + + case 39: + return sectors[4]; + + default: + return sectors[0]; + } +} + +class USB_MSC_BBB : public USB_class_driver { + private: + USB_generic& usb; + + uint32_t buf[16]; + + uint32_t pending_data_in; + + bool pending_write; + uint32_t pending_write_addr; + uint32_t pending_write_num; + uint32_t pending_write_recv; + + CBW cbw; + CSW csw; + + public: + USB_MSC_BBB(USB_generic& usbd) : usb(usbd) { + usb.register_driver(this); + } + + protected: + virtual SetupStatus handle_setup(uint8_t bmRequestType, uint8_t bRequest, uint16_t wValue, uint16_t wIndex, uint16_t wLength) { + // Get max LUN + if(bmRequestType == 0xa1 && bRequest == 0xfe) { + uint32_t max_lun = 0; + usb.write(0, &max_lun, 1); + return SetupStatus::Ok; + } + + // Mass Storage Reset + if(bmRequestType == 0x21 && bRequest == 0xff) { + usb.write(0, nullptr, 0); + return SetupStatus::Ok; + } + + return SetupStatus::Unhandled; + } + + virtual void handle_set_configuration(uint8_t configuration) { + if(configuration) { + usb.register_out_handler(this, 1); + usb.hw_conf_ep(0x01, EPType::Bulk, 64); + usb.hw_conf_ep(0x81, EPType::Bulk, 64); + + pending_data_in = 0; + pending_write = false; + } + } + + virtual void handle_out(uint8_t ep, uint32_t len) { + if(ep == 0) { + if(len) { + usb.write(0, nullptr, 0); + } + + } else if(ep == 1 && pending_data_in != 0) { + if(pending_write) { + usb_rblog.log("Handling write packet. (%d bytes)", len); + + //handle_write_block_packet(); + + } else { + usb_rblog.log("Ignoring %d bytes of received data.", len); + } + + if(len > pending_data_in) { + pending_data_in = 0; + } else { + pending_data_in -= len; + } + + if(pending_data_in == 0) { + usb.write(1, (uint32_t*)&csw, sizeof(CSW)); + } + + } else if(ep == 1) { + + // Read CBW. + if(len != sizeof(CBW) || usb.read(ep, (uint32_t*)&cbw, len) != sizeof(CBW)) { + usb_rblog.log("Invalid CBW size."); + return; // FIXME: Indicate error. + } + + if(cbw.dCBWSignature != 0x43425355) { + usb_rblog.log("Invalid CBW signature."); + return; // FIXME: Indicate error. + } + + usb_rblog.log("Received CBW, tl=%d, flags=%#02x", cbw.dCBWDataTransferLength, cbw.bmCBWFlags); + + bool cmd_ok = handle_scsi_cmd(cbw.CBWCB, cbw.bCBWCBLength); + + if(!cmd_ok && cbw.dCBWDataTransferLength != 0 && cbw.bmCBWFlags & 0x80) { + write_zero(); + } + + if(cmd_ok) { + csw = {0x53425355, cbw.dCBWTag, 0, 0x00}; + } else { + csw = {0x53425355, cbw.dCBWTag, cbw.dCBWDataTransferLength, 0x01}; + } + + if(cbw.dCBWDataTransferLength != 0 && !(cbw.bmCBWFlags & 0x80)) { + pending_data_in = cbw.dCBWDataTransferLength; + return; + } + + usb.write(1, (uint32_t*)&csw, sizeof(CSW)); + } + } + + void write_zero() { + uint32_t len = cbw.dCBWDataTransferLength; + while(len > 64) { + usb.write(1, nullbuf, 64); + while(!usb.ep_ready(1)); + len -= 64; + } + + usb.write(1, nullbuf, len); + while(!usb.ep_ready(1)); + } + + void handle_read_block() { + uint32_t block = (cbw.CBWCB[2] << 24) | (cbw.CBWCB[3] << 16) | (cbw.CBWCB[4] << 8) | cbw.CBWCB[5]; + uint16_t num_blocks = (cbw.CBWCB[7] << 8) | cbw.CBWCB[8]; + + while(num_blocks--) { + uint32_t* bufp = (uint32_t*)get_sector(block++); + + for(uint32_t i = 0; i < 512; i += 64) { + usb.write(1, bufp + i / 4, 64); + while(!usb.ep_ready(1)); + } + } + } + + bool handle_scsi_cmd(uint8_t* cmd, uint32_t len) { + pending_write = false; + + if(!len) { + return false; + } + + switch(cmd[0]) { + // TEST UNIT READY + case 0x00: + return true; + + // REQUEST SENSE + case 0x03: + usb.write(1, sensebuf, cbw.dCBWDataTransferLength); + while(!usb.ep_ready(1)); + return true; + + // INQUIRY + case 0x12: + usb.write(1, inquirybuf, cbw.dCBWDataTransferLength); + while(!usb.ep_ready(1)); + return true; + + // MODE SENSE + case 0x1a: + usb.write(1, modesensebuf, cbw.dCBWDataTransferLength > 4 ? 4 : cbw.dCBWDataTransferLength); + while(!usb.ep_ready(1)); + return true; + + // READ CAPACITY + case 0x25: + usb.write(1, capbuf, cbw.dCBWDataTransferLength); + while(!usb.ep_ready(1)); + return true; + + // READ (10) + case 0x28: + handle_read_block(); + return true; + + default: + return false; + } + + return true; + } +}; + +USB_MSC_BBB usb_msc_bbb(usb); + +int main() { + // Initialize system timer. + STK.LOAD = 72000000 / 8 / 1000; // 1000 Hz. + STK.CTRL = 0x03; + + RCC.enable(RCC.GPIOA); + RCC.enable(RCC.GPIOB); + + usb_disc.on(); + usb_disc.set_type(Pin::OpenDrain); + usb_disc.set_mode(Pin::Output); + + usb_dm.set_mode(Pin::AF); + usb_dp.set_mode(Pin::AF); + + RCC.enable(RCC.USB); + + usb.init(); + + Time::sleep(10); + usb_disc.off(); + + RCC.enable(RCC.SPI1); + + nfc_irq_out.set_mode(Pin::Input); + nfc_irq_in.on(); + nfc_irq_in.set_mode(Pin::Output); + + nfc_ss.on(); + nfc_ss.set_mode(Pin::Output); + nfc_sck.set_mode(Pin::AF); + nfc_miso.set_mode(Pin::AF); + nfc_mosi.set_mode(Pin::AF); + + SPI1.reg.CR1 = (1 << 9) | (1 << 8) | (1 << 6) | (4 << 3) | (1 << 2) | (0 << 1) | (0 << 0); // SSM, SSI, SPE, 36/32MHz, MSTR, CPOL=0, CPHA=0 + SPI1.reg.CR2 = (1 << 12) | (7 << 8); // FRXTH, DS = 8bit + + Time::sleep(1); + nfc_irq_in.off(); + Time::sleep(1); + nfc_irq_in.on(); + Time::sleep(1); + + uint8_t buf[64]; + bool cmd_sent = false; + uint32_t nfc_delay_until = 0; + + cr95hf.send_cmd(0x02, 2, (uint8_t*)"\x01\x05"); // Select ISO 15693 + cr95hf.get_response(64, buf); + + while(1) { + if(Time::time() < nfc_delay_until) { + // Just do nothing. + } else if(!cmd_sent) { + cr95hf.send_cmd(0x04, 3, (uint8_t*)"\x26\x01\x00"); // INVENTORY + cmd_sent = true; + } else if(!nfc_irq_out.get()) { + cr95hf.get_response(64, buf); + cmd_sent = false; + + if(buf[0] == 128 && buf[14] == 0) { + usb_disc.on(); + uint8_t* id_raw = &buf[11]; + uint8_t* id_ascii = sectors[4]; + for(uint32_t i = 0; i < 8; i++) { + *id_ascii++ = "0123456789ABCDEF"[*id_raw >> 4]; + *id_ascii++ = "0123456789ABCDEF"[*id_raw-- & 0xf]; + } + Time::sleep(10); + usb_disc.off(); + + nfc_delay_until = Time::time() + 5000; // Valid for five seconds. + } else { + nfc_delay_until = Time::time() + 100; // Retry in 100ms. + } + } + + usb.process(); + } +} -- cgit v1.2.3