1 /*
2  * Copyright (c) 2012 Neratec Solutions AG
3  *
4  * Permission to use, copy, modify, and/or distribute this software for any
5  * purpose with or without fee is hereby granted, provided that the above
6  * copyright notice and this permission notice appear in all copies.
7  *
8  * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
9  * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
10  * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
11  * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
12  * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
13  * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
14  * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
15  */
16 
17 #include <linux/slab.h>
18 #include <linux/spinlock.h>
19 
20 #include "ath.h"
21 #include "dfs_pattern_detector.h"
22 #include "dfs_pri_detector.h"
23 
24 struct ath_dfs_pool_stats global_dfs_pool_stats = {};
25 
26 #define DFS_POOL_STAT_INC(c) (global_dfs_pool_stats.c++)
27 #define DFS_POOL_STAT_DEC(c) (global_dfs_pool_stats.c--)
28 #define GET_PRI_TO_USE(MIN, MAX, RUNTIME) \
29 	(MIN + PRI_TOLERANCE == MAX - PRI_TOLERANCE ? \
30 	MIN + PRI_TOLERANCE : RUNTIME)
31 
32 /*
33  * struct pulse_elem - elements in pulse queue
34  */
35 struct pulse_elem {
36 	struct list_head head;
37 	u64 ts;
38 };
39 
40 /*
41  * pde_get_multiple() - get number of multiples considering a given tolerance
42  * Return value: factor if abs(val - factor*fraction) <= tolerance, 0 otherwise
43  */
pde_get_multiple(u32 val,u32 fraction,u32 tolerance)44 static u32 pde_get_multiple(u32 val, u32 fraction, u32 tolerance)
45 {
46 	u32 remainder;
47 	u32 factor;
48 	u32 delta;
49 
50 	if (fraction == 0)
51 		return 0;
52 
53 	delta = (val < fraction) ? (fraction - val) : (val - fraction);
54 
55 	if (delta <= tolerance)
56 		/* val and fraction are within tolerance */
57 		return 1;
58 
59 	factor = val / fraction;
60 	remainder = val % fraction;
61 	if (remainder > tolerance) {
62 		/* no exact match */
63 		if ((fraction - remainder) <= tolerance)
64 			/* remainder is within tolerance */
65 			factor++;
66 		else
67 			factor = 0;
68 	}
69 	return factor;
70 }
71 
72 /*
73  * DOC: Singleton Pulse and Sequence Pools
74  *
75  * Instances of pri_sequence and pulse_elem are kept in singleton pools to
76  * reduce the number of dynamic allocations. They are shared between all
77  * instances and grow up to the peak number of simultaneously used objects.
78  *
79  * Memory is freed after all references to the pools are released.
80  */
81 static u32 singleton_pool_references;
82 static LIST_HEAD(pulse_pool);
83 static LIST_HEAD(pseq_pool);
84 static DEFINE_SPINLOCK(pool_lock);
85 
pool_register_ref(void)86 static void pool_register_ref(void)
87 {
88 	spin_lock_bh(&pool_lock);
89 	singleton_pool_references++;
90 	DFS_POOL_STAT_INC(pool_reference);
91 	spin_unlock_bh(&pool_lock);
92 }
93 
pool_deregister_ref(void)94 static void pool_deregister_ref(void)
95 {
96 	spin_lock_bh(&pool_lock);
97 	singleton_pool_references--;
98 	DFS_POOL_STAT_DEC(pool_reference);
99 	if (singleton_pool_references == 0) {
100 		/* free singleton pools with no references left */
101 		struct pri_sequence *ps, *ps0;
102 		struct pulse_elem *p, *p0;
103 
104 		list_for_each_entry_safe(p, p0, &pulse_pool, head) {
105 			list_del(&p->head);
106 			DFS_POOL_STAT_DEC(pulse_allocated);
107 			kfree(p);
108 		}
109 		list_for_each_entry_safe(ps, ps0, &pseq_pool, head) {
110 			list_del(&ps->head);
111 			DFS_POOL_STAT_DEC(pseq_allocated);
112 			kfree(ps);
113 		}
114 	}
115 	spin_unlock_bh(&pool_lock);
116 }
117 
pool_put_pulse_elem(struct pulse_elem * pe)118 static void pool_put_pulse_elem(struct pulse_elem *pe)
119 {
120 	spin_lock_bh(&pool_lock);
121 	list_add(&pe->head, &pulse_pool);
122 	DFS_POOL_STAT_DEC(pulse_used);
123 	spin_unlock_bh(&pool_lock);
124 }
125 
pool_put_pseq_elem(struct pri_sequence * pse)126 static void pool_put_pseq_elem(struct pri_sequence *pse)
127 {
128 	spin_lock_bh(&pool_lock);
129 	list_add(&pse->head, &pseq_pool);
130 	DFS_POOL_STAT_DEC(pseq_used);
131 	spin_unlock_bh(&pool_lock);
132 }
133 
pool_get_pseq_elem(void)134 static struct pri_sequence *pool_get_pseq_elem(void)
135 {
136 	struct pri_sequence *pse = NULL;
137 	spin_lock_bh(&pool_lock);
138 	if (!list_empty(&pseq_pool)) {
139 		pse = list_first_entry(&pseq_pool, struct pri_sequence, head);
140 		list_del(&pse->head);
141 		DFS_POOL_STAT_INC(pseq_used);
142 	}
143 	spin_unlock_bh(&pool_lock);
144 	return pse;
145 }
146 
pool_get_pulse_elem(void)147 static struct pulse_elem *pool_get_pulse_elem(void)
148 {
149 	struct pulse_elem *pe = NULL;
150 	spin_lock_bh(&pool_lock);
151 	if (!list_empty(&pulse_pool)) {
152 		pe = list_first_entry(&pulse_pool, struct pulse_elem, head);
153 		list_del(&pe->head);
154 		DFS_POOL_STAT_INC(pulse_used);
155 	}
156 	spin_unlock_bh(&pool_lock);
157 	return pe;
158 }
159 
pulse_queue_get_tail(struct pri_detector * pde)160 static struct pulse_elem *pulse_queue_get_tail(struct pri_detector *pde)
161 {
162 	struct list_head *l = &pde->pulses;
163 	if (list_empty(l))
164 		return NULL;
165 	return list_entry(l->prev, struct pulse_elem, head);
166 }
167 
pulse_queue_dequeue(struct pri_detector * pde)168 static bool pulse_queue_dequeue(struct pri_detector *pde)
169 {
170 	struct pulse_elem *p = pulse_queue_get_tail(pde);
171 	if (p != NULL) {
172 		list_del_init(&p->head);
173 		pde->count--;
174 		/* give it back to pool */
175 		pool_put_pulse_elem(p);
176 	}
177 	return (pde->count > 0);
178 }
179 
180 /* remove pulses older than window */
pulse_queue_check_window(struct pri_detector * pde)181 static void pulse_queue_check_window(struct pri_detector *pde)
182 {
183 	u64 min_valid_ts;
184 	struct pulse_elem *p;
185 
186 	/* there is no delta time with less than 2 pulses */
187 	if (pde->count < 2)
188 		return;
189 
190 	if (pde->last_ts <= pde->window_size)
191 		return;
192 
193 	min_valid_ts = pde->last_ts - pde->window_size;
194 	while ((p = pulse_queue_get_tail(pde)) != NULL) {
195 		if (p->ts >= min_valid_ts)
196 			return;
197 		pulse_queue_dequeue(pde);
198 	}
199 }
200 
pulse_queue_enqueue(struct pri_detector * pde,u64 ts)201 static bool pulse_queue_enqueue(struct pri_detector *pde, u64 ts)
202 {
203 	struct pulse_elem *p = pool_get_pulse_elem();
204 	if (p == NULL) {
205 		p = kmalloc(sizeof(*p), GFP_ATOMIC);
206 		if (p == NULL) {
207 			DFS_POOL_STAT_INC(pulse_alloc_error);
208 			return false;
209 		}
210 		DFS_POOL_STAT_INC(pulse_allocated);
211 		DFS_POOL_STAT_INC(pulse_used);
212 	}
213 	INIT_LIST_HEAD(&p->head);
214 	p->ts = ts;
215 	list_add(&p->head, &pde->pulses);
216 	pde->count++;
217 	pde->last_ts = ts;
218 	pulse_queue_check_window(pde);
219 	if (pde->count >= pde->max_count)
220 		pulse_queue_dequeue(pde);
221 	return true;
222 }
223 
pseq_handler_create_sequences(struct pri_detector * pde,u64 ts,u32 min_count)224 static bool pseq_handler_create_sequences(struct pri_detector *pde,
225 					  u64 ts, u32 min_count)
226 {
227 	struct pulse_elem *p;
228 	list_for_each_entry(p, &pde->pulses, head) {
229 		struct pri_sequence ps, *new_ps;
230 		struct pulse_elem *p2;
231 		u32 tmp_false_count;
232 		u64 min_valid_ts;
233 		u32 delta_ts = ts - p->ts;
234 
235 		if (delta_ts < pde->rs->pri_min)
236 			/* ignore too small pri */
237 			continue;
238 
239 		if (delta_ts > pde->rs->pri_max)
240 			/* stop on too large pri (sorted list) */
241 			break;
242 
243 		/* build a new sequence with new potential pri */
244 		ps.count = 2;
245 		ps.count_falses = 0;
246 		ps.first_ts = p->ts;
247 		ps.last_ts = ts;
248 		ps.pri = GET_PRI_TO_USE(pde->rs->pri_min,
249 			pde->rs->pri_max, ts - p->ts);
250 		ps.dur = ps.pri * (pde->rs->ppb - 1)
251 				+ 2 * pde->rs->max_pri_tolerance;
252 
253 		p2 = p;
254 		tmp_false_count = 0;
255 		min_valid_ts = ts - ps.dur;
256 		/* check which past pulses are candidates for new sequence */
257 		list_for_each_entry_continue(p2, &pde->pulses, head) {
258 			u32 factor;
259 			if (p2->ts < min_valid_ts)
260 				/* stop on crossing window border */
261 				break;
262 			/* check if pulse match (multi)PRI */
263 			factor = pde_get_multiple(ps.last_ts - p2->ts, ps.pri,
264 						  pde->rs->max_pri_tolerance);
265 			if (factor > 0) {
266 				ps.count++;
267 				ps.first_ts = p2->ts;
268 				/*
269 				 * on match, add the intermediate falses
270 				 * and reset counter
271 				 */
272 				ps.count_falses += tmp_false_count;
273 				tmp_false_count = 0;
274 			} else {
275 				/* this is a potential false one */
276 				tmp_false_count++;
277 			}
278 		}
279 		if (ps.count <= min_count)
280 			/* did not reach minimum count, drop sequence */
281 			continue;
282 
283 		/* this is a valid one, add it */
284 		ps.deadline_ts = ps.first_ts + ps.dur;
285 		new_ps = pool_get_pseq_elem();
286 		if (new_ps == NULL) {
287 			new_ps = kmalloc(sizeof(*new_ps), GFP_ATOMIC);
288 			if (new_ps == NULL) {
289 				DFS_POOL_STAT_INC(pseq_alloc_error);
290 				return false;
291 			}
292 			DFS_POOL_STAT_INC(pseq_allocated);
293 			DFS_POOL_STAT_INC(pseq_used);
294 		}
295 		memcpy(new_ps, &ps, sizeof(ps));
296 		INIT_LIST_HEAD(&new_ps->head);
297 		list_add(&new_ps->head, &pde->sequences);
298 	}
299 	return true;
300 }
301 
302 /* check new ts and add to all matching existing sequences */
303 static u32
pseq_handler_add_to_existing_seqs(struct pri_detector * pde,u64 ts)304 pseq_handler_add_to_existing_seqs(struct pri_detector *pde, u64 ts)
305 {
306 	u32 max_count = 0;
307 	struct pri_sequence *ps, *ps2;
308 	list_for_each_entry_safe(ps, ps2, &pde->sequences, head) {
309 		u32 delta_ts;
310 		u32 factor;
311 
312 		/* first ensure that sequence is within window */
313 		if (ts > ps->deadline_ts) {
314 			list_del_init(&ps->head);
315 			pool_put_pseq_elem(ps);
316 			continue;
317 		}
318 
319 		delta_ts = ts - ps->last_ts;
320 		factor = pde_get_multiple(delta_ts, ps->pri,
321 					  pde->rs->max_pri_tolerance);
322 		if (factor > 0) {
323 			ps->last_ts = ts;
324 			ps->count++;
325 
326 			if (max_count < ps->count)
327 				max_count = ps->count;
328 		} else {
329 			ps->count_falses++;
330 		}
331 	}
332 	return max_count;
333 }
334 
335 static struct pri_sequence *
pseq_handler_check_detection(struct pri_detector * pde)336 pseq_handler_check_detection(struct pri_detector *pde)
337 {
338 	struct pri_sequence *ps;
339 
340 	if (list_empty(&pde->sequences))
341 		return NULL;
342 
343 	list_for_each_entry(ps, &pde->sequences, head) {
344 		/*
345 		 * we assume to have enough matching confidence if we
346 		 * 1) have enough pulses
347 		 * 2) have more matching than false pulses
348 		 */
349 		if ((ps->count >= pde->rs->ppb_thresh) &&
350 		    (ps->count * pde->rs->num_pri >= ps->count_falses))
351 			return ps;
352 	}
353 	return NULL;
354 }
355 
356 
357 /* free pulse queue and sequences list and give objects back to pools */
pri_detector_reset(struct pri_detector * pde,u64 ts)358 static void pri_detector_reset(struct pri_detector *pde, u64 ts)
359 {
360 	struct pri_sequence *ps, *ps0;
361 	struct pulse_elem *p, *p0;
362 	list_for_each_entry_safe(ps, ps0, &pde->sequences, head) {
363 		list_del_init(&ps->head);
364 		pool_put_pseq_elem(ps);
365 	}
366 	list_for_each_entry_safe(p, p0, &pde->pulses, head) {
367 		list_del_init(&p->head);
368 		pool_put_pulse_elem(p);
369 	}
370 	pde->count = 0;
371 	pde->last_ts = ts;
372 }
373 
pri_detector_exit(struct pri_detector * de)374 static void pri_detector_exit(struct pri_detector *de)
375 {
376 	pri_detector_reset(de, 0);
377 	pool_deregister_ref();
378 	kfree(de);
379 }
380 
pri_detector_add_pulse(struct pri_detector * de,struct pulse_event * event)381 static struct pri_sequence *pri_detector_add_pulse(struct pri_detector *de,
382 						   struct pulse_event *event)
383 {
384 	u32 max_updated_seq;
385 	struct pri_sequence *ps;
386 	u64 ts = event->ts;
387 	const struct radar_detector_specs *rs = de->rs;
388 
389 	/* ignore pulses not within width range */
390 	if ((rs->width_min > event->width) || (rs->width_max < event->width))
391 		return NULL;
392 
393 	if ((ts - de->last_ts) < rs->max_pri_tolerance)
394 		/* if delta to last pulse is too short, don't use this pulse */
395 		return NULL;
396 	/* radar detector spec needs chirp, but not detected */
397 	if (rs->chirp && rs->chirp != event->chirp)
398 		return NULL;
399 
400 	de->last_ts = ts;
401 
402 	max_updated_seq = pseq_handler_add_to_existing_seqs(de, ts);
403 
404 	if (!pseq_handler_create_sequences(de, ts, max_updated_seq)) {
405 		pri_detector_reset(de, ts);
406 		return NULL;
407 	}
408 
409 	ps = pseq_handler_check_detection(de);
410 
411 	if (ps == NULL)
412 		pulse_queue_enqueue(de, ts);
413 
414 	return ps;
415 }
416 
pri_detector_init(const struct radar_detector_specs * rs)417 struct pri_detector *pri_detector_init(const struct radar_detector_specs *rs)
418 {
419 	struct pri_detector *de;
420 
421 	de = kzalloc(sizeof(*de), GFP_ATOMIC);
422 	if (de == NULL)
423 		return NULL;
424 	de->exit = pri_detector_exit;
425 	de->add_pulse = pri_detector_add_pulse;
426 	de->reset = pri_detector_reset;
427 
428 	INIT_LIST_HEAD(&de->sequences);
429 	INIT_LIST_HEAD(&de->pulses);
430 	de->window_size = rs->pri_max * rs->ppb * rs->num_pri;
431 	de->max_count = rs->ppb * 2;
432 	de->rs = rs;
433 
434 	pool_register_ref();
435 	return de;
436 }
437