diff --git a/part7-bluetooth/Makefile b/part7-bluetooth/Makefile
index 143c51b..44bd023 100644
--- a/part7-bluetooth/Makefile
+++ b/part7-bluetooth/Makefile
@@ -1,7 +1,7 @@
 CFILES = $(wildcard *.c)
 OFILES = $(CFILES:.c=.o)
 LLVMPATH = /opt/homebrew/opt/llvm/bin
-GCCFLAGS = -Wall -O2 -ffreestanding -nostdinc -nostdlib -mcpu=cortex-a72+nosimd
+GCCFLAGS = -Wall -O0 -ffreestanding -nostdinc -nostdlib -mcpu=cortex-a72+nosimd
 
 all: clean kernel8.img
 
diff --git a/part7-bluetooth/bt.c b/part7-bluetooth/bt.c
index 0a1d6ad..7d148fe 100644
--- a/part7-bluetooth/bt.c
+++ b/part7-bluetooth/bt.c
@@ -350,15 +350,23 @@ void connect(unsigned char *addr)
     float BleGranularity = 1.25;
 
     unsigned int p = BleScanInterval / BleScanDivisor;
+    unsigned char lp = lo(p);
+    unsigned char hp = hi(p);
     unsigned int q = BleScanWindow / BleScanDivisor;
+    unsigned char lq = lo(q);
+    unsigned char hq = hi(q);
 
     unsigned int min_interval = connMinFreq / BleGranularity;
+    unsigned char lmini = lo(min_interval);
+    unsigned char hmini = hi(min_interval);
     unsigned int max_interval = connMaxFreq / BleGranularity;
+    unsigned char lmaxi = lo(max_interval);
+    unsigned char hmaxi = hi(max_interval);
 
-    params[0] = lo(p);
-    params[1] = hi(p);
-    params[2] = lo(q);
-    params[3] = hi(q);
+    params[0] = lp;
+    params[1] = hp;
+    params[2] = lq;
+    params[3] = hq;
     params[4] = 0;
     params[5] = 0;
     params[6] = addr[5];
@@ -368,10 +376,10 @@ void connect(unsigned char *addr)
     params[10] = addr[1];
     params[11] = addr[0];
     params[12] = 0;
-    params[13] = lo(min_interval);
-    params[14] = hi(min_interval);
-    params[15] = lo(max_interval);
-    params[16] = hi(max_interval);
+    params[13] = lmini;
+    params[14] = hmini;
+    params[15] = lmaxi;
+    params[16] = hmaxi;
     params[17] = 0;
     params[18] = 0;
     params[19] = 0x2a;
@@ -380,5 +388,6 @@ void connect(unsigned char *addr)
     params[22] = 0;
     params[23] = 0;
     params[24] = 0;
+
     if (hciCommand(OGF_LE_CONTROL, 0x0d, params, 25)) uart_writeText("createLEconnection failed\n");
 }
diff --git a/part7-bluetooth/fb.c b/part7-bluetooth/fb.c
index ff52a43..43fae91 100644
--- a/part7-bluetooth/fb.c
+++ b/part7-bluetooth/fb.c
@@ -5,6 +5,9 @@
 unsigned int width, height, pitch, isrgb;
 unsigned char *fb;
 
+int curx = 0;
+int cury = 0;
+
 void fb_init()
 {
     mbox[0] = 35*4; // Length of message in bytes
@@ -211,3 +214,47 @@ void wait_msec(unsigned int n)
     t+=((f/1000)*n)/1000;
     do{asm volatile ("mrs %0, cntpct_el0" : "=r"(r));}while(r<t);
 }
+
+int strlen(const char *str) {
+    const char *s;
+
+    for (s = str; *s; ++s);
+    return (s - str);
+}
+
+void debugstr(char *str) {
+    if (curx + (strlen(str) * 8)  >= 1920) {
+       curx = 0; cury += 8;
+    }
+    if (cury + 8 >= 1080) {
+       cury = 0;
+    }
+    drawString(curx, cury, str, 0x0f, 1);
+    curx += (strlen(str) * 8);
+}
+
+void debugcrlf(void) {
+    curx = 0; cury += 8;
+}
+
+void debugch(unsigned char b) {
+    unsigned int n;
+    int c;
+    for(c=4;c>=0;c-=4) {
+        n=(b>>c)&0xF;
+        n+=n>9?0x37:0x30;
+        debugstr((char *)&n);
+    }
+    debugstr(" ");
+}
+
+void debughex(unsigned int d) {
+    unsigned int n;
+    int c;
+    for(c=28;c>=0;c-=4) {
+        n=(d>>c)&0xF;
+        n+=n>9?0x37:0x30;
+        debugstr((char *)&n);
+    }
+    debugstr(" ");
+}
diff --git a/part7-bluetooth/fb.h b/part7-bluetooth/fb.h
index 6163d77..8beda43 100644
--- a/part7-bluetooth/fb.h
+++ b/part7-bluetooth/fb.h
@@ -7,3 +7,7 @@ void drawCircle(int x0, int y0, int radius, unsigned char attr, int fill);
 void drawLine(int x1, int y1, int x2, int y2, unsigned char attr);
 void moveRect(int oldx, int oldy, int width, int height, int shiftx, int shifty, unsigned char attr);
 void wait_msec(unsigned int n);
+void debugstr(char *str);
+void debugcrlf(void);
+void debugch(unsigned char b);
+void debughex(unsigned int d);
diff --git a/part7-bluetooth/io.h b/part7-bluetooth/io.h
index fdddcc0..b2f483e 100644
--- a/part7-bluetooth/io.h
+++ b/part7-bluetooth/io.h
@@ -1,5 +1,5 @@
 #define PERIPHERAL_BASE 0xFE000000
-#define SAFE_ADDRESS    0x02100000 // Somewhere safe to store a lot of data
+#define SAFE_ADDRESS    0x00210000 // Somewhere safe to store a lot of data
 
 void uart_init();
 void uart_writeText(char *buffer);
diff --git a/part7-bluetooth/kernel.c b/part7-bluetooth/kernel.c
index cff8917..e5e2066 100644
--- a/part7-bluetooth/kernel.c
+++ b/part7-bluetooth/kernel.c
@@ -2,9 +2,6 @@
 #include "bt.h"
 #include "fb.h"
 
-int curx = 0;
-int cury = 0;
-
 #define MAX_MSG_LEN    50
 #define MAX_READ_RUN   100
 
@@ -25,12 +22,8 @@ unsigned int got_echo_sid = 0;
 unsigned int got_echo_name = 0;
 unsigned char echo_addr[6];
 
-int strlen(const char *str) {
-    const char *s;
-
-    for (s = str; *s; ++s);
-    return (s - str);
-}
+unsigned int connected = 0;
+unsigned int connection_handle = 0;
 
 int memcmp(const char *str1, const char *str2, int count) {
     const char *s1 = (const char*)str1;
@@ -42,43 +35,6 @@ int memcmp(const char *str1, const char *str2, int count) {
     return 0;
 }
 
-void debugstr(char *str) {
-    if (curx + (strlen(str) * 8)  >= 1920) {
-       curx = 0; cury += 8; 
-    }
-    if (cury + 8 >= 1080) {
-       cury = 0;
-    }
-    drawString(curx, cury, str, 0x0f, 1);
-    curx += (strlen(str) * 8);
-}
-
-void debugcrlf(void) {
-    curx = 0; cury += 8;
-}
-
-void debugch(unsigned char b) {
-    unsigned int n;
-    int c;
-    for(c=4;c>=0;c-=4) {
-        n=(b>>c)&0xF;
-        n+=n>9?0x37:0x30;
-        debugstr((char *)&n);
-    }
-    debugstr(" ");
-}
-
-void debughex(unsigned int d) {
-    unsigned int n;
-    int c;
-    for(c=28;c>=0;c-=4) {
-        n=(d>>c)&0xF;
-        n+=n>9?0x37:0x30;
-        debugstr((char *)&n);
-    }
-    debugstr(" ");
-}
-
 void hci_poll2(unsigned char byte)
 {
     switch (poll_state) {
@@ -175,22 +131,91 @@ void bt_search(void) {
     }
 }
 
+void bt_conn()
+{
+    unsigned char *buf;
+
+    while ( (buf = hci_poll()) ) {
+       if (data_len >= 2) {
+          if (buf[0] == LE_CONNECT_CODE && !connected) {
+             connected = !buf[1];
+	     debughex(connected); debugstr(" ");
+	     connection_handle = buf[2] | (buf[3] << 8);
+	     debughex(connection_handle); debugstr(" ");
+	  }
+       }
+    }
+}
+
+void acl_poll()
+{
+    while (bt_isReadByteReady()) {
+       unsigned char byte = bt_readByte(); 
+
+       if (byte == HCI_EVENT_PKT) {
+	  bt_waitReadByte(); // opcode
+	  unsigned char length = bt_waitReadByte();
+	  for (int i=0;i<length;i++) bt_waitReadByte();
+       } else if (byte == HCI_ACL_PKT) {
+	  bt_waitReadByte(); // handle1
+	  bt_waitReadByte(); // handle2
+
+	  unsigned char h1 = bt_waitReadByte();
+	  unsigned char h2 = bt_waitReadByte();
+
+	  unsigned int dlen = h1 | (h2 << 8);
+	  unsigned char data[dlen];
+
+	  if (dlen > 7) {
+	     for (int i=0;i<dlen;i++) data[i] = bt_waitReadByte();
+
+	     unsigned int length = data[0] | (data[1] << 8);
+	     unsigned int channel = data[2] | (data[3] << 8);
+	     unsigned char opcode = data[4];
+
+	     if (length == 4 && opcode == 0x1b) {
+	        if (channel == 4 && data[5] == 0x2a && data[6] == 0x00) {
+		   debugcrlf();
+                   debugstr("Got ACL packet... ");
+                   debugch(data[7]); 
+                }
+	     }
+          }
+       }
+    }
+}
+
 void run_search(void) {
-    // Start scanning for echo
+    // Start scanning
     debugstr("Setting event mask... ");
     setLEeventmask(0xff);
     debugstr("Starting scanning... ");
     startActiveScanning();
 
-    // Enter an infinite loop
-    debugstr("Going loopy...");
+    // Search for the echo
+    debugstr("Waiting...");
     debugcrlf();
-
     while (!(got_echo_sid && got_echo_name)) bt_search();
     stopScanning();
     for (int c=0;c<=5;c++) debugch(echo_addr[c]);
+    debugcrlf();
 
+    // Connecting to echo
+    debugstr("Connecting to echo: ");
+    connect(echo_addr);
+    while (!connected) bt_conn();
+    debugstr("Connected!");
+    debugcrlf();
+
+    // Get the characteristic value
+    debugstr("Sending read request: ");
+    debughex(connection_handle); debugcrlf();
+    sendACLsubscribe(connection_handle);
+
+    // Enter an infinite loop
+    debugstr("Going loopy...");
     while (1) {
+       acl_poll();
        uart_update();
     }
 }