Uploading multi-core version of Breakout as part11

part10-multicore/README.md

@@ -153,3 +153,7 @@ Finally, we look at _kernel.c_, where we now have a single `main()`, but also:
 _As you run this, you'll see that these functions run in parallel on their respective cores. Welcome to symmetric multi-processing!_
 
 ![Code now running on all four cores of the Raspberry Pi 4](images/10-multicore-running.jpg)
+
+Coming up in part 11, we'll put all of this work together for a multi-core version of our Breakout game.
+
+[Go to part11-breakout-smp>](../part11-breakout-smp)

part11-breakout-smp/Makefile

@@ -0,0 +1,25 @@
+CFILES = $(wildcard *.c lib/*.c)
+OFILES = $(CFILES:.c=.o)
+LLVMPATH = /opt/homebrew/opt/llvm/bin
+CLANGFLAGS = -Wall -O2 -ffreestanding -nostdinc -nostdlib -mcpu=cortex-a72+nosimd
+
+all: clean kernel8.img
+
+boot/boot.o: boot/boot.S
+	$(LLVMPATH)/clang --target=aarch64-elf $(CLANGFLAGS) -c $< -o $@
+
+bin/BCM4345C0.o : bin/BCM4345C0.hcd
+	$(LLVMPATH)/llvm-objcopy -I binary -O elf64-littleaarch64 -B aarch64 $< $@
+
+bin/audio.o : bin/audio.bin
+	$(LLVMPATH)/llvm-objcopy -I binary -O elf64-littleaarch64 -B aarch64 $< $@
+
+%.o: %.c
+	$(LLVMPATH)/clang --target=aarch64-elf $(CLANGFLAGS) -c $< -o $@
+
+kernel8.img: boot/boot.o $(OFILES) bin/BCM4345C0.o bin/audio.o
+	$(LLVMPATH)/ld.lld -m aarch64elf -nostdlib boot/boot.o $(OFILES) bin/BCM4345C0.o bin/audio.o -T boot/link.ld -o kernel8.elf
+	$(LLVMPATH)/llvm-objcopy -O binary kernel8.elf kernel8.img
+
+clean:
+	/bin/rm kernel8.elf *.o bin/*.o boot/*.o lib/*.o *.img > /dev/null 2> /dev/null || true

part11-breakout-smp/README.md

@@ -0,0 +1,25 @@
+Writing a "bare metal" operating system for Raspberry Pi 4 (Part 11)
+====================================================================
+
+Putting it all together
+-----------------------
+Frankly, I'm unlikely to write much documentation for this part. I'm also only providing a Clang _Makefile_ for now. If you're using gcc, have a go at putting your own _Makefile_ together, referencing the previous parts.
+
+This part simply builds on the work we've done so far, and delivers a new Breakout codebase with:
+
+ * Gameplay running in the foreground on the main CPU core 0
+ * Graphics updated in the background on CPU core 1
+ * Looped 8-bit music playing in the background on CPU core 2
+ * Bluetooth communications managed in the background on CPU core 3
+
+I've taken the opportunity to organise the code a little better and so you'll see some changes to the _Makefile_, and a new directory structure in place. Tidy codebase = tidy mind!
+
+Important takeaway
+------------------
+As you read through this code, you'll maybe notice that a very different style has emerged. Multi-processing adds a dimension of complexity when coding and requires a total mindset shift.
+
+There's a lot more signalling/semaphores, so the cores can be sure they're doing the right thing at the right time.
+
+You're no longer "dry-running" a single, sequential thread. You're having to spot the potential for bugs/unexpected behaviour from the interplay of four concurrent threads!
+
+_Good luck as you explore the code!_

part11-breakout-smp/boot/boot.S

@@ -0,0 +1,80 @@
+#define LOCAL_CONTROL   0xff800000
+#define LOCAL_PRESCALER 0xff800008
+#define OSC_FREQ        54000000
+#define MAIN_STACK      0x400000
+
+.section ".text.boot"  // Make sure the linker puts this at the start of the kernel image
+
+.global _start  // Execution starts here
+
+_start:
+    ldr     x0, =LOCAL_CONTROL   // Sort out the timer
+    str     wzr, [x0]
+    mov     w1, 0x80000000
+    str     w1, [x0, #(LOCAL_PRESCALER - LOCAL_CONTROL)]
+
+    ldr     x0, =OSC_FREQ
+    msr     cntfrq_el0, x0
+    msr     cntvoff_el2, xzr
+
+    // Check processor ID is zero (executing on main core), else hang
+    mrs     x1, mpidr_el1
+    and     x1, x1, #3
+    cbz     x1, 2f
+
+    // We're not on the main core, so hang in an infinite wait loop
+    adr     x5, spin_cpu0
+1:  wfe
+    ldr     x4, [x5, x1, lsl #3]
+    cbz     x4, 1b
+
+    ldr     x2, =__stack_start   // Get ourselves a fresh stack - location depends on CPU core asking
+    lsl     x1, x1, #9           // Multiply core_number by 512
+    add     x3, x2, x1           // Add to the address
+    mov     sp, x3
+
+    mov     x0, #0
+    mov     x1, #0
+    mov     x2, #0
+    mov     x3, #0
+    br      x4
+    b       1b
+2:  // We're on the main core!
+
+    // Set stack to start somewhere safe
+    mov     sp, #MAIN_STACK
+
+    // Clean the BSS section
+    ldr     x1, =__bss_start     // Start address
+    ldr     w2, =__bss_size      // Size of the section
+3:  cbz     w2, 4f               // Quit loop if zero
+    str     xzr, [x1], #8
+    sub     w2, w2, #1
+    cbnz    w2, 3b               // Loop if non-zero
+
+    // Jump to our main() routine in C (make sure it doesn't return)
+4:  bl      main
+    // In case it does return, halt the master core too
+    b       1b
+
+.ltorg
+
+.org 0xd8
+.globl spin_cpu0
+spin_cpu0:
+        .quad 0
+
+.org 0xe0
+.globl spin_cpu1
+spin_cpu1:
+        .quad 0
+
+.org 0xe8
+.globl spin_cpu2
+spin_cpu2:
+        .quad 0
+
+.org 0xf0
+.globl spin_cpu3
+spin_cpu3:
+        .quad 0

part11-breakout-smp/boot/link.ld

@@ -0,0 +1,35 @@
+SECTIONS
+{
+    .text : { KEEP(*(.text.boot)) *(.text .text.* .gnu.linkonce.t*) }
+    .rodata : { *(.rodata .rodata.* .gnu.linkonce.r*) }
+    PROVIDE(_data = .);
+    .data : { *(.data .data.* .gnu.linkonce.d*) }
+    .bss (NOLOAD) : {
+        . = ALIGN(16);
+        __bss_start = .;
+        *(.bss .bss.*)
+        *(COMMON)
+        __bss_end = .;
+        __bss_size = (__bss_end - __bss_start)>>3;
+    }
+    .cpu1Stack :
+    {
+        . = ALIGN(16);
+        __stack_start  = .;
+        . = . + 512;
+        __cpu1_stack  = .;
+    }
+    .cpu2Stack :
+    {
+        . = . + 512;
+        __cpu2_stack  = .;
+    }
+    .cpu3Stack :
+    {
+        . = . + 512;
+        __cpu3_stack  = .;
+    }
+    _end = .;
+
+   /DISCARD/ : { *(.comment) *(.gnu*) *(.note*) *(.eh_frame*) }
+}

part11-breakout-smp/breakout.c

@@ -0,0 +1,260 @@
+#include "breakout.h"
+#include "include/fb.h"
+#include "include/multicore.h"
+
+// OBJECT INITIALISERS
+
+void initBricks(void)
+{
+    int brickwidth = 32;
+    int brickheight = 8;
+    int brickspacer = 20;
+    const int brickcols[] = { 0x11, 0x22, 0xEE, 0x44, 0x66 };
+
+    int ybrick = MARGIN + brickheight;
+
+    for (int i=0; i<ROWS; i++) {
+       int xbrick = MARGIN + (VIRTWIDTH/COLS/2) - (brickwidth/2);
+        
+       for (int j = 0; j<COLS; j++) {
+	  objects[numobjs].type = OBJ_BRICK;
+	  objects[numobjs].x = xbrick;
+	  objects[numobjs].y = ybrick;
+	  objects[numobjs].width = brickwidth;
+	  objects[numobjs].height = brickheight;
+	  objects[numobjs].color = brickcols[i];
+	  objects[numobjs].alive = 1;
+	  objects[numobjs].redraw = 1;
+	  objects[numobjs].move = 0;
+	  numobjs++;
+
+          xbrick += (VIRTWIDTH/COLS);
+        }
+        ybrick = ybrick + brickspacer;
+     }
+}
+
+void initBall(void)
+{
+    objects[numobjs].type = OBJ_BALL;
+    objects[numobjs].x = (WIDTH/2) - ballradius;
+    objects[numobjs].y = (HEIGHT/2) - ballradius;
+    objects[numobjs].width = ballradius * 2;
+    objects[numobjs].height = ballradius * 2;
+    objects[numobjs].color = 0x55;
+    objects[numobjs].alive = 1;
+    objects[numobjs].redraw = 1;
+    objects[numobjs].move = 0;
+    ball = &objects[numobjs];
+    numobjs++;
+}
+
+void initPaddle(void)
+{
+    int paddleheight = 20;
+    int startx = MARGIN + (dir * ((VIRTWIDTH - paddlewidth + MARGIN)/100));
+
+    objects[numobjs].type = OBJ_PADDLE;
+    objects[numobjs].x = startx;
+    objects[numobjs].y = (HEIGHT-MARGIN-paddleheight);
+    objects[numobjs].width = paddlewidth;
+    objects[numobjs].height = paddleheight;
+    objects[numobjs].color = 0x11;
+    objects[numobjs].alive = 1;
+    objects[numobjs].redraw = 1;
+    objects[numobjs].move = 0;
+    paddle = &objects[numobjs];
+    numobjs++;
+}
+
+void initScoreboard(int points, int lives)
+{
+    objects[numobjs].type = OBJ_SCOREBOARD;
+    objects[numobjs].x = lives;
+    objects[numobjs].y = points;
+    objects[numobjs].width = 0;  // unused
+    objects[numobjs].height = 0; // unused
+    objects[numobjs].color = 0x0f;
+    objects[numobjs].alive = 1;
+    objects[numobjs].redraw = 1;
+    objects[numobjs].move = 0;
+    scoreboard = &objects[numobjs];
+    numobjs++;
+}
+
+void updateScoreboard(int points, int lives)
+{
+    while (scoreboard->move || scoreboard->redraw);   // Wait for any current move/redraw to finish
+
+    scoreboard->x = lives;
+    scoreboard->y = points;
+    scoreboard->redraw = 1;
+}
+
+void initEndgame(void)
+{
+    objects[numobjs].type = OBJ_ENDGAME;
+    objects[numobjs].x = 0;
+    objects[numobjs].y = 0;
+    objects[numobjs].width = 0;  // unused
+    objects[numobjs].height = 0; // unused
+    objects[numobjs].color = 0;
+    objects[numobjs].alive = 0;
+    objects[numobjs].redraw = 0;
+    objects[numobjs].move = 0;
+    endgame = &objects[numobjs];
+    numobjs++;
+}
+
+void updateEndgame(int gameover, int welldone) 
+{
+    while (endgame->move || endgame->redraw);   // Wait for any current move/redraw to finish
+
+    endgame->x = gameover;
+    endgame->y = welldone;
+    endgame->color = welldone ? 0x02 : 0x04;
+    endgame->alive = gameover | welldone;
+    endgame->redraw = 1;
+}
+
+void removeObject(volatile struct Object *object)
+{
+    while (object->move || object->redraw);   // Wait for any current move/redraw to finish
+
+    object->alive = 0;
+    object->redraw = 1;
+}
+
+volatile struct Object *detectObjectCollision(volatile struct Object *with, int xoff, int yoff)
+{
+    for (int i=0; i<numobjs;i++) {
+	if (&objects[i] != with && objects[i].alive == 1) {
+	   if (with->x + xoff > objects[i].x + objects[i].width || objects[i].x > with->x + xoff + with->width) {
+              // with is too far left or right to collide
+	   } else if (with->y + yoff > objects[i].y + objects[i].height || objects[i].y > with->y + yoff + with->height) {
+              // with is too far up or down to collide
+	   } else {
+	      // Collision!
+	      return &objects[i];
+	   }
+        }
+    }
+    return 0;
+}
+
+void moveObject(volatile struct Object *object, int x, int y)
+{
+    while (object->move || object->redraw);   // Wait for any current move/redraw to finish
+
+    object->newx = x;
+    object->newy = y;
+    object->move = 1;
+}
+
+// THE GAME
+
+const int ballradius = 15;
+const int paddlewidth = 80;
+
+volatile unsigned char dir = 50;
+volatile unsigned int numobjs = 0;
+
+volatile struct Object *objects = (struct Object *)SAFE_ADDRESS;
+volatile struct Object *ball;
+volatile struct Object *paddle;
+volatile struct Object *scoreboard;
+volatile struct Object *endgame;
+
+void breakout_init()
+{
+    initBricks();
+    initBall();
+    initPaddle();
+    initScoreboard(0, NUM_LIVES);
+    initEndgame();
+}
+
+void breakout()
+{
+    volatile struct Object *foundObject;
+
+    int bricks = ROWS * COLS;
+    int lives = NUM_LIVES;
+    int points = 0;
+
+    int velocity_x = 1;
+    int velocity_y = 3;
+
+    while (lives > 0 && bricks > 0) {
+       // Are we going to hit anything?
+       foundObject = detectObjectCollision(ball, velocity_x, velocity_y);
+
+       if (foundObject) {
+          if (foundObject == paddle) {
+             velocity_y = -velocity_y;
+	     // Are we going to hit the side of the paddle
+	     if (ball->x + ball->width + velocity_x == paddle->x || ball->x + velocity_x == paddle->x + paddle->width) velocity_x = -velocity_x;
+          } else if (foundObject->type == OBJ_BRICK) {
+             removeObject(foundObject);
+             velocity_y = -velocity_y;
+             bricks--;
+             points++;
+             updateScoreboard(points, lives);
+          }
+       }
+
+       moveObject(ball, ball->x + velocity_x, ball->y + velocity_y);
+       wait_msec(0x186A);
+
+       // Check we're in the game arena still
+       if (ball->x + ball->width >= WIDTH-MARGIN) {
+          velocity_x = -velocity_x;
+       } else if (ball->x <= MARGIN) {
+          velocity_x = -velocity_x;
+       } else if (ball->y + ball->height >= HEIGHT-MARGIN) {
+          lives--;
+
+	  removeObject(ball);
+	  removeObject(paddle);
+
+          updateScoreboard(points, lives);
+	  if (lives) {
+             initBall();
+             initPaddle();
+	  }
+       } else if (ball->y <= MARGIN) {
+          velocity_y = -velocity_y;
+       }
+    }
+
+    // Show endgame state
+    if (bricks == 0) {
+       removeObject(ball);
+       removeObject(paddle);
+       updateEndgame(0, 1);
+    } else {
+       updateEndgame(1, 0);
+    }
+
+    wait_msec(0x500000); // Wait 5 seconds
+
+    // Clear endgame state
+    updateEndgame(0, 0);
+}
+
+void main()
+{
+    fb_init();                  // Init the screen for debug
+
+    start_core3(comms_core);    // Start the comms engine (core 3)
+    while (!comms_up);          // Wait for comms up
+
+    breakout_init();            // Initialise the game engine (core 0)
+    start_core1(gfx_core);      // Start the graphics engine (core 1)
+    start_core2(snd_core);      // Start the sound engine (core 2)
+
+    while (1) {               
+       breakout();              // Run the game
+       breakout_init();         // Re-initialise the game engine
+    }
+}

part11-breakout-smp/breakout.h

@@ -0,0 +1,65 @@
+// Heap memory allocation
+#define SAFE_ADDRESS  0x00400000 // Somewhere safe to store a lot of data
+
+// The screen
+#define WIDTH         1920
+#define HEIGHT        1080
+#define MARGIN        30
+#define VIRTWIDTH     (WIDTH-(2*MARGIN))
+#define FONT_BPG      8
+
+// For the bricks
+#define ROWS          5
+#define COLS          10
+
+// Gameplay
+#define NUM_LIVES     3
+
+extern const int ballradius;
+extern const int paddlewidth;
+
+// Object tracking
+
+struct Object
+{
+    unsigned int type;
+
+    unsigned int x;
+    unsigned int y;
+    unsigned int width;
+    unsigned int height;
+    unsigned char color;
+
+    unsigned char alive;
+    unsigned char redraw;
+
+    unsigned int newx;
+    unsigned int newy;
+    unsigned char move;
+};
+
+enum {
+    OBJ_NONE         = 0,
+    OBJ_BRICK        = 1,
+    OBJ_PADDLE       = 2,
+    OBJ_BALL         = 3,
+    OBJ_SCOREBOARD   = 4,
+    OBJ_ENDGAME      = 5
+};
+
+extern volatile unsigned int numobjs;
+extern volatile struct Object *objects;
+extern volatile struct Object *ball;
+extern volatile struct Object *paddle;
+extern volatile struct Object *scoreboard;
+extern volatile struct Object *endgame;
+
+extern volatile unsigned int comms_up;
+extern volatile unsigned char dir;
+
+// Shared functions
+
+void gfx_core(void);
+void snd_core(void);
+void comms_core(void);
+void moveObject(volatile struct Object *object, int x, int y);

part11-breakout-smp/breakout_comms.c

@@ -0,0 +1,243 @@
+#include "breakout.h"
+#include "include/multicore.h"
+#include "include/io.h"
+#include "include/bt.h"
+#include "include/fb.h"
+
+#define MAX_MSG_LEN    50
+#define MAX_READ_RUN   100
+
+unsigned char data_buf[MAX_MSG_LEN];
+unsigned int data_len;
+unsigned int messages_received = 0;
+unsigned int poll_state = 0;
+
+enum {
+    LE_EVENT_CODE             = 0x3e,
+    LE_CONNECT_CODE           = 0x01,
+    LE_ADREPORT_CODE          = 0x02,
+    HCI_ACL_PKT               = 0x02,
+    HCI_EVENT_PKT             = 0x04
+};
+
+unsigned int got_echo_sid = 0;
+unsigned int got_echo_name = 0;
+unsigned char echo_addr[6];
+
+unsigned int connected = 0;
+unsigned int connection_handle = 0;
+
+volatile unsigned int comms_up = 0;
+
+int memcmp(const char *str1, const char *str2, int count) {
+    const char *s1 = (const char*)str1;
+    const char *s2 = (const char*)str2;
+
+    while (count-- > 0) {
+       if (*s1++ != *s2++) return s1[-1] < s2[-1] ? -1 : 1;
+    }
+    return 0;
+}
+
+void hci_poll2(unsigned char byte)
+{
+    switch (poll_state) {
+       case 0:
+	  if (byte != HCI_EVENT_PKT) poll_state = 0;
+	  else poll_state = 1;
+	  break;
+       case 1:
+	  if (byte != LE_EVENT_CODE) poll_state = 0;
+	  else poll_state = 2;
+	  break;
+       case 2:
+	  if (byte > MAX_MSG_LEN) poll_state = 0;
+	  else {
+	     poll_state = 3;
+	     data_len = byte;
+	  }
+	  break;
+       default:
+	  data_buf[poll_state - 3] = byte;
+	  if (poll_state == data_len + 3 - 1) {
+	     messages_received++;
+             poll_state = 0;
+	  } else poll_state++;
+   }
+}
+
+unsigned char *hci_poll()
+{
+    unsigned int goal = messages_received + 1;
+
+    if (bt_isReadByteReady()) {
+       unsigned int run = 0;
+
+       while (run < MAX_READ_RUN && messages_received < goal && bt_isReadByteReady()) {
+          unsigned char byte = bt_readByte(); 
+	  hci_poll2(byte);
+	  run++;
+       }
+       if (run == MAX_READ_RUN) return 0;
+       else return data_buf;
+    }
+    return 0;
+}
+
+void bt_search(void) {
+    unsigned char *buf;
+
+    while ( (buf = hci_poll()) ) {
+       if (data_len >= 2) {
+          if (buf[0] == LE_ADREPORT_CODE) {
+             if (buf[1] == 1) { // num_reports
+                if (buf[2] == 0) { // event_type
+                   int bufindex = 0;
+		   unsigned char ad_len = buf[11];
+
+                   for (int c=9;c>=4;c--) echo_addr[9-c] = buf[bufindex + c]; // save the mac address
+                   bufindex += 11;
+
+                   got_echo_sid = 0; got_echo_name = 0; // Reset the search state machine
+                   do {
+                      ad_len = buf[bufindex];
+                      unsigned char ad_type = buf[bufindex + 1];
+                      bufindex += 2;
+
+                      if (ad_len >= 2) {
+                         if (ad_type == 0x03) {
+			    unsigned int sid = buf[bufindex] | (buf[bufindex + 1] << 8);
+			    if (sid == 0xEC00) {
+			       got_echo_sid = 1;
+			       debugstr("got sid... ");
+			    }
+                         } else if (ad_type == 0x09) {
+                            char remote_name[ad_len - 1];
+		            unsigned int d=0;
+
+		            while (d<ad_len - 1) {
+			       remote_name[d] = buf[bufindex + d];
+		               d++;
+		            }
+			    if (!memcmp(remote_name,"echo",4)) {
+			       got_echo_name = 1;
+			       debugstr("got name... ");
+			    }
+                         }
+                      }
+
+                      bufindex += ad_len - 1;
+                   } while (bufindex < data_len);
+                }
+             }
+          }
+       }
+    }
+}
+
+void bt_conn()
+{
+    unsigned char *buf;
+
+    while ( (buf = hci_poll()) ) {
+       if (!connected && data_len >= 2 && buf[0] == LE_CONNECT_CODE) {
+          connected = !*(buf+1);
+	  debughex(connected); debugstr(" ");
+	  connection_handle = *(buf+2) | (*(buf+3) << 8);
+	  debughex(connection_handle); debugstr(" ");
+
+	  if (connection_handle == 0) wait_msec(0x186A);
+       }
+    }
+}
+
+void acl_poll()
+{
+    while (bt_isReadByteReady()) {
+       unsigned char byte = bt_waitReadByte(); 
+
+       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) {
+	  unsigned char h1 = bt_waitReadByte(); // handle1
+	  unsigned char h2 = bt_waitReadByte(); // handle2
+          unsigned char thandle = h1 | (h2 << 8);
+
+	  unsigned char d1 = bt_waitReadByte();
+	  unsigned char d2 = bt_waitReadByte();
+
+	  unsigned int dlen = d1 | (d2 << 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 (thandle == connection_handle && length == 4 && opcode == 0x1b) {
+	        if (channel == 4 && data[5] == 0x2a && data[6] == 0x00) {
+	      	   dir = data[7];
+                   moveObject(paddle, MARGIN + (dir * ((VIRTWIDTH - paddlewidth + MARGIN)/100)), paddle->y);
+                }
+	     }
+          }
+       }
+    }
+}
+
+void comms_core(void)
+{
+    clear_core3();
+
+    uart_init();
+    bt_init();
+
+    debugstr("Initialising Bluetooth: ");
+    debugstr(">> reset: ");
+    bt_reset();
+    debugstr(">> firmware load: ");
+    bt_loadfirmware();
+    debugstr(">> set baud: ");
+    bt_setbaud();
+    debugstr(">> set bdaddr: ");
+    bt_setbdaddr();
+
+    // Print the BD_ADDR
+    unsigned char local_addr[6];
+    bt_getbdaddr(local_addr);
+    for (int c=5;c>=0;c--) debugch(local_addr[c]);
+    debugcrlf();
+
+    // Start scanning
+    debugstr("Setting event mask... ");
+    setLEeventmask(0xff);
+    debugstr("Starting scanning... ");
+    startActiveScanning();
+
+    // 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 && connection_handle)) bt_conn();
+    debugstr("Connected!");
+    debugcrlf();
+
+    // Subscribe to updates
+    debugstr("Sending read request: ");
+    debughex(connection_handle); debugcrlf();
+    sendACLsubscribe(connection_handle);
+
+    comms_up = 1;
+    while (1) acl_poll();
+}

part11-breakout-smp/breakout_gfx.c

@@ -0,0 +1,75 @@
+#include "breakout.h"
+#include "include/fb.h"
+#include "include/multicore.h"
+
+// GRAPHICS ROUTINES
+
+void gfx_drawObject(volatile struct Object *object)
+{
+    const int zoom = WIDTH/192;
+    const int strwidth = 10 * FONT_BPG * zoom;
+    const int strheight = FONT_BPG * zoom;
+
+    char tens, ones;
+
+    switch(object->type) {
+       case OBJ_BALL:
+          drawCircle(object->x + ballradius, object->y + ballradius, ballradius, object->alive ? object->color : 0, 1);
+          break;
+       case OBJ_SCOREBOARD:
+          tens = object->y / 10;
+          ones = object->y - (10 * tens);
+
+          drawString((WIDTH/2)-252, MARGIN-25, "Score: 0     Lives:  ", object->color, 3);
+          drawChar(tens + 0x30, (WIDTH/2)-252 + (8*8*3), MARGIN-25, object->color, 3);
+          drawChar(ones + 0x30, (WIDTH/2)-252 + (8*9*3), MARGIN-25, object->color, 3);
+          drawChar((char)object->x + 0x30, (WIDTH/2)-252 + (8*20*3), MARGIN-25, object->color, 3);
+          break;
+       case OBJ_ENDGAME:
+          if (object->alive) {
+             if (object->x) {
+                drawString((WIDTH/2)-(strwidth/2), (HEIGHT/2)-(strheight/2), "Game over!", object->color, zoom);
+             } else if (object->y) {
+                drawString((WIDTH/2)-(strwidth/2), (HEIGHT/2)-(strheight/2), "Well done!", object->color, zoom);
+             }
+          } else {
+             drawString((WIDTH/2)-(strwidth/2), (HEIGHT/2)-(strheight/2), "          ", 0, zoom);
+          }
+          break;
+       default:
+          drawRect(object->x, object->y, object->x + object->width, object->y + object->height, object->alive ? object->color : 0, 1);
+          break;
+    }
+    object->redraw = 0;
+}
+
+void gfx_moveObject(volatile struct Object *object)
+{
+    if (object->type == OBJ_BALL) {
+       drawCircle(object->x + ballradius, object->y + ballradius, ballradius, 0, 1);
+       object->redraw = 1;
+    } else {
+       moveRectAbs(object->x, object->y, object->width, object->height, object->newx, object->newy, 0);
+    }
+
+    object->x = object->newx; 
+    object->y = object->newy;
+
+    object->move = 0;
+    object->newx = 0; 
+    object->newy = 0;
+}
+
+void gfx_update(void)
+{
+    for (int i=0;i<numobjs;i++) {
+       if (objects[i].move) gfx_moveObject(&objects[i]);
+       if (objects[i].redraw) gfx_drawObject(&objects[i]);
+    }
+}
+
+void gfx_core(void)
+{
+    clear_core1();
+    while (1) gfx_update();
+}

part11-breakout-smp/breakout_snd.c

@@ -0,0 +1,102 @@
+#include "include/fb.h"
+#include "include/io.h"
+#include "include/multicore.h"
+
+#define PWM_BASE        (PERIPHERAL_BASE + 0x20C000 + 0x800) /* PWM1 register base address on RPi4 */
+#define PWM_LEGACY_BASE     (LEGACY_BASE + 0x20C000 + 0x800) /* PWM1 register base legacy address on RPi4 */
+#define CLOCK_BASE      (PERIPHERAL_BASE + 0x101000)
+
+#define BCM2711_PWMCLK_CNTL 40
+#define BCM2711_PWMCLK_DIV  41
+#define PM_PASSWORD 0x5A000000 
+
+#define BCM2711_PWM_CONTROL 0
+#define BCM2711_PWM_STATUS  1
+#define BCM2711_PWM_DMAC    2
+#define BCM2711_PWM0_RANGE  4
+#define BCM2711_PWM0_DATA   5
+#define BCM2711_PWM_FIFO    6
+#define BCM2711_PWM1_RANGE  8
+#define BCM2711_PWM1_DATA   9
+
+#define BCM2711_PWM1_USEFIFO  0x2000  /* Data from FIFO */
+#define BCM2711_PWM1_ENABLE   0x0100  /* Channel enable */
+#define BCM2711_PWM0_USEFIFO  0x0020  /* Data from FIFO */
+#define BCM2711_PWM0_ENABLE   0x0001  /* Channel enable */
+#define BCM2711_PWM_ENAB      0x80000000  /* PWM DMA Configuration: DMA Enable (bit 31 set) */
+
+#define BCM2711_GAPO2 0x20
+#define BCM2711_GAPO1 0x10
+#define BCM2711_RERR1 0x8
+#define BCM2711_WERR1 0x4
+#define BCM2711_FULL1 0x1
+#define ERRORMASK (BCM2711_GAPO2 | BCM2711_GAPO1 | BCM2711_RERR1 | BCM2711_WERR1)
+
+volatile unsigned* clk = (void*)CLOCK_BASE;
+volatile unsigned* pwm = (void*)PWM_BASE;
+
+void audio_init(void)
+{   
+    gpio_useAsAlt0(40); // Ensure PWM1 is mapped to GPIO 40/41
+    gpio_useAsAlt0(41);
+    wait_msec(2);
+
+    // Setup clock
+    
+    *(clk + BCM2711_PWMCLK_CNTL) = PM_PASSWORD | (1 << 5); // Stop clock
+    wait_msec(2);
+
+    int idiv = 2;
+    *(clk + BCM2711_PWMCLK_DIV)  = PM_PASSWORD | (idiv<<12);
+    *(clk + BCM2711_PWMCLK_CNTL) = PM_PASSWORD | 16 | 1;  // Osc + Enable
+    wait_msec(2);
+
+    // Setup PWM
+    
+    *(pwm + BCM2711_PWM_CONTROL) = 0;
+    wait_msec(2);
+    
+    *(pwm+BCM2711_PWM0_RANGE) = 0x264; // 44.1khz, Stereo, 8-bit (54Mhz / 44100 / 2)
+    *(pwm+BCM2711_PWM1_RANGE) = 0x264;
+    
+    *(pwm+BCM2711_PWM_CONTROL) =
+          BCM2711_PWM1_USEFIFO |
+          BCM2711_PWM1_ENABLE | 
+          BCM2711_PWM0_USEFIFO | 
+          BCM2711_PWM0_ENABLE | 1<<6;
+}
+
+void snd_core(void)
+{
+    clear_core2();
+
+    int i=0;
+    long status;
+
+    extern unsigned char _binary_bin_audio_bin_start[];
+    extern unsigned char _binary_bin_audio_bin_size[];
+    
+    unsigned int size = (long)&_binary_bin_audio_bin_size;
+    unsigned char *data = &(_binary_bin_audio_bin_start[0]);
+
+    // Initialise the audio
+    audio_init();
+
+    // Write data out to FIFO and loop infinitely
+
+    while (1) {
+       while (i < size) {
+          status = *(pwm + BCM2711_PWM_STATUS);
+          if (!(status & BCM2711_FULL1)) {
+             *(pwm+BCM2711_PWM_FIFO) = *(data + i);
+             i++;
+             *(pwm+BCM2711_PWM_FIFO) = *(data + i);
+             i++;
+          }
+          if ((status & ERRORMASK)) {
+             *(pwm+BCM2711_PWM_STATUS) = ERRORMASK;
+          }
+       }
+       i=0;
+    }
+}

part11-breakout-smp/include/bt.h

@@ -0,0 +1,15 @@
+void bt_reset();
+void bt_loadfirmware();
+void bt_setbaud();
+void bt_setbdaddr();
+void bt_getbdaddr(unsigned char *bdaddr);
+void bt_init();
+unsigned int bt_isReadByteReady();
+unsigned char bt_readByte();
+unsigned char bt_waitReadByte();
+void setLEeventmask(unsigned char mask);
+void startActiveScanning();
+void stopScanning();
+void startActiveAdvertising();
+void connect(unsigned char *addr);
+void sendACLsubscribe(unsigned int handle);

part11-breakout-smp/include/fb.h

@@ -0,0 +1,13 @@
+void fb_init();
+void drawPixel(int x, int y, unsigned char attr);
+void drawChar(unsigned char ch, int x, int y, unsigned char attr, int zoom);
+void drawString(int x, int y, char *s, unsigned char attr, int zoom);
+void drawRect(int x1, int y1, int x2, int y2, unsigned char attr, int fill);
+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 moveRectAbs(int oldx, int oldy, int width, int height, int newx, int newy, 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);

part11-breakout-smp/include/io.h

@@ -0,0 +1,15 @@
+#define PERIPHERAL_BASE 0xFE000000
+
+void uart_init();
+void uart_writeText(char *buffer);
+void uart_loadOutputFifo();
+unsigned char uart_readByte();
+unsigned int uart_isReadByteReady();
+void uart_writeByteBlockingActual(unsigned char ch);
+void uart_update();
+void mmio_write(long reg, unsigned int val);
+unsigned int mmio_read(long reg);
+void gpio_useAsAlt0(unsigned int pin_number);
+void gpio_useAsAlt3(unsigned int pin_number);
+void uart_hex(unsigned int d);
+void uart_byte(unsigned char b);

part11-breakout-smp/include/mb.h

@@ -0,0 +1,34 @@
+extern volatile unsigned int mbox[36];
+
+enum {
+    MBOX_REQUEST  = 0
+};
+
+enum {
+    MBOX_CH_POWER = 0,
+    MBOX_CH_FB    = 1,
+    MBOX_CH_VUART = 2,
+    MBOX_CH_VCHIQ = 3,
+    MBOX_CH_LEDS  = 4,
+    MBOX_CH_BTNS  = 5,
+    MBOX_CH_TOUCH = 6,
+    MBOX_CH_COUNT = 7,
+    MBOX_CH_PROP  = 8 // Request from ARM for response by VideoCore
+};
+
+enum {
+    MBOX_TAG_SETPOWER   = 0x28001,
+    MBOX_TAG_SETCLKRATE = 0x38002,
+
+    MBOX_TAG_SETPHYWH   = 0x48003,
+    MBOX_TAG_SETVIRTWH  = 0x48004,
+    MBOX_TAG_SETVIRTOFF = 0x48009,
+    MBOX_TAG_SETDEPTH   = 0x48005,
+    MBOX_TAG_SETPXLORDR = 0x48006,
+    MBOX_TAG_GETFB      = 0x40001,
+    MBOX_TAG_GETPITCH   = 0x40008,
+
+    MBOX_TAG_LAST       = 0
+};
+
+unsigned int mbox_call(unsigned char ch);

part11-breakout-smp/include/multicore.h

@@ -0,0 +1,11 @@
+extern unsigned int spin_cpu0;
+extern unsigned int spin_cpu1;
+extern unsigned int spin_cpu2;
+extern unsigned int spin_cpu3;
+
+void start_core1(void (*func)(void));
+void start_core2(void (*func)(void));
+void start_core3(void (*func)(void));
+void clear_core1(void);
+void clear_core2(void);
+void clear_core3(void);

part11-breakout-smp/include/terminal.h

@@ -0,0 +1,253 @@
+const unsigned int vgapal[16] = {
+    0x000000,
+    0x0000AA,
+    0x00AA00,
+    0x00AAAA,
+    0xAA0000,
+    0xAA00AA,
+    0xAA5500,
+    0xAAAAAA,
+    0x555555,
+    0x5555FF,
+    0x55FF55,
+    0x55FFFF,
+    0xFF5555,
+    0xFF55FF,
+    0xFFFF55,
+    0xFFFFFF
+};
+
+enum {
+    FONT_WIDTH     = 8,
+    FONT_HEIGHT    = 8,
+    FONT_BPG       = 8,  // Bytes per glyph
+    FONT_BPL       = 1,  // Bytes per line
+    FONT_NUMGLYPHS = 224
+};
+
+const unsigned char font[FONT_NUMGLYPHS][FONT_BPG] = {
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0000 (nul)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0001
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0002
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0003
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0004
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0005
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0006
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0007
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0008
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0009
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+000A
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+000B
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+000C
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+000D
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+000E
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+000F
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0010
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0011
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0012
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0013
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0014
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0015
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0016
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0017
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0018
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0019
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+001A
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+001B
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+001C
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+001D
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+001E
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+001F
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0020 (space)
+    { 0x18, 0x3C, 0x3C, 0x18, 0x18, 0x00, 0x18, 0x00},   // U+0021 (!)
+    { 0x36, 0x36, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0022 (")
+    { 0x36, 0x36, 0x7F, 0x36, 0x7F, 0x36, 0x36, 0x00},   // U+0023 (#)
+    { 0x0C, 0x3E, 0x03, 0x1E, 0x30, 0x1F, 0x0C, 0x00},   // U+0024 ($)
+    { 0x00, 0x63, 0x33, 0x18, 0x0C, 0x66, 0x63, 0x00},   // U+0025 (%)
+    { 0x1C, 0x36, 0x1C, 0x6E, 0x3B, 0x33, 0x6E, 0x00},   // U+0026 (&)
+    { 0x06, 0x06, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0027 (')
+    { 0x18, 0x0C, 0x06, 0x06, 0x06, 0x0C, 0x18, 0x00},   // U+0028 (()
+    { 0x06, 0x0C, 0x18, 0x18, 0x18, 0x0C, 0x06, 0x00},   // U+0029 ())
+    { 0x00, 0x66, 0x3C, 0xFF, 0x3C, 0x66, 0x00, 0x00},   // U+002A (*)
+    { 0x00, 0x0C, 0x0C, 0x3F, 0x0C, 0x0C, 0x00, 0x00},   // U+002B (+)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x0C, 0x06},   // U+002C (,)
+    { 0x00, 0x00, 0x00, 0x3F, 0x00, 0x00, 0x00, 0x00},   // U+002D (-)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x0C, 0x00},   // U+002E (.)
+    { 0x60, 0x30, 0x18, 0x0C, 0x06, 0x03, 0x01, 0x00},   // U+002F (/)
+    { 0x3E, 0x63, 0x73, 0x7B, 0x6F, 0x67, 0x3E, 0x00},   // U+0030 (0)
+    { 0x0C, 0x0E, 0x0C, 0x0C, 0x0C, 0x0C, 0x3F, 0x00},   // U+0031 (1)
+    { 0x1E, 0x33, 0x30, 0x1C, 0x06, 0x33, 0x3F, 0x00},   // U+0032 (2)
+    { 0x1E, 0x33, 0x30, 0x1C, 0x30, 0x33, 0x1E, 0x00},   // U+0033 (3)
+    { 0x38, 0x3C, 0x36, 0x33, 0x7F, 0x30, 0x78, 0x00},   // U+0034 (4)
+    { 0x3F, 0x03, 0x1F, 0x30, 0x30, 0x33, 0x1E, 0x00},   // U+0035 (5)
+    { 0x1C, 0x06, 0x03, 0x1F, 0x33, 0x33, 0x1E, 0x00},   // U+0036 (6)
+    { 0x3F, 0x33, 0x30, 0x18, 0x0C, 0x0C, 0x0C, 0x00},   // U+0037 (7)
+    { 0x1E, 0x33, 0x33, 0x1E, 0x33, 0x33, 0x1E, 0x00},   // U+0038 (8)
+    { 0x1E, 0x33, 0x33, 0x3E, 0x30, 0x18, 0x0E, 0x00},   // U+0039 (9)
+    { 0x00, 0x0C, 0x0C, 0x00, 0x00, 0x0C, 0x0C, 0x00},   // U+003A (:)
+    { 0x00, 0x0C, 0x0C, 0x00, 0x00, 0x0C, 0x0C, 0x06},   // U+003B (;)
+    { 0x18, 0x0C, 0x06, 0x03, 0x06, 0x0C, 0x18, 0x00},   // U+003C (<)
+    { 0x00, 0x00, 0x3F, 0x00, 0x00, 0x3F, 0x00, 0x00},   // U+003D (=)
+    { 0x06, 0x0C, 0x18, 0x30, 0x18, 0x0C, 0x06, 0x00},   // U+003E (>)
+    { 0x1E, 0x33, 0x30, 0x18, 0x0C, 0x00, 0x0C, 0x00},   // U+003F (?)
+    { 0x3E, 0x63, 0x7B, 0x7B, 0x7B, 0x03, 0x1E, 0x00},   // U+0040 (@)
+    { 0x0C, 0x1E, 0x33, 0x33, 0x3F, 0x33, 0x33, 0x00},   // U+0041 (A)
+    { 0x3F, 0x66, 0x66, 0x3E, 0x66, 0x66, 0x3F, 0x00},   // U+0042 (B)
+    { 0x3C, 0x66, 0x03, 0x03, 0x03, 0x66, 0x3C, 0x00},   // U+0043 (C)
+    { 0x1F, 0x36, 0x66, 0x66, 0x66, 0x36, 0x1F, 0x00},   // U+0044 (D)
+    { 0x7F, 0x46, 0x16, 0x1E, 0x16, 0x46, 0x7F, 0x00},   // U+0045 (E)
+    { 0x7F, 0x46, 0x16, 0x1E, 0x16, 0x06, 0x0F, 0x00},   // U+0046 (F)
+    { 0x3C, 0x66, 0x03, 0x03, 0x73, 0x66, 0x7C, 0x00},   // U+0047 (G)
+    { 0x33, 0x33, 0x33, 0x3F, 0x33, 0x33, 0x33, 0x00},   // U+0048 (H)
+    { 0x1E, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+0049 (I)
+    { 0x78, 0x30, 0x30, 0x30, 0x33, 0x33, 0x1E, 0x00},   // U+004A (J)
+    { 0x67, 0x66, 0x36, 0x1E, 0x36, 0x66, 0x67, 0x00},   // U+004B (K)
+    { 0x0F, 0x06, 0x06, 0x06, 0x46, 0x66, 0x7F, 0x00},   // U+004C (L)
+    { 0x63, 0x77, 0x7F, 0x7F, 0x6B, 0x63, 0x63, 0x00},   // U+004D (M)
+    { 0x63, 0x67, 0x6F, 0x7B, 0x73, 0x63, 0x63, 0x00},   // U+004E (N)
+    { 0x1C, 0x36, 0x63, 0x63, 0x63, 0x36, 0x1C, 0x00},   // U+004F (O)
+    { 0x3F, 0x66, 0x66, 0x3E, 0x06, 0x06, 0x0F, 0x00},   // U+0050 (P)
+    { 0x1E, 0x33, 0x33, 0x33, 0x3B, 0x1E, 0x38, 0x00},   // U+0051 (Q)
+    { 0x3F, 0x66, 0x66, 0x3E, 0x36, 0x66, 0x67, 0x00},   // U+0052 (R)
+    { 0x1E, 0x33, 0x07, 0x0E, 0x38, 0x33, 0x1E, 0x00},   // U+0053 (S)
+    { 0x3F, 0x2D, 0x0C, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+0054 (T)
+    { 0x33, 0x33, 0x33, 0x33, 0x33, 0x33, 0x3F, 0x00},   // U+0055 (U)
+    { 0x33, 0x33, 0x33, 0x33, 0x33, 0x1E, 0x0C, 0x00},   // U+0056 (V)
+    { 0x63, 0x63, 0x63, 0x6B, 0x7F, 0x77, 0x63, 0x00},   // U+0057 (W)
+    { 0x63, 0x63, 0x36, 0x1C, 0x1C, 0x36, 0x63, 0x00},   // U+0058 (X)
+    { 0x33, 0x33, 0x33, 0x1E, 0x0C, 0x0C, 0x1E, 0x00},   // U+0059 (Y)
+    { 0x7F, 0x63, 0x31, 0x18, 0x4C, 0x66, 0x7F, 0x00},   // U+005A (Z)
+    { 0x1E, 0x06, 0x06, 0x06, 0x06, 0x06, 0x1E, 0x00},   // U+005B ([)
+    { 0x03, 0x06, 0x0C, 0x18, 0x30, 0x60, 0x40, 0x00},   // U+005C (\)
+    { 0x1E, 0x18, 0x18, 0x18, 0x18, 0x18, 0x1E, 0x00},   // U+005D (])
+    { 0x08, 0x1C, 0x36, 0x63, 0x00, 0x00, 0x00, 0x00},   // U+005E (^)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0xFF},   // U+005F (_)
+    { 0x0C, 0x0C, 0x18, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+0060 (`)
+    { 0x00, 0x00, 0x1E, 0x30, 0x3E, 0x33, 0x6E, 0x00},   // U+0061 (a)
+    { 0x07, 0x06, 0x06, 0x3E, 0x66, 0x66, 0x3B, 0x00},   // U+0062 (b)
+    { 0x00, 0x00, 0x1E, 0x33, 0x03, 0x33, 0x1E, 0x00},   // U+0063 (c)
+    { 0x38, 0x30, 0x30, 0x3e, 0x33, 0x33, 0x6E, 0x00},   // U+0064 (d)
+    { 0x00, 0x00, 0x1E, 0x33, 0x3f, 0x03, 0x1E, 0x00},   // U+0065 (e)
+    { 0x1C, 0x36, 0x06, 0x0f, 0x06, 0x06, 0x0F, 0x00},   // U+0066 (f)
+    { 0x00, 0x00, 0x6E, 0x33, 0x33, 0x3E, 0x30, 0x1F},   // U+0067 (g)
+    { 0x07, 0x06, 0x36, 0x6E, 0x66, 0x66, 0x67, 0x00},   // U+0068 (h)
+    { 0x0C, 0x00, 0x0E, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+0069 (i)
+    { 0x30, 0x00, 0x30, 0x30, 0x30, 0x33, 0x33, 0x1E},   // U+006A (j)
+    { 0x07, 0x06, 0x66, 0x36, 0x1E, 0x36, 0x67, 0x00},   // U+006B (k)
+    { 0x0E, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+006C (l)
+    { 0x00, 0x00, 0x33, 0x7F, 0x7F, 0x6B, 0x63, 0x00},   // U+006D (m)
+    { 0x00, 0x00, 0x1F, 0x33, 0x33, 0x33, 0x33, 0x00},   // U+006E (n)
+    { 0x00, 0x00, 0x1E, 0x33, 0x33, 0x33, 0x1E, 0x00},   // U+006F (o)
+    { 0x00, 0x00, 0x3B, 0x66, 0x66, 0x3E, 0x06, 0x0F},   // U+0070 (p)
+    { 0x00, 0x00, 0x6E, 0x33, 0x33, 0x3E, 0x30, 0x78},   // U+0071 (q)
+    { 0x00, 0x00, 0x3B, 0x6E, 0x66, 0x06, 0x0F, 0x00},   // U+0072 (r)
+    { 0x00, 0x00, 0x3E, 0x03, 0x1E, 0x30, 0x1F, 0x00},   // U+0073 (s)
+    { 0x08, 0x0C, 0x3E, 0x0C, 0x0C, 0x2C, 0x18, 0x00},   // U+0074 (t)
+    { 0x00, 0x00, 0x33, 0x33, 0x33, 0x33, 0x6E, 0x00},   // U+0075 (u)
+    { 0x00, 0x00, 0x33, 0x33, 0x33, 0x1E, 0x0C, 0x00},   // U+0076 (v)
+    { 0x00, 0x00, 0x63, 0x6B, 0x7F, 0x7F, 0x36, 0x00},   // U+0077 (w)
+    { 0x00, 0x00, 0x63, 0x36, 0x1C, 0x36, 0x63, 0x00},   // U+0078 (x)
+    { 0x00, 0x00, 0x33, 0x33, 0x33, 0x3E, 0x30, 0x1F},   // U+0079 (y)
+    { 0x00, 0x00, 0x3F, 0x19, 0x0C, 0x26, 0x3F, 0x00},   // U+007A (z)
+    { 0x38, 0x0C, 0x0C, 0x07, 0x0C, 0x0C, 0x38, 0x00},   // U+007B ({)
+    { 0x18, 0x18, 0x18, 0x00, 0x18, 0x18, 0x18, 0x00},   // U+007C (|)
+    { 0x07, 0x0C, 0x0C, 0x38, 0x0C, 0x0C, 0x07, 0x00},   // U+007D (})
+    { 0x6E, 0x3B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+007E (~)
+    { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},   // U+007F
+    { 0x1E, 0x33, 0x03, 0x33, 0x1E, 0x18, 0x30, 0x1E},   // U+00C7 (C cedille)
+    { 0x00, 0x33, 0x00, 0x33, 0x33, 0x33, 0x7E, 0x00},   // U+00FC (u umlaut)
+    { 0x38, 0x00, 0x1E, 0x33, 0x3F, 0x03, 0x1E, 0x00},   // U+00E9 (e aigu)
+    { 0x7E, 0xC3, 0x3C, 0x60, 0x7C, 0x66, 0xFC, 0x00},   // U+00E2 (a circumflex)
+    { 0x33, 0x00, 0x1E, 0x30, 0x3E, 0x33, 0x7E, 0x00},   // U+00E4 (a umlaut)
+    { 0x07, 0x00, 0x1E, 0x30, 0x3E, 0x33, 0x7E, 0x00},   // U+00E0 (a grave)
+    { 0x0C, 0x0C, 0x1E, 0x30, 0x3E, 0x33, 0x7E, 0x00},   // U+00E5 (a ring)
+    { 0x00, 0x00, 0x1E, 0x03, 0x03, 0x1E, 0x30, 0x1C},   // U+00E7 (c cedille)
+    { 0x7E, 0xC3, 0x3C, 0x66, 0x7E, 0x06, 0x3C, 0x00},   // U+00EA (e circumflex)
+    { 0x33, 0x00, 0x1E, 0x33, 0x3F, 0x03, 0x1E, 0x00},   // U+00EB (e umlaut)
+    { 0x07, 0x00, 0x1E, 0x33, 0x3F, 0x03, 0x1E, 0x00},   // U+00E8 (e grave)
+    { 0x33, 0x00, 0x0E, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+00EF (i umlaut)
+    { 0x3E, 0x63, 0x1C, 0x18, 0x18, 0x18, 0x3C, 0x00},   // U+00EE (i circumflex)
+    { 0x07, 0x00, 0x0E, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+00EC (i grave)
+    { 0x63, 0x1C, 0x36, 0x63, 0x7F, 0x63, 0x63, 0x00},   // U+00C4 (A umlaut)
+    { 0x0C, 0x0C, 0x00, 0x1E, 0x33, 0x3F, 0x33, 0x00},   // U+00C5 (A ring)
+    { 0x07, 0x00, 0x3F, 0x06, 0x1E, 0x06, 0x3F, 0x00},   // U+00C8 (E grave)
+    { 0x00, 0x00, 0xFE, 0x30, 0xFE, 0x33, 0xFE, 0x00},   // U+00E6 (ae)
+    { 0x7C, 0x36, 0x33, 0x7F, 0x33, 0x33, 0x73, 0x00},   // U+00C6 (AE)
+    { 0x1E, 0x33, 0x00, 0x1E, 0x33, 0x33, 0x1E, 0x00},   // U+00F4 (o circumflex)
+    { 0x00, 0x33, 0x00, 0x1E, 0x33, 0x33, 0x1E, 0x00},   // U+00F6 (o umlaut)
+    { 0x00, 0x07, 0x00, 0x1E, 0x33, 0x33, 0x1E, 0x00},   // U+00F2 (o grave)
+    { 0x1E, 0x33, 0x00, 0x33, 0x33, 0x33, 0x7E, 0x00},   // U+00FB (u circumflex)
+    { 0x00, 0x07, 0x00, 0x33, 0x33, 0x33, 0x7E, 0x00},   // U+00F9 (u grave)
+    { 0x00, 0x33, 0x00, 0x33, 0x33, 0x3E, 0x30, 0x1F},   // U+00FF (y umlaut)
+    { 0xC3, 0x18, 0x3C, 0x66, 0x66, 0x3C, 0x18, 0x00},   // U+00D6 (O umlaut)
+    { 0x33, 0x00, 0x33, 0x33, 0x33, 0x33, 0x1E, 0x00},   // U+00DC (U umlaut)
+    { 0x18, 0x18, 0x7E, 0x03, 0x03, 0x7E, 0x18, 0x18},   // U+00A2 (dollarcents)
+    { 0x1C, 0x36, 0x26, 0x0F, 0x06, 0x67, 0x3F, 0x00},   // U+00A3 (pound sterling)
+    { 0x33, 0x33, 0x1E, 0x3F, 0x0C, 0x3F, 0x0C, 0x0C},   // U+00A5 (yen)
+    { 0x7C, 0xC6, 0x1C, 0x36, 0x36, 0x1C, 0x33, 0x1E},   // U+00A7 (paragraph)
+    { 0x70, 0xD8, 0x18, 0x3C, 0x18, 0x18, 0x1B, 0x0E},   // U+0192 (dutch florijn)
+    { 0x38, 0x00, 0x1E, 0x30, 0x3E, 0x33, 0x7E, 0x00},   // U+00E1 (a aigu)
+    { 0x1C, 0x00, 0x0E, 0x0C, 0x0C, 0x0C, 0x1E, 0x00},   // U+00ED (i augu)
+    { 0x00, 0x38, 0x00, 0x1E, 0x33, 0x33, 0x1E, 0x00},   // U+00F3 (o aigu)
+    { 0x00, 0x38, 0x00, 0x33, 0x33, 0x33, 0x7E, 0x00},   // U+00FA (u aigu)
+    { 0x00, 0x1F, 0x00, 0x1F, 0x33, 0x33, 0x33, 0x00},   // U+00F1 (n ~)
+    { 0x3F, 0x00, 0x33, 0x37, 0x3F, 0x3B, 0x33, 0x00},   // U+00D1 (N ~)
+    { 0x3C, 0x36, 0x36, 0x7C, 0x00, 0x00, 0x00, 0x00},   // U+00AA (superscript a)
+    { 0x1C, 0x36, 0x36, 0x1C, 0x00, 0x00, 0x00, 0x00},   // U+00BA (superscript 0)
+    { 0x0C, 0x00, 0x0C, 0x06, 0x03, 0x33, 0x1E, 0x00},   // U+00BF (inverted ?)
+    { 0x00, 0x00, 0x00, 0x3F, 0x03, 0x03, 0x00, 0x00},   // U+2310 (gun pointing right)
+    { 0x00, 0x00, 0x00, 0x3F, 0x30, 0x30, 0x00, 0x00},   // U+00AC (gun pointing left)
+    { 0xC3, 0x63, 0x33, 0x7B, 0xCC, 0x66, 0x33, 0xF0},   // U+00BD (1/2)
+    { 0xC3, 0x63, 0x33, 0xBD, 0xEC, 0xF6, 0xF3, 0x03},   // U+00BC (1/4)
+    { 0x18, 0x18, 0x00, 0x18, 0x18, 0x18, 0x18, 0x00},   // U+00A1 (inverted !)
+    { 0x00, 0xCC, 0x66, 0x33, 0x66, 0xCC, 0x00, 0x00},   // U+00AB (<<)
+    { 0x00, 0x33, 0x66, 0xCC, 0x66, 0x33, 0x00, 0x00},   // U+00BB (>>)
+    { 0x55, 0x00, 0xAA, 0x00, 0x55, 0x00, 0xAA, 0x00},   // U+2591 (25% solid)
+    { 0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA, 0x55, 0xAA},   // U+2592 (50% solid)
+    { 0xFF, 0xAA, 0xFF, 0x55, 0xFF, 0xAA, 0xFF, 0x55},   // U+2593 (75% solid)
+    { 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08, 0x08},   // U+2502 (thin vertical)
+    { 0x08, 0x08, 0x08, 0x08, 0x0f, 0x08, 0x08, 0x08},   // U+2524 (down L, left L, up L)
+    { 0x08, 0x08, 0x08, 0x0F, 0x08, 0x0F, 0x08, 0x08},   // U+2561 (up L, down L, left D)
+    { 0x14, 0x14, 0x14, 0x14, 0x17, 0x14, 0x14, 0x14},   // U+2562 (up D, down D, left L)
+    { 0x00, 0x00, 0x00, 0x00, 0x1F, 0x14, 0x14, 0x14},   // U+2556 (down D, left L)
+    { 0x00, 0x00, 0x00, 0x0F, 0x08, 0x0F, 0x08, 0x08},   // U+2555 (down L, left D)
+    { 0x14, 0x14, 0x14, 0x17, 0x10, 0x17, 0x14, 0x14},   // U+2563 (up D, down D, left D)
+    { 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14, 0x14},   // U+2551 (double vertical)
+    { 0x00, 0x00, 0x00, 0x1F, 0x10, 0x17, 0x14, 0x14},   // U+2557 (down D, left D)
+    { 0x14, 0x14, 0x14, 0x17, 0x10, 0x1F, 0x00, 0x00},   // U+255D (up D, left D)
+    { 0x14, 0x14, 0x14, 0x14, 0x1F, 0x00, 0x00, 0x00},   // U+255C (up D, left L)
+    { 0x08, 0x08, 0x08, 0x0F, 0x08, 0x0F, 0x00, 0x00},   // U+255B (up L, left D)
+    { 0x00, 0x00, 0x00, 0x00, 0x0f, 0x08, 0x08, 0x08},   // U+2510 (down L, left L)
+    { 0x08, 0x08, 0x08, 0x08, 0xf8, 0x00, 0x00, 0x00},   // U+2514 (up L, right L)
+    { 0x08, 0x08, 0x08, 0x08, 0xff, 0x00, 0x00, 0x00},   // U+2534 (up L, right L, left L)
+    { 0x00, 0x00, 0x00, 0x00, 0xff, 0x08, 0x08, 0x08},   // U+252C (down L, right L, left L)
+    { 0x08, 0x08, 0x08, 0x08, 0xf8, 0x08, 0x08, 0x08},   // U+251C (down L, right L, up L)
+    { 0x00, 0x00, 0x00, 0x00, 0xFF, 0x00, 0x00, 0x00},   // U+2500 (thin horizontal)
+    { 0x08, 0x08, 0x08, 0x08, 0xff, 0x08, 0x08, 0x08},   // U+253C (up L, right L, left L, down L)
+    { 0x08, 0x08, 0x08, 0xF8, 0x08, 0xF8, 0x08, 0x08},   // U+255E (up L, down L, right D)
+    { 0x14, 0x14, 0x14, 0x14, 0xF4, 0x14, 0x14, 0x14},   // U+255F (up D, down D, right L)
+    { 0x14, 0x14, 0x14, 0xF4, 0x04, 0xFC, 0x00, 0x00},   // U+255A (up D, right D)
+    { 0x00, 0x00, 0x00, 0xFC, 0x04, 0xF4, 0x14, 0x14},   // U+2554 (down D, right D)
+    { 0x14, 0x14, 0x14, 0xF7, 0x00, 0xFF, 0x00, 0x00},   // U+2569 (left D, right D, up D)
+    { 0x00, 0x00, 0x00, 0xFF, 0x00, 0xF7, 0x14, 0x14},   // U+2566 (left D, right D, down D)
+    { 0x14, 0x14, 0x14, 0xF4, 0x04, 0xF4, 0x14, 0x14},   // U+2560 (up D, down D, right D)
+    { 0x00, 0x00, 0x00, 0xFF, 0x00, 0xFF, 0x00, 0x00},   // U+2550 (double horizontal)
+    { 0x14, 0x14, 0x14, 0xF7, 0x00, 0xF7, 0x14, 0x14},   // U+256C (left D, right D, down D, up D)
+    { 0x08, 0x08, 0x08, 0xFF, 0x00, 0xFF, 0x00, 0x00},   // U+2567 (left D, right D, up L)
+    { 0x14, 0x14, 0x14, 0x14, 0xFF, 0x00, 0x00, 0x00},   // U+2568 (left L, right L, up D)
+    { 0x00, 0x00, 0x00, 0xFF, 0x00, 0xFF, 0x08, 0x08},   // U+2564 (left D, right D, down L)
+    { 0x00, 0x00, 0x00, 0x00, 0xFF, 0x14, 0x14, 0x14},   // U+2565 (left L, right L, down D)
+    { 0x14, 0x14, 0x14, 0x14, 0xFC, 0x00, 0x00, 0x00},   // U+2559 (up D, right L)
+    { 0x08, 0x08, 0x08, 0xF8, 0x08, 0xF8, 0x00, 0x00},   // U+2558 (up L, right D)
+    { 0x00, 0x00, 0x00, 0xF8, 0x08, 0xF8, 0x08, 0x08},   // U+2552 (down L, right D)
+    { 0x00, 0x00, 0x00, 0x00, 0xFC, 0x14, 0x14, 0x14},   // U+2553 (down D, right L)
+    { 0x14, 0x14, 0x14, 0x14, 0xFF, 0x14, 0x14, 0x14},   // U+256B (left L, right L, down D, up D)
+    { 0x08, 0x08, 0x08, 0xFF, 0x08, 0xFF, 0x08, 0x08},   // U+256A (left D, right D, down L, up L)
+    { 0x08, 0x08, 0x08, 0x08, 0x0f, 0x00, 0x00, 0x00},   // U+2518 (up L, left L)
+    { 0x00, 0x00, 0x00, 0x00, 0xf8, 0x08, 0x08, 0x08},   // U+250C (down L, right L)
+    { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF},   // U+2588 (solid)
+    { 0x00, 0x00, 0x00, 0x00, 0xFF, 0xFF, 0xFF, 0xFF},   // U+2584 (bottom half)
+    { 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F, 0x0F},   // U+258C (left half)
+    { 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0, 0xF0},   // U+2590 (right half)
+    { 0xFF, 0xFF, 0xFF, 0xFF, 0x00, 0x00, 0x00, 0x00},   // U+2580 (top half)
+};

part11-breakout-smp/lib/bt.c

@@ -0,0 +1,356 @@
+#include "../include/io.h"
+#include "../include/fb.h"
+
+// UART0
+
+enum {
+    ARM_UART0_BASE	= PERIPHERAL_BASE + 0x201000,
+    ARM_UART0_DR	= ARM_UART0_BASE + 0x00,
+    ARM_UART0_FR     	= ARM_UART0_BASE + 0x18,
+    ARM_UART0_IBRD   	= ARM_UART0_BASE + 0x24,
+    ARM_UART0_FBRD   	= ARM_UART0_BASE + 0x28,
+    ARM_UART0_LCRH   	= ARM_UART0_BASE + 0x2C,
+    ARM_UART0_CR     	= ARM_UART0_BASE + 0x30,
+    ARM_UART0_IFLS   	= ARM_UART0_BASE + 0x34,
+    ARM_UART0_IMSC   	= ARM_UART0_BASE + 0x38,
+    ARM_UART0_RIS    	= ARM_UART0_BASE + 0x3C,
+    ARM_UART0_MIS    	= ARM_UART0_BASE + 0x40,
+    ARM_UART0_ICR    	= ARM_UART0_BASE + 0x44
+};
+
+unsigned char lo(unsigned int val) { return (unsigned char)(val & 0xff); }
+unsigned char hi(unsigned int val) { return (unsigned char)((val & 0xff00) >> 8); }
+
+unsigned int bt_isReadByteReady() { return (!(mmio_read(ARM_UART0_FR) & 0x10)); }
+
+unsigned char bt_readByte()
+{
+    unsigned char ch = lo(mmio_read(ARM_UART0_DR));
+    return ch;
+}
+
+unsigned char bt_waitReadByte()
+{
+    while (!bt_isReadByteReady());
+    return bt_readByte();
+}
+
+void bt_writeByte(char byte)
+{
+    while ((mmio_read(ARM_UART0_FR) & 0x20) != 0);
+    mmio_write(ARM_UART0_DR, (unsigned int)byte);
+}
+
+void bt_flushrx()
+{
+    while (bt_isReadByteReady()) bt_readByte();
+}
+
+void bt_init()
+{
+    gpio_useAsAlt3(30);
+    gpio_useAsAlt3(31);
+    gpio_useAsAlt3(32);
+    gpio_useAsAlt3(33);
+
+    bt_flushrx();
+
+    mmio_write(ARM_UART0_IMSC, 0x00);
+    mmio_write(ARM_UART0_ICR,  0x7ff);
+    mmio_write(ARM_UART0_IBRD, 0x1a);
+    mmio_write(ARM_UART0_FBRD, 0x03);
+    mmio_write(ARM_UART0_IFLS, 0x08);
+    mmio_write(ARM_UART0_LCRH, 0x70);
+    mmio_write(ARM_UART0_CR,   0xB01);
+    mmio_write(ARM_UART0_IMSC, 0x430);
+
+    wait_msec(0x100000);
+}
+
+// HOST SETUP
+
+enum {
+    OGF_HOST_CONTROL          = 0x03,
+    OGF_LE_CONTROL            = 0x08,
+    OGF_VENDOR                = 0x3f,
+
+    COMMAND_SET_BDADDR        = 0x01,
+    COMMAND_RESET_CHIP        = 0x03,
+    COMMAND_SET_BAUD          = 0x18,
+    COMMAND_LOAD_FIRMWARE     = 0x2e,
+
+    HCI_COMMAND_PKT           = 0x01,
+    HCI_ACL_PKT               = 0x02,
+    HCI_EVENT_PKT             = 0x04,
+    COMMAND_COMPLETE_CODE     = 0x0e,
+    CONNECT_COMPLETE_CODE     = 0x0f,
+
+    LL_SCAN_ACTIVE            = 0x01,
+    LL_ADV_NONCONN_IND        = 0x03
+};
+
+int hciCommandBytes(unsigned char *opcodebytes, volatile unsigned char *data, unsigned char length)
+{
+    unsigned char c=0;
+
+    bt_writeByte(HCI_COMMAND_PKT);
+    bt_writeByte(opcodebytes[0]);
+    bt_writeByte(opcodebytes[1]);
+    bt_writeByte(length);
+
+    while (c++<length) bt_writeByte(*data++);
+
+    if (bt_waitReadByte() != HCI_EVENT_PKT) return 1;
+
+    unsigned char code = bt_waitReadByte();
+    if (code == CONNECT_COMPLETE_CODE) {
+       if (bt_waitReadByte() != 4) return 2;
+
+       unsigned char err = bt_waitReadByte();
+       if (err != 0) {
+	  uart_writeText("Saw HCI COMMAND STATUS error "); uart_hex(err); uart_writeText("\n");
+	  return 12;
+       }
+
+       if (bt_waitReadByte() == 0) return 3;
+       if (bt_waitReadByte() != opcodebytes[0]) return 4;
+       if (bt_waitReadByte() != opcodebytes[1]) return 5;
+    } else if (code == COMMAND_COMPLETE_CODE) {
+       if (bt_waitReadByte() != 4) return 6;
+       if (bt_waitReadByte() == 0) return 7;
+       if (bt_waitReadByte() != opcodebytes[0]) return 8;
+       if (bt_waitReadByte() != opcodebytes[1]) return 9;
+       if (bt_waitReadByte() != 0) return 10;
+    } else return 11;
+
+    return 0;
+}
+
+int hciCommand(unsigned short ogf, unsigned short ocf, volatile unsigned char *data, unsigned char length)
+{
+    unsigned short opcode = ogf << 10 | ocf;
+    unsigned char opcodebytes[2] = { lo(opcode), hi(opcode) };
+
+    return hciCommandBytes(opcodebytes, data, length);
+}
+
+void bt_reset() {
+    volatile unsigned char empty[] = {};
+    if (hciCommand(OGF_HOST_CONTROL, COMMAND_RESET_CHIP, empty, 0)) uart_writeText("bt_reset() failed\n");
+}
+
+void bt_loadfirmware()
+{
+    volatile unsigned char empty[] = {};
+    if (hciCommand(OGF_VENDOR, COMMAND_LOAD_FIRMWARE, empty, 0)) uart_writeText("loadFirmware() failed\n");
+
+    extern unsigned char _binary_bin_BCM4345C0_hcd_start[];
+    extern unsigned char _binary_bin_BCM4345C0_hcd_size[];
+
+    unsigned int c=0;
+    unsigned int size = (long)&_binary_bin_BCM4345C0_hcd_size;
+
+    unsigned char opcodebytes[2];
+    unsigned char length;
+    unsigned char *data = &(_binary_bin_BCM4345C0_hcd_start[0]);
+
+    while (c < size) {
+        opcodebytes[0] = *data;
+        opcodebytes[1] = *(data+1);
+        length =         *(data+2);
+        data += 3;
+
+        if (hciCommandBytes(opcodebytes, data, length)) {
+	   uart_writeText("Firmware data load failed\n");
+	   break;
+	}
+
+	data += length;
+        c += 3 + length;
+    }
+
+    wait_msec(0x100000);
+}
+
+void bt_setbaud()
+{
+    volatile unsigned char command[6] = { 0, 0, 0x00, 0xc2, 0x01, 0x00 }; // little endian, 115200
+    if (hciCommand(OGF_VENDOR, COMMAND_SET_BAUD, command, 6)) uart_writeText("bt_setbaud() failed\n");
+}
+
+void bt_setbdaddr()
+{
+    volatile unsigned char command[6] = { 0xee, 0xff, 0xc0, 0xee, 0xff, 0xc0 }; // reversed
+    if (hciCommand(OGF_VENDOR, COMMAND_SET_BDADDR, command, 6)) uart_writeText("bt_setbdaddr() failed\n");
+}
+
+void bt_getbdaddr(unsigned char *bdaddr) {
+    bt_writeByte(HCI_COMMAND_PKT);
+    bt_writeByte(0x09);
+    bt_writeByte(0x10);
+    bt_writeByte(0x00);
+
+    if (bt_waitReadByte() != HCI_EVENT_PKT) return;
+    if (bt_waitReadByte() != COMMAND_COMPLETE_CODE) return;
+    if (bt_waitReadByte() != 0x0a) return;
+    if (bt_waitReadByte() != 1) return;
+    if (bt_waitReadByte() != 0x09) return;
+    if (bt_waitReadByte() != 0x10) return;
+    if (bt_waitReadByte() != 0x00) return;
+
+    for (int c=0;c<6;c++) bdaddr[c] = bt_waitReadByte();
+}
+
+void sendACLsubscribe(unsigned int handle)
+{
+    bt_writeByte(HCI_ACL_PKT);
+
+    bt_writeByte(lo(handle));
+    bt_writeByte(hi(handle));
+
+    unsigned int length = 0x0009;
+    bt_writeByte(lo(length));
+    bt_writeByte(hi(length));
+
+    unsigned int data_length = 0x0005;
+    bt_writeByte(lo(data_length));
+    bt_writeByte(hi(data_length));
+
+    unsigned int channel = 0x0004;
+    bt_writeByte(lo(channel));
+    bt_writeByte(hi(channel));
+
+    volatile unsigned char command[5] = { 0x12, 0x2b, 0x00, 0x01, 0x00 };
+
+    unsigned int c=0;
+    while (c++<data_length) bt_writeByte(command[c-1]);
+}
+
+void setLEeventmask(unsigned char mask)
+{
+    volatile unsigned char command[8] = { 0 };
+    command[0] = mask;
+
+    if (hciCommand(OGF_LE_CONTROL, 0x01, command, 8)) uart_writeText("setLEeventmask failed\n");
+}
+
+void setLEscanenable(unsigned char state, unsigned char duplicates) {
+    volatile unsigned char command[2];
+    command[0] = state;
+    command[1] = duplicates;
+    if (hciCommand(OGF_LE_CONTROL, 0x0c, command, 2)) uart_writeText("setLEscanenable failed\n");
+}
+
+void setLEscanparameters(unsigned char type, unsigned char linterval, unsigned char hinterval, unsigned char lwindow, unsigned char hwindow, unsigned char own_address_type, unsigned char filter_policy) {
+    volatile unsigned char command[7];
+    command[0] = type;
+    command[1] = linterval;
+    command[2] = hinterval;
+    command[3] = lwindow;
+    command[4] = hwindow;
+    command[5] = own_address_type;
+    command[6] = filter_policy;
+    if (hciCommand(OGF_LE_CONTROL, 0x0b, command, 7)) uart_writeText("setLEscanparameters failed\n");
+}
+
+void setLEadvertenable(unsigned char state) {
+    volatile unsigned char command[1];
+    command[0] = state;
+    if (hciCommand(OGF_LE_CONTROL, 0x0a, command, 1)) uart_writeText("setLEadvertenable failed\n");
+}
+
+void setLEadvertparameters(unsigned char type, unsigned char linterval_min, unsigned char hinterval_min, unsigned char linterval_max, unsigned char hinterval_max, unsigned char own_address_type, unsigned char filter_policy) {
+    volatile unsigned char command[15] = { 0 };
+
+    command[0] = linterval_min;
+    command[1] = hinterval_min;
+    command[2] = linterval_max;
+    command[3] = hinterval_max;
+    command[4] = type;
+    command[5] = own_address_type;
+    command[13] = 0x07;
+    command[14] = filter_policy;
+
+    if (hciCommand(OGF_LE_CONTROL, 0x06, command, 15)) uart_writeText("setLEadvertparameters failed\n");
+}
+
+void setLEadvertdata() {
+    static unsigned char command[32] = { 
+       0x19,
+       0x02, 0x01, 0x06,
+       0x03, 0x03, 0xAA, 0xFE,
+       0x11, 0x16, 0xAA, 0xFE, 0x10, 0x00, 0x03,
+       0x69, 0x73, 0x6f, 0x6d, 0x65, 0x74, 0x69, 0x6d,
+       0x2e, 0x65, 0x73,
+       0, 0, 0, 0, 0, 0
+    };
+
+    if (hciCommand(OGF_LE_CONTROL, 0x08, command, 32)) uart_writeText("setLEadvertdata failed\n");
+}
+
+void stopScanning() {
+    setLEscanenable(0, 0);
+}
+
+void stopAdvertising() {
+    setLEadvertenable(0);
+}
+
+void startActiveScanning() {
+    float BleScanInterval = 60; // every 60ms
+    float BleScanWindow = 60;
+    float BleScanDivisor = 0.625;
+
+    unsigned int p = BleScanInterval / BleScanDivisor;
+    unsigned int q = BleScanWindow / BleScanDivisor;
+
+    setLEscanparameters(LL_SCAN_ACTIVE, lo(p), hi(p), lo(q), hi(q), 0, 0);
+    setLEscanenable(1, 0);
+}
+
+void startActiveAdvertising() {
+    float advertMinFreq = 100; // every 100ms
+    float advertMaxFreq = 100; // every 100ms
+    float bleGranularity = 0.625;
+
+    unsigned int min_interval = advertMinFreq / bleGranularity;
+    unsigned int max_interval = advertMaxFreq / bleGranularity;
+
+    setLEadvertparameters(LL_ADV_NONCONN_IND, lo(min_interval), hi(min_interval), lo(max_interval), hi(max_interval), 0, 0);
+    setLEadvertdata();
+    setLEadvertenable(1);
+}
+
+void connect(unsigned char *addr)
+{
+    float BleScanInterval = 60; // every 60ms
+    float BleScanWindow = 60;
+    float BleScanDivisor = 0.625;
+
+    float connMinFreq = 30; // every 30ms
+    float connMaxFreq = 50; // every 50ms
+    float BleGranularity = 1.25;
+
+    unsigned int p = BleScanInterval / BleScanDivisor;
+    unsigned int q = BleScanWindow / BleScanDivisor;
+
+    unsigned int min_interval = connMinFreq / BleGranularity;
+    unsigned int max_interval = connMaxFreq / BleGranularity;
+
+    volatile unsigned char command[25] = { 0 };
+
+    command[0] = lo(p);
+    command[2] = lo(q);
+    command[6] = *(addr+5);
+    command[7] = *(addr+4);
+    command[8] = *(addr+3);
+    command[9] = *(addr+2);
+    command[10] = *(addr+1);
+    command[11] = *addr;
+    command[13] = lo(min_interval);
+    command[15] = lo(max_interval);
+    command[19] = 0x2a;
+    command[20] = 0x00;
+
+    if (hciCommand(OGF_LE_CONTROL, 0x0d, command, 25)) uart_writeText("createLEconnection failed\n");
+}

part11-breakout-smp/lib/fb.c

@@ -0,0 +1,261 @@
+#include "../include/io.h"
+#include "../include/mb.h"
+#include "../include/terminal.h"
+
+#define DEBUG 0
+
+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
+    mbox[1] = MBOX_REQUEST;
+
+    mbox[2] = MBOX_TAG_SETPHYWH; // Tag identifier
+    mbox[3] = 8; // Value size in bytes
+    mbox[4] = 8; // Value size in bytes (again!)
+    mbox[5] = 1920; // Value(width)
+    mbox[6] = 1080; // Value(height)
+
+    mbox[7] = MBOX_TAG_SETVIRTWH;
+    mbox[8] = 8;
+    mbox[9] = 8;
+    mbox[10] = 1920;
+    mbox[11] = 1080;
+
+    mbox[12] = MBOX_TAG_SETVIRTOFF;
+    mbox[13] = 8;
+    mbox[14] = 8;
+    mbox[15] = 0; // Value(x)
+    mbox[16] = 0; // Value(y)
+
+    mbox[17] = MBOX_TAG_SETDEPTH;
+    mbox[18] = 4;
+    mbox[19] = 4;
+    mbox[20] = 32; // Bits per pixel
+
+    mbox[21] = MBOX_TAG_SETPXLORDR;
+    mbox[22] = 4;
+    mbox[23] = 4;
+    mbox[24] = 1; // RGB
+
+    mbox[25] = MBOX_TAG_GETFB;
+    mbox[26] = 8;
+    mbox[27] = 8;
+    mbox[28] = 4096; // FrameBufferInfo.pointer
+    mbox[29] = 0;    // FrameBufferInfo.size
+
+    mbox[30] = MBOX_TAG_GETPITCH;
+    mbox[31] = 4;
+    mbox[32] = 4;
+    mbox[33] = 0; // Bytes per line
+
+    mbox[34] = MBOX_TAG_LAST;
+
+    // Check call is successful and we have a pointer with depth 32
+    if (mbox_call(MBOX_CH_PROP) && mbox[20] == 32 && mbox[28] != 0) {
+        mbox[28] &= 0x3FFFFFFF; // Convert GPU address to ARM address
+        width = mbox[10];       // Actual physical width
+        height = mbox[11];      // Actual physical height
+        pitch = mbox[33];       // Number of bytes per line
+        isrgb = mbox[24];       // Pixel order
+        fb = (unsigned char *)((long)mbox[28]);
+    }
+}
+
+void drawPixel(int x, int y, unsigned char attr)
+{
+    int offs = (y * pitch) + (x * 4);
+    *((unsigned int*)(fb + offs)) = vgapal[attr & 0x0f];
+}
+
+void drawRect(int x1, int y1, int x2, int y2, unsigned char attr, int fill)
+{
+    int y=y1;
+
+    while (y <= y2) {
+       int x=x1;
+       while (x <= x2) {
+	  if ((x == x1 || x == x2) || (y == y1 || y == y2)) drawPixel(x, y, attr);
+	  else if (fill) drawPixel(x, y, (attr & 0xf0) >> 4);
+          x++;
+       }
+       y++;
+    }
+}
+
+void drawLine(int x1, int y1, int x2, int y2, unsigned char attr)  
+{  
+    int dx, dy, p, x, y;
+
+    dx = x2-x1;
+    dy = y2-y1;
+    x = x1;
+    y = y1;
+    p = 2*dy-dx;
+
+    while (x<x2) {
+       if (p >= 0) {
+          drawPixel(x,y,attr);
+          y++;
+          p = p+2*dy-2*dx;
+       } else {
+          drawPixel(x,y,attr);
+          p = p+2*dy;
+       }
+       x++;
+    }
+}
+
+void drawCircle(int x0, int y0, int radius, unsigned char attr, int fill)
+{
+    int x = radius;
+    int y = 0;
+    int err = 0;
+ 
+    while (x >= y) {
+	if (fill) {
+	   drawLine(x0 - y, y0 + x, x0 + y, y0 + x, (attr & 0xf0) >> 4);
+	   drawLine(x0 - x, y0 + y, x0 + x, y0 + y, (attr & 0xf0) >> 4);
+	   drawLine(x0 - x, y0 - y, x0 + x, y0 - y, (attr & 0xf0) >> 4);
+	   drawLine(x0 - y, y0 - x, x0 + y, y0 - x, (attr & 0xf0) >> 4);
+	}
+	drawPixel(x0 - y, y0 + x, attr);
+	drawPixel(x0 + y, y0 + x, attr);
+	drawPixel(x0 - x, y0 + y, attr);
+        drawPixel(x0 + x, y0 + y, attr);
+	drawPixel(x0 - x, y0 - y, attr);
+	drawPixel(x0 + x, y0 - y, attr);
+	drawPixel(x0 - y, y0 - x, attr);
+	drawPixel(x0 + y, y0 - x, attr);
+
+	if (err <= 0) {
+	    y += 1;
+	    err += 2*y + 1;
+	}
+ 
+	if (err > 0) {
+	    x -= 1;
+	    err -= 2*x + 1;
+	}
+    }
+}
+
+void drawChar(unsigned char ch, int x, int y, unsigned char attr, int zoom)
+{
+    unsigned char *glyph = (unsigned char *)&font + (ch < FONT_NUMGLYPHS ? ch : 0) * FONT_BPG;
+
+    for (int i=1;i<=(FONT_HEIGHT*zoom);i++) {
+	for (int j=0;j<(FONT_WIDTH*zoom);j++) {
+	    unsigned char mask = 1 << (j/zoom);
+	    unsigned char col = (*glyph & mask) ? attr & 0x0f : (attr & 0xf0) >> 4;
+
+	    drawPixel(x+j, y+i, col);
+	}
+	glyph += (i%zoom) ? 0 : FONT_BPL;
+    }
+}
+
+void drawString(int x, int y, char *s, unsigned char attr, int zoom)
+{
+    while (*s) {
+       if (*s == '\r') {
+          x = 0;
+       } else if(*s == '\n') {
+          x = 0; y += (FONT_HEIGHT*zoom);
+       } else {
+	  drawChar(*s, x, y, attr, zoom);
+          x += (FONT_WIDTH*zoom);
+       }
+       s++;
+    }
+}
+
+void moveRectAbs(int oldx, int oldy, int width, int height, int newx, int newy, unsigned char attr)
+{
+    unsigned int xcount = 0, ycount = 0;
+    unsigned int bitmap[width][height]; // This is very unsafe if it's too big for the stack...
+    unsigned int offs;
+
+    // Save the bitmap
+    while (xcount < width) {
+       while (ycount < height) {
+          offs = ((oldy + ycount) * pitch) + ((oldx + xcount) * 4);
+
+	  bitmap[xcount][ycount] = *((unsigned int*)(fb + offs));
+	  ycount++;
+       }
+       ycount=0;
+       xcount++;
+    }
+    // Wipe it out with background colour
+    drawRect(oldx, oldy, oldx + width, oldy + width, attr, 1);
+    // Draw it again
+    for (int i=newx;i<newx + width;i++) {
+        for (int j=newy;j<newy + height;j++) {
+            offs = (j * pitch) + (i * 4);
+	    *((unsigned int*)(fb + offs)) = bitmap[i-newx][j-newy];
+        }
+    }
+}
+
+void wait_msec(unsigned int n)
+{
+    register unsigned long f, t, r;
+
+    // Get the current counter frequency
+    asm volatile ("mrs %0, cntfrq_el0" : "=r"(f));
+    // Read the current counter
+    asm volatile ("mrs %0, cntpct_el0" : "=r"(t));
+    // Calculate expire value for counter
+    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;
+    }
+    if (DEBUG == 1) 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(" ");
+}

part11-breakout-smp/lib/io.c

@@ -0,0 +1,199 @@
+#include "../include/io.h"
+
+// GPIO
+
+enum {
+    GPFSEL0         = PERIPHERAL_BASE + 0x200000,
+    GPSET0          = PERIPHERAL_BASE + 0x20001C,
+    GPCLR0          = PERIPHERAL_BASE + 0x200028,
+    GPPUPPDN0       = PERIPHERAL_BASE + 0x2000E4
+};
+
+enum {
+    GPIO_MAX_PIN       = 53,
+    GPIO_FUNCTION_OUT  = 1,
+    GPIO_FUNCTION_ALT5 = 2,
+    GPIO_FUNCTION_ALT3 = 7,
+    GPIO_FUNCTION_ALT0 = 4
+};
+
+enum {
+    Pull_None = 0,
+    Pull_Down = 1, // Are down and up the right way around?
+    Pull_Up = 2
+};
+
+void mmio_write(long reg, unsigned int val) { *(volatile unsigned int *)reg = val; }
+unsigned int mmio_read(long reg) { return *(volatile unsigned int *)reg; }
+
+unsigned int gpio_call(unsigned int pin_number, unsigned int value, unsigned int base, unsigned int field_size, unsigned int field_max) {
+    unsigned int field_mask = (1 << field_size) - 1;
+  
+    if (pin_number > field_max) return 0;
+    if (value > field_mask) return 0; 
+
+    unsigned int num_fields = 32 / field_size;
+    unsigned int reg = base + ((pin_number / num_fields) * 4);
+    unsigned int shift = (pin_number % num_fields) * field_size;
+
+    unsigned int curval = mmio_read(reg);
+    curval &= ~(field_mask << shift);
+    curval |= value << shift;
+    mmio_write(reg, curval);
+
+    return 1;
+}
+
+unsigned int gpio_set     (unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPSET0, 1, GPIO_MAX_PIN); }
+unsigned int gpio_clear   (unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPCLR0, 1, GPIO_MAX_PIN); }
+unsigned int gpio_pull    (unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPPUPPDN0, 2, GPIO_MAX_PIN); }
+unsigned int gpio_function(unsigned int pin_number, unsigned int value) { return gpio_call(pin_number, value, GPFSEL0, 3, GPIO_MAX_PIN); }
+
+void gpio_useAsAlt0(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_ALT0);
+}
+
+void gpio_useAsAlt3(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_ALT3);
+}
+
+void gpio_useAsAlt5(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_ALT5);
+}
+
+void gpio_initOutputPinWithPullNone(unsigned int pin_number) {
+    gpio_pull(pin_number, Pull_None);
+    gpio_function(pin_number, GPIO_FUNCTION_OUT);
+}
+
+void gpio_setPinOutputBool(unsigned int pin_number, unsigned int onOrOff) {
+    if (onOrOff) {
+        gpio_set(pin_number, 1);
+    } else {
+        gpio_clear(pin_number, 1);
+    }
+}
+
+// UART
+
+enum {
+    AUX_BASE        = PERIPHERAL_BASE + 0x215000,
+    AUX_IRQ         = AUX_BASE,
+    AUX_ENABLES     = AUX_BASE + 4,
+    AUX_MU_IO_REG   = AUX_BASE + 64,
+    AUX_MU_IER_REG  = AUX_BASE + 68,
+    AUX_MU_IIR_REG  = AUX_BASE + 72,
+    AUX_MU_LCR_REG  = AUX_BASE + 76,
+    AUX_MU_MCR_REG  = AUX_BASE + 80,
+    AUX_MU_LSR_REG  = AUX_BASE + 84,
+    AUX_MU_MSR_REG  = AUX_BASE + 88,
+    AUX_MU_SCRATCH  = AUX_BASE + 92,
+    AUX_MU_CNTL_REG = AUX_BASE + 96,
+    AUX_MU_STAT_REG = AUX_BASE + 100,
+    AUX_MU_BAUD_REG = AUX_BASE + 104,
+    AUX_UART_CLOCK  = 500000000,
+    UART_MAX_QUEUE  = 16 * 1024
+};
+
+#define AUX_MU_BAUD(baud) ((AUX_UART_CLOCK/(baud*8))-1)
+
+unsigned char uart_output_queue[UART_MAX_QUEUE];
+unsigned int uart_output_queue_write = 0;
+unsigned int uart_output_queue_read = 0;
+
+void uart_init() {
+    mmio_write(AUX_ENABLES, 1); //enable UART1
+    mmio_write(AUX_MU_IER_REG, 0);
+    mmio_write(AUX_MU_CNTL_REG, 0);
+    mmio_write(AUX_MU_LCR_REG, 3); //8 bits
+    mmio_write(AUX_MU_MCR_REG, 0);
+    mmio_write(AUX_MU_IER_REG, 0);
+    mmio_write(AUX_MU_IIR_REG, 0xC6); //disable interrupts
+    mmio_write(AUX_MU_BAUD_REG, AUX_MU_BAUD(115200));
+    gpio_useAsAlt5(14);
+    gpio_useAsAlt5(15);
+    mmio_write(AUX_MU_CNTL_REG, 3); //enable RX/TX
+}
+
+unsigned int uart_isOutputQueueEmpty() {
+    return uart_output_queue_read == uart_output_queue_write;
+}
+
+unsigned int uart_isReadByteReady()  { return mmio_read(AUX_MU_LSR_REG) & 0x01; }
+unsigned int uart_isWriteByteReady() { return mmio_read(AUX_MU_LSR_REG) & 0x20; }
+
+unsigned char uart_readByte() {
+    while (!uart_isReadByteReady());
+    return (unsigned char)mmio_read(AUX_MU_IO_REG);
+}
+
+void uart_writeByteBlockingActual(unsigned char ch) {
+    while (!uart_isWriteByteReady()); 
+    mmio_write(AUX_MU_IO_REG, (unsigned int)ch);
+}
+
+void uart_loadOutputFifo() {
+    while (!uart_isOutputQueueEmpty() && uart_isWriteByteReady()) {
+        uart_writeByteBlockingActual(uart_output_queue[uart_output_queue_read]);
+        uart_output_queue_read = (uart_output_queue_read + 1) & (UART_MAX_QUEUE - 1); // Don't overrun
+    }
+}
+
+void uart_writeByteBlocking(unsigned char ch) {
+    unsigned int next = (uart_output_queue_write + 1) & (UART_MAX_QUEUE - 1); // Don't overrun
+
+    while (next == uart_output_queue_read) uart_loadOutputFifo();
+
+    uart_output_queue[uart_output_queue_write] = ch;
+    uart_output_queue_write = next;
+}
+
+void uart_writeText(char *buffer) {
+    while (*buffer) {
+       if (*buffer == '\n') uart_writeByteBlockingActual('\r');
+       uart_writeByteBlockingActual(*buffer++);
+    }
+}
+
+void uart_drainOutputQueue() {
+    while (!uart_isOutputQueueEmpty()) uart_loadOutputFifo();
+}
+
+void uart_update() {
+    uart_loadOutputFifo();
+
+    if (uart_isReadByteReady()) {
+       unsigned char ch = uart_readByte();
+       if (ch == '\r') uart_writeText("\n"); else uart_writeByteBlocking(ch);
+    }
+}
+
+void uart_hex(unsigned int d) {
+    unsigned int n;
+    int c;
+    for(c=28;c>=0;c-=4) {
+        // get highest tetrad
+        n=(d>>c)&0xF;
+        // 0-9 => '0'-'9', 10-15 => 'A'-'F'
+        n+=n>9?0x37:0x30;
+
+        uart_writeByteBlockingActual(n);
+    }
+}
+
+void uart_byte(unsigned char b) {
+    unsigned int n;
+    int c;
+    for(c=4;c>=0;c-=4) {
+        // get highest tetrad
+        n=(b>>c)&0xF;
+        // 0-9 => '0'-'9', 10-15 => 'A'-'F'
+        n+=n>9?0x37:0x30;
+
+        uart_writeByteBlockingActual(n);
+    }
+    uart_writeByteBlockingActual(' ');
+}

part11-breakout-smp/lib/mb.c

@@ -0,0 +1,39 @@
+#include "../include/io.h"
+
+// The buffer must be 16-byte aligned as only the upper 28 bits of the address can be passed via the mailbox
+volatile unsigned int __attribute__((aligned(16))) mbox[36];
+
+enum {
+    VIDEOCORE_MBOX = (PERIPHERAL_BASE + 0x0000B880),
+    MBOX_READ      = (VIDEOCORE_MBOX + 0x0),
+    MBOX_POLL      = (VIDEOCORE_MBOX + 0x10),
+    MBOX_SENDER    = (VIDEOCORE_MBOX + 0x14),
+    MBOX_STATUS    = (VIDEOCORE_MBOX + 0x18),
+    MBOX_CONFIG    = (VIDEOCORE_MBOX + 0x1C),
+    MBOX_WRITE     = (VIDEOCORE_MBOX + 0x20),
+    MBOX_RESPONSE  = 0x80000000,
+    MBOX_FULL      = 0x80000000,
+    MBOX_EMPTY     = 0x40000000
+};
+
+unsigned int mbox_call(unsigned char ch)
+{
+    // 28-bit address (MSB) and 4-bit value (LSB)
+    unsigned int r = ((unsigned int)((long) &mbox) &~ 0xF) | (ch & 0xF);
+
+    // Wait until we can write
+    while (mmio_read(MBOX_STATUS) & MBOX_FULL);
+    
+    // Write the address of our buffer to the mailbox with the channel appended
+    mmio_write(MBOX_WRITE, r);
+
+    while (1) {
+        // Is there a reply?
+        while (mmio_read(MBOX_STATUS) & MBOX_EMPTY);
+
+        // Is it a reply to our message?
+        if (r == mmio_read(MBOX_READ)) return mbox[1]==MBOX_RESPONSE; // Is it successful?
+           
+    }
+    return 0;
+}

part11-breakout-smp/lib/multicore.c

@@ -0,0 +1,44 @@
+#include "../include/multicore.h"
+
+void store32(unsigned long address, unsigned long value)
+{
+    *(unsigned long *) address = value;
+}
+
+unsigned long load32(unsigned long address)
+{
+    return *(unsigned long *) address;
+}
+
+void start_core1(void (*func)(void))
+{
+    store32((unsigned long)&spin_cpu1, (unsigned long)func);
+    asm volatile ("sev");
+}
+
+void start_core2(void (*func)(void))
+{
+    store32((unsigned long)&spin_cpu2, (unsigned long)func);
+    asm volatile ("sev");
+}
+
+void start_core3(void (*func)(void))
+{
+    store32((unsigned long)&spin_cpu3, (unsigned long)func);
+    asm volatile ("sev");
+}
+
+void clear_core1(void) 
+{
+    store32((unsigned long)&spin_cpu1, 0);
+}
+
+void clear_core2(void) 
+{
+    store32((unsigned long)&spin_cpu2, 0);
+}
+
+void clear_core3(void) 
+{
+    store32((unsigned long)&spin_cpu3, 0);
+}

part8-breakout-ble/kernel.c

@@ -407,7 +407,9 @@ void breakout()
     else drawString((WIDTH/2)-(strwidth/2), (HEIGHT/2)-(strheight/2), "Game over!", 0x04, zoom);
 
     wait_msec(0x500000); // Wait 5 seconds
+
     drawRect((WIDTH/2)-(strwidth/2), (HEIGHT/2)-(strheight/2), (WIDTH/2)+(strwidth/2), (HEIGHT/2)+(strheight/2), 0, 1);
+    numobjs = 0;
 }
 
 void main()