sweetberry: add usb power logging interface

This allows logging of power data over sweetberry

BUG=chromium:608039
TEST=log power data
BRANCH=None

Change-Id: I6f642384cbf223959294c7bd99bca0f9206775b8
Signed-off-by: Nick Sanders <nsanders@chromium.org>
Reviewed-on: https://chromium-review.googlesource.com/385540
Reviewed-by: Todd Broch <tbroch@chromium.org>

8 files changed, 1056 insertions, 12 deletions

diff --git a/board/sweetberry/board.c b/board/sweetberry/board.c
index 7f1018d315..2bb97fde39 100644
--- a/board/sweetberry/board.c
+++ b/board/sweetberry/board.c
@@ -19,6 +19,7 @@
 #include "util.h"
 #include "usb_dwc_hw.h"
 #include "usb_dwc_console.h"
+#include "usb_power.h"
 #include "usb_dwc_update.h"
 
 /******************************************************************************
@@ -36,11 +37,16 @@ const void *const usb_strings[] = {
 
 BUILD_ASSERT(ARRAY_SIZE(usb_strings) == USB_STR_COUNT);
 
+/* USB power interface. */
+USB_POWER_CONFIG(sweetberry_power, USB_IFACE_POWER, USB_EP_POWER);
+
+
 struct dwc_usb usb_ctl = {
 	.ep = {
 		&ep0_ctl,
 		&ep_console_ctl,
 		&usb_update_ep_ctl,
+		&sweetberry_power_ep_ctl,
 	},
 	.speed = USB_SPEED_FS,
 	.phy_type = USB_PHY_ULPI,
diff --git a/board/sweetberry/board.h b/board/sweetberry/board.h
index b73d5ad188..754101df1e 100644
--- a/board/sweetberry/board.h
+++ b/board/sweetberry/board.h
@@ -16,7 +16,7 @@
 /* Enable console recasting of GPIO type. */
 #define CONFIG_CMD_GPIO_EXTENDED
 
-/* The UART console can be on flax USART3 (PC10/PC11) */
+/* The UART console can be on flex USART3 (PC10/PC11) */
 /* The UART console can be on header USART4 (PA0/PA1) */
 #undef CONFIG_UART_CONSOLE
 #define CONFIG_UART_CONSOLE 4
@@ -42,6 +42,7 @@
 #define CONFIG_USB_CONSOLE
 #define CONFIG_STREAM_USB
 #define CONFIG_USB_UPDATE
+#define CONFIG_USB_POWER
 
 #undef CONFIG_USB_MAXPOWER_MA
 #define CONFIG_USB_MAXPOWER_MA 100
@@ -52,13 +53,15 @@
 /* USB interface indexes (use define rather than enum to expand them) */
 #define USB_IFACE_CONSOLE	0
 #define USB_IFACE_UPDATE	1
-#define USB_IFACE_COUNT		2
+#define USB_IFACE_POWER		2
+#define USB_IFACE_COUNT		3
 
 /* USB endpoint indexes (use define rather than enum to expand them) */
 #define USB_EP_CONTROL		0
 #define USB_EP_CONSOLE		1
 #define USB_EP_UPDATE		2
-#define USB_EP_COUNT		3
+#define USB_EP_POWER		3
+#define USB_EP_COUNT		4
 
 /* This is not actually a Chromium EC so disable some features. */
 #undef CONFIG_WATCHDOG_HELP
diff --git a/chip/stm32/build.mk b/chip/stm32/build.mk
index 8575463c7a..cb5f8cacd6 100644
--- a/chip/stm32/build.mk
+++ b/chip/stm32/build.mk
@@ -60,6 +60,7 @@ chip-$(CONFIG_PWM)+=pwm.o
 ifeq ($(CHIP_FAMILY),stm32f4)
 chip-$(CONFIG_USB)+=usb_dwc.o usb_endpoints.o
 chip-$(CONFIG_USB_CONSOLE)+=usb_dwc_console.o
+chip-$(CONFIG_USB_POWER)+=usb_power.o
 chip-$(CONFIG_STREAM_USB)+=usb_dwc_stream.o
 chip-$(CONFIG_USB_I2C)+=usb_dwc_i2c.o
 else
diff --git a/chip/stm32/usb_dwc.c b/chip/stm32/usb_dwc.c
index 5af53b9446..1b009a25cf 100644
--- a/chip/stm32/usb_dwc.c
+++ b/chip/stm32/usb_dwc.c
@@ -245,10 +245,15 @@ int usb_write_ep(uint32_t ep_num, int len, void *data)
 {
 	struct dwc_usb_ep *ep = usb_ctl.ep[ep_num];
 
+	if (GR_USB_DIEPCTL(ep_num) & DXEPCTL_EPENA) {
+		CPRINTS("usb_write_ep ep%d: FAIL: tx already in progress!");
+		return 0;
+	}
+
 	/* We will send as many packets as necessary, including a final
 	 * packet of < USB_MAX_PACKET_SIZE (maybe zero length)
 	 */
-	ep->in_packets = (len + USB_MAX_PACKET_SIZE)/USB_MAX_PACKET_SIZE;
+	ep->in_packets = (len + USB_MAX_PACKET_SIZE - 1) / USB_MAX_PACKET_SIZE;
 	ep->in_pending = len;
 	ep->in_data = data;
 
@@ -256,12 +261,9 @@ int usb_write_ep(uint32_t ep_num, int len, void *data)
 
 	GR_USB_DIEPTSIZ(ep_num) |= DXEPTSIZ_PKTCNT(ep->in_packets);
 	GR_USB_DIEPTSIZ(ep_num) |= DXEPTSIZ_XFERSIZE(len);
-	GR_USB_DIEPDMA(ep_num) = (uint32_t)ep->in_data;
+	GR_USB_DIEPDMA(ep_num) = (uint32_t)(ep->in_data);
 
-
-	/* We're sending this much.
-	 * TODO: we should support sending more than one packet.
-	 */
+	/* We could support longer multi-dma transfers here. */
 	ep->in_pending -= len;
 	ep->in_packets -= ep->in_packets;
 	ep->in_data += len;
@@ -275,13 +277,20 @@ int usb_write_ep(uint32_t ep_num, int len, void *data)
 void usb_epN_tx(uint32_t ep_num)
 {
 	struct dwc_usb_ep *ep = usb_ctl.ep[ep_num];
-
 	uint32_t dieptsiz = GR_USB_DIEPTSIZ(ep_num);
 
+	if (GR_USB_DIEPCTL(ep_num) & DXEPCTL_EPENA) {
+		CPRINTS("usb_epN_tx ep%d: tx still active.", ep_num);
+		return;
+	}
+
 	/* clear the Tx/IN interrupts */
 	GR_USB_DIEPINT(ep_num) = 0xffffffff;
 
-	/* Let's assume this is actually true. */
+	/*
+	 * Let's assume this is actually true.
+	 * We could support multi-dma transfers here.
+	 */
 	ep->in_packets = 0;
 	ep->in_pending = dieptsiz & GC_USB_DIEPTSIZ1_XFERSIZE_MASK;
 
@@ -303,7 +312,7 @@ void usb_epN_tx(uint32_t ep_num)
 int usb_read_ep(uint32_t ep_num, int len, void *data)
 {
 	struct dwc_usb_ep *ep = usb_ctl.ep[ep_num];
-	int packets = (len + USB_MAX_PACKET_SIZE)/USB_MAX_PACKET_SIZE;
+	int packets = (len + USB_MAX_PACKET_SIZE - 1) / USB_MAX_PACKET_SIZE;
 
 	ep->out_data = data;
 	ep->out_pending = 0;
diff --git a/chip/stm32/usb_power.c b/chip/stm32/usb_power.c
new file mode 100644
index 0000000000..b1ba698cc7
--- /dev/null
+++ b/chip/stm32/usb_power.c
@@ -0,0 +1,655 @@
+/* Copyright 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#include "common.h"
+#include "dma.h"
+#include "hooks.h"
+#include "i2c.h"
+#include "link_defs.h"
+#include "registers.h"
+#include "timer.h"
+#include "usb_descriptor.h"
+#include "usb_power.h"
+#include "util.h"
+
+#define CPRINTS(format, args...) cprints(CC_I2C, format, ## args)
+
+
+
+static int usb_power_init_inas(struct usb_power_config const *config);
+static int usb_power_read(struct usb_power_config const *config);
+static int usb_power_write_line(struct usb_power_config const *config);
+
+
+static int8_t usb_power_map_error(int error)
+{
+	switch (error) {
+	case EC_SUCCESS:       return USB_POWER_SUCCESS;
+	case EC_ERROR_TIMEOUT: return USB_POWER_ERROR_TIMEOUT;
+	case EC_ERROR_BUSY:    return USB_POWER_ERROR_BUSY;
+	default:	       return USB_POWER_ERROR_UNKNOWN | (error & 0x7f);
+	}
+}
+
+void usb_power_deferred_rx(struct usb_power_config const *config)
+{
+	int rx_count = rx_ep_pending(config->endpoint);
+
+	/* Handle an incoming command if available */
+	if (rx_count)
+		usb_power_read(config);
+}
+
+void usb_power_deferred_tx(struct usb_power_config const *config)
+{
+	struct usb_power_state *state = config->state;
+
+	if (!tx_ep_is_ready(config->endpoint))
+		return;
+
+	/* We've replied, set up the next read. */
+	if (!rx_ep_is_active(config->endpoint)) {
+		/* Remove any active dma region from output buffer */
+		state->reports_xmit_active = state->reports_tail;
+
+		/* Wait for the next command */
+		usb_read_ep(config->endpoint,
+			config->ep->out_databuffer_max,
+			config->ep->out_databuffer);
+		return;
+	}
+}
+
+/* Reset stream */
+void usb_power_reset(struct usb_power_config const *config)
+{
+	config->ep->out_databuffer = config->state->rx_buf;
+	config->ep->out_databuffer_max = sizeof(config->state->rx_buf);
+	config->ep->in_databuffer = config->state->tx_buf;
+	config->ep->in_databuffer_max = sizeof(config->state->tx_buf);
+
+	epN_reset(config->endpoint);
+
+	/* Flush any queued data */
+	hook_call_deferred(config->ep->rx_deferred, 0);
+	hook_call_deferred(config->ep->tx_deferred, 0);
+}
+
+
+/* Write one or more power records to USB */
+static int usb_power_write_line(struct usb_power_config const *config)
+{
+	struct usb_power_state *state = config->state;
+	struct usb_power_report *r = (struct usb_power_report *)(
+		state->reports_data_area +
+		(USB_POWER_RECORD_SIZE(state->ina_count)
+		* state->reports_tail));
+	/* status + size + timestamps + power list */
+	size_t bytes = USB_POWER_RECORD_SIZE(state->ina_count);
+
+	/* Check if queue has active data. */
+	if (config->state->reports_head != config->state->reports_tail) {
+		int recordcount = 1;
+
+		/* We'll concatenate all the upcoming recrds. */
+		if (config->state->reports_tail < config->state->reports_head)
+			recordcount = config->state->reports_head -
+				      config->state->reports_tail;
+		else
+			recordcount = state->max_cached -
+				      config->state->reports_tail;
+
+		state->reports_xmit_active = state->reports_tail;
+		state->reports_tail = (state->reports_tail + recordcount) %
+			state->max_cached;
+
+		usb_write_ep(config->endpoint, bytes * recordcount, r);
+		return bytes;
+	}
+
+	CPRINTS("usb_power_write_line: no data rs: %d, rc: %d",
+		USB_POWER_RECORD_SIZE(state->ina_count),
+		USB_POWER_MAX_CACHED(state->ina_count));
+	return 0;
+}
+
+
+static int usb_power_state_reset(struct usb_power_config const *config)
+{
+	struct usb_power_state *state = config->state;
+
+	state->state = USB_POWER_STATE_OFF;
+	state->reports_head = 0;
+	state->reports_tail = 0;
+	state->reports_xmit_active = 0;
+
+	CPRINTS("[RESET] STATE -> OFF");
+	return USB_POWER_SUCCESS;
+}
+
+
+static int usb_power_state_stop(struct usb_power_config const *config)
+{
+	struct usb_power_state *state = config->state;
+
+	/* Only a valid transition from CAPTURING */
+	if (state->state != USB_POWER_STATE_CAPTURING) {
+		CPRINTS("[STOP] Error not capturing.");
+		return USB_POWER_ERROR_NOT_CAPTURING;
+	}
+
+	state->state = USB_POWER_STATE_SETUP;
+	state->reports_head = 0;
+	state->reports_tail = 0;
+	state->reports_xmit_active = 0;
+	state->stride_bytes = 0;
+	CPRINTS("[STOP] STATE: CAPTURING -> SETUP");
+	return USB_POWER_SUCCESS;
+}
+
+
+
+static int usb_power_state_start(struct usb_power_config const *config,
+				 union usb_power_command_data *cmd, int count)
+{
+	struct usb_power_state *state = config->state;
+	int integration_us = cmd->start.integration_us;
+
+	if (state->state != USB_POWER_STATE_SETUP) {
+		CPRINTS("[START] Error not setup.");
+		return USB_POWER_ERROR_NOT_SETUP;
+	}
+
+	if (count != 6) {
+		CPRINTS("[START] Error count %d is not 6", (int)count);
+		return USB_POWER_ERROR_READ_SIZE;
+	}
+
+	if (integration_us == 0) {
+		CPRINTS("[START] integration_us cannot be 0");
+		return USB_POWER_ERROR_UNKNOWN;
+	}
+
+	/* Calculate the reports array */
+	state->stride_bytes = USB_POWER_RECORD_SIZE(state->ina_count);
+	state->max_cached = USB_POWER_MAX_CACHED(state->ina_count);
+
+	state->integration_us = integration_us;
+	usb_power_init_inas(config);
+
+	state->state = USB_POWER_STATE_CAPTURING;
+	CPRINTS("[START] STATE: SETUP -> CAPTURING %dus", integration_us);
+
+	/* Find our starting time. */
+	config->state->base_time = get_time().val;
+
+	hook_call_deferred(config->deferred_cap, state->integration_us);
+	return USB_POWER_SUCCESS;
+}
+
+
+static int usb_power_state_settime(struct usb_power_config const *config,
+				 union usb_power_command_data *cmd, int count)
+{
+	if (count != sizeof(struct usb_power_command_settime)) {
+		CPRINTS("[SETTIME] Error: count %d is not %d",
+			(int)count, sizeof(struct usb_power_command_settime));
+		return USB_POWER_ERROR_READ_SIZE;
+	}
+
+	/* Find the offset between microcontroller clock and host clock. */
+	if (cmd->settime.time)
+		config->state->wall_offset = cmd->settime.time - get_time().val;
+	else
+		config->state->wall_offset = 0;
+
+	return EC_SUCCESS;
+}
+
+
+static int usb_power_state_addina(struct usb_power_config const *config,
+				 union usb_power_command_data *cmd, int count)
+{
+	struct usb_power_state *state = config->state;
+	struct usb_power_ina_cfg *ina;
+
+	/* Only valid from OFF or SETUP */
+	if ((state->state != USB_POWER_STATE_OFF) &&
+	    (state->state != USB_POWER_STATE_SETUP)) {
+		CPRINTS("[ADDINA] Error incorrect state.");
+		return USB_POWER_ERROR_NOT_SETUP;
+	}
+
+	if (count != sizeof(struct usb_power_command_addina)) {
+		CPRINTS("[ADDINA] Error count %d is not %d",
+			(int)count, sizeof(struct usb_power_command_addina));
+		return USB_POWER_ERROR_READ_SIZE;
+	}
+
+	if (state->ina_count >= USB_POWER_MAX_READ_COUNT) {
+		CPRINTS("[ADDINA] Error INA list full");
+		return USB_POWER_ERROR_FULL;
+	}
+
+	/* Transition to SETUP state if necessary and clear INA data */
+	if (state->state == USB_POWER_STATE_OFF) {
+		state->state = USB_POWER_STATE_SETUP;
+		state->ina_count = 0;
+	}
+
+	/* Select INA to configure */
+	ina = state->ina_cfg + state->ina_count;
+
+	ina->port = cmd->addina.port;
+	ina->addr = (cmd->addina.addr) << 1;  /* 7 to 8 bit addr. */
+	ina->rs = cmd->addina.rs;
+
+	state->ina_count += 1;
+	return USB_POWER_SUCCESS;
+}
+
+static int usb_power_read(struct usb_power_config const *config)
+{
+	/*
+	 * If there is a USB packet waiting we process it and generate a
+	 * response.
+	 */
+	uint8_t count		= rx_ep_pending(config->endpoint);
+	uint8_t result		= USB_POWER_SUCCESS;
+	union usb_power_command_data *cmd =
+		(union usb_power_command_data *)config->ep->out_databuffer;
+
+	struct usb_power_state *state = config->state;
+	struct dwc_usb_ep *ep = config->ep;
+
+	/* Bytes to return */
+	int in_msgsize = 1;
+
+	if (count < 2)
+		return EC_ERROR_INVAL;
+
+	/* State machine. */
+	switch (cmd->command) {
+	case USB_POWER_CMD_RESET:
+		result = usb_power_state_reset(config);
+		break;
+
+	case USB_POWER_CMD_STOP:
+		result = usb_power_state_stop(config);
+		break;
+
+	case USB_POWER_CMD_START:
+		result = usb_power_state_start(config, cmd, count);
+		if (result == USB_POWER_SUCCESS) {
+			/* Send back actual integration time. */
+			ep->in_databuffer[1] =
+				(state->integration_us >> 0) & 0xff;
+			ep->in_databuffer[2] =
+				(state->integration_us >> 8) & 0xff;
+			ep->in_databuffer[3] =
+				(state->integration_us >> 16) & 0xff;
+			ep->in_databuffer[4] =
+				(state->integration_us >> 24) & 0xff;
+			in_msgsize += 4;
+		}
+		break;
+
+	case USB_POWER_CMD_ADDINA:
+		result = usb_power_state_addina(config, cmd, count);
+		break;
+
+	case USB_POWER_CMD_SETTIME:
+		result = usb_power_state_settime(config, cmd, count);
+		break;
+
+	case USB_POWER_CMD_NEXT:
+		if (state->state == USB_POWER_STATE_CAPTURING) {
+			int ret;
+
+			ret = usb_power_write_line(config);
+			if (ret)
+				return EC_SUCCESS;
+
+			CPRINTS("[CAP] busy");
+			result = USB_POWER_ERROR_BUSY;
+		} else {
+			CPRINTS("[STOP] Error not capturing.");
+			result = USB_POWER_ERROR_NOT_CAPTURING;
+		}
+		break;
+
+	default:
+		CPRINTS("[ERROR] Unknown command 0x%04x", (int)cmd->command);
+		result = USB_POWER_ERROR_UNKNOWN;
+		break;
+	}
+
+	/* Return result code if applicable. */
+	usb_power_map_error(0);
+	ep->in_databuffer[0] = result;
+
+	usb_write_ep(config->endpoint, in_msgsize, ep->in_databuffer);
+
+	return EC_SUCCESS;
+}
+
+
+
+/******************************************************************************
+ * INA231 interface.
+ * List the registers and fields here.
+ * TODO(nsanders): combine with the currently incompatible common INA drivers.
+ */
+
+#define INA231_REG_CONF 0
+#define INA231_REG_RSHV 1
+#define INA231_REG_BUSV 2
+#define INA231_REG_PWR  3
+#define INA231_REG_CURR 4
+#define INA231_REG_CAL  5
+#define INA231_REG_EN   6
+
+
+#define INA231_CONF_AVG(val)		(((int)(val & 0x7)) << 9)
+#define INA231_CONF_BUS_TIME(val)	(((int)(val & 0x7)) << 6)
+#define INA231_CONF_SHUNT_TIME(val)	(((int)(val & 0x7)) << 3)
+#define INA231_CONF_MODE(val)		(((int)(val & 0x7)) << 0)
+#define INA231_MODE_OFF			0x0
+#define INA231_MODE_SHUNT		0x5
+#define INA231_MODE_BUS			0x6
+#define INA231_MODE_BOTH		0x7
+
+
+
+uint16_t ina2xx_readagain(uint8_t port, uint8_t addr)
+{
+	int res;
+	uint16_t val;
+
+	res = i2c_xfer(port, addr, NULL, 0, (uint8_t *)&val, sizeof(uint16_t),
+		      I2C_XFER_SINGLE);
+	if (res) {
+		CPRINTS("INA2XX I2C readagain failed p:%d a:%02x",
+			(int)port, (int)addr);
+		return 0x0bad;
+	}
+	return (val >> 8) | ((val & 0xff) << 8);
+}
+
+
+uint16_t ina2xx_read(uint8_t port, uint8_t addr, uint8_t reg)
+{
+	int res;
+	int val;
+
+	res = i2c_read16(port, addr, reg, &val);
+	if (res) {
+		CPRINTS("INA2XX I2C read failed p:%d a:%02x, r:%02x",
+			(int)port, (int)addr, (int)reg);
+		return 0x0bad;
+	}
+	return (val >> 8) | ((val & 0xff) << 8);
+}
+
+int ina2xx_write(uint8_t port, uint8_t addr, uint8_t reg, uint16_t val)
+{
+	int res;
+	uint16_t be_val = (val >> 8) | ((val & 0xff) << 8);
+
+	res = i2c_write16(port, addr, reg, be_val);
+	if (res)
+		CPRINTS("INA2XX I2C write failed");
+	return res;
+}
+
+
+
+/******************************************************************************
+ * Background tasks
+ *
+ * Here we setup the INAs and read them at the specified interval.
+ * INA samples are stored in a ringbuffer that can be fetched using the
+ * USB commands.
+ */
+
+/* INA231 integration and averaging time presets, indexed by register value */
+static const int average_settings[] = {
+	1, 4, 16, 64, 128, 256, 512, 1024};
+static const int conversion_time_us[] =	{
+	140, 204, 332, 588, 1100, 2116, 4156, 8244};
+
+static int usb_power_init_inas(struct usb_power_config const *config)
+{
+	struct usb_power_state *state = config->state;
+	int i;
+	int shunt_time = 0;
+	int avg = 0;
+	int target_us = state->integration_us;
+
+	if (state->state != USB_POWER_STATE_SETUP) {
+		CPRINTS("[ERROR] usb_power_init_inas while not SETUP");
+		return -1;
+	}
+
+	/* Find an INA preset integration time less than specified */
+	while (shunt_time < 7) {
+		if (conversion_time_us[shunt_time + 1] > target_us)
+			break;
+		shunt_time++;
+	}
+
+	/* Find an averaging setting from the INA presets that fits. */
+	while (avg < 7) {
+		if ((conversion_time_us[shunt_time] *
+		     average_settings[avg + 1])
+		    > target_us)
+			break;
+		avg++;
+	}
+
+	state->integration_us =
+		conversion_time_us[shunt_time] * average_settings[avg];
+
+	for (i = 0; i < state->ina_count; i++) {
+		int value;
+		int ret;
+		struct usb_power_ina_cfg *ina = state->ina_cfg + i;
+
+#ifdef USB_POWER_VERBOSE
+		{
+		int conf, cal;
+
+		conf = ina2xx_read(ina->port, ina->addr, INA231_REG_CONF);
+		cal = ina2xx_read(ina->port, ina->addr, INA231_REG_CAL);
+		CPRINTS("[CAP] %d (%d,0x%02x): conf:%x, cal:%x",
+			i, ina->port, ina->addr, conf, cal);
+		}
+#endif
+		/*
+		 * Calculate INA231 Calibration register
+		 * CurrentLSB = uA per div = 80mV / (Rsh * 2^15)
+		 * CurrentLSB uA = 80000000nV / (Rsh mOhm * 0x8000)
+		 */
+		ina->scale = 80000000 / (ina->rs * 0x8000);
+
+		/*
+		 * CAL = .00512 / (CurrentLSB * Rsh)
+		 * CAL = 5120000 / (uA * mOhm)
+		 */
+		value = 5120000 / (ina->scale * ina->rs);
+		ret = ina2xx_write(ina->port, ina->addr, INA231_REG_CAL, value);
+		if (ret != EC_SUCCESS) {
+			CPRINTS("[CAP] usb_power_init_inas CAL FAIL: %d", ret);
+			return ret;
+		}
+#ifdef USB_POWER_VERBOSE
+		{
+		int actual;
+
+		actual = ina2xx_read(ina->port, ina->addr, INA231_REG_CAL);
+		CPRINTS("[CAP] scale: %d uA/div, %d uW/div, cal:%x act:%x",
+			ina->scale, ina->scale*25, value, actual);
+		}
+#endif
+		/* Conversion time, shunt + bus, set average. */
+		value = INA231_CONF_MODE(INA231_MODE_BOTH) |
+			INA231_CONF_SHUNT_TIME(shunt_time) |
+			INA231_CONF_BUS_TIME(shunt_time) |
+			INA231_CONF_AVG(avg);
+		ret = ina2xx_write(
+			ina->port, ina->addr, INA231_REG_CONF, value);
+		if (ret != EC_SUCCESS) {
+			CPRINTS("[CAP] usb_power_init_inas CONF FAIL: %d", ret);
+			return ret;
+		}
+#ifdef USB_POWER_VERBOSE
+		{
+		int actual;
+
+		actual = ina2xx_read(ina->port, ina->addr, INA231_REG_CONF);
+		CPRINTS("[CAP] %d (%d,0x%02x): conf:%x, act:%x",
+			i, ina->port, ina->addr, value, actual);
+		}
+#endif
+#ifdef USB_POWER_VERBOSE
+		{
+		int busv_mv =
+			(ina2xx_read(ina->port, ina->addr, INA231_REG_BUSV)
+			 * 125) / 100;
+
+		CPRINTS("[CAP] %d (%d,0x%02x): busv:%dmv",
+			i, ina->port, ina->addr, busv_mv);
+		}
+#endif
+		/* Initialize read from power register. This register address
+		 * will be cached and all ina2xx_readagain() calls will read
+		 * from the same address.
+		 */
+		ina2xx_read(ina->port, ina->addr, INA231_REG_PWR);
+	}
+
+	return EC_SUCCESS;
+}
+
+
+/*
+ * Read each INA's power integration measurement.
+ *
+ * INAs recall the most recent address, so no register access write is
+ * necessary, simply read 16 bits from each INA and fill the result into
+ * the power record.
+ *
+ * If the power record ringbuffer is full, fail with USB_POWER_ERROR_OVERFLOW.
+ */
+static int usb_power_get_samples(struct usb_power_config const *config)
+{
+	uint64_t time = get_time().val;
+	struct usb_power_state *state = config->state;
+	struct usb_power_report *r = (struct usb_power_report *)(
+		state->reports_data_area +
+		(USB_POWER_RECORD_SIZE(state->ina_count)
+		* state->reports_head));
+	struct usb_power_ina_cfg *inas = state->ina_cfg;
+	int i;
+
+	/* TODO(nsanders): Would we prefer to evict oldest? */
+	if (((state->reports_head + 1) % USB_POWER_MAX_CACHED(state->ina_count))
+	    == state->reports_xmit_active) {
+		CPRINTS("Overflow! h:%d a:%d t:%d (%d)",
+			state->reports_head, state->reports_xmit_active,
+			state->reports_tail,
+			USB_POWER_MAX_CACHED(state->ina_count));
+		return USB_POWER_ERROR_OVERFLOW;
+	}
+
+	r->status = USB_POWER_SUCCESS;
+	r->size = state->ina_count;
+	if (config->state->wall_offset)
+		time = time + config->state->wall_offset;
+	else
+		time -= config->state->base_time;
+	r->timestamp = time;
+
+	for (i = 0; i < state->ina_count; i++) {
+		int power;
+		struct usb_power_ina_cfg *ina = inas + i;
+
+		/* Read INA231.
+		 * ina2xx_read(ina->port, ina->addr, INA231_REG_PWR);
+		 * Readagain cached this address so we'll save an I2C
+		 * transaction.
+		 */
+		power = ina2xx_readagain(ina->port, ina->addr);
+		r->power[i] = power;
+#ifdef USB_POWER_VERBOSE
+		{
+		int current;
+		int voltage;
+		int bvoltage;
+
+		voltage = ina2xx_read(ina->port, ina->addr, INA231_REG_RSHV);
+		bvoltage = ina2xx_read(ina->port, ina->addr, INA231_REG_BUSV);
+		current = ina2xx_read(ina->port, ina->addr, INA231_REG_CURR);
+		power = ina2xx_read(ina->port, ina->addr, INA231_REG_PWR);
+		}
+		{
+		int uV = ((int)voltage * 25) / 10;
+		int mV = ((int)bvoltage * 125) / 100;
+		int uA = (uV * 1000) / ina->rs;
+		int CuA = ((int)current * ina->scale);
+		int uW = ((int)power * ina->scale*25);
+
+		CPRINTS("[CAP] %d (%d,0x%02x): %dmV / %dmO = %dmA",
+			i, ina->port, ina->addr, uV/1000, ina->rs, uA/1000);
+		CPRINTS("[CAP] %duV %dmV %duA %dCuA "
+			"%duW v:%04x, b:%04x, p:%04x",
+			uV, mV, uA, CuA, uW, voltage, bvoltage, power);
+		}
+#endif
+	}
+
+	/* Mark this slot as used. */
+	state->reports_head = (state->reports_head + 1) %
+		USB_POWER_MAX_CACHED(state->ina_count);
+
+	return EC_SUCCESS;
+}
+
+/*
+ * This function is called every [interval] uS, and reads the accumulated
+ * values of the INAs, and reschedules itself for the next interval.
+ *
+ * It will stop collecting frames if a ringbuffer overflow is
+ * detected, or a stop request is seen..
+ */
+void usb_power_deferred_cap(struct usb_power_config const *config)
+{
+	int ret;
+	uint64_t timeout = get_time().val + config->state->integration_us;
+	uint64_t timein;
+
+	/* Exit if we have stopped capturing in the meantime. */
+	if (config->state->state != USB_POWER_STATE_CAPTURING)
+		return;
+
+	/* Get samples for this timeslice */
+	ret = usb_power_get_samples(config);
+	if (ret == USB_POWER_ERROR_OVERFLOW) {
+		CPRINTS("[CAP] usb_power_deferred_cap: OVERFLOW");
+		return;
+	}
+
+	/* Calculate time remaining until next slice. */
+	timein = get_time().val;
+	if (timeout > timein)
+		timeout = timeout - timein;
+	else
+		timeout = 0;
+
+	/* Double check if we are still capturing. */
+	if (config->state->state == USB_POWER_STATE_CAPTURING)
+		hook_call_deferred(config->deferred_cap, timeout);
+}
+
diff --git a/chip/stm32/usb_power.h b/chip/stm32/usb_power.h
new file mode 100644
index 0000000000..d6306c1a05
--- /dev/null
+++ b/chip/stm32/usb_power.h
@@ -0,0 +1,362 @@
+/* Copyright 2016 The Chromium OS Authors. All rights reserved.
+ * Use of this source code is governed by a BSD-style license that can be
+ * found in the LICENSE file.
+ */
+
+#ifndef __CROS_EC_USB_POWER_H
+#define __CROS_EC_USB_POWER_H
+
+/* Power monitoring USB interface for Chrome EC */
+
+#include "compile_time_macros.h"
+#include "hooks.h"
+#include "usb_descriptor.h"
+#include "usb_dwc_hw.h"
+
+/*
+ * Command:
+ *     +--------------+-----------------------------------+
+ *     | command : 2B |					  |
+ *     +--------------+-----------------------------------+
+ *
+ *     command:	2 bytes
+ *
+ *     reset:	0x0000
+ *
+ *     +--------+
+ *     | 0x0000 |
+ *     +--------+
+ *
+ *     stop:	0x0001
+ *
+ *     +--------+
+ *     | 0x0001 |
+ *     +--------+
+ *
+ *     addina:	0x0002
+ *
+ *     +--------+--------------------------+-------------+--------------+-----------+-------------+--------+
+ *     | 0x0002 | 1B: 4b: extender 4b: bus | 1B:INA type | 1B: INA addr | 1B: extra | 4B: voltage | 4B: Rs |
+ *     +--------+--------------------------+-------------+--------------+-----------+-------------+--------+
+ *
+ *     start:	0x0003
+ *
+ *     +--------+----------------------+
+ *     | 0x0003 | 4B: integration time |
+ *     +--------+----------------------+
+ *
+ *     next:	0x0004
+ *
+ *     +--------+
+ *     | 0x0004 |
+ *     +--------+
+ *
+ *     settime:	0x0005
+ *
+ *     +--------+---------------------+
+ *     | 0x0005 | 8B: Wall clock time |
+ *     +--------+---------------------+
+ *
+ *
+ *
+ * Response:
+ *     +-------------+----------+----------------+------------------+
+ *     | status : 1B | size: 1B | timestamp : 8B | payload : <= 58B | Pad to multiple of 4 byte.
+ *     +-------------+----------+----------------+------------------+
+ *
+ *     status: 1 byte status
+ *	 0x00: Success
+ *	 0x01: I2C Error
+ *	 0x02: Overflow
+ *	     This can happen if data acquisition is faster than USB reads.
+ *	 0x03: No configuration set.
+ *	 0x04: No active capture.
+ *	 0x05: Timeout.
+ *	 0x06: Busy, outgoing queue is empty.
+ *	 0x07: Size, command length is incorrect for command type..
+ *	 0x08: More INAs specified than board limit.
+ *	 0x80: Unknown error
+ *
+ *     size: 1 byte incoming INA reads count
+ *
+ *     timestamp: 4 byte timestamp associated with these samples
+ *
+ *     read payload: up to 58 bytes of data, 29x INA reads of current
+ *
+ */
+
+enum usb_power_error {
+	USB_POWER_SUCCESS		= 0x00,
+	USB_POWER_ERROR_I2C		= 0x01,
+	USB_POWER_ERROR_OVERFLOW	= 0x02,
+	USB_POWER_ERROR_NOT_SETUP	= 0x03,
+	USB_POWER_ERROR_NOT_CAPTURING	= 0x04,
+	USB_POWER_ERROR_TIMEOUT		= 0x05,
+	USB_POWER_ERROR_BUSY		= 0x06,
+	USB_POWER_ERROR_READ_SIZE	= 0x07,
+	USB_POWER_ERROR_FULL		= 0x08,
+	USB_POWER_ERROR_UNKNOWN		= 0x80,
+};
+
+enum usb_power_command {
+	USB_POWER_CMD_RESET	= 0x0000,
+	USB_POWER_CMD_STOP	= 0x0001,
+	USB_POWER_CMD_ADDINA	= 0x0002,
+	USB_POWER_CMD_START	= 0x0003,
+	USB_POWER_CMD_NEXT	= 0x0004,
+	USB_POWER_CMD_SETTIME	= 0x0005,
+};
+
+enum usb_power_states {
+	USB_POWER_STATE_OFF	= 0,
+	USB_POWER_STATE_SETUP,
+	USB_POWER_STATE_CAPTURING,
+};
+
+#define USB_POWER_MAX_READ_COUNT 64
+#define USB_POWER_MIN_CACHED 10
+
+struct usb_power_ina_cfg {
+	/*
+	 * Relevant config for INA usage.
+	 */
+	/* i2c bus. TODO(nsanders): specify what kind of index. */
+	int port;
+	/* 7-bit i2c addr */
+	int addr;
+
+	/* Base voltage. mV */
+	int mv;
+
+	/* Shunt resistor. mOhm */
+	int rs;
+	/* uA per div as reported from INA */
+	int scale;
+};
+
+
+struct __attribute__ ((__packed__)) usb_power_report {
+	uint8_t status;
+	uint8_t size;
+	uint64_t timestamp;
+	uint16_t power[USB_POWER_MAX_READ_COUNT];
+};
+
+/* Must be 4 byte aligned */
+#define USB_POWER_RECORD_SIZE(ina_count)				\
+	((((sizeof(struct usb_power_report)				\
+	- (sizeof(uint16_t) * USB_POWER_MAX_READ_COUNT)			\
+	+ (sizeof(uint16_t) * (ina_count))) + 3) / 4) * 4)
+
+#define USB_POWER_DATA_SIZE						\
+	(sizeof(struct usb_power_report) * (USB_POWER_MIN_CACHED + 1))
+#define USB_POWER_MAX_CACHED(ina_count)					\
+	(USB_POWER_DATA_SIZE / USB_POWER_RECORD_SIZE(ina_count))
+
+
+struct usb_power_state {
+	/*
+	 * The power data acquisition must be setup, then started, in order to
+	 * return data.
+	 * States are OFF, SETUP, and CAPTURING.
+	 */
+	int state;
+
+	struct usb_power_ina_cfg ina_cfg[USB_POWER_MAX_READ_COUNT];
+	int ina_count;
+	int integration_us;
+	/* Start of sampling. */
+	uint64_t base_time;
+	/* Offset between microcontroller timestamp and host wall clock. */
+	uint64_t wall_offset;
+
+	/* Cached power reports for sending on USB. */
+	/* Actual backing data for variable sized record queue. */
+	uint8_t reports_data_area[USB_POWER_DATA_SIZE];
+	/* Size of power report struct for this config. */
+	int stride_bytes;
+	/* Max power records storeable in this config */
+	int max_cached;
+	struct usb_power_report *reports;
+
+	/* Head and tail pointers for output ringbuffer */
+	/* Head adds newly probed power data. */
+	int reports_head;
+	/* Tail contains oldest records not yet sent to USB */
+	int reports_tail;
+	/* Xmit_active -> tail is active usb DMA */
+	int reports_xmit_active;
+
+	/* Pointers to RAM. */
+	uint8_t rx_buf[USB_MAX_PACKET_SIZE];
+	uint8_t tx_buf[USB_MAX_PACKET_SIZE * 4];
+};
+
+
+/*
+ * Compile time Per-USB gpio configuration stored in flash.  Instances of this
+ * structure are provided by the user of the USB gpio.  This structure binds
+ * together all information required to operate a USB gpio.
+ */
+struct usb_power_config {
+	/* In RAM state of the USB power interface. */
+	struct usb_power_state *state;
+
+	/* USB endpoint state.*/
+	struct dwc_usb_ep *ep;
+
+	/* Interface and endpoint indicies. */
+	int interface;
+	int endpoint;
+
+	/* Deferred function to call to handle power request. */
+	const struct deferred_data *deferred;
+	const struct deferred_data *deferred_cap;
+};
+
+struct __attribute__ ((__packed__)) usb_power_command_start {
+	uint16_t command;
+	uint32_t integration_us;
+};
+
+struct __attribute__ ((__packed__)) usb_power_command_addina {
+	uint16_t command;
+	uint8_t port;
+	uint8_t type;
+	uint8_t addr;
+	uint8_t extra;
+	uint32_t rs;
+};
+
+struct __attribute__ ((__packed__)) usb_power_command_settime {
+	uint16_t command;
+	uint64_t time;
+};
+
+union usb_power_command_data {
+	uint16_t command;
+	struct usb_power_command_start start;
+	struct usb_power_command_addina addina;
+	struct usb_power_command_settime settime;
+};
+
+
+/*
+ * Convenience macro for defining a USB INA Power driver.
+ *
+ * NAME is used to construct the names of the trampoline functions and the
+ * usb_power_config struct, the latter is just called NAME.
+ *
+ * INTERFACE is the index of the USB interface to associate with this
+ * driver.
+ *
+ * ENDPOINT is the index of the USB bulk endpoint used for receiving and
+ * transmitting bytes.
+ */
+#define USB_POWER_CONFIG(NAME,						\
+		       INTERFACE,					\
+		       ENDPOINT)					\
+	static void CONCAT2(NAME, _deferred_tx_)(void);			\
+	DECLARE_DEFERRED(CONCAT2(NAME, _deferred_tx_));			\
+	static void CONCAT2(NAME, _deferred_rx_)(void);			\
+	DECLARE_DEFERRED(CONCAT2(NAME, _deferred_rx_));			\
+	static void CONCAT2(NAME, _deferred_cap_)(void);		\
+	DECLARE_DEFERRED(CONCAT2(NAME, _deferred_cap_));		\
+	struct usb_power_state CONCAT2(NAME, _state_) = {		\
+		.state = USB_POWER_STATE_OFF,				\
+		.ina_count = 0,						\
+		.integration_us = 0,					\
+		.reports_head = 0,					\
+		.reports_tail = 0,					\
+		.wall_offset = 0,					\
+	};								\
+	static struct dwc_usb_ep CONCAT2(NAME, _ep_ctl) = {		\
+		.max_packet = USB_MAX_PACKET_SIZE,			\
+		.tx_fifo = ENDPOINT,					\
+		.out_pending = 0,					\
+		.out_data = 0,						\
+		.out_databuffer = 0,					\
+		.out_databuffer_max = 0,				\
+		.rx_deferred = &CONCAT2(NAME, _deferred_rx__data),	\
+		.in_packets = 0,					\
+		.in_pending = 0,					\
+		.in_data = 0,						\
+		.in_databuffer = 0,					\
+		.in_databuffer_max = 0,					\
+		.tx_deferred = &CONCAT2(NAME, _deferred_tx__data),	\
+	};								\
+	struct usb_power_config const NAME = {				\
+		.state     = &CONCAT2(NAME, _state_),			\
+		.ep	= &CONCAT2(NAME, _ep_ctl),			\
+		.interface = INTERFACE,					\
+		.endpoint  = ENDPOINT,					\
+		.deferred_cap  = &CONCAT2(NAME, _deferred_cap__data),	\
+	};								\
+	const struct usb_interface_descriptor				\
+	USB_IFACE_DESC(INTERFACE) = {					\
+		.bLength	    = USB_DT_INTERFACE_SIZE,		\
+		.bDescriptorType    = USB_DT_INTERFACE,			\
+		.bInterfaceNumber   = INTERFACE,			\
+		.bAlternateSetting  = 0,				\
+		.bNumEndpoints      = 2,				\
+		.bInterfaceClass    = USB_CLASS_VENDOR_SPEC,		\
+		.bInterfaceSubClass = USB_SUBCLASS_GOOGLE_POWER,	\
+		.bInterfaceProtocol = USB_PROTOCOL_GOOGLE_POWER,	\
+		.iInterface	 = 0,				\
+	};								\
+	const struct usb_endpoint_descriptor				\
+	USB_EP_DESC(INTERFACE, 0) = {					\
+		.bLength	  = USB_DT_ENDPOINT_SIZE,		\
+		.bDescriptorType  = USB_DT_ENDPOINT,			\
+		.bEndpointAddress = 0x80 | ENDPOINT,			\
+		.bmAttributes     = 0x02 /* Bulk IN */,			\
+		.wMaxPacketSize   = USB_MAX_PACKET_SIZE,		\
+		.bInterval	= 1,					\
+	};								\
+	const struct usb_endpoint_descriptor				\
+	USB_EP_DESC(INTERFACE, 1) = {					\
+		.bLength	  = USB_DT_ENDPOINT_SIZE,		\
+		.bDescriptorType  = USB_DT_ENDPOINT,			\
+		.bEndpointAddress = ENDPOINT,				\
+		.bmAttributes     = 0x02 /* Bulk OUT */,		\
+		.wMaxPacketSize   = USB_MAX_PACKET_SIZE,		\
+		.bInterval	= 0,					\
+	};								\
+	static void CONCAT2(NAME, _ep_tx_)   (void) { usb_epN_tx(ENDPOINT); } \
+	static void CONCAT2(NAME, _ep_rx_)   (void) { usb_epN_rx(ENDPOINT); } \
+	static void CONCAT2(NAME, _ep_reset_)(void)			\
+	{								\
+			usb_power_reset(&NAME);				\
+	}								\
+	USB_DECLARE_EP(ENDPOINT,					\
+		       CONCAT2(NAME, _ep_tx_),				\
+		       CONCAT2(NAME, _ep_rx_),				\
+		       CONCAT2(NAME, _ep_reset_));			\
+	static void CONCAT2(NAME, _deferred_tx_)(void)			\
+	{ usb_power_deferred_tx(&NAME); }				\
+	static void CONCAT2(NAME, _deferred_rx_)(void)			\
+	{ usb_power_deferred_rx(&NAME); }				\
+	static void CONCAT2(NAME, _deferred_cap_)(void)			\
+	{ usb_power_deferred_cap(&NAME); }
+
+
+/*
+ * Handle power request in a deferred callback.
+ */
+void usb_power_deferred_rx(struct usb_power_config const *config);
+void usb_power_deferred_tx(struct usb_power_config const *config);
+void usb_power_deferred_cap(struct usb_power_config const *config);
+
+/*
+ * These functions are used by the trampoline functions defined above to
+ * connect USB endpoint events with the generic USB GPIO driver.
+ */
+void usb_power_tx(struct usb_power_config const *config);
+void usb_power_rx(struct usb_power_config const *config);
+void usb_power_reset(struct usb_power_config const *config);
+
+
+
+
+#endif /* __CROS_EC_USB_DWC_POWER_H */
+
diff --git a/include/config.h b/include/config.h
index aaf575fe7f..a87cdcda6e 100644
--- a/include/config.h
+++ b/include/config.h
@@ -2336,6 +2336,11 @@
 #undef CONFIG_USB_I2C
 
 /*****************************************************************************/
+/* USB Power monitoring interface config */
+#undef CONFIG_USB_POWER
+
+
+/*****************************************************************************/
 
 /* Support computing hash of code for verified boot */
 #undef CONFIG_VBOOT_HASH
diff --git a/include/usb_descriptor.h b/include/usb_descriptor.h
index 1920db7938..7cb64e35aa 100644
--- a/include/usb_descriptor.h
+++ b/include/usb_descriptor.h
@@ -190,6 +190,9 @@ struct usb_endpoint_descriptor {
 /* We can use any protocol we want */
 #define USB_PROTOCOL_GOOGLE_CR50_NON_HC_FW_UPDATE 0xff
 
+#define USB_SUBCLASS_GOOGLE_POWER  0x54
+#define USB_PROTOCOL_GOOGLE_POWER  0x01
+
 /* Control requests */
 
 /* bRequestType fields */