uc_stm32_thread.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <uavcan_stm32/thread.hpp>
00006 #include <uavcan_stm32/clock.hpp>
00007 #include <uavcan_stm32/can.hpp>
00008 #include "internal.hpp"
00009 
00010 
00011 namespace uavcan_stm32
00012 {
00013 
00014 #if UAVCAN_STM32_CHIBIOS
00015 /*
00016  * BusEvent
00017  */
00018 bool BusEvent::wait(uavcan::MonotonicDuration duration)
00019 {
00020     static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
00021 
00022     const uavcan::int64_t msec = duration.toMSec();
00023     msg_t ret = msg_t();
00024 
00025     if (msec <= 0)
00026     {
00027 # if (CH_KERNEL_MAJOR == 2)
00028         ret = sem_.waitTimeout(TIME_IMMEDIATE);
00029 # else // ChibiOS 3+
00030         ret = sem_.wait(TIME_IMMEDIATE);
00031 # endif
00032     }
00033     else
00034     {
00035 # if (CH_KERNEL_MAJOR == 2)
00036         ret = sem_.waitTimeout((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
00037 # else // ChibiOS 3+
00038         ret = sem_.wait((msec > MaxDelayMSec) ? MS2ST(MaxDelayMSec) : MS2ST(msec));
00039 # endif
00040     }
00041 # if (CH_KERNEL_MAJOR == 2)
00042     return ret == RDY_OK;
00043 # else // ChibiOS 3+
00044     return ret == MSG_OK;
00045 # endif
00046 }
00047 
00048 void BusEvent::signal()
00049 {
00050     sem_.signal();
00051 }
00052 
00053 void BusEvent::signalFromInterrupt()
00054 {
00055 # if (CH_KERNEL_MAJOR == 2)
00056     chSysLockFromIsr();
00057     sem_.signalI();
00058     chSysUnlockFromIsr();
00059 # else // ChibiOS 3+
00060     chSysLockFromISR();
00061     sem_.signalI();
00062     chSysUnlockFromISR();
00063 # endif
00064 }
00065 
00066 /*
00067  * Mutex
00068  */
00069 void Mutex::lock()
00070 {
00071     mtx_.lock();
00072 }
00073 
00074 void Mutex::unlock()
00075 {
00076 # if (CH_KERNEL_MAJOR == 2)
00077     chibios_rt::BaseThread::unlockMutex();
00078 # else // ChibiOS 3+
00079     mtx_.unlock();
00080 # endif
00081 }
00082 
00083 
00084 #elif UAVCAN_STM32_FREERTOS
00085 
00086 bool BusEvent::wait(uavcan::MonotonicDuration duration)
00087 {
00088     static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF;
00089 
00090     const uavcan::int64_t msec = duration.toMSec();
00091 
00092     BaseType_t ret;
00093 
00094     if (msec <= 0)
00095     {
00096         ret = xSemaphoreTake( sem_, ( TickType_t ) 0 );
00097     }
00098     else
00099     {
00100         ret = xSemaphoreTake( sem_, (msec > MaxDelayMSec) ? (MaxDelayMSec/portTICK_RATE_MS) : (msec/portTICK_RATE_MS));
00101     }
00102     return ret == pdTRUE;
00103 }
00104 
00105 void BusEvent::signal()
00106 {
00107     xSemaphoreGive( sem_ );
00108 }
00109 
00110 void BusEvent::signalFromInterrupt()
00111 {
00112     higher_priority_task_woken = pdFALSE;
00113 
00114     xSemaphoreGiveFromISR( sem_, &higher_priority_task_woken );
00115 }
00116 
00117 void BusEvent::yieldFromISR()
00118 {
00119     portYIELD_FROM_ISR( higher_priority_task_woken );
00120 }
00121 
00122 /*
00123  * Mutex
00124  */
00125 void Mutex::lock()
00126 {
00127     xSemaphoreTake( mtx_, portMAX_DELAY );
00128 }
00129 
00130 void Mutex::unlock()
00131 {
00132     xSemaphoreGive( mtx_ );
00133 }
00134 
00135 
00136 #elif UAVCAN_STM32_NUTTX
00137 
00138 const unsigned BusEvent::MaxPollWaiters;
00139 const char* const BusEvent::DevName = "/dev/uavcan/busevent";
00140 
00141 int BusEvent::openTrampoline(::file* filp)
00142 {
00143     return static_cast<BusEvent*>(filp->f_inode->i_private)->open(filp);
00144 }
00145 
00146 int BusEvent::closeTrampoline(::file* filp)
00147 {
00148     return static_cast<BusEvent*>(filp->f_inode->i_private)->close(filp);
00149 }
00150 
00151 int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup)
00152 {
00153     return static_cast<BusEvent*>(filp->f_inode->i_private)->poll(filp, fds, setup);
00154 }
00155 
00156 int BusEvent::open(::file* filp)
00157 {
00158     (void)filp;
00159     return 0;
00160 }
00161 
00162 int BusEvent::close(::file* filp)
00163 {
00164     (void)filp;
00165     return 0;
00166 }
00167 
00168 int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup)
00169 {
00170     CriticalSectionLocker locker;
00171     int ret = -1;
00172 
00173     if (setup)
00174     {
00175         ret = addPollWaiter(fds);
00176         if (ret == 0)
00177         {
00178             /*
00179              * Two events can be reported via POLLIN:
00180              *  - The RX queue is not empty. This event is level-triggered.
00181              *  - Transmission complete. This event is edge-triggered.
00182              * FIXME Since TX event is edge-triggered, it can be lost between poll() calls.
00183              */
00184             fds->revents |= fds->events & (can_driver_.hasReadableInterfaces() ? POLLIN : 0);
00185             if (fds->revents != 0)
00186             {
00187                 (void)sem_post(fds->sem);
00188             }
00189         }
00190     }
00191     else
00192     {
00193         ret = removePollWaiter(fds);
00194     }
00195 
00196     return ret;
00197 }
00198 
00199 int BusEvent::addPollWaiter(::pollfd* fds)
00200 {
00201     for (unsigned i = 0; i < MaxPollWaiters; i++)
00202     {
00203         if (pollset_[i] == UAVCAN_NULLPTR)
00204         {
00205             pollset_[i] = fds;
00206             return 0;
00207         }
00208     }
00209     return -ENOMEM;
00210 }
00211 
00212 int BusEvent::removePollWaiter(::pollfd* fds)
00213 {
00214     for (unsigned i = 0; i < MaxPollWaiters; i++)
00215     {
00216         if (fds == pollset_[i])
00217         {
00218             pollset_[i] = UAVCAN_NULLPTR;
00219             return 0;
00220         }
00221     }
00222     return -EINVAL;
00223 }
00224 
00225 BusEvent::BusEvent(CanDriver& can_driver)
00226     : can_driver_(can_driver)
00227     , signal_(false)
00228 {
00229     std::memset(&file_ops_, 0, sizeof(file_ops_));
00230     std::memset(pollset_, 0, sizeof(pollset_));
00231     file_ops_.open  = &BusEvent::openTrampoline;
00232     file_ops_.close = &BusEvent::closeTrampoline;
00233     file_ops_.poll  = &BusEvent::pollTrampoline;
00234     // TODO: move to init(), add proper error handling
00235     if (register_driver(DevName, &file_ops_, 0666, static_cast<void*>(this)) != 0)
00236     {
00237         std::abort();
00238     }
00239 }
00240 
00241 BusEvent::~BusEvent()
00242 {
00243     (void)unregister_driver(DevName);
00244 }
00245 
00246 bool BusEvent::wait(uavcan::MonotonicDuration duration)
00247 {
00248     // TODO blocking wait
00249     const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration;
00250     while (clock::getMonotonic() < deadline)
00251     {
00252         {
00253             CriticalSectionLocker locker;
00254             if (signal_)
00255             {
00256                 signal_ = false;
00257                 return true;
00258             }
00259         }
00260         ::usleep(1000);
00261     }
00262     return false;
00263 }
00264 
00265 void BusEvent::signalFromInterrupt()
00266 {
00267     signal_ = true;  // HACK
00268     for (unsigned i = 0; i < MaxPollWaiters; i++)
00269     {
00270         ::pollfd* const fd = pollset_[i];
00271         if (fd != UAVCAN_NULLPTR)
00272         {
00273             fd->revents |= fd->events & POLLIN;
00274             if ((fd->revents != 0) && (fd->sem->semcount <= 0))
00275             {
00276                 (void)sem_post(fd->sem);
00277             }
00278         }
00279     }
00280 }
00281 
00282 #endif
00283 
00284 }