static void requestConfigurationDescriptor(char line[])
{
// Parse the device index
DWORD devIdx = 0;
line = parseNumber(line, devIdx);
if (!line) {
printf("Please provide a decimal device number following the command\n");
return;
}
if (devIdx >= gDeviceListSize || devIdx < 0) {
printf("Invalid device index '%d' provided\n", devIdx);
return;
}
// Parse the optional configuration number
DWORD configuration = UKW_ACTIVE_CONFIGURATION;
line = parseNumber(line, configuration);
// Parse the optional buffer size number
DWORD bufferLength = MAX_CONFIG_BUFFER;
if (line) {
// See if there is another parameter for buffer length
line = parseNumber(line, bufferLength);
if (bufferLength > MAX_CONFIG_BUFFER) {
printf("Provided buffer length is too large, the maximum is %d\n", MAX_CONFIG_BUFFER);
return;
}
}
// Print the command which is about to be issued
if (configuration == UKW_ACTIVE_CONFIGURATION) {
printf("No configuration specified, reading active configuration from device %d, length %d\n",
devIdx, bufferLength);
} else {
printf("Requesting descriptor for configuration %d from device %d, length %d\n",
configuration, devIdx, bufferLength);
}
// All parameters decoded, issue command
UKW_DEVICE device = gDeviceList[devIdx];
unsigned char descBuf[MAX_CONFIG_BUFFER];
DWORD descLength = 0;
BOOL status = UkwGetConfigDescriptor(device, configuration, descBuf, bufferLength, &descLength);
if (status) {
printf("Retrieved descriptor of length %d\n", descLength);
printHexDump(descBuf, descLength);
} else {
printf("Failed to retrieve configuration descriptor with error %d\n", GetLastError());
}
}
/*
* Send a message to a client of the task handler.
*
* pHeader the header containing details
* of the destination for the
* response.
* msgType the message type to send.
* pSendMsgBody pointer to the body of the
* REquest message to send.
* May be PNULL.
* sendMsgBodyLength the length of the data that
* pSendMsg points to.
*
* @return true if the message send is
* successful otherwise false.
*/
Bool taskHandlerResponder (RoboOneTaskReqHeader *pHeader, TaskHandlerMsgType msgType, void *pSendMsgBody, UInt16 sendMsgBodyLength)
{
ClientReturnCode returnCode;
Bool success = false;
Msg *pSendMsg;
Char *pIpAddress = "127.0.0.1";
ASSERT_PARAM (pHeader != PNULL, (unsigned long) pHeader);
ASSERT_PARAM (msgType < MAX_NUM_TASK_HANDLER_MSGS, msgType);
ASSERT_PARAM (sendMsgBodyLength <= MAX_MSG_BODY_LENGTH, sendMsgBodyLength);
pSendMsg = malloc (sizeof (*pSendMsg));
if (pSendMsg != PNULL)
{
if (pHeader->sourceServerIpAddressStringPresent)
{
pIpAddress = &(pHeader->sourceServerIpAddressString[0]);
}
/* Put in the bit before the body */
pSendMsg->msgLength = 0;
pSendMsg->msgType = msgType;
pSendMsg->msgLength += sizeof (pSendMsg->msgType);
/* Put in any body to send */
if (pSendMsgBody != PNULL)
{
memcpy (&pSendMsg->msgBody[0], pSendMsgBody, sendMsgBodyLength);
}
pSendMsg->msgLength += sendMsgBodyLength;
printDebug ("TH Responder: sending message %s, length %d, to port %d, IP address %s, hex dump:\n", pgTaskHandlerMessageNames[pSendMsg->msgType], pSendMsg->msgLength, pHeader->sourceServerPort, pIpAddress);
printHexDump (pSendMsg, pSendMsg->msgLength + 1);
returnCode = runMessagingClient (pHeader->sourceServerPort, pIpAddress, pSendMsg, PNULL);
printDebug ("TH Responder: message system returnCode: %d\n", returnCode);
if (returnCode == CLIENT_SUCCESS)
{
success = true;
}
free (pSendMsg);
}
return success;
}
/*
* Handle a whole message received from the client
* and send back a response.
*
* pReceivedMsg a pointer to the buffer containing the
* incoming message.
* pSendMsg a pointer to a message buffer to put
* the response into. Not touched if return
* code is a failure one.
*
* @return whatever doAction() returns.
*/
ServerReturnCode serverHandleMsg (Msg *pReceivedMsg, Msg *pSendMsg)
{
ServerReturnCode returnCode;
ASSERT_PARAM (pReceivedMsg != PNULL, (unsigned long) pReceivedMsg);
ASSERT_PARAM (pSendMsg != PNULL, (unsigned long) pSendMsg);
/* Check the type */
ASSERT_PARAM (pReceivedMsg->msgType < MAX_NUM_HARDWARE_MSGS, pReceivedMsg->msgType);
printDebug ("HW Server received message %s, length %d.\n", pgHardwareMessageNames[pReceivedMsg->msgType], pReceivedMsg->msgLength);
printHexDump (pReceivedMsg, pReceivedMsg->msgLength + 1);
/* Do the thang */
returnCode = doAction ((HardwareMsgType) pReceivedMsg->msgType, pReceivedMsg->msgBody, pSendMsg);
printDebug ("HW Server responding with message %s, length %d.\n", pgHardwareMessageNames[pSendMsg->msgType], pSendMsg->msgLength);
return returnCode;
}
/*
* Send a timer expiry message.
*
* pTimer the timer related to the
* expiry message to send.
*
* @return true if the message send is
* is successful, otherwise false.
*/
static Bool sendTimerExpiryMsg (Timer *pTimer)
{
ClientReturnCode returnCode;
Bool success = false;
ASSERT_PARAM (pTimer != PNULL, (unsigned long) pTimer);
printDebug ("Timer Server: sending expiry message to port %d, msgType 0x%08x, length %d, hex dump:\n", pTimer->sourcePort, pTimer->expiryMsg.msgType, pTimer->expiryMsg.msgLength);
printHexDump (&(pTimer->expiryMsg), pTimer->expiryMsg.msgLength + 1);
returnCode = runMessagingClient (pTimer->sourcePort, PNULL, &(pTimer->expiryMsg), PNULL);
printDebug ("Timer Server: message system returnCode: %d\n", returnCode);
if (returnCode == CLIENT_SUCCESS)
{
success = true;
}
return success;
}
/*
* Handle a whole message received from the client
* and send back a response.
*
* pReceivedMsg a pointer to the buffer containing the
* incoming message.
* pSendMsg a pointer to a message buffer to put
* the response into. Not touched if return
* code is a failure one.
*
* @return whatever doAction() returns.
*/
ServerReturnCode serverHandleMsg (Msg *pReceivedMsg, Msg *pSendMsg)
{
ServerReturnCode returnCode;
ASSERT_PARAM (pReceivedMsg != PNULL, (unsigned long) pReceivedMsg);
ASSERT_PARAM (pSendMsg != PNULL, (unsigned long) pSendMsg);
/* Check the type */
ASSERT_PARAM (pReceivedMsg->msgType < MAX_NUM_TASK_HANDLER_MSGS, pReceivedMsg->msgType);
printDebug ("TH Server received message %s, length %d.\n", pgTaskHandlerMessageNames[pReceivedMsg->msgType], pReceivedMsg->msgLength);
printHexDump (pReceivedMsg, pReceivedMsg->msgLength + 1);
/* Do the thang */
returnCode = doAction ((TaskHandlerMsgType) pReceivedMsg->msgType, pReceivedMsg->msgBody, pSendMsg);
if (pSendMsg->msgLength > 0)
{
printDebug ("TH Server responding with message %s, length %d.\n", pgTaskHandlerMessageNames[pSendMsg->msgType], pSendMsg->msgLength);
}
return returnCode;
}
int writeKey(const char * resource_id, unsigned int user, unsigned int seat, unsigned int ldbid, unsigned char* buffer, unsigned int doHexdump)
{
int rval = 0, size = 0;
int numPerRow = 8;
printf("- %s - User: %u - Seat: %u - ldbid: 0x%x\n\n", __FUNCTION__, user, seat, ldbid);
printf(" ResourceID: \"%s\"\n", resource_id);
printf(" Data : \"%s\"\n", buffer);
size = pclKeyWriteData(ldbid, resource_id, user, seat, buffer, (int)strlen((char*)buffer));
if(doHexdump == 1)
{
if(size >= 120)
numPerRow = 16;
printHexDump(buffer, numPerRow);
}
return rval;
}
/*
* Handle a whole message received from the client.
*
* pReceivedMsg a pointer to the buffer containing the
* incoming message.
* pSendMsg a pointer to a message buffer to put
* the response into, may be PNULL as
* this server never sends responses.
*
* @return whatever doAction() returns.
*/
ServerReturnCode serverHandleMsg (Msg *pReceivedMsg, Msg *pSendMsg)
{
ServerReturnCode returnCode;
ASSERT_PARAM (pReceivedMsg != PNULL, (unsigned long) pReceivedMsg);
/* Never return any confirmations so set this length to zero */
if (pSendMsg != PNULL)
{
pSendMsg->msgLength = 0;
}
/* Check the type */
ASSERT_PARAM (pReceivedMsg->msgType < MAX_NUM_TIMER_MSGS, pReceivedMsg->msgType);
printDebug ("T Server received message %s, length %d.\n", pgTimerMessageNames[pReceivedMsg->msgType], pReceivedMsg->msgLength);
printHexDump (pReceivedMsg, pReceivedMsg->msgLength + 1);
/* Do the thang */
returnCode = doAction ((TimerMsgType) pReceivedMsg->msgType, pReceivedMsg->msgBody);
return returnCode;
}
int readKey(const char * resource_id, unsigned int user, unsigned int seat, unsigned int ldbid, unsigned int doHexdump, unsigned char* buffer, int size)
{
int rval = 0;
int numPerRow = 8;
printf("- %s - User: %u - Seat: %u - ldbid: 0x%x\n\n", __FUNCTION__, user, seat, ldbid);
printf(" ResourceID: \"%s\"\n", resource_id);
rval = pclKeyReadData(ldbid, resource_id, user, seat, buffer, size);
if(rval >=0)
printf(" Data : \"%s\"\n", buffer);
else
printf("Failed to read: %d\n", rval);
if(doHexdump == 1)
{
if(size >= 120)
numPerRow = 16;
printHexDump(buffer, numPerRow);
}
return rval;
}
int main(int argc, const char** argv)
{
if (argc < 2)
{
std::cerr << "Usage: " << argv[0] << " <node-id>" << std::endl;
return 1;
}
const int self_node_id = std::stoi(argv[1]);
/*
* Initializing the node.
* Hardware and software version information is paramount for firmware update process.
*/
uavcan::Node<16384> node(getCanDriver(), getSystemClock());
node.setNodeID(self_node_id);
node.setName("org.uavcan.tutorial.updatee");
uavcan::protocol::HardwareVersion hwver; // TODO initialize correct values
hwver.major = 1;
node.setHardwareVersion(hwver);
uavcan::protocol::SoftwareVersion swver; // TODO initialize correct values
swver.major = 1;
node.setSoftwareVersion(swver);
const int node_start_res = node.start();
if (node_start_res < 0)
{
throw std::runtime_error("Failed to start the node; error: " + std::to_string(node_start_res));
}
/*
* Storage for the firmware downloader object.
* Can be replaced with a smart pointer instead.
*/
uavcan::LazyConstructor<FirmwareLoader> fw_loader;
/*
* Initializing the BeginFirmwareUpdate server.
*/
uavcan::ServiceServer<uavcan::protocol::file::BeginFirmwareUpdate> bfu_server(node);
const int bfu_res = bfu_server.start(
[&fw_loader, &node]
(const uavcan::ReceivedDataStructure<uavcan::protocol::file::BeginFirmwareUpdate::Request>& req,
uavcan::protocol::file::BeginFirmwareUpdate::Response resp)
{
std::cout << "Firmware update request:\n" << req << std::endl;
if (fw_loader.isConstructed())
{
resp.error = resp.ERROR_IN_PROGRESS;
}
else
{
const auto source_node_id = (req.source_node_id == 0) ? req.getSrcNodeID() : req.source_node_id;
fw_loader.construct<uavcan::INode&, uavcan::NodeID, decltype(req.image_file_remote_path.path)>
(node, source_node_id, req.image_file_remote_path.path);
}
std::cout << "Response:\n" << resp << std::endl;
});
if (bfu_res < 0)
{
throw std::runtime_error("Failed to start the BeginFirmwareUpdate server: " + std::to_string(bfu_res));
}
/*
* Running the node normally.
* All of the work will be done in background.
*/
while (true)
{
if (fw_loader.isConstructed())
{
node.setModeSoftwareUpdate();
if (fw_loader->getStatus() != FirmwareLoader::Status::InProgress)
{
if (fw_loader->getStatus() == FirmwareLoader::Status::Success)
{
auto image = fw_loader->getImage();
std::cout << "Firmware download succeeded [" << image.size() << " bytes]" << std::endl;
printHexDump(std::begin(image), std::end(image));
// TODO: save the firmware image somewhere.
}
else
{
std::cout << "Firmware download failed" << std::endl;
// TODO: handle the error, e.g. retry download, send a log message, etc.
}
fw_loader.destroy();
}
}
else
{
node.setModeOperational();
}
const int res = node.spin(uavcan::MonotonicDuration::fromMSec(500));
if (res < 0)
{
std::cerr << "Transient failure: " << res << std::endl;
}
}
}
//
// Process the arguments given a pointer to a typetag field (including comma).
// Specify in 'argOffset' where the arguments start.
//
void osc_dispatch_callbacks(char * typetag, int argOffset, int maxLength) {
int * intVal = NULL;
int argNum = 1;
UARTprintf("Packet:\n");
printHexDump(typetag, 32);
UARTprintf("osc_dispatch_callbacks(\"%s\", %d, %d)\n", typetag, argOffset, maxLength);
if (typetag[0] != ',') {
UARTprintf("[WARNING] This lacks a comma and is thus not a typetag?\n");
}
while (typetag[argNum] && argNum < maxLength) {
switch(typetag[argNum]) {
case 'i': // 32-bit integer
intVal = (int *) (typetag + argOffset);
g_osc_state.intCallback(argNum, ntohl(*intVal));
argOffset += 4;
break;
case 'f': // 32-bit float
g_osc_state.floatCallback(argNum, bigEndianFloat(typetag + argOffset));
argOffset += 4;
break;
case 'S': // Symbol
case 's': // String
g_osc_state.stringCallback(argNum, typetag + argOffset, maxLength - argOffset);
// For some reason, strnlen is implicitly declared here though
// I've #included string.h. The code still links and runs though.
argOffset += strnlen(typetag + argOffset, maxLength - argOffset);
// Pad to be a multiple of 4 bytes (as OSC strings must be)
argOffset = (argOffset + 3) & ~0x03;
break;
case 'b': // Binary blob (32-bit size comes first)
// This value gives the length of the blob in bytes.
intVal = (int *) (typetag + argOffset);
argOffset += 4;
g_osc_state.blobCallback(argNum, typetag + argOffset, ntohl(*intVal));
argOffset += *intVal;
// Pad to be a multiple of 4 bytes (as blob data must also be)
argOffset = (argOffset + 3) & ~0x03;
break;
case 'h':
// Untested (oscP5 won't send longs?)
{
// long val = bigEndianLong(typetag + argOffset);
// Can't print longs this way:
// UARTprintf("Arg %d: Long %ld\n", argNum, val);
}
argOffset += 8;
break;
case 't':
UARTprintf("[ERROR] Arg %d is a timetag; not implemented!\n", argNum);
break;
case 'd':
// Untested
{
//double val = bigEndianDouble(typetag + argOffset);
UARTprintf("Arg %d: Double (not printing value)\n", argNum);
}
argOffset += 8;
break;
case 'c':
// Character is ASCII, but represented in 32 bits
{
int * tmp = (int *) (typetag + argOffset);
UARTprintf("Arg %d: ASCII character %d\n", argNum, ntohl(*tmp));
}
argOffset += 4;
break;
case 'r':
// Untested
UARTprintf("Arg %d: RGBA color ", argNum);
printHexDump(typetag + argOffset, 4);
argOffset += 4;
// {
// intVal = (int *) (typetag + argOffset);
// ntohl(*intVal) ...
// }
break;
case 'm':
// Untested
UARTprintf("Arg %d: 4 bytes of MIDI data, ", argNum);
printHexDump(typetag + argOffset, 4);
argOffset += 4;
break;
// For args T, F, N, I, no bytes are allocated.
case 'T':
UARTprintf("Arg %d: Boolean true\n", argNum);
break;
case 'F':
UARTprintf("Arg %d: Boolean false\n", argNum);
break;
case 'N':
// Untested
UARTprintf("Arg %d: Nil\n", argNum);
break;
case 'I':
// Untested
UARTprintf("Arg %d: Infinitum\n", argNum);
break;
// Arrays (not handled):
case '[':
case ']':
UARTprintf("Arrays are not supported! \n", argNum);
break;
default:
{
char tmp[2];
tmp[1] = 0;
tmp[0] = typetag[argNum];
UARTprintf("Arg %d has unknown typetag %s\n", argNum, tmp);
}
break;
}
++argNum;
}
}
bool genClientContext(
BYTE* apbIn,
DWORD adwIn,
BYTE* apbOut,
DWORD* apdwOut,
bool* apfDone,
char* astrTarget,
CredHandle* aphCred,
SecHandle* aphContext)
{
TimeStamp tsLifetime;
SECURITY_STATUS ssResult = 0;
if(!apbIn)
{
SCHANNEL_CRED schCred = {0};
schCred.dwVersion = SCHANNEL_CRED_VERSION;
schCred.cCreds = 0;
schCred.paCred = NULL;
schCred.hRootStore = 0;
schCred.grbitEnabledProtocols = SP_PROT_TLS1_2;
/*schCred.dwFlags = SCH_CRED_NO_SERVERNAME_CHECK
| SCH_CRED_MANUAL_CRED_VALIDATION;*/
ssResult = ::AcquireCredentialsHandle(
NULL,
"Schannel",
SECPKG_CRED_OUTBOUND,
NULL,
&schCred,
NULL,
NULL,
aphCred,
&tsLifetime);
if(ssResult != SEC_E_OK)
{
// SEC_E_INSUFFICIENT_MEMORY, SEC_E_INTERNAL_ERROR,
// SEC_E_NO_CREDENTIALS, SEC_E_NOT_OWNER,
// SEC_E_SECPKG_NOT_FOUND, SEC_E_UNKNOWN_CREDENTIALS
std::cout << std::hex <<
"Error in ::AcquireCredentialsHandle = " << ssResult << '\n';
return false;
}
}
SecBufferDesc outBuffDesc = {0};
SecBuffer outSecBuff = {0};
SecBufferDesc inBuffDesc = {0};
SecBuffer inSecBuff[2] = {0};
// prepare output
outBuffDesc.ulVersion = 0;
outBuffDesc.cBuffers = 1;
outBuffDesc.pBuffers = &outSecBuff;
outSecBuff.cbBuffer = *apdwOut;
outSecBuff.BufferType = SECBUFFER_TOKEN;
outSecBuff.pvBuffer = apbOut;
ULONG ulContextAttr = 0;
if(apbIn)
{
// prepare input
inBuffDesc.ulVersion = 0;
inBuffDesc.cBuffers = 2;
inBuffDesc.pBuffers = inSecBuff;
inSecBuff[0].BufferType = SECBUFFER_TOKEN;
inSecBuff[0].cbBuffer = adwIn;
inSecBuff[0].pvBuffer = apbIn;
inSecBuff[1].BufferType = SECBUFFER_EMPTY;
inSecBuff[1].cbBuffer = 0;
inSecBuff[1].pvBuffer = NULL;
ssResult = ::InitializeSecurityContext(
aphCred,
aphContext,
astrTarget,
ISC_REQ_MANUAL_CRED_VALIDATION,
0,
0, // not used in Schannel
&inBuffDesc,
0,
NULL,
&outBuffDesc,
&ulContextAttr,
&tsLifetime);
}
else
{
ssResult = ::InitializeSecurityContext(
aphCred,
NULL,
astrTarget,
ISC_REQ_MANUAL_CRED_VALIDATION,
0,
0,
NULL,
0,
aphContext,
&outBuffDesc,
&ulContextAttr,
&tsLifetime);
}
if(ssResult < 0)
{
std::cout << std::hex
<< "error in ::InitializeSecurityContext = " << ssResult << '\n';
return false;
}
if(ssResult == SEC_I_COMPLETE_NEEDED ||
ssResult == SEC_I_COMPLETE_AND_CONTINUE)
{
ssResult = ::CompleteAuthToken(aphContext, &outBuffDesc);
if(ssResult < 0)
{
std::cout << std::hex <<
"Error in ::CompleteAuthToken = " << ssResult << '\n';
return false;
}
}
*apfDone = !(
ssResult == SEC_I_CONTINUE_NEEDED ||
ssResult == SEC_I_COMPLETE_NEEDED ||
ssResult == SEC_I_COMPLETE_AND_CONTINUE);
*apdwOut = outSecBuff.cbBuffer;
std::cout << "Token buffer generated " << outSecBuff.cbBuffer
<< " bytes:\n";
printHexDump(outSecBuff.cbBuffer, (BYTE*)outSecBuff.pvBuffer);
std::cout << std::hex
<< "::InitializeSecurityContext result = "
<< ssResult << '\n';
return true;
}
bool sendEncrypted(
SOCKET aSocket,
const void* apOutMessage,
size_t aszMsgLength)
{
size_t szTotalSize = getStreamSizes().cbHeader
+ getStreamSizes().cbMaximumMessage
+ getStreamSizes().cbTrailer;
std::vector<BYTE> vBuffer(szTotalSize, 0);
SecBufferDesc buffDesc = {0};
SecBuffer secBuff[4] = {0};
buffDesc.cBuffers = 4;
buffDesc.pBuffers = secBuff;
secBuff[0].BufferType = SECBUFFER_STREAM_HEADER;
secBuff[0].cbBuffer = getStreamSizes().cbHeader;
secBuff[0].pvBuffer = &vBuffer[0];
secBuff[1].BufferType = SECBUFFER_DATA;
secBuff[2].BufferType = SECBUFFER_STREAM_TRAILER;
secBuff[3].BufferType = SECBUFFER_EMPTY;
secBuff[3].pvBuffer = NULL;
secBuff[3].cbBuffer = 0;
while(aszMsgLength > 0)
{
size_t szEncryptSize = min(
aszMsgLength,
getStreamSizes().cbMaximumMessage);
secBuff[1].cbBuffer = szEncryptSize;
secBuff[1].pvBuffer = &vBuffer[0] + secBuff[0].cbBuffer;
memcpy(secBuff[1].pvBuffer, apOutMessage, szEncryptSize);
secBuff[2].cbBuffer = getStreamSizes().cbTrailer;
secBuff[2].pvBuffer = static_cast<BYTE*>(secBuff[1].pvBuffer)
+ szEncryptSize;
SECURITY_STATUS ssResult = ::EncryptMessage(
&g_hContext,
0,
&buffDesc,
0);
if(ssResult != SEC_E_OK)
{
std::cout << std::hex <<
"Error in ::EncryptMessage = " << ssResult << '\n';
return false;
}
std::cout << "\nEncrypted:\n";
printHexDump(
secBuff[0].cbBuffer + secBuff[1].cbBuffer + secBuff[2].cbBuffer,
static_cast<BYTE*>(secBuff[0].pvBuffer));
bool fResult = SendMsg(
aSocket,
static_cast<BYTE*>(secBuff[0].pvBuffer),
secBuff[0].cbBuffer + secBuff[1].cbBuffer + secBuff[2].cbBuffer);
if(!fResult)
{
std::cout << "cannot send message\n";
return false;
}
apOutMessage = static_cast<const BYTE*>(apOutMessage) + szEncryptSize;
aszMsgLength -= szEncryptSize;
}
return true;
}
