155 lines
4.4 KiB
C
155 lines
4.4 KiB
C
/*
|
||
Copyright 2017 fishpepper <AT> gmail.com
|
||
|
||
This program is free software: you can redistribute it and/or modify
|
||
it under the terms of the GNU General Public License as published by
|
||
the Free Software Foundation, either version 3 of the License, or
|
||
(at your option) any later version.
|
||
|
||
This program is distributed in the hope that it will be useful,
|
||
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||
GNU General Public License for more details.
|
||
|
||
You should have received a copy of the GNU General Public License
|
||
along with this program. If not, see <http:// www.gnu.org/licenses/>.
|
||
|
||
author: fishpepper <AT> gmail.com
|
||
*/
|
||
|
||
#include "hal_timeout.h"
|
||
#include "hal_ppm.h"
|
||
#include "debug.h"
|
||
#include "wdt.h"
|
||
|
||
#include <time.h>
|
||
#include <string.h>
|
||
#include <errno.h>
|
||
|
||
struct timespec timer_1;
|
||
struct timespec timer_2;
|
||
struct timespec timer_ppm;
|
||
|
||
void hal_timeout_init(void) {
|
||
memset(&timer_1, 0, sizeof(timer_1));
|
||
memset(&timer_2, 0, sizeof(timer_2));
|
||
clock_gettime(CLOCK_REALTIME, &timer_ppm);
|
||
}
|
||
|
||
/* Returns true if a is greather then b */
|
||
static inline uint8_t timeval_gt(const struct timespec *a, const struct timespec *b) {
|
||
if (a->tv_sec > b->tv_sec)
|
||
return 1;
|
||
if (a->tv_sec == b->tv_sec && a->tv_nsec > b->tv_nsec)
|
||
return 1;
|
||
return 0;
|
||
}
|
||
|
||
static inline void timer_add(struct timespec *timer, int us) {
|
||
timer->tv_nsec += us * 1000;
|
||
if (timer->tv_nsec > 1000000000) {
|
||
timer->tv_nsec -= 1000000000;
|
||
timer->tv_sec += 1;
|
||
}
|
||
}
|
||
|
||
/* Subtract the ‘struct timeval’ values X and Y,
|
||
storing the result in RESULT.
|
||
Return 1 if the difference is negative, otherwise 0. */
|
||
static inline int timeval_subtract(struct timespec *result,
|
||
struct timespec *x, struct timespec *y) {
|
||
/* Perform the carry for the later subtraction by updating y. */
|
||
if (x->tv_nsec < y->tv_nsec) {
|
||
int nsec = (y->tv_nsec - x->tv_nsec) / 1000000000 + 1;
|
||
y->tv_nsec -= 1000000000 * nsec;
|
||
y->tv_sec += nsec;
|
||
}
|
||
if (x->tv_nsec - y->tv_nsec > 1000000000) {
|
||
int nsec = (x->tv_nsec - y->tv_nsec) / 1000000000;
|
||
y->tv_nsec += 1000000000 * nsec;
|
||
y->tv_sec -= nsec;
|
||
}
|
||
|
||
/* Compute the time remaining to wait.
|
||
tv_nsec is certainly positive. */
|
||
result->tv_sec = x->tv_sec - y->tv_sec;
|
||
result->tv_nsec = x->tv_nsec - y->tv_nsec;
|
||
|
||
/* Return 1 if result is negative. */
|
||
return x->tv_sec < y->tv_sec;
|
||
}
|
||
|
||
void hal_timeout_set_100us(uint32_t hus) {
|
||
clock_gettime(CLOCK_REALTIME, &timer_1);
|
||
timer_add(&timer_1, hus * 100);
|
||
}
|
||
|
||
void hal_timeout2_set_100us(uint32_t hus) {
|
||
clock_gettime(CLOCK_REALTIME, &timer_2);
|
||
timer_add(&timer_2, hus * 100);
|
||
}
|
||
|
||
void hal_timeout_set(uint32_t ms) {
|
||
clock_gettime(CLOCK_REALTIME, &timer_1);
|
||
timer_add(&timer_1, ms * 1000);
|
||
}
|
||
|
||
|
||
uint8_t hal_timeout_timed_out(void) {
|
||
struct timespec now;
|
||
clock_gettime(CLOCK_REALTIME, &now);
|
||
|
||
if (timeval_gt(&now, &timer_ppm)) {
|
||
hal_ppm_tick();
|
||
}
|
||
|
||
return timeval_gt(&now, &timer_1);
|
||
}
|
||
|
||
uint8_t hal_timeout2_timed_out(void) {
|
||
struct timespec now;
|
||
clock_gettime(CLOCK_REALTIME, &now);
|
||
|
||
if (timeval_gt(&now, &timer_ppm)) {
|
||
hal_ppm_tick();
|
||
}
|
||
return timeval_gt(&now, &timer_2);
|
||
}
|
||
|
||
void hal_timeout_delay_us(int32_t timeout_us) {
|
||
struct timespec now, sleep, delay_timer, rem;
|
||
clock_gettime(CLOCK_REALTIME, &now);
|
||
memcpy(&delay_timer, &now, sizeof(struct timespec));
|
||
timer_add(&delay_timer, timeout_us);
|
||
|
||
do {
|
||
/* If ppm happens before delay timer times out. (delay_timer gt timer_ppm) */
|
||
if (timeval_gt(&delay_timer, &timer_ppm)) {
|
||
/* Calculate how far to sleep */
|
||
if (timeval_subtract(&sleep, &now, &timer_ppm) > 0) {
|
||
nanosleep(&sleep, &rem);
|
||
}
|
||
|
||
/* Get current time again */
|
||
clock_gettime(CLOCK_REALTIME, &now);
|
||
if (timeval_gt(&now, &timer_ppm)) {
|
||
hal_ppm_tick();
|
||
}
|
||
|
||
/* Loop, as a new ppm timer might be scheduled again before the delay ends */
|
||
continue;
|
||
}
|
||
|
||
/* Calculate how long to sleep */
|
||
if (timeval_subtract(&sleep, &now, &delay_timer) > 0) {
|
||
nanosleep(&sleep, &rem);
|
||
}
|
||
} while (0);
|
||
}
|
||
|
||
void hal_timeout_add_ppm(uint32_t us) {
|
||
timer_add(&timer_ppm, us);
|
||
}
|
||
|
||
|