diff --git a/Makefile b/Makefile
index 921debb..b0d99f0 100644
--- a/Makefile
+++ b/Makefile
@@ -81,6 +81,10 @@
 DIRS+=diags
 endif
 
+ifeq ($(BR2_TARGET_GENERIC_PLATFORM_NAME),gflt110)
+DIRS+=diags
+endif
+
 ifeq ($(BUILD_CONMAN),y)
 DIRS+=conman
 endif
diff --git a/diags/Makefile b/diags/Makefile
index 76855ce..399c44a 100644
--- a/diags/Makefile
+++ b/diags/Makefile
@@ -7,6 +7,9 @@
 ifeq ($(BR2_TARGET_GENERIC_PLATFORM_NAME),gfsc100)
  DIRS += spacecast
 endif
+ifeq ($(BR2_TARGET_GENERIC_PLATFORM_NAME),gflt110)
+ DIRS += chameleon
+endif
 
 PYTHON?=python
 
diff --git a/diags/chameleon/Makefile b/diags/chameleon/Makefile
new file mode 100644
index 0000000..e6af150
--- /dev/null
+++ b/diags/chameleon/Makefile
@@ -0,0 +1,42 @@
+default:	all
+
+BINARY = diags
+
+TARGETS=$(BINARY)
+INSTALL=install
+PREFIX=$(DESTDIR)/usr
+BINDIR=$(PREFIX)/sbin
+LIBDIR=$(PREFIX)/lib
+INCLUDEDIR=$(PREFIX)/include
+
+CC=$(CROSS_COMPILE)gcc
+CXX=$(CROSS_COMPILE)g++
+RM=rm -f
+
+CFLAGS = -Wall -Werror -Wimplicit -Wno-unknown-pragmas -D_GNU_SOURC -g
+CFLAGS += $(EXTRA_CFLAGS)
+LDFLAGS += $(EXTRA_LDFLAGS)
+
+IFLAGS += $(patsubst %,-I%,$(INC_DIRS))
+CFILES = $(wildcard ../common/*.c *.c)
+OFILES = $(patsubst %.c,%.o,$(CFILES))
+
+all:	$(TARGETS)
+
+install:
+	$(INSTALL) -m 0755 $(BINARY) $(BINDIR)/
+
+install-libs:
+	@:
+
+test:
+	@:
+
+$(BINARY):	$(OFILES)
+	$(CC) $^ $(LDFLAGS) -o $@
+
+%.o:	%.c
+	$(CC) $(CFLAGS) $(IFLAGS) -c $^ -c
+
+clean:
+	$(RM) $(OFILES) $(BINARY)
diff --git a/diags/chameleon/common.h b/diags/chameleon/common.h
new file mode 100644
index 0000000..f790704
--- /dev/null
+++ b/diags/chameleon/common.h
@@ -0,0 +1,14 @@
+/*
+ * (C) Copyright 2015 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#ifndef VENDOR_GOOGLE_DIAGS_LOCKDOWN_COMMON_H_
+#define VENDOR_GOOGLE_DIAGS_LOCKDOWN_COMMON_H_
+
+#define MAX_PKT_SIZE 4096
+#define FAIL_TEXT "FAILED:"
+#define PASS_TEXT "PASSED:"
+
+#endif  // VENDOR_GOOGLE_DIAGS_LOCKDOWN_COMMON_H_
diff --git a/diags/chameleon/datapath.c b/diags/chameleon/datapath.c
new file mode 100644
index 0000000..d5da3c4
--- /dev/null
+++ b/diags/chameleon/datapath.c
@@ -0,0 +1,248 @@
+/*
+ * (C) Copyright 2015 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include <errno.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include "../common/io.h"
+#include "../common/util.h"
+
+#define AVANTA_BASE_ADDR 0xF1000000
+#define GPON_RECV_STATUS_FEC0 (AVANTA_BASE_ADDR + 0x000AC80C)
+#define GPON_RECV_STATUS_FEC1 (AVANTA_BASE_ADDR + 0x000AC810)
+#define GPON_RECV_STATUS_FEC2 (AVANTA_BASE_ADDR + 0x000AC814)
+#define GPON_RECV_STATUS_SUPER_FRAME_CNT (AVANTA_BASE_ADDR + 0x000AC818)
+#define PON_PHY_TEST_PRBS_COUNTER_0 (AVANTA_BASE_ADDR + 0x000A2E70)
+#define PON_PHY_TEST_PRBS_COUNTER_1 (AVANTA_BASE_ADDR + 0x000A2E74)
+#define PON_PHY_TEST_PRBS_COUNTER_2 (AVANTA_BASE_ADDR + 0x000A2E78)
+#define PON_PHY_TEST_PRBS_ERROR_COUNTER_0 (AVANTA_BASE_ADDR + 0x000A2E7C)
+#define PON_PHY_TEST_PRBS_ERROR_COUNTER_1 (AVANTA_BASE_ADDR + 0x000A2E80)
+#define PON_PHY_CTRL0 (AVANTA_BASE_ADDR + 0x000184F4)
+#define PON_PHY_RESET_BIT 0x8
+
+static int set_pon_phy_out_of_reset() {
+  unsigned int phy_ctrl0;
+
+  if (read_physical_addr(PON_PHY_CTRL0, &phy_ctrl0) != 0) {
+    printf("Read address 0x%x failed\n", PON_PHY_CTRL0);
+    return -1;
+  }
+  if (phy_ctrl0 & PON_PHY_RESET_BIT) {
+    phy_ctrl0 &= ~PON_PHY_RESET_BIT;
+    if (write_physical_addr(PON_PHY_CTRL0, phy_ctrl0) != 0) {
+      printf("Write address 0x%x value 0x%x failed\n", PON_PHY_CTRL0,
+             phy_ctrl0);
+      return -1;
+    }
+  }
+  return 0;
+}
+
+static void soc_reg_read_usage(void) {
+  printf("soc_reg_read <addr>\n");
+  printf("read Marvell 88F6601 registers\n");
+  printf("Example:\n");
+  printf("soc_reg_read 0x00018810\n");
+}
+
+int soc_reg_read(int argc, char *argv[]) {
+  unsigned int reg_addr, addr, value;
+
+  if (argc != 2) {
+    soc_reg_read_usage();
+    return -1;
+  }
+
+  reg_addr = strtoul(argv[1], NULL, 16);
+  addr = reg_addr + AVANTA_BASE_ADDR;
+  if (read_physical_addr(addr, &value) != 0) {
+    printf("Read address 0x%x failed\n", addr);
+    return -1;
+  }
+  printf("0x%x = 0x%x\n", reg_addr, value);
+
+  return 0;
+}
+
+static void soc_reg_write_usage(void) {
+  printf("soc_reg_write <addr> <data>\n");
+  printf("write Marvell 88F6601 registers\n");
+  printf("Example:\n");
+  printf("soc_reg_write 0x0007241C 0x0\n");
+}
+
+int soc_reg_write(int argc, char *argv[]) {
+  unsigned int reg_addr, addr, value;
+
+  if (argc != 3) {
+    soc_reg_write_usage();
+    return -1;
+  }
+
+  reg_addr = strtoul(argv[1], NULL, 16);
+  value = strtoul(argv[2], NULL, 16);
+  addr = reg_addr + AVANTA_BASE_ADDR;
+  if (write_physical_addr(addr, value) != 0) {
+    printf("Write address 0x%x value 0x%x failed\n", addr, value);
+    return -1;
+  }
+  printf("0x%x set to 0x%x\n", reg_addr, value);
+
+  return 0;
+}
+
+static void gpon_rx_status_usage(void) {
+  printf("gpon_rx_status\n");
+  printf("read Marvell 88F6601 GPON RX status registers\n");
+  printf("Example:\n");
+  printf("gpon_rx_status\n");
+}
+
+int gpon_rx_status(int argc, char *argv[]) {
+  unsigned int fec0, fec1, fec2, frame_cnt;
+
+  if (argc != 1 || argv == NULL) {
+    gpon_rx_status_usage();
+    return -1;
+  }
+
+  if (read_physical_addr(GPON_RECV_STATUS_FEC0, &fec0) != 0) {
+    printf("Read address 0x%x failed\n", GPON_RECV_STATUS_FEC0);
+    return -1;
+  }
+  if (read_physical_addr(GPON_RECV_STATUS_FEC1, &fec1) != 0) {
+    printf("Read address 0x%x failed\n", GPON_RECV_STATUS_FEC1);
+    return -1;
+  }
+  if (read_physical_addr(GPON_RECV_STATUS_FEC2, &fec2) != 0) {
+    printf("Read address 0x%x failed\n", GPON_RECV_STATUS_FEC2);
+    return -1;
+  }
+  if (read_physical_addr(GPON_RECV_STATUS_SUPER_FRAME_CNT, &frame_cnt) != 0) {
+    printf("Read address 0x%x failed\n", GPON_RECV_STATUS_SUPER_FRAME_CNT);
+    return -1;
+  }
+  printf(
+      "Bytes Received: 0x%x COR: 0x%x RX words Received: 0x%x Frame CNT: "
+      "0x%x\n",
+      fec0, fec1, fec2, frame_cnt);
+
+  return 0;
+}
+
+static void rx_prbs_cnt_usage(void) {
+  printf("rx_prbs_cnt\n");
+  printf("read Marvell 88F6601 RX PRBS coutner registers\n");
+  printf("Example:\n");
+  printf("rx_prbs_cnt\n");
+}
+
+int rx_prbs_cnt(int argc, char *argv[]) {
+  unsigned int cnt0, cnt1, cnt2;
+
+  if (argc != 1 || argv == NULL) {
+    rx_prbs_cnt_usage();
+    return -1;
+  }
+
+  if (set_pon_phy_out_of_reset() != 0) {
+    printf("Failed to take PHY out of reset\n");
+    return -1;
+  }
+  if (read_physical_addr(PON_PHY_TEST_PRBS_COUNTER_0, &cnt0) != 0) {
+    printf("Read address 0x%x failed\n", PON_PHY_TEST_PRBS_COUNTER_0);
+    return -1;
+  }
+  if (read_physical_addr(PON_PHY_TEST_PRBS_COUNTER_1, &cnt1) != 0) {
+    printf("Read address 0x%x failed\n", PON_PHY_TEST_PRBS_COUNTER_0);
+    return -1;
+  }
+  if (read_physical_addr(PON_PHY_TEST_PRBS_COUNTER_2, &cnt2) != 0) {
+    printf("Read address 0x%x failed\n", PON_PHY_TEST_PRBS_COUNTER_0);
+    return -1;
+  }
+  printf("RX PRBS count: 0x%x%04x%04x\n", cnt0, cnt1, cnt2);
+
+  return 0;
+}
+
+static void rx_prbs_err_cnt_usage(void) {
+  printf("rx_prbs_err_cnt\n");
+  printf("read Marvell 88F6601 RX PRBS error coutner registers\n");
+  printf("Example:\n");
+  printf("rx_prbs_err_cnt\n");
+}
+
+int rx_prbs_err_cnt(int argc, char *argv[]) {
+  unsigned int cnt0, cnt1;
+
+  if (argc != 1 || argv == NULL) {
+    rx_prbs_err_cnt_usage();
+    return -1;
+  }
+
+  if (set_pon_phy_out_of_reset() != 0) {
+    printf("Failed to take PHY out of reset\n");
+    return -1;
+  }
+  if (read_physical_addr(PON_PHY_TEST_PRBS_ERROR_COUNTER_0, &cnt0) != 0) {
+    printf("Read address 0x%x failed\n", PON_PHY_TEST_PRBS_ERROR_COUNTER_0);
+    return -1;
+  }
+  if (read_physical_addr(PON_PHY_TEST_PRBS_ERROR_COUNTER_0, &cnt1) != 0) {
+    printf("Read address 0x%x failed\n", PON_PHY_TEST_PRBS_ERROR_COUNTER_0);
+    return -1;
+  }
+  printf("RX PRBS error count: 0x%x%04x\n", cnt0, cnt1);
+
+  return 0;
+}
+
+static void gpon_cnts_usage(void) {
+  printf("gpon_cnts\n");
+  printf("dump all of Marvell 88F6601 GPON related error coutner registers\n");
+  printf("Example:\n");
+  printf("gpon_cnts\n");
+}
+
+int gpon_cnts(int argc, char *argv[]) {
+  if (argc != 1 || argv == NULL) {
+    gpon_cnts_usage();
+    return -1;
+  }
+
+  system_cmd("cat /sys/devices/platform/gpon/pm/bwMapCnt");
+  system_cmd("cat /sys/devices/platform/gpon/pm/fecCnt");
+  system_cmd("cat /sys/devices/platform/gpon/pm/gemCnt");
+  system_cmd("cat /sys/devices/platform/gpon/pm/rxPloamCnt");
+  system_cmd("cat /sys/devices/platform/gpon/pm/stdCnt");
+  system_cmd("cat /sys/devices/platform/gpon/pm/txPktCnt");
+  system_cmd("cat /sys/devices/platform/gpon/pm/txPloamCnt");
+
+  return 0;
+}
+
+static void gpon_alarms_usage(void) {
+  printf("gpon_alarms\n");
+  printf("Show 88F6601 GPON alarms\n");
+  printf("Example:\n");
+  printf("gpon_alarms\n");
+}
+
+int gpon_alarms(int argc, char *argv[]) {
+  if (argc != 1 || argv == NULL) {
+    gpon_alarms_usage();
+    return -1;
+  }
+
+  system_cmd("cat /sys/devices/platform/gpon/info/alarmGpon");
+
+  return 0;
+}
diff --git a/diags/chameleon/devmem3 b/diags/chameleon/devmem3
new file mode 100644
index 0000000..119b8b7
--- /dev/null
+++ b/diags/chameleon/devmem3
Binary files differ
diff --git a/diags/chameleon/diagutil.c b/diags/chameleon/diagutil.c
new file mode 100644
index 0000000..b2768d4
--- /dev/null
+++ b/diags/chameleon/diagutil.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright 2015 Google Inc.
+ * All rights reserved.
+ * diagutil -- Linux-based Hardware Diagnostic Utilities
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+
+#define DIAGS_VERSION "1.1"
+
+/* External Functions */
+int ioread(int argc, char **argv);
+int iowrite(int argc, char **argv);
+int iowrite_only(int argc, char *argv[]);
+int i2cread(int argc, char *argv[]);
+int i2cwrite(int argc, char *argv[]);
+int i2cprobe(int argc, char *argv[]);
+int gpio_stat(int argc, char *argv[]);
+int gpio_set_dir(int argc, char *argv[]);
+int gpio_set_out_val(int argc, char *argv[]);
+int gpio_set_tx_enable(int argc, char *argv[]);
+int gpio_mailbox(int argc, char *argv[]);
+int get_temp(int argc, char *argv[]);
+int set_leds(int argc, char *argv[]);
+int get_leds(int argc, char *argv[]);
+int phy_read(int argc, char *argv[]);
+int phy_write(int argc, char *argv[]);
+int loopback_test(int argc, char *argv[]);
+int phy_read(int argc, char *argv[]);
+int phy_write(int argc, char *argv[]);
+int soc_reg_read(int argc, char *argv[]);
+int soc_reg_write(int argc, char *argv[]);
+int gpon_rx_status(int argc, char *argv[]);
+int rx_prbs_cnt(int argc, char *argv[]);
+int rx_prbs_err_cnt(int argc, char *argv[]);
+int gpon_cnts(int argc, char *argv[]);
+int gpon_alarms(int argc, char *argv[]);
+int sfp_reg_read(int argc, char *argv[]);
+int sfp_reg_write(int argc, char *argv[]);
+int sfp_diags_reg_read(int argc, char *argv[]);
+int sfp_diags_reg_write(int argc, char *argv[]);
+int sfp_info(int argc, char *argv[]);
+int sfp_vendor(int argc, char *argv[]);
+int sfp_pn(int argc, char *argv[]);
+int sfp_wavelength(int argc, char *argv[]);
+int sfp_set_wavelength(int argc, char *argv[]);
+int sfp_set_pw(int argc, char *argv[]);
+
+/* Define the command structure */
+typedef struct {
+  const char *name;
+  int (*funcp)(int, char **);
+} tCOMMAND;
+
+void printVersion() { printf("%s\n", DIAGS_VERSION); }
+
+int version(int argc, char *argv[]) {
+  // This is to avoid unused params warning
+  if ((argc != 1) || (argv[0] == NULL)) {
+    printf("Invalid command parameter\n");
+  }
+  printVersion();
+  return 0;
+}
+
+/* Table of supported commands */
+tCOMMAND command_list[] = {
+    {"ioread", ioread},
+    {"iowrite", iowrite},
+    {"iowrite_only", iowrite_only},
+    {"", NULL},
+    {"i2cread", i2cread},
+    {"i2cwrite", i2cwrite},
+    {"i2cprobe", i2cprobe},
+    {"", NULL},
+    {"gpio_stat", gpio_stat},
+    {"gpio_set_dir", gpio_set_dir},
+    {"gpio_set_out_val", gpio_set_out_val},
+    {"gpio_set_tx_enable", gpio_set_tx_enable},
+    {"gpio_mailbox", gpio_mailbox},
+    {"get_temp", get_temp},
+    {"set_leds", set_leds},
+    {"get_leds", get_leds},
+    {"", NULL},
+    {"phy_read", phy_read},
+    {"phy_write", phy_write},
+    {"loopback_test", loopback_test},
+    {"", NULL},
+    {"soc_reg_read", soc_reg_read},
+    {"soc_reg_write", soc_reg_write},
+    {"gpon_rx_status", gpon_rx_status},
+    {"rx_prbs_cnt", rx_prbs_cnt},
+    {"rx_prbs_err_cnt", rx_prbs_err_cnt},
+    {"gpon_cnts", gpon_cnts},
+    {"gpon_alarms", gpon_alarms},
+    {"", NULL},
+    {"sfp_reg_read", sfp_reg_read},
+    {"sfp_reg_write", sfp_reg_write},
+    {"sfp_diags_reg_read", sfp_diags_reg_read},
+    {"sfp_diags_reg_write", sfp_diags_reg_write},
+    {"sfp_info", sfp_info},
+    {"sfp_vendor", sfp_vendor},
+    {"sfp_pn", sfp_pn},
+    {"sfp_wavelength", sfp_wavelength},
+    {"sfp_set_wavelength", sfp_set_wavelength},
+    {"sfp_set_pw", sfp_set_pw},
+    {"", NULL},
+    {"version", version},
+    {"", NULL},
+    {NULL, NULL},
+};
+
+static void usage(void) {
+  int i;
+
+  printf("Supported commands:\n");
+
+  for (i = 0; command_list[i].name != NULL; ++i) {
+    printf("\t%s\n", command_list[i].name);
+  }
+
+  return;
+}
+
+int main(int argc, char *argv[]) {
+  int i;
+
+  if (argc > 1) {
+    /* Search the command list for a match */
+    for (i = 0; command_list[i].name != NULL; ++i) {
+      if (strcmp(argv[1], command_list[i].name) == 0) {
+        return command_list[i].funcp(argc - 1, &argv[1]);
+      }
+    }
+  }
+
+  /* no command or bad command */
+  usage();
+
+  return 0;
+}
diff --git a/diags/chameleon/eth_test.c b/diags/chameleon/eth_test.c
new file mode 100644
index 0000000..734fc94
--- /dev/null
+++ b/diags/chameleon/eth_test.c
@@ -0,0 +1,538 @@
+/*
+ * (C) Copyright 2015 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <linux/if_packet.h>
+#include <linux/types.h>
+#include <net/if.h>
+#include <netdb.h>
+#include <netinet/ether.h>
+#include <netinet/in.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include "../common/io.h"
+#include "../common/util.h"
+#include "common.h"
+#include "mdio.h"
+
+#define ETH_PORT_NAME "eth0"
+#define MAX_NET_IF 2
+#define BUF_SIZ 1536
+#define ETH_TEST_MAX_CMD 4096
+#define ETH_TEST_MAX_RSP 4096
+#define ETH_TRAFFIC_PORT "wan0"
+#define ETH_TRAFFIC_REPORT_PERIOD 60
+#define ETH_TRAFFIC_MAX_REPORT_PERIOD 300
+#define ETH_TRAFFIC_TEST_PERIOD_SYMBOL "-p"
+// 100 Mb/s
+#define ETH_TRAFFIC_PER_PERIOD_MAX \
+  (((unsigned int)ETH_TRAFFIC_MAX_REPORT_PERIOD) * ((unsigned int)13107200))
+
+#define SERVER_PORT 8888
+#define MAX_CMD_SIZE 256
+#define SCAN_CMD_FORMAT "%256s"
+#define MAX_INT 0x7FFFFFFF
+
+#define ETH_SEND_DELAY_IN_USEC 1000
+#define ETH_MAX_LAN_PORTS 2
+#define ETH_WAIT_AFTER_LOOPBACK_SET 5
+#define ETH_PKTS_SENT_BEFORE_WAIT 0xFF
+#define ETH_PKTS_LEN_DEFAULT 128
+#define ETH_BUFFER_SIZE (ETH_PKTS_SENT_BEFORE_WAIT * ETH_PKTS_LEN_DEFAULT)
+#define ETH_LOOPBACK_PASS_FACTOR 0.8  // 80%
+#define ETH_TEST_FLUSH_NUM 5
+
+#define ETH_RX_NAME "RX"
+#define ETH_TX_NAME "TX"
+#define ETH_PACKETS_NAME "packets:"
+#define ETH_ERRORS_NAME "errors:"
+#define ETH_BYTES_NAME "bytes:"
+#define ONE_MEG (1024 * 1024)
+
+#define ETH_DEBUG_PORT_ADDR_REG 0x1D
+#define ETH_DEBUG_PORT_DATA_REG 0x1E
+#define ETH_EXT_LPBK_PORT_ADDR_OFFSET 0xB
+#define ETH_EXT_LPBK_PORT_SET_DATA 0x3C40
+#define ETH_EXT_LPBK_PORT_CLEAR_DATA 0xBC00
+#define ETH_STAT_CLEAR_CMD "ifstat > /dev/null"
+#define ETH_STAT_CMD "ifstat %s | sed '1,3d;5d'"
+#define ETH_STAT_RX_POS 5
+#define ETH_STAT_TX_POS 7
+#define ETH_STAT_WAIT_PERIOD 1  // sec
+#define ETH_STAT_PERCENT_MARGIN 95
+
+#define ETH0_SMI_REG 0xF1072004
+
+void send_mac_pkt(char *if_name, char *out_name, unsigned int xfer_len,
+                  unsigned int xfer_wait, int n,
+                  const unsigned char *dst_mac1) {
+  int sockfd, i;
+  struct ifreq if_idx;
+  struct ifreq if_mac, out_mac;
+  int tx_len = 0;
+  char sendbuf[BUF_SIZ];
+  struct ether_header *eh = (struct ether_header *)sendbuf;
+  struct sockaddr_ll socket_address;
+  unsigned char dst_mac[6] = {0, 0, 0, 0, 0, 0};
+
+  /* Open RAW socket to send on */
+  if ((sockfd = socket(AF_PACKET, SOCK_RAW, IPPROTO_RAW)) == -1) {
+    perror("socket");
+  }
+
+  /* Get the index of the interface to send on */
+  memset(&if_idx, 0, sizeof(if_idx));
+  safe_strncpy(if_idx.ifr_name, if_name, IFNAMSIZ - 1);
+  if (ioctl(sockfd, SIOCGIFINDEX, &if_idx) < 0) {
+    perror("SIOCGIFINDEX");
+  }
+  /* Get the MAC address of the interface to send on */
+  memset(&out_mac, 0, sizeof(out_mac));
+  if (out_name != NULL) {
+    safe_strncpy(out_mac.ifr_name, out_name, IFNAMSIZ - 1);
+    if (ioctl(sockfd, SIOCGIFHWADDR, &out_mac) < 0) {
+      perror("out SIOCGIFHWADDR");
+    }
+  }
+  memset(&if_mac, 0, sizeof(if_mac));
+  safe_strncpy(if_mac.ifr_name, if_name, IFNAMSIZ - 1);
+  if (ioctl(sockfd, SIOCGIFHWADDR, &if_mac) < 0) {
+    perror("SIOCGIFHWADDR");
+  }
+  if (out_name != NULL) {
+    dst_mac[0] = ((uint8_t *)&out_mac.ifr_hwaddr.sa_data)[0];
+    dst_mac[1] = ((uint8_t *)&out_mac.ifr_hwaddr.sa_data)[1];
+    dst_mac[2] = ((uint8_t *)&out_mac.ifr_hwaddr.sa_data)[2];
+    dst_mac[3] = ((uint8_t *)&out_mac.ifr_hwaddr.sa_data)[3];
+    dst_mac[4] = ((uint8_t *)&out_mac.ifr_hwaddr.sa_data)[4];
+    dst_mac[5] = ((uint8_t *)&out_mac.ifr_hwaddr.sa_data)[5];
+  } else if (dst_mac1 != NULL) {
+    dst_mac[0] = dst_mac1[0];
+    dst_mac[1] = dst_mac1[1];
+    dst_mac[2] = dst_mac1[2];
+    dst_mac[3] = dst_mac1[3];
+    dst_mac[4] = dst_mac1[4];
+    dst_mac[5] = dst_mac1[5];
+  } else {
+    printf("Invalid out_name and dst_mac.\n");
+    return;
+  }
+
+  /* Construct the Ethernet header */
+  // memset(sendbuf, 0, BUF_SIZ);
+  for (i = 0; i < BUF_SIZ; ++i) {
+    sendbuf[i] = 0xA5;
+  }
+  /* Ethernet header */
+  eh->ether_shost[0] = ((uint8_t *)&if_mac.ifr_hwaddr.sa_data)[0];
+  eh->ether_shost[1] = ((uint8_t *)&if_mac.ifr_hwaddr.sa_data)[1];
+  eh->ether_shost[2] = ((uint8_t *)&if_mac.ifr_hwaddr.sa_data)[2];
+  eh->ether_shost[3] = ((uint8_t *)&if_mac.ifr_hwaddr.sa_data)[3];
+  eh->ether_shost[4] = ((uint8_t *)&if_mac.ifr_hwaddr.sa_data)[4];
+  eh->ether_shost[5] = ((uint8_t *)&if_mac.ifr_hwaddr.sa_data)[5];
+  eh->ether_dhost[0] = dst_mac[0];
+  eh->ether_dhost[1] = dst_mac[1];
+  eh->ether_dhost[2] = dst_mac[2];
+  eh->ether_dhost[3] = dst_mac[3];
+  eh->ether_dhost[4] = dst_mac[4];
+  eh->ether_dhost[5] = dst_mac[5];
+  /* Ethertype field */
+  eh->ether_type = htons(ETH_P_IP);
+  tx_len += sizeof(struct ether_header);
+
+  /* Packet data */
+  sendbuf[tx_len++] = 0xde;
+  sendbuf[tx_len++] = 0xad;
+  sendbuf[tx_len++] = 0xbe;
+  sendbuf[tx_len++] = 0xef;
+
+  /* Index of the network device */
+  socket_address.sll_ifindex = if_idx.ifr_ifindex;
+  /* Address length*/
+  socket_address.sll_halen = ETH_ALEN;
+  /* Destination MAC */
+  socket_address.sll_addr[0] = dst_mac[0];
+  socket_address.sll_addr[1] = dst_mac[1];
+  socket_address.sll_addr[2] = dst_mac[2];
+  socket_address.sll_addr[3] = dst_mac[3];
+  socket_address.sll_addr[4] = dst_mac[4];
+  socket_address.sll_addr[5] = dst_mac[5];
+
+  /* Send packet */
+  if (n < 0) {
+    while (1) {
+      if (sendto(sockfd, sendbuf, xfer_len, 0,
+                 (struct sockaddr *)&socket_address,
+                 sizeof(struct sockaddr_ll)) < 0) {
+        printf("Send failed at msg %d\n", i);
+        break;
+      }
+      if (xfer_wait > 0) {
+        if ((i & ETH_PKTS_SENT_BEFORE_WAIT) == 0) {
+          usleep(xfer_wait);
+        }
+      }
+    }
+  } else {
+    for (i = 0; i < n; ++i) {
+      if (sendto(sockfd, sendbuf, xfer_len, 0,
+                 (struct sockaddr *)&socket_address,
+                 sizeof(struct sockaddr_ll)) < 0) {
+        printf("Send failed at msg %d\n", i);
+        break;
+      }
+      if (xfer_wait > 0) {
+        if ((i & ETH_PKTS_SENT_BEFORE_WAIT) == 0) {
+          usleep(xfer_wait);
+        }
+      }
+    }
+  }
+  close(sockfd);
+}
+
+/* If extra is not NULL, rsp is returned as the string followed extra */
+int scan_command(char *command, char *rsp, char *extra) {
+  FILE *fp;
+  fp = popen(command, "r");
+  if (fp != NULL) {
+    if (extra != NULL) {
+      while (fscanf(fp, "%s", rsp) != EOF) {
+        if (!strcmp(rsp, extra)) {
+          if (fscanf(fp, "%s", rsp) <= 0)
+            return -1;
+          else
+            return 0;
+        }
+      }
+    } else {
+      fscanf(fp, SCAN_CMD_FORMAT, rsp);
+    }
+    pclose(fp);
+  } else {
+    return -1;
+  }
+  return 0;
+}
+
+int net_stat(unsigned int *rx_bytes, unsigned int *tx_bytes, char *name) {
+  static unsigned int tx_stat = 0;
+  static unsigned int rx_stat = 0;
+  char command[MAX_CMD_SIZE], rsp[MAX_CMD_SIZE];
+  unsigned int tmp;
+
+  if (strcmp(name, ETH_PORT_NAME) != 0) {
+    return -1;
+  }
+
+  snprintf(command, sizeof(command),
+           "cat /sys/class/net/%s/statistics/tx_bytes", name);
+  if (scan_command(command, rsp, NULL) == 0) *tx_bytes = strtoul(rsp, NULL, 10);
+
+  snprintf(command, sizeof(command),
+           "cat /sys/class/net/%s/statistics/rx_bytes", name);
+  if (scan_command(command, rsp, NULL) == 0) *rx_bytes = strtoul(rsp, NULL, 10);
+
+  if (*tx_bytes >= tx_stat) {
+    *tx_bytes -= tx_stat;
+    tx_stat += *tx_bytes;
+  } else {
+    tmp = *tx_bytes;
+    // tx_bytes is uint. It will continue to increment till wrap around
+    // When it wraps around, the current value will be less than the
+    // previous one. That is why this logic kicked in.
+    *tx_bytes += (0xffffffff - tx_stat);
+    tx_stat = tmp;
+  }
+
+  if (*rx_bytes >= rx_stat) {
+    *rx_bytes -= rx_stat;
+    rx_stat += *rx_bytes;
+  } else {
+    tmp = *rx_bytes;
+    // rx_bytes is uint. It will continue to increment till wrap around
+    // When it wraps around, the current value will be less than the
+    // previous one. That is why this logic kicked in.
+    *rx_bytes += (0xffffffff - rx_stat);
+    rx_stat = tmp;
+  }
+  return 0;
+}
+
+// Return 0 if lost carrier. Otherwise, 1
+int get_carrier_state(char *name) {
+  char command[MAX_CMD_SIZE], rsp[MAX_CMD_SIZE];
+
+  snprintf(command, sizeof(command), "cat /sys/class/net/%s/carrier", name);
+  if (scan_command(command, rsp, NULL) == 0) {
+    if (strcmp(rsp, "0") != 0) return 1;
+  }
+  return 0;
+}
+
+// This is the same as sleep but monitor the link carrier every second
+// Return true if the carrier is good every second. Otherwise false
+bool sleep_and_check_carrier(int duration, char *if_name) {
+  bool good_carrier = true;
+  int i;
+  for (i = 0; i < duration; ++i) {
+    if (get_carrier_state(if_name) == 0) good_carrier = false;
+    sleep(1);
+  }
+  return good_carrier;
+}
+
+int get_if_ip(char *name, unsigned int *ip) {
+  char command[ETH_TEST_MAX_CMD], rsp[ETH_TEST_MAX_RSP];
+  bool found = false;
+
+  snprintf(command, sizeof(command), "ip addr show %s", name);
+  if (scan_command(command, rsp, "inet") == 0) {
+    if (sscanf(rsp, "%u.%u.%u.%u", ip, (ip + 1), (ip + 2), (ip + 3)) <= 0) {
+      return -1;
+    }
+    found = true;
+  }
+
+  if (!found) {
+    return -1;
+  }
+
+  return 0;
+}
+
+static void phy_read_usage(void) {
+  printf("phy_read <ifname> <reg>\n");
+  printf("Example:\n");
+  printf("phy_read %s 2\n", ETH_PORT_NAME);
+}
+
+int phy_read(int argc, char *argv[]) {
+  int rc = 0;
+  unsigned int tmp, reg, val = 0;
+
+  if (argc != 3) {
+    phy_read_usage();
+    return -1;
+  }
+
+  if (strcmp(argv[1], ETH_PORT_NAME) != 0) {
+    printf("Currently support only port %s\n", ETH_PORT_NAME);
+    return -1;
+  }
+
+  reg = strtol(argv[2], NULL, 0);
+  tmp = reg;
+  reg = (reg << 21) + (1 << 26);
+  rc = write_physical_addr(ETH0_SMI_REG, reg);
+  rc += read_physical_addr(ETH0_SMI_REG, &val);
+  val &= 0xFFFF;
+  printf("PHY %s Reg 0x%x is 0x%x\n", argv[1], tmp, val);
+  return 0;
+}
+
+static void phy_write_usage(void) {
+  printf("phy_write <ifname> <reg> <val>\n");
+  printf("Example:\n");
+  printf("phy_write i%s 22 0x6\n", ETH_PORT_NAME);
+}
+
+int phy_write(int argc, char *argv[]) {
+  int rc = 0;
+  int tmp, reg, val;
+
+  if (argc != 4) {
+    phy_write_usage();
+    return -1;
+  }
+
+  if (strcmp(argv[1], ETH_PORT_NAME) != 0) {
+    printf("Currently support only port %s\n", ETH_PORT_NAME);
+    return -1;
+  }
+
+  reg = strtol(argv[2], NULL, 0);
+  tmp = reg;
+  val = strtol(argv[3], NULL, 16);
+  reg = (reg << 21) + (0 << 26);
+  val &= 0xFFFF;
+  rc = write_physical_addr(ETH0_SMI_REG, reg);
+  rc += write_physical_addr(ETH0_SMI_REG, (reg + val));
+  printf("PHY %s Reg 0x%x = 0x%x\n", argv[1], tmp, val);
+  return 0;
+}
+
+static void send_if_usage(void) {
+  printf("send_if <source if> <num> [-t <delay between pkts send>]\n");
+  printf("Example:\n");
+  printf("send_if lan0 100\n");
+  printf("send 100 msg out of lan0\n");
+}
+
+int send_if(int argc, char *argv[]) {
+  int n;
+  char if_name[IFNAMSIZ];
+  unsigned int xfer_wait = ETH_SEND_DELAY_IN_USEC;
+  unsigned char dst_mac[6] = {0, 0, 0, 0, 0, 0};
+
+  /* Get interface name */
+  if (argc == 5) {
+    if (strcmp(argv[3], "-t") == 0) {
+      xfer_wait = strtoul(argv[4], NULL, 10);
+    } else {
+      send_if_usage();
+      return -1;
+    }
+  } else if (argc != 3) {
+    send_if_usage();
+    return -1;
+  }
+
+  strcpy(if_name, argv[1]);
+  n = strtol(argv[2], NULL, 10);
+
+  send_mac_pkt(if_name, NULL, BUF_SIZ, xfer_wait, n, dst_mac);
+
+  printf("Sent %d pkt of size %d from %s to %s\n", n, BUF_SIZ, argv[1],
+         argv[2]);
+
+  return 0;
+}
+
+static void loopback_test_usage(void) {
+  printf(
+      "loopback_test <interface> <duration in secs> "
+      "[<%s print-period in secs>]\n",
+      ETH_TRAFFIC_TEST_PERIOD_SYMBOL);
+  printf("- duration >=1 or -1 (forever)\n");
+  printf("- print-period >= 0 and <= %d\n", ETH_TRAFFIC_MAX_REPORT_PERIOD);
+  printf("- print-period > 0 if duration > 0\n");
+  printf("- print-period = 0 prints only the summary\n");
+}
+
+int loopback_test(int argc, char *argv[]) {
+  int duration, num = -1, collected_count = 0;
+  int pid, pid1, print_period = ETH_TRAFFIC_REPORT_PERIOD;
+  unsigned int pkt_len = ETH_PKTS_LEN_DEFAULT, rx_bytes, tx_bytes;
+  bool print_every_period = true, traffic_problem = false, problem = false;
+  float average_throughput = 0.0, throughput;
+  unsigned char dst_mac[6] = {0, 0, 0, 0, 0, 0};
+
+  if ((argc != 3) && (argc != 5)) {
+    loopback_test_usage();
+    return -1;
+  }
+
+  if (strcmp(argv[1], ETH_PORT_NAME) != 0) {
+    printf("Invalid Ethernet Interface %s\n", argv[1]);
+    return -1;
+  }
+
+  duration = strtol(argv[2], NULL, 0);
+  if ((duration < -1) || (duration == 0)) {
+    loopback_test_usage();
+    return -1;
+  }
+
+  if (argc == 5) {
+    if (strcmp(argv[3], ETH_TRAFFIC_TEST_PERIOD_SYMBOL) != 0) {
+      loopback_test_usage();
+      return -1;
+    }
+
+    print_period = strtoul(argv[4], NULL, 0);
+    if (((print_period == 0) && (duration < 0)) || (print_period < 0) ||
+        (print_period > ETH_TRAFFIC_MAX_REPORT_PERIOD)) {
+      loopback_test_usage();
+      return -1;
+    }
+    if (print_period == 0) {
+      print_every_period = false;
+      print_period = ETH_TRAFFIC_REPORT_PERIOD;
+    }
+  }
+
+  // eth_external_loopback(argv[1], true);
+  system_cmd("ethtool -s " ETH_PORT_NAME " autoneg off duplex full speed 100");
+  sleep(2);
+
+  net_stat(&rx_bytes, &tx_bytes, argv[1]);
+
+  pid = fork();
+  if (pid < 0) {
+    printf("Server fork error %d, errno %d\n", pid, errno);
+    return -1;
+  }
+  if (pid == 0) {
+    // Child process
+    send_mac_pkt(argv[1], NULL, pkt_len, 0, num, dst_mac);
+    exit(0);
+  }
+  // Parent process
+  pid1 = pid;
+
+  while (duration != 0) {
+    if (duration >= 0) {
+      if (duration <= print_period) {
+        problem = !sleep_and_check_carrier(duration, argv[1]);
+        print_period = duration;
+        duration = 0;
+        kill(pid1, SIGKILL);
+        // printf("Killed processes %d and %d\n", pid1, pid2);
+      } else {
+        duration -= print_period;
+        problem = !sleep_and_check_carrier(print_period, argv[1]);
+      }
+    } else {
+      problem = !sleep_and_check_carrier(print_period, argv[1]);
+    }
+
+    if (duration > 0) kill(pid1, SIGSTOP);
+    sleep(ETH_STAT_WAIT_PERIOD);
+    net_stat(&rx_bytes, &tx_bytes, argv[1]);
+    if (duration > 0) kill(pid1, SIGCONT);
+    ++collected_count;
+    // Give 1% margin
+    if ((rx_bytes == 0) ||
+        (((tx_bytes / 100) * ETH_STAT_PERCENT_MARGIN) > rx_bytes)) {
+      problem = true;
+    }
+    if ((rx_bytes > ETH_TRAFFIC_PER_PERIOD_MAX) ||
+        (tx_bytes > ETH_TRAFFIC_PER_PERIOD_MAX)) {
+      problem = true;
+    }
+    traffic_problem |= problem;
+    if (!problem) {
+      throughput = (((float)rx_bytes) * 8) / (float)(print_period * ONE_MEG);
+      average_throughput += throughput;
+    } else {
+      throughput = 0.0;
+    }
+    if (print_every_period) {
+      printf("%s %s: %3.3f Mb/s (%d:%d)\n", (problem) ? FAIL_TEXT : PASS_TEXT,
+             argv[1], throughput, tx_bytes, rx_bytes);
+    }
+    problem = false;
+  }
+
+  // eth_external_loopback(argv[1], false);
+  system_cmd("ethtool -s " ETH_PORT_NAME " autoneg on");
+
+  average_throughput /= ((float)collected_count);
+  printf("%s overall %s: %3.3f Mb/s\n",
+         (traffic_problem) ? FAIL_TEXT : PASS_TEXT, argv[1],
+         average_throughput);
+
+  return 0;
+}
diff --git a/diags/chameleon/gpio.c b/diags/chameleon/gpio.c
new file mode 100644
index 0000000..0ec2bd2
--- /dev/null
+++ b/diags/chameleon/gpio.c
@@ -0,0 +1,407 @@
+/*
+ * (C) Copyright 2016 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include <errno.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include "../common/io.h"
+#include "../common/util.h"
+#include "i2c.h"
+
+#define STOP_STR "stop"
+#define START_STR "start"
+#define LINE_MAX 128
+#define GET_TEMP "cat /sys/devices/platform/KW2Thermal.0/temp1_input"
+#define RED_LED "red"
+#define RED_LED_BRIGHTNESS "/sys/class/leds/sys-red/brightness"
+#define BLUE_LED "blue"
+#define BLUE_LED_BRIGHTNESS "/sys/class/leds/sys-blue/brightness"
+#define MPP_CONTROL_REG 0x00018000
+#define GPIO_DATA_OUT_REG 0x00018100
+#define GPIO_DATA_OUT_EN_REG 0x00018104
+#define GPIO_DATA_IN_REG 0x00018110
+#define GPIO_HIGH_DATA_OUT_REG 0x00018124
+#define GPIO_HIGH_DATA_OUT_EN_REG 0x00018128
+#define GPIO_HIGH_DATA_IN_REG 0x00018134
+#define GPIO_HIGH_PIN_START_NUM 32
+#define GPIO_MAX_PIN_NUM 37
+#define GPIO_DIR_IN_STR "in"
+#define GPIO_DIR_OUT_STR "out"
+#define GPIO_21_PON_TX_DIS 21
+#define AVANTA_BASE_ADDR 0xF1000000
+
+#define BOARD_TEMP_BUS 0
+#define BOARD_TEMP_ADDR 0x48
+#define BOARD_TEMP_ADDR_LEN 1
+#define BOARD_TEMP_REG 0
+#define BOARD_TEMP_LEN 2
+
+static void gpio_set_tx_enable_usage(void) {
+  printf("gpio_set_tx_enable <0 | 1>\n");
+  printf("Example:\n");
+  printf("gpio_set_tx_enable 0\n");
+  printf("set TX_ENABLE pin to low\n");
+}
+
+int gpio_set_tx_enable(int argc, char *argv[]) {
+  unsigned int tx_en_val, out_en, out, mask = 1 << GPIO_21_PON_TX_DIS;
+  unsigned int out_en_reg = AVANTA_BASE_ADDR + GPIO_DATA_OUT_EN_REG;
+  unsigned int out_reg = AVANTA_BASE_ADDR + GPIO_DATA_OUT_REG;
+  unsigned int mpp_ctrl =
+      AVANTA_BASE_ADDR + MPP_CONTROL_REG + (4 * (GPIO_21_PON_TX_DIS / 8));
+
+  if (argc != 2) {
+    gpio_set_tx_enable_usage();
+    return -1;
+  }
+  tx_en_val = get_num(argv[1]);
+  if (tx_en_val != 0 && tx_en_val != 1) {
+    printf("Invalid TX_ENABLE value %d\n", tx_en_val);
+    gpio_set_tx_enable_usage();
+    return -1;
+  }
+  if (read_physical_addr(out_en_reg, &out_en) != 0) {
+    printf("Read address 0x%x failed\n", out_en_reg);
+    return -1;
+  }
+  if ((out_en & mask) != 0) {
+    out_en &= ~mask;
+    if (write_physical_addr(out_en_reg, out_en) != 0) {
+      printf("Write address 0x%x of value 0x%x failed\n", out_en_reg, out_en);
+      return -1;
+    }
+  }
+  if (read_physical_addr(out_reg, &out) != 0) {
+    printf("Read address 0x%x failed\n", out_reg);
+    return -1;
+  }
+  if ((out & mask) != (tx_en_val << GPIO_21_PON_TX_DIS)) {
+    out = (out & (~mask)) | (tx_en_val << GPIO_21_PON_TX_DIS);
+    if (write_physical_addr(out_reg, out) != 0) {
+      printf("Write address 0x%x of value 0x%x failed\n", out_reg, out);
+      return -1;
+    }
+  }
+  // set MPP function to gpio
+  if (read_physical_addr(mpp_ctrl, &out) != 0) {
+    printf("Read address 0x%x failed\n", mpp_ctrl);
+    return -1;
+  }
+  out &= ~(0xFF << (4 * (GPIO_21_PON_TX_DIS % 8)));
+  if (write_physical_addr(mpp_ctrl, out) != 0) {
+    printf("Write address 0x%x of value 0x%x failed\n", mpp_ctrl, out);
+    return -1;
+  }
+  printf("Set TX_ENABLE to %d\n", tx_en_val);
+  return 0;
+}
+
+static void gpio_stat_usage(void) {
+  printf("gpio_stat <GPIO pin num (0 to %d)>\n", GPIO_MAX_PIN_NUM);
+  printf("Example:\n");
+  printf("gpio_stat 21\n");
+  printf("Display the status of the specified GPIO pin\n");
+}
+
+int gpio_stat(int argc, char *argv[]) {
+  unsigned int pin_num, pin_mask, in, out, out_en;
+  unsigned int out_reg, out_en_reg, in_reg;
+
+  if (argc != 2) {
+    gpio_stat_usage();
+    return -1;
+  }
+
+  pin_num = get_num(argv[1]);
+
+  if (pin_num > GPIO_MAX_PIN_NUM) {
+    printf("Invalid GPIO pin number %d\n", pin_num);
+    gpio_stat_usage();
+    return -1;
+  }
+  if (pin_num >= GPIO_HIGH_PIN_START_NUM) {
+    out_reg = AVANTA_BASE_ADDR + GPIO_HIGH_DATA_OUT_REG;
+    out_en_reg = AVANTA_BASE_ADDR + GPIO_HIGH_DATA_OUT_EN_REG;
+    in_reg = AVANTA_BASE_ADDR + GPIO_HIGH_DATA_IN_REG;
+    pin_mask = 1 << (pin_num - GPIO_HIGH_PIN_START_NUM);
+  } else {
+    out_reg = AVANTA_BASE_ADDR + GPIO_DATA_OUT_REG;
+    out_en_reg = AVANTA_BASE_ADDR + GPIO_DATA_OUT_EN_REG;
+    in_reg = AVANTA_BASE_ADDR + GPIO_DATA_IN_REG;
+    pin_mask = 1 << pin_num;
+  }
+  if (read_physical_addr(out_reg, &out) != 0) {
+    printf("Read address 0x%x failed\n", out_reg);
+    return -1;
+  }
+  if (read_physical_addr(out_en_reg, &out_en) != 0) {
+    printf("Read address 0x%x failed\n", out_en_reg);
+    return -1;
+  }
+  if (read_physical_addr(in_reg, &in) != 0) {
+    printf("Read address 0x%x failed\n", in_reg);
+    return -1;
+  }
+  printf("GPIO pin %d: DIR: %s IN: 0x%x OUT: 0x%x\n", pin_num,
+         (out_en & pin_mask) ? "in" : "out", (in & pin_mask) ? 1 : 0,
+         (out & pin_mask) ? 1 : 0);
+  printf("GPIO regs: EN 0x%08x OUT 0x%08x IN 0x%08x MASK 0x%08x\n", out_en, out,
+         in, pin_mask);
+
+  return 0;
+}
+
+static void gpio_set_dir_usage(void) {
+  printf("gpio_set_dir <GPIO pin num (0 to %d)> <%s | %s>\n", GPIO_MAX_PIN_NUM,
+         GPIO_DIR_IN_STR, GPIO_DIR_OUT_STR);
+  printf("Example:\n");
+  printf("gpio_set_dir 21 %s\n", GPIO_DIR_OUT_STR);
+  printf("set the specified GPIO pin to input or output\n");
+}
+
+int gpio_set_dir(int argc, char *argv[]) {
+  unsigned int pin_num, pin_mask, out_en, out_en_reg;
+  bool is_output = true;
+
+  if (argc != 3) {
+    gpio_set_dir_usage();
+    return -1;
+  }
+
+  pin_num = get_num(argv[1]);
+
+  if (pin_num > GPIO_MAX_PIN_NUM) {
+    printf("Invalid GPIO pin number %d\n", pin_num);
+    gpio_set_dir_usage();
+    return -1;
+  }
+
+  if (strcmp(argv[2], GPIO_DIR_IN_STR) == 0)
+    is_output = false;
+  else if (strcmp(argv[2], GPIO_DIR_OUT_STR) == 0)
+    is_output = true;
+  else {
+    printf("Invalid GPIO pin direction %s\n", argv[2]);
+    gpio_set_dir_usage();
+    return -1;
+  }
+
+  if (pin_num >= GPIO_HIGH_PIN_START_NUM) {
+    out_en_reg = AVANTA_BASE_ADDR + GPIO_HIGH_DATA_OUT_EN_REG;
+    pin_mask = 1 << (pin_num - GPIO_HIGH_PIN_START_NUM);
+  } else {
+    out_en_reg = AVANTA_BASE_ADDR + GPIO_DATA_OUT_EN_REG;
+    pin_mask = 1 << pin_num;
+  }
+  if (read_physical_addr(out_en_reg, &out_en) != 0) {
+    printf("Read address 0x%x failed\n", out_en_reg);
+    return -1;
+  }
+  if (is_output)
+    out_en &= ~pin_mask;
+  else
+    out_en |= pin_mask;
+  if (write_physical_addr(out_en_reg, out_en) != 0) {
+    printf("Write address 0x%x of value 0x%x failed\n", out_en_reg, out_en);
+    return -1;
+  }
+  printf("GPIO pin %d set as %s\n", pin_num, (is_output) ? "output" : "input");
+
+  return 0;
+}
+
+static void gpio_set_out_val_usage(void) {
+  printf("gpio_set_out_val <GPIO pin num (0 to %d)> <0 | 1>\n",
+         GPIO_MAX_PIN_NUM);
+  printf("Example:\n");
+  printf("gpio_set_out_val 21 0\n");
+  printf("set the specified GPIO pin output to 0\n");
+}
+
+int gpio_set_out_val(int argc, char *argv[]) {
+  unsigned int pin_num, pin_val, pin_mask, out, out_reg;
+
+  if (argc != 3) {
+    gpio_set_out_val_usage();
+    return -1;
+  }
+
+  pin_num = get_num(argv[1]);
+  pin_val = get_num(argv[2]);
+
+  if (pin_num > GPIO_MAX_PIN_NUM) {
+    printf("Invalid GPIO pin number %d\n", pin_num);
+    gpio_set_out_val_usage();
+    return -1;
+  }
+
+  if (pin_val != 0 && pin_val != 1) {
+    printf("Invalid GPIO pin value %d\n", pin_val);
+    gpio_set_out_val_usage();
+    return -1;
+  }
+
+  if (pin_num >= GPIO_HIGH_PIN_START_NUM) {
+    out_reg = AVANTA_BASE_ADDR + GPIO_HIGH_DATA_OUT_REG;
+    pin_mask = 1 << (pin_num - GPIO_HIGH_PIN_START_NUM);
+  } else {
+    out_reg = AVANTA_BASE_ADDR + GPIO_DATA_OUT_REG;
+    pin_mask = 1 << pin_num;
+  }
+  if (read_physical_addr(out_reg, &out) != 0) {
+    printf("Read address 0x%x failed\n", out_reg);
+    return -1;
+  }
+  if (pin_val)
+    out |= pin_mask;
+  else
+    out &= ~pin_mask;
+  if (write_physical_addr(out_reg, out) != 0) {
+    printf("Write address 0x%x of value 0x%x failed\n", out_reg, out);
+    return -1;
+  }
+  printf("GPIO pin %d output set as %d\n", pin_num, pin_val);
+
+  return 0;
+}
+
+static void gpio_mailbox_usage(void) {
+  printf("gpio_mailbox <%s | %s>\n", STOP_STR, START_STR);
+  printf("Example:\n");
+  printf("gpio_mailbox %s\n", STOP_STR);
+  printf("Stop gpio_mailbox from running\n");
+}
+
+int gpio_mailbox(int argc, char *argv[]) {
+  if (argc != 2) {
+    gpio_mailbox_usage();
+    return -1;
+  }
+
+  if (strcmp(argv[1], STOP_STR) == 0) {
+    system_cmd("pkill -9 -f gpio-mailbox");
+  } else if (strcmp(argv[1], START_STR) == 0) {
+    system_cmd("gpio-mailbox 2>&1 | logos gpio-mailbox &");
+  } else {
+    gpio_mailbox_usage();
+    return -1;
+  }
+
+  return 0;
+}
+
+static void get_temp_usage(void) {
+  printf("get_temp\n");
+  printf("display CPU temperature in mili-degree C\n");
+  printf("Example\n");
+  printf("  prism-diags get_temp\n");
+}
+
+int get_temp(int argc, char *argv[]) {
+  float temp;
+  int t;
+  uint8_t value[BOARD_TEMP_LEN];
+  FILE *fp;
+
+  if (argc != 1 || argv == NULL) {
+    get_temp_usage();
+    return -1;
+  }
+  if (i2cr(BOARD_TEMP_BUS, BOARD_TEMP_ADDR, BOARD_TEMP_REG, BOARD_TEMP_ADDR_LEN,
+           BOARD_TEMP_LEN, value) != 0) {
+    printf("Temp sensor read address 0x%x failed\n", BOARD_TEMP_ADDR);
+    return -1;
+  }
+  temp = (float)(value[0]) + (((float)(value[1])) / 256.0);
+  if (value[0] & 0x80) {
+    temp -= 256.0;
+  }
+  printf("  Board Temp: %3.3f\n", temp);
+  fp = popen(GET_TEMP, "r");
+  if (fp != NULL) {
+    if (fscanf(fp, "%d", &t) <= 0) {
+      printf("Failed to read CPU temp\n");
+      pclose(fp);
+      return -1;
+    }
+    pclose(fp);
+    temp = (float)t / 1000.0;
+    printf("  CPU Temp: %3.3f\n", temp);
+  } else {
+    printf("Failed to get CPU temp\n");
+    return -1;
+  }
+
+  return 0;
+}
+
+static void set_leds_usage(void) {
+  printf("set_leds <%s | %s> <value>\n", RED_LED, BLUE_LED);
+  printf("set specified LED brightness to <valueue>\n");
+  printf("  max value is 100. set value to 0 to turn it off\n");
+  printf("Example\n");
+  printf("  prism-diags set_leds %s 10\n", RED_LED);
+}
+
+int set_leds(int argc, char *argv[]) {
+  int value;
+  char cmd[LINE_MAX];
+
+  if (argc != 3) {
+    set_leds_usage();
+    return -1;
+  }
+  value = strtol(argv[2], NULL, 10);
+
+  if (strcmp(argv[1], RED_LED) == 0) {
+    sprintf(cmd, "echo %d > %s", value, RED_LED_BRIGHTNESS);
+  } else if (strcmp(argv[1], BLUE_LED) == 0) {
+    sprintf(cmd, "echo %d > %s", value, BLUE_LED_BRIGHTNESS);
+  } else {
+    printf("Unknown LED\n");
+    set_leds_usage();
+    return -1;
+  }
+  system_cmd(cmd);
+  printf("Set %s LED brightness to %d\n", argv[1], value);
+
+  return 0;
+}
+
+static void get_leds_usage(void) {
+  printf("get_leds <%s | %s>\n", RED_LED, BLUE_LED);
+  printf("get specified LED brightness\n");
+  printf("Example\n");
+  printf("  prism-diags get_leds %s\n", RED_LED);
+}
+
+int get_leds(int argc, char *argv[]) {
+  char cmd[LINE_MAX];
+
+  if (argc != 2) {
+    get_leds_usage();
+    return -1;
+  }
+
+  if (strcmp(argv[1], RED_LED) == 0) {
+    sprintf(cmd, "cat %s", RED_LED_BRIGHTNESS);
+  } else if (strcmp(argv[1], BLUE_LED) == 0) {
+    sprintf(cmd, "cat %s", BLUE_LED_BRIGHTNESS);
+  } else {
+    printf("Unknown LED\n");
+    get_leds_usage();
+    return -1;
+  }
+  printf("%s LED brightness is ", argv[1]);
+  fflush(stdout);
+  system_cmd(cmd);
+
+  return 0;
+}
diff --git a/diags/chameleon/i2c.c b/diags/chameleon/i2c.c
new file mode 100644
index 0000000..1107bfc
--- /dev/null
+++ b/diags/chameleon/i2c.c
@@ -0,0 +1,163 @@
+/*
+ * (C) Copyright 2014 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include <errno.h>
+#include <fcntl.h>
+#include <inttypes.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/file.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "i2c.h"
+
+extern int ioctl(int, int, void *);
+
+int i2cr(int controller, uint8_t device_addr, uint32_t cell_addr,
+         uint32_t addr_len, uint32_t data_len, uint8_t *buf) {
+  char filename[FILENAME_SIZE];
+  int file;
+  int i;
+  uint32_t temp;
+  unsigned char addrbuf[4]; /* up to 4 byte addressing ! */
+  int return_code = 0;
+  struct i2c_msg message[2];
+  struct i2c_rdwr_ioctl_data rdwr_arg;
+  unsigned int read_data_len;
+
+  /* open the I2C adapter */
+  snprintf(filename, sizeof(filename), I2C_DEV_FILE, controller);
+  if ((file = open(filename, O_RDWR)) < 0) {
+    printf("I2C Error open file %s: %s", filename, strerror(errno));
+    return -1;
+  }
+
+  if ((return_code = flock(file, LOCK_EX)) < 0) {
+    printf("I2C Error lock file %s: %s", filename, strerror(errno));
+    goto i2cr_cleanup;
+  }
+
+  /* if we need to send addr, use combined transaction */
+  if (addr_len > 0) {
+    /* build struct i2c_msg 0 */
+    message[0].addr = device_addr;
+    message[0].flags = 0;
+    message[0].len = addr_len;
+    temp = cell_addr;
+    /* store addr into buffer */
+    for (i = addr_len - 1; i >= 0; i--) {
+      addrbuf[i] = temp & 0xff;
+      temp >>= 8;
+    }
+    message[0].buf = addrbuf;
+
+    /* build struct i2c_msg 1 */
+    message[1].addr = device_addr;
+    message[1].flags = I2C_M_RD;
+    message[1].len = data_len;
+    message[1].buf = buf;
+
+    /* build arg */
+    rdwr_arg.msgs = message;
+    rdwr_arg.nmsgs = 2;
+
+    return_code = ioctl(file, I2C_RDWR, &rdwr_arg);
+
+    /* since we pass in rdwr_arg.nmsgs = 2, expect a return of 2 on success */
+    if (return_code == 2) {
+      return_code = 0;
+    }
+
+    goto i2cr_cleanup;
+  }
+
+  /* setup device address */
+  if ((return_code = ioctl(file, I2C_SLAVE, (void *)(uintptr_t)device_addr)) <
+      0) {
+    printf("I2C Error: Could not set device address to %x: %s", device_addr,
+           strerror(errno));
+    goto i2cr_cleanup;
+  }
+
+  /* now read data out of the device */
+  read_data_len = read(file, buf, data_len);
+  if (read_data_len == data_len) {
+    return_code = 0;
+  }
+
+i2cr_cleanup:
+  flock(file, LOCK_UN);
+  close(file);
+
+  return return_code;
+}
+
+int i2cw(int controller, uint8_t device_addr, uint32_t cell_addr,
+         uint32_t addr_len, uint32_t data_len, uint8_t *buf) {
+  char filename[FILENAME_SIZE];
+  int file;
+  int i;
+  uint32_t temp;
+  uint8_t tempbuf[I2C_PAGE_SIZE + 4];
+  int return_code;
+  uint8_t *writebuf = buf;
+  unsigned int write_data_len;
+
+  /* check data len */
+  if (data_len > I2C_PAGE_SIZE) {
+    return -1;
+  }
+
+  /* open the corrrsponding I2C adapter */
+  snprintf(filename, sizeof(filename), I2C_DEV_FILE, controller);
+  if ((file = open(filename, O_RDWR)) < 0) {
+    printf("I2C Error open file %s: %s", filename, strerror(errno));
+    return -1;
+  }
+
+  if ((return_code = flock(file, LOCK_EX)) < 0) {
+    printf("I2C Error lock file %s: %s", filename, strerror(errno));
+    goto i2cw_cleanup;
+  }
+
+  /* setup device address */
+  if ((return_code =
+           ioctl(file, I2C_SLAVE_FORCE, (void *)(uintptr_t)device_addr)) < 0) {
+    printf("I2C Error: Could not set device address to %x: %s", device_addr,
+           strerror(errno));
+    goto i2cw_cleanup;
+  }
+
+  /* if we need to send addr */
+  if (addr_len > 0) {
+    temp = cell_addr;
+    /* store addr into buffer */
+    for (i = (int)(addr_len - 1); i >= 0; i--) {
+      tempbuf[i] = temp & 0xff;
+      temp >>= 8;
+    }
+    /* copy data over into tempbuf, right after the cell address */
+    for (i = 0; i < (int)(data_len); i++) {
+      tempbuf[addr_len + i] = buf[i];
+    }
+    writebuf = tempbuf;
+  }
+
+  /* now write addr + data out to the device */
+  write_data_len = write(file, writebuf, addr_len + data_len);
+
+  if (write_data_len == (addr_len + data_len)) {
+    return_code = 0;
+  }
+
+i2cw_cleanup:
+  flock(file, LOCK_UN);
+  close(file);
+
+  return return_code;
+}
diff --git a/diags/chameleon/i2c.h b/diags/chameleon/i2c.h
new file mode 100644
index 0000000..0c5f028
--- /dev/null
+++ b/diags/chameleon/i2c.h
@@ -0,0 +1,44 @@
+/*
+ * (C) Copyright 2014 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#ifndef optimus_i2c_h
+#define optimus_i2c_h
+
+#include <inttypes.h>
+
+#define I2C_DEV_FILE   "/dev/i2c-%d"
+#define I2C_PAGE_SIZE  16
+#define I2C_M_RD       0x01
+#define FILENAME_SIZE  64
+
+/*
+ * I2C Message - used for pure i2c transaction, also from /dev interface
+ */
+struct i2c_msg {
+  uint16_t addr;  /* slave address */
+  uint16_t flags;
+  uint16_t len;   /* msg length */
+  uint8_t *buf;   /* pointer to msg data */
+};
+
+/* This is the structure as used in the I2C_RDWR ioctl call */
+struct i2c_rdwr_ioctl_data {
+  struct i2c_msg *msgs;  /* pointers to i2c_msgs */
+  uint32_t nmsgs;        /* number of i2c_msgs */
+};
+
+#define I2C_SLAVE        0x0703  /* Change slave address */
+#define I2C_SLAVE_FORCE  0x0706  /* Use this slave address, even if it
+                                    is already in use by a driver! */
+#define I2C_RDWR         0x0707  /* Combined R/W transfer (one stop only) */
+
+int i2cr(int controller, uint8_t device_addr, uint32_t cell_addr,
+         uint32_t addr_len, uint32_t data_len, uint8_t* buf);
+
+int i2cw(int controller, uint8_t device_addr, uint32_t cell_addr,
+         uint32_t addr_len, uint32_t data_len, uint8_t* buf);
+
+#endif  // optimus_i2c_h
diff --git a/diags/chameleon/i2c_cmd.c b/diags/chameleon/i2c_cmd.c
new file mode 100644
index 0000000..a7bfe32
--- /dev/null
+++ b/diags/chameleon/i2c_cmd.c
@@ -0,0 +1,182 @@
+/*
+ * (C) Copyright 2014 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "i2c.h"
+
+#define I2C_READ_BUF_SIZE 1024
+#define DISPLAY_WIDTH 8
+
+int i2cread(int argc, char *argv[]);
+int i2cwrite(int argc, char *argv[]);
+int i2cprobe(int argc, char *argv[]);
+
+static void i2cread_usage(void) {
+  printf(
+      "i2cread bus# dev-address register-offset"
+      " address-len num-byte-to-read\n");
+  printf("Example:\n");
+  printf("i2cread 1 0x2c 0x40 1 1\n");
+  printf(
+      "Read from bus 1  device 0x2c, register 0x40,"
+      " address length is 1, read 1 byte\n");
+}
+
+int i2cread(int argc, char *argv[]) {
+  uint8_t device_addr;
+  uint32_t cell_addr;
+  uint32_t addr_len;
+  uint32_t data_len;
+  uint8_t *buf;
+  int j, k;
+  int return_code;
+  int controller;
+
+  if (argc < 6) {
+    i2cread_usage();
+    return -1;
+  }
+
+  controller = strtoul(argv[1], NULL, 0);
+  device_addr = (uint8_t)strtoul(argv[2], NULL, 0);
+  cell_addr = strtoul(argv[3], NULL, 0);
+  addr_len = strtoul(argv[4], NULL, 0);
+  data_len = strtoul(argv[5], NULL, 0);
+
+  if (data_len >= I2C_READ_BUF_SIZE) {
+    printf("ERROR: Size %s too large\n", argv[5]);
+    return -1;
+  }
+
+  buf = (uint8_t *)malloc(I2C_READ_BUF_SIZE);
+  if (buf == NULL) {
+    printf("ERROR: malloc failed (out of memory)\n");
+    return -1;
+  }
+
+  return_code =
+      i2cr(controller, device_addr, cell_addr, addr_len, data_len, buf);
+  if (return_code != 0) {
+    printf("Read ERROR: return code = %d\n", return_code);
+    free(buf);
+    return return_code;
+  }
+
+  /* display */
+  for (j = 0; j < (int)(data_len); j += DISPLAY_WIDTH) {
+    printf("\n@0x%04X\t:", cell_addr + j);
+    for (k = j; (k < (int)(data_len)) && (k < (j + DISPLAY_WIDTH)); k++) {
+      printf("%02X", buf[k]);
+    }
+    /* fill up space if finish before display width */
+    if ((k == (int)(data_len)) && (k < (j + DISPLAY_WIDTH))) {
+      for (k = data_len; k < (j + DISPLAY_WIDTH); k++) {
+        printf("  ");
+      }
+    }
+    printf("\t");
+    for (k = j; (k < (int)(data_len)) && (k < (j + DISPLAY_WIDTH)); k++) {
+      if ((buf[k] >= 0x20) && (buf[k] < 0x7f)) {
+        printf("%c", buf[k]);
+      } else {
+        printf("%c", '.');
+      }
+    }
+    printf("\n");
+  }
+
+  printf("\n--------------------------------------------\n");
+
+  free(buf);
+  return 0;
+}
+
+static void i2cwrite_usage(void) {
+  printf(
+      "i2cwrite bus# dev-address register-offset"
+      " address-len data-len data\n");
+  printf("Example:\n");
+  printf("i2cwrite 1 0x2c 0x40 1 1 0x80\n");
+  printf(
+      "Write to bus 1  device 0x2c, register 0x40,"
+      " address length is 1, 1 byte data, data value is 0x80\n");
+}
+
+int i2cwrite(int argc, char *argv[]) {
+  uint8_t device_addr;
+  uint32_t cell_addr;
+  uint32_t addr_len;
+  uint32_t data_len;
+  uint32_t data;
+  uint8_t buf[4];
+  int return_code;
+  int controller;
+  int i;
+
+  if (argc < 6) {
+    i2cwrite_usage();
+    return -1;
+  }
+
+  controller = strtoul(argv[1], NULL, 0);
+  device_addr = (uint8_t)strtoul(argv[2], NULL, 0);
+  cell_addr = strtoul(argv[3], NULL, 0);
+  addr_len = strtoul(argv[4], NULL, 0);
+  data_len = strtoul(argv[5], NULL, 0);
+
+  if (data_len > 4) {
+    printf("ERROR: Size %s too large\n", argv[5]);
+    return -1;
+  }
+
+  data = strtoul(argv[6], NULL, 0);
+
+  /* store data into buffer */
+  for (i = data_len - 1; i >= 0; i--) {
+    buf[i] = data & 0xff;
+    data >>= 8;
+  }
+
+  return_code =
+      i2cw(controller, device_addr, cell_addr, addr_len, data_len, buf);
+  if (return_code != 0) {
+    printf("Write ERROR: return code = %d\n", return_code);
+    return return_code;
+  }
+
+  return 0;
+}
+
+static void i2cprobe_usage(void) {
+  printf("i2cprobe bus#\n");
+  printf("Example:\n");
+  printf("i2cprobe 2\n");
+}
+
+int i2cprobe(int argc, char *argv[]) {
+  uint8_t device_addr;
+  uint8_t buf[1];
+  int return_code;
+  int controller;
+
+  if (argc < 2) {
+    i2cprobe_usage();
+    return -1;
+  }
+
+  controller = strtoul(argv[1], NULL, 0);
+
+  for (device_addr = 1; device_addr < 127; device_addr++) {
+    return_code = i2cr(controller, device_addr, 0, 1, 1, buf);
+    if (return_code == 0) {
+      printf("Address 0x%02X responding\n", device_addr);
+    }
+  }
+
+  return 0;
+}
diff --git a/diags/chameleon/mdio.c b/diags/chameleon/mdio.c
new file mode 100644
index 0000000..0df12ca
--- /dev/null
+++ b/diags/chameleon/mdio.c
@@ -0,0 +1,66 @@
+#include <errno.h>
+#include <net/if.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <syslog.h>
+#include <unistd.h>
+
+#include <linux/sockios.h>
+#include <linux/types.h>
+
+static int skfd = -1;
+static struct ifreq ifr;
+
+struct mii_data {
+  __u16 phy_id;
+  __u16 reg_num;
+  __u16 val_in;
+  __u16 val_out;
+};
+
+int mdio_read(int location) {
+  struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
+  mii->reg_num = location;
+  if (ioctl(skfd, SIOCGMIIREG, &ifr) < 0) {
+    fprintf(stderr, "SIOCGMIIREG on %s failed: %s\n", ifr.ifr_name,
+            strerror(errno));
+    return -1;
+  }
+  return mii->val_out;
+}
+
+int mdio_write(int location, int value) {
+  struct mii_data *mii = (struct mii_data *)&ifr.ifr_data;
+  mii->reg_num = location;
+  mii->val_in = value;
+  if (ioctl(skfd, SIOCSMIIREG, &ifr) < 0) {
+    fprintf(stderr, "SIOCSMIIREG on %s failed: %s\n", ifr.ifr_name,
+            strerror(errno));
+    return -1;
+  }
+  return 0;
+}
+
+void mdio_init() {
+  if ((skfd = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
+    perror("socket init failed");
+    exit(-1);
+  }
+}
+
+int mdio_set_interface(const char *ifname) {
+  strncpy(ifr.ifr_name, ifname, IFNAMSIZ);
+  if (ioctl(skfd, SIOCGMIIPHY, &ifr) < 0) {
+    if (errno != ENODEV)
+      fprintf(stderr, "SIOCGMIIPHY on '%s' failed: %s\n", ifname,
+              strerror(errno));
+    return -1;
+  }
+  return 0;
+}
+
+void mdio_done() { close(skfd); }
diff --git a/diags/chameleon/mdio.h b/diags/chameleon/mdio.h
new file mode 100644
index 0000000..16159c5
--- /dev/null
+++ b/diags/chameleon/mdio.h
@@ -0,0 +1,17 @@
+#ifndef VENDOR_GOOGLE_DIAGS_SPACECAST_MDIO_H_
+#define VENDOR_GOOGLE_DIAGS_SPACECAST_MDIO_H_
+
+// In order to read/write PHY registers, it needs to do it in the
+// following sequence:
+// mdio_init
+// mdio_set_interface
+// mdio_read and/or mdio_write (as many as required)
+// mdio_done
+
+int mdio_read(int location);
+int mdio_write(int location, int value);
+void mdio_init();
+int mdio_set_interface(const char* ifname);
+void mdio_done();
+
+#endif  // VENDOR_GOOGLE_DIAGS_SPACECAST_MDIO_H_
diff --git a/diags/chameleon/sfp.c b/diags/chameleon/sfp.c
new file mode 100644
index 0000000..e17e02d
--- /dev/null
+++ b/diags/chameleon/sfp.c
@@ -0,0 +1,350 @@
+/*
+ * (C) Copyright 2015 Google, Inc.
+ * All rights reserved.
+ *
+ */
+
+#include <errno.h>
+#include <signal.h>
+#include <stdbool.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include "../common/io.h"
+#include "../common/util.h"
+#include "i2c.h"
+
+#define SFP_BUS 0
+#define SFP_A0_ADDR 0x50
+#define SFP_A2_ADDR 0x51
+#define SFP_ADDR_LEN 1
+#define SFP_REG_LEN 1
+#define SFP_MAX_DATA_LEN 128
+#define SFP_INFO_REG_ADDR 0x60
+#define SFP_INFO_REG_LEN 16
+#define SFP_VENDOR_REG_ADDR 20
+#define SFP_VENDOR_REG_LEN 16
+#define SFP_PN_REG_ADDR 40
+#define SFP_PN_REG_LEN 16
+#define SFP_SN_REG_ADDR 68
+#define SFP_SN_REG_LEN 16
+#define SFP_DATE_REG_ADDR 84
+#define SFP_DATE_REG_LEN 8
+#define SFP_WL_REG_ADDR 144
+#define SFP_WL_REG_LEN 2
+#define SFP_PW_REG 0x7B
+#define SFP_PW_REG_LEN 4
+
+static void sfp_reg_read_usage(void) {
+  printf("sfp_reg_read <addr>\n");
+  printf("read SFP registers\n");
+  printf("Example:\n");
+  printf("sfp_reg_read 0x40\n");
+}
+
+int sfp_reg_read(int argc, char *argv[]) {
+  unsigned int reg_addr;
+  uint8_t value;
+
+  if (argc != 2) {
+    sfp_reg_read_usage();
+    return -1;
+  }
+
+  reg_addr = get_num(argv[1]);
+  if (i2cr(SFP_BUS, SFP_A0_ADDR, reg_addr, SFP_ADDR_LEN, SFP_REG_LEN, &value) !=
+      0) {
+    printf("SFP read address 0x%x failed\n", reg_addr);
+    return -1;
+  }
+  printf("SFP 0x%x = 0x%x\n", reg_addr, value);
+
+  return 0;
+}
+
+static void sfp_reg_write_usage(void) {
+  printf("sfp_reg_write <addr> <data>\n");
+  printf("write SFP registers\n");
+  printf("Example:\n");
+  printf("sfp_reg_write 0x60 0x0\n");
+}
+
+int sfp_reg_write(int argc, char *argv[]) {
+  unsigned int reg_addr;
+  uint8_t value;
+
+  if (argc != 3) {
+    sfp_reg_write_usage();
+    return -1;
+  }
+
+  reg_addr = get_num(argv[1]);
+  value = get_num(argv[2]);
+  if (i2cw(SFP_BUS, SFP_A0_ADDR, reg_addr, SFP_ADDR_LEN, SFP_REG_LEN, &value) !=
+      0) {
+    printf("SFP write address 0x%x value 0x%x failed\n", reg_addr, value);
+    return -1;
+  }
+  printf("SFP 0x%x set to 0x%x\n", reg_addr, value);
+
+  return 0;
+}
+
+static void sfp_diags_reg_read_usage(void) {
+  printf("sfp_diags_reg_read <addr>\n");
+  printf("read SFP 0xA2 registers\n");
+  printf("Example:\n");
+  printf("sfp_diags_reg_read 0x40\n");
+}
+
+int sfp_diags_reg_read(int argc, char *argv[]) {
+  unsigned int reg_addr;
+  uint8_t value;
+
+  if (argc != 2) {
+    sfp_diags_reg_read_usage();
+    return -1;
+  }
+
+  reg_addr = get_num(argv[1]);
+  if (i2cr(SFP_BUS, SFP_A2_ADDR, reg_addr, SFP_ADDR_LEN, SFP_REG_LEN, &value) !=
+      0) {
+    printf("SFP 0xA2 read address 0x%x failed\n", reg_addr);
+    return -1;
+  }
+  printf("SFP 0xA2 0x%x = 0x%x\n", reg_addr, value);
+
+  return 0;
+}
+
+static void sfp_diags_reg_write_usage(void) {
+  printf("sfp_diags_reg_write <addr> <data>\n");
+  printf("write SFP 0xA2 registers\n");
+  printf("Example:\n");
+  printf("sfp_diags_reg_write 0x60 0x0\n");
+}
+
+int sfp_diags_reg_write(int argc, char *argv[]) {
+  unsigned int reg_addr;
+  uint8_t value;
+
+  if (argc != 3) {
+    sfp_diags_reg_write_usage();
+    return -1;
+  }
+
+  reg_addr = get_num(argv[1]);
+  value = get_num(argv[2]);
+  if (i2cw(SFP_BUS, SFP_A2_ADDR, reg_addr, SFP_ADDR_LEN, SFP_REG_LEN, &value) !=
+      0) {
+    printf("SFP write 0xA2 address 0x%x value 0x%x failed\n", reg_addr, value);
+    return -1;
+  }
+  printf("SFP 0xA2 0x%x set to 0x%x\n", reg_addr, value);
+
+  return 0;
+}
+
+static void sfp_info_usage(void) {
+  printf("sfp_info\n");
+  printf("read SFP info\n");
+  printf("Example:\n");
+  printf("sfp_info\n");
+}
+
+int sfp_info(int argc, char *argv[]) {
+  uint8_t value[SFP_INFO_REG_LEN];
+  float temp;
+  float vcc, tx_bias, tx_power, rx_power, mod_curr;
+
+  if (argc != 1 || argv == NULL) {
+    sfp_info_usage();
+    return -1;
+  }
+
+  if (i2cr(SFP_BUS, SFP_A2_ADDR, SFP_INFO_REG_ADDR, SFP_ADDR_LEN,
+           SFP_INFO_REG_LEN, value) != 0) {
+    printf("SFP read address %d failed\n", SFP_INFO_REG_ADDR);
+    return -1;
+  }
+  if (value[0] & 0x80) {
+    value[0] &= 0x7F;
+    temp = -128.0 + value[0] + ((float)value[1]) / 256.0;
+  } else {
+    temp = value[0] + ((float)value[1]) / 256.0;
+  }
+  vcc = ((float)((value[2] << 8) + value[3])) / 10000.0;
+  tx_bias = ((float)((value[4] << 8) + value[5])) / 1000.0;
+  tx_power = ((float)((value[6] << 8) + value[7])) / 10000.0;
+  rx_power = ((float)((value[8] << 8) + value[9])) / 10000.0;
+  mod_curr = ((float)((value[12] << 8) + value[13])) / 1000.0;
+  printf("SFP temp: %f, Vcc: %3.3f V, TX bias %3.3f mA\n", temp, vcc, tx_bias);
+  printf("    TX power: %3.3f mW, RX power: %3.3f mW, mod curr: %3.3f mA\n",
+         tx_power, rx_power, mod_curr);
+
+  return 0;
+}
+
+static void sfp_vendor_usage(void) {
+  printf("sfp_vendor\n");
+  printf("read SFP vendor\n");
+  printf("Example:\n");
+  printf("sfp_vendor\n");
+}
+
+int sfp_vendor(int argc, char *argv[]) {
+  uint8_t value[SFP_MAX_DATA_LEN];
+
+  if (argc != 1 || argv == NULL) {
+    sfp_vendor_usage();
+    return -1;
+  }
+
+  printf("SFP vendor:\n");
+  if (i2cr(SFP_BUS, SFP_A0_ADDR, SFP_VENDOR_REG_ADDR, SFP_ADDR_LEN,
+           SFP_VENDOR_REG_LEN, value) != 0) {
+    printf("SFP read address %d failed\n", SFP_VENDOR_REG_ADDR);
+    return -1;
+  }
+  value[SFP_VENDOR_REG_LEN] = '\0';
+  printf("  Name: %s\n", value);
+  if (i2cr(SFP_BUS, SFP_A0_ADDR, SFP_PN_REG_ADDR, SFP_ADDR_LEN, SFP_PN_REG_LEN,
+           value) != 0) {
+    printf("SFP read address %d failed\n", SFP_PN_REG_ADDR);
+    return -1;
+  }
+  value[SFP_PN_REG_LEN] = '\0';
+  printf("  PN:   %s\n", value);
+  if (i2cr(SFP_BUS, SFP_A0_ADDR, SFP_SN_REG_ADDR, SFP_ADDR_LEN, SFP_SN_REG_LEN,
+           value) != 0) {
+    printf("SFP read address %d failed\n", SFP_SN_REG_ADDR);
+    return -1;
+  }
+  value[SFP_SN_REG_LEN] = '\0';
+  printf("  SN:   %s\n", value);
+  if (i2cr(SFP_BUS, SFP_A0_ADDR, SFP_DATE_REG_ADDR, SFP_ADDR_LEN,
+           SFP_DATE_REG_LEN, value) != 0) {
+    printf("SFP read address %d failed\n", SFP_DATE_REG_ADDR);
+    return -1;
+  }
+  value[SFP_DATE_REG_LEN] = '\0';
+  printf("  Date: %s\n", value);
+
+  return 0;
+}
+
+static void sfp_pn_usage(void) {
+  printf("sfp_pn\n");
+  printf("read SFP part number\n");
+  printf("Example:\n");
+  printf("sfp_pn\n");
+}
+
+int sfp_pn(int argc, char *argv[]) {
+  uint8_t value[SFP_MAX_DATA_LEN];
+
+  if (argc != 1 || argv == NULL) {
+    sfp_pn_usage();
+    return -1;
+  }
+
+  if (i2cr(SFP_BUS, SFP_A0_ADDR, SFP_PN_REG_ADDR, SFP_ADDR_LEN, SFP_PN_REG_LEN,
+           value) != 0) {
+    printf("SFP read address %d failed\n", SFP_PN_REG_ADDR);
+    return -1;
+  }
+  value[SFP_PN_REG_LEN] = '\0';
+  printf("SFP part number: %s\n", value);
+
+  return 0;
+}
+
+static void sfp_wavelength_usage(void) {
+  printf("sfp_wavelength\n");
+  printf("read SFP laser wavelength\n");
+  printf("Example:\n");
+  printf("sfp_wavelength\n");
+}
+
+int sfp_wavelength(int argc, char *argv[]) {
+  uint8_t value[SFP_MAX_DATA_LEN];
+  unsigned int data;
+
+  if (argc != 1 || argv == NULL) {
+    sfp_wavelength_usage();
+    return -1;
+  }
+
+  if (i2cr(SFP_BUS, SFP_A2_ADDR, SFP_WL_REG_ADDR, SFP_ADDR_LEN, SFP_WL_REG_LEN,
+           value) != 0) {
+    printf("SFP read address %d failed\n", SFP_WL_REG_ADDR);
+    return -1;
+  }
+  data = (value[0] << 8) + value[1];
+  printf("SFP wavelength: %d\n", data);
+
+  return 0;
+}
+
+static void sfp_set_wavelength_usage(void) {
+  printf("sfp_set_wavelength <wavelength>\n");
+  printf("set SFP laser wavelength\n");
+  printf("Example:\n");
+  printf("sfp_set_wavelength 1520\n");
+}
+
+int sfp_set_wavelength(int argc, char *argv[]) {
+  uint8_t value[SFP_MAX_DATA_LEN];
+  unsigned int data;
+
+  if (argc != 2) {
+    sfp_set_wavelength_usage();
+    return -1;
+  }
+
+  data = get_num(argv[1]);
+  value[0] = (data >> 8);
+  value[1] = data & 0xFF;
+  if (i2cw(SFP_BUS, SFP_A2_ADDR, SFP_WL_REG_ADDR, SFP_ADDR_LEN, SFP_WL_REG_LEN,
+           value) != 0) {
+    printf("SFP write address %d of %d failed\n", SFP_WL_REG_ADDR, data);
+    return -1;
+  }
+  data = (value[0] << 8) + value[1];
+  printf("SFP wavelength: %d\n", data);
+
+  return 0;
+}
+
+static void sfp_set_pw_usage(void) {
+  printf("sfp_set_pw <password>\n");
+  printf("set SFP access password\n");
+  printf("Example:\n");
+  printf("sfp_set_pw 0x80818283\n");
+}
+
+int sfp_set_pw(int argc, char *argv[]) {
+  uint8_t value[SFP_MAX_DATA_LEN];
+  unsigned int data;
+
+  if (argc != 2) {
+    sfp_set_pw_usage();
+    return -1;
+  }
+
+  data = get_num(argv[1]);
+  value[0] = (data >> 24) & 0xFF;
+  value[1] = (data >> 16) & 0xFF;
+  value[2] = (data >> 8) & 0xFF;
+  value[3] = data & 0xFF;
+  if (i2cw(SFP_BUS, SFP_A2_ADDR, SFP_PW_REG, SFP_ADDR_LEN, SFP_PW_REG_LEN,
+           value) != 0) {
+    printf("SFP write address %d of %d failed\n", SFP_PW_REG, data);
+    return -1;
+  }
+  printf("SFP password set to 0x%2x%2x%2x%2x\n", value[0], value[1], value[2],
+         value[3]);
+
+  return 0;
+}
diff --git a/diags/common/util.c b/diags/common/util.c
index 41802b1..fae99ab 100644
--- a/diags/common/util.c
+++ b/diags/common/util.c
@@ -77,3 +77,16 @@
     printf("ERROR: system command %s return %d\n", cmd, rc);
   }
 }
+
+unsigned int get_num(char *numstr) {
+  unsigned int value;
+
+  if (strlen(numstr) > 2) {
+    if (strncmp(numstr, "0x", 2) == 0) {
+      value = strtoul(numstr, NULL, 16);
+      return value;
+    }
+  }
+  value = strtoul(numstr, NULL, 10);
+  return value;
+}
diff --git a/diags/common/util.h b/diags/common/util.h
index 340b582..f6717b7 100644
--- a/diags/common/util.h
+++ b/diags/common/util.h
@@ -23,4 +23,6 @@
 
 void system_cmd(const char *cmd);
 
+unsigned int get_num(char *numstr);
+
 #endif  // VENDOR_GOOGLE_DIAGS_WINDCHARGER_UTIL_H_