Shad Ansari | 2f7f9be | 2017-06-07 13:34:53 -0700 | [diff] [blame] | 1 | /* |
| 2 | <:copyright-BRCM:2016:DUAL/GPL:standard |
| 3 | |
| 4 | Broadcom Proprietary and Confidential.(c) 2016 Broadcom |
| 5 | All Rights Reserved |
| 6 | |
| 7 | Unless you and Broadcom execute a separate written software license |
| 8 | agreement governing use of this software, this software is licensed |
| 9 | to you under the terms of the GNU General Public License version 2 |
| 10 | (the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php, |
| 11 | with the following added to such license: |
| 12 | |
| 13 | As a special exception, the copyright holders of this software give |
| 14 | you permission to link this software with independent modules, and |
| 15 | to copy and distribute the resulting executable under terms of your |
| 16 | choice, provided that you also meet, for each linked independent |
| 17 | module, the terms and conditions of the license of that module. |
| 18 | An independent module is a module which is not derived from this |
| 19 | software. The special exception does not apply to any modifications |
| 20 | of the software. |
| 21 | |
| 22 | Not withstanding the above, under no circumstances may you combine |
| 23 | this software in any way with any other Broadcom software provided |
| 24 | under a license other than the GPL, without Broadcom's express prior |
| 25 | written consent. |
| 26 | |
| 27 | :> |
| 28 | */ |
| 29 | |
| 30 | #include "bcmos_system.h" |
| 31 | #ifdef BCMOS_MSG_QUEUE_DOMAIN_SOCKET |
| 32 | #include <sys/un.h> |
| 33 | #endif |
| 34 | #include <sys/resource.h> |
| 35 | |
| 36 | /* task control blocks */ |
| 37 | extern STAILQ_HEAD(task_list, bcmos_task) task_list; |
| 38 | |
| 39 | /* global OS lock */ |
| 40 | extern bcmos_mutex bcmos_res_lock; |
| 41 | |
| 42 | /* |
| 43 | * Init |
| 44 | */ |
| 45 | |
| 46 | #define TIMER_SIG SIGRTMIN |
| 47 | |
| 48 | /* Initialize system library */ |
| 49 | bcmos_errno bcmos_sys_init(void) |
| 50 | { |
| 51 | return BCM_ERR_OK; |
| 52 | } |
| 53 | |
| 54 | /* Clean-up system library */ |
| 55 | void bcmos_sys_exit(void) |
| 56 | { |
| 57 | } |
| 58 | |
| 59 | /* |
| 60 | * Task management |
| 61 | */ |
| 62 | |
| 63 | /* |
| 64 | * Default task handler |
| 65 | */ |
| 66 | |
| 67 | /* Create a new task */ |
| 68 | bcmos_errno bcmos_task_create(bcmos_task *task, const bcmos_task_parm *parm) |
| 69 | { |
| 70 | F_bcmos_task_handler handler; |
| 71 | pthread_attr_t pthread_attr; |
| 72 | struct sched_param pthread_sched_param = {}; |
| 73 | int pthread_prio; |
| 74 | uint32_t stack_size; |
| 75 | void *data; |
| 76 | int rc; |
| 77 | |
| 78 | if (!task || !parm) |
| 79 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "task %p, parm %p\n", task, parm); |
| 80 | memset(task, 0, sizeof(*task)); |
| 81 | if (parm->handler) |
| 82 | { |
| 83 | /* "traditional task */ |
| 84 | handler = parm->handler; |
| 85 | data = (void *)parm->data; |
| 86 | } |
| 87 | else |
| 88 | { |
| 89 | /* "integrated" task */ |
| 90 | handler = bcmos_dft_task_handler; |
| 91 | data = task; |
| 92 | |
| 93 | /* Initialize and lock mutex to wait on */ |
| 94 | rc = bcmos_sem_create(&task->active_sem, 0, task->parm.flags, parm->name); |
| 95 | if (rc) |
| 96 | { |
| 97 | BCMOS_TRACE_ERR("Task %s: can't create active_sem. Error %s (%d)\n", |
| 98 | parm->name, bcmos_strerror(rc), rc); |
| 99 | return rc; |
| 100 | } |
| 101 | } |
| 102 | |
| 103 | task->parm = *parm; |
| 104 | /* Copy name to make sure that it is not released - in case it was on the stack */ |
| 105 | if (task->parm.name) |
| 106 | { |
| 107 | strncpy(task->name, task->parm.name, sizeof(task->name) - 1); |
| 108 | task->parm.name = task->name; |
| 109 | } |
| 110 | bcmos_fastlock_init(&task->active_lock, 0); |
| 111 | bcmos_mutex_lock(&bcmos_res_lock); |
| 112 | STAILQ_INSERT_TAIL(&task_list, task, list); |
| 113 | bcmos_mutex_unlock(&bcmos_res_lock); |
| 114 | task->magic = BCMOS_TASK_MAGIC; |
| 115 | /* pthread priorities are 1..32, 32 being the highest */ |
| 116 | pthread_prio = 32 - (int)parm->priority; |
| 117 | if (pthread_prio <= 0) |
| 118 | pthread_prio = 1; |
| 119 | stack_size = PTHREAD_STACK_MIN; |
| 120 | if (parm->stack_size && parm->stack_size > PTHREAD_STACK_MIN) |
| 121 | stack_size = parm->stack_size; |
| 122 | rc = pthread_attr_init(&pthread_attr); |
| 123 | pthread_sched_param.sched_priority = pthread_prio; |
| 124 | rc = rc ? rc : pthread_attr_setinheritsched(&pthread_attr, PTHREAD_EXPLICIT_SCHED); |
| 125 | rc = rc ? rc : pthread_attr_setschedpolicy(&pthread_attr, SCHED_RR); |
| 126 | rc = rc ? rc : pthread_attr_setschedparam(&pthread_attr, &pthread_sched_param); |
| 127 | rc = rc ? rc : pthread_attr_setstacksize(&pthread_attr, stack_size); |
| 128 | rc = rc ? rc : pthread_create(&task->sys_task.t, &pthread_attr, (void *(*)(void *))handler, data); |
| 129 | pthread_attr_destroy(&pthread_attr); |
| 130 | if (rc == EPERM) |
| 131 | { |
| 132 | BCMOS_TRACE_INFO("Thread %s: priority %d is ignored. Start as root to honor priorities\n", |
| 133 | parm->name, (int)parm->priority); |
| 134 | rc = pthread_create(&task->sys_task.t, NULL, (void *(*)(void *))handler, data); |
| 135 | } |
| 136 | if (rc) |
| 137 | { |
| 138 | bcmos_mutex_lock(&bcmos_res_lock); |
| 139 | STAILQ_REMOVE(&task_list, task, bcmos_task, list); |
| 140 | bcmos_mutex_unlock(&bcmos_res_lock); |
| 141 | task->magic = 0; |
| 142 | if (!parm->handler) |
| 143 | { |
| 144 | bcmos_sem_destroy(&task->active_sem); |
| 145 | } |
| 146 | BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "%s (%d)\n", strerror(rc), rc); |
| 147 | } |
| 148 | return BCM_ERR_OK; |
| 149 | } |
| 150 | |
| 151 | /* Destroy task */ |
| 152 | bcmos_errno bcmos_task_destroy(bcmos_task *task) |
| 153 | { |
| 154 | void *res; |
| 155 | int rc_cancel; |
| 156 | int rc; |
| 157 | |
| 158 | if (task->magic != BCMOS_TASK_MAGIC) |
| 159 | { |
| 160 | return BCM_ERR_PARM; |
| 161 | } |
| 162 | task->destroy_request = BCMOS_TRUE; |
| 163 | task->magic = BCMOS_TASK_MAGIC_DESTROYED; |
| 164 | bcmos_mutex_lock(&bcmos_res_lock); |
| 165 | STAILQ_REMOVE(&task_list, task, bcmos_task, list); |
| 166 | bcmos_mutex_unlock(&bcmos_res_lock); |
| 167 | /* The task may be waiting on semaphore. Kick it */ |
| 168 | if (!task->parm.handler) |
| 169 | { |
| 170 | bcmos_sem_post(&task->active_sem); |
| 171 | } |
| 172 | rc_cancel = pthread_cancel(task->sys_task.t); |
| 173 | rc = pthread_join(task->sys_task.t, &res); |
| 174 | return (rc || ((!rc_cancel) && (res != PTHREAD_CANCELED))) ? BCM_ERR_SYSCALL_ERR : 0; |
| 175 | } |
| 176 | |
| 177 | /** Get current task |
| 178 | * \returns task handle or NULL if not in task context |
| 179 | */ |
| 180 | bcmos_task *bcmos_task_current(void) |
| 181 | { |
| 182 | pthread_t pt = pthread_self(); |
| 183 | bcmos_task *t, *tmp; |
| 184 | |
| 185 | STAILQ_FOREACH_SAFE(t, &task_list, list, tmp) |
| 186 | { |
| 187 | if (pthread_equal(pt, t->sys_task.t)) |
| 188 | break; |
| 189 | } |
| 190 | return t; |
| 191 | } |
| 192 | |
| 193 | /* |
| 194 | * Recursive mutex |
| 195 | */ |
| 196 | |
| 197 | /* Create recursive mutex */ |
| 198 | bcmos_errno bcmos_mutex_create(bcmos_mutex *mutex, uint32_t flags, const char *name) |
| 199 | { |
| 200 | int err; |
| 201 | err = pthread_mutexattr_init(&mutex->attr); |
| 202 | err = err ? err : pthread_mutexattr_settype(&mutex->attr, PTHREAD_MUTEX_RECURSIVE_NP); |
| 203 | err = err ? err : pthread_mutex_init(&mutex->m, &mutex->attr); |
| 204 | if (err) |
| 205 | BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "errno=%s\n", strerror(err)); |
| 206 | return BCM_ERR_OK; |
| 207 | } |
| 208 | |
| 209 | /** Destroy mutex |
| 210 | * \param[in] mutex Mutex control block |
| 211 | */ |
| 212 | void bcmos_mutex_destroy(bcmos_mutex *mutex) |
| 213 | { |
| 214 | pthread_mutex_destroy(&mutex->m); |
| 215 | pthread_mutexattr_destroy(&mutex->attr); |
| 216 | } |
| 217 | |
| 218 | /* calculate absolute time equal to the current time + timeout */ |
| 219 | static inline void _bcmos_get_abs_time(struct timespec *ts, uint32_t timeout) |
| 220 | { |
| 221 | int rc; |
| 222 | rc = clock_gettime(CLOCK_REALTIME, ts); |
| 223 | BUG_ON(rc); |
| 224 | ts->tv_sec += timeout / 1000000; |
| 225 | ts->tv_nsec += (timeout % 1000000) * 1000; |
| 226 | if (ts->tv_nsec > 1000000000) |
| 227 | { |
| 228 | ts->tv_sec += ts->tv_nsec / 1000000000; |
| 229 | ts->tv_nsec %= 1000000000; |
| 230 | } |
| 231 | } |
| 232 | |
| 233 | /** Lock mutex |
| 234 | */ |
| 235 | void bcmos_mutex_lock(bcmos_mutex *mutex) |
| 236 | { |
| 237 | int rc; |
| 238 | rc = pthread_mutex_lock(&mutex->m); |
| 239 | BUG_ON(rc); |
| 240 | return; |
| 241 | } |
| 242 | |
| 243 | /** Release mutex |
| 244 | * \param[in] mutex Mutex control block |
| 245 | */ |
| 246 | void bcmos_mutex_unlock(bcmos_mutex *mutex) |
| 247 | { |
| 248 | int rc; |
| 249 | rc = pthread_mutex_unlock(&mutex->m); |
| 250 | if (rc) |
| 251 | BCMOS_TRACE_ERR("pthread_mutex_unlock()->%d\n", rc); |
| 252 | BUG_ON(rc); |
| 253 | } |
| 254 | |
| 255 | /* |
| 256 | * Semaphores |
| 257 | * Most of semaphore functions are inline |
| 258 | */ |
| 259 | |
| 260 | /* Create semaphore */ |
| 261 | bcmos_errno bcmos_sem_create(bcmos_sem *sem, uint32_t count, uint32_t flags, const char *name) |
| 262 | { |
| 263 | sem_init(&sem->s, 0, count); |
| 264 | return BCM_ERR_OK; |
| 265 | } |
| 266 | |
| 267 | /* Destroy semaphore */ |
| 268 | void bcmos_sem_destroy(bcmos_sem *sem) |
| 269 | { |
| 270 | sem_destroy(&sem->s); |
| 271 | } |
| 272 | |
| 273 | /* Decrement semaphore counter. Wait if the counter is 0 */ |
| 274 | bcmos_errno bcmos_sem_wait(bcmos_sem *sem, uint32_t timeout) |
| 275 | { |
| 276 | int rc; |
| 277 | struct timespec ts; |
| 278 | |
| 279 | /* Init end time if necessary */ |
| 280 | if (timeout && timeout != BCMOS_WAIT_FOREVER) |
| 281 | { |
| 282 | _bcmos_get_abs_time(&ts, timeout); |
| 283 | } |
| 284 | |
| 285 | do |
| 286 | { |
| 287 | if (timeout == BCMOS_WAIT_FOREVER) |
| 288 | { |
| 289 | rc = sem_wait(&sem->s); |
| 290 | } |
| 291 | else if (timeout) |
| 292 | { |
| 293 | rc = sem_timedwait(&sem->s, &ts); |
| 294 | } |
| 295 | else |
| 296 | { |
| 297 | rc = sem_trywait(&sem->s); |
| 298 | } |
| 299 | } while (rc && errno == EINTR); |
| 300 | |
| 301 | if (rc) |
| 302 | { |
| 303 | bcmos_errno err; |
| 304 | |
| 305 | rc = errno; |
| 306 | if (rc == ETIMEDOUT) |
| 307 | { |
| 308 | err = BCM_ERR_TIMEOUT; |
| 309 | } |
| 310 | else |
| 311 | { |
| 312 | err = BCM_ERR_INTERNAL; |
| 313 | BCMOS_TRACE_ERR("sem_wait()->%d\n", rc); |
| 314 | } |
| 315 | return err; |
| 316 | } |
| 317 | |
| 318 | return BCM_ERR_OK; |
| 319 | } |
| 320 | |
| 321 | |
| 322 | /* |
| 323 | * Timers |
| 324 | */ |
| 325 | |
| 326 | /** Get current timestamp |
| 327 | * \returns the current system timestamp (us) |
| 328 | */ |
| 329 | uint32_t bcmos_timestamp(void) |
| 330 | { |
| 331 | struct timespec tp; |
| 332 | clock_gettime(CLOCK_MONOTONIC, &tp); |
| 333 | return (tp.tv_sec * 1000000 + tp.tv_nsec / 1000); |
| 334 | } |
| 335 | |
| 336 | /** Get current timestamp - 64 bit |
| 337 | * \returns the current system timestamp (us) |
| 338 | */ |
| 339 | uint64_t bcmos_timestamp64(void) |
| 340 | { |
| 341 | struct timespec tp; |
| 342 | clock_gettime(CLOCK_MONOTONIC, &tp); |
| 343 | return ((uint64_t)tp.tv_sec * 1000000LL + (uint64_t)tp.tv_nsec / 1000LL); |
| 344 | } |
| 345 | |
| 346 | /* |
| 347 | * Timer handlers |
| 348 | */ |
| 349 | |
| 350 | |
| 351 | |
| 352 | /* For posix we must create timer task, because there is no way |
| 353 | * to enforce protection between a signal handler and a thread |
| 354 | */ |
| 355 | static bcmos_task _bcmos_timer_task; |
| 356 | static sem_t _bcmos_timer_lock; |
| 357 | |
| 358 | static int _bcmos_tmr_task_handler(long data) |
| 359 | { |
| 360 | bcmos_sys_timer *timer = (bcmos_sys_timer *)data; |
| 361 | bcmos_task *this_task = bcmos_task_current(); |
| 362 | |
| 363 | while(!this_task->destroy_request) |
| 364 | { |
| 365 | sem_wait(&_bcmos_timer_lock); |
| 366 | timer->handler(timer->data); |
| 367 | } |
| 368 | this_task->destroyed = BCMOS_TRUE; |
| 369 | |
| 370 | return 0; |
| 371 | } |
| 372 | |
| 373 | /* timer signal handler */ |
| 374 | static void timer_sig_handler(int sig, siginfo_t *si, void *uc) |
| 375 | { |
| 376 | BUG_ON(si->si_code != SI_TIMER); |
| 377 | sem_post(&_bcmos_timer_lock); |
| 378 | } |
| 379 | |
| 380 | /* Create timer */ |
| 381 | bcmos_errno bcmos_sys_timer_create(bcmos_sys_timer *timer, bcmos_sys_timer_handler handler, void *data) |
| 382 | { |
| 383 | static bcmos_task_parm tmr_task_parm = { |
| 384 | .name = "tmr_task", |
| 385 | .priority = BCMOS_TASK_PRIORITY_0, |
| 386 | .handler = _bcmos_tmr_task_handler |
| 387 | }; |
| 388 | bcmos_errno rc; |
| 389 | |
| 390 | struct sigaction sa; |
| 391 | struct sigevent sev = {}; |
| 392 | |
| 393 | if (!timer || !handler) |
| 394 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "timer %p, handler %p\n", timer, handler); |
| 395 | |
| 396 | timer->handler = handler; |
| 397 | timer->data = data; |
| 398 | |
| 399 | sem_init(&_bcmos_timer_lock, 0, 0); |
| 400 | tmr_task_parm.data = (long)timer; |
| 401 | rc = bcmos_task_create(&_bcmos_timer_task, &tmr_task_parm); |
| 402 | if (rc) |
| 403 | BCMOS_TRACE_RETURN(rc, "Can't create timer task\n"); |
| 404 | |
| 405 | sa.sa_flags = SA_SIGINFO; |
| 406 | sa.sa_sigaction = timer_sig_handler; |
| 407 | sigemptyset(&sa.sa_mask); |
| 408 | if (sigaction(TIMER_SIG, &sa, NULL) == -1) |
| 409 | perror("sigaction"); |
| 410 | /* Prevent timer signal from interrupting system calls */ |
| 411 | if (siginterrupt(TIMER_SIG, 0) == -1) |
| 412 | perror("siginterrupt"); |
| 413 | |
| 414 | /* Create librt timer */ |
| 415 | sev.sigev_notify = SIGEV_SIGNAL; |
| 416 | sev.sigev_signo = TIMER_SIG; |
| 417 | sev.sigev_value.sival_ptr = timer; |
| 418 | if (timer_create(CLOCK_REALTIME, &sev, &timer->t) == -1) |
| 419 | BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "errno %s\n", strerror(errno)); |
| 420 | |
| 421 | return BCM_ERR_OK; |
| 422 | } |
| 423 | |
| 424 | /* Destroy timer */ |
| 425 | void bcmos_sys_timer_destroy(bcmos_sys_timer *timer) |
| 426 | { |
| 427 | timer_delete(timer->t); |
| 428 | sem_destroy(&_bcmos_timer_lock); |
| 429 | bcmos_task_destroy(&_bcmos_timer_task); |
| 430 | } |
| 431 | |
| 432 | /* (Re)start timer */ |
| 433 | void bcmos_sys_timer_start(bcmos_sys_timer *timer, uint32_t delay) |
| 434 | { |
| 435 | struct itimerspec its; |
| 436 | |
| 437 | its.it_value.tv_sec = delay / 1000000; |
| 438 | its.it_value.tv_nsec = (delay % 1000000) * 1000; |
| 439 | its.it_interval.tv_sec = 0; |
| 440 | its.it_interval.tv_nsec = 0; |
| 441 | if (timer_settime(timer->t, 0, &its, NULL) == -1) |
| 442 | { |
| 443 | BCMOS_TRACE_ERR("timer_settime errno %s\n", strerror(errno)); |
| 444 | BUG(); |
| 445 | } |
| 446 | } |
| 447 | |
| 448 | /* Stop timer if running */ |
| 449 | void bcmos_sys_timer_stop(bcmos_sys_timer *timer) |
| 450 | { |
| 451 | struct itimerspec its; |
| 452 | |
| 453 | BUG_ON(!timer); |
| 454 | memset(&its, 0, sizeof(its)); |
| 455 | timer_settime(timer->t, 0, &its, NULL); |
| 456 | } |
| 457 | |
| 458 | |
| 459 | /* Suspend current task for a time */ |
| 460 | void bcmos_usleep(uint32_t us) |
| 461 | { |
| 462 | uint32_t ts = bcmos_timestamp(); |
| 463 | uint32_t tse = ts + us; |
| 464 | int32_t d = (us > 1000000) ? 1000000 : us; |
| 465 | |
| 466 | do |
| 467 | { |
| 468 | usleep(d); |
| 469 | d = tse - bcmos_timestamp(); |
| 470 | if (d > 1000000) d = 1000000; |
| 471 | } while (d > 0); |
| 472 | } |
| 473 | |
| 474 | /* |
| 475 | * Memory management |
| 476 | */ |
| 477 | |
| 478 | /* Allocate memory from the main heap */ |
| 479 | void *bcmos_alloc(uint32_t size) |
| 480 | { |
| 481 | return malloc(size); |
| 482 | } |
| 483 | |
| 484 | /* Release memory to the main pool */ |
| 485 | void bcmos_free(void *ptr) |
| 486 | { |
| 487 | free(ptr); |
| 488 | } |
| 489 | |
| 490 | /* |
| 491 | * Byte memory pool |
| 492 | */ |
| 493 | |
| 494 | /* Memory block header */ |
| 495 | typedef struct bcmos_byte_memblk |
| 496 | { |
| 497 | bcmos_byte_pool *pool; /** pool that owns the block */ |
| 498 | uint32_t size; /** block size (bytes) including bcmos_byte_memblk header */ |
| 499 | #ifdef BCMOS_MEM_DEBUG |
| 500 | uint32_t magic; /** magic number */ |
| 501 | #define BCMOS_MEM_MAGIC_ALLOC (('m'<<24) | ('b' << 16) | ('l' << 8) | 'k') |
| 502 | #define BCMOS_MEM_MAGIC_FREE (('m'<<24) | ('b' << 16) | ('l' << 8) | '~') |
| 503 | #endif |
| 504 | } bcmos_byte_memblk; |
| 505 | |
| 506 | /* Create byte memory pool */ |
| 507 | bcmos_errno bcmos_byte_pool_create(bcmos_byte_pool *pool, const bcmos_byte_pool_parm *parm) |
| 508 | { |
| 509 | if (!pool || !parm) |
| 510 | { |
| 511 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "pool %p, parm %p\n", pool, parm); |
| 512 | } |
| 513 | |
| 514 | BCM_MEMZERO_STRUCT(pool); |
| 515 | pool->parm = *parm; |
| 516 | if (!pool->parm.size) |
| 517 | { |
| 518 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "size %u\n", parm->size); |
| 519 | } |
| 520 | #ifdef BCMOS_MEM_DEBUG |
| 521 | pool->magic = BCMOS_BYTE_POOL_VALID; |
| 522 | #endif |
| 523 | return BCM_ERR_OK; |
| 524 | } |
| 525 | |
| 526 | /* Destroy memory pool */ |
| 527 | bcmos_errno bcmos_byte_pool_destroy(bcmos_byte_pool *pool) |
| 528 | { |
| 529 | if (pool->allocated) |
| 530 | { |
| 531 | BCMOS_TRACE_RETURN(BCM_ERR_STATE, "%u bytes of memory are still allocated from the pool %s\n", |
| 532 | pool->allocated, pool->parm.name); |
| 533 | } |
| 534 | #ifdef BCMOS_MEM_DEBUG |
| 535 | pool->magic = BCMOS_BYTE_POOL_DELETED; |
| 536 | #endif |
| 537 | return BCM_ERR_OK; |
| 538 | } |
| 539 | |
| 540 | /* Allocate memory from memory pool */ |
| 541 | void *bcmos_byte_pool_alloc(bcmos_byte_pool *pool, uint32_t size) |
| 542 | { |
| 543 | bcmos_byte_memblk *blk; |
| 544 | uint32_t byte_size; |
| 545 | void *ptr; |
| 546 | |
| 547 | #ifdef BCMOS_MEM_DEBUG |
| 548 | BUG_ON(pool->magic != BCMOS_BYTE_POOL_VALID); |
| 549 | #endif |
| 550 | |
| 551 | if (size + pool->allocated > pool->parm.size) |
| 552 | return NULL; |
| 553 | |
| 554 | byte_size = size + sizeof(bcmos_byte_memblk); |
| 555 | #ifdef BCMOS_MEM_DEBUG |
| 556 | byte_size += sizeof(uint32_t); /* block suffix */ |
| 557 | #endif |
| 558 | /* ToDo: Maintain LL of allocated blocks */ |
| 559 | blk = (bcmos_byte_memblk *)malloc(byte_size); |
| 560 | if (!blk) |
| 561 | return NULL; |
| 562 | ptr = (void *)(blk + 1); |
| 563 | blk->size = byte_size; |
| 564 | pool->allocated += byte_size; |
| 565 | blk->pool = pool; |
| 566 | #ifdef BCMOS_MEM_DEBUG |
| 567 | blk->magic = BCMOS_MEM_MAGIC_ALLOC; |
| 568 | *(uint32_t *)((long)blk + byte_size - sizeof(uint32_t)) = BCMOS_MEM_MAGIC_ALLOC; |
| 569 | #endif |
| 570 | |
| 571 | return ptr; |
| 572 | } |
| 573 | |
| 574 | /* Release memory allocated using bcmos_byte_pool_alloc() */ |
| 575 | void bcmos_byte_pool_free(void *ptr) |
| 576 | { |
| 577 | bcmos_byte_memblk *blk; |
| 578 | bcmos_byte_pool *pool; |
| 579 | |
| 580 | BUG_ON(!ptr); |
| 581 | blk = (bcmos_byte_memblk *)((long)ptr - sizeof(bcmos_byte_memblk)); |
| 582 | pool = blk->pool; |
| 583 | #ifdef BCMOS_MEM_DEBUG |
| 584 | BUG_ON(pool->magic != BCMOS_BYTE_POOL_VALID); |
| 585 | BUG_ON(blk->magic != BCMOS_MEM_MAGIC_ALLOC); |
| 586 | BUG_ON(*(uint32_t *)((long)blk + blk->size - sizeof(uint32_t)) != BCMOS_MEM_MAGIC_ALLOC); |
| 587 | blk->magic = BCMOS_MEM_MAGIC_FREE; |
| 588 | #endif |
| 589 | pool->allocated -= blk->size; |
| 590 | free(blk); |
| 591 | } |
| 592 | |
| 593 | void _bcmos_backtrace(void) |
| 594 | { |
| 595 | void *array[32]; |
| 596 | size_t size; |
| 597 | char **strings; |
| 598 | size_t i; |
| 599 | |
| 600 | size = backtrace(array, sizeof(array)/sizeof(array[0])); |
| 601 | strings = backtrace_symbols(array, size); |
| 602 | |
| 603 | printf("Obtained %zd stack frames.\n", size); |
| 604 | |
| 605 | for (i = 0; i < size; i++) |
| 606 | printf("%s\n", strings[i]); |
| 607 | |
| 608 | free(strings); |
| 609 | } |
| 610 | |
| 611 | /* stub for int enable */ |
| 612 | void bcmos_int_enable(int irq) |
| 613 | { |
| 614 | } |
| 615 | |
| 616 | void bcmos_int_disable(int irq) |
| 617 | { |
| 618 | } |
| 619 | |
| 620 | /* stub for int connect */ |
| 621 | int bcmos_int_connect(int irq, int cpu, int flags, |
| 622 | int (*isr)(int irq, void *priv), const char *name, void *priv) |
| 623 | { |
| 624 | return 0; |
| 625 | } |
| 626 | |
| 627 | /* Functions common for domain socket and UDP socket - based message queues */ |
| 628 | #if defined(BCMOS_MSG_QUEUE_DOMAIN_SOCKET) || defined(BCMOS_MSG_QUEUE_UDP_SOCKET) |
| 629 | |
| 630 | /** "send" to socket */ |
| 631 | static bcmos_errno _bcmos_socket_send(bcmos_msg_queue *queue, uint8_t *buf, uint32_t buf_length) |
| 632 | { |
| 633 | int rc; |
| 634 | /* Message queue without remote endpoint address doesn't support transmit */ |
| 635 | if (!queue->ep_extra_data_size) |
| 636 | return BCM_ERR_NOT_SUPPORTED; |
| 637 | rc = sendto(queue->ep, buf, buf_length, 0, |
| 638 | (struct sockaddr *)queue->ep_extra_data, queue->ep_extra_data_size); |
| 639 | return (rc == buf_length) ? BCM_ERR_OK : BCM_ERR_COMM_FAIL; |
| 640 | } |
| 641 | |
| 642 | /** "recv" from socket */ |
| 643 | static bcmos_errno _bcmos_socket_recv(bcmos_msg_queue *queue, uint32_t timeout, uint8_t **buf, uint32_t *buf_length) |
| 644 | { |
| 645 | int rc; |
| 646 | int wait = 0; |
| 647 | |
| 648 | /* Message queue without local endpoint address doesn't support receive */ |
| 649 | if (!queue->q.parm.local_ep_address) |
| 650 | return BCM_ERR_NOT_SUPPORTED; |
| 651 | |
| 652 | if (timeout && timeout != BCMOS_WAIT_FOREVER) |
| 653 | { |
| 654 | fd_set read_fds; |
| 655 | struct timeval tv; |
| 656 | |
| 657 | FD_ZERO(&read_fds); |
| 658 | FD_SET(queue->ep, &read_fds); |
| 659 | tv.tv_sec = timeout / 1000000; |
| 660 | tv.tv_usec = (timeout % 1000000) * 1000; |
| 661 | rc = select(queue->ep + 1, &read_fds, NULL, NULL, &tv); |
| 662 | if (rc < 0) |
| 663 | { |
| 664 | return BCM_ERR_COMM_FAIL; |
| 665 | } |
| 666 | if (!rc || !FD_ISSET(queue->ep, &read_fds)) |
| 667 | return BCM_ERR_TIMEOUT; |
| 668 | wait = MSG_DONTWAIT; |
| 669 | } |
| 670 | |
| 671 | rc = recv(queue->ep, queue->recv_buf, queue->q.parm.max_mtu, wait); |
| 672 | if (rc < 0) |
| 673 | { |
| 674 | return BCM_ERR_COMM_FAIL; |
| 675 | } |
| 676 | if (rc == 0) |
| 677 | return BCM_ERR_NOENT; |
| 678 | |
| 679 | *buf = queue->recv_buf; |
| 680 | *buf_length = rc; |
| 681 | return BCM_ERR_OK; |
| 682 | } |
| 683 | |
| 684 | |
| 685 | static bcmos_errno _bcmos_socket_close(bcmos_msg_queue *queue) |
| 686 | { |
| 687 | /* Close domain socket */ |
| 688 | if (queue->ep > 0) |
| 689 | { |
| 690 | close(queue->ep); |
| 691 | } |
| 692 | if (queue->ep_extra_data) |
| 693 | { |
| 694 | bcmos_free(queue->ep_extra_data); |
| 695 | } |
| 696 | return BCM_ERR_OK; |
| 697 | } |
| 698 | |
| 699 | |
| 700 | /* Pack message for over-the-socket transmission. |
| 701 | * This function is good for case when both queue ends are on the same CPU |
| 702 | * and there is no need to do any translation. |
| 703 | */ |
| 704 | static bcmos_errno _bcmos_transparent_pack(bcmos_msg_queue *queue, bcmos_msg *msg, uint8_t **buf, uint32_t *buf_length) |
| 705 | { |
| 706 | uint32_t size = BCMOS_MSG_HDR_SIZE + msg->size; |
| 707 | |
| 708 | if (size > queue->q.parm.max_mtu) |
| 709 | { |
| 710 | BCMOS_TRACE_RETURN(BCM_ERR_OVERFLOW, "Attempt to send message longer than configured max_mtu %u > %u\n", |
| 711 | size, queue->q.parm.max_mtu); |
| 712 | } |
| 713 | bcmos_msg_hdr_pack(msg, queue->send_buf, msg->size); |
| 714 | if (msg->size) |
| 715 | { |
| 716 | BUG_ON(msg->data == NULL); |
| 717 | memcpy(queue->send_buf + BCMOS_MSG_HDR_SIZE, msg->data, msg->size); |
| 718 | } |
| 719 | *buf = queue->send_buf; |
| 720 | *buf_length = size; |
| 721 | return BCM_ERR_OK; |
| 722 | } |
| 723 | |
| 724 | /** "unpack" message. In case of domain socket both queue ends are in the same CPU. |
| 725 | * Message is sent as-is |
| 726 | */ |
| 727 | static bcmos_errno _bcmos_transparent_unpack(bcmos_msg_queue *queue, uint8_t *buf, uint32_t buf_length, bcmos_msg **msg) |
| 728 | { |
| 729 | bcmos_msg *m; |
| 730 | |
| 731 | if (buf_length < BCMOS_MSG_HDR_SIZE) |
| 732 | { |
| 733 | BCMOS_TRACE_RETURN(BCM_ERR_INTERNAL, "Received message is too short (%u)\n", buf_length); |
| 734 | } |
| 735 | |
| 736 | /* Adjust buf_length to account for difference in packed and unpacked message header sizes */ |
| 737 | buf_length -= BCMOS_MSG_HDR_SIZE; |
| 738 | buf_length += sizeof(bcmos_msg); |
| 739 | |
| 740 | m = bcmos_alloc(buf_length); |
| 741 | if (!m) |
| 742 | { |
| 743 | BCMOS_TRACE_RETURN(BCM_ERR_NOMEM, "Received message discarded: %u bytes\n", buf_length); |
| 744 | } |
| 745 | |
| 746 | bcmos_msg_hdr_unpack(buf, m); |
| 747 | m->release = NULL; |
| 748 | m->data_release = NULL; |
| 749 | if (m->size != buf_length - sizeof(bcmos_msg)) |
| 750 | { |
| 751 | BCMOS_TRACE_ERR("Received message is insane. Expected data length %u, got %u\n", |
| 752 | m->size, buf_length - sizeof(bcmos_msg)); |
| 753 | bcmos_free(m); |
| 754 | return BCM_ERR_INTERNAL; |
| 755 | } |
| 756 | if (m->size) |
| 757 | { |
| 758 | m->data = m->start = (void *)(m + 1); |
| 759 | memcpy(m->data, &buf[BCMOS_MSG_HDR_SIZE], m->size); |
| 760 | } |
| 761 | else |
| 762 | { |
| 763 | m->data = m->start = NULL; |
| 764 | } |
| 765 | *msg = m; |
| 766 | |
| 767 | return BCM_ERR_OK; |
| 768 | } |
| 769 | |
| 770 | |
| 771 | #endif |
| 772 | |
| 773 | /* Domain-socket-based inter-process communication */ |
| 774 | #ifdef BCMOS_MSG_QUEUE_DOMAIN_SOCKET |
| 775 | |
| 776 | bcmos_errno bcmos_sys_msg_queue_domain_socket_open(bcmos_msg_queue *queue) |
| 777 | { |
| 778 | struct sockaddr_un sa; |
| 779 | |
| 780 | /* Open domain socket */ |
| 781 | queue->ep = socket(AF_UNIX, SOCK_DGRAM, 0); |
| 782 | if (queue->ep < 0) |
| 783 | { |
| 784 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't create domain socket. error %s\n", strerror(errno)); |
| 785 | } |
| 786 | |
| 787 | memset(&sa, 0, sizeof(sa)); |
| 788 | sa.sun_family = AF_UNIX; |
| 789 | |
| 790 | /* If local_ep_address is set - the queue supports receive */ |
| 791 | if (queue->q.parm.local_ep_address) |
| 792 | { |
| 793 | strncpy(sa.sun_path, queue->q.parm.local_ep_address, sizeof(sa.sun_path) - 1); |
| 794 | /* In linux path can start from 0 character */ |
| 795 | if (!sa.sun_path[0]) |
| 796 | { |
| 797 | strncpy(&sa.sun_path[1], &queue->q.parm.local_ep_address[1], sizeof(sa.sun_path) - 1); |
| 798 | } |
| 799 | if (bind(queue->ep, (struct sockaddr *)&sa, sizeof(sa)) < 0) |
| 800 | { |
| 801 | int err = errno; |
| 802 | close(queue->ep); |
| 803 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't bind domain socket to %s. error %s\n", |
| 804 | queue->q.parm.local_ep_address, strerror(err)); |
| 805 | } |
| 806 | } |
| 807 | |
| 808 | /* If remote_ep_address is set - the queue supports transmit */ |
| 809 | if (queue->q.parm.remote_ep_address) |
| 810 | { |
| 811 | queue->ep_extra_data = bcmos_calloc(sizeof(struct sockaddr_un)); |
| 812 | if (!queue->ep_extra_data) |
| 813 | { |
| 814 | close(queue->ep); |
| 815 | return BCM_ERR_NOMEM; |
| 816 | } |
| 817 | strncpy(sa.sun_path, queue->q.parm.remote_ep_address, sizeof(sa.sun_path) - 1); |
| 818 | /* In linux path can start from 0 character */ |
| 819 | if (!sa.sun_path[0]) |
| 820 | { |
| 821 | strncpy(&sa.sun_path[1], &queue->q.parm.remote_ep_address[1], sizeof(sa.sun_path) - 1); |
| 822 | } |
| 823 | memcpy(queue->ep_extra_data, &sa, sizeof(sa)); |
| 824 | queue->ep_extra_data_size = sizeof(sa); |
| 825 | } |
| 826 | |
| 827 | /* Set callbacks */ |
| 828 | queue->q.parm.close = _bcmos_socket_close; |
| 829 | queue->q.parm.send = _bcmos_socket_send; |
| 830 | queue->q.parm.recv = _bcmos_socket_recv; |
| 831 | if (!queue->q.parm.pack) |
| 832 | queue->q.parm.pack = _bcmos_transparent_pack; |
| 833 | if (!queue->q.parm.unpack) |
| 834 | queue->q.parm.unpack = _bcmos_transparent_unpack; |
| 835 | |
| 836 | return BCM_ERR_OK; |
| 837 | } |
| 838 | |
| 839 | #endif |
| 840 | |
| 841 | /* UDP-socket-based inter-process communication */ |
| 842 | #ifdef BCMOS_MSG_QUEUE_UDP_SOCKET |
| 843 | |
| 844 | static bcmos_errno _bcmos_parse_ip_port(const char *s, struct sockaddr_in *sa) |
| 845 | { |
| 846 | uint32_t ip; |
| 847 | int n; |
| 848 | uint32_t ip1, ip2, ip3, ip4, port; |
| 849 | |
| 850 | n = sscanf(s, "%u.%u.%u.%u:%u", &ip1, &ip2, &ip3, &ip4, &port); |
| 851 | if (n != 5 || ip1 > 0xff || ip2 > 0xff || ip3 > 0xff || ip4 > 0xff || port > 0xffff) |
| 852 | { |
| 853 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't parse %s. Must be ip_address:port\n", s); |
| 854 | } |
| 855 | ip = (ip1 << 24) | (ip2 << 16) | (ip3 << 8) | ip4; |
| 856 | sa->sin_port = htons(port); |
| 857 | sa->sin_addr.s_addr = htonl(ip); |
| 858 | |
| 859 | return BCM_ERR_OK; |
| 860 | } |
| 861 | |
| 862 | bcmos_errno bcmos_sys_msg_queue_udp_socket_open(bcmos_msg_queue *queue) |
| 863 | { |
| 864 | struct sockaddr_in sa; |
| 865 | bcmos_errno rc; |
| 866 | |
| 867 | /* Open UDP socket */ |
| 868 | queue->ep = socket(AF_INET, SOCK_DGRAM, 0); |
| 869 | if (queue->ep < 0) |
| 870 | { |
| 871 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't create UDP socket. error %s\n", strerror(errno)); |
| 872 | } |
| 873 | |
| 874 | memset(&sa, 0, sizeof(sa)); |
| 875 | sa.sin_family = AF_INET; |
| 876 | |
| 877 | /* If local_ep_address is set - the queue supports receive */ |
| 878 | if (queue->q.parm.local_ep_address) |
| 879 | { |
| 880 | rc = _bcmos_parse_ip_port(queue->q.parm.local_ep_address, &sa); |
| 881 | if (rc) |
| 882 | return rc; |
| 883 | if (!sa.sin_addr.s_addr) |
| 884 | sa.sin_addr.s_addr = INADDR_ANY; |
| 885 | if (bind(queue->ep, (struct sockaddr *)&sa, sizeof(sa)) < 0) |
| 886 | { |
| 887 | int err = errno; |
| 888 | close(queue->ep); |
| 889 | BCMOS_TRACE_RETURN(BCM_ERR_PARM, "Can't bind UDP socket to %s. error %s\n", |
| 890 | queue->q.parm.local_ep_address, strerror(err)); |
| 891 | } |
| 892 | } |
| 893 | |
| 894 | /* If remote_ep_address is set - the queue supports transmit */ |
| 895 | if (queue->q.parm.remote_ep_address) |
| 896 | { |
| 897 | rc = _bcmos_parse_ip_port(queue->q.parm.remote_ep_address, &sa); |
| 898 | if (rc) |
| 899 | return rc; |
| 900 | queue->ep_extra_data = bcmos_calloc(sizeof(sa)); |
| 901 | if (!queue->ep_extra_data) |
| 902 | { |
| 903 | close(queue->ep); |
| 904 | return BCM_ERR_NOMEM; |
| 905 | } |
| 906 | memcpy(queue->ep_extra_data, &sa, sizeof(sa)); |
| 907 | queue->ep_extra_data_size = sizeof(sa); |
| 908 | } |
| 909 | |
| 910 | /* Set callbacks */ |
| 911 | queue->q.parm.close = _bcmos_socket_close; |
| 912 | queue->q.parm.send = _bcmos_socket_send; |
| 913 | queue->q.parm.recv = _bcmos_socket_recv; |
| 914 | if (!queue->q.parm.pack) |
| 915 | queue->q.parm.pack = _bcmos_transparent_pack; |
| 916 | if (!queue->q.parm.unpack) |
| 917 | queue->q.parm.unpack = _bcmos_transparent_unpack; |
| 918 | |
| 919 | return BCM_ERR_OK; |
| 920 | } |
| 921 | #endif |