1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36#include <linux/slab.h>
37#include <linux/types.h>
38#include <linux/stddef.h>
39#include <linux/spinlock.h>
40#include <linux/mISDNif.h>
41#include "core.h"
42
43static u_int *debug;
44static LIST_HEAD(iclock_list);
45static DEFINE_RWLOCK(iclock_lock);
46static u16 iclock_count;
47static struct timeval iclock_tv;
48static int iclock_tv_valid;
49static struct mISDNclock *iclock_current;
50
51void
52mISDN_init_clock(u_int *dp)
53{
54 debug = dp;
55 do_gettimeofday(&iclock_tv);
56}
57
58static void
59select_iclock(void)
60{
61 struct mISDNclock *iclock, *bestclock = NULL, *lastclock = NULL;
62 int pri = -128;
63
64 list_for_each_entry(iclock, &iclock_list, list) {
65 if (iclock->pri > pri) {
66 pri = iclock->pri;
67 bestclock = iclock;
68 }
69 if (iclock_current == iclock)
70 lastclock = iclock;
71 }
72 if (lastclock && bestclock != lastclock) {
73
74 if (*debug & DEBUG_CLOCK)
75 printk(KERN_DEBUG "Old clock source '%s' disable.\n",
76 lastclock->name);
77 lastclock->ctl(lastclock->priv, 0);
78 }
79 if (bestclock && bestclock != iclock_current) {
80
81 if (*debug & DEBUG_CLOCK)
82 printk(KERN_DEBUG "New clock source '%s' enable.\n",
83 bestclock->name);
84 bestclock->ctl(bestclock->priv, 1);
85 }
86 if (bestclock != iclock_current) {
87
88 iclock_tv_valid = 0;
89 }
90 iclock_current = bestclock;
91}
92
93struct mISDNclock
94*mISDN_register_clock(char *name, int pri, clockctl_func_t *ctl, void *priv)
95{
96 u_long flags;
97 struct mISDNclock *iclock;
98
99 if (*debug & (DEBUG_CORE | DEBUG_CLOCK))
100 printk(KERN_DEBUG "%s: %s %d\n", __func__, name, pri);
101 iclock = kzalloc(sizeof(struct mISDNclock), GFP_ATOMIC);
102 if (!iclock) {
103 printk(KERN_ERR "%s: No memory for clock entry.\n", __func__);
104 return NULL;
105 }
106 strncpy(iclock->name, name, sizeof(iclock->name)-1);
107 iclock->pri = pri;
108 iclock->priv = priv;
109 iclock->ctl = ctl;
110 write_lock_irqsave(&iclock_lock, flags);
111 list_add_tail(&iclock->list, &iclock_list);
112 select_iclock();
113 write_unlock_irqrestore(&iclock_lock, flags);
114 return iclock;
115}
116EXPORT_SYMBOL(mISDN_register_clock);
117
118void
119mISDN_unregister_clock(struct mISDNclock *iclock)
120{
121 u_long flags;
122
123 if (*debug & (DEBUG_CORE | DEBUG_CLOCK))
124 printk(KERN_DEBUG "%s: %s %d\n", __func__, iclock->name,
125 iclock->pri);
126 write_lock_irqsave(&iclock_lock, flags);
127 if (iclock_current == iclock) {
128 if (*debug & DEBUG_CLOCK)
129 printk(KERN_DEBUG
130 "Current clock source '%s' unregisters.\n",
131 iclock->name);
132 iclock->ctl(iclock->priv, 0);
133 }
134 list_del(&iclock->list);
135 select_iclock();
136 write_unlock_irqrestore(&iclock_lock, flags);
137}
138EXPORT_SYMBOL(mISDN_unregister_clock);
139
140void
141mISDN_clock_update(struct mISDNclock *iclock, int samples, struct timeval *tv)
142{
143 u_long flags;
144 struct timeval tv_now;
145 time_t elapsed_sec;
146 int elapsed_8000th;
147
148 write_lock_irqsave(&iclock_lock, flags);
149 if (iclock_current != iclock) {
150 printk(KERN_ERR "%s: '%s' sends us clock updates, but we do "
151 "listen to '%s'. This is a bug!\n", __func__,
152 iclock->name,
153 iclock_current ? iclock_current->name : "nothing");
154 iclock->ctl(iclock->priv, 0);
155 write_unlock_irqrestore(&iclock_lock, flags);
156 return;
157 }
158 if (iclock_tv_valid) {
159
160 iclock_count += samples;
161 if (tv) {
162 iclock_tv.tv_sec = tv->tv_sec;
163 iclock_tv.tv_usec = tv->tv_usec;
164 } else
165 do_gettimeofday(&iclock_tv);
166 } else {
167
168 if (tv) {
169 tv_now.tv_sec = tv->tv_sec;
170 tv_now.tv_usec = tv->tv_usec;
171 } else
172 do_gettimeofday(&tv_now);
173 elapsed_sec = tv_now.tv_sec - iclock_tv.tv_sec;
174 elapsed_8000th = (tv_now.tv_usec / 125)
175 - (iclock_tv.tv_usec / 125);
176 if (elapsed_8000th < 0) {
177 elapsed_sec -= 1;
178 elapsed_8000th += 8000;
179 }
180
181 iclock_count += elapsed_sec * 8000 + elapsed_8000th;
182 iclock_tv.tv_sec = tv_now.tv_sec;
183 iclock_tv.tv_usec = tv_now.tv_usec;
184 iclock_tv_valid = 1;
185 if (*debug & DEBUG_CLOCK)
186 printk("Received first clock from source '%s'.\n",
187 iclock_current ? iclock_current->name : "nothing");
188 }
189 write_unlock_irqrestore(&iclock_lock, flags);
190}
191EXPORT_SYMBOL(mISDN_clock_update);
192
193unsigned short
194mISDN_clock_get(void)
195{
196 u_long flags;
197 struct timeval tv_now;
198 time_t elapsed_sec;
199 int elapsed_8000th;
200 u16 count;
201
202 read_lock_irqsave(&iclock_lock, flags);
203
204 do_gettimeofday(&tv_now);
205 elapsed_sec = tv_now.tv_sec - iclock_tv.tv_sec;
206 elapsed_8000th = (tv_now.tv_usec / 125) - (iclock_tv.tv_usec / 125);
207 if (elapsed_8000th < 0) {
208 elapsed_sec -= 1;
209 elapsed_8000th += 8000;
210 }
211
212 count = iclock_count + elapsed_sec * 8000 + elapsed_8000th;
213 read_unlock_irqrestore(&iclock_lock, flags);
214 return count;
215}
216EXPORT_SYMBOL(mISDN_clock_get);
217
218