535 lines
17 KiB
C
535 lines
17 KiB
C
/*
|
|
ChibiOS - Copyright (C) 2006,2007,2008,2009,2010,2011,2012,2013,2014,
|
|
2015,2016,2017,2018,2019,2020,2021 Giovanni Di Sirio.
|
|
|
|
This file is part of ChibiOS.
|
|
|
|
ChibiOS is free software; you can redistribute it and/or modify
|
|
it under the terms of the GNU General Public License as published by
|
|
the Free Software Foundation version 3 of the License.
|
|
|
|
ChibiOS is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
/**
|
|
* @file seloops.c
|
|
* @brief Safety Extensions Loops module code.
|
|
*
|
|
* @addtogroup SELOOPS
|
|
* @{
|
|
*/
|
|
|
|
#include "ch.h"
|
|
|
|
/*===========================================================================*/
|
|
/* Module local definitions. */
|
|
/*===========================================================================*/
|
|
|
|
/*===========================================================================*/
|
|
/* Module exported variables. */
|
|
/*===========================================================================*/
|
|
|
|
/*===========================================================================*/
|
|
/* Module local types. */
|
|
/*===========================================================================*/
|
|
|
|
/*===========================================================================*/
|
|
/* Module local variables. */
|
|
/*===========================================================================*/
|
|
|
|
/*===========================================================================*/
|
|
/* Module local functions. */
|
|
/*===========================================================================*/
|
|
|
|
/*===========================================================================*/
|
|
/* Module exported functions. */
|
|
/*===========================================================================*/
|
|
|
|
/**
|
|
* @brief Wait for all specified bits to be set or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAllSet8X(volatile uint8_t *p, uint8_t mask,
|
|
rtcnt_t tmo, uint8_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint8_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) == mask) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for all specified bits to be set or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAllSet16X(volatile uint16_t *p, uint16_t mask,
|
|
rtcnt_t tmo, uint16_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint16_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) == mask) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for all specified bits to be set or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAllSet32X(volatile uint32_t *p, uint32_t mask,
|
|
rtcnt_t tmo, uint32_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint32_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) == mask) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for any of specified bits to be set or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAnySet8X(volatile uint8_t *p, uint8_t mask,
|
|
rtcnt_t tmo, uint8_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint8_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) != (uint8_t)0) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for any of specified bits to be set or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAnySet16X(volatile uint16_t *p, uint16_t mask,
|
|
rtcnt_t tmo, uint16_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint16_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) != (uint16_t)0) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for any of specified bits to be set or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAnySet32X(volatile uint32_t *p, uint32_t mask,
|
|
rtcnt_t tmo, uint32_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint32_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) != (uint32_t)0) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for all specified bits to be cleared or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAllClear8X(volatile uint8_t *p, uint8_t mask,
|
|
rtcnt_t tmo, uint8_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint8_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) == (uint8_t)0) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for all specified bits to be cleared or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAllClear16X(volatile uint16_t *p, uint32_t mask,
|
|
rtcnt_t tmo, uint16_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint16_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) == (uint16_t)0) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for all specified bits to be cleared or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAllClear32X(volatile uint32_t *p, uint32_t mask,
|
|
rtcnt_t tmo, uint32_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint32_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) == (uint32_t)0) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for any of specified bits to be cleared or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAnyClear8X(volatile uint8_t *p, uint8_t mask,
|
|
rtcnt_t tmo, uint8_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint8_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) != mask) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for any of specified bits to be cleared or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAnyClear16X(volatile uint16_t *p, uint16_t mask,
|
|
rtcnt_t tmo, uint16_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint16_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) != mask) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/**
|
|
* @brief Wait for any of specified bits to be cleared or a timeout.
|
|
*
|
|
* @param[in] p address of the register
|
|
* @param[in] mask mask of bits to be checked
|
|
* @param[in] tmo timeout in counter cycles
|
|
* @param[in] valp pointer to where to store the last read value
|
|
* or @p NULL
|
|
* @return The final result.
|
|
* @retval false if the condition has been verified.
|
|
* @retval true if a timeout occurred.
|
|
*
|
|
* @xclass
|
|
*/
|
|
bool seRegWaitAnyClear32X(volatile uint32_t *p, uint32_t mask,
|
|
rtcnt_t tmo, uint32_t *valp) {
|
|
rtcnt_t start, end;
|
|
|
|
/* Time window for the operation to complete.*/
|
|
start = chSysGetRealtimeCounterX();
|
|
end = start + tmo;
|
|
|
|
/* Testing the condition continuously until it becomes true or the
|
|
timeout expires, it is done at least once.*/
|
|
do {
|
|
/* Getting register value and storing it outside if required.*/
|
|
uint32_t v = *p;
|
|
if (valp != NULL) {
|
|
*valp = v;
|
|
}
|
|
|
|
/* Condition check and end of the loop if met.*/
|
|
if ((v & mask) != mask) {
|
|
return false;
|
|
}
|
|
} while (chSysIsCounterWithinX(chSysGetRealtimeCounterX(), start, end));
|
|
|
|
return true;
|
|
}
|
|
|
|
/** @} */
|