void thinkos_ev_wait_svc(int32_t * arg)
{
unsigned int wq = arg[0];
unsigned int no = wq - THINKOS_EVENT_BASE;
int self = thinkos_rt.active;
unsigned int ev;
#if THINKOS_ENABLE_ARG_CHECK
if (no >= THINKOS_EVENT_MAX) {
DCC_LOG1(LOG_ERROR, "object %d is not an event set!", wq);
arg[0] = THINKOS_EINVAL;
return;
}
#if THINKOS_ENABLE_EVENT_ALLOC
if (__bit_mem_rd(&thinkos_rt.ev_alloc, no) == 0) {
DCC_LOG1(LOG_ERROR, "invalid event set %d!", wq);
arg[0] = THINKOS_EINVAL;
return;
}
#endif
#endif
cm3_cpsid_i();
/* check for any pending unmasked event */
if ((ev = __clz(__rbit(thinkos_rt.ev[no].pend &
thinkos_rt.ev[no].mask))) < 32) {
DCC_LOG2(LOG_MSG, "set=0x%08x msk=0x%08x",
thinkos_rt.ev[no].pend, thinkos_rt.ev[no].mask);
__bit_mem_wr(&thinkos_rt.ev[no].pend, ev, 0);
DCC_LOG2(LOG_INFO, "pending event %d.%d!", wq, ev);
arg[0] = ev;
cm3_cpsie_i();
return;
}
/* insert into the wait queue */
__thinkos_wq_insert(wq, self);
/* wait for event */
/* remove from the ready wait queue */
__bit_mem_wr(&thinkos_rt.wq_ready, thinkos_rt.active, 0);
#if THINKOS_ENABLE_TIMESHARE
/* if the ready queue is empty, collect
the threads from the CPU wait queue */
if (thinkos_rt.wq_ready == 0) {
thinkos_rt.wq_ready = thinkos_rt.wq_tmshare;
thinkos_rt.wq_tmshare = 0;
}
#endif
cm3_cpsie_i();
DCC_LOG2(LOG_INFO, "<%d> waiting for event %d.xx ...", self, wq);
/* signal the scheduler ... */
__thinkos_defer_sched();
}
/* initialize the idle thread */
struct thinkos_context * __thinkos_idle_init(void)
{
struct thinkos_context * idle_ctx;
idle_ctx = (struct thinkos_context *)THINKOS_IDLE_STACK_BASE;
#if THINKOS_IDLE_STACK_BSS
DCC_LOG1(LOG_MSG, "BSS idle stack @ 0x%08x", THINKOS_IDLE_STACK_BASE);
#endif
#if THINKOS_IDLE_STACK_ALLOC
DCC_LOG1(LOG_MSG, "Alloc idle stack @ 0x%08x", THINKOS_IDLE_STACK_BASE);
#endif
idle_ctx->pc = (uint32_t)thinkos_idle_task;
idle_ctx->lr = (uint32_t)__thinkos_thread_exit;
idle_ctx->xpsr = CM_EPSR_T; /* set the thumb bit */
thinkos_rt.ctx[THINKOS_THREAD_IDLE] = idle_ctx;
#if (THINKOS_THREADS_MAX < 32)
/* put the IDLE thread in the ready queue */
__bit_mem_wr(&thinkos_rt.wq_ready, THINKOS_THREADS_MAX, 1);
#endif
#if THINKOS_ENABLE_THREAD_INFO
/* set the IDLE thread info */
thinkos_rt.th_inf[THINKOS_THREAD_IDLE] = &thinkos_idle_inf;
#endif
return idle_ctx;
}
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 stm32f_serial_init(struct stm32f_serial_drv * drv,
unsigned int baudrate, unsigned int flags)
{
struct stm32_usart * uart = drv->uart;
DCC_LOG1(LOG_TRACE, "UART=0x%08x", uart);
DCC_LOG1(LOG_TRACE, "SERIAL_RX_FIFO_LEN=%d", SERIAL_RX_FIFO_LEN);
DCC_LOG1(LOG_TRACE, "SERIAL_TX_FIFO_LEN=%d", SERIAL_TX_FIFO_LEN);
DCC_LOG1(LOG_TRACE, "SERIAL_ENABLE_TX_MUTEX=%d", SERIAL_ENABLE_TX_MUTEX);
drv->rx_flag = thinkos_flag_alloc();
drv->tx_flag = thinkos_flag_alloc();
#if SERIAL_ENABLE_TX_MUTEX
drv->tx_mutex = thinkos_mutex_alloc();
DCC_LOG1(LOG_TRACE, "tx_mutex=%d", drv->tx_mutex);
#endif
drv->tx_fifo.head = drv->tx_fifo.tail = 0;
drv->rx_fifo.head = drv->rx_fifo.tail = 0;
drv->txie = CM3_BITBAND_DEV(&uart->cr1, 7);
thinkos_flag_give(drv->tx_flag);
stm32_usart_init(uart);
stm32_usart_baudrate_set(uart, baudrate);
stm32_usart_mode_set(uart, SERIAL_8N1);
/* enable RX interrupt */
uart->cr1 |= USART_RXNEIE | USART_IDLEIE;
/* enable UART */
stm32_usart_enable(uart);
return 0;
}
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 thinkos_irq_wait_svc(int32_t * arg)
{
unsigned int irq = arg[0];
int32_t self = thinkos_rt.active;
#if THINKOS_ENABLE_ARG_CHECK
if (irq >= THINKOS_IRQ_MAX) {
DCC_LOG1(LOG_ERROR, "invalid IRQ %d!", irq);
arg[0] = THINKOS_EINVAL;
return;
}
#endif
DCC_LOG1(LOG_MSG, "IRQ %d", irq);
/* store the thread info */
thinkos_rt.irq_th[irq] = self;
/* clear pending interrupt */
cm3_irq_pend_clr(irq);
/* enable this interrupt source */
cm3_irq_enable(irq);
/* prepare to wait ... */
__thinkos_wait(self);
}
int serial_ctrl_task(struct vcom * vcom)
{
struct serial_dev * serial = vcom->serial;
struct usb_cdc_class * usb = vcom->usb;
struct usb_cdc_state prev_state;
struct usb_cdc_state state;
DCC_LOG1(LOG_TRACE, "[%d] started.", thinkos_thread_self());
usb_cdc_state_get(usb, &prev_state);
while (1) {
usb_cdc_ctrl_event_wait(usb, 0);
usb_cdc_state_get(usb, &state);
#if 0
if (state.cfg != prev_state.cfg) {
DCC_LOG1(LOG_TRACE, "[%d] config changed.", thinkos_thread_self());
prev_state.cfg = state.cfg;
}
if (state.ctrl != prev_state.ctrl) {
DCC_LOG1(LOG_TRACE, "[%d] control changed.", thinkos_thread_self());
prev_state.ctrl = state.ctrl;
}
#endif
}
return 0;
}
int usb_ctrl_task(struct vcom * vcom)
{
struct serial_dev * serial = vcom->serial;
struct usb_cdc_class * usb = vcom->usb;
struct usb_cdc_state prev_state;
struct usb_cdc_state state;
DCC_LOG1(LOG_TRACE, "[%d] started.", thinkos_thread_self());
usb_cdc_state_get(usb, &prev_state);
while (1) {
usb_cdc_ctrl_event_wait(usb, 0);
usb_cdc_state_get(usb, &state);
if (memcmp(&state.cfg, &prev_state.cfg, sizeof(struct serial_config)) != 0) {
DCC_LOG1(LOG_TRACE, "[%d] config changed.", thinkos_thread_self());
prev_state.cfg = state.cfg;
}
if (memcmp(&state.ctrl, &prev_state.ctrl, sizeof(struct serial_control)) != 0) {
DCC_LOG1(LOG_TRACE, "[%d] control changed.", thinkos_thread_self());
prev_state.ctrl = state.ctrl;
}
}
return 0;
}
void thinkos_sem_init_svc(int32_t * arg)
{
unsigned int wq = arg[0];
unsigned int sem = wq - THINKOS_SEM_BASE;
uint32_t value = (uint32_t)arg[1];
#if THINKOS_ENABLE_ARG_CHECK
if (sem >= THINKOS_SEMAPHORE_MAX) {
DCC_LOG1(LOG_ERROR, "object %d is not a semaphore!", wq);
arg[0] = THINKOS_EINVAL;
return;
}
#if THINKOS_ENABLE_SEM_ALLOC
if (__bit_mem_rd(thinkos_rt.sem_alloc, sem) == 0) {
DCC_LOG1(LOG_ERROR, "invalid semaphore %d!", wq);
arg[0] = THINKOS_EINVAL;
return;
}
#endif
#endif
DCC_LOG2(LOG_TRACE, "sem[%d] <= %d", sem, value);
thinkos_rt.sem_val[sem] = value;
arg[0] = 0;
}
static int uart_console_write(struct uart_console_dev * dev, const void * buf,
unsigned int len)
{
char * cp = (char *)buf;
int c;
int n;
DCC_LOG1(LOG_INFO, "len=%d", len);
#if ENABLE_UART_TX_MUTEX
thinkos_mutex_lock(dev->tx_mutex);
#endif
for (n = 0; n < len; n++) {
c = cp[n];
if (c == '\n') {
DCC_LOG(LOG_INFO, "CR");
uart_putc(dev, '\r');
}
uart_putc(dev, c);
}
#if ENABLE_UART_TX_MUTEX
thinkos_mutex_unlock(dev->tx_mutex);
#endif
DCC_LOG1(LOG_INFO, "cnt=%d", n);
return n;
}
void __attribute__((noreturn)) serial_recv_task(struct vcom * vcom)
{
struct serial_dev * serial = vcom->serial;
struct usb_cdc_class * cdc = vcom->cdc;
uint8_t buf[VCOM_BUF_SIZE];
int len;
DCC_LOG1(LOG_TRACE, "[%d] started.", thinkos_thread_self());
/* wait for line configuration */
usb_cdc_acm_lc_wait(cdc);
/* enable serial */
serial_enable(serial);
for (;;) {
len = serial_read(serial, buf, VCOM_BUF_SIZE, 1000);
if (len > 0) {
// dbg_write(buf, len);
if (vcom->mode == VCOM_MODE_CONVERTER) {
led_flash(LED_AMBER, 50);
usb_cdc_write(cdc, buf, len);
}
if (vcom->mode == VCOM_MODE_SDU_TRACE) {
led_flash(LED_AMBER, 50);
sdu_decode(buf, len);
}
#if RAW_TRACE
if (len == 1)
DCC_LOG1(LOG_TRACE, "RX: %02x", buf[0]);
else if (len == 2)
DCC_LOG2(LOG_TRACE, "RX: %02x %02x",
buf[0], buf[1]);
else if (len == 3)
DCC_LOG3(LOG_TRACE, "RX: %02x %02x %02x",
buf[0], buf[1], buf[2]);
else if (len == 4)
DCC_LOG4(LOG_TRACE, "RX: %02x %02x %02x %02x",
buf[0], buf[1], buf[2], buf[3]);
else if (len == 5)
DCC_LOG5(LOG_TRACE, "RX: %02x %02x %02x %02x %02x",
buf[0], buf[1], buf[2], buf[3], buf[4]);
else if (len == 6)
DCC_LOG6(LOG_TRACE, "RX: %02x %02x %02x %02x %02x %02x",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
else if (len == 7)
DCC_LOG7(LOG_TRACE, "RX: %02x %02x %02x %02x %02x %02x %02x ",
buf[0], buf[1], buf[2], buf[3],
buf[4], buf[5], buf[6]);
else
DCC_LOG8(LOG_TRACE, "RX: %02x %02x %02x %02x %02x %02x "
"%02x %02x ...", buf[0], buf[1], buf[2], buf[3],
buf[4], buf[5], buf[6], buf[7]);
#endif
#if SDU_TRACE
RX(buf, len);
#endif
}
}
}
static void dac_timer_init(uint32_t freq)
{
struct stm32f_rcc * rcc = STM32F_RCC;
struct stm32f_tim * tim = STM32F_TIM2;
uint32_t div;
uint32_t pre;
uint32_t n;
/* get the total divisior */
div = ((stm32f_tim1_hz) + (freq / 2)) / freq;
/* get the minimum pre scaler */
pre = (div / 65536) + 1;
/* get the reload register value */
n = (div * 2 + pre) / (2 * pre);
DCC_LOG1(LOG_TRACE, "stm32f_tim1_hz=%dHz", stm32f_tim1_hz);
DCC_LOG3(LOG_TRACE, "freq=%dHz pre=%d n=%d", freq, pre, n);
DCC_LOG1(LOG_TRACE, "real freq=%dHz\n", stm32f_tim1_hz / pre / n);
/* Timer clock enable */
rcc->apb1enr |= RCC_TIM2EN;
/* Timer configuration */
tim->psc = pre - 1;
tim->arr = n - 1;
tim->cnt = 0;
tim->egr = 0;
tim->dier = TIM_UIE; /* Update interrupt enable */
tim->ccmr1 = TIM_OC1M_PWM_MODE1;
tim->ccr1 = tim->arr / 2;
tim->cr2 = TIM_MMS_OC1REF;
}
int serdrv_send(struct serdrv * dev, const void * buf, int len)
{
uint8_t * cp = (uint8_t *)buf;
int rem = len;
DCC_LOG1(LOG_INFO, "len=%d", len);
while (rem) {
unsigned int head;
int free;
int n;
int i;
thinkos_flag_take(SERDRV_TX_FLAG);
head = dev->tx_fifo.head;
free = UART_TX_FIFO_BUF_LEN - (int8_t)(head - dev->tx_fifo.tail);
DCC_LOG3(LOG_MSG, "head=%d tail=%d n=%d", head, dev->tx_fifo.tail, n);
n = MIN(rem, free);
for (i = 0; i < n; ++i)
dev->tx_fifo.buf[head++ & (UART_TX_FIFO_BUF_LEN - 1)] = *cp++;
dev->tx_fifo.head = head;
*dev->txie = 1;
if (free > n)
thinkos_flag_give(SERDRV_TX_FLAG);
rem -= n;
DCC_LOG1(LOG_INFO, "rem=%d", rem);
}
return len;
}
void thinkos_sem_timedwait_svc(int32_t * arg)
{
unsigned int wq = arg[0];
unsigned int sem = wq - THINKOS_SEM_BASE;
uint32_t ms = (uint32_t)arg[1];
int self = thinkos_rt.active;
#if THINKOS_ENABLE_ARG_CHECK
if (sem >= THINKOS_SEMAPHORE_MAX) {
DCC_LOG1(LOG_ERROR, "object %d is not a semaphore!", wq);
arg[0] = THINKOS_EINVAL;
return;
}
#if THINKOS_ENABLE_SEM_ALLOC
if (__bit_mem_rd(thinkos_rt.sem_alloc, sem) == 0) {
DCC_LOG1(LOG_ERROR, "invalid semaphore %d!", wq);
arg[0] = THINKOS_EINVAL;
return;
}
#endif
#endif
/* avoid possible race condition on sem_val */
/* this is only necessary in case we use the __uthread_sem_post() call
inside interrupt handlers */
/* TODO: study the possibility of using exclusive access instead of
disabling interrupts. */
cm3_cpsid_i();
if (thinkos_rt.sem_val[sem] > 0) {
thinkos_rt.sem_val[sem]--;
arg[0] = 0;
} else {
/* insert into the semaphore wait queue */
__thinkos_tmdwq_insert(wq, self, ms);
DCC_LOG2(LOG_INFO, "<%d> waiting on semaphore %d...", self, wq);
/* wait for event */
/* remove from the ready wait queue */
__bit_mem_wr(&thinkos_rt.wq_ready, self, 0);
#if THINKOS_ENABLE_TIMESHARE
/* if the ready queue is empty, collect
the threads from the CPU wait queue */
if (thinkos_rt.wq_ready == 0) {
thinkos_rt.wq_ready = thinkos_rt.wq_tmshare;
thinkos_rt.wq_tmshare = 0;
}
#endif
/* Set the default return value to timeout. The
sem_post call will change this to 0 */
arg[0] = THINKOS_ETIMEDOUT;
}
/* reenable interrupts ... */
cm3_cpsie_i();
/* signal the scheduler ... */
__thinkos_defer_sched();
}
void __attribute__((noreturn)) ifnet_input_task(void * arg)
{
struct ifnet * ifn = NULL;
unsigned int proto;
unsigned int idx;
uint8_t * pkt;
uint8_t * src;
int ret;
int len;
for (;;) {
DCC_LOG(LOG_INFO, "wait...");
/* wait for an event form a network interface */
idx = thinkos_ev_wait(__ifnet__.evset);
#if 0
if (idx < 0) {
DCC_LOG1(LOG_ERROR, "thinkos_ev_wait() failed: %d", idx);
abort();
} else if (idx > IFNET_INTERFACES_MAX) {
DCC_LOG1(LOG_ERROR, "thinkos_ev_wait() invalid event: %d", idx);
abort();
}
#endif
/* lookup the interface */
ifn = &__ifnet__.ifn[idx];
/* get the packet from the network interface */
while ((len = ifn_pkt_recv(ifn, &src, &proto, &pkt)) > 0) {
tcpip_net_lock();
NETIF_STAT_ADD(ifn, rx_pkt, 1);
if (proto == NTOHS(ETH_P_IP)) {
DCC_LOG(LOG_INFO, "IP");
DBG("IFNET: IP packet received.");
ret = ip_input(ifn, (struct iphdr *)pkt, len);
} else if (proto == NTOHS(ETH_P_ARP)) {
DCC_LOG(LOG_INFO, "ARP");
ret = etharp_input(ifn, (struct etharp*)pkt, len);
} else {
NETIF_STAT_ADD(ifn, rx_drop, 1);
DCC_LOG1(LOG_TRACE, "unhandled protocol: %d", proto);
WARN("IFNET: unhandled protocol: %d", proto);
ret = 0;
}
tcpip_net_unlock();
if (ret <= 0) {
ifn_pkt_free(ifn, pkt);
} else {
__ifnet__.stats.err++;
WARN("IFNET: not releasing packet: %d", pkt);
}
}
}
}
static void tcp_parse_options(struct tcp_pcb * tp, struct tcphdr * th,
uint8_t * p, int len)
{
int n = len;
int opt;
int val;
while (len) {
opt = p[0];
if (opt == TCPOPT_END) {
/* end of options */
DCC_LOG(LOG_MSG, "option END");
break;
}
if (opt == TCPOPT_NOP) {
DCC_LOG(LOG_MSG, "option NOP");
n = 1;
} else {
n = p[1];
if (n <= 0) {
DCC_LOG1(LOG_INFO, "invalid option len: %d", n);
break;
}
switch (opt) {
case TCPOPT_MSS:
if ((n == TCPOLEN_MSS) && ((th->th_flags & TH_SYN))) {
/* FIXME: endianess */
val = ((int)p[2] << 8) + (int)p[3];
/* Only set the maximum segment size if lower than
the initialy configured one. */
if (val < tp->t_maxseg)
tp->t_maxseg = val;
DCC_LOG1(LOG_INFO, "option MSS: %d", val);
}
break;
case TCPOPT_WINDOW:
DCC_LOG(LOG_INFO, "unsuported option WINDOW");
break;
case TCPOPT_SACK_PERMITTED:
DCC_LOG(LOG_INFO, "unsuported option SACK_PERMITTED");
break;
case TCPOPT_SACK:
DCC_LOG(LOG_INFO, "unsuported option SACK");
break;
case TCPOPT_TIMESTAMP:
DCC_LOG(LOG_INFO, "unsuported option TIMESTAMP");
break;
default:
DCC_LOG1(LOG_INFO, "unsuported option: %d", opt);
}
}
len -= n;
p += n;
};
}
void profclk_init(void)
{
/* Enable trace */
CM3_DCB->demcr |= DCB_DEMCR_TRCENA;
/* Enable cycle counter */
CM3_DWT->ctrl |= DWT_CTRL_CYCCNTENA;
DCC_LOG1(LOG_TRACE, "DWT_CTRL=0x%08x", CM3_DWT->ctrl);
DCC_LOG1(LOG_TRACE, "DWT_CYCCNT=0x%08x", CM3_DWT->cyccnt);
}
int db_pw_list_enc(struct microjs_json_parser * jsn,
struct microjs_val * val,
unsigned int opt, void * ptr)
{
uint32_t buf[(sizeof(struct pw_list) + SLCDEV_PW_LIST_LEN_MAX *
sizeof(struct pw_entry)) / sizeof(uint32_t)];
struct pw_list * lst = (struct pw_list *)buf;
struct pw_entry * pw;
int ret;
int typ;
lst->cnt = 0;
pw = &lst->pw[0];
if ((typ = microjs_json_get_val(jsn, val)) == MICROJS_JSON_ARRAY) {
/* list of PWs */
do {
if (lst->cnt == SLCDEV_PW_LIST_LEN_MAX) {
DCC_LOG(LOG_ERROR, "too many items!");
return -1;
}
DCC_LOG1(LOG_INFO, "PW[%d]", lst->cnt);
typ = microjs_json_get_val(jsn, val);
if ((ret = db_pw_enc(jsn, typ, val, pw)) < 0) {
DCC_LOG(LOG_ERROR, "db_parse_pw() failed!");
return ret;
}
DCC_LOG3(LOG_INFO, "PW[%d]: min=%d max=%d",
lst->cnt, pw->min, pw->max);
lst->cnt++;
pw++;
} while ((typ = microjs_json_get_val(jsn, val)) == MICROJS_JSON_ARRAY);
if (typ != MICROJS_JSON_END_ARRAY) {
DCC_LOG(LOG_ERROR, "expecting array closing!");
return -1;
}
} else {
/* single PW */
if ((ret = db_pw_enc(jsn, typ, val, pw)) < 0) {
DCC_LOG(LOG_ERROR, "db_parse_pw() failed!");
return ret;
}
DCC_LOG2(LOG_INFO, "PW: min=%d max=%d", pw->min, pw->max);
lst->cnt++;
}
DCC_LOG1(LOG_INFO, "lst.cnt=%d", lst->cnt);
ret = db_stack_push(lst, sizeof(struct pw_list) + lst->cnt *
sizeof(struct pw_entry), ptr);
return ret;
}
void __attribute__((noreturn)) usb_recv_task(struct vcom * vcom)
{
struct serial_dev * serial = vcom->serial;
usb_cdc_class_t * cdc = vcom->cdc;
uint8_t buf[VCOM_BUF_SIZE];
int len;
DCC_LOG1(LOG_TRACE, "[%d] started.", thinkos_thread_self());
DCC_LOG2(LOG_TRACE, "vcom->%p, cdc->%p", vcom, cdc);
for (;;) {
len = usb_cdc_read(cdc, buf, VCOM_BUF_SIZE, 1000);
if (vcom->mode == VCOM_MODE_CONVERTER) {
if (len > 0) {
led_flash(LED_RED, 50);
serial_write(serial, buf, len);
#if RAW_TRACE
if (len == 1)
DCC_LOG1(LOG_TRACE, "TX: %02x", buf[0]);
else if (len == 2)
DCC_LOG2(LOG_TRACE, "TX: %02x %02x",
buf[0], buf[1]);
else if (len == 3)
DCC_LOG3(LOG_TRACE, "TX: %02x %02x %02x",
buf[0], buf[1], buf[2]);
else if (len == 4)
DCC_LOG4(LOG_TRACE, "TX: %02x %02x %02x %02x",
buf[0], buf[1], buf[2], buf[3]);
else if (len == 5)
DCC_LOG5(LOG_TRACE, "TX: %02x %02x %02x %02x %02x",
buf[0], buf[1], buf[2], buf[3], buf[4]);
else if (len == 6)
DCC_LOG6(LOG_TRACE, "TX: %02x %02x %02x %02x %02x %02x",
buf[0], buf[1], buf[2], buf[3], buf[4], buf[5]);
else if (len == 7)
DCC_LOG7(LOG_TRACE, "TX: %02x %02x %02x %02x %02x %02x %02x ",
buf[0], buf[1], buf[2], buf[3],
buf[4], buf[5], buf[6]);
else
DCC_LOG8(LOG_TRACE, "TX: %02x %02x %02x %02x %02x %02x "
"%02x %02x ...", buf[0], buf[1], buf[2], buf[3],
buf[4], buf[5], buf[6], buf[7]);
#endif
#if SDU_TRACE
TX(buf, len);
#endif
// dbg_write(buf, len);
}
} else {
// forward to service input
vcom_service_input(vcom, buf, len);
}
}
}
int net_probe(void)
{
void * pkt;
int ret;
/* drain the transmmit queue */
pkt = rs485_pkt_drain(&net.link);
if (pkt != NULL)
pktbuf_free(pkt);
/* set the probe pin low */
net.probe_mode = true;
stm32_gpio_clr(RS485_MODE);
DCC_LOG(LOG_TRACE, "Probe mode.");
thinkos_flag_clr(net.probe_flag);
if ((pkt = pktbuf_alloc()) != NULL) {
uint32_t seq;
while ((seq = rand()) == 0);
/* send a probe packet */
sprintf((char *)pkt, "PRB=%08x", seq);
DCC_LOG1(LOG_TRACE, "seq=0x%08x", seq);
rs485_pkt_enqueue(&net.link, pkt, 12);
net.stat.tx.pkt_cnt++;
net.stat.tx.octet_cnt += 12;
/* wait for the end of transmission */
pkt = rs485_pkt_drain(&net.link);
pktbuf_free(pkt);
if ((ret = thinkos_flag_timedwait(net.probe_flag, 10)) == 0) {
if (seq != net.probe_seq) {
DCC_LOG(LOG_WARNING, "probe sequence mismatch!");
ret = -1;
}
} else if (ret == THINKOS_ETIMEDOUT) {
DCC_LOG(LOG_WARNING, "probe sequence timedout!");
}
DCC_LOG1(LOG_TRACE, "seq=0x%08x", seq);
} else {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
ret = -1;
}
/* set the probe pin high */
net.probe_mode = false;
stm32_gpio_set(RS485_MODE);
DCC_LOG(LOG_TRACE, "Probe mode.");
return ret;
}
void thinkos_irq_ctl_svc(int32_t * arg)
{
unsigned int req = arg[0];
unsigned int irq = arg[1];
#if THINKOS_ENABLE_ARG_CHECK
int irq_max = ((uintptr_t)&__sizeof_rom_vectors / sizeof(void *)) - 16;
if (irq >= irq_max) {
DCC_LOG1(LOG_ERROR, "invalid IRQ %d!", irq);
__thinkos_error(THINKOS_ERR_IRQ_INVALID);
arg[0] = THINKOS_EINVAL;
return;
}
#endif
arg[0] = 0;
switch (req) {
case THINKOS_IRQ_ENABLE:
DCC_LOG1(LOG_TRACE, "enabling IRQ %d", irq);
/* clear pending interrupt */
cm3_irq_enable(irq);
break;
case THINKOS_IRQ_DISABLE:
cm3_irq_disable(irq);
break;
case THINKOS_IRQ_PRIORITY_SET:
{
int priority = arg[2];
if (priority > IRQ_PRIORITY_VERY_LOW)
priority = IRQ_PRIORITY_VERY_LOW;
else if (priority < IRQ_PRIORITY_VERY_HIGH)
priority = IRQ_PRIORITY_VERY_HIGH;
/* set the interrupt priority */
cm3_irq_pri_set(irq, priority);
}
break;
// case THINKOS_IRQ_SVC_SET:
// break;
default:
DCC_LOG1(LOG_ERROR, "invalid IRQ ctl request %d!", req);
arg[0] = THINKOS_EINVAL;
break;
}
}
struct tcp_pcb * tcp_accept(const struct tcp_pcb * __mux)
{
struct tcp_listen_pcb * mux = (struct tcp_listen_pcb *)__mux;
struct tcp_pcb * tp;
if (__mux == NULL)
DCC_LOG(LOG_WARNING, "NULL pointer");
DCC_LOG1(LOG_INFO, "<%04x> waiting...", (int)mux);
if (thinkos_sem_wait(mux->t_sem) < 0) {
DCC_LOG2(LOG_ERROR, "<%04x> thinkos_sem_wait(%d) failed!",
(int)mux, mux->t_sem);
return NULL;
}
tcpip_net_lock();
if (mux->t_state != TCPS_LISTEN) {
DCC_LOG1(LOG_WARNING, "<%04x> invalid state!", (int)mux);
tp = NULL;
} else {
unsigned int tail;
tail = mux->t_tail;
tp = (struct tcp_pcb *)mux->t_backlog[tail++];
/* wrap */
if (tail == mux->t_max)
tail = 0;
mux->t_tail = tail;
#ifdef ENABLE_SANITY
if (tp == NULL)
DCC_LOG(LOG_PANIC, "NULL pointer");
#endif
DCC_LOG4(LOG_INFO, "<%04x> --> <%04x> %I:%d", (int)mux,
(int)tp, tp->t_faddr, ntohs(tp->t_fport));
// thinkos_sleep(100);
}
tcpip_net_unlock();
return tp;
}
void arm926_on_debug_dentry(jtag_tap_t * tap, armice_context_t * ct)
{
uint32_t data;
DCC_LOG(LOG_TRACE, ".");
/* CP15 */
jtag_arm926_cp15_rd(tap, CP15_C0_IDCODE, &data);
DCC_LOG1(LOG_TRACE, "CP15 IDCODE: %08x", data);
jtag_arm926_cp15_rd(tap, CP15_C0_CACHE_TYPE, &data);
DCC_LOG2(LOG_TRACE, "ICahe: %d, DCache: %d",
CACHE_SIZE(C0_ISIZE(data)), CACHE_SIZE(C0_DSIZE(data)));
jtag_arm926_cp15_rd(tap, CP15_C0_TCM_TYPE, &data);
DCC_LOG2(LOG_TRACE, "CP15 DTCM:%d ITCM:%d",
C0_TCM_DTCM(data), C0_TCM_ITCM(data));
jtag_arm926_cp15_rd(tap, CP15_ADDR(7, 0, 15, 0), &data);
data |= 0x7;
jtag_arm926_cp15_wr(tap, CP15_ADDR(7, 0, 15, 0), data);
#if 0
/* TODO: save MMU and Cache control */
jtag_arm926_cp15_rd(tap, CP15_ADDR(0, 0, 5, 0), &d_fsr);
jtag_arm926_cp15_rd(tap, CP15_ADDR(0, 1, 5, 0), &i_fsr);
jtag_arm926_cp15_rd(tap, CP15_ADDR(0, 0, 6, 0), &d_far);
#endif
}
int i2c_master_wr(unsigned int addr, const void * buf, int len)
{
struct stm32f_i2c * i2c = STM32F_I2C1;
int ret;
xfer.ptr = (uint8_t *)buf;
xfer.rem = len;
xfer.cnt = len;
/* – To enter Transmitter mode, a master sends the slave
address with LSB reset. */
xfer.addr = addr << 1;
xfer.ret = -2;
DCC_LOG2(LOG_INFO, "addr=0x%02x len=%d", addr, len);
// tracef("addr=0x%02x len=%d", addr, len);
__thinkos_flag_clr(xfer.flag);
i2c->cr1 = I2C_START | I2C_ACK | I2C_PE; /* generate a Start condition */
if (thinkos_flag_timedwait(xfer.flag, 100) == THINKOS_ETIMEDOUT) {
// tracef("thinkos_flag_timedwait() tmo %d %d ", xfer.cnt, xfer.flag);
DCC_LOG(LOG_TRACE, "Timeout...");
i2c_master_reset();
ret = -1;
} else
ret = xfer.ret;
DCC_LOG1(LOG_INFO, "ret=%d", ret);
return ret;
}
int sndbuf_pool_test(void)
{
sndbuf_t * lst[SNDBUF_POOL_SIZE];
sndbuf_t * buf;
int i;
int j = 0;
do {
for (i = 0; i < SNDBUF_POOL_SIZE; ++i) {
buf = sndbuf_alloc();
if (buf == NULL) {
DCC_LOG1(LOG_PANIC, "%d: sndbuf_alloc() failed!", i);
return -1;
}
lst[i] = buf;
}
buf = sndbuf_alloc();
if (buf != NULL) {
DCC_LOG2(LOG_PANIC, "%d: sndbuf_alloc() != NULL!", i, buf);
return -1;
}
for (i = 0; i < SNDBUF_POOL_SIZE; ++i) {
sndbuf_free(lst[i]);
}
} while (++j < 2);
DCC_LOG(LOG_TRACE, "success.");
return 0;
}
int net_recv(void * buf, int len)
{
void * pkt;
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
DBG("pktbuf_alloc() failed!\n");
return -1;
}
len = rs485_pkt_receive(&net.link, &pkt, pktbuf_len);
// DBG("len=%d\n", len);
DCC_LOG1(LOG_TRACE, "%d", len);
DCC_LOG2(LOG_TRACE, "pkt=%p len=%d", pkt, len);
if (pkt != NULL) {
memcpy(buf, pkt, len);
pktbuf_free(pkt);
}
return len;
}
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;
}
int jtag3ctrl_init(const void * rbf, int size)
{
int ret;
thinkos_escalate(jtag3ctrl_bus_init, NULL);
if ((ret = altera_configure(rbf, size)) < 0) {
DCC_LOG1(LOG_ERROR, "altera_configure() failed: %d!", ret);
INF(" # altera_configure() failed: %d!", ret);
return ret;
} else {
INF("- FPGA configuration done (%d bytes)", ret);
}
/* Enable clock output */
stm32f_mco2_enable();
/* wait for the FPGA initialize */
thinkos_sleep(20);
/* initial configuration */
reg_wr(REG_CFG, 0);
return 0;
}
int ipv4_arp_query(in_addr_t __ipaddr)
{
struct route * rt;
struct ifnet * ifn;
int ret = -ENOENT;
tcpip_net_lock();
DCC_LOG1(LOG_TRACE, "route lookup %I", __ipaddr);
/* lookup for the interface of the local address */
if (((rt = __route_lookup(__ipaddr)) != NULL) &&
(rt->rt_gateway == INADDR_ANY)) {
/* get interface */
ifn = rt->rt_ifn;
/* ARP request */
ret = ifn_arpquery(ifn, __ipaddr);
}
tcpip_net_unlock();
return ret;
}
void thinkos_irq_wait_svc(int32_t * arg, int self)
{
unsigned int irq = arg[0];
#if THINKOS_ENABLE_ARG_CHECK
if (irq >= THINKOS_IRQ_MAX) {
DCC_LOG1(LOG_ERROR, "invalid IRQ %d!", irq);
__thinkos_error(THINKOS_ERR_IRQ_INVALID);
arg[0] = THINKOS_EINVAL;
return;
}
#endif
/* clear pending interrupt */
cm3_irq_pend_clr(irq);
/* wait for event */
__thinkos_suspend(self);
/* store the thread info */
thinkos_rt.irq_th[irq] = self;
/* signal the scheduler ... */
__thinkos_defer_sched();
/* enable this interrupt source */
cm3_irq_enable(irq);
}
