diff --git a/drivers/usb/typec/mmi_tcpc/Kbuild b/drivers/usb/typec/mmi_tcpc/Kbuild index c8bd27a190d3..5ae487705624 100644 --- a/drivers/usb/typec/mmi_tcpc/Kbuild +++ b/drivers/usb/typec/mmi_tcpc/Kbuild @@ -40,7 +40,8 @@ tcpc_class-objs += tcpci_event.o \ pd_process_evt_dbg.o pd_process_evt_tcp.o pd_process_evt_com.o \ pd_policy_engine_src.o pd_policy_engine_snk.o pd_policy_engine_ufp.o pd_policy_engine_vcs.o \ pd_policy_engine_dfp.o pd_policy_engine_dr.o pd_policy_engine_drs.o pd_policy_engine_prs.o \ - pd_policy_engine_dbg.o pd_policy_engine_com.o pd_dpm_alt_mode_dc.o pd_adapter.o + pd_policy_engine_dbg.o pd_policy_engine_com.o pd_dpm_alt_mode_dc.o pd_adapter.o \ + pd_dpm_mmi_adapter.o endif obj-m += tcpc_class.o diff --git a/drivers/usb/typec/mmi_tcpc/inc/pd_core.h b/drivers/usb/typec/mmi_tcpc/inc/pd_core.h index f0ca1884be2a..996c09f2c589 100644 --- a/drivers/usb/typec/mmi_tcpc/inc/pd_core.h +++ b/drivers/usb/typec/mmi_tcpc/inc/pd_core.h @@ -671,6 +671,7 @@ #define USB_VID_RICHTEK 0x29cf /* demo uvdm */ #define USB_VID_DIRECTCHARGE 0x29cf /* direct charge */ #define USB_VID_MQP 0x1748 +#define USB_VID_MMI_ADAPTER 0x22b8 /* PD counter definitions */ #define PD_MESSAGE_ID_COUNT 7 @@ -857,6 +858,11 @@ struct pd_port { /* miss msg */ bool miss_msg; uint8_t rx_cap; + +#ifdef CONFIG_SUPPORT_MMI_ADAPTER + uint8_t mmi_adapter_state; +#endif /* CONFIG_SUPPORT_MMI_ADAPTER */ + /* PD */ bool msg_output_lock; diff --git a/drivers/usb/typec/mmi_tcpc/inc/pd_dpm_core.h b/drivers/usb/typec/mmi_tcpc/inc/pd_dpm_core.h index 07f15ff30ffa..f38c94df5e9c 100644 --- a/drivers/usb/typec/mmi_tcpc/inc/pd_dpm_core.h +++ b/drivers/usb/typec/mmi_tcpc/inc/pd_dpm_core.h @@ -324,6 +324,16 @@ extern bool dc_parse_svid_data(struct pd_port *pd_port, struct svdm_svid_data *svid_data); #endif /* CONFIG_USB_PD_ALT_MODE_RTDC */ +#ifdef CONFIG_SUPPORT_MMI_ADAPTER +extern bool mmi_dfp_u_notify_discover_svid( + struct pd_port *pd_port, struct svdm_svid_data *svid_data, bool ack); +extern bool mmi_dfp_notify_uvdm(struct pd_port *pd_port, + struct svdm_svid_data *svid_data, bool ack); +extern int mmi_notify_pe_ready(struct pd_port *pd_port, + struct svdm_svid_data *svid_data); +extern bool mmi_dfp_notify_pe_startup( + struct pd_port *pd_port, struct svdm_svid_data *svid_data); +#endif /* CONFIG_SUPPORT_MMI_ADAPTER */ /** * pd_dpm_get_ready_reaction diff --git a/drivers/usb/typec/mmi_tcpc/inc/tcpci_config.h b/drivers/usb/typec/mmi_tcpc/inc/tcpci_config.h index d91fccfbced8..2b48bbb020eb 100644 --- a/drivers/usb/typec/mmi_tcpc/inc/tcpci_config.h +++ b/drivers/usb/typec/mmi_tcpc/inc/tcpci_config.h @@ -29,6 +29,11 @@ #define CONFIG_TYPEC_CAP_NORP_SRC /* #define CONFIG_COMPATIBLE_APPLE_TA */ +#define CONFIG_SUPPORT_MMI_ADAPTER +#ifdef CONFIG_SUPPORT_MMI_ADAPTER +//#define CONFIG_SUPPORT_MMI_ADAPTER_GET_TEMP +#endif /* CONFIG_SUPPORT_MMI_ADAPTER */ + #if 0 /* CONFIG_MTK_GAUGE_VERSION == 30 */ #define CONFIG_RECV_BAT_ABSENT_NOTIFY #endif /* CONFIG_MTK_GAUGE_VERSION */ diff --git a/drivers/usb/typec/mmi_tcpc/inc/tcpci_core.h b/drivers/usb/typec/mmi_tcpc/inc/tcpci_core.h index 2b5e17b25fd7..6f996eb43b17 100644 --- a/drivers/usb/typec/mmi_tcpc/inc/tcpci_core.h +++ b/drivers/usb/typec/mmi_tcpc/inc/tcpci_core.h @@ -59,6 +59,8 @@ #define UVDM_INFO_ENABLE 1 #define TCPM_DBG_ENABLE 1 +#define MMI_DBG_ENABLE 1 + #ifdef CONFIG_USB_PD_ALT_MODE_RTDC #define DC_INFO_ENABLE 1 #define DC_DBG_ENABLE 1 @@ -702,4 +704,11 @@ static inline bool pd_check_rev30(struct pd_port *pd_port) #endif /* CONFIG_USB_PD_ALT_MODE_RTDC */ +#if MMI_DBG_ENABLE +#define MMI_INFO(format, args...) \ + RT_DBG_INFO(CONFIG_TCPC_DBG_PRESTR "MMI> " format, ##args) +#else +#define MMI_INFO(format, args...) +#endif + #endif /* #ifndef __LINUX_RT_TCPCI_CORE_H */ diff --git a/drivers/usb/typec/mmi_tcpc/pd_dpm_core.c b/drivers/usb/typec/mmi_tcpc/pd_dpm_core.c index 4725a5cb963c..90440096f49f 100644 --- a/drivers/usb/typec/mmi_tcpc/pd_dpm_core.c +++ b/drivers/usb/typec/mmi_tcpc/pd_dpm_core.c @@ -93,6 +93,17 @@ static const struct svdm_svid_ops svdm_svid_ops[] = { .reset_state = dc_reset_state, }, #endif /* CONFIG_USB_PD_ALT_MODE_RTDC */ + +#ifdef CONFIG_SUPPORT_MMI_ADAPTER + { + .name = "mmi adapter", + .svid = USB_VID_MMI_ADAPTER, + .notify_pe_startup = mmi_dfp_notify_pe_startup, + .dfp_inform_svids = mmi_dfp_u_notify_discover_svid, + .dfp_notify_uvdm = mmi_dfp_notify_uvdm, + .notify_pe_ready = mmi_notify_pe_ready, + }, +#endif /* CONFIG_SUPPORT_MMI_ADAPTER */ }; int dpm_check_supported_modes(void) diff --git a/drivers/usb/typec/mmi_tcpc/pd_dpm_mmi_adapter.c b/drivers/usb/typec/mmi_tcpc/pd_dpm_mmi_adapter.c new file mode 100644 index 000000000000..f518fd07acf9 --- /dev/null +++ b/drivers/usb/typec/mmi_tcpc/pd_dpm_mmi_adapter.c @@ -0,0 +1,289 @@ +#include +#include "inc/tcpci.h" +#include "inc/pd_policy_engine.h" +#include "inc/pd_dpm_core.h" +#include "pd_dpm_prv.h" +#include "inc/tcpci_core.h" +#include + +#ifdef CONFIG_SUPPORT_MMI_ADAPTER + +#define CMD_HEADER_REQUEST 0x01 +#define CMD_HEADER_ACK 0x02 + +#define CMD_CHARGER_CAPACITY 0x01 +#define CMD_CHARGER_AUTHENTICATION 0x02 +#define CMD_CHARGER_TEMPERATURE 0x03 +#define CMD_CHARGER_VERIFY 0x04 + +#define CMD(header,cmd) ((header << 8) + cmd) +#define MMI_UVDM_CMD(vid,header,cmd) ((vid << 16) + CMD(header,cmd)) + +#define CONFIG_MOTO_ADAPTER 0x4d4f544f +#define CONFIG_ADAPTER_68W 0x30363841 + +#define SHA256_NUM 4 +static uint32_t random_num[SHA256_NUM] = {0}; +#define MMI_KEY "Motorola-Hyper-Proprietary-2021Gen1" + +enum MMI_STATE { + MMI_NONE = 0, + MMI_REQUEST_CAP, + MMI_ENCRYPT, + MMI_REQUEST_TEMP, + MMI_VERIFY, + MMI_READY, +}; + +static const char * const mmi_state[] = { + "None", + "Request cap", + "Encrypt", + "Request temp", + "Verify", + "mmi adapter ready", +}; + +static int hmac_sha256(u8 *key, u8 ksize, char *plaintext, u8 psize, u8 *output) +{ + struct crypto_shash *tfm; + struct shash_desc *shash; + int ret; + + if (!ksize) + return -EINVAL; + + tfm = crypto_alloc_shash("hmac(sha256)", 0, 0); + if (IS_ERR(tfm)) { + MMI_INFO("crypto_alloc_ahash failed: err %ld", PTR_ERR(tfm)); + return PTR_ERR(tfm); + } + + ret = crypto_shash_setkey(tfm, key, ksize); + if (ret) { + MMI_INFO("crypto_ahash_setkey failed: err %d", ret); + goto failed; + } + + shash = kzalloc(sizeof(*shash) + crypto_shash_descsize(tfm), + GFP_KERNEL); + if (!shash) { + ret = -ENOMEM; + goto failed; + } + + shash->tfm = tfm; + + ret = crypto_shash_digest(shash, plaintext, psize, output); + + kfree(shash); + +failed: + crypto_free_shash(tfm); + return ret; +} + +static int mmi_hmac_sha256(u8 *key, u8 ksize,uint32_t *plaintext, + u8 psize,uint32_t *output) +{ + uint8_t *data_in; + uint8_t *data_out; + int i = 0,j = 0; + + data_in = kzalloc(sizeof(*data_in) * psize * 4,GFP_KERNEL); + if (!data_in) { + return -ENOMEM; + } + + data_out = kzalloc(sizeof(*data_out) * psize * 4,GFP_KERNEL); + if (!data_out) { + kfree(data_in); + return -ENOMEM; + } + + for (i = 0;i < psize;i++) { + for (j = 0; j < sizeof(*plaintext); j++) { + data_in[(i * 4) + 3 - j] = (plaintext[i] >> (j * 8)) & 0xff; + } + } + + hmac_sha256(key,ksize,data_in,4*psize,data_out); + + for (i = 0;i < psize;i++) { + output[i] = data_out[3 + (4 * i)] + (data_out[2 + (4 * i)] << 8) + + (data_out[1 + (4 * i)] << 16) + (data_out[0 + (4 * i)] << 24); + } + + kfree(data_in); + kfree(data_out); + return 0; +} + +static bool mmi_uvdm_ack(struct pd_port *pd_port, + struct svdm_svid_data *svid_data) +{ + uint16_t vid = svid_data->svid; + uint32_t data = pd_port->uvdm_data[0]; + if ((data >> 16) != vid) { + return false; + } + if (((data & 0xff) == pd_port->mmi_adapter_state) && + ((data >> 8) & 0xff) == CMD_HEADER_ACK) { + return true; + } + return false; +} + +static void mmi_adapter_set_state(struct pd_port *pd_port,uint8_t state) { + MMI_INFO("%s -> %s\n",mmi_state[pd_port->mmi_adapter_state], + mmi_state[state]); + pd_port->mmi_adapter_state = state; +} + + +static void mmi_adapter_cap_request(struct pd_port *pd_port, + struct svdm_svid_data *svid_data) +{ + pd_port->uvdm_cnt = 1; + pd_port->uvdm_wait_resp = true; + pd_port->uvdm_data[0] = MMI_UVDM_CMD(svid_data->svid, + CMD_HEADER_REQUEST,CMD_CHARGER_CAPACITY); + pd_put_tcp_vdm_event(pd_port,TCP_DPM_EVT_UVDM); +} + +static void mmi_adapter_encrypt(struct pd_port *pd_port, + struct svdm_svid_data *svid_data) +{ + uint8_t index = 0; + pd_port->uvdm_cnt = 1 + SHA256_NUM; + pd_port->uvdm_wait_resp = true; + + pd_port->uvdm_data[0] = MMI_UVDM_CMD(svid_data->svid, + CMD_HEADER_REQUEST,CMD_CHARGER_AUTHENTICATION); + for (index = 0;index < SHA256_NUM;index++) { + get_random_bytes(&(random_num[index]),sizeof(*random_num)); + //random_num[index] = index + 1; + pd_port->uvdm_data[index+1] = random_num[index]; + } + pd_put_tcp_vdm_event(pd_port, TCP_DPM_EVT_UVDM); +} + +static void mmi_adapter_req_temp(struct pd_port *pd_port, + struct svdm_svid_data *svid_data) +{ + pd_port->uvdm_cnt = 1; + pd_port->uvdm_wait_resp = true; + pd_port->uvdm_data[0] = MMI_UVDM_CMD(svid_data->svid, + CMD_HEADER_REQUEST,CMD_CHARGER_TEMPERATURE); + pd_put_tcp_vdm_event(pd_port,TCP_DPM_EVT_UVDM); +} + +static void mmi_adapter_verify(struct pd_port *pd_port, + struct svdm_svid_data *svid_data) +{ + pd_port->uvdm_cnt = 1; + pd_port->uvdm_wait_resp = true; + pd_port->uvdm_data[0] = MMI_UVDM_CMD(svid_data->svid, + CMD_HEADER_ACK,CMD_CHARGER_VERIFY); + pd_put_tcp_vdm_event(pd_port,TCP_DPM_EVT_UVDM); +} + +bool mmi_dfp_notify_pe_startup( + struct pd_port *pd_port, struct svdm_svid_data *svid_data) +{ + mmi_adapter_set_state(pd_port,MMI_NONE); + return true; +} + +bool mmi_dfp_u_notify_discover_svid( + struct pd_port *pd_port, struct svdm_svid_data *svid_data, bool ack) +{ + MMI_INFO("discover\n"); + if (!ack) { + return false; + } + mmi_adapter_set_state(pd_port,MMI_REQUEST_CAP); + return true; +} + +bool mmi_dfp_notify_uvdm(struct pd_port *pd_port, + struct svdm_svid_data *svid_data, bool ack) +{ + uint32_t out[SHA256_NUM] = {0}; + char key[] = MMI_KEY; + int i = 0; + switch(pd_port->mmi_adapter_state) { + case MMI_REQUEST_CAP: + if (mmi_uvdm_ack(pd_port,svid_data) && + pd_port->uvdm_data[1] == CONFIG_MOTO_ADAPTER && + pd_port->uvdm_data[2] == CONFIG_ADAPTER_68W) { +#ifdef CONFIG_SUPPORT_MMI_ADAPTER_GET_TEMP + mmi_adapter_set_state(pd_port,MMI_REQUEST_TEMP); +#else + mmi_adapter_set_state(pd_port,MMI_ENCRYPT); +#endif /* CONFIG_SUPPORT_MMI_ADAPTER_GET_TEMP */ + return true; + } + break; + case MMI_REQUEST_TEMP: + if (mmi_uvdm_ack(pd_port,svid_data)) { + MMI_INFO("now adapter temp = %d\n",pd_port->uvdm_data[1]); + mmi_adapter_set_state(pd_port,MMI_ENCRYPT); + return true; + } + break; + case MMI_ENCRYPT: + if (mmi_uvdm_ack(pd_port,svid_data)) { + /** + * todo hmac-sha256 check + * src data : pd_port->uvdm_data[1-4] + * random : random_num + */ + mmi_hmac_sha256(key,sizeof(key),random_num,SHA256_NUM,out); + for (i = 0;i < SHA256_NUM;i++) { + if (pd_port->uvdm_data[i+1] != out[i]) + goto out; + } + mmi_adapter_set_state(pd_port,MMI_VERIFY); + return true; + } + break; + case MMI_VERIFY: + if (mmi_uvdm_ack(pd_port,svid_data) && + pd_port->uvdm_data[1] == 0x01) { + mmi_adapter_set_state(pd_port,MMI_READY); + dpm_reaction_set(pd_port, DPM_REACTION_GET_SOURCE_CAP); + return true; + } + break; + default: + break; + } +out: + mmi_adapter_set_state(pd_port,MMI_NONE); + return true; +} + +int mmi_notify_pe_ready(struct pd_port *pd_port, + struct svdm_svid_data *svid_data) +{ + switch(pd_port->mmi_adapter_state) { + case MMI_REQUEST_CAP: + mmi_adapter_cap_request(pd_port,svid_data); + break; + case MMI_REQUEST_TEMP: + mmi_adapter_req_temp(pd_port,svid_data); + break; + case MMI_ENCRYPT: + mmi_adapter_encrypt(pd_port,svid_data); + break; + case MMI_VERIFY: + mmi_adapter_verify(pd_port,svid_data); + break; + default: + break; + } + return 0; +} + +#endif /* CONFIG_SUPPORT_MMI_ADAPTER */ \ No newline at end of file