ksal.c (5941B)
1 /* 2 * Copyright 2017 Broadcom 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, version 2, as 6 * published by the Free Software Foundation (the "GPL"). 7 * 8 * This program is distributed in the hope that it will be useful, but 9 * WITHOUT ANY WARRANTY; without even the implied warranty of 10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 * General Public License version 2 (GPLv2) for more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * version 2 (GPLv2) along with this source code. 15 */ 16 /* 17 * $Id: ksal.c,v 1.1 Broadcom SDK $ 18 * $Copyright: (c) 2005 Broadcom Corp. 19 * All Rights Reserved.$ 20 */ 21 22 #include <sal/core/sync.h> 23 #include <sal/core/thread.h> 24 25 #include "lkm.h" 26 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26) 27 #include <linux/semaphore.h> 28 #else 29 #include <asm/semaphore.h> 30 #endif 31 #include <linux/interrupt.h> 32 #include <linux/sched.h> 33 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0) 34 #include <linux/sched/rt.h> 35 #endif 36 #include <linux/time.h> 37 38 #ifdef MAX_USER_RT_PRIO 39 /* Assume 2.6 scheduler */ 40 #define SAL_YIELD(task) \ 41 yield() 42 #else 43 /* Assume 2.4 scheduler */ 44 #define SAL_YIELD(task) \ 45 do { \ 46 task->policy |= SCHED_YIELD; \ 47 schedule(); \ 48 } while (0) 49 #endif 50 51 #define SECOND_USEC (1000000) 52 #define USECS_PER_JIFFY (SECOND_USEC / HZ) 53 #define USEC_TO_JIFFIES(usec) ((usec + (USECS_PER_JIFFY - 1)) / USECS_PER_JIFFY) 54 55 #define sal_alloc(size, desc) kmalloc(size, GFP_KERNEL) 56 #define sal_free(ptr) kfree(ptr) 57 58 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,12) 59 #define WQ_SLEEP(a, b) wait_event_interruptible_timeout(a, NULL, b) 60 #else 61 #define WQ_SLEEP(a, b) interruptible_sleep_on_timeout(&(a), b) 62 #endif 63 /* 64 * sem_ctrl_t 65 * 66 * The semaphore control type uses the binary property to implement 67 * timed semaphores with improved performance using wait queues. 68 */ 69 70 typedef struct sem_ctrl_s { 71 struct semaphore sem; 72 int binary; 73 int cnt; 74 wait_queue_head_t wq; 75 } sem_ctrl_t; 76 77 sal_sem_t 78 sal_sem_create(char *desc, int binary, int initial_count) 79 { 80 sem_ctrl_t *s; 81 82 if ((s = sal_alloc(sizeof(*s), desc)) != 0) { 83 sema_init(&s->sem, initial_count); 84 s->binary = binary; 85 if (s->binary) { 86 init_waitqueue_head(&s->wq); 87 } 88 } 89 90 return (sal_sem_t) s; 91 } 92 93 void 94 sal_sem_destroy(sal_sem_t b) 95 { 96 sem_ctrl_t *s = (sem_ctrl_t *) b; 97 98 if (s == NULL) { 99 return; 100 } 101 102 /* 103 * the linux kernel does not have a sema_destroy(s) 104 */ 105 sal_free(s); 106 } 107 108 int 109 sal_sem_take(sal_sem_t b, int usec) 110 { 111 sem_ctrl_t *s = (sem_ctrl_t *) b; 112 int err; 113 114 if (usec == sal_sem_FOREVER && !in_interrupt()) { 115 err = down_interruptible(&s->sem); 116 } else { 117 int time_wait = 1; 118 int cnt = s->cnt; 119 120 for (;;) { 121 if (down_trylock(&s->sem) == 0) { 122 err = 0; 123 break; 124 } 125 126 if (s->binary) { 127 128 /* Wait for event or timeout */ 129 130 if (time_wait > 1) { 131 err = 1; 132 break; 133 } 134 err = wait_event_interruptible_timeout(s->wq, cnt != s->cnt, 135 USEC_TO_JIFFIES(usec)); 136 if (err < 0) { 137 break; 138 } 139 time_wait++; 140 141 } else { 142 143 /* Retry algorithm with exponential backoff */ 144 145 if (time_wait > usec) { 146 time_wait = usec; 147 } 148 149 sal_usleep(time_wait); 150 151 usec -= time_wait; 152 153 if (usec == 0) { 154 err = ETIMEDOUT; 155 break; 156 } 157 158 if ((time_wait *= 2) > 100000) { 159 time_wait = 100000; 160 } 161 } 162 } 163 } 164 return err ? -1 : 0; 165 } 166 167 int 168 sal_sem_give(sal_sem_t b) 169 { 170 sem_ctrl_t *s = (sem_ctrl_t *) b; 171 172 up(&s->sem); 173 if (s->binary) { 174 s->cnt++; 175 wake_up_interruptible(&s->wq); 176 } 177 return 0; 178 } 179 180 uint32 181 sal_time_usecs(void) 182 { 183 struct timeval ltv; 184 do_gettimeofday(<v); 185 return (ltv.tv_sec * SECOND_USEC + ltv.tv_usec); 186 } 187 188 void 189 sal_usleep(uint32 usec) 190 { 191 uint32 start_usec; 192 wait_queue_head_t queue; 193 194 if (usec <= SECOND_USEC / HZ) { 195 start_usec = sal_time_usecs(); 196 do { 197 SAL_YIELD(current); 198 } while ((sal_time_usecs() - start_usec) < usec); 199 } else { 200 init_waitqueue_head(&queue); 201 WQ_SLEEP(queue, USEC_TO_JIFFIES(usec)); 202 } 203 } 204 205 void 206 sal_udelay(uint32 usec) 207 { 208 static volatile int _sal_udelay_counter; 209 static int loops = 0; 210 int ix, iy; 211 212 if (loops == 0 || usec == 0) { /* Need calibration? */ 213 int max_loops; 214 int start = 0, stop = 0; 215 int mpt = USECS_PER_JIFFY; /* usec/tick */ 216 217 for (loops = 1; loops < 0x1000 && stop == start; loops <<= 1) { 218 /* Wait for clock turn over */ 219 for (stop = start = jiffies; start == stop; start = jiffies) { 220 /* Empty */ 221 } 222 sal_udelay(mpt); /* Single recursion */ 223 stop = jiffies; 224 } 225 226 max_loops = loops / 2; /* Loop above overshoots */ 227 228 start = stop = 0; 229 230 if (loops < 4) { 231 loops = 4; 232 } 233 234 for (loops /= 4; loops < max_loops && stop == start; loops++) { 235 /* Wait for clock turn over */ 236 for (stop = start = jiffies; start == stop; start = jiffies) { 237 /* Empty */ 238 } 239 sal_udelay(mpt); /* Single recursion */ 240 stop = jiffies; 241 } 242 } 243 244 for (iy = 0; iy < usec; iy++) { 245 for (ix = 0; ix < loops; ix++) { 246 _sal_udelay_counter++; /* Prevent optimizations */ 247 } 248 } 249 }