diff --git a/src/lib.rs b/src/lib.rs
index 321be66..39b95aa 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -1,7 +1,173 @@
 #![no_std]
 
 mod types;
+    fn poll(&self) -> PollResult {
+        free(|cs| {
+            let usb = self.usb.borrow(cs);
 
-use types::*;
+            let (usbint, udint, udien) = (usb.usbint.read(), usb.udint.read(), usb.udien.read());
+            if usbint.vbusti().bit_is_set() {
+                usb.usbint.write(|w| w.vbusti().clear_bit());
+                if usb.usbsta.read().vbus().bit_is_set() {
+                    return PollResult::Resume;
+                } else {
+                    return PollResult::Suspend;
+                }
+            }
+            if udint.suspi().bit_is_set() && udien.suspe().bit_is_set() {
+                return PollResult::Suspend;
+            }
+            if udint.wakeupi().bit_is_set() && udien.wakeupe().bit_is_set() {
+                return PollResult::Resume;
+            }
+            if udint.eorsti().bit_is_set() {
+                return PollResult::Reset;
+            }
+            if udint.sofi().bit_is_set() {
+                usb.udint.write(|w| w.sofi().clear_bit());
+            }
 
+            // Can only query endpoints while clock is running
+            // (e.g. not in suspend state)
+            if usb.usbcon.read().frzclk().bit_is_clear() {
+                let (mut ep_out, mut ep_setup, mut ep_in_complete) = (0u8, 0u8, 0u8);
 
+                for (index, _ep) in self
+                    .ep_table
+                    .iter()
+                    .enumerate()
+                    .filter(|(_i, e)| e.is_allocated)
+                {
+                    if self.select_endpoint(cs, index).is_err() {
+                        // Endpoint selection has stopped working...
+                        break;
+                    }
+
+                    let ueintx = usb.ueintx.read();
+                    if ueintx.rxouti().bit_is_set() {
+                        ep_out |= 1 << index;
+                    }
+                    if ueintx.rxstpi().bit_is_set() {
+                        ep_setup |= 1 << index;
+                    }
+                    if ueintx.txini().bit_is_set() {
+                        ep_in_complete |= 1 << index;
+                    }
+                }
+                if ep_out | ep_setup | ep_in_complete != 0 {
+                    return PollResult::Data {
+                        ep_out: ep_out as u16,
+                        ep_in_complete: ep_in_complete as u16,
+                        ep_setup: ep_setup as u16,
+                    };
+                }
+            }
+
+            PollResult::None
+        })
+    }
+
+    fn read(&self, ep_addr: EndpointAddress, buf: &mut [u8]) -> UsbResult<usize> {
+        free(|cs| {
+            let usb = self.usb.borrow(cs);
+
+            match self.select_endpoint(cs, ep_addr.index()) {
+                Ok(()) => {
+                    let target_endpoint = self.ep_table[ep_addr.index()];
+
+                    let ueintx = usb.ueintx.read();
+
+                    if ueintx.rxouti().bit_is_clear() {
+                        return Err(UsbError::WouldBlock);
+                    }
+
+                    if target_endpoint.ep_type == 0 {
+                        let bytes_count_to_read: usize = (usb.uebchx.read().bits() as usize) << 8
+                            | (usb.uebclx.read().bits() as usize);
+
+                        if bytes_count_to_read > buf.len() {
+                            return Err(UsbError::BufferOverflow);
+                        }
+
+                        for slot in &mut buf[..bytes_count_to_read] {
+                            *slot = usb.uedatx.read().bits();
+                        }
+
+                        usb.ueintx
+                            .write(|w| w.rxouti().clear_bit().rxstpi().clear_bit());
+
+                        Ok(bytes_count_to_read)
+                    } else {
+                        usb.ueintx.write(|w| w.rxouti().clear_bit());
+
+                        let mut bytes_read = 0;
+                        for slot in buf {
+                            if usb.ueintx.read().rwal().bit_is_clear() {
+                                break;
+                            }
+                            *slot = usb.uedatx.read().bits();
+                            bytes_read += 1;
+                        }
+
+                        if usb.ueintx.read().rwal().bit_is_set() {
+                            return Err(UsbError::BufferOverflow);
+                        }
+
+                        usb.ueintx.write(|w| w.fifocon().clear_bit());
+                        Ok(bytes_read)
+                    }
+                }
+                Err(err) => Err(err),
+            }
+        })
+    }
+    fn write(&self, ep_addr: EndpointAddress, buf: &[u8]) -> UsbResult<usize> {
+        free(|cs| {
+            let usb = self.usb.borrow(cs);
+
+            match self.select_endpoint(cs, ep_addr.index()) {
+                Ok(()) => {
+                    let target_endpoint = self.ep_table[ep_addr.index()];
+
+                    let ueintx = usb.ueintx.read();
+
+                    if ueintx.rxouti().bit_is_clear() {
+                        return Err(UsbError::WouldBlock);
+                    }
+
+                    if target_endpoint.ep_type == 0 {
+                        let bytes_count_to_read: usize = (usb.uebchx.read().bits() as usize) << 8
+                            | (usb.uebclx.read().bits() as usize);
+
+                        if bytes_count_to_read > buf.len() {
+                            return Err(UsbError::BufferOverflow);
+                        }
+
+                        buf.iter()
+                            .for_each(|&byte| usb.uedatx.write(|w| w.bits(byte)));
+
+                        usb.ueintx
+                            .write(|w| w.rxouti().clear_bit().rxstpi().clear_bit());
+
+                        Ok(bytes_count_to_read)
+                    } else {
+                        usb.ueintx
+                            .write(|w| w.txini().clear_bit().rxouti().clear_bit());
+
+                        for &byte in buf {
+                            if usb.ueintx.read().rwal().bit_is_set() {
+                                return Err(UsbError::BufferOverflow);
+                            } else {
+                                usb.uedatx.write(|w| w.bits(byte));
+                            }
+                        }
+
+                        usb.ueintx
+                            .write(|w| w.fifocon().clear_bit().rxouti().clear_bit());
+                        Ok(buf.len())
+                    }
+                }
+                Err(err) => Err(err),
+            }
+        })
+    }