diff --git a/docs/PCA9546.pdf b/docs/PCA9546-01.pdf
similarity index 100%
rename from docs/PCA9546.pdf
rename to docs/PCA9546-01.pdf
diff --git a/docs/PCA9546-02.pdf b/docs/PCA9546-02.pdf
new file mode 100755
index 0000000..0c3f40e
Binary files /dev/null and b/docs/PCA9546-02.pdf differ
diff --git a/src/bus/twi.c b/src/bus/twi.c
index 3ddee18..402c799 100644
--- a/src/bus/twi.c
+++ b/src/bus/twi.c
@@ -1,12 +1,104 @@
+#include "common.h"
 #include "bus/twi.h"
+
 #include <avr/io.h>
+#include <assert.h>
+
+#define  TW_START        0x08
+#define  TW_REP_START    0x10
+#define  TW_MT_SLA_ACK   0x18
+#define  TW_MT_SLA_NACK  0x20
+#define  TW_MT_DATA_ACK  0x28
+#define  TW_MR_SLA_ACK   0x40
+#define  TW_MR_SLA_NACK  0x48
+#define  TW_MR_DATA_ACK  0x50
 
 int TWI_Init(void)
 {
 	// Set SCL bit rate to 400kHz
 
-	TWSR = 0x00; // TWI status register
-	TWBR = 0x0C; // TWI bit rate register
+	TWSR = 0x00;  // TWI status register
+	TWBR = 0x0C;  // TWI bit rate register
+
+	return 0;
+}
+
+int TWI_Start(char addr, char mode)
+{
+	unsigned int status;
+
+	assert(mode == 0 || mode == 1);
+
+	// Send start condition
+
+	TWCR = (1 << TWEN)    // Enable TWI
+	     | (1 << TWINT)   // Clear interrupt flag
+	     | (1 << TWSTA);  // Send start condition
+
+	// Wait until start condition sent
+
+	while ((TWCR & (1 << TWINT)) == 0);
+	if ((TWSR & 0xf8) != TW_START)
+		return -1;
+
+	// Send slave address
+
+	addr = addr << 1;     // Lowest bit for mode
+	TWDR = addr + mode;   // Mode 0=R, 1=W
+	TWCR = (1 << TWEN)    // Enable TWI
+	     | (1 << TWINT);  // Clear interrupt flag
+
+	// Wait until slave address sent
+
+	while ((TWCR & (1 << TWINT)) == 0);
+	status = TWSR & 0xF8;
+
+	if ((mode == 0 && status != TW_MT_SLA_ACK) ||
+	    (mode == 1 && status != TW_MR_SLA_ACK))
+		return -2;
+
+	return 0;
+}
+
+int TWI_Write(char data)
+{
+	TWDR = data;
+	TWCR = (1 << TWEN)    // Enable TWI
+	     | (1 << TWINT);  // Clear interrupt flag
+
+	while ((TWCR & (1 << TWINT)) == 0);
+
+	if ((TWSR & 0xF8) != TW_MT_DATA_ACK)
+		return -1;
+
+	return 0;
+}
+
+int TWI_Wait_ACK(void)
+{
+	unsigned int status;
+
+	while ((TWCR & (1 << TWINT)) == 0);
+	status = TWSR & 0xF8;
+
+	if (status != TW_MT_DATA_ACK &&
+	    status != TW_MR_DATA_ACK)
+		return -1;
+
+	return 0;
+}
+
+int TWI_Stop(void)
+{
+	// Send stop condition
+
+	TWCR = (1 << TWEN)    // Enable TWI
+	     | (1 << TWINT)   // Clear interrupt flag
+	     | (1 << TWSTO);  // Send stop condition
+
+	// Wait until stop condition sent
+
+	while (TWCR & (1 << TWSTO));
 
 	return 0;
 }
diff --git a/src/bus/twi.h b/src/bus/twi.h
index f41bc18..c675a26 100644
--- a/src/bus/twi.h
+++ b/src/bus/twi.h
@@ -2,5 +2,9 @@
 #define MAD_CORE_BUS_TWI_H
 
 int TWI_Init(void);
+int TWI_Start(char addr, char mode);
+int TWI_Write(char data);
+int TWI_Wait_ACK(void);
+int TWI_Stop(void);
 
 #endif // MAD_CORE_BUS_TWI_H
diff --git a/src/bus/usart.c b/src/bus/usart.c
index a5a88c0..de92af8 100644
--- a/src/bus/usart.c
+++ b/src/bus/usart.c
@@ -1,5 +1,6 @@
 #include "common.h"
 #include "bus/usart.h"
+
 #include <avr/io.h>
 #include <avr/interrupt.h>
 //#include <util/atomic.h>
diff --git a/src/common.h b/src/common.h
index c190a6a..3378357 100644
--- a/src/common.h
+++ b/src/common.h
@@ -9,4 +9,7 @@
 void Info(const char *fmt, ...);
 void Error(const char *fmt, ...);
 
+#include <util/delay.h>
+#define Sleep(ms) _delay_ms(ms)
+
 #endif // MAD_CORE_COMMON_H
diff --git a/src/main.c b/src/main.c
index 7c1f3e4..c4570d0 100644
--- a/src/main.c
+++ b/src/main.c
@@ -3,26 +3,67 @@
 #include "bus/twi.h"
 
 #include <avr/interrupt.h>
-#include <util/delay.h>
-#define Sleep(ms) _delay_ms(ms)
 
-int main(int argc, char **argv)
+int main(void)
 {
-	USART_Init();
-	TWI_Init();
-
-	sei();
-
-	Info("Initializing...");
-	Sleep(500);
+	char crb;
 
 	// ADS1115 (ADC)
 	// PCA9546 (Multiplexer TWI)
 	// ATH20 (3x)
 	// BMP280 (3x)
 
-	UNUSED(argc);
-	UNUSED(argv);
+	USART_Init();
+	TWI_Init();
+
+	sei();
+
+	Info("Initializing...");
+
+	// PCA9546 I2C Multiplexer
+	// =======================
+
+	TWI_Stop();
+
+	// Excerpts taken from the PCA9546A datasheet:
+	// https://www.ti.com/lit/ds/symlink/pca9546a.pdf
+
+	// Following a start condition, the bus master
+	// must output the address of the slave it is
+	// accessing.
+
+	//  7  6  5  4   |   3   2   1   0
+	//  1  1  1  0   |  A2  A1  A0  RW
+	//  FIXED_____   |  SELECTABLE____
+
+	TWI_Start(0x70, 0);
+	TWI_Wait_ACK();
+
+	// Following the successful acknowledgment of
+	// the slave address, the bus master sends a byte
+	// which is stored in the control register.
+
+	//  7  6  5  4   |   3   2   1   0
+	//  X  X  X  X   |  B3  B2  B1  B0
+	//               |  CHANNEL_______
+
+	crb = 0x01; //  Channel 0  |  00000001
+	//  = 0x02  //  Channel 1  |  00000010
+	//  = 0x04  //  Channel 2  |  00000100
+	//  = 0x08  //  Channel 3  |  00001000
+
+	TWI_Write(crb);
+
+	// When a channel is selected, the channel
+	// becomes active after a stop condition has been
+	// placed on the I2C bus. A stop condition always
+	// must occur right after the acknowledge cycle.
+
+	TWI_Wait_ACK();
+	TWI_Stop();
+
+	Info("Switched to TWI channel 0.");
+	Sleep(500);
 
 	return 0;
 }