void cm3_default_isr(int irq)
//void cm3_default_isr(void)
{
// int irq;
int th;
// irq = cm3_ipsr_get() - 16;
/* disable this interrupt source */
cm3_irq_disable(irq);
th = thinkos_rt.irq_th[irq];
#if DEBUG
thinkos_rt.irq_th[irq] = THINKOS_THREAD_IDLE;
DCC_LOG2(LOG_MSG, "<%d> IRQ %d", th, irq);
/* TODO: create a wait queue for IRQ waiting. */
if (th >= THINKOS_THREAD_IDLE) {
DCC_LOG2(LOG_ERROR, "<%d> IRQ %d invalid thread!", th, irq);
return;
}
#endif
/* insert the thread into ready queue */
__bit_mem_wr(&thinkos_rt.wq_ready, th, 1);
/* signal the scheduler ... */
__thinkos_preempt();
}
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
}
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();
}
void thinkos_cond_hook(void)
{
int32_t * arg = (int32_t *)cm3_sp_get();
unsigned int ret = arg[0];
unsigned int mwq = arg[1];
unsigned int mutex;
uint32_t lr = cm3_lr_get();
int self = thinkos_rt.active;
int th = self;
mutex = mwq - THINKOS_MUTEX_BASE;
(void)lr;
(void)ret;
(void)mwq;
(void)mutex;
DCC_LOG3(LOG_TRACE, "<%d> mutex=%d lr=0x%08x...", th, mwq, lr);
for(;;);
if (thinkos_rt.lock[mutex] == -1) {
thinkos_rt.lock[mutex] = th;
DCC_LOG2(LOG_TRACE, "<%d> mutex %d locked", th, mwq);
return;
}
/* insert into the mutex wait queue */
__thinkos_wq_insert(mwq, th);
DCC_LOG2(LOG_TRACE , "<%d> waiting on mutex %d...", th, mwq);
}
/* Receive a file and write it into the flash using the YMODEM preotocol */
int dmon_ymodem_flash(struct dmon_comm * comm,
uint32_t addr, unsigned int size)
{
/* FIXME: generalize the application load by removing the low
level flash calls dependency */
#ifdef STM32_FLASH_MEM
/* The YMODEM state machine is allocated at the top of
the stack, make sure there is no app running before
calling the dmon_ymodem_flash()! */
struct ymodem_rcv * ry = ((struct ymodem_rcv *)&_stack) - 1;
uint32_t base = (uint32_t)STM32_FLASH_MEM;
uint32_t offs = addr - base;
int ret;
DCC_LOG2(LOG_INFO, "sp=%p ry=%p", cm3_sp_get(), ry);
DCC_LOG2(LOG_INFO, "offs=0x%08x size=%d", offs, size);
dmon_ymodem_rcv_init(ry, true, false);
ry->fsize = size;
DCC_LOG(LOG_INFO, "Starting...");
while ((ret = dmon_ymodem_rcv_pkt(comm, ry)) >= 0) {
if ((ret == 0) && (ry->xmodem) )
break;
int len = ret;
if (ry->pktno == 1) {
char * cp;
int fsize;
cp = (char *)ry->pkt.data;
DCC_LOGSTR(LOG_INFO, "fname='%s'", cp);
while (*cp != '\0')
cp++;
/* skip null */
cp++;
fsize = dec2int(cp);
if (fsize == 0) {
ret = 0;
break;
}
DCC_LOG1(LOG_INFO, "fsize='%d'", fsize);
ry->fsize = fsize;
DCC_LOG(LOG_INFO, "YMODEM first packet...");
} else {
if (ry->pktno == 2) {
stm32_flash_erase(offs, ry->fsize);
}
stm32_flash_write(offs, ry->pkt.data, len);
offs += len;
}
}
return ret;
#else
return -1;
#endif
}
int cmd_nand(FILE * f, int argc, char ** argv)
{
struct debugger * dbg = &debugger;
value_t val;
uint32_t addr;
uint32_t size;
int n;
argc--;
argv++;
if (argc) {
if ((n = eval_uint32(&val, argc, argv)) < 0) {
DCC_LOG(LOG_WARNING, "eval_uint32(), addr");
return n;
}
argc -= n;
argv += n;
addr = val.uint32;
DCC_LOG2(LOG_INFO, "addr=%08x n=%d", addr, n);
} else
addr = (uint32_t)dbg->dump.base;
if (argc) {
if ((n = eval_uint32(&val, argc, argv)) < 0) {
DCC_LOG(LOG_WARNING, "eval_uint32(), size");
return n;
}
size = val.uint32;
DCC_LOG2(LOG_TRACE, "size=%d n=%d", size, n);
argc -= n;
argv += n;
} else
size = (dbg->dump.size + 3) & ~0x03;
if (argc) {
fprintf(f, "Too many arguments...\n");
return -1;
}
if (size == 0)
size = 64;
dbg->dump.base = addr & ~0x03;
dbg->dump.size = 0;
size = mem_hexdump(f, dbg->dump.base, size);
dbg->dump.base += size;
dbg->dump.size = size;
return 0;
}
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 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;
}
int cmd_bp_clear(FILE * f, int argc, char ** argv)
{
value_t val;
uint32_t addr;
uint32_t size = 0;
int n;
argc--;
argv++;
if (!argc) {
struct dbg_bp * bp;
/* clear all */
while (target_breakpoint_get(NULL, &bp) == 0)
target_breakpoint_delete(bp);
return 0;
}
if ((n = eval_uint32(&val, argc, argv)) < 0) {
DCC_LOG(LOG_WARNING, "target_eval(), addr");
return n;
}
addr = val.uint32;
DCC_LOG2(LOG_TRACE, "addr=%d n=%d", addr, n);
argc -= n;
argv += n;
if (argc) {
if ((n = eval_uint32(&val, argc, argv)) < 0) {
DCC_LOG(LOG_WARNING, "target_eval(), size");
return n;
}
size = val.uint32;
DCC_LOG2(LOG_TRACE, "size=%d n=%d", size, n);
argc -= n;
argv += n;
}
if (argc) {
fprintf(f, "Too many arguments...\n");
return -1;
}
target_breakpoint_clear(addr, size);
target_watchpoint_clear(addr, size);
return 0;
}
int raw_recvfrom_tmo(struct raw_pcb * __raw, void * __buf,
int __len, struct sockaddr_in * __sin,
unsigned int msec)
{
int n;
int err;
DCC_LOG2(LOG_TRACE, "<%05x> len=%d", (int)__raw, __len);
tcpip_net_lock();
DCC_LOG2(LOG_INFO, "<%05x> lock [%d]", (int)__raw, net_mutex);
while (__raw->r_len == 0) {
if (__raw->r_flags & RF_NONBLOCK) {
tcpip_net_unlock();
return -EAGAIN;
}
DCC_LOG3(LOG_TRACE, "<%05x> wait [%d, %d]", (int)__raw,
__raw->r_cond, net_mutex);
if ((err = __os_cond_timedwait(__raw->r_cond, net_mutex, msec)) < 0) {
tcpip_net_unlock();
return err;
}
DCC_LOG2(LOG_TRACE, "<%05x> lock [%d]", (int)__raw, net_mutex);
}
if (__sin != NULL) {
__sin->sin_family = AF_INET;
__sin->sin_port = 0;
__sin->sin_addr.s_addr = __raw->r_faddr;
}
n = (__raw->r_len > __len) ? __len : __raw->r_len;
memcpy(__buf, __raw->r_buf, n);
__raw->r_len = 0;
DCC_LOG2(LOG_TRACE, "<%05x> len=%d", (int)__raw, n);
tcpip_net_unlock();
return n;
}
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 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;
}
int net_send(const void * buf, int len)
{
void * pkt;
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
return -1;
}
DCC_LOG2(LOG_TRACE, "pkt=%p len=%d", pkt, len);
len = MIN(len, pktbuf_len);
memcpy(pkt, buf, len);
pkt = rs485_pkt_enqueue(&net.link, pkt, len);
net.stat.tx.pkt_cnt++;
net.stat.tx.octet_cnt += len;
if (pkt != NULL)
pktbuf_free(pkt);
return 0;
}
static int db_stack_push(void * buf, unsigned int len, void ** ptr)
{
uint32_t pos;
uint32_t offs;
int ret;
pos = (db_stack + 3) & ~3;
offs = FLASH_BLK_DB_BIN_OFFS + pos;
DCC_LOG3(LOG_INFO, "buf=0x%08x len=%d offs=%06x", buf, len, offs);
if ((ret = stm32_flash_write(offs, buf, len)) < 0) {
DCC_LOG(LOG_WARNING, "stm32_flash_write() failed!");
return -1;
}
/* update stack */
db_stack = pos + len;
/* check for collision */
if (db_stack > FLASH_BLK_DB_BIN_SIZE) {
DCC_LOG2(LOG_ERROR, "no memory stack=%d limit=%d!",
db_stack, FLASH_BLK_DB_BIN_SIZE);
return -1;
}
if (ptr != NULL)
*ptr = (void *)(STM32_MEM_FLASH + offs);
return len;
}
int icmp_input(struct ifnet * __if, struct iphdr * __ip,
struct icmphdr * __icmp, int __len)
{
__len -= ICMP_MINLEN;
ICMP_PROTO_STAT_ADD(rx_ok, 1);
switch (__icmp->type) {
case ICMP_ECHO:
DCC_LOG(LOG_INFO, "ICMP: echo request");
return icmp_echoreplay(__if, __ip, __icmp, __len);
case ICMP_DEST_UNREACH:
DCC_LOG(LOG_TRACE, "ICMP: dest unreach");
#if (ENABLE_NET_UDP)
struct icmp * icp = (struct icmp *)__icmp;
struct udphdr * udp;
udp = (struct udphdr *)icmp_skip_ip_hdr(&icp->icmp_ip, __len);
if ((icp->icmp_ip.proto == IPPROTO_UDP) && (udp != NULL) &&
(icp->icmp_code == ICMP_PORT_UNREACH)) {
DCC_LOG1(LOG_WARNING, "UDP port unreach: %d", ntohs(udp->dport));
udp_port_unreach(icp->icmp_ip.daddr, udp->dport,
icp->icmp_ip.saddr, udp->sport);
}
#endif
break;
default:
ICMP_PROTO_STAT_ADD(rx_drop, 1);
DCC_LOG2(LOG_WARNING, "ICMP: %d (%d)", __icmp->type, __len);
}
return 0;
}
int stm32f1_flash_write(cm3ice_ctrl_t * ctrl, ice_mem_ref_t * mem,
const void * buf, ice_size_t len)
{
uint16_t data;
uint32_t addr;
uint8_t * ptr;
uint32_t cr;
int n;
int i;
n = (len + 1) / 2;
ptr = (uint8_t *)buf;
addr = mem->base + mem->offs;
cm3ice_rd32(ctrl, STM32F_BASE_FLASH + FLASH_CR, &cr);
if (cr & LOCK) {
DCC_LOG(LOG_TRACE, "unlocking flash...");
/* unlock flash write */
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_KEYR, FLASH_KEY1);
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_KEYR, FLASH_KEY2);
}
DCC_LOG2(LOG_TRACE, "0x%08x len=%d", addr, len);
for (i = 0; i < n; i++) {
data = ptr[0] | (ptr[1] << 8);
stm32f10x_flash_wr16(ctrl, addr, data);
ptr += 2;
addr += 2;
}
return n * 2;
}
struct i2c_io_blk * i2c_slave_init(unsigned int scl_freq,
unsigned int addr,
void * mem, unsigned int size)
{
struct stm32f_i2c * i2c = STM32F_I2C1;
struct stm32f_rcc * rcc = STM32F_RCC;
uint32_t pclk = stm32f_apb1_hz;
struct i2c_io_blk * io = &i2c_io;
#if defined(STM32F1X)
struct stm32f_afio * afio = STM32F_AFIO;
#endif
io->mem = mem;
io->max = size - 1;
io->idx = 0;
io->cnt = 0;
stm32f_gpio_mode(I2C1_SCL, ALT_FUNC, OPEN_DRAIN);
stm32f_gpio_mode(I2C1_SDA, ALT_FUNC, OPEN_DRAIN);
#if defined(STM32F1X)
/* Use alternate pins for I2C1 */
afio->mapr |= AFIO_I2C1_REMAP;
#endif
#if defined(STM32F4X)
stm32f_gpio_af(I2C1_SCL, GPIO_AF4);
stm32f_gpio_af(I2C1_SDA, GPIO_AF4);
#endif
/* Enable I2C clock */
rcc->apb1enr |= RCC_I2C1EN;
i2c->cr1 = I2C_SWRST;
DCC_LOG3(LOG_TRACE, "CR1=0x%04x CR2=0x%04x CCR=0x%04x",
i2c->cr1, i2c->cr2, i2c->ccr);
DCC_LOG3(LOG_TRACE, "OAR1=0x%04x OAR2=0x%04x TRISE=0X%04X",
i2c->oar1, i2c->oar2, i2c->trise);
DCC_LOG2(LOG_TRACE, "SR1=0x%04x SR2=0x%04x ", i2c->sr1, i2c->sr2);
i2c->cr1 = 0;
/* I2C Control register 2 (I2C_CR2) */
i2c->cr2 = I2C_FREQ_SET(pclk / 1000000);
/* I2C Own address register 1 (I2C_OAR1) */
i2c->oar1 = addr << 1;
/* I2C Own address register 2 (I2C_OAR2) */
i2c->oar2 = addr << 1;
/* I2C Clock control register (I2C_CCR) */
i2c->ccr = I2C_CCR_SET(pclk / scl_freq / 2);
/* I2C TRISE register (I2C_TRISE) */
i2c->trise = I2C_TRISE_SET((pclk / 1000000) + 1);
i2c_io.event = thinkos_ev_alloc();
return io;
}
static int fpb_probe(jtag_tap_t * tap, armv7m_fpb_ctrl_t * fpb,
uint32_t addr)
{
uint32_t fp_ctrl;
fpb->base = addr;
DCC_LOG1(LOG_TRACE, "FPB @ 0x%08x", fpb->base);
if (jtag_mem_ap_rd32(tap, fpb->base + FP_CTRL_OFFS,
&fp_ctrl) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
DCC_LOG2(LOG_TRACE, "FPB: code=%d lit=%d", FP_NUM_CODE(fp_ctrl),
FP_NUM_LIT(fp_ctrl));
fpb->code_max = FP_NUM_CODE(fp_ctrl);
fpb->lit_max = FP_NUM_LIT(fp_ctrl);
fpb->code_base = fpb->base + FP_COMP0_OFFS;
fpb->lit_base = fpb->code_base + 4 * fpb->code_max;
fpb->code_bmp = 0;
fpb->lit_bmp = 0;
return ICE_OK;
}
/* simulate a relay module */
void module_sim_relay(struct microjs_vm * vm, struct slcdev_sim * sim,
struct ss_device * dev, struct db_dev_model * model)
{
uint32_t ctl = sim->ctls;
DCC_LOG2(LOG_INFO, "addr=%d ctl=0x%04x", dev->addr, ctl);
module_contorl_seq(dev, ctl);
if ((ctl & 0x81) == 0) {
/* 1. Bit 10 = 0, sent two consecutive times,
will reset PW3 latches */
DCC_LOG(LOG_INFO, "Reset PW3 latches.");
}
switch (ctl & CONTROL_OUT_MSK) {
case CONTROL_OUT_ON:
DCC_LOG(LOG_INFO, "Set");
dev->pw2 = device_db_pw_lookup(model->pw2lst, 2);
break;
case CONTROL_OUT_OFF:
DCC_LOG(LOG_INFO, "Reset");
dev->pw2 = device_db_pw_lookup(model->pw2lst, 1);
break;
}
module_pw4_default(dev, model);
}
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;
}
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 stm32l1_flash_erase(cm3ice_ctrl_t * ctrl,
ice_mem_ref_t * mem,
ice_size_t len)
{
uint32_t pecr;
uint32_t addr;
addr = mem->base + mem->offs;
DCC_LOG2(LOG_TRACE, "addr=0x%08x len=%d", addr, len);
cm3ice_rd32(ctrl, STM32F_BASE_FLASH + FLASH_PECR, &pecr);
DCC_LOG1(LOG_TRACE, "PECR=0x%08x", pecr);
if (pecr & PRGLOCK) {
DCC_LOG(LOG_TRACE, "unlocking flash...");
if (pecr & PELOCK) {
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_PEKEYR, PEKEY1);
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_PEKEYR, PEKEY2);
}
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_PRGKEYR, PRGKEYR1);
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_PRGKEYR, PRGKEYR2);
}
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_PECR, ERASE | PROG);
cm3ice_wr32(ctrl, addr, 0x00000000);
if (stm32l1xx_flash_bsy_wait(ctrl) < 0)
len = -1;
cm3ice_wr32(ctrl, STM32F_BASE_FLASH + FLASH_PECR, 0);
return len;
}
int net_recv(void * buf, int len)
{
void * pkt;
pkt = pktbuf_alloc();
if (pkt == NULL) {
DCC_LOG(LOG_ERROR, "pktbuf_alloc() failed!");
printf("%s(): pktbuf_alloc() failed!\n", __func__);
return -1;
}
len = rs485_pkt_receive(&link, &pkt, len);
// printf("%s(): len=%d\n", __func__, 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;
}
int i2c_read(unsigned int addr, unsigned int reg, void * buf, int n)
{
uint8_t pkt[1];
int ret;
DCC_LOG2(LOG_TRACE, "addr=0x%02x reg=%d", addr, reg);
thinkos_mutex_lock(i2c_mutex);
pkt[0] = reg;
if ((ret = i2c_master_wr(addr, pkt, 1)) > 0) {
if ((ret = i2c_master_rd(addr, buf, n)) > 0) {
DCC_LOG(LOG_INFO, "ok.");
} else {
DCC_LOG(LOG_WARNING, "i2c_master_rd() failed!");
}
} else {
DCC_LOG(LOG_WARNING, "i2c_master_wr() failed!");
}
thinkos_mutex_unlock(i2c_mutex);
return ret;
}
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;
}
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 int core_read_memory(bfd_vma addr, uint8_t * buf,
unsigned int len, struct disassemble_info *dinfo)
{
cm3ice_ctrl_t * ctrl = (cm3ice_ctrl_t *)dinfo->application_data;
uint16_t data[len / sizeof(uint16_t)];
unsigned int pos;
DCC_LOG2(LOG_INFO, "addr=0x%08x len=%d", addr, len);
for (pos = 0; pos < len / 2; pos++) {
if (cm3ice_rd16(ctrl, addr, &data[pos]) != ICE_OK) {
DCC_LOG(LOG_WARNING, "ice_rd16(), fail");
return -1;
}
}
if (dp_stickyerr_get(ctrl->tap)) {
DCC_LOG1(LOG_WARNING, "addr=0x%08x, STICKYERR flag set!", addr);
dp_stickyerr_clr(ctrl->tap);
return -1;
}
memcpy(buf, data, len);
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;
}
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 mod_mem_on_load(struct ice_mem_entry * mem, int mod_id)
{
int id;
DCC_LOG2(LOG_INFO, "mem=0x%p mod_id=%d", mem, mod_id);
for(id = 0; mem[id].op != NULL; ++id) {
uint32_t addr;
#if 0
uint32_t size;
if ((size = mem[id].blk.count * mem[id].blk.size) == 0)
continue; /* skip empty blocks */
#endif
addr = mem[id].addr.base + mem[id].addr.offs;
(void)addr;
DCC_LOG3(LOG_INFO, "mem=\"%s\" id=%d addr=0x%08x",
mem[id].name, id, addr);
if (var_global_add(mod_id, mem[id].name, TYPE_UINT32, id) < 0) {
DCC_LOG(LOG_WARNING, "var_global_add() fail!");
break;
}
}
return 0;
}
