static void _sema4_isr(pointer p)
{
CORE_MUTEX_DEVICE_PTR device_ptr = (CORE_MUTEX_DEVICE_PTR) p;
uint_32 i;
vuchar_ptr gate_ptr;
uchar lock = _psp_core_num()+1;
uint_16 mask = 0x8000;
boolean waiting;
for (i=0;i<SEMA4_NUM_GATES;i++) {
if (*device_ptr->CPNTF_PTR & mask ) {
// An interrupt is pending on this mutex, the only way to clear it is to lock it (either
// by this core or another)
gate_ptr = device_ptr->MUTEX_PTR[idx[i]]->GATE_PTR;
*gate_ptr = lock;
if (*gate_ptr == lock) {
// Now, check to see if any task is waiting for it
waiting = FALSE;
if (device_ptr->MUTEX_PTR[idx[i]]) {
if (_taskq_get_value(device_ptr->MUTEX_PTR[idx[i]]->WAIT_Q)) {
_taskq_resume(device_ptr->MUTEX_PTR[idx[i]]->WAIT_Q, FALSE);
waiting = TRUE;
}
}
if (!waiting) {
// No task was waiting, give it back - this can occur due to a failed trylock
*gate_ptr = SEMA4_UNLOCK;
}
}
}
mask >>= 1;
}
}
uint_32 _core_mutex_unlock( CORE_MUTEX_PTR core_mutex_ptr )
{
uchar lock = _psp_core_num()+1;
vuchar_ptr gate_ptr;
#if MQX_CHECK_ERRORS
if (core_mutex_ptr == NULL) {
return MQX_INVALID_POINTER;
}
if (core_mutex_ptr->VALID != CORE_MUTEX_VALID) {
return MQX_INVALID_POINTER;
}
#endif
gate_ptr = core_mutex_ptr->GATE_PTR;
// Make sure it is locked by this core
if ( *gate_ptr != lock) {
return MQX_NOT_RESOURCE_OWNER;
}
#if BSPCFG_CORE_MUTEX_STATS
core_mutex_ptr->UNLOCKS++;
#endif
// Unlock if
*gate_ptr = SEMA4_UNLOCK;
// See if this core has any other tasks waiting for this lock
if (_taskq_get_value(core_mutex_ptr->WAIT_Q)) {
// if so, have to queue the next request
_sema4_int_disable();
*gate_ptr = lock;
if (*gate_ptr == lock) {
// if we got it, wake up the next task
_taskq_resume(core_mutex_ptr->WAIT_Q, FALSE);
}
_sema4_int_enable();
}
return COREMUTEX_OK;
}
/*!
* \brief Lock the core mutex.
*
* This function attempts to lock a mutex. If the mutex is already locked
* by another task the function blocks and waits until it is possible to lock
* the mutex for the calling task.
*
* \param[in] core_mutex_ptr Pointer to core_mutex structure.
*
* \return MQX_INVALID_POINTER (Wrong pointer to the core_mutex structure provided.)
* \return COREMUTEX_OK (Core mutex successfully locked.)
*
* \see _core_mutex_unlock
* \see _core_mutex_trylock
*/
uint32_t _core_mutex_lock( CORE_MUTEX_PTR core_mutex_ptr )
{
unsigned char lock = _psp_core_num()+1;
#if MQX_CHECK_ERRORS
if (core_mutex_ptr == NULL) {
return MQX_INVALID_POINTER;
}
if (core_mutex_ptr->VALID != CORE_MUTEX_VALID) {
return MQX_INVALID_POINTER;
}
#endif
_sema4_int_disable();
#if BSPCFG_CORE_MUTEX_STATS
core_mutex_ptr->LOCKS++;
#endif
/* Check to see if this core already own it */
if (*core_mutex_ptr->GATE_PTR == lock) {
/* Yes, then we have to wait for owning task to release it */
#if BSPCFG_CORE_MUTEX_STATS
core_mutex_ptr->WAITS++;
#endif
_taskq_suspend(core_mutex_ptr->WAIT_Q);
} else {
/* can only try to lock the mutex if another task is not waiting otherwise we need to get in line */
if (_taskq_get_value(core_mutex_ptr->WAIT_Q)==0) {
*core_mutex_ptr->GATE_PTR = lock;
}
if (*core_mutex_ptr->GATE_PTR != lock) {
#if BSPCFG_CORE_MUTEX_STATS
core_mutex_ptr->WAITS++;
#endif
_taskq_suspend(core_mutex_ptr->WAIT_Q);
/* Our turn now */
}
}
_sema4_int_enable();
return COREMUTEX_OK;
}
uint_32 _core_mutex_trylock( CORE_MUTEX_PTR core_mutex_ptr )
{
vuchar_ptr gate_ptr;
uchar lock = _psp_core_num()+1;
boolean locked = FALSE;
#if MQX_CHECK_ERRORS
if (core_mutex_ptr == NULL) {
return MQX_INVALID_POINTER;
}
if (core_mutex_ptr->VALID != CORE_MUTEX_VALID) {
return MQX_INVALID_POINTER;
}
#endif
gate_ptr = core_mutex_ptr->GATE_PTR;
_sema4_int_disable();
// If any other task is pending on the semaphore, then it's already locked
if (_taskq_get_value(core_mutex_ptr->WAIT_Q)==0) {
// Or if it's not unlocked...
if (*gate_ptr == SEMA4_UNLOCK) {
*gate_ptr = lock;
// double check to ensure another core didn't lock it before we could
locked = (*gate_ptr == lock);
}
}
_sema4_int_enable();
if(locked) {
return COREMUTEX_LOCKED;
}
else {
return COREMUTEX_UNLOCKED;
}
}
