int drain_log(int level /* how many to leave */)
{
char msg[1];
int l, n;
if (level < 0) level = 50;
n = msgQNumMsgs(MpMisc.LogQ);
while(level < msgQNumMsgs(MpMisc.LogQ)) {
l = msgQReceive(MpMisc.LogQ, msg, 1, VX_NO_WAIT);
numDiscarded++;
}
return n;
}
void MpBuf_close()
{
#ifdef BUFFER_INSTRUMENTATION /* [ */
delete spCounterMutex;
spCounterMutex = NULL;
#ifdef _VXWORKS /* [ */
StartLogging();
#endif /* _VXWORKS ] */
dumpBufs(1);
#ifdef _VXWORKS /* [ */
while (0<msgQNumMsgs(MpMisc.LogQ)) taskDelay(1);
taskDelay(12);
#endif /* _VXWORKS ] */
#endif /* BUFFER_INSTRUMENTATION ] */
MpBuf_delRef(MpMisc.XXXsilence);
MpMisc.XXXsilence = NULL;
MpBuf_delRef(MpMisc.XXXlongSilence);
MpMisc.XXXlongSilence = NULL;
MpBufPool_delete(MpMisc.UcbPool, TRUE);
MpMisc.UcbPool = NULL;
MpBufPool_delete(MpMisc.RtpPool, TRUE);
MpMisc.RtpPool = NULL;
MpBufPool_delete(MpMisc.RtcpPool, TRUE);
MpMisc.RtcpPool = NULL;
MpBufPool_delete(MpMisc.DMAPool, TRUE);
MpMisc.DMAPool = NULL;
}
UINT32_T msg_queue_contain(OSI_LIB_HANDLE MgMsgHandle)
{
#ifdef WINDOWS_OS
MSG_HANDLE_ENTRY_PTR MgMsgHandleEntryPtr;
UINT32_T RearIndex,HeadIndex,MsgLength;
MgMsgHandleEntryPtr=(MSG_HANDLE_ENTRY_PTR)MgMsgHandle;
MsgLength=MgMsgHandleEntryPtr->MsgLength;
RearIndex=MgMsgHandleEntryPtr->RearIndex%MsgLength;
HeadIndex=MgMsgHandleEntryPtr->HeadIndex%MsgLength;
if (RearIndex>=HeadIndex)
{
return RearIndex-HeadIndex;
}
else
{
return MsgLength-(HeadIndex-RearIndex);
}
#endif
#ifdef VXWORKS_OS
return msgQNumMsgs((MSG_Q_ID)MgMsgHandle);
#endif
#ifdef LINUX_OS
printf("This function is not yet available Linux!\n");
return AII_ERROR;
#endif
}
static int __wind_msgq_nummsgs(struct task_struct *curr, struct pt_regs *regs)
{
xnhandle_t handle = __xn_reg_arg1(regs);
wind_msgq_t *msgq;
int nummsgs;
if (!__xn_access_ok
(curr, VERIFY_WRITE, __xn_reg_arg2(regs), sizeof(nummsgs)))
return -EFAULT;
msgq = (wind_msgq_t *)xnregistry_fetch(handle);
if (!msgq)
return S_objLib_OBJ_ID_ERROR;
nummsgs = msgQNumMsgs((MSG_Q_ID)msgq);
if (nummsgs == ERROR)
return wind_errnoget();
__xn_copy_to_user(curr, (void __user *)__xn_reg_arg2(regs), &nummsgs,
sizeof(nummsgs));
return 0;
}
/*
* =====================================================================
* Function:MsgQNumMsgs()
* Description: current amount of messages in message queue
* Input: msgQId -- msgQ id
* Output: N/A
* Return: message amount on success or ERROR otherwise.
*======================================================================
*/
int MsgQNumMsgs(MSG_QUEUE_ID msgQId)
{
#ifdef LINUX_OS
struct mq_attr mqAttr = {0};
int rval;
if (msgQId == AII_NULL)
{
return (-1);
}
rval = Mq_getattr((mqd_t)(*msgQId), (struct mq_attr*)&mqAttr);
if (rval != 0)
{
debug_info(DEBUG_LEVEL_4, "MsgQNumMsgs() Mq_getattr error!");
return (-1);
}
return mqAttr.mq_curmsgs;
#elif VXWORKS_OS
if (msgQId == AII_NULL)
{
return (-1);
}
return msgQNumMsgs(msgQId);
#endif
}
/************************************************************************/
static int logTaskPrio = 250;
int setLogTaskPrio(int p) {
int save = logTaskPrio;
logTaskPrio = 0xff & p;
return save;
}
static int logging = 1;
static int printingLog = 0;
static int numDiscarded = 0;
int drain_log(int level /* how many to leave */)
{
char msg[1];
int l, n;
if (level < 0) level = 50;
n = msgQNumMsgs(MpMisc.LogQ);
while(level < msgQNumMsgs(MpMisc.LogQ)) {
l = msgQReceive(MpMisc.LogQ, msg, 1, VX_NO_WAIT);
numDiscarded++;
}
return n;
}
int StartLogging()
{
int i;
i = logging;
logging = 1;
return i;
}
int StopLogging(int opt)
{
int i;
logging = 0;
i = msgQNumMsgs(MpMisc.LogQ);
if (opt) drain_log(50);
return i;
}
int Zprintf0(int force, char *buf)
{
int l;
int n;
int ret;
int msgret;
char *str;
char junk;
str = buf;
l = sipx_min(ABSOLUTE_MAX_LOG_MSG_LEN, strlen(buf));
ret = l;
if (0 == MpMisc.LogQ) {
ret = force ? fwrite(str, 1, ret, stderr) : 0;
return ret;
}
if (force || logging) {
if ((ret > MpMisc.logMsgSize) && !intContext()) {
taskLock();
}
while (l > 0) {
n = sipx_min(l, MpMisc.logMsgSize);
msgret = msgQSend(MpMisc.LogQ, buf, n,
VX_NO_WAIT, MSG_PRI_NORMAL);
if (ERROR == msgret) {
// int r2 =
msgQReceive(MpMisc.LogQ, &junk, 1, VX_NO_WAIT);
// osPrintf("discard message; r1=%d, r2=%d, q=0x%X: '%s'\n",
// msgret, r2, MpMisc.LogQ, buf);
numDiscarded++;
msgret = msgQSend(MpMisc.LogQ, buf, n,
VX_NO_WAIT, MSG_PRI_NORMAL);
}
if ((ERROR == msgret) && force) {
fwrite(str, 1, ret, stderr);
l = 0;
} else {
l -= n;
buf += n;
}
}
if ((ret > MpMisc.logMsgSize) && !intContext()) {
taskUnlock();
}
} else {
ret = 0;
}
return ret;
}
int Zprintfx(int force, char *fmt, int a, int b, int c, int d, int e, int f)
{
int l, ret;
char buf[256];
if (force || logging) {
sprintf(buf, fmt, a, b, c, d, e, f);
l = Zprintf0(force, buf);
} else {
l = 0;
}
return l;
}
int NoPrintLog()
{
int save = printingLog;
if (printingLog) {
numDiscarded = 0;
}
osPrintf("\n");
printingLog = 0;
return save;
}
int PrintLog(int num)
{
int save = printingLog;
if (!printingLog) {
if (num > 0) drain_log(num);
fprintf(stderr, "%d messages discarded\n", numDiscarded);
numDiscarded = 0;
}
printingLog = 1;
return save;
}
extern "C" int NPL(){return NoPrintLog();}
extern "C" int PL(int n){return PrintLog(n);}
static int printTask(int t1, int t2, int t3, int t4, int t5,
int t6, int t7, int t8, int t9, int t10)
{
char msg[LOG_MSGQ_ITEM_LEN+1];
int l;
int lastTick;
int thisTick;
int Hz;
int qHz;
Hz = sysClkRateGet();
qHz = Hz / 4;
#define LOG_ANNOUNCE_MINUTES 5
lastTick = (int) tickGet() - (LOG_ANNOUNCE_MINUTES * 61 * Hz);
while (1) {
while (!printingLog) taskDelay(qHz);
l = msgQReceive(MpMisc.LogQ, msg, LOG_MSGQ_ITEM_LEN, VX_WAIT_FOREVER);
thisTick = (int) tickGet();
if ((LOG_ANNOUNCE_MINUTES * 60 * Hz) < (thisTick - lastTick)) {
lastTick = thisTick;
fprintf(stderr, "MsgLogger: %d.%03d seconds since boot\n",
thisTick/Hz, ((thisTick % Hz) * 1000) / Hz);
}
if (l > 0) {
msg[l] = 0;
fwrite(msg, 1, l, stderr);
} else {
osPrintf("\n\nLogger: quitting!\n\n");
return 0;
}
}
}
MSG_Q_ID startLogging(int nmsgs, int maxlen)
{
MpMisc.LogQ = msgQCreate(nmsgs, maxlen, MSG_Q_FIFO);
if (NULL == MpMisc.LogQ) {
osPrintf("cannot create LogQ! Quitting\n");
} else {
Zprintf("logQ is 0x%X\n", (int) MpMisc.LogQ, 0,0,0,0,0);
taskSpawn("Logger", logTaskPrio, 0, 8192, (FUNCPTR) printTask,
0, 0, 0,0,0,0,0,0,0,0);
}
return MpMisc.LogQ;
}
#ifdef DEBUG /* [ */
int jamLog(char *fmt, int count, int a, int b, int c, int d, int e)
{
int i;
for (i=0; i<count; i++) {
Zprintfx(1, fmt, i, a, b, c, d, e);
}
return msgQNumMsgs(MpMisc.LogQ);
}
int StopLogging(int opt)
{
int i;
logging = 0;
i = msgQNumMsgs(MpMisc.LogQ);
if (opt) drain_log(50);
return i;
}
static void UDP_ClearMsgQ(MSG_Q_ID id)
{
UDP_BtmInfo_t btmInfo;
while(msgQNumMsgs(id) > 0)
{
msgQReceive(id, (char *)&btmInfo, sizeof(btmInfo), 100);
}
cdValidFlag = FALSE;
DBG_INFO_0("[UDP] Clear msgq");
}
// unpack udp data and msgq send
static uint32 UDP_UnpackRecvBtmInfoData(MSG_Q_ID id)
{
sint32 i = 0;
sint32 j = 0;
STATUS rs = ERROR;
uint8 btmRst = 0;
uint8 ret = 0U; // 临时变量，返回值
uint32 offset = UDP_MSG_HEAD_LEN; // 临时变量，地址偏移
ret = ArrayToVariable_uint32(&(udpRecvBtmInfo.btmId), udpRecvBuf + offset);
offset += ret;
ret = ArrayToVariable_uint32(&(udpRecvBtmInfo.triggerPulse), udpRecvBuf + offset);
offset += ret;
ret = ArrayToVariable_uint32(&(udpRecvBtmInfo.dataLength), udpRecvBuf + offset);
offset += ret;
if (udpRecvBtmInfo.dataLength > UDP_BTM_DATA_BUF_LEN)
{
udpRecvBtmInfo.dataLength = UDP_BTM_DATA_BUF_LEN;
}
for (j = 0; j < udpRecvBtmInfo.dataLength; j++)
{
ret = ArrayToVariable_uint8(&(udpRecvBtmInfo.dataBuf[j]), udpRecvBuf + offset);
offset += ret;
}
if (SPD_GetLeftSpdPluseNum() < udpRecvBtmInfo.triggerPulse)
{
btmRst = UDP_CheckBtmIdList(id);
if (BTM_ID_CHECKRST_OK == btmRst)
{
// send btm data to msg queue
rs = msgQSend(id, (char *)&udpRecvBtmInfo, sizeof(udpRecvBtmInfo), NO_WAIT, MSG_PRI_NORMAL);
if (rs == ERROR)
{
DBG_ERRO("[UDP] msgQSend error! msgQnum=%d\n", msgQNumMsgs(id));
}
}
else
{
DBG_INFO("[UDP] btm id have been in list\n");
}
}
else
{
DBG_INFO("[UDP] recv old btm trigger pulse\n");
}
return offset;
}
static void rootTask(long a0, long a1, long a2, long a3, long a4,
long a5, long a6, long a7, long a8, long a9)
{
TASK_ID tid;
int ret, n;
traceobj_enter(&trobj);
qid = msgQCreate(NMESSAGES, sizeof(int), MSG_Q_PRIORITY);
traceobj_assert(&trobj, qid != 0);
traceobj_mark(&trobj, 3);
ret = taskPrioritySet(taskIdSelf(), 10);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 4);
tid = taskSpawn("peerTask",
11,
0, 0, peerTask, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0);
traceobj_assert(&trobj, tid != ERROR);
traceobj_mark(&trobj, 5);
n = 0;
do
ret = msgQSend(qid, (char *)&messages[n], sizeof(int), NO_WAIT, MSG_PRI_URGENT);
while(n++ < NMESSAGES && ret != ERROR);
traceobj_assert(&trobj, ret == ERROR && errno == S_objLib_OBJ_UNAVAILABLE && n == NMESSAGES + 1);
traceobj_mark(&trobj, 6);
ret = taskDelay(10);
traceobj_assert(&trobj, ret == OK);
traceobj_mark(&trobj, 7);
ret = msgQNumMsgs(qid);
traceobj_assert(&trobj, ret == 0);
traceobj_verify(&trobj, tseq, sizeof(tseq) / sizeof(int));
traceobj_exit(&trobj);
}
static int
cleanMsgesToAupdt()
{
char xmsge[ CONSOLE_MSGE_SIZE ];
int numleft;
if (pMsgesToAupdt != NULL)
{
numleft = msgQNumMsgs(pMsgesToAupdt);
if (numleft > 0)
{
while (msgQReceive(pMsgesToAupdt, &xmsge[ 0 ], CONSOLE_MSGE_SIZE,
NO_WAIT) > 0)
{
DPRINT(1,"cleanMsgesToAupdt: Msg Discarded.\n");
}
}
}
return( numleft );
}
LOGICAL mgIsQEmpty(OSI_LIB_HANDLE MgMsgHandle)
{
#ifdef WINDOWS_OS
MSG_HANDLE_ENTRY_PTR MgMsgHandleEntryPtr;
MgMsgHandleEntryPtr=(MSG_HANDLE_ENTRY_PTR)MgMsgHandle;
return MgMsgHandleEntryPtr->HeadIndex==MgMsgHandleEntryPtr->RearIndex;
#endif
#ifdef VXWORKS_OS
return msgQNumMsgs((MSG_Q_ID)MgMsgHandle)==0;
#endif
#ifdef LINUX_OS
printf("This function is not yet available Linux!\n");
return AII_ERROR;
#endif
}
static int __wind_msgq_nummsgs(struct pt_regs *regs)
{
xnhandle_t handle = __xn_reg_arg1(regs);
wind_msgq_t *msgq;
int nummsgs;
msgq = (wind_msgq_t *)xnregistry_fetch(handle);
if (!msgq)
return S_objLib_OBJ_ID_ERROR;
nummsgs = msgQNumMsgs((MSG_Q_ID)msgq);
if (nummsgs == ERROR)
return wind_errnoget();
return __xn_safe_copy_to_user((void __user *)__xn_reg_arg2(regs), &nummsgs,
sizeof(nummsgs));
}
static uint8 UDP_CheckBtmIdList(MSG_Q_ID id)
{
sint32 i;
sint32 j;
int msgNum;
msgNum = msgQNumMsgs(id) + 1;
for (i = 0; i < msgNum; i++)
{
if (udpRecvBtmInfo.btmId == btmIdList[i])
{
return BTM_ID_CHECKRST_ERR;
}
}
for (j = BTM_ID_LIST_SIZE-1; j > 0; j--)
{
btmIdList[j] = btmIdList[j - 1];
}
btmIdList[0] = udpRecvBtmInfo.btmId;
return BTM_ID_CHECKRST_OK;
}
