// Place data passed from other tasks into send queue
void putMsgOnSendQueue(char* data) {
sendData.enqueueCount++;
// Error simulation code, skip send enqueue
if (sendData.enqueueCount % MESSAGE_SKIP_DIV == 1)
return;
if (sendData.sendQ_LR != 0) {
// Check for full queue. If no spaces available, call to remove oldest data.
if (uxQueueSpacesAvailable( sendData.sendQ_LR ) == 0) {
// If message is not removed from queue, return and signal error
if (removeSendQueueData() == 0) {
dbgOutputVal(SEND_FULLQUEUE);
//stopAll();
return;
}
}
// Send to queue, with at least one vacancy guaranteed for this data
if( xQueueSend( sendData.sendQ_LR, (void*) data, portMAX_DELAY) != pdPASS )
{
dbgOutputVal(MOTOR_SENDTOSENDQ_FAIL);
}
}
}
static void vInterruptCountingSemaphoreTask( void *pvParameters )
{
BaseType_t xCount;
const TickType_t xDelay = pdMS_TO_TICKS( intsemINTERRUPT_MUTEX_GIVE_PERIOD_MS ) * ( intsemMAX_COUNT + 1 );
( void ) pvParameters;
for( ;; ) {
/* Expect to start with the counting semaphore empty. */
if( uxQueueMessagesWaiting( ( QueueHandle_t ) xISRCountingSemaphore ) != 0 ) {
xErrorDetected = pdTRUE;
}
/* Wait until it is expected that the interrupt will have filled the
counting semaphore. */
xOkToGiveCountingSemaphore = pdTRUE;
vTaskDelay( xDelay );
xOkToGiveCountingSemaphore = pdFALSE;
/* Now it is expected that the counting semaphore is full. */
if( uxQueueMessagesWaiting( ( QueueHandle_t ) xISRCountingSemaphore ) != intsemMAX_COUNT ) {
xErrorDetected = pdTRUE;
}
if( uxQueueSpacesAvailable( ( QueueHandle_t ) xISRCountingSemaphore ) != 0 ) {
xErrorDetected = pdTRUE;
}
ulCountingSemaphoreLoops++;
/* Expect to be able to take the counting semaphore intsemMAX_COUNT
times. A block time of 0 is used as the semaphore should already be
there. */
xCount = 0;
while( xSemaphoreTake( xISRCountingSemaphore, 0 ) == pdPASS ) {
xCount++;
}
if( xCount != intsemMAX_COUNT ) {
xErrorDetected = pdTRUE;
}
/* Now raise the priority of this task so it runs immediately that the
semaphore is given from the interrupt. */
vTaskPrioritySet( NULL, configMAX_PRIORITIES - 1 );
/* Block to wait for the semaphore to be given from the interrupt. */
xOkToGiveCountingSemaphore = pdTRUE;
xSemaphoreTake( xISRCountingSemaphore, portMAX_DELAY );
xSemaphoreTake( xISRCountingSemaphore, portMAX_DELAY );
xOkToGiveCountingSemaphore = pdFALSE;
/* Reset the priority so as not to disturbe other tests too much. */
vTaskPrioritySet( NULL, tskIDLE_PRIORITY );
ulCountingSemaphoreLoops++;
}
}
UBaseType_t MPU_uxQueueSpacesAvailable( const QueueHandle_t xQueue )
{
BaseType_t xRunningPrivileged = xPortRaisePrivilege();
UBaseType_t uxReturn;
uxReturn = uxQueueSpacesAvailable( xQueue );
vPortResetPrivilege( xRunningPrivileged );
return uxReturn;
}
// Check local motor queue for vacancies. If none, remove oldest data.
// After, add the parameter char* message to local motor queue.
void putDataOnMotorQ(char* data) {
if (motorData.motorQ_LR != 0) {
// Check for full queue. If no spaces available, call to remove oldest data.
if (uxQueueSpacesAvailable( motorData.motorQ_LR ) == 0) {
// If message is not removed from queue, return and signal error
if (removeMotorQueueData() == 0) {
dbgOutputVal(MOTOR_FULLQUEUE);
stopAll();
return;
}
}
// Send to queue, with at least one vacancy guaranteed for this data
if( xQueueSend( motorData.motorQ_LR, (void*) data, portMAX_DELAY) != pdPASS )
{
dbgOutputVal(RECEIVE_SENDTOMOTORQ_FAIL);
}
}
}
// Place data passed from other tasks into send queue
void putMsgOnSendQueue(char* data) {
if (sendData.sendQ_CD != 0) {
// Check for full queue. If no spaces available, call to remove oldest data.
if (uxQueueSpacesAvailable( sendData.sendQ_CD ) == 0) {
// If message is not removed from queue, return and signal error
if (removeSendQueueData() == 0) {
dbgOutputVal(SEND_FULLQUEUE);
//stopAll();
return;
}
}
// Send to queue, with at least one vacancy guaranteed for this data
if( xQueueSend( sendData.sendQ_CD, (void*) data, portMAX_DELAY) != pdPASS )
{
dbgOutputVal(MOTOR_SENDTOSENDQ_FAIL);
}
}
}
BaseType_t FreeRTOS_FD_SET( xSocket_t xSocket, xSocketSet_t xSocketSet )
{
xFreeRTOS_Socket_t *pxSocket = ( xFreeRTOS_Socket_t * ) xSocket;
BaseType_t xReturn = pdFALSE;
UBaseType_t uxMessagesWaiting;
configASSERT( xSocket );
/* Is the socket already a member of a select group? */
if( pxSocket->xSelectQueue == NULL )
{
taskENTER_CRITICAL();
{
/* Are there packets queued on the socket already? */
uxMessagesWaiting = uxQueueMessagesWaiting( pxSocket->xWaitingPacketSemaphore );
/* Are there enough notification spaces in the select queue for the
number of packets already queued on the socket? */
if( uxQueueSpacesAvailable( ( xQueueHandle ) xSocketSet ) >= uxMessagesWaiting )
{
/* Store a pointer to the select group in the socket for
future reference. */
pxSocket->xSelectQueue = ( xQueueHandle ) xSocketSet;
while( uxMessagesWaiting > 0 )
{
/* Add notifications of the number of packets that are
already queued on the socket to the select queue. */
xQueueSendFromISR( pxSocket->xSelectQueue, &pxSocket, NULL );
uxMessagesWaiting--;
}
xReturn = pdPASS;
}
}
taskEXIT_CRITICAL();
}
return xReturn;
}
static void prvDemoQueueSpaceFunctions( void *pvParameters )
{
QueueHandle_t xQueue = NULL;
const unsigned portBASE_TYPE uxQueueLength = 10;
unsigned portBASE_TYPE uxReturn, x;
/* Remove compiler warnings. */
( void ) pvParameters;
/* Create the queue that will be used. Nothing is actually going to be
sent or received so the queue item size is set to 0. */
xQueue = xQueueCreate( uxQueueLength, 0 );
configASSERT( xQueue );
for( ;; )
{
for( x = 0; x < uxQueueLength; x++ )
{
/* Ask how many messages are available... */
uxReturn = uxQueueMessagesWaiting( xQueue );
/* Check the number of messages being reported as being available
is as expected, and force an assert if not. */
if( uxReturn != x )
{
/* xQueue cannot be NULL so this is deliberately causing an
assert to be triggered as there is an error. */
configASSERT( xQueue == NULL );
}
/* Ask how many spaces remain in the queue... */
uxReturn = uxQueueSpacesAvailable( xQueue );
/* Check the number of spaces being reported as being available
is as expected, and force an assert if not. */
if( uxReturn != ( uxQueueLength - x ) )
{
/* xQueue cannot be NULL so this is deliberately causing an
assert to be triggered as there is an error. */
configASSERT( xQueue == NULL );
}
/* Fill one more space in the queue. */
xQueueSendToBack( xQueue, NULL, 0 );
}
/* Perform the same check while the queue is full. */
uxReturn = uxQueueMessagesWaiting( xQueue );
if( uxReturn != uxQueueLength )
{
configASSERT( xQueue == NULL );
}
uxReturn = uxQueueSpacesAvailable( xQueue );
if( uxReturn != 0 )
{
configASSERT( xQueue == NULL );
}
/* The queue is full, start again. */
xQueueReset( xQueue );
#if( configUSE_PREEMPTION == 0 )
taskYIELD();
#endif
}
}
/**
* \brief BLE manager task
*/
static void ble_mgr_task(void *pvParameters)
{
ad_ble_hdr_t *msg_rx;
uint32_t ulNotifiedValue;
OS_BASE_TYPE xResult;
int8_t wdog_id;
/* Register task to be monitored by watch dog. */
wdog_id = sys_watchdog_register(false);
for (;;) {
/* Notify watch dog on each loop since there's no other trigger for this. */
sys_watchdog_notify(wdog_id);
/* Suspend monitoring while task is blocked on OS_TASK_NOTIFY_WAIT(). */
sys_watchdog_suspend(wdog_id);
/*
* Wait on any of the event group bits, then clear them all.
*/
xResult = OS_TASK_NOTIFY_WAIT(0x0, OS_TASK_NOTIFY_ALL_BITS, &ulNotifiedValue,
OS_TASK_NOTIFY_FOREVER);
OS_ASSERT(xResult == OS_OK);
/* Resume watch dog monitoring. */
sys_watchdog_notify_and_resume(wdog_id);
if (ulNotifiedValue & mainBIT_ADAPTER_EVENT_QUEUE) {
/* Make sure there are messages waiting on the queue. */
if (!uxQueueMessagesWaiting(adapter_if->evt_q)) {
goto no_event;
}
/* Check if there is free space on BLE manager's event queue. */
if (uxQueueSpacesAvailable(mgr_if.evt_q)) {
/* Get message from queue. */
OS_QUEUE_GET(adapter_if->evt_q, &msg_rx, 0);
OS_ASSERT(msg_rx->op_code < AD_BLE_OP_CODE_LAST);
#ifdef BLE_STACK_PASSTHROUGH_MODE
{
OS_IRB new_irb;
/* Fill-in new IRB fields. */
new_irb.status = IRB_PENDING;
new_irb.class_id = IRB_BLE;
new_irb.ptr_buf = msg_rx;
/* Send directly to BLE manager's event queue. */
ble_mgr_event_queue_send(&new_irb, OS_QUEUE_FOREVER);
}
#else
if (msg_rx->op_code == AD_BLE_OP_CODE_STACK_MSG) {
irb_ble_stack_msg_t *stack_msg =
(irb_ble_stack_msg_t*) msg_rx;
/* In non-passthrough we only expect GTL messages. */
OS_ASSERT(stack_msg->msg_type == GTL_MSG);
/*
* during reset we ignore messages other than GAPM_CMP_EVT
* and GAPM_RESET operation
*/
if (reset) {
struct gapm_cmp_evt *evt;
if (stack_msg->msg.gtl.msg_id != GAPM_CMP_EVT) {
goto rx_done;
}
evt = (void *) stack_msg->msg.gtl.param;
if (evt->operation != GAPM_RESET) {
goto rx_done;
}
}
/*
* Check if someone is waiting for this message.
* if not, try to handle message as an event.
*/
if (!ble_gtl_waitqueue_match(&stack_msg->msg.gtl)) {
if (!ble_gtl_handle_event(&stack_msg->msg.gtl)) {
/* Stack message is not handled by the manager. */
#ifdef DEBUG
configASSERT(0);
#endif
}
}
}
else if (msg_rx->op_code == AD_BLE_OP_CODE_ADAPTER_MSG) {
ad_ble_msg_t *ad_msg = (ad_ble_msg_t *) msg_rx;
/* In non-passthrough we only expect GTL messages. */
OS_ASSERT(ad_msg->operation < AD_BLE_OP_LAST);
/* Check if someone is waiting for this message. */
ble_ad_msg_waitqueue_match(ad_msg);
}
rx_done:
OS_FREE(msg_rx);
#endif
/*
* Check if there are more messages waiting in the BLE adapter's
* event queue.
*/
if (uxQueueMessagesWaiting(adapter_if->evt_q)) {
OS_TASK_NOTIFY(mgr_if.task,
mainBIT_ADAPTER_EVENT_QUEUE, OS_NOTIFY_SET_BITS);
}
}
else {
/* Set blocked flag to true. */
ble_mgr_blocked = true;
}
}
no_event:
if (ulNotifiedValue & mainBIT_MANAGER_COMMAND_QUEUE) {
if (uxQueueMessagesWaiting(mgr_if.cmd_q)) {
OS_IRB irb_rx;
/* Get IRB from the queue. */
OS_QUEUE_GET(mgr_if.cmd_q, &irb_rx, 0);
if (irb_rx.status == IRB_COMPLETED) {
/* Free message buffer if it was not freed by application. */
irb_ble_free_msg(&irb_rx);
}
else if (irb_rx.status == IRB_ERROR) {
ble_mgr_event_queue_send(&irb_rx, OS_QUEUE_FOREVER);
}
else if (irb_rx.status == IRB_PENDING) {
/* New IRB from application. */
if (!ble_irb_handle_msg(&irb_rx)) {
/*
* No handler found for IRB - free command buffer
* because nothing else will free it.
*/
OS_FREE(irb_rx.ptr_buf);
irb_rx.ptr_buf = NULL;
}
}
/* Check if blocked and if there is space on the event queue. */
if (ble_mgr_blocked && uxQueueSpacesAvailable(mgr_if.evt_q)) {
/* Set flag to false. */
ble_mgr_blocked = false;
/* Notify task to resume getting BLE adapter events. */
OS_TASK_NOTIFY(mgr_if.task, mainBIT_ADAPTER_EVENT_QUEUE,
OS_NOTIFY_SET_BITS);
}
/* Check if there are messages waiting in the command queue. */
if (uxQueueMessagesWaiting(mgr_if.cmd_q)) {
OS_TASK_NOTIFY(mgr_if.task,
mainBIT_MANAGER_COMMAND_QUEUE, OS_NOTIFY_SET_BITS);
}
}
}
/*
* Check this bit as last one since previous commands may also update storage. In
* such case changes will be written to flash already and there's no need to execute
* this twice in a row.
*/
if (ulNotifiedValue & mainBIT_COMMIT_STORAGE) {
/*
* To commit anything modified in storage it's enough to acquire and then
* immediately release lock - if dirty flag was set, contents of storage
* will be written to flash automatically.
*/
storage_acquire();
storage_release();
}
/* Check if BLE adapter is blocked and if there is free space on its event queue. */
if (ble_mgr_adapter_is_blocked() && uxQueueSpacesAvailable(adapter_if->evt_q)) {
/* Notify BLE adapter that there is free space on its event queue. */
ad_ble_notify_event_queue_avail();
}
}
}
