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
}
/**
* @brief Transmits data via the I2C bus as master.
* @details Number of receiving bytes must be 0 or more than 1 on STM32F1x.
* This is hardware restriction.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[in] txbuf pointer to the transmit buffer
* @param[in] txbytes number of bytes to be transmitted
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
I2C_TypeDef *dp = i2cp->i2c;
VirtualTimer vt;
#if defined(STM32F1XX_I2C)
chDbgCheck(((rxbytes == 0) || ((rxbytes > 1) && (rxbuf != NULL))),
"i2c_lld_master_transmit_timeout");
#endif
/* Global timeout for the whole operation.*/
if (timeout != TIME_INFINITE)
chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);
/* Releases the lock from high level driver.*/
chSysUnlock();
/* Initializes driver fields, LSB = 0 -> write.*/
i2cp->addr = addr << 1;
i2cp->errors = 0;
/* TX DMA setup.*/
dmaStreamSetMemory0(i2cp->dmatx, txbuf);
dmaStreamSetTransactionSize(i2cp->dmatx, txbytes);
/* RX DMA setup.*/
dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
/* Waits until BUSY flag is reset and the STOP from the previous operation
is completed, alternatively for a timeout condition.*/
while ((dp->SR2 & I2C_SR2_BUSY) || (dp->CR1 & I2C_CR1_STOP)) {
chSysLock();
if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
return RDY_TIMEOUT;
chSysUnlock();
}
/* This lock will be released in high level driver.*/
chSysLock();
/* Atomic check on the timer in order to make sure that a timeout didn't
happen outside the critical zone.*/
if ((timeout != TIME_INFINITE) && !chVTIsArmedI(&vt))
return RDY_TIMEOUT;
/* Starts the operation.*/
dp->CR2 |= I2C_CR2_ITEVTEN;
dp->CR1 |= I2C_CR1_START;
/* Waits for the operation completion or a timeout.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
if ((timeout != TIME_INFINITE) && chVTIsArmedI(&vt))
chVTResetI(&vt);
return chThdSelf()->p_u.rdymsg;
}
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;
}
/**
* @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 MSG_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 MSG_TIMEOUT if a timeout occurs.
*
* @sclass
*/
msg_t chSchGoSleepTimeoutS(tstate_t newstate, systime_t time) {
chDbgCheckClassS();
if (TIME_INFINITE != time) {
virtual_timer_t vt;
chVTDoSetI(&vt, time, wakeup, currp);
chSchGoSleepS(newstate);
if (chVTIsArmedI(&vt))
chVTDoResetI(&vt);
}
else {
chSchGoSleepS(newstate);
}
return currp->p_u.rdymsg;
}
uint8_t i2c_t::CmdWriteWrite(uint8_t Addr,
uint8_t *WPtr1, uint8_t WLength1,
uint8_t *WPtr2, uint8_t WLength2) {
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(WLength1 != 0) {
if(WaitEv8() != OK) return FAILURE;
dmaStreamSetMemory0(PDmaTx, WPtr1);
dmaStreamSetMode (PDmaTx, I2C_DMATX_MODE);
dmaStreamSetTransactionSize(PDmaTx, WLength1);
chSysLock();
PRequestingThread = chThdSelf();
dmaStreamEnable(PDmaTx);
chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
chSysUnlock();
dmaStreamDisable(PDmaTx);
}
if(WLength2 != 0) {
if(WaitEv8() != OK) return FAILURE;
dmaStreamSetMemory0(PDmaTx, WPtr2);
dmaStreamSetMode (PDmaTx, I2C_DMATX_MODE);
dmaStreamSetTransactionSize(PDmaTx, WLength2);
chSysLock();
PRequestingThread = chThdSelf();
dmaStreamEnable(PDmaTx);
chSchGoSleepS(THD_STATE_SUSPENDED); // Sleep until end
chSysUnlock();
dmaStreamDisable(PDmaTx);
}
WaitBTF();
SendStop();
return OK;
}
/**
* @brief Sends the current thread sleeping and sets a reference variable.
* @note This function must reschedule, it can only be called from thread
* context.
*
* @param[in] trp a pointer to a thread reference object
* @return The wake up message.
*
* @sclass
*/
msg_t chThdSuspendS(thread_reference_t *trp) {
thread_t *tp = chThdGetSelfX();
chDbgAssert(*trp == NULL, "not NULL");
*trp = tp;
tp->p_u.wtobjp = &trp;
chSchGoSleepS(CH_STATE_SUSPENDED);
return chThdGetSelfX()->p_u.rdymsg;
}
static THD_FUNCTION(thEDisplay, arg)
{
(void)arg;
chRegSetThreadName("eDisplay");
kbb_display.header.preamble = KBB_PREAMBLE;
kbb_display.header.msg_class = KBB_CLASS_EXTERNAL;
kbb_display.header.msg_subclass = KBB_SUBCLASS_EXTERNAL_01;
kbb_display.header.msg_size = KBB_DISPLAY_SIZE;
while( !chThdShouldTerminateX() )
{
chSysLock();
eDisplayThreadForSleep = chThdGetSelfX();
chSchGoSleepS(CH_STATE_SUSPENDED);
chSysUnlock();
if ( !ed_serialPort )
continue;
if ( ed_serialPort == &Serial1 )
if ( kbse_getBaudSerial1() < 115200 )
continue;
if ( ed_serialPort == &Serial2 )
if ( kbse_getBaudSerial2() < 115200 )
continue;
const kbb_current_msg_t * msg = kbw_getCurrentMsg();
memcpy(&kbb_display.ltc_frame, &msg->ltc_frame, sizeof(kbb_display.ltc_frame));
memcpy(&kbb_display.smpte_time, &msg->smpte_time, sizeof(kbb_display.smpte_time));
kbb_display.ecef[0] = msg->nav_sol.ecefX;
kbb_display.ecef[1] = msg->nav_sol.ecefY;
kbb_display.ecef[2] = msg->nav_sol.ecefZ;
memcpy(&kbb_display.vnav, &msg->vnav, sizeof(kbb_display.vnav));
kbb_display.temperature = msg->temperature;
int32_t * i32buf = (int32_t*)kbb_display.__pad;
i32buf[0] = AA;
i32buf[1] = BB;
i32buf[2] = CC;
uint8_t * buf = (uint8_t*) &kbb_display;
kbb_display.header.checksum = calc_checksum_16(buf+KBB_CHECKSUM_START,
KBB_DISPLAY_SIZE-KBB_CHECKSUM_START);
// display!!!
int size_written = sdWrite(ed_serialPort, buf, KBB_DISPLAY_SIZE);
if ( size_written != KBB_DISPLAY_SIZE )
{
kbg_toggleLED3();
ASSERT(size_written==KBB_DISPLAY_SIZE,"thEDisplay", "Wrong sent data!!");
}
}
return;
}
/**
* @brief Transmits data via the I2C bus as master.
* @details When performing reading through write you can not write more than
* 3 bytes of data to I2C slave. This is SAM7 platform limitation.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[in] txbuf pointer to the transmit buffer
* @param[in] txbytes number of bytes to be transmitted
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout this value is ignored on SAM7 platform.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
*
* @notapi
*/
msg_t i2c_lld_master_transmit_timeout(I2CDriver *i2cp, i2caddr_t addr,
const uint8_t *txbuf, size_t txbytes,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
(void)timeout;
/* SAM7 specific check */
chDbgCheck(((rxbytes == 0) || ((txbytes > 0) && (txbytes < 4) && (rxbuf != NULL))),
"i2c_lld_master_transmit_timeout");
/* prepare to read through write operation */
if (rxbytes > 0){
AT91C_BASE_TWI->TWI_MMR |= txbytes << 8;
/* store internal slave address in TWI_IADR registers */
AT91C_BASE_TWI->TWI_IADR = 0;
while (txbytes > 0){
AT91C_BASE_TWI->TWI_IADR = (AT91C_BASE_TWI->TWI_IADR << 8);
AT91C_BASE_TWI->TWI_IADR |= *(txbuf++);
txbytes--;
}
/* Internal address of I2C slave was set in special Atmel registers.
* Now we must call read function. The I2C cell automatically sends
* bytes from IADR register to bus and issues repeated start. */
return i2c_lld_master_receive_timeout(i2cp, addr, rxbuf, rxbytes, timeout);
}
else{
if (txbytes == 1){
/* In single data byte master read or write, the START and STOP
* must both be set. */
AT91C_BASE_TWI->TWI_CR |= AT91C_TWI_STOP;
}
AT91C_BASE_TWI->TWI_MMR = 0;
AT91C_BASE_TWI->TWI_MMR |= addr << 16;
/* enable just needed interrupts */
AT91C_BASE_TWI->TWI_IER = AT91C_TWI_TXRDY | AT91C_TWI_NACK;
/* correct size and pointer because first byte will be written
* for issue start condition */
i2cp->txbuf = txbuf + 1;
i2cp->txbytes = txbytes - 1;
/* According to datasheet there is no need to set START manually
* we just need to write first byte in THR */
AT91C_BASE_TWI->TWI_THR = txbuf[0];
/* Waits for the operation completion.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
return chThdSelf()->p_u.rdymsg;
}
}
void cc1101_t::TransmitSync(void *Ptr) {
// WaitUntilChannelIsBusy(); // If this is not done, time after time FIFO is destroyed
while(IState != CC_STB_IDLE) EnterIdle();
WriteTX((uint8_t*)Ptr, IPktSz);
// Enter TX and wait IRQ
chSysLock();
PWaitingThread = chThdSelf();
EnterTX();
chSchGoSleepS(THD_STATE_SUSPENDED);
chSysUnlock(); // Will be here when IRQ fires
}
void cc1101_t::TransmitSync(rPkt_t *pPkt) {
// WaitUntilChannelIsBusy(); // If this is not done, time after time FIFO is destroyed
while(IState != CC_STB_IDLE) EnterIdle();
WriteTX((uint8_t*)pPkt, RPKT_LEN);
EnterTX();
// Waiting for the IRQ to happen
chSysLock();
PWaitingThread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
chSysUnlock(); // Will be here when IRQ will fire
}
/**
* @brief Suspends the thread and waits for an incoming message.
* @post After receiving a message the function @p chMsgGet() must be
* called in order to retrieve the message and then @p chMsgRelease()
* must be invoked in order to acknowledge the reception and send
* the answer.
* @note If the message is a pointer then you can assume that the data
* pointed by the message is stable until you invoke @p chMsgRelease()
* because the sending thread is suspended until then.
*
* @return A reference to the thread carrying the message.
*
* @api
*/
Thread *chMsgWait(void) {
Thread *tp;
chSysLock();
if (!chMsgIsPendingI(currp))
chSchGoSleepS(THD_STATE_WTMSG);
tp = fifo_remove(&currp->p_msgqueue);
tp->p_state = THD_STATE_SNDMSG;
chSysUnlock();
return tp;
}
/**
* @brief Receives data via the I2C bus as master.
* @details Number of receiving bytes must be more than 1 because of stm32
* hardware restrictions.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
I2C_TypeDef *dp = i2cp->i2c;
VirtualTimer vt;
msg_t rdymsg;
chDbgCheck((rxbytes > 1), "i2c_lld_master_receive_timeout");
/* Global timeout for the whole operation.*/
chVTSetI(&vt, timeout, i2c_lld_safety_timeout, (void *)i2cp);
/* Releases the lock from high level driver.*/
chSysUnlock();
/* Initializes driver fields, LSB = 1 -> receive.*/
i2cp->addr = (addr << 1) | 0x01;
i2cp->errors = 0;
/* RX DMA setup.*/
dmaStreamSetMemory0(i2cp->dmarx, rxbuf);
dmaStreamSetTransactionSize(i2cp->dmarx, rxbytes);
/* Waits until BUSY flag is reset and the STOP from the previous operation
is completed, alternatively for a timeout condition.*/
while ((dp->SR2 & I2C_SR2_BUSY) || (dp->CR1 & I2C_CR1_STOP)) {
if (!chVTIsArmedI(&vt)) {
chSysLock();
return RDY_TIMEOUT;
}
}
/* This lock will be released in high level driver.*/
chSysLock();
/* Atomic check on the timer in order to make sure that a timeout didn't
happen outside the critical zone.*/
if (!chVTIsArmedI(&vt))
return RDY_TIMEOUT;
/* Starts the operation.*/
dp->CR2 |= I2C_CR2_ITEVTEN;
dp->CR1 |= I2C_CR1_START | I2C_CR1_ACK;
/* Waits for the operation completion or a timeout.*/
i2cp->thread = chThdSelf();
chSchGoSleepS(THD_STATE_SUSPENDED);
rdymsg = chThdSelf()->p_u.rdymsg;
if (rdymsg != RDY_TIMEOUT)
chVTResetI(&vt);
return rdymsg;
}
void logPanic(const char *panicTxt)
/*
intended to be called from the SYSTEM_HALT_HOOK
*/
{
if (!panicTxt)
panicTxt = "<stack crash>";
debugPuts("\nPANIC!");
debugPuts(panicTxt);
chSysLock();
chSchGoSleepS(THD_STATE_FINAL);
}
/**
* @brief Receives data via the I2C bus as master.
* @details Number of receiving bytes must be more than 1 on STM32F1x. This is
* hardware restriction.
*
* @param[in] i2cp pointer to the @p I2CDriver object
* @param[in] addr slave device address
* @param[out] rxbuf pointer to the receive buffer
* @param[in] rxbytes number of bytes to be received
* @param[in] timeout the number of ticks before the operation timeouts,
* the following special values are allowed:
* - @a TIME_INFINITE no timeout.
* .
* @return The operation status.
* @retval RDY_OK if the function succeeded.
* @retval RDY_RESET if one or more I2C errors occurred, the errors can
* be retrieved using @p i2cGetErrors().
* @retval RDY_TIMEOUT if a timeout occurred before operation end. <b>After a
* timeout the driver must be stopped and restarted
* because the bus is in an uncertain state</b>.
*
* @notapi
*/
msg_t i2c_lld_master_receive_timeout(I2CDriver *i2cp, i2caddr_t addr,
uint8_t *rxbuf, size_t rxbytes,
systime_t timeout) {
(void)i2cp;
(void)addr;
(void)rxbuf;
(void)rxbytes;
(void)timeout;
uint32_t status;
uint32_t stop_sent;
i2cdef_t i2c = i2cp->i2c;
/* Set read mode, slave address and 3 internal address byte lengths */
i2c->TWI_MMR = 0;
i2c->TWI_MMR = TWI_MMR_MREAD | TWI_MMR_DADR(addr->chip) | ((addr->len << TWI_MMR_IADRSZ_Pos) & TWI_MMR_IADRSZ_Msk);
/* Set internal address for remote chip */
i2c->TWI_IADR = 0;
i2c->TWI_IADR = twi_mk_addr(addr->addr, addr->len);
/* Send a START condition */
if (rxbytes = 1) {
i2c->TWI_CR = TWI_CR_START | TWI_CR_STOP;
while (!(i2c->TWI_SR & TWI_SR_RXRDY));
*rxbuf = i2c->TWI_RHR;
} else {
i2c->TWI_PTCR = TWI_PTCR_RXTDIS | TWI_PTCR_TXTDIS;
i2c->TWI_IER = TWI_IER_ENDRX | TWI_IER_NACK | TWI_IER_OVRE;
chSysLock();
i2cp->thread = chThdSelf();
i2c->TWI_RNPR = 0;
i2c->TWI_RNCR = 0;
i2c->TWI_RPR = (uint32_t) rxbuf;
i2c->TWI_RCR = rxbytes - 2;
i2c->TWI_PTCR = TWI_PTCR_RXTEN;
i2cp->curbuf = rxbuf + (rxbytes - 2);
i2c->TWI_CR = TWI_CR_START;
chSchGoSleepS(THD_STATE_SUSPENDED);
return chThdSelf()->p_u.rdymsg;
}
return RDY_OK;
}
void
vexSpiTickDelay(int16_t tick)
{
//gptStart(spiGpt, &gpt2cfg);
//gptPolledDelay(spiGpt, tick);
chSysLock();
spiThread = chThdSelf();
gptStartOneShotI( spiGpt, tick );
chSchGoSleepS(THD_STATE_SUSPENDED);
chSysUnlock();
}
/**
* @brief Suspends the thread and waits for an incoming message.
* @post After receiving a message the function @p chMsgGet() must be
* called in order to retrieve the message and then @p chMsgRelease()
* must be invoked in order to acknowledge the reception and send
* the answer.
* @note If the message is a pointer then you can assume that the data
* pointed by the message is stable until you invoke @p chMsgRelease()
* because the sending thread is suspended until then.
*
* @return A reference to the thread carrying the message.
*
* @api
*/
thread_t *chMsgWait(void) {
thread_t *tp;
chSysLock();
if (!chMsgIsPendingI(currp)) {
chSchGoSleepS(CH_STATE_WTMSG);
}
tp = queue_fifo_remove(&currp->msgqueue);
tp->state = CH_STATE_SNDMSG;
chSysUnlock();
return tp;
}
void UsbCDC_t::IRxTask() {
while(true) {
msg_t r = SDU2.vmt->get(&UsbCDC.SDU2);
if(r > 0) {
if(Cmd.PutChar((char)r) == pdrNewCmd) {
chSysLock();
App.SignalEvtI(EVTMSK_USB_NEW_CMD);
chSchGoSleepS(CH_STATE_SUSPENDED); // Wait until cmd processed
chSysUnlock(); // Will be here when application signals that cmd processed
}
} // if byte rx
} // while true
}