int dwc_dh_pk(uint8_t nd, uint8_t *exp, uint8_t *pk, uint8_t *hash)
{
int retval;
uint8_t m3[385];
#ifndef DH_TEST_VECTORS
DWC_RANDOM_BYTES(exp, 32);
#endif
/* Compute the pkd */
if ((retval = dwc_dh_modpow(dh_g, 4,
exp, 32,
dh_p, 384, pk))) {
return retval;
}
m3[384] = nd;
DWC_MEMCPY(&m3[0], pk, 384);
DWC_SHA256(m3, 385, hash);
dh_dump("PK", pk, 384);
dh_dump("SHA-256(M3)", hash, 32);
return 0;
}
static int _setup(dwc_otg_pcd_t *pcd, uint8_t *bytes)
{
DWC_DEBUGPL(DBG_PCD, "%s(): enter \n", __func__);
const usb_device_request_t *ctrlReq = (usb_device_request_t *)bytes;
int value = 0;
int idx;
usb_device_request_t ctrlConvertedReq;
usb_device_request_t *ctrl = &ctrlConvertedReq;
ctrl->bmRequestType = (uByte)(ctrlReq->bmRequestType);
ctrl->bRequest = (uByte)(ctrlReq->bRequest);
ctrl->wValue[0] = ctrlReq->wValue[0];
ctrl->wValue[1] = ctrlReq->wValue[1];
ctrl->wIndex[0] = ctrlReq->wIndex[0];
ctrl->wIndex[1] = ctrlReq->wIndex[1];
ctrl->wLength[0] = ctrlReq->wLength[0];
ctrl->wLength[1] = ctrlReq->wLength[1];
/*
* usually this stores reply data in the pre-allocated ep0 buffer,
* but config change events will reconfigure hardware.
*/
static_data->dl_req.data.zero = 0;
if (UT_GET_RECIPIENT(ctrl->bmRequestType) == UT_INTERFACE) {
if ((UT_GET_DIR(ctrl->bmRequestType) == UT_WRITE)
&& (UGETW(ctrl->wLength) > 0)) {
static_data->dl_req.data.buf =
&static_data->ep0_buffer[0];
DWC_MEMCPY(&static_data->dl_req.out_req, ctrl,
sizeof(*ctrl));
static_data->dl_req.data.length =
DWC_MIN(UGETW(
ctrl->wLength),
static_data->ep0_buffer_size);
// either shorter than asked, or multiple of the MPS require ZLP.
static_data->dl_req.data.zero =
((value < UGETW(ctrl->wLength))
&&
((value %
pcd->ep0.dwc_ep.
maxpacket) ==
0)) ? 1 : 0;
// Elaborate when getting the time.
dwc_otg_pcd_ep_queue(
pcd, /*void* ep_hanlde */ NULL,
static_data->dl_req.data.buf,
(dwc_dma_t)static_data->dl_req.
data.buf,
static_data->dl_req.data.length,
static_data->dl_req.data.zero,
/*void* req_handle */ &static_data
->dl_req, 0);
} else {
static_data->dl_req.data.buf =
&static_data->ep0_buffer[0];
if (static_data->usb_class_handler(ctrl, &value,
&static_data->dl_req
.data.buf) != 0) {
value = -DWC_E_NOT_SUPPORTED;
}
}
} else {
switch (ctrl->bRequest) {
case UR_GET_DESCRIPTOR:
__DWC_ERROR("ctrl->Length = %d \n",
UGETW(ctrl->wLength));
switch (UGETW(ctrl->wValue) >> 8) {
case UDESC_DEVICE:
value =
DWC_MIN(UGETW(ctrl->wLength),
USB_DEVICE_DESCRIPTOR_SIZE);
static_data->dl_req.data.buf = (uint8_t *)
static_data->
usb_device_descriptor;
break;
case UDESC_CONFIG:
value = DWC_MIN(UGETW(ctrl->wLength),
UGETW(static_data->
usb_config_descriptor->
wTotalLength));
static_data->dl_req.data.buf = (uint8_t *)
static_data->
usb_config_descriptor;
break;
case UDESC_OTHER_SPEED_CONFIGURATION:
__DWC_ERROR
(
"UDESC_OTHER_SPEED_CONFIGURATION NOT IMPLEMENTED! \n");
break;
case UDESC_STRING:
/* wIndex == language code.
* this driver only handles one language, you can
* add string tables for other languages, using
* any UTF-8 characters
*/
idx = UGETW(ctrl->wValue) & 0xFF;
__DWC_ERROR("UDESC_STRING idx = %d \n", idx);
__DWC_ERROR(
"UDESC_STRING bLength = %d \n",
&static_data->strings_desc[idx].
bLength);
if (idx > static_data->usb_strings_count) {
if (idx == 0xEE) {
// Microsoft MTP extension
value = 0;
} else {
value = -DWC_E_INVALID;
}
} else {
value = DWC_MIN(UGETW(
ctrl->wLength),
static_data->
strings_desc[idx].
bLength);
static_data->dl_req.data.buf =
(uint8_t *)
&
static_data->strings_desc[idx];
}
break;
}
break;
case UR_SET_CONFIG:
static_data->usb_config = UGETW(ctrl->wValue);
value = 0;
{
int i;
for (i = 0;
i < static_data->usb_num_endpoints;
i++) {
int ret = dwc_otg_pcd_ep_enable(
pcd,
(const
uint8_t
*)&static_data->usb_endpoints[
i],
static_data
->usb_endpoints[i].
bEndpointAddress);
__DWC_WARN(
"Enabled endpoint: %x ret: %d\n",
static_data->usb_endpoints[i
].bEndpointAddress,
ret);
}
struct usb_event evt;
evt.event = USB_EVENT_SET_CONFIG;
evt.event_data.config = static_data->usb_config;
if (static_data->usb_event_cb) {
static_data->usb_event_cb(&evt);
}
}
break;
case UR_GET_CONFIG:
static_data->dl_req.data.buf = &static_data->usb_config;
value = DWC_MIN(UGETW(ctrl->wLength), 1);
break;
/* until we add altsetting support, or other interfaces,
* only 0/0 are possible. pxa2xx only supports 0/0 (poorly)
* and already killed pending endpoint I/O.
*/
case UR_SET_INTERFACE:
value = 0;
break;
case UR_GET_INTERFACE:
__DWC_ERROR("UR_GET_INTERFACE NOT IMPLEMENTED! \n");
goto unknown;
break;
default:
unknown:
value = -DWC_E_NOT_SUPPORTED;
break;
}
}
/* respond with data transfer before status phase? */
__DWC_ERROR("%s(): req.length = %d - max length is 64 \n", __func__,
value);
if (value >= 0) {
static_data->dl_req.data.length = value;
// either shorter than asked, or multiple of the MPS require ZLP.
static_data->dl_req.data.zero = ((value < UGETW(ctrl->wLength))
&& ((value %
pcd->ep0.dwc_ep.maxpacket)
==
0)) ? 1 : 0;
// Elaborate when getting the time.
dwc_otg_pcd_ep_queue(pcd, /*void* ep_hanlde */ NULL,
static_data->dl_req.data.buf,
(dwc_dma_t)static_data->dl_req.data.buf,
static_data->dl_req.data.length,
static_data->dl_req.data.zero,
/*void* req_handle */ NULL, 0);
}
/* device either stalls (value < 0) or reports success */
return value;
}
int dwc_dh_derive_keys(uint8_t nd, uint8_t *pkh, uint8_t *pkd,
uint8_t *exp, int is_host,
char *dd, uint8_t *ck, uint8_t *kdk)
{
int retval;
uint8_t mv[784];
uint8_t sha_result[32];
uint8_t dhkey[384];
uint8_t shared_secret[384];
char *message;
uint32_t vd;
uint8_t *pk;
if (is_host) {
pk = pkd;
}
else {
pk = pkh;
}
if ((retval = dwc_dh_modpow(pk, 384,
exp, 32,
dh_p, 384, shared_secret))) {
return retval;
}
dh_dump("Shared Secret", shared_secret, 384);
DWC_SHA256(shared_secret, 384, dhkey);
dh_dump("DHKEY", dhkey, 384);
DWC_MEMCPY(&mv[0], pkd, 384);
DWC_MEMCPY(&mv[384], pkh, 384);
DWC_MEMCPY(&mv[768], "displayed digest", 16);
dh_dump("MV", mv, 784);
DWC_SHA256(mv, 784, sha_result);
dh_dump("SHA-256(MV)", sha_result, 32);
dh_dump("First 32-bits of SHA-256(MV)", sha_result, 4);
dh_swap_bytes(sha_result, &vd, 4);
#ifdef DEBUG
DWC_PRINTF("Vd (decimal) = %d\n", vd);
#endif
switch (nd) {
case 2:
vd = vd % 100;
DWC_SPRINTF(dd, "%02d", vd);
break;
case 3:
vd = vd % 1000;
DWC_SPRINTF(dd, "%03d", vd);
break;
case 4:
vd = vd % 10000;
DWC_SPRINTF(dd, "%04d", vd);
break;
}
#ifdef DEBUG
DWC_PRINTF("Display Digits: %s\n", dd);
#endif
message = "connection key";
DWC_HMAC_SHA256(message, DWC_STRLEN(message), dhkey, 32, sha_result);
dh_dump("HMAC(SHA-256, DHKey, connection key)", sha_result, 32);
DWC_MEMCPY(ck, sha_result, 16);
message = "key derivation key";
DWC_HMAC_SHA256(message, DWC_STRLEN(message), dhkey, 32, sha_result);
dh_dump("HMAC(SHA-256, DHKey, key derivation key)", sha_result, 32);
DWC_MEMCPY(kdk, sha_result, 32);
return 0;
}
