EL4121 Embedded System / mbed-os

mbed-os

Dependents:   cobaLCDJoyMotor_Thread odometry_omni_3roda_v3 odometry_omni_3roda_v1 odometry_omni_3roda_v2 ... more

hal/mbed_ticker_api.c@0:b74591d5ab33, 2017-12-11 (annotated)

Committer:
be_bryan
Date:
Mon Dec 11 17:54:04 2017 +0000
Revision:
0:b74591d5ab33
motor ++

Who changed what in which revision?

UserRevisionLine numberNew contents of line
be_bryan 0:b74591d5ab33 1 /* mbed Microcontroller Library
be_bryan 0:b74591d5ab33 2 * Copyright (c) 2015 ARM Limited
be_bryan 0:b74591d5ab33 3 *
be_bryan 0:b74591d5ab33 4 * Licensed under the Apache License, Version 2.0 (the "License");
be_bryan 0:b74591d5ab33 5 * you may not use this file except in compliance with the License.
be_bryan 0:b74591d5ab33 6 * You may obtain a copy of the License at
be_bryan 0:b74591d5ab33 7 *
be_bryan 0:b74591d5ab33 8 * http://www.apache.org/licenses/LICENSE-2.0
be_bryan 0:b74591d5ab33 9 *
be_bryan 0:b74591d5ab33 10 * Unless required by applicable law or agreed to in writing, software
be_bryan 0:b74591d5ab33 11 * distributed under the License is distributed on an "AS IS" BASIS,
be_bryan 0:b74591d5ab33 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
be_bryan 0:b74591d5ab33 13 * See the License for the specific language governing permissions and
be_bryan 0:b74591d5ab33 14 * limitations under the License.
be_bryan 0:b74591d5ab33 15 */
be_bryan 0:b74591d5ab33 16 #include <stdio.h>
be_bryan 0:b74591d5ab33 17 #include <stddef.h>
be_bryan 0:b74591d5ab33 18 #include "hal/ticker_api.h"
be_bryan 0:b74591d5ab33 19 #include "platform/mbed_critical.h"
be_bryan 0:b74591d5ab33 20 #include "mbed_assert.h"
be_bryan 0:b74591d5ab33 21
be_bryan 0:b74591d5ab33 22 static void schedule_interrupt(const ticker_data_t *const ticker);
be_bryan 0:b74591d5ab33 23 static void update_present_time(const ticker_data_t *const ticker);
be_bryan 0:b74591d5ab33 24
be_bryan 0:b74591d5ab33 25 /*
be_bryan 0:b74591d5ab33 26 * Initialize a ticker instance.
be_bryan 0:b74591d5ab33 27 */
be_bryan 0:b74591d5ab33 28 static void initialize(const ticker_data_t *ticker)
be_bryan 0:b74591d5ab33 29 {
be_bryan 0:b74591d5ab33 30 // return if the queue has already been initialized, in that case the
be_bryan 0:b74591d5ab33 31 // interface used by the queue is already initialized.
be_bryan 0:b74591d5ab33 32 if (ticker->queue->initialized) {
be_bryan 0:b74591d5ab33 33 return;
be_bryan 0:b74591d5ab33 34 }
be_bryan 0:b74591d5ab33 35
be_bryan 0:b74591d5ab33 36 ticker->interface->init();
be_bryan 0:b74591d5ab33 37
be_bryan 0:b74591d5ab33 38 const ticker_info_t *info = ticker->interface->get_info();
be_bryan 0:b74591d5ab33 39 uint32_t frequency = info->frequency;
be_bryan 0:b74591d5ab33 40 if (info->frequency == 0) {
be_bryan 0:b74591d5ab33 41 MBED_ASSERT(0);
be_bryan 0:b74591d5ab33 42 frequency = 1000000;
be_bryan 0:b74591d5ab33 43 }
be_bryan 0:b74591d5ab33 44
be_bryan 0:b74591d5ab33 45 uint32_t bits = info->bits;
be_bryan 0:b74591d5ab33 46 if ((info->bits > 32) || (info->bits < 4)) {
be_bryan 0:b74591d5ab33 47 MBED_ASSERT(0);
be_bryan 0:b74591d5ab33 48 bits = 32;
be_bryan 0:b74591d5ab33 49 }
be_bryan 0:b74591d5ab33 50 uint32_t max_delta = 0x7 << (bits - 4); // 7/16th
be_bryan 0:b74591d5ab33 51 uint64_t max_delta_us =
be_bryan 0:b74591d5ab33 52 ((uint64_t)max_delta * 1000000 + frequency - 1) / frequency;
be_bryan 0:b74591d5ab33 53
be_bryan 0:b74591d5ab33 54 ticker->queue->event_handler = NULL;
be_bryan 0:b74591d5ab33 55 ticker->queue->head = NULL;
be_bryan 0:b74591d5ab33 56 ticker->queue->tick_last_read = ticker->interface->read();
be_bryan 0:b74591d5ab33 57 ticker->queue->tick_remainder = 0;
be_bryan 0:b74591d5ab33 58 ticker->queue->frequency = frequency;
be_bryan 0:b74591d5ab33 59 ticker->queue->bitmask = ((uint64_t)1 << bits) - 1;
be_bryan 0:b74591d5ab33 60 ticker->queue->max_delta = max_delta;
be_bryan 0:b74591d5ab33 61 ticker->queue->max_delta_us = max_delta_us;
be_bryan 0:b74591d5ab33 62 ticker->queue->present_time = 0;
be_bryan 0:b74591d5ab33 63 ticker->queue->initialized = true;
be_bryan 0:b74591d5ab33 64
be_bryan 0:b74591d5ab33 65 update_present_time(ticker);
be_bryan 0:b74591d5ab33 66 schedule_interrupt(ticker);
be_bryan 0:b74591d5ab33 67 }
be_bryan 0:b74591d5ab33 68
be_bryan 0:b74591d5ab33 69 /**
be_bryan 0:b74591d5ab33 70 * Set the event handler function of a ticker instance.
be_bryan 0:b74591d5ab33 71 */
be_bryan 0:b74591d5ab33 72 static void set_handler(const ticker_data_t *const ticker, ticker_event_handler handler)
be_bryan 0:b74591d5ab33 73 {
be_bryan 0:b74591d5ab33 74 ticker->queue->event_handler = handler;
be_bryan 0:b74591d5ab33 75 }
be_bryan 0:b74591d5ab33 76
be_bryan 0:b74591d5ab33 77 /*
be_bryan 0:b74591d5ab33 78 * Convert a 32 bit timestamp into a 64 bit timestamp.
be_bryan 0:b74591d5ab33 79 *
be_bryan 0:b74591d5ab33 80 * A 64 bit timestamp is used as the point of time of reference while the
be_bryan 0:b74591d5ab33 81 * timestamp to convert is relative to this point of time.
be_bryan 0:b74591d5ab33 82 *
be_bryan 0:b74591d5ab33 83 * The lower 32 bits of the timestamp returned will be equal to the timestamp to
be_bryan 0:b74591d5ab33 84 * convert.
be_bryan 0:b74591d5ab33 85 *
be_bryan 0:b74591d5ab33 86 * If the timestamp to convert is less than the lower 32 bits of the time
be_bryan 0:b74591d5ab33 87 * reference then the timestamp to convert is seen as an overflowed value and
be_bryan 0:b74591d5ab33 88 * the upper 32 bit of the timestamp returned will be equal to the upper 32 bit
be_bryan 0:b74591d5ab33 89 * of the reference point + 1.
be_bryan 0:b74591d5ab33 90 * Otherwise, the upper 32 bit returned will be equal to the upper 32 bit of the
be_bryan 0:b74591d5ab33 91 * reference point.
be_bryan 0:b74591d5ab33 92 *
be_bryan 0:b74591d5ab33 93 * @param ref: The 64 bit timestamp of reference.
be_bryan 0:b74591d5ab33 94 * @param timestamp: The timestamp to convert.
be_bryan 0:b74591d5ab33 95 */
be_bryan 0:b74591d5ab33 96 static us_timestamp_t convert_timestamp(us_timestamp_t ref, timestamp_t timestamp)
be_bryan 0:b74591d5ab33 97 {
be_bryan 0:b74591d5ab33 98 bool overflow = timestamp < ((timestamp_t) ref) ? true : false;
be_bryan 0:b74591d5ab33 99
be_bryan 0:b74591d5ab33 100 us_timestamp_t result = (ref & ~((us_timestamp_t)UINT32_MAX)) | timestamp;
be_bryan 0:b74591d5ab33 101 if (overflow) {
be_bryan 0:b74591d5ab33 102 result += (1ULL<<32);
be_bryan 0:b74591d5ab33 103 }
be_bryan 0:b74591d5ab33 104
be_bryan 0:b74591d5ab33 105 return result;
be_bryan 0:b74591d5ab33 106 }
be_bryan 0:b74591d5ab33 107
be_bryan 0:b74591d5ab33 108 /**
be_bryan 0:b74591d5ab33 109 * Update the present timestamp value of a ticker.
be_bryan 0:b74591d5ab33 110 */
be_bryan 0:b74591d5ab33 111 static void update_present_time(const ticker_data_t *const ticker)
be_bryan 0:b74591d5ab33 112 {
be_bryan 0:b74591d5ab33 113
be_bryan 0:b74591d5ab33 114 ticker_event_queue_t *queue = ticker->queue;
be_bryan 0:b74591d5ab33 115 uint32_t ticker_time = ticker->interface->read();
be_bryan 0:b74591d5ab33 116 if (ticker_time == ticker->queue->tick_last_read) {
be_bryan 0:b74591d5ab33 117 // No work to do
be_bryan 0:b74591d5ab33 118 return;
be_bryan 0:b74591d5ab33 119 }
be_bryan 0:b74591d5ab33 120
be_bryan 0:b74591d5ab33 121 uint64_t elapsed_ticks = (ticker_time - queue->tick_last_read) & queue->bitmask;
be_bryan 0:b74591d5ab33 122 queue->tick_last_read = ticker_time;
be_bryan 0:b74591d5ab33 123
be_bryan 0:b74591d5ab33 124 uint64_t elapsed_us;
be_bryan 0:b74591d5ab33 125 if (1000000 == queue->frequency) {
be_bryan 0:b74591d5ab33 126 // Optimized for 1MHz
be_bryan 0:b74591d5ab33 127
be_bryan 0:b74591d5ab33 128 elapsed_us = elapsed_ticks;
be_bryan 0:b74591d5ab33 129 } else if (32768 == queue->frequency) {
be_bryan 0:b74591d5ab33 130 // Optimized for 32KHz
be_bryan 0:b74591d5ab33 131
be_bryan 0:b74591d5ab33 132 uint64_t us_x_ticks = elapsed_ticks * 1000000;
be_bryan 0:b74591d5ab33 133 elapsed_us = us_x_ticks >> 15;
be_bryan 0:b74591d5ab33 134
be_bryan 0:b74591d5ab33 135 // Update remainder
be_bryan 0:b74591d5ab33 136 queue->tick_remainder += us_x_ticks - (elapsed_us << 15);
be_bryan 0:b74591d5ab33 137 if (queue->tick_remainder >= queue->frequency) {
be_bryan 0:b74591d5ab33 138 elapsed_us += 1;
be_bryan 0:b74591d5ab33 139 queue->tick_remainder -= queue->frequency;
be_bryan 0:b74591d5ab33 140 }
be_bryan 0:b74591d5ab33 141 } else {
be_bryan 0:b74591d5ab33 142 // General case
be_bryan 0:b74591d5ab33 143
be_bryan 0:b74591d5ab33 144 uint64_t us_x_ticks = elapsed_ticks * 1000000;
be_bryan 0:b74591d5ab33 145 elapsed_us = us_x_ticks / queue->frequency;
be_bryan 0:b74591d5ab33 146
be_bryan 0:b74591d5ab33 147 // Update remainder
be_bryan 0:b74591d5ab33 148 queue->tick_remainder += us_x_ticks - elapsed_us * queue->frequency;
be_bryan 0:b74591d5ab33 149 if (queue->tick_remainder >= queue->frequency) {
be_bryan 0:b74591d5ab33 150 elapsed_us += 1;
be_bryan 0:b74591d5ab33 151 queue->tick_remainder -= queue->frequency;
be_bryan 0:b74591d5ab33 152 }
be_bryan 0:b74591d5ab33 153 }
be_bryan 0:b74591d5ab33 154
be_bryan 0:b74591d5ab33 155 // Update current time
be_bryan 0:b74591d5ab33 156 queue->present_time += elapsed_us;
be_bryan 0:b74591d5ab33 157 }
be_bryan 0:b74591d5ab33 158
be_bryan 0:b74591d5ab33 159 /**
be_bryan 0:b74591d5ab33 160 * Given the absolute timestamp compute the hal tick timestamp.
be_bryan 0:b74591d5ab33 161 */
be_bryan 0:b74591d5ab33 162 static timestamp_t compute_tick(const ticker_data_t *const ticker, us_timestamp_t timestamp)
be_bryan 0:b74591d5ab33 163 {
be_bryan 0:b74591d5ab33 164 ticker_event_queue_t *queue = ticker->queue;
be_bryan 0:b74591d5ab33 165 us_timestamp_t delta_us = timestamp - queue->present_time;
be_bryan 0:b74591d5ab33 166
be_bryan 0:b74591d5ab33 167 timestamp_t delta = ticker->queue->max_delta;
be_bryan 0:b74591d5ab33 168 if (delta_us <= ticker->queue->max_delta_us) {
be_bryan 0:b74591d5ab33 169 // Checking max_delta_us ensures the operation will not overflow
be_bryan 0:b74591d5ab33 170
be_bryan 0:b74591d5ab33 171 if (1000000 == queue->frequency) {
be_bryan 0:b74591d5ab33 172 // Optimized for 1MHz
be_bryan 0:b74591d5ab33 173
be_bryan 0:b74591d5ab33 174 delta = delta_us;
be_bryan 0:b74591d5ab33 175 if (delta > ticker->queue->max_delta) {
be_bryan 0:b74591d5ab33 176 delta = ticker->queue->max_delta;
be_bryan 0:b74591d5ab33 177 }
be_bryan 0:b74591d5ab33 178 } else if (32768 == queue->frequency) {
be_bryan 0:b74591d5ab33 179 // Optimized for 32KHz
be_bryan 0:b74591d5ab33 180
be_bryan 0:b74591d5ab33 181 delta = (delta_us << 15) / 1000000;
be_bryan 0:b74591d5ab33 182 if (delta > ticker->queue->max_delta) {
be_bryan 0:b74591d5ab33 183 delta = ticker->queue->max_delta;
be_bryan 0:b74591d5ab33 184 }
be_bryan 0:b74591d5ab33 185 } else {
be_bryan 0:b74591d5ab33 186 // General case
be_bryan 0:b74591d5ab33 187
be_bryan 0:b74591d5ab33 188 delta = delta_us * queue->frequency / 1000000;
be_bryan 0:b74591d5ab33 189 if (delta > ticker->queue->max_delta) {
be_bryan 0:b74591d5ab33 190 delta = ticker->queue->max_delta;
be_bryan 0:b74591d5ab33 191 }
be_bryan 0:b74591d5ab33 192 }
be_bryan 0:b74591d5ab33 193 }
be_bryan 0:b74591d5ab33 194 return (queue->tick_last_read + delta) & queue->bitmask;
be_bryan 0:b74591d5ab33 195 }
be_bryan 0:b74591d5ab33 196
be_bryan 0:b74591d5ab33 197 /**
be_bryan 0:b74591d5ab33 198 * Return 1 if the tick has incremented to or past match_tick, otherwise 0.
be_bryan 0:b74591d5ab33 199 */
be_bryan 0:b74591d5ab33 200 int _ticker_match_interval_passed(timestamp_t prev_tick, timestamp_t cur_tick, timestamp_t match_tick)
be_bryan 0:b74591d5ab33 201 {
be_bryan 0:b74591d5ab33 202 if (match_tick > prev_tick) {
be_bryan 0:b74591d5ab33 203 return (cur_tick >= match_tick) || (cur_tick < prev_tick);
be_bryan 0:b74591d5ab33 204 } else {
be_bryan 0:b74591d5ab33 205 return (cur_tick < prev_tick) && (cur_tick >= match_tick);
be_bryan 0:b74591d5ab33 206 }
be_bryan 0:b74591d5ab33 207 }
be_bryan 0:b74591d5ab33 208
be_bryan 0:b74591d5ab33 209 /**
be_bryan 0:b74591d5ab33 210 * Compute the time when the interrupt has to be triggered and schedule it.
be_bryan 0:b74591d5ab33 211 *
be_bryan 0:b74591d5ab33 212 * If there is no event in the queue or the next event to execute is in more
be_bryan 0:b74591d5ab33 213 * than ticker.queue.max_delta ticks from now then the ticker irq will be
be_bryan 0:b74591d5ab33 214 * scheduled in ticker.queue.max_delta ticks. Otherwise the irq will be
be_bryan 0:b74591d5ab33 215 * scheduled to happen when the running counter reach the timestamp of the
be_bryan 0:b74591d5ab33 216 * first event in the queue.
be_bryan 0:b74591d5ab33 217 *
be_bryan 0:b74591d5ab33 218 * @note If there is no event in the queue then the interrupt is scheduled to
be_bryan 0:b74591d5ab33 219 * in ticker.queue.max_delta. This is necessary to keep track
be_bryan 0:b74591d5ab33 220 * of the timer overflow.
be_bryan 0:b74591d5ab33 221 */
be_bryan 0:b74591d5ab33 222 static void schedule_interrupt(const ticker_data_t *const ticker)
be_bryan 0:b74591d5ab33 223 {
be_bryan 0:b74591d5ab33 224 ticker_event_queue_t *queue = ticker->queue;
be_bryan 0:b74591d5ab33 225 update_present_time(ticker);
be_bryan 0:b74591d5ab33 226
be_bryan 0:b74591d5ab33 227 if (ticker->queue->head) {
be_bryan 0:b74591d5ab33 228 us_timestamp_t present = ticker->queue->present_time;
be_bryan 0:b74591d5ab33 229 us_timestamp_t match_time = ticker->queue->head->timestamp;
be_bryan 0:b74591d5ab33 230
be_bryan 0:b74591d5ab33 231 // if the event at the head of the queue is in the past then schedule
be_bryan 0:b74591d5ab33 232 // it immediately.
be_bryan 0:b74591d5ab33 233 if (match_time <= present) {
be_bryan 0:b74591d5ab33 234 ticker->interface->fire_interrupt();
be_bryan 0:b74591d5ab33 235 return;
be_bryan 0:b74591d5ab33 236 }
be_bryan 0:b74591d5ab33 237
be_bryan 0:b74591d5ab33 238 timestamp_t match_tick = compute_tick(ticker, match_time);
be_bryan 0:b74591d5ab33 239 ticker->interface->set_interrupt(match_tick);
be_bryan 0:b74591d5ab33 240 timestamp_t cur_tick = ticker->interface->read();
be_bryan 0:b74591d5ab33 241
be_bryan 0:b74591d5ab33 242 if (_ticker_match_interval_passed(queue->tick_last_read, cur_tick, match_tick)) {
be_bryan 0:b74591d5ab33 243 ticker->interface->fire_interrupt();
be_bryan 0:b74591d5ab33 244 }
be_bryan 0:b74591d5ab33 245 } else {
be_bryan 0:b74591d5ab33 246 uint32_t match_tick =
be_bryan 0:b74591d5ab33 247 (queue->tick_last_read + queue->max_delta) & queue->bitmask;
be_bryan 0:b74591d5ab33 248 ticker->interface->set_interrupt(match_tick);
be_bryan 0:b74591d5ab33 249 }
be_bryan 0:b74591d5ab33 250 }
be_bryan 0:b74591d5ab33 251
be_bryan 0:b74591d5ab33 252 void ticker_set_handler(const ticker_data_t *const ticker, ticker_event_handler handler)
be_bryan 0:b74591d5ab33 253 {
be_bryan 0:b74591d5ab33 254 initialize(ticker);
be_bryan 0:b74591d5ab33 255 set_handler(ticker, handler);
be_bryan 0:b74591d5ab33 256 }
be_bryan 0:b74591d5ab33 257
be_bryan 0:b74591d5ab33 258 void ticker_irq_handler(const ticker_data_t *const ticker)
be_bryan 0:b74591d5ab33 259 {
be_bryan 0:b74591d5ab33 260 ticker->interface->clear_interrupt();
be_bryan 0:b74591d5ab33 261
be_bryan 0:b74591d5ab33 262 /* Go through all the pending TimerEvents */
be_bryan 0:b74591d5ab33 263 while (1) {
be_bryan 0:b74591d5ab33 264 if (ticker->queue->head == NULL) {
be_bryan 0:b74591d5ab33 265 break;
be_bryan 0:b74591d5ab33 266 }
be_bryan 0:b74591d5ab33 267
be_bryan 0:b74591d5ab33 268 // update the current timestamp used by the queue
be_bryan 0:b74591d5ab33 269 update_present_time(ticker);
be_bryan 0:b74591d5ab33 270
be_bryan 0:b74591d5ab33 271 if (ticker->queue->head->timestamp <= ticker->queue->present_time) {
be_bryan 0:b74591d5ab33 272 // This event was in the past:
be_bryan 0:b74591d5ab33 273 // point to the following one and execute its handler
be_bryan 0:b74591d5ab33 274 ticker_event_t *p = ticker->queue->head;
be_bryan 0:b74591d5ab33 275 ticker->queue->head = ticker->queue->head->next;
be_bryan 0:b74591d5ab33 276 if (ticker->queue->event_handler != NULL) {
be_bryan 0:b74591d5ab33 277 (*ticker->queue->event_handler)(p->id); // NOTE: the handler can set new events
be_bryan 0:b74591d5ab33 278 }
be_bryan 0:b74591d5ab33 279 /* Note: We continue back to examining the head because calling the
be_bryan 0:b74591d5ab33 280 * event handler may have altered the chain of pending events. */
be_bryan 0:b74591d5ab33 281 } else {
be_bryan 0:b74591d5ab33 282 break;
be_bryan 0:b74591d5ab33 283 }
be_bryan 0:b74591d5ab33 284 }
be_bryan 0:b74591d5ab33 285
be_bryan 0:b74591d5ab33 286 schedule_interrupt(ticker);
be_bryan 0:b74591d5ab33 287 }
be_bryan 0:b74591d5ab33 288
be_bryan 0:b74591d5ab33 289 void ticker_insert_event(const ticker_data_t *const ticker, ticker_event_t *obj, timestamp_t timestamp, uint32_t id)
be_bryan 0:b74591d5ab33 290 {
be_bryan 0:b74591d5ab33 291 core_util_critical_section_enter();
be_bryan 0:b74591d5ab33 292
be_bryan 0:b74591d5ab33 293 // update the current timestamp
be_bryan 0:b74591d5ab33 294 update_present_time(ticker);
be_bryan 0:b74591d5ab33 295 us_timestamp_t absolute_timestamp = convert_timestamp(
be_bryan 0:b74591d5ab33 296 ticker->queue->present_time,
be_bryan 0:b74591d5ab33 297 timestamp
be_bryan 0:b74591d5ab33 298 );
be_bryan 0:b74591d5ab33 299 core_util_critical_section_exit();
be_bryan 0:b74591d5ab33 300
be_bryan 0:b74591d5ab33 301 // defer to ticker_insert_event_us
be_bryan 0:b74591d5ab33 302 ticker_insert_event_us(
be_bryan 0:b74591d5ab33 303 ticker,
be_bryan 0:b74591d5ab33 304 obj, absolute_timestamp, id
be_bryan 0:b74591d5ab33 305 );
be_bryan 0:b74591d5ab33 306 }
be_bryan 0:b74591d5ab33 307
be_bryan 0:b74591d5ab33 308 void ticker_insert_event_us(const ticker_data_t *const ticker, ticker_event_t *obj, us_timestamp_t timestamp, uint32_t id)
be_bryan 0:b74591d5ab33 309 {
be_bryan 0:b74591d5ab33 310 core_util_critical_section_enter();
be_bryan 0:b74591d5ab33 311
be_bryan 0:b74591d5ab33 312 // update the current timestamp
be_bryan 0:b74591d5ab33 313 update_present_time(ticker);
be_bryan 0:b74591d5ab33 314
be_bryan 0:b74591d5ab33 315 // initialise our data
be_bryan 0:b74591d5ab33 316 obj->timestamp = timestamp;
be_bryan 0:b74591d5ab33 317 obj->id = id;
be_bryan 0:b74591d5ab33 318
be_bryan 0:b74591d5ab33 319 /* Go through the list until we either reach the end, or find
be_bryan 0:b74591d5ab33 320 an element this should come before (which is possibly the
be_bryan 0:b74591d5ab33 321 head). */
be_bryan 0:b74591d5ab33 322 ticker_event_t *prev = NULL, *p = ticker->queue->head;
be_bryan 0:b74591d5ab33 323 while (p != NULL) {
be_bryan 0:b74591d5ab33 324 /* check if we come before p */
be_bryan 0:b74591d5ab33 325 if (timestamp < p->timestamp) {
be_bryan 0:b74591d5ab33 326 break;
be_bryan 0:b74591d5ab33 327 }
be_bryan 0:b74591d5ab33 328 /* go to the next element */
be_bryan 0:b74591d5ab33 329 prev = p;
be_bryan 0:b74591d5ab33 330 p = p->next;
be_bryan 0:b74591d5ab33 331 }
be_bryan 0:b74591d5ab33 332
be_bryan 0:b74591d5ab33 333 /* if we're at the end p will be NULL, which is correct */
be_bryan 0:b74591d5ab33 334 obj->next = p;
be_bryan 0:b74591d5ab33 335
be_bryan 0:b74591d5ab33 336 /* if prev is NULL we're at the head */
be_bryan 0:b74591d5ab33 337 if (prev == NULL) {
be_bryan 0:b74591d5ab33 338 ticker->queue->head = obj;
be_bryan 0:b74591d5ab33 339 } else {
be_bryan 0:b74591d5ab33 340 prev->next = obj;
be_bryan 0:b74591d5ab33 341 }
be_bryan 0:b74591d5ab33 342
be_bryan 0:b74591d5ab33 343 schedule_interrupt(ticker);
be_bryan 0:b74591d5ab33 344
be_bryan 0:b74591d5ab33 345 core_util_critical_section_exit();
be_bryan 0:b74591d5ab33 346 }
be_bryan 0:b74591d5ab33 347
be_bryan 0:b74591d5ab33 348 void ticker_remove_event(const ticker_data_t *const ticker, ticker_event_t *obj)
be_bryan 0:b74591d5ab33 349 {
be_bryan 0:b74591d5ab33 350 core_util_critical_section_enter();
be_bryan 0:b74591d5ab33 351
be_bryan 0:b74591d5ab33 352 // remove this object from the list
be_bryan 0:b74591d5ab33 353 if (ticker->queue->head == obj) {
be_bryan 0:b74591d5ab33 354 // first in the list, so just drop me
be_bryan 0:b74591d5ab33 355 ticker->queue->head = obj->next;
be_bryan 0:b74591d5ab33 356 schedule_interrupt(ticker);
be_bryan 0:b74591d5ab33 357 } else {
be_bryan 0:b74591d5ab33 358 // find the object before me, then drop me
be_bryan 0:b74591d5ab33 359 ticker_event_t* p = ticker->queue->head;
be_bryan 0:b74591d5ab33 360 while (p != NULL) {
be_bryan 0:b74591d5ab33 361 if (p->next == obj) {
be_bryan 0:b74591d5ab33 362 p->next = obj->next;
be_bryan 0:b74591d5ab33 363 break;
be_bryan 0:b74591d5ab33 364 }
be_bryan 0:b74591d5ab33 365 p = p->next;
be_bryan 0:b74591d5ab33 366 }
be_bryan 0:b74591d5ab33 367 }
be_bryan 0:b74591d5ab33 368
be_bryan 0:b74591d5ab33 369 core_util_critical_section_exit();
be_bryan 0:b74591d5ab33 370 }
be_bryan 0:b74591d5ab33 371
be_bryan 0:b74591d5ab33 372 timestamp_t ticker_read(const ticker_data_t *const ticker)
be_bryan 0:b74591d5ab33 373 {
be_bryan 0:b74591d5ab33 374 return ticker_read_us(ticker);
be_bryan 0:b74591d5ab33 375 }
be_bryan 0:b74591d5ab33 376
be_bryan 0:b74591d5ab33 377 us_timestamp_t ticker_read_us(const ticker_data_t *const ticker)
be_bryan 0:b74591d5ab33 378 {
be_bryan 0:b74591d5ab33 379 initialize(ticker);
be_bryan 0:b74591d5ab33 380 update_present_time(ticker);
be_bryan 0:b74591d5ab33 381 return ticker->queue->present_time;
be_bryan 0:b74591d5ab33 382 }
be_bryan 0:b74591d5ab33 383
be_bryan 0:b74591d5ab33 384 int ticker_get_next_timestamp(const ticker_data_t *const data, timestamp_t *timestamp)
be_bryan 0:b74591d5ab33 385 {
be_bryan 0:b74591d5ab33 386 int ret = 0;
be_bryan 0:b74591d5ab33 387
be_bryan 0:b74591d5ab33 388 /* if head is NULL, there are no pending events */
be_bryan 0:b74591d5ab33 389 core_util_critical_section_enter();
be_bryan 0:b74591d5ab33 390 if (data->queue->head != NULL) {
be_bryan 0:b74591d5ab33 391 *timestamp = data->queue->head->timestamp;
be_bryan 0:b74591d5ab33 392 ret = 1;
be_bryan 0:b74591d5ab33 393 }
be_bryan 0:b74591d5ab33 394 core_util_critical_section_exit();
be_bryan 0:b74591d5ab33 395
be_bryan 0:b74591d5ab33 396 return ret;
be_bryan 0:b74591d5ab33 397 }