/**
* System task, periodically executes every SYSTEM_UPDATE_PERIOD_MS
*/
static void systemTask(void *parameters)
{
/* create all modules thread */
MODULE_TASKCREATE_ALL;
if (PIOS_heap_malloc_failed_p()) {
/* We failed to malloc during task creation,
* system behaviour is undefined. Reset and let
* the BootFault code recover for us.
*/
PIOS_SYS_Reset();
}
#if defined(PIOS_INCLUDE_IAP)
/* Record a successful boot */
PIOS_IAP_WriteBootCount(0);
#endif
// Initialize vars
idleCounter = 0;
idleCounterClear = 0;
// Listen for SettingPersistance object updates, connect a callback function
ObjectPersistenceConnectQueue(objectPersistenceQueue);
#if (defined(COPTERCONTROL) || defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
// Run this initially to make sure the configuration is checked
configuration_check();
// Whenever the configuration changes, make sure it is safe to fly
if (StabilizationSettingsHandle())
StabilizationSettingsConnectCallback(configurationUpdatedCb);
if (SystemSettingsHandle())
SystemSettingsConnectCallback(configurationUpdatedCb);
if (ManualControlSettingsHandle())
ManualControlSettingsConnectCallback(configurationUpdatedCb);
if (FlightStatusHandle())
FlightStatusConnectCallback(configurationUpdatedCb);
#endif
#if (defined(REVOLUTION) || defined(SIM_OSX)) && ! (defined(SIM_POSIX))
if (StateEstimationHandle())
StateEstimationConnectCallback(configurationUpdatedCb);
#endif
// Main system loop
while (1) {
// Update the system statistics
updateStats();
// Update the system alarms
updateSystemAlarms();
#if defined(WDG_STATS_DIAGNOSTICS)
updateWDGstats();
#endif
#if defined(DIAG_TASKS)
// Update the task status object
TaskMonitorUpdateAll();
#endif
// Flash the heartbeat LED
#if defined(PIOS_LED_HEARTBEAT)
PIOS_LED_Toggle(PIOS_LED_HEARTBEAT);
DEBUG_MSG("+ 0x%08x\r\n", 0xDEADBEEF);
#endif /* PIOS_LED_HEARTBEAT */
// Turn on the error LED if an alarm is set
if (indicateError()) {
#if defined (PIOS_LED_ALARM)
PIOS_LED_On(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
} else {
#if defined (PIOS_LED_ALARM)
PIOS_LED_Off(PIOS_LED_ALARM);
#endif /* PIOS_LED_ALARM */
}
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
UAVObjEvent ev;
int delayTime = flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMED ?
SYSTEM_UPDATE_PERIOD_MS / (LED_BLINK_RATE_HZ * 2) :
SYSTEM_UPDATE_PERIOD_MS;
if (PIOS_Queue_Receive(objectPersistenceQueue, &ev, delayTime) == true) {
// If object persistence is updated call the callback
objectUpdatedCb(&ev);
}
}
}
void SchannelContext::encryptAndSendData(const SafeByteArray& data)
{
if (m_streamSizes.cbMaximumMessage == 0)
return;
SecBuffer outBuffers[4] = {0};
// Calculate the largest required size of the send buffer
size_t messageBufferSize = (data.size() > m_streamSizes.cbMaximumMessage)
? m_streamSizes.cbMaximumMessage
: data.size();
// Allocate a packet for the encrypted data
SafeByteArray sendBuffer;
sendBuffer.resize(m_streamSizes.cbHeader + messageBufferSize + m_streamSizes.cbTrailer);
size_t bytesSent = 0;
do
{
size_t bytesLeftToSend = data.size() - bytesSent;
// Calculate how much of the send buffer we'll be using for this chunk
size_t bytesToSend = (bytesLeftToSend > m_streamSizes.cbMaximumMessage)
? m_streamSizes.cbMaximumMessage
: bytesLeftToSend;
// Copy the plain text data into the send buffer
memcpy(&sendBuffer[0] + m_streamSizes.cbHeader, &data[0] + bytesSent, bytesToSend);
outBuffers[0].pvBuffer = &sendBuffer[0];
outBuffers[0].cbBuffer = m_streamSizes.cbHeader;
outBuffers[0].BufferType = SECBUFFER_STREAM_HEADER;
outBuffers[1].pvBuffer = &sendBuffer[0] + m_streamSizes.cbHeader;
outBuffers[1].cbBuffer = (unsigned long)bytesToSend;
outBuffers[1].BufferType = SECBUFFER_DATA;
outBuffers[2].pvBuffer = &sendBuffer[0] + m_streamSizes.cbHeader + bytesToSend;
outBuffers[2].cbBuffer = m_streamSizes.cbTrailer;
outBuffers[2].BufferType = SECBUFFER_STREAM_TRAILER;
outBuffers[3].pvBuffer = 0;
outBuffers[3].cbBuffer = 0;
outBuffers[3].BufferType = SECBUFFER_EMPTY;
SecBufferDesc outBufferDesc = {0};
outBufferDesc.cBuffers = 4;
outBufferDesc.pBuffers = outBuffers;
outBufferDesc.ulVersion = SECBUFFER_VERSION;
SECURITY_STATUS status = EncryptMessage(m_ctxtHandle, 0, &outBufferDesc, 0);
if (status != SEC_E_OK)
{
indicateError();
return;
}
sendDataOnNetwork(&sendBuffer[0], outBuffers[0].cbBuffer + outBuffers[1].cbBuffer + outBuffers[2].cbBuffer);
bytesSent += bytesToSend;
} while (bytesSent < data.size());
}
void SchannelContext::decryptAndProcessData(const SafeByteArray& data)
{
SecBuffer inBuffers[4] = {0};
appendNewData(data);
while (!m_receivedData.empty())
{
//
// MSDN:
// When using the Schannel SSP with contexts that are not connection oriented, on input,
// the structure must contain four SecBuffer structures. Exactly one buffer must be of type
// SECBUFFER_DATA and contain an encrypted message, which is decrypted in place. The remaining
// buffers are used for output and must be of type SECBUFFER_EMPTY. For connection-oriented
// contexts, a SECBUFFER_DATA type buffer must be supplied, as noted for nonconnection-oriented
// contexts. Additionally, a second SECBUFFER_TOKEN type buffer that contains a security token
// must also be supplied.
//
inBuffers[0].pvBuffer = (char*)(&m_receivedData[0]);
inBuffers[0].cbBuffer = (unsigned long)m_receivedData.size();
inBuffers[0].BufferType = SECBUFFER_DATA;
inBuffers[1].BufferType = SECBUFFER_EMPTY;
inBuffers[2].BufferType = SECBUFFER_EMPTY;
inBuffers[3].BufferType = SECBUFFER_EMPTY;
SecBufferDesc inBufferDesc = {0};
inBufferDesc.cBuffers = 4;
inBufferDesc.pBuffers = inBuffers;
inBufferDesc.ulVersion = SECBUFFER_VERSION;
size_t inData = m_receivedData.size();
SECURITY_STATUS status = DecryptMessage(m_ctxtHandle, &inBufferDesc, 0, NULL);
if (status == SEC_E_INCOMPLETE_MESSAGE)
{
// Wait for more data to arrive
break;
}
else if (status == SEC_I_RENEGOTIATE)
{
// TODO: Handle renegotiation scenarios
indicateError();
break;
}
else if (status == SEC_I_CONTEXT_EXPIRED)
{
indicateError();
break;
}
else if (status != SEC_E_OK)
{
indicateError();
break;
}
SecBuffer* pDataBuffer = NULL;
SecBuffer* pExtraBuffer = NULL;
for (int i = 0; i < 4; ++i)
{
if (inBuffers[i].BufferType == SECBUFFER_DATA)
pDataBuffer = &inBuffers[i];
else if (inBuffers[i].BufferType == SECBUFFER_EXTRA)
pExtraBuffer = &inBuffers[i];
}
if (pDataBuffer && pDataBuffer->cbBuffer > 0 && pDataBuffer->pvBuffer != NULL)
forwardDataToApplication(pDataBuffer->pvBuffer, pDataBuffer->cbBuffer);
// If there is extra data left over from the decryption operation, we call DecryptMessage() again
if (pExtraBuffer)
{
m_receivedData.erase(m_receivedData.begin(), m_receivedData.end() - pExtraBuffer->cbBuffer);
}
else
{
// We're done
m_receivedData.erase(m_receivedData.begin(), m_receivedData.begin() + inData);
}
}
}
void SchannelContext::connect()
{
ScopedCertContext pCertContext;
m_state = Connecting;
// If a user name is specified, then attempt to find a client
// certificate. Otherwise, just create a NULL credential.
if (!m_cert_name.empty())
{
if (m_my_cert_store == NULL)
{
m_my_cert_store = CertOpenSystemStore(0, m_cert_store_name.c_str());
if (!m_my_cert_store)
{
///// printf( "**** Error 0x%x returned by CertOpenSystemStore\n", GetLastError() );
indicateError();
return;
}
}
pCertContext = findCertificateInStore( m_my_cert_store, m_cert_name );
if (pCertContext == NULL)
{
///// printf("**** Error 0x%x returned by CertFindCertificateInStore\n", GetLastError());
indicateError();
return;
}
}
// We use an empty list for client certificates
PCCERT_CONTEXT clientCerts[1] = {0};
SCHANNEL_CRED sc = {0};
sc.dwVersion = SCHANNEL_CRED_VERSION;
/////SSL3?
sc.grbitEnabledProtocols = SP_PROT_SSL3_CLIENT | SP_PROT_TLS1_CLIENT | SP_PROT_TLS1_1_CLIENT | SP_PROT_TLS1_2_CLIENT;
sc.dwFlags = SCH_CRED_MANUAL_CRED_VALIDATION;
if (pCertContext)
{
sc.cCreds = 1;
sc.paCred = pCertContext.GetPointer();
sc.dwFlags |= SCH_CRED_NO_DEFAULT_CREDS;
}
else
{
sc.cCreds = 0; // Let Crypto API find the appropriate certificate for us
sc.paCred = clientCerts;
sc.dwFlags |= SCH_CRED_USE_DEFAULT_CREDS;
}
// Swiften performs the server name check for us
sc.dwFlags |= SCH_CRED_NO_SERVERNAME_CHECK;
SECURITY_STATUS status = AcquireCredentialsHandle(
NULL,
UNISP_NAME,
SECPKG_CRED_OUTBOUND,
NULL,
&sc,
NULL,
NULL,
m_credHandle.Reset(),
NULL);
if (status != SEC_E_OK)
{
// We failed to obtain the credentials handle
indicateError();
return;
}
SecBuffer outBuffers[2];
// We let Schannel allocate the output buffer for us
outBuffers[0].pvBuffer = NULL;
outBuffers[0].cbBuffer = 0;
outBuffers[0].BufferType = SECBUFFER_TOKEN;
// Contains alert data if an alert is generated
outBuffers[1].pvBuffer = NULL;
outBuffers[1].cbBuffer = 0;
outBuffers[1].BufferType = SECBUFFER_ALERT;
// Make sure the output buffers are freed
ScopedSecBuffer scopedOutputData(&outBuffers[0]);
ScopedSecBuffer scopedOutputAlertData(&outBuffers[1]);
SecBufferDesc outBufferDesc = {0};
outBufferDesc.cBuffers = 2;
outBufferDesc.pBuffers = outBuffers;
outBufferDesc.ulVersion = SECBUFFER_VERSION;
// Create the initial security context
status = InitializeSecurityContext(
m_credHandle,
NULL,
NULL,
m_ctxtFlags,
0,
0,
NULL,
0,
m_ctxtHandle.Reset(),
&outBufferDesc,
&m_secContext,
NULL);
if (status != SEC_E_OK && status != SEC_I_CONTINUE_NEEDED)
{
// We failed to initialize the security context
handleCertError(status);
indicateError();
return;
}
// Start the handshake
sendDataOnNetwork(outBuffers[0].pvBuffer, outBuffers[0].cbBuffer);
if (status == SEC_E_OK)
{
status = validateServerCertificate();
if (status != SEC_E_OK)
handleCertError(status);
m_state = Connected;
determineStreamSizes();
onConnected();
}
}
void SchannelContext::continueHandshake(const SafeByteArray& data)
{
appendNewData(data);
while (!m_receivedData.empty())
{
SecBuffer inBuffers[2];
// Provide Schannel with the remote host's handshake data
inBuffers[0].pvBuffer = (char*)(&m_receivedData[0]);
inBuffers[0].cbBuffer = (unsigned long)m_receivedData.size();
inBuffers[0].BufferType = SECBUFFER_TOKEN;
inBuffers[1].pvBuffer = NULL;
inBuffers[1].cbBuffer = 0;
inBuffers[1].BufferType = SECBUFFER_EMPTY;
SecBufferDesc inBufferDesc = {0};
inBufferDesc.cBuffers = 2;
inBufferDesc.pBuffers = inBuffers;
inBufferDesc.ulVersion = SECBUFFER_VERSION;
SecBuffer outBuffers[2];
// We let Schannel allocate the output buffer for us
outBuffers[0].pvBuffer = NULL;
outBuffers[0].cbBuffer = 0;
outBuffers[0].BufferType = SECBUFFER_TOKEN;
// Contains alert data if an alert is generated
outBuffers[1].pvBuffer = NULL;
outBuffers[1].cbBuffer = 0;
outBuffers[1].BufferType = SECBUFFER_ALERT;
// Make sure the output buffers are freed
ScopedSecBuffer scopedOutputData(&outBuffers[0]);
ScopedSecBuffer scopedOutputAlertData(&outBuffers[1]);
SecBufferDesc outBufferDesc = {0};
outBufferDesc.cBuffers = 2;
outBufferDesc.pBuffers = outBuffers;
outBufferDesc.ulVersion = SECBUFFER_VERSION;
SECURITY_STATUS status = InitializeSecurityContext(
m_credHandle,
m_ctxtHandle,
NULL,
m_ctxtFlags,
0,
0,
&inBufferDesc,
0,
NULL,
&outBufferDesc,
&m_secContext,
NULL);
if (status == SEC_E_INCOMPLETE_MESSAGE)
{
// Wait for more data to arrive
break;
}
else if (status == SEC_I_CONTINUE_NEEDED)
{
SecBuffer* pDataBuffer = &outBuffers[0];
SecBuffer* pExtraBuffer = &inBuffers[1];
if (pDataBuffer && pDataBuffer->cbBuffer > 0 && pDataBuffer->pvBuffer != NULL)
sendDataOnNetwork(pDataBuffer->pvBuffer, pDataBuffer->cbBuffer);
if (pExtraBuffer->BufferType == SECBUFFER_EXTRA)
m_receivedData.erase(m_receivedData.begin(), m_receivedData.end() - pExtraBuffer->cbBuffer);
else
m_receivedData.clear();
break;
}
else if (status == SEC_E_OK)
{
status = validateServerCertificate();
if (status != SEC_E_OK)
handleCertError(status);
SecBuffer* pExtraBuffer = &inBuffers[1];
if (pExtraBuffer && pExtraBuffer->cbBuffer > 0)
m_receivedData.erase(m_receivedData.begin(), m_receivedData.end() - pExtraBuffer->cbBuffer);
else
m_receivedData.clear();
m_state = Connected;
determineStreamSizes();
onConnected();
}
else
{
// We failed to initialize the security context
handleCertError(status);
indicateError();
return;
}
}
}