sndbuf_t * sndbuf_alloc(void) { struct sndbuf_blk * blk; uint32_t primask; /* critical section enter */ primask = cm3_primask_get(); cm3_primask_set(1); if ((blk = sndbuf_pool.free) != NULL) { sndbuf_pool.free = blk->next; blk->ref = 1; sndbuf_pool.alloc_cnt++; DCC_LOG1(LOG_INFO, "buf=0x%08x", blk); } else { sndbuf_pool.error++; DCC_LOG2(LOG_ERROR, "failed!, allocs=%d frees=%d", sndbuf_pool.alloc_cnt, sndbuf_pool.free_cnt); DBG("failed!, allocs=%d frees=%d", sndbuf_pool.alloc_cnt, sndbuf_pool.free_cnt); } /* critical section exit */ cm3_primask_set(primask); return (sndbuf_t *)blk; }
sndbuf_t * sndbuf_use(sndbuf_t * buf) { uint32_t primask; /* critical section enter */ primask = cm3_primask_get(); cm3_primask_set(1); /* check whether the buffer is valid or not */ if (buf == NULL) { DCC_LOG(LOG_PANIC, "NULL pointer!"); } else { if (buf->ref == 0) { DCC_LOG1(LOG_WARNING, "buf=0x%08x invalid!", buf); buf = NULL; } else { DCC_LOG1(LOG_INFO, "buf=%d", buf - (sndbuf_t *)sndbuf_pool.blk); buf->ref++; } } /* critical section exit */ cm3_primask_set(primask); return buf; }
void sndbuf_free(sndbuf_t * buf) { struct sndbuf_blk * blk = (struct sndbuf_blk *)buf; uint32_t primask; /* critical section enter */ primask = cm3_primask_get(); cm3_primask_set(1); if (buf == NULL) { DCC_LOG(LOG_PANIC, "NULL pointer!"); } else { /* decrement reference counter */ if (buf->ref == 0) { DCC_LOG1(LOG_WARNING, "buf=0x%08x invalid!", buf); } else { DCC_LOG2(LOG_INFO, "buf=%d ref=%d", buf - (sndbuf_t *)sndbuf_pool.blk, buf->ref); if (--buf->ref == 0) { blk->next = sndbuf_pool.free; sndbuf_pool.free = blk; sndbuf_pool.free_cnt++; } } } /* critical section exit */ cm3_primask_set(primask); }
int stm32_flash_erase(unsigned int offs, unsigned int len) { struct stm32_flash * flash = STM32_FLASH; uint32_t addr; uint32_t pecr; int rem = len; int cnt; offs &= ~(FLASH_PAGE_SIZE - 1); addr = (uint32_t)STM32_MEM_FLASH + offs; DCC_LOG2(LOG_INFO, "addr=0x%08x len=%d", addr, len); pecr = flash->pecr; DCC_LOG1(LOG_INFO, "PECR=0x%08x", pecr); if (pecr & FLASH_PRGLOCK) { DCC_LOG(LOG_INFO, "unlocking flash..."); if (pecr & FLASH_PELOCK) { flash->pekeyr = FLASH_PEKEY1; flash->pekeyr = FLASH_PEKEY2; } flash->prgkeyr= FLASH_PRGKEYR1; flash->prgkeyr= FLASH_PRGKEYR2; } cnt = 0; rem = len; while (rem) { uint32_t pri; uint32_t sr; DCC_LOG1(LOG_INFO, "addr=0x%08x", addr); pri = cm3_primask_get(); cm3_primask_set(1); sr = stm32l_flash_blk_erase(flash, (uint32_t *)addr); cm3_primask_set(pri); if (sr & FLASH_ERR) { #if DEBUG DCC_LOG6(LOG_WARNING, "erase failed: %s%s%s%s%s%s", sr & FLASH_RDERR ? "RDERR" : "", sr & FLASH_OPTVERRUSR ? "OPTVERRUSR" : "", sr & FLASH_OPTVERR ? "OPTVERR " : "", sr & FLASH_SIZERR ? "SIZERR " : "", sr & FLASH_PGAERR ? "PGAERR" : "", sr & FLASH_WRPERR ? "WRPERR" : ""); #endif cnt = -1; break; } addr += FLASH_PAGE_SIZE; rem -= FLASH_PAGE_SIZE; cnt += FLASH_PAGE_SIZE; } return cnt; }
int stm32_flash_write(uint32_t offs, const void * buf, unsigned int len) { struct stm32_flash * flash = STM32_FLASH; uint32_t data; uint32_t * addr; uint8_t * ptr; uint32_t cr; uint32_t sr; uint32_t pri; int n; int i; if (offs & 0x00000003) { DCC_LOG(LOG_ERROR, "offset must be 32bits aligned!"); return -1; } n = (len + 3) / 4; ptr = (uint8_t *)buf; addr = (uint32_t *)((uint32_t)STM32_FLASH_MEM + offs); cr = flash->cr; if (cr & FLASH_LOCK) { DCC_LOG(LOG_TRACE, "unlocking flash..."); /* unlock flash write */ flash->keyr = FLASH_KEY1; flash->keyr = FLASH_KEY2; } DCC_LOG2(LOG_INFO, "0x%08x len=%d", addr, len); /* Clear errors */ flash->sr = FLASH_ERR; pri = cm3_primask_get(); for (i = 0; i < n; i++) { data = ptr[0] | (ptr[1] << 8) | (ptr[2] << 16) | (ptr[3] << 24); DCC_LOG2(LOG_MSG, "0x%08x data=0x%04x", addr, data); cr = FLASH_PG | FLASH_PSIZE_32; cm3_primask_set(1); sr = stm32f2x_flash_wr32(flash, cr, addr, data); cm3_primask_set(pri); if (sr & FLASH_ERR) { DCC_LOG(LOG_WARNING, "stm32f2x_flash_wr32() failed!"); return -1; } ptr += 4; addr++; } return n * 4; }
void pktbuf_free(void * ptr) { struct pktbuf * buf = (struct pktbuf *)ptr; uint32_t primask; /* critical section enter */ primask = cm3_primask_get(); cm3_primask_set(1); buf->next = pktbuf_pool.free; pktbuf_pool.free = buf; /* critical section exit */ cm3_primask_set(primask); }
void thinkos_rt_snapshot_svc(int32_t * arg) { uint32_t * dst = (uint32_t *)arg[0]; uint32_t pri = cm3_primask_get(); uint32_t * src; int i; cm3_primask_set(1); src = (uint32_t *)&thinkos_rt; for (i = 0; i < (sizeof(struct thinkos_rt) / 4); ++i) dst[i] = src[i]; cm3_primask_set(pri); }
void * pktbuf_alloc(void) { struct pktbuf * buf; uint32_t primask; /* critical section enter */ primask = cm3_primask_get(); cm3_primask_set(1); if ((buf = pktbuf_pool.free) != NULL) pktbuf_pool.free = buf->next; else pktbuf_pool.error++; /* critical section exit */ cm3_primask_set(primask); return (void *)buf; }
int stm32_flash_erase(unsigned int offs, unsigned int len) { struct stm32_flash * flash = STM32_FLASH; unsigned int cnt; uint32_t pri; uint32_t cr; uint32_t sr; pri = cm3_primask_get(); cr = flash->cr; if (cr & FLASH_LOCK) { DCC_LOG(LOG_TRACE, "unlocking flash..."); /* unlock flash write */ flash->keyr = FLASH_KEY1; flash->keyr = FLASH_KEY2; } cnt = 0; while (cnt < len) { unsigned int page; unsigned int sect; unsigned int size; page = offs >> 14; if ((page << 14) != (offs)) { DCC_LOG(LOG_ERROR, "offset must be a aligned to a page boundary."); return -2; }; if (page < 4) { sect = page; size = 16384; } else if (page == 4) { sect = 4; size = 65536; } else if ((page % 8) == 0) { sect = ((page - 7) / 8) + 5; size = 131072; } else { DCC_LOG(LOG_ERROR, "offset must be a aligned to a " "sector boundary."); return -3; } /* Clear errors */ flash->sr = FLASH_ERR; cr = FLASH_STRT | FLASH_SER | FLASH_SNB(sect); cm3_primask_set(1); sr = stm32f2x_flash_sect_erase(flash, cr); cm3_primask_set(pri); if (sr & FLASH_ERR) { DCC_LOG1(LOG_WARNING, "stm32f2x_flash_sect_erase() failed" " sr=%08x!", sr); return -1; } cnt += size; offs += size; } return cnt; }
int stm32_flash_write(uint32_t offs, const void * buf, unsigned int len) { struct stm32_flash * flash = STM32_FLASH; uint8_t * ptr; uint32_t pecr; int rem; DCC_LOG2(LOG_INFO, "offs=0x%06x len=%d", offs, len); if (offs & 3) { DCC_LOG(LOG_ERROR, "offset must be word aligned!"); return -1; } pecr = flash->pecr; DCC_LOG1(LOG_INFO, "PECR=0x%08x", pecr); if (pecr & FLASH_PRGLOCK) { DCC_LOG(LOG_TRACE, "unlocking flash..."); if (pecr & FLASH_PELOCK) { flash->pekeyr = FLASH_PEKEY1; flash->pekeyr = FLASH_PEKEY2; } flash->prgkeyr= FLASH_PRGKEYR1; flash->prgkeyr= FLASH_PRGKEYR2; } ptr = (uint8_t *)buf; rem = len; while (rem > 3) { uint32_t pos; uint32_t cnt; uint32_t pri; uint32_t sr; /* adjust offset for half page alignement */ pos = offs - (offs & ~((FLASH_PAGE_SIZE / 2) - 1)); offs -= pos; /* make sure we don't exceed the page boundary */ cnt = MIN((FLASH_PAGE_SIZE / 2) - pos, rem); DCC_LOG3(LOG_INFO, "1. offs=0x%06x pos=%d cnt=%d", offs, pos, cnt); /* adjust counter to count words */ cnt &= ~3; pri = cm3_primask_get(); cm3_primask_set(1); /* start half page write */ sr = stm32l_flash_pg_wr(offs, ptr, pos, cnt); cm3_primask_set(pri); if (sr & FLASH_ERR) { DCC_LOG(LOG_WARNING, "Flash write failed!"); return -1; } ptr += cnt; rem -= cnt; offs += pos + cnt; } if (rem) { uint32_t pos; uint32_t pri; uint8_t data[4]; int ret; int i; /* FIXME: single word writing.... */ /* adjust offset for half page alignement */ pos = offs - (offs & ~((FLASH_PAGE_SIZE / 2) - 1)); offs -= pos; /* copy data to temporary buffer */ for (i = 0; i < rem; ++i) data[i] = ptr[i]; /* fill the remaining buffer */ for (; i < 4; ++i) data[i] = 0; DCC_LOG3(LOG_INFO, "2. offs=0x%06x pos=%d cnt=%d", offs, pos, 4); pri = cm3_primask_get(); cm3_primask_set(1); /* start half page write */ ret = stm32l_flash_pg_wr(offs, data, pos, 4); cm3_primask_set(pri); if (ret < 0) { DCC_LOG(LOG_WARNING, "Flash write failed!"); return ret; } }; return len; }