C++ chSchGoSleepS函数代码示例

uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
        uint8_t *WPtr, uint8_t WLength,
        uint8_t *RPtr, uint8_t RLength) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    // Start TX DMA if needed
    if(WLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr);
        dmaStreamSetTransactionSize(PDmaTx, WLength);
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSysLock();
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    // Read if needed
    if(RLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        // Send repeated start
        SendStart();
        if(WaitEv5() != OK) return FAILURE;
        SendAddrWithRead(Addr);
        if(WaitEv6() != OK) { SendStop(); return FAILURE; }
        // If number of data to be read is 1, then DMA cannot be used
        if(RLength == 1) {
            AckDisable();
            SendStop();
            if(WaitRx() != OK) return FAILURE;
            *RPtr = ReceiveData();
            if(WaitStop() != OK) return FAILURE;
            return OK;
        }
        else {  // more than 1 byte, use DMA
            AckEnable();
            dmaStreamSetMemory0(PDmaRx, RPtr);
            dmaStreamSetTransactionSize(PDmaRx, RLength);
            DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
            PRequestingThread = chThdSelf();
            dmaStreamEnable(PDmaRx);
            chSysLock();
            chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
            chSysUnlock();
            dmaStreamDisable(PDmaRx);
        } // if lng==1
    } // if != 0
    SendStop();
    return OK;
}

uint8_t i2c_t::CmdWriteRead(uint8_t Addr,
        uint8_t *WPtr, uint8_t WLength,
        uint8_t *RPtr, uint8_t RLength) {
    if(IBusyWait() != OK) return FAILURE;
    // Clear flags
    ii2c->SR1 = 0;
    while(RxIsNotEmpty()) (void)ii2c->DR;   // Read DR until it empty
    ClearAddrFlag();
    // Start transmission
    SendStart();
    if(WaitEv5() != OK) return FAILURE;
    SendAddrWithWrite(Addr);
    if(WaitEv6() != OK) { SendStop(); return FAILURE; }
    ClearAddrFlag();
    // Start TX DMA if needed
    if(WLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        dmaStreamSetMemory0(PDmaTx, WPtr);
        dmaStreamSetMode   (PDmaTx, I2C_DMATX_MODE);
        dmaStreamSetTransactionSize(PDmaTx, WLength);
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaTx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaTx);
    }
    // Read if needed
    if(RLength != 0) {
        if(WaitEv8() != OK) return FAILURE;
        // Send repeated start
        SendStart();
        if(WaitEv5() != OK) return FAILURE;
        SendAddrWithRead(Addr);
        if(WaitEv6() != OK) { SendStop(); return FAILURE; }
        // If single byte is to be received, disable ACK before clearing ADDR flag
        if(RLength == 1) AckDisable();
        else AckEnable();
        ClearAddrFlag();
        dmaStreamSetMemory0(PDmaRx, RPtr);
        dmaStreamSetMode   (PDmaRx, I2C_DMARX_MODE);
        dmaStreamSetTransactionSize(PDmaRx, RLength);
        DmaLastTransferSet(); // Inform DMA that this is last transfer => do not ACK last byte
        chSysLock();
        PRequestingThread = chThdSelf();
        dmaStreamEnable(PDmaRx);
        chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
        chSysUnlock();
        dmaStreamDisable(PDmaRx);
    } // if != 0
    else WaitBTF(); // if nothing to read, just stop
    SendStop();
    return OK;
}

/**
 * @brief   Puts the current thread to sleep into the specified state with
 *          timeout specification.
 * @details The thread goes into a sleeping state, if it is not awakened
 *          explicitly within the specified timeout then it is forcibly
 *          awakened with a @p RDY_TIMEOUT low level message. The possible
 *          @ref thread_states are defined into @p threads.h.
 *
 * @param[in] newstate  the new thread state
 * @param[in] time      the number of ticks before the operation timeouts, the
 *                      special values are handled as follow:
 *                      - @a TIME_INFINITE the thread enters an infinite sleep
 *                        state, this is equivalent to invoking
 *                        @p chSchGoSleepS() but, of course, less efficient.
 *                      - @a TIME_IMMEDIATE this value is not allowed.
 *                      .
 * @return              The wakeup message.
 * @retval RDY_TIMEOUT if a timeout occurs.
 *
 * @sclass
 */
msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) {

  if (TIME_INFINITE != time) {
    VirtualTimer vt;

    chVTSetI(&vt, time, wakeup, currp);
    chSchGoSleepS(newstate);
    if (chVTIsArmedI(&vt))
      chVTResetI(&vt);
  }
  else
    chSchGoSleepS(newstate);
  return currp->p_u.rdymsg;
}

static msg_t lsm303_acc_update_thread(void *p) {
  I2CDriver *i2cp = (I2CDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/

    chSysLock();
    acctp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == LSM303_ACC_DATA_READY) {
      lsm303_acc_update(i2cp);
      while (status_a != 0x00) {
        lsm303_acc_update(i2cp);
      }
//      chprintf((BaseChannel *)&SERIAL_DRIVER, "ACC T: %d X: %d Y: %d Z: %d\r\n",
//                 acc_update_time, acc_data.x, acc_data.y, acc_data.z);
    }
  }

  return RDY_OK;
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 *
 * @param[in] sps       pointer to a @p semaphore_t structure to be signaled
 * @param[in] spw       pointer to a @p semaphore_t structure to wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval MSG_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval MSG_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(semaphore_t *sps, semaphore_t *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL));
  chDbgAssert(((sps->s_cnt >= (cnt_t)0) && queue_isempty(&sps->s_queue)) ||
              ((sps->s_cnt < (cnt_t)0) && queue_notempty(&sps->s_queue)),
              "inconsistent semaphore");
  chDbgAssert(((spw->s_cnt >= (cnt_t)0) && queue_isempty(&spw->s_queue)) ||
              ((spw->s_cnt < (cnt_t)0) && queue_notempty(&spw->s_queue)),
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= (cnt_t)0) {
    chSchReadyI(queue_fifo_remove(&sps->s_queue))->p_u.rdymsg = MSG_OK;
  }
  if (--spw->s_cnt < (cnt_t)0) {
    thread_t *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtsemp = spw;
    chSchGoSleepS(CH_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = MSG_OK;
  }
  chSysUnlock();

  return msg;
}

static THD_FUNCTION(txWrite, arg)
{
	(void)(arg);
	while(!0){
            chSysLock();
            tp_W = chThdGetSelfX();
            chSchGoSleepS(CH_STATE_SUSPENDED);
            chSysUnlock();

            sdWrite(&SD3, &temda, strlen(temda));
		/* Get contents of next mail in mailbox pointed by arg1 to arg2
		 * and wait mail arg3 mSeconds if there is no message is present.
		 *
		 * Returns MSG_OK if succesfully a mail fetched from mailbox*/
		//if(chMBFetch(&serialMbox, (msg_t *)&toSend, TIME_INFINITE) == MSG_OK)
	//	{
			/* Write arg3 bytes from arg2 to device pointed by arg1(SD3 for this example).
			 * Type of arg2 should be (uint8_t *) otherwise only first 8 bit will be send.*/
		//	sdWrite(&SD3, &toSend,1);

		//}
		//wifiInit();
		chThdSleepMilliseconds(1000);
	}
}

void LedRGB_t::Task(void) {
    if(ICurrentColor == INeededColor) {
        chSysLock();
        IsSleeping = true;
        chSchGoSleepS(THD_STATE_SUSPENDED);
        chSysUnlock();
    }
    else {
        chThdSleepMilliseconds(LED_SETUP_DELAY_MS);
        // Red channel
        if (ICurrentColor.Red != INeededColor.Red) {
            if(INeededColor.Red < ICurrentColor.Red) ICurrentColor.Red--;
            else ICurrentColor.Red++;
        }
        // Green channel
        if (ICurrentColor.Green != INeededColor.Green) {
            if(INeededColor.Green < ICurrentColor.Green) ICurrentColor.Green--;
            else ICurrentColor.Green++;
        }
        // Blue channel
        if (ICurrentColor.Blue != INeededColor.Blue) {
            if(INeededColor.Blue < ICurrentColor.Blue) ICurrentColor.Blue--;
            else ICurrentColor.Blue++;
        }
        SetColor(ICurrentColor);
    }
}

/**
 * @brief   Performs atomic signal and wait operations on two semaphores.
 * @pre     The configuration option @p CH_USE_SEMSW must be enabled in order
 *          to use this function.
 *
 * @param[in] sps       pointer to a @p Semaphore structure to be signaled
 * @param[in] spw       pointer to a @p Semaphore structure to be wait on
 * @return              A message specifying how the invoking thread has been
 *                      released from the semaphore.
 * @retval RDY_OK       if the thread has not stopped on the semaphore or the
 *                      semaphore has been signaled.
 * @retval RDY_RESET    if the semaphore has been reset using @p chSemReset().
 *
 * @api
 */
msg_t chSemSignalWait(Semaphore *sps, Semaphore *spw) {
  msg_t msg;

  chDbgCheck((sps != NULL) && (spw != NULL), "chSemSignalWait");

  chDbgAssert(((sps->s_cnt >= 0) && isempty(&sps->s_queue)) ||
              ((sps->s_cnt < 0) && notempty(&sps->s_queue)),
              "chSemSignalWait(), #1",
              "inconsistent semaphore");

  chDbgAssert(((spw->s_cnt >= 0) && isempty(&spw->s_queue)) ||
              ((spw->s_cnt < 0) && notempty(&spw->s_queue)),
              "chSemSignalWait(), #2",
              "inconsistent semaphore");

  chSysLock();
  if (++sps->s_cnt <= 0)
    chSchReadyI(fifo_remove(&sps->s_queue))->p_u.rdymsg = RDY_OK;
  if (--spw->s_cnt < 0) {
    Thread *ctp = currp;
    sem_insert(ctp, &spw->s_queue);
    ctp->p_u.wtobjp = spw;
    chSchGoSleepS(THD_STATE_WTSEM);
    msg = ctp->p_u.rdymsg;
  }
  else {
    chSchRescheduleS();
    msg = RDY_OK;
  }
  chSysUnlock();
  return msg;
}

void Uart_t::IRxTask() {
    while(true) {
        chThdSleepMilliseconds(UART_RX_POLLING_MS);
        // Get number of bytes to process
#if defined STM32F2XX || defined STM32F4XX
        int32_t Sz = UART_RXBUF_SZ - UART_DMA_RX->stream->NDTR;   // Number of bytes copied to buffer since restart
#else
        int32_t Sz = UART_RXBUF_SZ - UART_DMA_RX->channel->CNDTR;   // Number of bytes copied to buffer since restart
#endif
        if(Sz != SzOld) {
            int32_t ByteCnt = Sz - SzOld;
            if(ByteCnt < 0) ByteCnt += UART_RXBUF_SZ;   // Handle buffer circulation
            SzOld = Sz;
            // Iterate received bytes
            for(int32_t i=0; i<ByteCnt; i++) {
                char c = IRxBuf[RIndx++];
                if(RIndx >= UART_RXBUF_SZ) RIndx = 0;
                if(Cmd.PutChar(c) == pdrNewCmd) {
                    chSysLock();
                    App.SignalEvtI(EVTMSK_UART_NEW_CMD);
                    chSchGoSleepS(CH_STATE_SUSPENDED); // Wait until cmd processed
                    chSysUnlock();  // Will be here when application signals that cmd processed
                }
            } // for
        } // if sz
    } // while true
}

void Usb_t::ITask() {
    while(1) {
        chSysLock();
        if(!Ep[0].IsTxPending) {
            chSchGoSleepS(THD_STATE_SUSPENDED);     // Will wake up by IRQ
        }
        chSysUnlock();

        // Handle RX
//        while(OTG_FS->GINTSTS & GINTSTS_RXFLVL) RxHandler();

        // Handle TX
        for(uint8_t i=0; i<USB_EP_CNT; i++) {
            if(Ep[i].IsTxPending) {
                // Block IRQ for not to interfere in FIFO filling operation
                chSysLock();
                DisableIrqs();
                Ep[i].IsTxPending = false;
                //chSysUnlock();
                bool Done = Ep[i].TxFifoHandler();
                //Uart.Printf("Done: %u\r\n", Done);
                // Unblock IRQs back
                //chSysLock();
                EnableIrqs();
                if(!Done) Ep[i].EnableInFifoEmptyIRQ();
                chSysUnlock();
            } // if pending
        } // for
    } // while 1
}

static msg_t lsm303_mag_update_thread(void *p) {
  I2CDriver *i2cp = (I2CDriver *)p;

  while (TRUE) {
    msg_t msg;

    /* Waiting for the IRQ to happen.*/

    chSysLock();
    magtp = chThdSelf();
    chSchGoSleepS(THD_STATE_SUSPENDED);
    msg = chThdSelf()->p_u.rdymsg;
    chSysUnlock();

    /* If data ready, update all axis.*/
    if (msg == LSM303_MAG_DATA_READY) {
      lsm303_mag_update(i2cp);
//      if (status_m != 0x00) {
//        lsm303_mag_update(i2cp);
//      }
    }
  }

  return RDY_OK;
}

/**
 * @brief   Locks the specified mutex.
 *
 * @param[in] mp        pointer to the @p Mutex structure
 */
void chMtxLockS(Mutex *mp) {
  Thread *ctp = currp;

  chDbgCheck(mp != NULL, "chMtxLockS");

  /* Ia the mutex already locked? */
  if (mp->m_owner != NULL) {
    /* Priority inheritance protocol; explores the thread-mutex dependencies
       boosting the priority of all the affected threads to equal the priority
       of the running thread requesting the mutex.*/
    Thread *tp = mp->m_owner;
    /* Does the running thread have higher priority than the mutex
       ownning thread? */
    while (tp->p_prio < ctp->p_prio) {
      /* Make priority of thread tp match the running thread's priority.*/
      tp->p_prio = ctp->p_prio;
      /* The following states need priority queues reordering.*/
      switch (tp->p_state) {
      case THD_STATE_WTMTX:
        /* Re-enqueues the mutex owner with its new priority.*/
        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
        tp = ((Mutex *)tp->p_u.wtobjp)->m_owner;
        continue;
#if CH_USE_CONDVARS | CH_USE_SEMAPHORES_PRIORITY | CH_USE_MESSAGES_PRIORITY
#if CH_USE_CONDVARS
      case THD_STATE_WTCOND:
#endif
#if CH_USE_SEMAPHORES_PRIORITY
      case THD_STATE_WTSEM:
#endif
#if CH_USE_MESSAGES_PRIORITY
      case THD_STATE_SNDMSG:
#endif
        /* Re-enqueues tp with its new priority on the queue.*/
        prio_insert(dequeue(tp), (ThreadsQueue *)tp->p_u.wtobjp);
        break;
#endif
      case THD_STATE_READY:
        /* Re-enqueues  tp with its new priority on the ready list.*/
        chSchReadyI(dequeue(tp));
      }
      break;
    }
    /* Sleep on the mutex.*/
    prio_insert(ctp, &mp->m_queue);
    ctp->p_u.wtobjp = mp;
    chSchGoSleepS(THD_STATE_WTMTX);
    /* It is assumed that the thread performing the unlock operation assigns
       the mutex to this thread.*/
    chDbgAssert(mp->m_owner == ctp, "chMtxLockS(), #1", "not owner");
    chDbgAssert(ctp->p_mtxlist == mp, "chMtxLockS(), #2", "not owned");
  }
  else {
    /* It was not owned, inserted in the owned mutexes list.*/
    mp->m_owner = ctp;
    mp->m_next = ctp->p_mtxlist;
    ctp->p_mtxlist = mp;
  }
}

int ThreadWait::sleep() {
	chSysLock();
	thread_to_wake = chThdSelf();
	chSchGoSleepS(THD_STATE_SUSPENDED);
	const auto result = chThdSelf()->p_u.rdymsg;
	chSysUnlock();
	return result;
}

static msg_t usb_lld_pump(void *p) {
  USBDriver *usbp = (USBDriver *)p;
  stm32_otg_t *otgp = usbp->otg;

  chRegSetThreadName("usb_lld_pump");
  chSysLock();
  while (TRUE) {
    usbep_t ep;
    uint32_t epmask;

    /* Nothing to do, going to sleep.*/
    if ((usbp->state == USB_STOP) ||
        ((usbp->txpending == 0) && !(otgp->GINTSTS & GINTSTS_RXFLVL))) {
      otgp->GINTMSK |= GINTMSK_RXFLVLM;
      usbp->thd_wait = chThdSelf();
      chSchGoSleepS(THD_STATE_SUSPENDED);
    }
    chSysUnlock();

    /* Checks if there are TXFIFOs to be filled.*/
    for (ep = 0; ep <= usbp->otgparams->num_endpoints; ep++) {

      /* Empties the RX FIFO.*/
      while (otgp->GINTSTS & GINTSTS_RXFLVL) {
        otg_rxfifo_handler(usbp);
      }

      epmask = (1 << ep);
      if (usbp->txpending & epmask) {
        bool_t done;

        chSysLock();
        /* USB interrupts are globally *suspended* because the peripheral
           does not allow any interference during the TX FIFO filling
           operation.
           Synopsys document: DesignWare Cores USB 2.0 Hi-Speed On-The-Go (OTG)
             "The application has to finish writing one complete packet before
              switching to a different channel/endpoint FIFO. Violating this
              rule results in an error.".*/
        otgp->GAHBCFG &= ~GAHBCFG_GINTMSK;
        usbp->txpending &= ~epmask;
        chSysUnlock();

        done = otg_txfifo_handler(usbp, ep);

        chSysLock();
        otgp->GAHBCFG |= GAHBCFG_GINTMSK;
        if (!done)
          otgp->DIEPEMPMSK |= epmask;
        chSysUnlock();
      }
    }
    chSysLock();
  }
  chSysUnlock();
  return 0;
}

  msg_t ThreadReference::suspendS(void) {

    chDbgAssert(thread_ref == NULL,
                "already referenced");

    thread_ref = chThdGetSelfX();
    chSchGoSleepS(CH_STATE_SUSPENDED);
    return thread_ref->p_u.rdymsg;
  }