Fixed SB messages with timeout.
@@ -207,6 +207,10 @@ | ||
207 | 207 | /* Falls through.*/ |
208 | 208 | case CH_STATE_QUEUED: |
209 | 209 | /* Falls through.*/ |
210 | +#if CH_CFG_USE_MESSAGES == TRUE | |
211 | + case CH_STATE_SNDMSGQ: | |
212 | + /* Falls through.*/ | |
213 | +#endif | |
210 | 214 | #if (CH_CFG_USE_CONDVARS == TRUE) && (CH_CFG_USE_CONDVARS_TIMEOUT == TRUE) |
211 | 215 | case CH_STATE_WTCOND: |
212 | 216 | #endif |
@@ -918,6 +918,29 @@ | ||
918 | 918 | ectxp->r0 = SB_ERR_ENOSYS; |
919 | 919 | } |
920 | 920 | |
921 | +static thread_t *sb_msg_wait_timeout_s(sysinterval_t timeout) { | |
922 | + thread_t *currtp = chThdGetSelfX(); | |
923 | + thread_t *tp; | |
924 | + | |
925 | + chDbgCheckClassS(); | |
926 | + | |
927 | + /* The sender thread could have timed out in sbSendMessageTimeout() so | |
928 | + repeating the wait if it did.*/ | |
929 | + do { | |
930 | + if (!chMsgIsPendingI(currtp)) { | |
931 | + if (chSchGoSleepTimeoutS(CH_STATE_WTMSG, timeout) != MSG_OK) { | |
932 | + return NULL; | |
933 | + } | |
934 | + } | |
935 | + } while(ch_queue_isempty(&currtp->msgqueue)); | |
936 | + | |
937 | + /* Dequeuing the sender thread and returning it.*/ | |
938 | + tp = threadref(ch_queue_fifo_remove(&currtp->msgqueue)); | |
939 | + tp->state = CH_STATE_SNDMSG; | |
940 | + | |
941 | + return tp; | |
942 | +} | |
943 | + | |
921 | 944 | /*===========================================================================*/ |
922 | 945 | /* Module exported functions. */ |
923 | 946 | /*===========================================================================*/ |
@@ -1005,15 +1028,20 @@ | ||
1005 | 1028 | #if CH_CFG_USE_MESSAGES == TRUE |
1006 | 1029 | sb_class_t *sbcp = (sb_class_t *)chThdGetSelfX()->ctx.syscall.p; |
1007 | 1030 | |
1031 | + chSysLock(); | |
1032 | + | |
1008 | 1033 | if (sbcp->msg_tp == NULL) { |
1009 | - sbcp->msg_tp = chMsgWait(); | |
1034 | + sbcp->msg_tp = sb_msg_wait_timeout_s(TIME_INFINITE); | |
1010 | 1035 | ectxp->r0 = (uint32_t)chMsgGet(sbcp->msg_tp); |
1011 | 1036 | } |
1012 | 1037 | else { |
1013 | - chMsgRelease(sbcp->msg_tp, MSG_RESET); | |
1038 | + thread_t *tp = sbcp->msg_tp; | |
1014 | 1039 | sbcp->msg_tp = NULL; |
1040 | + chMsgReleaseS(tp, MSG_RESET); | |
1015 | 1041 | ectxp->r0 = SB_ERR_EBUSY; |
1016 | 1042 | } |
1043 | + | |
1044 | + chSysUnlock(); | |
1017 | 1045 | #else |
1018 | 1046 | ectxp->r0 = SB_ERR_NOT_IMPLEMENTED; |
1019 | 1047 | #endif |
@@ -1023,14 +1051,19 @@ | ||
1023 | 1051 | #if CH_CFG_USE_MESSAGES == TRUE |
1024 | 1052 | sb_class_t *sbcp = (sb_class_t *)chThdGetSelfX()->ctx.syscall.p; |
1025 | 1053 | |
1054 | + chSysLock(); | |
1055 | + | |
1026 | 1056 | if (sbcp->msg_tp != NULL) { |
1027 | - chMsgRelease(sbcp->msg_tp, (msg_t )ectxp->r0); | |
1057 | + thread_t *tp = sbcp->msg_tp; | |
1028 | 1058 | sbcp->msg_tp = NULL; |
1059 | + chMsgReleaseS(tp, (msg_t )ectxp->r0); | |
1029 | 1060 | ectxp->r0 = SB_ERR_NOERROR; |
1030 | 1061 | } |
1031 | 1062 | else { |
1032 | 1063 | ectxp->r0 = SB_ERR_EBUSY; |
1033 | 1064 | } |
1065 | + | |
1066 | + chSysUnlock(); | |
1034 | 1067 | #else |
1035 | 1068 | ectxp->r0 = SB_ERR_NOT_IMPLEMENTED; |
1036 | 1069 | #endif |
@@ -201,7 +201,7 @@ | ||
201 | 201 | /* If a timeout occurred while the boxed thread already received the message |
202 | 202 | then this thread needs to "unregister" as sender, the boxed error will |
203 | 203 | get SB_ERR_EBUSY when/if trying to reply.*/ |
204 | - if (sbcp->msg_tp == ctp) { | |
204 | + if ((msg == MSG_TIMEOUT) && (sbcp->msg_tp == ctp)) { | |
205 | 205 | sbcp->msg_tp = NULL; |
206 | 206 | } |
207 | 207 |