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;
}
