1 /* MSPDebug - debugging tool for MSP430 MCUs
2 * Copyright (C) 2009-2012 Daniel Beer
3 *
4 * This program is free software; you can redistribute it and/or modify
5 * it under the terms of the GNU General Public License as published by
6 * the Free Software Foundation; either version 2 of the License, or
7 * (at your option) any later version.
8 *
9 * This program is distributed in the hope that it will be useful,
10 * but WITHOUT ANY WARRANTY; without even the implied warranty of
11 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
12 * GNU General Public License for more details.
13 *
14 * You should have received a copy of the GNU General Public License
15 * along with this program; if not, write to the Free Software
16 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
17 */
18
19 #include <stdlib.h>
20 #include <string.h>
21 #include "powerbuf.h"
22
powerbuf_new(unsigned int max_samples,unsigned int interval_us)23 powerbuf_t powerbuf_new(unsigned int max_samples, unsigned int interval_us)
24 {
25 powerbuf_t pb = malloc(sizeof(*pb));
26
27 if (!pb)
28 return NULL;
29
30 if (max_samples <= 0) {
31 free(pb);
32 return NULL;
33 }
34
35 memset(pb, 0, sizeof(*pb));
36
37 pb->current_ua = malloc(sizeof(pb->current_ua[0]) * max_samples);
38 if (!pb->current_ua) {
39 free(pb);
40 return NULL;
41 }
42
43 pb->mab = malloc(sizeof(pb->mab[0]) * max_samples);
44 if (!pb->mab) {
45 free(pb->current_ua);
46 free(pb);
47 return NULL;
48 }
49
50 pb->sorted = malloc(sizeof(pb->sorted[0]) * max_samples);
51 if (!pb->sorted) {
52 free(pb->current_ua);
53 free(pb->mab);
54 free(pb);
55 return NULL;
56 }
57
58 pb->interval_us = interval_us;
59 pb->max_samples = max_samples;
60
61 pb->session_head = pb->session_tail = 0;
62 pb->current_head = pb->current_tail = 0;
63
64 return pb;
65 }
66
powerbuf_free(powerbuf_t pb)67 void powerbuf_free(powerbuf_t pb)
68 {
69 free(pb->current_ua);
70 free(pb->mab);
71 free(pb->sorted);
72 free(pb);
73 }
74
powerbuf_clear(powerbuf_t pb)75 void powerbuf_clear(powerbuf_t pb)
76 {
77 pb->session_head = pb->session_tail = 0;
78 pb->current_head = pb->current_tail = 0;
79 pb->sort_valid = 0;
80 }
81
session_length(powerbuf_t pb,unsigned int idx)82 static unsigned int session_length(powerbuf_t pb, unsigned int idx)
83 {
84 const unsigned int next_idx = (idx + 1) % POWERBUF_MAX_SESSIONS;
85 unsigned int end_index = pb->current_head;
86
87 /* If a session follows this one, the end index is the start
88 * index of the following session. Otherwise, it's the end index
89 * of the entire current/MAB buffer.
90 */
91 if (next_idx != pb->session_head)
92 end_index = pb->sessions[next_idx].start_index;
93
94 /* Return (end-start) modulo max_samples */
95 return (end_index + pb->max_samples - pb->sessions[idx].start_index) %
96 pb->max_samples;
97 }
98
pop_oldest_session(powerbuf_t pb)99 static void pop_oldest_session(powerbuf_t pb)
100 {
101 unsigned int length = session_length(pb, pb->session_tail);
102
103 /* Remove corresponding samples from the tail of the current/MAB
104 * buffers.
105 */
106 pb->current_tail = (pb->current_tail + length) % pb->max_samples;
107
108 /* Remove the session from the session buffer. */
109 pb->session_tail = (pb->session_tail + 1) % POWERBUF_MAX_SESSIONS;
110 }
111
powerbuf_begin_session(powerbuf_t pb,time_t when)112 void powerbuf_begin_session(powerbuf_t pb, time_t when)
113 {
114 struct powerbuf_session *s;
115 unsigned int next_head;
116
117 /* If the most recent session is empty, remove it */
118 powerbuf_end_session(pb);
119
120 /* If the session buffer is full, drop the oldest */
121 next_head = (pb->session_head + 1) % POWERBUF_MAX_SESSIONS;
122 if (next_head == pb->session_tail)
123 pop_oldest_session(pb);
124
125 /* Copy data to the head */
126 s = &pb->sessions[pb->session_head];
127 s->wall_clock = when;
128 s->start_index = pb->current_head;
129 s->total_ua = 0;
130
131 /* Advance the head pointer */
132 pb->session_head = next_head;
133 }
134
135 /* Return the index of the nth most recent session */
rev_index(powerbuf_t pb,unsigned int n)136 static unsigned int rev_index(powerbuf_t pb, unsigned int n)
137 {
138 return (pb->session_head + POWERBUF_MAX_SESSIONS - 1 - n) %
139 POWERBUF_MAX_SESSIONS;
140 }
141
powerbuf_end_session(powerbuf_t pb)142 void powerbuf_end_session(powerbuf_t pb)
143 {
144 /* (head-1) modulo MAX_SESSIONS */
145 const unsigned int last_idx = rev_index(pb, 0);
146
147 /* If there are no sessions, do nothing */
148 if (pb->session_head == pb->session_tail)
149 return;
150
151 /* If no samples were added since the session began, decrement
152 * the head pointer.
153 */
154 if (pb->sessions[last_idx].start_index == pb->current_head)
155 pb->session_head = last_idx;
156 }
157
powerbuf_num_sessions(powerbuf_t pb)158 unsigned int powerbuf_num_sessions(powerbuf_t pb)
159 {
160 /* (head-tail) modulo MAX_SESSIONS */
161 return (pb->session_head + POWERBUF_MAX_SESSIONS - pb->session_tail) %
162 POWERBUF_MAX_SESSIONS;
163 }
164
powerbuf_session_info(powerbuf_t pb,unsigned int rev_idx,unsigned int * length)165 const struct powerbuf_session *powerbuf_session_info(powerbuf_t pb,
166 unsigned int rev_idx, unsigned int *length)
167 {
168 /* (head-1-rev_idx) modulo MAX_SESSIONS */
169 const unsigned int idx_map = rev_index(pb, rev_idx);
170
171 if (length)
172 *length = session_length(pb, idx_map);
173
174 return &pb->sessions[idx_map];
175 }
176
ensure_room(powerbuf_t pb,unsigned int required)177 static void ensure_room(powerbuf_t pb, unsigned int required)
178 {
179 unsigned int room =
180 (pb->current_tail + pb->max_samples - pb->current_head - 1) %
181 pb->max_samples;
182
183 /* Drop old sessions if they're smaller than what we need to
184 * reclaim.
185 */
186 while (room < required && powerbuf_num_sessions(pb) > 1) {
187 const unsigned int len = session_length(pb, pb->session_tail);
188
189 if (room + len > required)
190 break;
191
192 pop_oldest_session(pb);
193 room += len;
194 }
195
196 /* If we still lack space, it must be because the oldest session
197 * is larger than what we still need to reclaim (we'll never be
198 * asked to reclaim more than the buffer can hold).
199 *
200 * We also know at this point that (required-room) is <= the
201 * length of the oldest session.
202 */
203 while (room < required) {
204 struct powerbuf_session *old =
205 &pb->sessions[pb->session_tail];
206 unsigned int cont_len = pb->max_samples - old->start_index;
207 int i;
208
209 if (cont_len + room > required)
210 cont_len = required - room;
211
212 /* Un-integrate current */
213 for (i = 0; i < cont_len; i++)
214 old->total_ua -=
215 pb->current_ua[old->start_index + i];
216
217 /* Advance the start index and buffer tail */
218 old->start_index = (old->start_index + cont_len) %
219 pb->max_samples;
220 pb->current_tail = (pb->current_tail + cont_len) %
221 pb->max_samples;
222
223 room += cont_len;
224 }
225 }
226
powerbuf_add_samples(powerbuf_t pb,unsigned int count,const unsigned int * current_ua,const address_t * mab)227 void powerbuf_add_samples(powerbuf_t pb, unsigned int count,
228 const unsigned int *current_ua,
229 const address_t *mab)
230 {
231 struct powerbuf_session *cur = &pb->sessions[rev_index(pb, 0)];
232 int i;
233
234 /* If no session is active, do nothing */
235 if (pb->session_head == pb->session_tail)
236 return;
237
238 /* Make sure that we can't overflow the buffer in a single
239 * chunk.
240 */
241 if (count > pb->max_samples - 1) {
242 int extra = pb->max_samples - 1 - count;
243
244 current_ua += extra;
245 mab += extra;
246 count -= extra;
247 }
248
249 /* Push old samples/sessions out of the buffer if we need to. */
250 ensure_room(pb, count);
251
252 /* Add current integral to the session's running count */
253 for (i = 0; i < count; i++)
254 cur->total_ua += current_ua[i];
255
256 /* Add samples in contiguous chunks */
257 while (count) {
258 unsigned int cont_len = pb->max_samples - pb->current_head;
259
260 /* Don't copy more than we have */
261 if (cont_len > count)
262 cont_len = count;
263
264 /* Copy samples */
265 memcpy(pb->current_ua + pb->current_head, current_ua,
266 sizeof(pb->current_ua[0]) * cont_len);
267 memcpy(pb->mab + pb->current_head, mab,
268 sizeof(pb->mab[0]) * cont_len);
269 pb->current_head = (pb->current_head +
270 cont_len) % pb->max_samples;
271
272 /* Advance source pointers and count */
273 current_ua += cont_len;
274 mab += cont_len;
275 count -= cont_len;
276 }
277
278 pb->sort_valid = 0;
279 }
280
powerbuf_last_mab(powerbuf_t pb)281 address_t powerbuf_last_mab(powerbuf_t pb)
282 {
283 const struct powerbuf_session *s = &pb->sessions[rev_index(pb, 0)];
284 const unsigned int last = (pb->current_head + pb->max_samples - 1) %
285 pb->max_samples;
286
287 if (s->start_index == pb->current_head)
288 return 0;
289
290 return pb->mab[last];
291 }
292
sift_down(powerbuf_t pb,int start,int end)293 static void sift_down(powerbuf_t pb, int start, int end)
294 {
295 int root = start;
296
297 while (root * 2 + 1 <= end) {
298 int left_child = root * 2 + 1;
299 int biggest = root;
300 unsigned int temp;
301
302 /* Find the largest of
303 * (root, left child, right child)
304 */
305 if (pb->mab[pb->sorted[biggest]] <
306 pb->mab[pb->sorted[left_child]])
307 biggest = left_child;
308 if (left_child + 1 <= end &&
309 (pb->mab[pb->sorted[biggest]] <
310 pb->mab[pb->sorted[left_child + 1]]))
311 biggest = left_child + 1;
312
313 /* If no changes are needed, the heap property is ok and
314 * we can stop.
315 */
316 if (biggest == root)
317 break;
318
319 /* Swap the root with its largest child */
320 temp = pb->sorted[biggest];
321 pb->sorted[biggest] = pb->sorted[root];
322 pb->sorted[root] = temp;
323
324 /* Continue to push down the old root (now a child) */
325 root = biggest;
326 }
327 }
328
heapify(powerbuf_t pb,int num_samples)329 static void heapify(powerbuf_t pb, int num_samples)
330 {
331 int start = (num_samples - 2) / 2;
332
333 while (start >= 0) {
334 sift_down(pb, start, num_samples - 1);
335 start--;
336 }
337 }
338
heap_extract(powerbuf_t pb,int num_samples)339 static void heap_extract(powerbuf_t pb, int num_samples)
340 {
341 int end = num_samples - 1;
342
343 while (end > 0) {
344 unsigned int temp;
345
346 /* Swap the top of the heap with the end of the array,
347 * and shrink the heap.
348 */
349 temp = pb->sorted[0];
350 pb->sorted[0] = pb->sorted[end];
351 pb->sorted[end] = temp;
352 end--;
353
354 /* Fix up the heap (push down the new root) */
355 sift_down(pb, 0, end);
356 }
357 }
358
powerbuf_sort(powerbuf_t pb)359 void powerbuf_sort(powerbuf_t pb)
360 {
361 const unsigned int num_samples =
362 (pb->current_head + pb->max_samples - pb->current_tail) %
363 pb->max_samples;
364 unsigned int i;
365
366 if (pb->sort_valid)
367 return;
368
369 /* Prepare an index list */
370 for (i = 0; i < num_samples; i++)
371 pb->sorted[i] = (pb->current_tail + i) % pb->max_samples;
372
373 if (num_samples < 2) {
374 pb->sort_valid = 1;
375 return;
376 }
377
378 heapify(pb, num_samples);
379 heap_extract(pb, num_samples);
380 pb->sort_valid = 1;
381 }
382
383 /* Find the index within the sorted index of the first sample with an
384 * MAB >= the given mab parameter.
385 */
find_mab_ge(powerbuf_t pb,address_t mab)386 static int find_mab_ge(powerbuf_t pb, address_t mab)
387 {
388 const int num_samples =
389 (pb->current_head + pb->max_samples - pb->current_tail) %
390 pb->max_samples;
391 int low = 0;
392 int high = num_samples - 1;
393
394 while (low <= high) {
395 int mid = (low + high) / 2;
396
397 if (pb->mab[pb->sorted[mid]] < mab)
398 low = mid + 1;
399 else if ((mid <= 0) || (pb->mab[pb->sorted[mid - 1]] < mab))
400 return mid;
401 else
402 high = mid - 1;
403 }
404
405 return -1;
406 }
407
powerbuf_get_by_mab(powerbuf_t pb,address_t mab,unsigned long long * sum_ua)408 int powerbuf_get_by_mab(powerbuf_t pb, address_t mab,
409 unsigned long long *sum_ua)
410 {
411 const unsigned int num_samples =
412 (pb->current_head + pb->max_samples - pb->current_tail) %
413 pb->max_samples;
414 int i;
415 int count = 0;
416
417 if (!pb->sort_valid)
418 powerbuf_sort(pb);
419
420 i = find_mab_ge(pb, mab);
421 if (i < 0)
422 return 0;
423
424 *sum_ua = 0;
425
426 while ((i < num_samples) && (pb->mab[pb->sorted[i]] == mab)) {
427 *sum_ua += pb->current_ua[pb->sorted[i]];
428 count++;
429 i++;
430 }
431
432 return count;
433 }
434