void usb_init()
{
/* Initialize USB Subsystem */
/* See makefile for mode constraints */
USB_Init();
sem_init(&cdc_run, 0);
bcond_init(&usb_active, usb_con());
STREAM_INIT( cdc_instr, CDC_BUF_SIZE);
STREAM_INIT( cdc_outstr, CDC_BUF_SIZE);
cdc_outstr.kick = NULL;
THREAD_START(usb_thread, STACK_USB);
}
static vsf_err_t vsfusbd_MSCBOT_SendCSW(struct vsfusbd_device_t *device,
struct vsfusbd_MSCBOT_param_t *param)
{
struct USBMSC_CSW_t *CSW = ¶m->CSW;
uint32_t remain_size;
CSW->dCSWSignature = SYS_TO_LE_U32(USBMSC_CSW_SIGNATURE);
CSW->dCSWTag = param->CBW.dCBWTag;
remain_size = param->CBW.dCBWDataTransferLength;
if (param->scsi_transact != NULL)
{
remain_size -= param->scsi_transact->data_size;
vsfscsi_release_transact(param->scsi_transact);
}
CSW->dCSWDataResidue = SYS_TO_LE_U32(remain_size);
param->bufstream.mem.buffer.buffer = (uint8_t *)¶m->CSW;
param->bufstream.mem.buffer.size = sizeof(param->CSW);
param->bufstream.mem.read = true;
param->bufstream.stream.op = &bufstream_op;
STREAM_INIT(¶m->bufstream);
param->transact.ep = param->ep_in;
param->transact.data_size = sizeof(param->CSW);
param->transact.stream = (struct vsf_stream_t *)¶m->bufstream;
param->transact.cb.on_finish = vsfusbd_MSCBOT_on_idle;
param->transact.cb.param = param;
return vsfusbd_ep_send(param->device, ¶m->transact);
}
// mal2scsi
static uint8_t* vsf_mal2scsi_prepare_transact(struct vsfscsi_lun_t *lun,
uint32_t size, bool write, bool bufstream)
{
struct vsfscsi_transact_t *transact = &lun->dev->transact;
struct vsf_scsistream_t *stream = (struct vsf_scsistream_t *)lun->stream;
uint8_t *buffer = stream->mbuf.buffer_list[0];
if (NULL == buffer)
{
return NULL;
}
if (bufstream)
{
struct vsf_bufstream_t *bufstream = (struct vsf_bufstream_t *)stream;
memset(buffer, 0, size);
// setup bufstream
bufstream->mem.buffer.buffer = buffer;
bufstream->mem.buffer.size = size;
bufstream->mem.read = true;
stream->stream.op = &bufstream_op;
stream->stream.callback_tx.param = transact;
stream->stream.callback_tx.on_inout = NULL;
stream->stream.callback_tx.on_connect = NULL;
stream->stream.callback_tx.on_disconnect = NULL;
STREAM_INIT(stream);
}
else
{
struct vsf_mbufstream_t *mbufstream = (struct vsf_mbufstream_t *)stream;
// setup malstream
mbufstream->mem.multibuf = stream->mbuf;
stream->stream.op = &mbufstream_op;
STREAM_INIT(stream);
}
transact->data_size = size;
transact->err = VSFERR_NONE;
transact->lun = lun;
return buffer;
}
static void vsfusbd_MSCBOT_on_idle(void *p)
{
struct vsfusbd_MSCBOT_param_t *param = (struct vsfusbd_MSCBOT_param_t *)p;
param->bufstream.mem.buffer.buffer = (uint8_t *)¶m->CBW;
param->bufstream.mem.buffer.size = sizeof(param->CBW);
param->bufstream.mem.read = false;
param->bufstream.stream.op = &bufstream_op;
STREAM_INIT(¶m->bufstream);
param->transact.ep = param->ep_out;
param->transact.data_size = sizeof(param->CBW);
param->transact.stream = (struct vsf_stream_t *)¶m->bufstream;
param->transact.cb.on_finish = vsfusbd_MSCBOT_on_cbw;
param->transact.cb.param = param;
vsfusbd_ep_recv(param->device, ¶m->transact);
}
void TEST_SendCmd0ToWiredInterface(void)
{
uint8 aOutputBuffer[30];
BinaryStream stOutputStream;
STREAM_INIT(&stOutputStream, aOutputBuffer, sizeof(aOutputBuffer));
uint8 aComposedCommand[sizeof(C000_ReadUniqueIdentifier_Resp)];
uint8 ucRespCode;
// Cmd0 response
ucRespCode = Execute_C000_ReadUniqueIdentifier(NULL,
(C000_ReadUniqueIdentifier_Resp*)aComposedCommand,
MASTER_WIRED_PRIMARY_IDX,
WIRED_CMD,
30);
ComposeResponseToCommmand(MASTER_WIRED_PRIMARY_IDX, (void*)aComposedCommand, CMDID_C000_ReadUniqueIdentifier, NULL, ucRespCode, &stOutputStream);
DLLHART_TRANSMIT_Response(stOutputStream.nextByte - aOutputBuffer, aOutputBuffer);
g_sDllGlobals.m_Token.m_OwnToken = 1;
}
void WHartNetworkData::TransmitIndicate(const WHartLocalStatus& localStatus, WHartPriority priority,
const WHartDllAddress& dllSrc, WHartPayload& npdu)
{
// LOG_DEBUG("TransmitIndicate: localStatus=" << localStatus.status << ", priority=" << priority << ", dllSrc="
// << dllSrc << ", NPDU=" << npdu);
BinaryStream stream;
STREAM_INIT(&stream, npdu.data, npdu.dataLen);
//parse network header
uint8_t controlByte;
STREAM_READ_UINT8(&stream, &controlByte);
uint8_t ttl;
STREAM_READ_UINT8(&stream, &ttl);
uint16_t asn;
STREAM_READ_UINT16(&stream, &asn);
//HACK:[andrei.petrut] - write ASN to common data so it can be used in the next packet...
commonData.lastReadASN = asn;
uint16_t graphID;
STREAM_READ_UINT16(&stream, &graphID);
WHartAddress dest;
StreamReadNetAddress(&stream, dest, (controlByte & CONTROLTYPE_DESTADDRESS_MASK) == 0);
WHartAddress src;
StreamReadNetAddress(&stream, src, (controlByte & CONTROLTYPE_SRCADDRESS_MASK) == 0);
//TODO:[andy] - implement isBroadcast
bool isForMe = (dest == commonData.myNickname || dest == commonData.myUniqueID);
bool isBroadcast = false;
bool isSourceRoute = ((controlByte & CONTROLTYPE_1STSOURCEROUTE_MASK) != 0);
WHartShortAddress proxy = 0;
if (controlByte & CONTROLTYPE_PROXYROUTE_MASK)
{
STREAM_READ_UINT16(&stream, &proxy);
}
Route tempRoute;
if (isSourceRoute)
{
tempRoute.destinationType = Route::dtSourceRoute;
tempRoute.graphID = graphID;
for (int i = 0; i < 4; i++)
{
WHartShortAddress addr;
STREAM_READ_UINT16(&stream, &addr);
if (addr != 0xFFFF)
{
tempRoute.sourceRoutePath.push_back(addr);
}
}
if (controlByte & CONTROLTYPE_2NDSOURCEROUTE_MASK)
{
for (int i = 0; i < 4; i++)
{
WHartShortAddress addr;
STREAM_READ_UINT16(&stream, &addr);
if (addr != 0xFFFF)
{
tempRoute.sourceRoutePath.push_back(addr);
}
}
}
}
// LOG_DEBUG("TransmitIndicate: isForMe=" << (int) isForMe << ", isBroadcast=" << (int) isBroadcast << ", isSourceRoute=" << (int)isSourceRoute << ", destAddress="
// << dest << ", TTL=" << ToStringHexa(ttl) << ", ASN=" << ToStringHexa(asn) << ", graphID="
// << ToStringHexa(graphID));
if (isForMe || isBroadcast)
{
uint8_t securityControl; //contain session
STREAM_READ_UINT8(&stream, &securityControl);
WHartSessionKey::SessionKeyCode sessionKey = (WHartSessionKey::SessionKeyCode) (securityControl & 0x0F);
uint8_t sessionType = securityControl & SECURITYCONTROL_SECURITYTYPE_MASK;
//if join flow detected OnJoinCalback is called, in order to create session with the new device/ap if it is
//provisioned correctly
if ((sessionType == WHartSessionKey::joinKeyed) && (OnJoinCallback))
{
OnJoinCallback(src);
}
SessionTable::iterator session = FindSession(src, sessionKey);
if (session == sessionTable.end())
{
LOG_WARN("Session for Dest=" << src << "with type=" << (int) sessionKey
<< " not found! packet droped.");
return;//invalid session
}
else
{
// LOG_DEBUG("Session used for packet is=" << sessionKey << " and dest=" << src << " and sessionID=" << (int)session->sessionID);
}
uint32_t counter;
if (sessionType == WHartSessionKey::sessionKeyed)
{
uint8_t lsbNonce;
STREAM_READ_UINT8(&stream, &lsbNonce);
// LOG_DEBUG("Read small nonce counter = " << (int)lsbNonce << " . Need to reconstruct nonce counter...");
// LOG_DEBUG("Session counter = " << session->receiveNonceCounter <<
// " Session history size = " << sizeof(session->nonceCounterHistory)<<
// " and compare value=" << ((1 + ((int)session->receiveNonceCounter & 0xFF) - (int)sizeof(session->nonceCounterHistory) * 8)));
counter = ReconstructNonceCounter(*session, lsbNonce);
// LOG_DEBUG("Reconstructed nonce counter=" << counter);
} else
{
STREAM_READ_UINT32(&stream, &counter);
// LOG_DEBUG("Read entire nonce counter=" << counter);
}
// reconstruct nonce counter end
WHartPayload spdu(stream.nextByte, stream.remainingBytes);
uint8_t dataTPDU[CommonData::MAX_PAYLOAD];
WHartPayload tpdu(dataTPDU, sizeof(dataTPDU));
// LOG_DEBUG("Creating PDU with size=" << stream.nextByte - npdu.data << " with stream next byte="
// << nlib::detail::BytesToString(stream.nextByte, stream.nextByte - npdu.data));
CryptoStatus decryptStatus = EncryptDecrypt(*session, counter, WHartPayload(npdu.data, stream.nextByte - npdu.data), spdu,
tpdu, src, dest, false);
if (decryptStatus != whartcsSuccess)
{
LOG_WARN("transmitIndicate failed to decrypt Error=" << CryptoStatusToString(decryptStatus));
return;
}
//reconstruct nonce counter
if (sessionType == WHartSessionKey::joinKeyed) // join flow
{
LOG_DEBUG("Join flow detected.");
session->receiveNonceCounter = counter;
} else
{
int noncePosition = session->receiveNonceCounter - counter;
if (noncePosition > (int) (NONCECOUNTERHISTORY_BIT_LENGTH))
{
//discard packet
LOG_INFO("Received packet with nonce counter outside of the sliding window (nonce=" << counter
<< ", windowStart=" << session->receiveNonceCounter << ", windowSize="
<< (int) NONCECOUNTERHISTORY_BIT_LENGTH << "). Discarding packet...");
return;
}
if (noncePosition < 0)
{
// LOG_DEBUG("New nonce received (old=" << session->receiveNonceCounter << ", new=" << counter << ". Sliding window to accommodate new nonce (with steps=" << -noncePosition << ")...");
if (noncePosition <= -32)
{
session->nonceCounterHistory = 0;
}
else
{
session->nonceCounterHistory = session->nonceCounterHistory >> (-noncePosition);
}
noncePosition = 0;
session->receiveNonceCounter = counter;
}
uint32_t mask = 0x01 << (NONCECOUNTERHISTORY_BIT_LENGTH - noncePosition - 1);
if (session->nonceCounterHistory & mask)
{
// already received packet with this nonce, discard
LOG_WARN("Already received packet with nonce=" << counter << " from src=" << src << ". Discarding...");
return;
}
// mark nonce in history
session->nonceCounterHistory = session->nonceCounterHistory | mask;
}
//forward to upper
++nextRoutedHandler;
WHartHandle handle = MAKE_WHARTHANDLE(1, nextRoutedHandler);
// LOG_DEBUG("transmitIndicate forward to TL" << " handle=" << handle << ", src=" << src << ", priority="
// << priority << ", TPDU=" << tpdu);
upper->TransmitIndicate(handle, src, priority, tpdu, (WHartSessionKey::SessionKeyCode) (securityControl & 0x0F));
} else
// vsfos
static vsf_err_t vsfos_thread(struct vsfsm_pt_t *pt, vsfsm_evt_t evt)
{
struct vsfos_modifs_t *ifs = (struct vsfos_modifs_t *)pt->user_data;
vsf_err_t err;
vsfsm_pt_begin(pt);
vsfhal_core_init(NULL);
vsfhal_tickclk_init();
vsfhal_tickclk_start();
// timer
VSFPOOL_INIT(&ifs->vsftimer_pool, struct vsftimer_t, VSFOSCFG_VSFSM_PENDSVQ_LEN);
vsftimer_init(&ifs->vsftimer_memop);
vsfhal_tickclk_config_cb(vsfos_tickclk_callback_int, NULL);
// file
VSFPOOL_INIT(&ifs->vfsfile_pool, struct vsfile_vfsfile_t, VSFOSCFG_VFS_NO);
vsfile_init(&ifs->fs.memop);
ifs->caller_pt.state = 0;
vsfsm_pt_entry(pt);
err = vsfile_addfile(&ifs->caller_pt, evt, NULL, "msc_root", VSFILE_ATTR_DIRECTORY);
if (err != 0) return err;
ifs->caller_pt.state = 0;
vsfsm_pt_entry(pt);
err = vsfile_getfile(&ifs->caller_pt, evt, NULL, "/msc_root", &ifs->file);
if (err != 0) return err;
ifs->caller_pt.state = 0;
ifs->caller_pt.user_data = &ifs->mal.fakefat32;
err = vsfile_mount(&ifs->caller_pt, evt, (struct vsfile_fsop_t *)&fakefat32_fs_op, ifs->file);
if (err != 0) return err;
// vsfip
{
struct vsfip_buffer_t *buffer;
int i;
buffer = &ifs->tcpip.buffer_pool.buffer[0];
for (i = 0; i < VSFOSCFG_VSFIP_BUFFER_NUM; i++)
{
buffer->buffer = ifs->tcpip.buffer_mem[i];
buffer++;
}
}
VSFPOOL_INIT(&ifs->tcpip.buffer_pool, struct vsfip_buffer_t, VSFOSCFG_VSFIP_BUFFER_NUM);
VSFPOOL_INIT(&ifs->tcpip.socket_pool, struct vsfip_socket_t, VSFOSCFG_VSFIP_SOCKET_NUM);
VSFPOOL_INIT(&ifs->tcpip.tcppcb_pool, struct vsfip_tcppcb_t, VSFOSCFG_VSFIP_TCPPCB_NUM);
vsfip_init((struct vsfip_mem_op_t *)&ifs->tcpip.mem_op);
STREAM_INIT(&ifs->usbd.cdc.stream_rx);
STREAM_INIT(&ifs->usbd.cdc.stream_tx);
vsfscsi_init(&ifs->mal.scsi_dev);
vsfusbd_device_init(&ifs->usbd.device);
vsfshell_init(&ifs->shell);
vsfos_busybox_init(&ifs->shell);
if ((ifs->hwcfg->usbd.pullup.port != IFS_DUMMY_PORT) && (vsfhal_gpio_if != NULL))
{
uint8_t port = ifs->hwcfg->usbd.pullup.port;
uint8_t pin = ifs->hwcfg->usbd.pullup.pin;
vsfhal_gpio_init(port);
vsfhal_gpio_clear(port, 1 << pin);
vsfhal_gpio_config_pin(port, pin, GPIO_OUTPP);
}
ifs->usbd.device.drv->disconnect();
vsfsm_pt_delay(pt, 200);
if ((ifs->hwcfg->usbd.pullup.port != IFS_DUMMY_PORT) && (vsfhal_gpio_if != NULL))
{
uint8_t port = ifs->hwcfg->usbd.pullup.port;
uint8_t pin = ifs->hwcfg->usbd.pullup.pin;
vsfhal_gpio_set(port, 1 << pin);
}
ifs->usbd.device.drv->connect();
while (1)
{
vsfsm_pt_delay(pt, 1000);
asm("nop");
vsfsm_pt_delay(pt, 1000);
asm("nop");
vsfsm_pt_delay(pt, 1000);
asm("nop");
vsfsm_pt_delay(pt, 1000);
asm("nop");
vsfsm_pt_delay(pt, 1000);
asm("nop");
vsfsm_pt_delay(pt, 1000);
asm("nop");
}
vsfsm_pt_end(pt);
return VSFERR_NONE;
}
static struct vsfsm_state_t *
vsfusbd_HID_evt_handler(struct vsfsm_t *sm, vsfsm_evt_t evt)
{
struct vsfusbd_HID_param_t *param =
(struct vsfusbd_HID_param_t *)sm->user_data;
struct vsfusbd_device_t *device = param->device;
uint8_t i;
switch (evt)
{
case VSFSM_EVT_INIT:
if (param->ep_out != 0)
{
vsfusbd_set_OUT_handler(device, param->ep_out,
vsfusbd_HID_OUT_hanlder);
device->drv->ep.enable_OUT(param->ep_out);
}
param->bufstream.mem.read = true;
param->bufstream.stream.op = &bufstream_op;
param->bufstream.stream.callback_tx.on_connect = NULL;
param->bufstream.stream.callback_tx.on_disconnect = NULL;
param->bufstream.stream.callback_tx.on_inout = NULL;
param->output_state = HID_OUTPUT_STATE_WAIT;
param->busy = false;
for(i = 0; i < param->num_of_report; i++)
{
struct vsfusbd_HID_report_t *report = ¶m->reports[i];
report->pos = 0;
report->idle_cnt = 0;
report->changed = true;
}
// enable timer
param->timer4ms.sm = sm;
param->timer4ms.evt = VSFUSBD_HID_EVT_TIMER4MS;
param->timer4ms.interval = 4;
param->timer4ms.trigger_cnt = -1;
vsftimer_enqueue(¶m->timer4ms);
break;
case VSFUSBD_HID_EVT_TIMER4MS:
{
struct vsfusbd_HID_report_t *report;
uint8_t i;
for (i = 0; i < param->num_of_report; i++)
{
report = ¶m->reports[i];
if ((report->type == USB_HID_REPORT_INPUT) &&
(report->idle != 0))
{
report->idle_cnt++;
}
}
if (param->busy)
{
break;
}
}
// fall through
case VSFUSBD_HID_EVT_INREPORT:
{
struct vsfusbd_transact_t *transact = ¶m->IN_transact;
struct vsfusbd_HID_report_t *report;
uint8_t i;
for (i = 0; i < param->num_of_report; i++)
{
report = ¶m->reports[i];
if ((report->type == USB_HID_REPORT_INPUT) &&
(report->changed || ((report->idle != 0) &&
(report->idle_cnt >= report->idle))))
{
param->bufstream.mem.buffer = report->buffer;
STREAM_INIT(¶m->bufstream);
STREAM_CONNECT_TX(¶m->bufstream);
transact->ep = param->ep_in;
transact->data_size = report->buffer.size;
transact->stream = (struct vsf_stream_t *)¶m->bufstream;
transact->zlp = false;
transact->cb.on_finish = vsfusbd_HID_INREPORT_callback;
transact->cb.param = param;
vsfusbd_ep_send(device, transact);
report->changed = false;
report->idle_cnt = 0;
param->busy = true;
break;
}
}
}
break;
}
return NULL;
}
