From 9a96a54561edcf7b1128742d1e4cc0b2986cc19c Mon Sep 17 00:00:00 2001 From: "Luigi F. Cruz" Date: Mon, 10 Jul 2023 20:57:34 -0300 Subject: [PATCH] Add initial USB-C PD code. --- apps/CMakeLists.txt | 3 +- apps/usb_power_delivery/CMakeLists.txt | 14 + apps/usb_power_delivery/test.c | 73 +++++ lib/CMakeLists.txt | 2 + lib/fusb/CMakeLists.txt | 11 + lib/fusb/fusb.h | 333 +++++++++++++++++++ lib/usb_pd/CMakeLists.txt | 10 + lib/usb_pd/usb_pd.h | 438 +++++++++++++++++++++++++ 8 files changed, 883 insertions(+), 1 deletion(-) create mode 100644 apps/usb_power_delivery/CMakeLists.txt create mode 100644 apps/usb_power_delivery/test.c create mode 100644 lib/fusb/CMakeLists.txt create mode 100644 lib/fusb/fusb.h create mode 100644 lib/usb_pd/CMakeLists.txt create mode 100644 lib/usb_pd/usb_pd.h diff --git a/apps/CMakeLists.txt b/apps/CMakeLists.txt index 478bd0a..e0d1dd4 100644 --- a/apps/CMakeLists.txt +++ b/apps/CMakeLists.txt @@ -4,4 +4,5 @@ add_subdirectory(adc_dma_chain) add_subdirectory(piccolosdr) add_subdirectory(barometer) add_subdirectory(filesystem) -add_subdirectory(altimeter) \ No newline at end of file +add_subdirectory(altimeter) +add_subdirectory(usb_power_delivery) diff --git a/apps/usb_power_delivery/CMakeLists.txt b/apps/usb_power_delivery/CMakeLists.txt new file mode 100644 index 0000000..16cdead --- /dev/null +++ b/apps/usb_power_delivery/CMakeLists.txt @@ -0,0 +1,14 @@ +cmake_minimum_required(VERSION 3.12) + +project(pico-usbpd) + +add_executable(usbpd_example test.c) + +target_link_libraries(usbpd_example LINK_PUBLIC fusb usbpd) + +pico_add_extra_outputs(usbpd_example) + +pico_enable_stdio_usb(usbpd_example 1) +pico_enable_stdio_uart(usbpd_example 0) + +set(CMAKE_EXPORT_COMPILE_COMMANDS ON) diff --git a/apps/usb_power_delivery/test.c b/apps/usb_power_delivery/test.c new file mode 100644 index 0000000..1fb651d --- /dev/null +++ b/apps/usb_power_delivery/test.c @@ -0,0 +1,73 @@ +#include +#include + +#include "pico/stdio.h" +#include "pico/stdlib.h" + +#define DEBUG 1 +#include "fusb.h" +#include "usb_pd.h" + +int main(void) { + stdio_init_all(); + + sleep_ms(2500); + + printf("Hello from Pi Pico!\n"); + + fusb_t fusb; + fusb.i2c.addr = 0x25; + fusb.i2c.inst = i2c1; + fusb.i2c.rate = 1000000; + fusb.i2c.scl = 18; + fusb.i2c.sda = 19; + + usbpd_t usbpd; + + printf("Starting FUSB302...\n"); + if (!fusb_init(&fusb)) { + return 1; + } + + int i = 0; + while (1) { + while (fusb.packets == 0) { + sleep_ms(15); + } + fusb.packets -= 1; + + uint8_t sop[4]; + fusb_read_fifo(&fusb, 1, &sop); + + if (sop[0] == 0xe0) { + fusb_read_fifo(&fusb, 2, &sop); + usb_pd_parse_header(&usbpd, sop); + for (uint16_t i = 0; i < usbpd.number_of_data_objects; i++) { + fusb_read_fifo(&fusb, 4, &sop); + usb_pd_parse_pdo(&usbpd, sop); + } + fusb_read_fifo(&fusb, 4, &sop); + + if (usbpd.last_msg_kind == USBPD_MSG_KIND_DATA && + usbpd.last_msg_type == USBPD_DATA_MSG_SOURCE_CAPABILITIES) { + uint8_t payload[6]; + usb_pd_generate_header(&usbpd, + payload, + 1, + 0, + USBPD_POWER_PORT_ROLE_SINK, + USBPD_SPEC_REVISION_3, + USBPD_DATA_PORT_ROLE_UPSTREAM, + USBPD_DATA_MSG_REQUEST); + usb_pd_generate_rdo_fvs(&usbpd, payload + 2, 2, false, false, false, false, false, false, 1000, 1000); + // usb_pd_generate_rdo_pps(&usbpd, payload + 2, 6, false, false, false, false, false, 11000, 0); + fusb_write_fifo(&fusb, 6, payload); + } + } + + } + + printf("Goodbye!\n"); + + return 0; +} diff --git a/lib/CMakeLists.txt b/lib/CMakeLists.txt index f9cce72..a27d921 100644 --- a/lib/CMakeLists.txt +++ b/lib/CMakeLists.txt @@ -2,3 +2,5 @@ add_subdirectory(bmp180) add_subdirectory(bmp390) add_subdirectory(usb_network_stack) add_subdirectory(littlefs) +add_subdirectory(fusb) +add_subdirectory(usb_pd) diff --git a/lib/fusb/CMakeLists.txt b/lib/fusb/CMakeLists.txt new file mode 100644 index 0000000..c440b6c --- /dev/null +++ b/lib/fusb/CMakeLists.txt @@ -0,0 +1,11 @@ +cmake_minimum_required(VERSION 3.12) + +add_library(fusb fusb.h) + +target_link_libraries(fusb + pico_stdlib + pico_stdio + hardware_i2c +) + +target_include_directories(fusb PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/lib/fusb/fusb.h b/lib/fusb/fusb.h new file mode 100644 index 0000000..5404b32 --- /dev/null +++ b/lib/fusb/fusb.h @@ -0,0 +1,333 @@ +#ifndef FUSB_H +#define FUSB_H + +#include +#include +#include +#include + +#include "pico/stdio.h" +#include "pico/stdlib.h" +#include "hardware/i2c.h" + +typedef struct { + int addr; + int rate; + int scl; + int sda; + i2c_inst_t* inst; +} i2c_t; + +typedef struct { + i2c_t i2c; + bool source; + uint8_t revision; + bool auto_crc; + uint8_t polarity; + uint16_t packets; +} fusb_t; + +static fusb_t* ff; + +#define ASSERT_OK(X) { if (X == false) return false; }; + +void read_i2c(fusb_t* fusb, uint32_t size, uint8_t addr, void* data) { + i2c_write_blocking(fusb->i2c.inst, fusb->i2c.addr, &addr, 1, true); + i2c_read_blocking(fusb->i2c.inst, fusb->i2c.addr, data, size, false); +} + +void write_reg_i2c(fusb_t* fusb, uint8_t addr, uint8_t reg) { + uint8_t payload[] = { addr, reg }; + i2c_write_blocking(fusb->i2c.inst, fusb->i2c.addr, payload, 2, false); +} + +void write_i2c(fusb_t* fusb, uint32_t size, void* data) { + i2c_write_blocking(fusb->i2c.inst, fusb->i2c.addr, data, size, false); +} + +bool fusb_check_chip_id(fusb_t* fusb) { + uint8_t chip_id; + read_i2c(fusb, 1, 0x01, &chip_id); +#ifdef DEBUG + printf("[FUSB] Chip ID: 0x%x\n", chip_id); +#endif + return true; +} + +bool fusb_reset(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Device reset.\n"); +#endif + uint8_t reg; + read_i2c(fusb, 1, 0x0c, ®); + reg |= 0x01; + write_reg_i2c(fusb, 0x0c, reg); + return true; +} + +bool fusb_pd_reset(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Power delivery reset.\n"); +#endif + uint8_t reg; + read_i2c(fusb, 1, 0x0c, ®); + reg |= 0x02; + write_reg_i2c(fusb, 0x0c, reg); + return true; +} + +bool fusb_power_on(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Power device on.\n"); +#endif + uint8_t reg; + read_i2c(fusb, 1, 0x0b, ®); + reg |= 0x0f; + write_reg_i2c(fusb, 0x0b, reg); + return true; +} + +bool fusb_config(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Configuring device.\n"); +#endif + write_reg_i2c(fusb, 0x0a, 0b01101111); + write_reg_i2c(fusb, 0x0e, 0b10111110); + write_reg_i2c(fusb, 0x0f, 0b00000001); + write_reg_i2c(fusb, 0x06, 0b00000100); + write_reg_i2c(fusb, 0x08, 0b00000100); + write_reg_i2c(fusb, 0x09, 0b00000111); + return true; +} + +bool fusb_pd_config(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Configuring PD.\n"); +#endif + uint8_t cmd = 0x00; + + if (fusb->source) { + cmd |= 0b10010000; + } + + cmd |= fusb->revision << 5; + + if (fusb->auto_crc) { + cmd |= 0b00000100; + } + + cmd |= fusb->polarity; + + write_reg_i2c(fusb, 0x03, cmd); + + return true; +} + +bool fusb_flush_rx_fifo(fusb_t* fusb) { + uint8_t reg; + read_i2c(fusb, 1, 0x07, ®); + reg |= 0x04; + write_reg_i2c(fusb, 0x07, reg); + return true; +} + +bool fusb_flush_tx_fifo(fusb_t* fusb) { + uint8_t reg; + read_i2c(fusb, 1, 0x06, ®); + reg |= 0x40; + write_reg_i2c(fusb, 0x06, reg); + return true; +} + +bool fusb_connect_cc(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Connecting CC.\n"); +#endif + uint8_t reg; + read_i2c(fusb, 1, 0x02, ®); + reg &= (~0b1100 & 0xFF); + reg |= (fusb->polarity << 2); + write_reg_i2c(fusb, 0x02, reg); + return true; +} + +bool fusb_pd_enable_toggle(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Toggle enabled.\n"); +#endif + uint8_t reg; + read_i2c(fusb, 1, 0x08, ®); + reg |= 0x01; + write_reg_i2c(fusb, 0x08, reg); + return true; +} + +bool fusb_pd_disable_toggle(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Toggle disabled.\n"); +#endif + uint8_t reg; + read_i2c(fusb, 1, 0x08, ®); + reg &= ~0x01; + write_reg_i2c(fusb, 0x08, reg); + return true; +} + +bool fusb_pd_handle_toggle_snk(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Handle toggle sink.\n"); +#endif + // Set polarity and pull. + ASSERT_OK(fusb_connect_cc(fusb)); + ASSERT_OK(fusb_pd_config(fusb)); + + // Check if CC is still present. + uint8_t reg; + read_i2c(fusb, 1, 0x40, ®); + if (!(reg & 0x03)) { + printf("Restarting toggling.\n"); + fusb_pd_enable_toggle(fusb); + return true; + } + + // Disable toggle. + fusb_pd_disable_toggle(ff); + + return true; +} + +void i2c_isr(unsigned int gpio, uint32_t events) { + uint8_t reg[7]; + read_i2c(ff, 7, 0x3c, ®); + +#define TRACE_DEBUG + +#ifdef TRACE_DEBUG + printf("0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x 0x%02x\n", + reg[0], reg[1], reg[2], reg[3], reg[4], reg[5], reg[6]); + + printf("I_HARDRESET %x\n", (reg[2] & 0x01) >> 0); + printf("HARDRESET %x\n", (reg[0] & 0x01) >> 0); + + printf("I_TOGDONE %x\n", (reg[2] & 0x40) >> 6); + printf("TOGSS %x\n", (reg[1] & 0x38) >> 3); + + printf("I_COMP_CHNG %x\n", (reg[6] & 0x20) >> 5); + printf("COMP %x\n", (reg[4] & 0x20) >> 5); +#endif + + if (reg[2] & 0x40) { + switch ((reg[1] & 0x38) >> 3) { + case 0b101: +#ifdef DEBUG + printf("[FUSB] Sink on CC1 attached.\n"); +#endif + ff->polarity = 0x01; + fusb_pd_handle_toggle_snk(ff); + break; + case 0b110: +#ifdef DEBUG + printf("[FUSB] Sink on CC2 attached.\n"); +#endif + ff->polarity = 0x02; + fusb_pd_handle_toggle_snk(ff); + break; + default: + printf("[FUSB] Not defined toggle state detected.\n"); + break; + } + } + + if ((reg[2] & 0x01) && (reg[0] & 0x01)) { +#ifdef DEBUG + printf("[FUSB] Hard reset.\n"); +#endif + if (!ff->packets) { + fusb_pd_reset(ff); + } + } + + if (reg[6] & 0x10) { +#ifdef DEBUG + printf("[FUSB] New packet (%d).\n", ff->packets); +#endif + ff->packets += 1; + } + + if ((reg[6] & 0x80) && !(reg[4] & 0x80)) { +#ifdef DEBUG + printf("[FUSB] Detach occured.\n"); +#endif + ff->polarity = 0x00; + fusb_connect_cc(ff); + fusb_pd_config(ff); + fusb_flush_tx_fifo(ff); + fusb_flush_rx_fifo(ff); + fusb_pd_reset(ff); + fusb_pd_enable_toggle(ff); + } +} + +bool fusb_init(fusb_t* fusb) { +#ifdef DEBUG + printf("[FUSB] Connecting device.\n"); +#endif + // Initializing the I2C. + i2c_init(fusb->i2c.inst, fusb->i2c.rate); + + // Configuring the I2C pins. + gpio_set_function(fusb->i2c.scl, GPIO_FUNC_I2C); + gpio_set_function(fusb->i2c.sda, GPIO_FUNC_I2C); + gpio_pull_up(fusb->i2c.scl); + gpio_pull_up(fusb->i2c.sda); + + // Setting up the interrupt handler. + gpio_set_irq_enabled_with_callback(20, GPIO_IRQ_EDGE_FALL, true, &i2c_isr); + ff = fusb; + +#ifdef DEBUG + printf("[FUSB] Initializing device.\n"); +#endif + + fusb->auto_crc = true; + fusb->revision = 0x10; + fusb->source = false; + fusb->packets = 0; + + ASSERT_OK(fusb_check_chip_id(fusb)); + ASSERT_OK(fusb_reset(fusb)); + ASSERT_OK(fusb_check_chip_id(fusb)); + + ASSERT_OK(fusb_config(fusb)); + ASSERT_OK(fusb_power_on(fusb)); + ASSERT_OK(fusb_flush_tx_fifo(fusb)); + ASSERT_OK(fusb_flush_rx_fifo(fusb)); + + ASSERT_OK(fusb_pd_enable_toggle(fusb)); + + return true; +} + +bool fusb_read_fifo(fusb_t* fusb, uint32_t size, void* data) { + read_i2c(fusb, size, 0x43, data); + return true; +} + +bool fusb_write_fifo(fusb_t* fusb, uint32_t size, void* data) { +#ifdef DEBUG + printf("[FUSB] Writing FIFO (size = %d).\n", size); +#endif + uint8_t sop[] = { 0x43, 0x12, 0x12, 0x12, 0x13, 0x80 | size }; + write_i2c(fusb, 6, sop); + + uint8_t payload[70] = { 0x43 }; + memcpy(payload + 1, data, size); + write_i2c(fusb, size + 1, payload); + + uint8_t eop[] = { 0x43, 0xff, 0x14, 0xfe, 0xa1 }; + write_i2c(fusb, 5, eop); + + return true; +} + +#endif diff --git a/lib/usb_pd/CMakeLists.txt b/lib/usb_pd/CMakeLists.txt new file mode 100644 index 0000000..cece10d --- /dev/null +++ b/lib/usb_pd/CMakeLists.txt @@ -0,0 +1,10 @@ +cmake_minimum_required(VERSION 3.12) + +add_library(usbpd usb_pd.h) + +target_link_libraries(usbpd + pico_stdlib + pico_stdio +) + +target_include_directories(usbpd PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/lib/usb_pd/usb_pd.h b/lib/usb_pd/usb_pd.h new file mode 100644 index 0000000..03d42ec --- /dev/null +++ b/lib/usb_pd/usb_pd.h @@ -0,0 +1,438 @@ +#ifndef USB_PD_H +#define USB_PD_H + +#include +#include +#include +#include + +#include "pico/stdio.h" +#include "pico/stdlib.h" + +typedef struct { + uint16_t number_of_data_objects; + uint16_t last_msg_kind; + uint16_t last_msg_type; +} usbpd_t; + +#define ASSERT_OK(X) { if (X == false) return false; }; + +#define USBPD_MSG_KIND_DATA 0b10 +#define USBPD_MSG_KIND_CONTROL 0b01 + +#define USBPD_CONTROL_MSG_GOODCRC 0b0001 +#define USBPD_CONTROL_MSG_GOTOMIN 0b0010 +#define USBPD_CONTROL_MSG_ACCEPT 0b0011 +#define USBPD_CONTROL_MSG_REJECT 0b0100 +#define USBPD_CONTROL_MSG_PING 0b0101 +#define USBPD_CONTROL_MSG_PS_RDY 0b0110 +#define USBPD_CONTROL_MSG_GET_SOURCE_CAP 0b0111 +#define USBPD_CONTROL_MSG_GET_SINK_CAP 0b1000 +#define USBPD_CONTROL_MSG_DR_SWAP 0b1001 +#define USBPD_CONTROL_MSG_PR_SWAP 0b1010 +#define USBPD_CONTROL_MSG_VCONN_SWAP 0b1011 +#define USBPD_CONTROL_MSG_WAIT 0b1100 +#define USBPD_CONTROL_MSG_SOFT_RESET 0b1101 +#define USBPD_CONTROL_MSG_NOT_SUPPORTED 0b0000 + +const char* get_control_message_type_name(int message_type) { + switch (message_type) { + case USBPD_CONTROL_MSG_GOODCRC: return "GoodCRC"; + case USBPD_CONTROL_MSG_GOTOMIN: return "GotoMin"; + case USBPD_CONTROL_MSG_ACCEPT: return "Accept"; + case USBPD_CONTROL_MSG_REJECT: return "Reject"; + case USBPD_CONTROL_MSG_PING: return "Ping"; + case USBPD_CONTROL_MSG_PS_RDY: return "PS_RDY"; + case USBPD_CONTROL_MSG_GET_SOURCE_CAP: return "Get_Source_Cap"; + case USBPD_CONTROL_MSG_GET_SINK_CAP: return "Get_Sink_Cap"; + case USBPD_CONTROL_MSG_DR_SWAP: return "DR_Swap"; + case USBPD_CONTROL_MSG_PR_SWAP: return "PR_Swap"; + case USBPD_CONTROL_MSG_VCONN_SWAP: return "VCONN_Swap"; + case USBPD_CONTROL_MSG_WAIT: return "Wait"; + case USBPD_CONTROL_MSG_SOFT_RESET: return "Soft_Reset"; + case USBPD_CONTROL_MSG_NOT_SUPPORTED: return "Not Supported"; + default: return "Unknown"; + } +} + +#define USBPD_DATA_MSG_SOURCE_CAPABILITIES 0b0001 +#define USBPD_DATA_MSG_REQUEST 0b0010 +#define USBPD_DATA_MSG_BIST 0b0011 +#define USBPD_DATA_MSG_SINK_CAPABILITIES 0b0100 +#define USBPD_DATA_MSG_BATTERY_STATUS 0b0101 +#define USBPD_DATA_MSG_ALERT 0b0110 +#define USBPD_DATA_MSG_GET_COUNTRY_INFO 0b0111 +#define USBPD_DATA_MSG_COUNTRY_INFO 0b1000 +#define USBPD_DATA_MSG_VENDOR_DEFINED 0b1111 + +const char* get_data_message_type_name(int message_type) { + switch (message_type) { + case USBPD_DATA_MSG_SOURCE_CAPABILITIES: return "Source_Capabilities"; + case USBPD_DATA_MSG_REQUEST: return "Request"; + case USBPD_DATA_MSG_BIST: return "BIST"; + case USBPD_DATA_MSG_SINK_CAPABILITIES: return "Sink_Capabilities"; + case USBPD_DATA_MSG_BATTERY_STATUS: return "Battery_Status"; + case USBPD_DATA_MSG_ALERT: return "Alert"; + case USBPD_DATA_MSG_GET_COUNTRY_INFO: return "Get_Country_Info"; + case USBPD_DATA_MSG_COUNTRY_INFO: return "Country_Info"; + case USBPD_DATA_MSG_VENDOR_DEFINED: return "Vendor_Defined"; + default: return "Unknown"; + } +} + +#define USBPD_SPEC_REVISION_1 0b00 +#define USBPD_SPEC_REVISION_2 0b01 +#define USBPD_SPEC_REVISION_3 0b10 + +const char* get_specification_revision_name(int revision) { + switch (revision) { + case USBPD_SPEC_REVISION_1: return "Revision_1"; + case USBPD_SPEC_REVISION_2: return "Revision_2"; + case USBPD_SPEC_REVISION_3: return "Revision_3"; + default: return "Unknown"; + } +} + +#define USBPD_DATA_PORT_ROLE_UPSTREAM 0b0 +#define USBPD_DATA_PORT_ROLE_DOWNSTREAM 0b1 + +const char* get_data_port_role_name(int role) { + switch (role) { + case USBPD_DATA_PORT_ROLE_UPSTREAM: return "Upstream"; + case USBPD_DATA_PORT_ROLE_DOWNSTREAM: return "Downstream"; + default: return "Unknown"; + } +} + +#define USBPD_POWER_PORT_ROLE_SINK 0b0 +#define USBPD_POWER_PORT_ROLE_SOURCE 0b1 + +const char* get_power_port_role_name(int role) { + switch (role) { + case USBPD_POWER_PORT_ROLE_SINK: return "Sink"; + case USBPD_POWER_PORT_ROLE_SOURCE: return "Source"; + default: return "Unknown"; + } +} + +void split_to_bytes(uint64_t num, uint8_t n_bytes, uint8_t* byte_array) { + for (int i = 0; i < n_bytes; i++) { + byte_array[i] = num & 0xFF; + num >>= 8; + } +} + +bool usb_pd_parse_header(usbpd_t* usbpd, uint8_t* header_bytes) { + uint16_t header = (header_bytes[1] << 8) | header_bytes[0]; + + uint16_t extended = (header >> 15) & 0b1; + uint16_t num_data_objects = (header >> 12) & 0b1111; + uint16_t message_id = (header >> 9) & 0b111; + uint16_t power_role = (header >> 8) & 0b1; + uint16_t specification_revision = (header >> 6) & 0b11; + uint16_t data_role = (header >> 5) & 0b1; + uint16_t message_type = header & 0b1111; + +#ifdef DEBUG + const char* message_type_name; + const char* specification_revision_name = get_specification_revision_name(specification_revision); + const char* data_port_role_name = get_data_port_role_name(data_role); + const char* power_port_role_name = get_power_port_role_name(power_role); + + if (num_data_objects == 0) { + usbpd->last_msg_kind = USBPD_MSG_KIND_CONTROL; + message_type_name = get_control_message_type_name(message_type); + } else { + usbpd->last_msg_kind = USBPD_MSG_KIND_DATA; + message_type_name = get_data_message_type_name(message_type); + } + + printf("=== USB-PD Message Header ================\n"); + printf("Message Kind: %s\n", num_data_objects == 0 ? "Control" : "Data"); + printf("Message Type: %s (%d)\n", message_type_name, message_type); + printf("Message ID: %d\n", message_id); + printf("Number of Data Objects: %d\n", num_data_objects); + printf("Specification Revision: %s (%d)\n", specification_revision_name, specification_revision); + printf("Power Port Role: %s\n", power_port_role_name); + printf("Data Port Role: %s\n", data_port_role_name); + printf("Extended: %s\n", extended ? "YES" : "NO"); + printf("==========================================\n"); +#endif + + usbpd->number_of_data_objects = num_data_objects; + usbpd->last_msg_type = message_type; + + return true; +} + +bool usb_pd_parse_pdo_fixed_supply(usbpd_t* usbpd, uint32_t pdo) { + uint32_t max_current = pdo & 0b1111111111; + uint32_t voltage = (pdo >> 10) & 0b1111111111; + uint32_t peak_current = (pdo >> 20) & 0b11; + uint32_t epr_capable = (pdo >> 23) & 0b1; + uint32_t uem_supported = (pdo >> 24) & 0b1; + uint32_t dual_role_power = (pdo >> 25) & 0b1; + uint32_t usb_comms_capable = (pdo >> 26) & 0b1; + uint32_t unconstrained_power = (pdo >> 27) & 0b1; + uint32_t usb_suspend_supported = (pdo >> 28) & 0b1; + + max_current = max_current * 10; + voltage = voltage * 50; + +#ifdef DEBUG + printf("=== USB-PD Fixed Supply PDO ==============\n"); + printf("Max Current: %.3f A\n", max_current / 1000.0); + printf("Voltage: %.3f V\n", voltage / 1000.0); + printf("Peak Current: %s\n", (char*[]){"Not Supported", "Level 1", "Level 2", "Level 3"}[peak_current]); + if (voltage == 5000) { + printf("==========================================\n"); + printf("EPR Capable: %s\n", epr_capable ? "YES" : "NO"); + printf("Unchunked Extended Messages Supported: %s\n", uem_supported ? "YES" : "NO"); + printf("Dual-Role Power: %s\n", dual_role_power ? "YES" : "NO"); + printf("USB Communications Capable: %s\n", usb_comms_capable ? "YES" : "NO"); + printf("Uncostrained Power: %s\n", unconstrained_power ? "YES" : "NO"); + printf("USB Suspend Supported: %s\n", usb_suspend_supported ? "YES" : "NO"); + } + printf("==========================================\n"); +#endif + + return true; +} + +bool usb_pd_parse_pdo_pps(usbpd_t* usbpd, uint32_t pdo) { + uint32_t max_voltage = (pdo >> 17) & 0b11111111; + uint32_t min_voltage = (pdo >> 8) & 0b11111111; + uint32_t max_current = pdo & 0b1111111; + + max_voltage = max_voltage * 100; + min_voltage = min_voltage * 100; + max_current = max_current * 50; + +#ifdef DEBUG + printf("=== USB-PD Programmable Power Supply PDO =\n"); + printf("Voltage: %.3f V to %.3f V\n", min_voltage / 1000.0, max_voltage / 1000.0); + printf("Max Current: %.3f A\n", max_current / 1000.0); + printf("==========================================\n"); +#endif + + return true; +} + +bool usb_pd_parse_pdo_vps(usbpd_t* usbpd, uint32_t pdo) { + uint32_t max_voltage = (pdo >> 20) & 0b1111111111; + uint32_t min_voltage = (pdo >> 10) & 0b1111111111; + uint32_t max_current = pdo & 0b1111111111; + + max_voltage = max_voltage * 50; + min_voltage = min_voltage * 50; + max_current = max_current * 10; + +#ifdef DEBUG + printf("=== USB-PD Variable Power Supply PDO =====\n"); + printf("Voltage: %.3f V to %.3f V\n", min_voltage / 1000.0, max_voltage / 1000.0); + printf("Max Current: %.3f A\n", max_current / 1000.0); + printf("==========================================\n"); +#endif + + return true; +} + +bool usb_pd_parse_pdo_bps(usbpd_t* usbpd, uint32_t pdo) { + uint32_t max_voltage = (pdo >> 20) & 0b1111111111; + uint32_t min_voltage = (pdo >> 10) & 0b1111111111; + uint32_t max_current = pdo & 0b1111111111; + + max_voltage = max_voltage * 50; + min_voltage = min_voltage * 50; + max_current = max_current * 250; + +#ifdef DEBUG + printf("=== USB-PD Battery Power Supply PDO ======\n"); + printf("Voltage: %.3f V to %.3f V\n", min_voltage / 1000.0, max_voltage / 1000.0); + printf("Max Current: %.3f W\n", max_current / 1000.0); + printf("==========================================\n"); +#endif + + return true; +} + +bool usb_pd_parse_pdo(usbpd_t* usbpd, uint8_t* pdo_bytes) { + uint32_t pdo = (pdo_bytes[3] << 24) | (pdo_bytes[2] << 16) | (pdo_bytes[1] << 8) | pdo_bytes[0]; + + if (usbpd->last_msg_type == USBPD_DATA_MSG_SOURCE_CAPABILITIES) { + uint8_t pdo_type = (pdo >> 30) & 0b11; + + if (pdo_type == 0b00) { + return usb_pd_parse_pdo_fixed_supply(usbpd, pdo); + } + + if (pdo_type == 0b01) { + return usb_pd_parse_pdo_bps(usbpd, pdo); + } + + if (pdo_type == 0b10) { + return usb_pd_parse_pdo_vps(usbpd, pdo); + } + + if (pdo_type == 0b11) { + return usb_pd_parse_pdo_pps(usbpd, pdo); + } + } + + if (usbpd->last_msg_type == USBPD_DATA_MSG_VENDOR_DEFINED) { + printf("[USB-PD] Vendor PDO are not supported yet.\n"); + } + + return false; +} + +bool usb_pd_generate_header(usbpd_t* usbpd, + uint8_t* payload, + uint16_t number_of_objects, + uint16_t message_id, + uint16_t power_role, + uint16_t specification_name, + uint16_t data_role, + uint16_t data_message_type) { +#ifdef DEBUG + printf("[USB-PD] Generating header.\n"); +#endif + uint16_t header = 0x0000; + + header |= (0b0 << 15); + header |= (number_of_objects << 12); + header |= (message_id << 9); + header |= (power_role << 8); + header |= (specification_name << 6); + header |= (data_role << 5); + header |= (data_message_type); + + split_to_bytes(header, 2, payload); + usb_pd_parse_header(usbpd, payload); + + return true; +} + +bool usb_pd_parse_rdo(usbpd_t* usbpd, uint8_t* rdo_bytes) { + uint32_t pdo = (rdo_bytes[3] << 24) | (rdo_bytes[2] << 16) | (rdo_bytes[1] << 8) | rdo_bytes[0]; + + return true; +} + +bool usb_pd_parse_rdo_fvs(usbpd_t* usbpd, uint32_t header) { + uint16_t object_position = (header >> 28) & 0b1111; + bool give_back = (header >> 27) & 0b1; + bool capability_mismatch = (header >> 26) & 0b1; + bool usb_communications_capable = (header >> 25) & 0b1; + bool no_usb_suspend = (header >> 24) & 0b1; + bool unchunked_messages_support = (header >> 23) & 0b1; + bool epr_support = (header >> 22) & 0b1; + uint16_t output_current = ((header >> 10) & 0b1111111111) * 10; + uint16_t max_output_current = (header & 0b1111111111) * 10; + +#ifdef DEBUG + printf("=== USB-PD Var/Fix Power Supply RDO ======\n"); + printf("Object Position: %d\n", object_position); + printf("Give Back: %s\n", give_back ? "YES" : "NO"); + printf("Capability Mismatch: %s\n", capability_mismatch ? "YES" : "NO"); + printf("USB Communications Capable: %s\n", usb_communications_capable ? "YES" : "NO"); + printf("No USB Suspend: %s\n", no_usb_suspend ? "YES" : "NO"); + printf("Unchunked Messages Support: %s\n", unchunked_messages_support ? "YES" : "NO"); + printf("EPR Support: %s\n", epr_support ? "YES" : "NO"); + printf("Output Current: %.3f A\n", output_current / 1000.0); + printf("Max Output Current: %.3f A\n", max_output_current / 1000.0); + printf("==========================================\n"); +#endif + + return true; +} + +bool usb_pd_parse_rdo_pps(usbpd_t* usbpd, uint32_t header) { + uint16_t object_position = (header >> 28) & 0b1111; + bool capability_mismatch = (header >> 26) & 0b1; + bool usb_communications_capable = (header >> 25) & 0b1; + bool no_usb_suspend = (header >> 24) & 0b1; + bool unchunked_messages_support = (header >> 23) & 0b1; + bool epr_support = (header >> 22) & 0b1; + uint16_t output_voltage = ((header >> 9) & 0b111111111111) * 20; + uint16_t output_current = (header & 0b1111111) * 50; + +#ifdef DEBUG + printf("=== USB-PD Programmable Power Supply RDO =\n"); + printf("Object Position: %d\n", object_position); + printf("Capability Mismatch: %s\n", capability_mismatch ? "YES" : "NO"); + printf("USB Communications Capable: %s\n", usb_communications_capable ? "YES" : "NO"); + printf("No USB Suspend: %s\n", no_usb_suspend ? "YES" : "NO"); + printf("Unchunked Messages Support: %s\n", unchunked_messages_support ? "YES" : "NO"); + printf("EPR Support: %s\n", epr_support ? "YES" : "NO"); + printf("Output Voltage: %.3f V\n", output_voltage / 1000.0); + printf("Output Current: %.3f A\n", output_current / 1000.0); + printf("==========================================\n"); +#endif + + return true; +} + +bool usb_pd_generate_rdo_pps(usbpd_t* usbpd, + uint8_t* payload, + uint16_t object_position, + bool capability_mismatch, + bool usb_communications_capable, + bool no_usb_suspend, + bool unchunked_messages_support, + bool epr_support, + uint16_t output_voltage, + uint16_t output_current) { +#ifdef DEBUG + printf("[USB-PD] Generating PPS RDO.\n"); +#endif + uint32_t header = 0x00000000; + + header |= (object_position << 28); + header |= (capability_mismatch << 26); + header |= (usb_communications_capable << 25); + header |= (no_usb_suspend << 24); + header |= (unchunked_messages_support << 23); + header |= (epr_support << 22); + header |= ((output_voltage / 20) << 9); + header |= ((output_current / 50) << 0); + + split_to_bytes(header, 4, payload); + usb_pd_parse_rdo_pps(usbpd, header); + + return true; +} + +bool usb_pd_generate_rdo_fvs(usbpd_t* usbpd, + uint8_t* payload, + uint16_t object_position, + bool give_back, + bool capability_mismatch, + bool usb_communications_capable, + bool no_usb_suspend, + bool unchunked_messages_support, + bool epr_support, + uint16_t output_current, + uint16_t max_output_current) { +#ifdef DEBUG + printf("[USB-PD] Generating FVS RDO.\n"); +#endif + uint32_t header = 0x00000000; + + header |= (object_position << 28); + header |= (give_back << 27); + header |= (capability_mismatch << 26); + header |= (usb_communications_capable << 25); + header |= (no_usb_suspend << 24); + header |= (unchunked_messages_support << 23); + header |= (epr_support << 22); + header |= ((output_current / 10) << 10); + header |= ((max_output_current / 10) << 0); + + split_to_bytes(header, 4, payload); + usb_pd_parse_rdo_fvs(usbpd, header); + + return true; +} + +#endif