Read ISO15693 UID and emulate mass storage device.

6 files changed, 566 insertions, 0 deletions

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
+Subproject ef34f43a98f03e11c08819dfaa77b8f7f5705b2
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 <rcc/rcc.h>
+#include <gpio/gpio.h>
+#include <os/time.h>
+#include <spi/spi.h>
+#include <usb/usb.h>
+#include <usb/descriptor.h>
+
+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();
+	}
+}