int cm3ice_status(cm3ice_ctrl_t * ctrl)
{
jtag_tap_t * tap = ctrl->tap;
uint32_t dhcsr;
DCC_LOG(LOG_MSG, "1.");
if (ctrl->polling) {
DCC_LOG(LOG_MSG, "2.");
dhcsr = ctrl->dhcsr;
} else {
DCC_LOG(LOG_MSG, "3.");
// ctrl->jtag_lock = true;
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR,
&dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
ctrl->jtag_lock = false;
return ICE_ST_FAULT;
}
// ctrl->jtag_lock = false;
}
DCC_LOG5(LOG_INFO, "S_RESET_ST=%d S_RETIRE_ST=%d S_LOCKUP=%d "\
"S_SLEEP=%d S_HALT=%d", (dhcsr & DHCSR_S_RESET_ST) ? 1 : 0,
(dhcsr & DHCSR_S_RETIRE_ST) ? 1 : 0,
(dhcsr & DHCSR_S_LOCKUP) ? 1 : 0,
(dhcsr & DHCSR_S_SLEEP) ? 1 : 0,
(dhcsr & DHCSR_S_HALT) ? 1 : 0);
return (dhcsr & DHCSR_S_HALT) ? ICE_ST_HALT : 0;
}
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
}
}
}
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 cm3ice_system_reset(cm3ice_ctrl_t * ctrl)
{
jtag_tap_t * tap = ctrl->tap;
uint32_t dhcsr;
bool halt = false;
DCC_LOG(LOG_TRACE, ".");
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR, &dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
if (dhcsr & DHCSR_S_HALT) {
DCC_LOG(LOG_TRACE, "core halted");
halt = true;
}
if (jtag_mem_ap_wr32(tap, ARMV7M_AIRCR, AIRCR_VECTKEY |
AIRCR_SYSRESETREQ ) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
if (!halt)
return ICE_OK;
do {
DCC_LOG(LOG_TRACE, "halt 2. ...");
/* halt the core */
if (jtag_mem_ap_wr32(tap, ARMV7M_DHCSR, DHCSR_DBGKEY | DHCSR_C_HALT |
DHCSR_C_DEBUGEN) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR,
&dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
DCC_LOG5(LOG_TRACE, "S_RESET_ST=%d S_RETIRE_ST=%d S_LOCKUP=%d "\
"S_SLEEP=%d S_HALT=%d", (dhcsr & DHCSR_S_RESET_ST) ? 1 : 0,
(dhcsr & DHCSR_S_RETIRE_ST) ? 1 : 0,
(dhcsr & DHCSR_S_LOCKUP) ? 1 : 0,
(dhcsr & DHCSR_S_SLEEP) ? 1 : 0,
(dhcsr & DHCSR_S_HALT) ? 1 : 0);
} while ((dhcsr & DHCSR_S_HALT) == 0);
return ICE_OK;
}
int cm3ice_poll(cm3ice_ctrl_t * ctrl, ice_comm_t * comm)
{
jtag_tap_t * tap = ctrl->tap;
uint32_t dhcsr;
int ret;
// uint32_t dfsr;
#if 0
if (jtag_mem_ap_rd32(tap, ARMV7M_DFSR, &dfsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
ctrl->polling = false;
return ICE_ST_FAULT;
}
DCC_LOG5(LOG_TRACE, " EXTERNAL=%d VCATCH=%d DWTTRAP=%d BKPT=%d HALTED=%d",
(dfsr & DFSR_EXTERNAL) ? 1: 0, (dfsr & DFSR_VCATCH) ? 1: 0,
(dfsr & DFSR_DWTTRAP) ? 1: 0, (dfsr & DFSR_BKPT) ? 1: 0,
(dfsr & DFSR_HALTED) ? 1: 0);
#endif
DCC_LOG(LOG_TRACE, "cm3ice_comm_sync()...");
if ((ret = cm3ice_comm_sync(ctrl, comm)) != 0) {
DCC_LOG(LOG_WARNING, "cm3ice_comm_sync() failed!");
ctrl->polling = false;
return ret;
}
while (ctrl->poll_enabled) {
if ((ret = cm3ice_comm_poll(ctrl, comm)) != 0) {
DCC_LOG(LOG_WARNING, "cm3ice_comm_poll() failed!");
ctrl->polling = false;
return ret;
}
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR,
&dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
ctrl->polling = false;
return ICE_ST_FAULT;
}
ctrl->dhcsr = dhcsr;
if (dhcsr & DHCSR_S_HALT)
break;
}
ctrl->polling = false;
return (dhcsr & DHCSR_S_HALT) ? ICE_ST_HALT : 0;
}
void usb_cdc_dump_stat(usb_cdc_class_t * cl)
{
struct usb_cdc_acm_dev * dev = (struct usb_cdc_acm_dev *)cl;
struct thinkos_rt rt;
(void)dev;
thinkos_rt_snapshot(&rt);
DCC_LOG5(LOG_INFO, "ep=%d wr=%d blk=%d TX_DONE(%d)=%d",
tx_ep, tx_wr, tx_blk, TX_DONE,
thinkos_flag_val(TX_DONE));
thinkos_trace_rt(&rt);
}
int usb_cdc_read(usb_cdc_class_t * cl, void * buf,
unsigned int len, unsigned int msec)
{
struct usb_cdc_acm_dev * dev = (struct usb_cdc_acm_dev *)cl;
int ret;
int n;
DCC_LOG2(LOG_INFO, "len=%d msec=%d", len, msec);
if ((n = dev->rx_cnt - dev->rx_pos) > 0) {
DCC_LOG(LOG_INFO, "read from intern buffer");
goto read_from_buffer;
};
usb_dev_ep_nak(dev->usb, dev->out_ep, false);
if ((ret = thinkos_sem_timedwait(RX_SEM, msec)) < 0) {
if (ret == THINKOS_ETIMEDOUT) {
DCC_LOG(LOG_INFO, "timeout!!");
}
return ret;
}
if (len >= CDC_EP_IN_MAX_PKT_SIZE) {
n = usb_dev_ep_pkt_recv(dev->usb, dev->out_ep, buf, len);
DCC_LOG1(LOG_INFO, "wakeup, pkt rcv extern buffer: %d bytes", n);
return n;
}
n = usb_dev_ep_pkt_recv(dev->usb, dev->out_ep,
dev->rx_buf, CDC_EP_IN_MAX_PKT_SIZE);
DCC_LOG1(LOG_INFO, "wakeup, pkt rcv intern buffer: %d bytes", n);
{
char * s = (char *)dev->rx_buf;
(void)s;
if (n == 1)
DCC_LOG1(LOG_INFO, "%02x", s[0]);
else if (n == 2)
DCC_LOG2(LOG_INFO, "%02x %02x", s[0], s[1]);
else if (n == 3)
DCC_LOG3(LOG_INFO, "%02x %02x %02x", s[0], s[1], s[2]);
else if (n == 4)
DCC_LOG4(LOG_INFO, "%02x %02x %02x %02x",
s[0], s[1], s[2], s[3]);
else if (n == 5)
DCC_LOG5(LOG_INFO, "%02x %02x %02x %02x %02x",
s[0], s[1], s[2], s[3], s[4]);
else if (n == 6)
DCC_LOG6(LOG_INFO, "%02x %02x %02x %02x %02x %02x",
s[0], s[1], s[2], s[3], s[4], s[5]);
else if (n == 7)
DCC_LOG7(LOG_INFO, "%02x %02x %02x %02x %02x %02x %02x",
s[0], s[1], s[2], s[3], s[4], s[5], s[6]);
else if (n == 8)
DCC_LOG8(LOG_INFO, "%02x %02x %02x %02x %02x %02x %02x %02x",
s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7]);
else
DCC_LOG8(LOG_INFO, "%02x %02x %02x %02x %02x %02x %02x %02x ...",
s[0], s[1], s[2], s[3], s[4], s[5], s[6], s[7]);
}
dev->rx_pos = 0;
dev->rx_cnt = n;
read_from_buffer:
DCC_LOG(LOG_INFO, "reading from buffer");
/* get data from the rx buffer if not empty */
n = MIN(n, len);
memcpy(buf, &dev->rx_buf[dev->rx_pos], n);
dev->rx_pos += n;
return n;
#if 0
{
int rem;
uint8_t * cp = (uint8_t *)buf;
rem = n;
while (rem > 4) {
DCC_LOG4(LOG_INFO, "%02x %02x %02x %02x",
cp[0], cp[1], cp[2], cp[3]);
rem -= 4;
cp += 4;
}
switch (rem) {
case 3:
DCC_LOG3(LOG_INFO, "%02x %02x %02x", cp[0], cp[1], cp[2]);
break;
case 2:
DCC_LOG2(LOG_INFO, "%02x %02x", cp[0], cp[1]);
break;
case 1:
if ((*cp) >= ' ') {
DCC_LOG1(LOG_INFO, "'%c'", cp[0]);
} else {
DCC_LOG1(LOG_INFO, "%02x", cp[0]);
}
break;
}
}
#endif
}
int usb_cdc_on_setup(usb_class_t * cl, struct usb_request * req, void ** ptr) {
struct usb_cdc_acm_dev * dev = (struct usb_cdc_acm_dev *) cl;
int value = req->value;
int index = req->index;
int len = 0;
int desc;
/* Handle supported standard device request Cf
Table 9-3 in USB specification Rev 1.1 */
switch ((req->request << 8) | req->type) {
case STD_GET_DESCRIPTOR:
desc = value >> 8;
if (desc == USB_DESCRIPTOR_DEVICE) {
/* Return Device Descriptor */
*ptr = (void *)&cdc_acm_desc_dev;
len = sizeof(struct usb_descriptor_device);
DCC_LOG1(LOG_INFO, "GetDesc: Device: len=%d", len);
break;
}
if (desc == USB_DESCRIPTOR_CONFIGURATION) {
/* Return Configuration Descriptor */
*ptr = (void *)&cdc_acm_desc_cfg;
len = sizeof(struct cdc_acm_descriptor_config);
DCC_LOG1(LOG_INFO, "GetDesc: Config: len=%d", len);
break;
}
if (desc == USB_DESCRIPTOR_STRING) {
int n = value & 0xff;
DCC_LOG1(LOG_INFO, "GetDesc: String[%d]", n);
if (n < dev->strcnt) {
*ptr = (void *)dev->str[n];
len = dev->str[n][0];
}
break;
}
len = -1;
DCC_LOG1(LOG_INFO, "GetDesc: %d ?", desc);
break;
case STD_SET_ADDRESS:
DCC_LOG1(LOG_INFO, "SetAddr: %d -------- [ADDRESS]", value);
/* signal any pending threads */
// __thinkos_ev_raise(dev->rx_ev);
break;
case STD_SET_CONFIGURATION: {
DCC_LOG1(LOG_INFO, "SetCfg: %d", value);
if (value) {
dev->in_ep = usb_dev_ep_init(dev->usb, &usb_cdc_in_info, NULL, 0);
dev->out_ep = usb_dev_ep_init(dev->usb, &usb_cdc_out_info, NULL, 0);
usb_dev_ep_nak(dev->usb, dev->out_ep, true);
dev->int_ep = usb_dev_ep_init(dev->usb, &usb_cdc_int_info, NULL, 0);
} else {
usb_dev_ep_disable(dev->usb, dev->in_ep);
usb_dev_ep_disable(dev->usb, dev->out_ep);
usb_dev_ep_disable(dev->usb, dev->int_ep);
}
/* signal any pending threads */
thinkos_flag_set_i(CTL_FLAG);
DCC_LOG(LOG_INFO, "[CONFIGURED]");
break;
}
case STD_GET_CONFIGURATION:
DCC_LOG(LOG_INFO, "GetCfg");
// data = (udp->glb_stat & UDP_CONFG) ? 1 : 0;
// usb_ep0_send_word(dev, 0);
break;
case STD_GET_STATUS_INTERFACE:
DCC_LOG(LOG_INFO, "GetStIf");
// usb_ep0_send_word(dev, 0);
break;
case STD_GET_STATUS_ZERO:
DCC_LOG(LOG_INFO, "GetStZr");
// usb_ep0_send_word(dev, 0);
break;
case STD_GET_STATUS_ENDPOINT:
index &= 0x0f;
DCC_LOG1(LOG_INFO, "GetStEpt:%d", index);
#if 0
if ((udp->glb_stat & UDP_CONFG) && (index <= 3)) {
data = (udp->csr[index] & UDP_EPEDS) ? 0 : 1;
usb_ep0_send_word(dev, data);
break;
}
if ((udp->glb_stat & UDP_FADDEN) && (index == 0)) {
data = (udp->csr[index] & UDP_EPEDS) ? 0 : 1;
usb_ep0_send_word(dev, data);
break;
}
#endif
break;
case SET_LINE_CODING:
if ((dev->acm.flags & ACM_LC_SET) == 0) {
dev->acm.flags |= ACM_LC_SET;
// thinkos_flag_give_i(TX_DONE);
thinkos_flag_give_i(TX_LOCK);
}
thinkos_flag_set_i(CTL_FLAG);
DCC_LOG3(LOG_INFO, "CDC SetLn: idx=%d val=%d len=%d",
index, value, len);
memcpy(&dev->acm.lc, dev->ctr_buf, sizeof(struct cdc_line_coding));
DCC_LOG1(LOG_INFO, "dsDTERate=%d", dev->acm.lc.dwDTERate);
DCC_LOG1(LOG_INFO, "bCharFormat=%d", dev->acm.lc.bCharFormat);
DCC_LOG1(LOG_INFO, "bParityType=%d", dev->acm.lc.bParityType);
DCC_LOG1(LOG_INFO, "bDataBits=%d", dev->acm.lc.bDataBits);
break;
case GET_LINE_CODING:
DCC_LOG(LOG_INFO, "CDC GetLn");
/* Return Line Coding */
*ptr = (void *)&dev->acm.lc;
len = sizeof(struct cdc_line_coding);
break;
case SET_CONTROL_LINE_STATE:
dev->acm.control = value;
/* there might have threads waiting for
modem control line changes (DTR, RTS)
wake them up */
thinkos_flag_set_i(CTL_FLAG);
DCC_LOG3(LOG_INFO, "CDC SetCtrl: idx=%d val=%d len=%d",
index, value, len);
DCC_LOG2(LOG_INFO, "CDC_DTE_PRESENT=%d ACTIVATE_CARRIER=%d",
(value & CDC_DTE_PRESENT) ? 1 : 0,
(value & CDC_ACTIVATE_CARRIER) ? 1 : 0);
break;
default:
DCC_LOG5(LOG_INFO, "CDC t=%x r=%x v=%x i=%d l=%d",
req->type, req->request, value, index, len);
break;
}
return len;
}
int udp_sendto(struct udp_pcb * __up, void * __buf, int __len,
const struct sockaddr_in * __sin)
{
struct iphdr * ip;
struct udphdr * uh;
in_addr_t daddr;
int dport;
in_addr_t saddr;
struct route * rt;
#if (ENABLE_NET_UDP_CHECKSUM)
unsigned int sum;
#endif
uint8_t * ptr;
int mtu;
struct ifnet * ifn;
int ret;
int retry = 0;
DCC_LOG2(LOG_INFO, "<%05x> len=%d", (int)__up, __len);
if (__up == NULL) {
DCC_LOG1(LOG_WARNING, "<%05x> invalid pcb", (int)__up);
return -EFAULT;
}
if (__buf == NULL) {
DCC_LOG1(LOG_WARNING, "<%05x> invalid buffer", (int)__up);
return -EFAULT;
}
tcpip_net_lock();
#if (ENABLE_NET_SANITY_CHECK)
if (pcb_find((struct pcb *)__up, &__udp__.pcb) < 0) {
DCC_LOG1(LOG_ERROR, "<%05x> pcb_find()", (int)__up);
tcpip_net_unlock();
/* TODO: errno */
return -1;
}
#endif
if ((__up->u_lport) == 0) {
DCC_LOG1(LOG_WARNING, "<%05x> not bound", (int)__up);
tcpip_net_unlock();
/* TODO: errno */
return -3;
}
if (__sin == NULL) {
if ((dport = __up->u_fport) == 0) {
DCC_LOG1(LOG_WARNING, "<%05x> connection refused", (int)__up);
tcpip_net_unlock();
return -ECONNREFUSED;
}
if ((daddr = __up->u_faddr) == INADDR_ANY) {
DCC_LOG1(LOG_WARNING, "<%05x> not connected", (int)__up);
tcpip_net_unlock();
return -ENOTCONN;
}
} else {
if ((dport = __sin->sin_port) == 0) {
DCC_LOG1(LOG_WARNING, "<%05x> invalid port", (int)__up);
tcpip_net_unlock();
return -ECONNREFUSED;
}
if ((daddr = __sin->sin_addr.s_addr) == INADDR_ANY) {
DCC_LOG1(LOG_WARNING, "<%05x> invalid address", (int)__up);
tcpip_net_unlock();
return -EDESTADDRREQ;
}
}
if ((rt = __route_lookup(daddr)) == NULL) {
DCC_LOG2(LOG_WARNING, "<%05x> no route to host: %I", (int)__up, daddr);
tcpip_net_unlock();
UDP_PROTO_STAT_ADD(tx_drop, 1);
UDP_PROTO_STAT_ADD(tx_err, 1);
return -EHOSTUNREACH;
}
ifn = (struct ifnet *)rt->rt_ifn;
mtu = ifn->if_mtu - sizeof(struct iphdr);
if ((__len <= 0) || (__len > mtu)) {
DCC_LOG3(LOG_WARNING, "<%04x> invalid length %d (max: %d)",
(int)__up, __len, mtu);
tcpip_net_unlock();
/* TODO: errno */
return -7;
}
/* get the source address */
if ((saddr = __up->u_laddr) == INADDR_ANY) {
saddr = ifn->if_ipv4_addr;
}
again:
ip = (struct iphdr *)ifn_mmap(ifn, sizeof(struct iphdr) +
sizeof(struct udphdr) + __len);
if (ip == NULL) {
DCC_LOG1(LOG_WARNING, "<%04x> ifn_mmap() fail", (int)__up);
tcpip_net_unlock();
/* TODO: errno */
return -1;
}
DCC_LOG2(LOG_TRACE, "<%05x> ip=%p", (int)__up, ip);
iph_template(ip, IPPROTO_UDP, udp_def_ttl, udp_def_tos);
uh = (struct udphdr *)ip->opt;
/* build the ip header */
ip = mk_iphdr(ip, saddr, daddr, sizeof(struct udphdr) + __len);
/* fill the udp header fields */
uh = (struct udphdr *)ip->opt;
uh->dport = dport;
uh->sport = __up->u_lport;
uh->len = htons(__len + sizeof(struct udphdr));
#if (ENABLE_NET_UDP_CHECKSUM)
/* initialize the udp checksum */
sum = uh->len << 1;
sum += uh->dport;
sum += uh->sport;
sum += (IPPROTO_UDP << 8);
sum += ((uint16_t *)(void *)&(ip->saddr))[0] +
((uint16_t *)(void *)&(ip->saddr))[1];
sum += ((uint16_t *)(void *)&(ip->daddr))[0] +
((uint16_t *)(void *)&(ip->daddr))[1];
#endif
ptr = (uint8_t *)uh + sizeof(struct udphdr);
memcpy(ptr, __buf, __len);
#if (ENABLE_NET_UDP_CHECKSUM)
if (__len) {
sum = in_chksum(sum, ptr, __len);
}
uh->chksum = ~sum;
#else
uh->chksum = 0;
#endif
#if 0
DCC_LOG(LOG_INFO, "IP %d.%d.%d.%d:%d > %d.%d.%d.%d:%d: %d",
IP4_ADDR1(ip->saddr), IP4_ADDR2(ip->saddr), IP4_ADDR3(ip->saddr),
IP4_ADDR4(ip->saddr), ntohs(uh->sport), IP4_ADDR1(ip->daddr),
IP4_ADDR2(ip->daddr), IP4_ADDR3(ip->daddr), IP4_ADDR4(ip->daddr),
ntohs(uh->dport), ntohs(uh->len));
#endif
if ((ret = ip_output(ifn, rt, ip)) < 0) {
ifn_munmap(ifn, ip);
/* if the reason to fail was an arp failure
try query an address pending for resolution ... */
if ((ret == -EAGAIN) && (retry < 10)) {
etharp_query_pending();
tcpip_net_unlock();
DCC_LOG2(LOG_WARNING, "<%05x> again, retry=%d!", (int)__up, retry);
thinkos_sleep(10 + retry * 10);
retry++;
tcpip_net_lock();
goto again;
}
DCC_LOG1(LOG_ERROR, "<%05x> ip_output() fail!", (int)__up);
UDP_PROTO_STAT_ADD(tx_drop, 1);
} else {
UDP_PROTO_STAT_ADD(tx_ok, 1);
DCC_LOG5(LOG_INFO, "IP %I:%d > %I:%d: %d",
ip->saddr, ntohs(uh->sport), ip->daddr,
ntohs(uh->dport), ntohs(uh->len));
#if (LOG_LEVEL < LOG_INFO)
DCC_LOG(LOG_INFO, "sent.");
#endif
ret = __len;
}
tcpip_net_unlock();
return ret;
}
int cm3ice_connect(cm3ice_ctrl_t * ctrl, uint32_t idmask, uint32_t idcomp)
{
jtag_tap_t * tap = ctrl->tap;
uint32_t dhcsr;
// uint32_t dfsr;
int ret;
if (dp_stickyerr_get(tap)) {
DCC_LOG(LOG_WARNING, "JTAD DP STICKYERR flag set!");
dp_stickyerr_clr(tap);
}
DCC_LOG2(LOG_TRACE, "idmask=%08x idcomp=%08x", idmask, idcomp);
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR, &dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
DCC_LOG1(LOG_TRACE, "DHCSR: 0x%08x", dhcsr);
if (!(dhcsr & DHCSR_C_DEBUGEN)) {
/* enable debug */
if (jtag_mem_ap_wr32(tap, ARMV7M_DHCSR, DHCSR_DBGKEY |
DHCSR_C_DEBUGEN) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
}
/* enable the FPB unit */
if ((ret = fpb_enable(tap, &ctrl->fpb)) != ICE_OK) {
DCC_LOG(LOG_WARNING, "fpb_enable() failed!");
return ret;
}
if (dhcsr & DHCSR_S_HALT) {
} else {
ctrl->core.cache_bmp = 0;
}
/*
if (jtag_mem_ap_rd32(tap, ARMV7M_DFSR, &dfsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
*/
/* clear DFSR */
if (jtag_mem_ap_wr32(tap, ARMV7M_DFSR, 0x1f) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
DCC_LOG5(LOG_TRACE, "S_RESET_ST=%d S_RETIRE_ST=%d S_LOCKUP=%d "\
"S_SLEEP=%d S_HALT=%d", (dhcsr & DHCSR_S_RESET_ST) ? 1 : 0,
(dhcsr & DHCSR_S_RETIRE_ST) ? 1 : 0,
(dhcsr & DHCSR_S_LOCKUP) ? 1 : 0,
(dhcsr & DHCSR_S_SLEEP) ? 1 : 0,
(dhcsr & DHCSR_S_HALT) ? 1 : 0);
DCC_LOG5(LOG_TRACE, "S_REGRDY=%d C_MASKINTS=%d C_STEP=%d "\
"C_HALT=%d C_DEBUGEN=%d", (dhcsr & DHCSR_S_REGRDY) ? 1 : 0,
(dhcsr & DHCSR_C_MASKINTS) ? 1 : 0,
(dhcsr & DHCSR_C_STEP) ? 1 : 0,
(dhcsr & DHCSR_C_HALT) ? 1 : 0,
(dhcsr & DHCSR_C_DEBUGEN) ? 1 : 0);
if (dp_stickyerr_get(tap)) {
DCC_LOG(LOG_ERROR, "JTAD DP STICKYERR flag is set!");
return -1;
}
return 0;
}
int cm3ice_core_reset(cm3ice_ctrl_t * ctrl)
{
jtag_tap_t * tap = ctrl->tap;
uint32_t dhcsr;
uint32_t demcr;
bool halt = false;
DCC_LOG(LOG_TRACE, ".");
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR, &dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
if (jtag_mem_ap_rd32(tap, ARMV7M_DEMCR, &demcr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
if (dhcsr & DHCSR_S_HALT) {
DCC_LOG(LOG_TRACE, "core halted");
halt = true;
if ((demcr & DEMCR_VC_CORERESET) == 0) {
if (jtag_mem_ap_wr32(tap, ARMV7M_DEMCR, demcr |
DEMCR_VC_CORERESET) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
}
if ((dhcsr & DHCSR_C_DEBUGEN) == 0) {
if (jtag_mem_ap_wr32(tap, ARMV7M_DHCSR, DHCSR_DBGKEY | dhcsr |
DHCSR_C_DEBUGEN) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
}
} else {
DCC_LOG(LOG_TRACE, "halting...");
/* halt the core but leave the C_DEBUGEN set
To force the processor to enter Debug state as soon as it comes
out of reset, a debugger set DHCSR.C_DEBUGEN to 1, to enable
halting debut, and sets DEMCR.VC_CORERESET to 1 to enable vector
catch on the Reset exception. When the processor comes out
of reset it sets DHCSR.C_HALT to 1,
and enters Debug state.
*/
if (jtag_mem_ap_wr32(tap, ARMV7M_DHCSR, DHCSR_DBGKEY | DHCSR_C_HALT |
DHCSR_C_DEBUGEN) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
if (demcr & DEMCR_VC_CORERESET) {
if (jtag_mem_ap_wr32(tap, ARMV7M_DEMCR,
demcr & ~DEMCR_VC_CORERESET) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
}
halt = false;
}
/* Local reset */
if (jtag_mem_ap_wr32(tap, ARMV7M_AIRCR, AIRCR_VECTKEY | AIRCR_VECTRESET |
AIRCR_VECTCLRACTIVE) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
return ICE_OK;
do {
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR,
&dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
DCC_LOG5(LOG_TRACE, "S_RESET_ST=%d S_RETIRE_ST=%d S_LOCKUP=%d "\
"S_SLEEP=%d S_HALT=%d", (dhcsr & DHCSR_S_RESET_ST) ? 1 : 0,
(dhcsr & DHCSR_S_RETIRE_ST) ? 1 : 0,
(dhcsr & DHCSR_S_LOCKUP) ? 1 : 0,
(dhcsr & DHCSR_S_SLEEP) ? 1 : 0,
(dhcsr & DHCSR_S_HALT) ? 1 : 0);
} while ((dhcsr & DHCSR_S_RESET_ST) == 0);
if (!halt)
return ICE_OK;
do {
DCC_LOG(LOG_TRACE, "halt 2. ...");
/* halt the core */
if (jtag_mem_ap_wr32(tap, ARMV7M_DHCSR, DHCSR_DBGKEY | DHCSR_C_HALT |
DHCSR_C_DEBUGEN) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_wr32() failed!");
return ICE_ERR_JTAG;
}
if (jtag_mem_ap_rd32(tap, ARMV7M_DHCSR,
&dhcsr) != JTAG_ADI_ACK_OK_FAULT) {
DCC_LOG(LOG_WARNING, "jtag_mem_ap_rd32() failed!");
return ICE_ERR_JTAG;
}
DCC_LOG5(LOG_TRACE, "S_RESET_ST=%d S_RETIRE_ST=%d S_LOCKUP=%d "\
"S_SLEEP=%d S_HALT=%d", (dhcsr & DHCSR_S_RESET_ST) ? 1 : 0,
(dhcsr & DHCSR_S_RETIRE_ST) ? 1 : 0,
(dhcsr & DHCSR_S_LOCKUP) ? 1 : 0,
(dhcsr & DHCSR_S_SLEEP) ? 1 : 0,
(dhcsr & DHCSR_S_HALT) ? 1 : 0);
} while ((dhcsr & DHCSR_S_HALT) == 0);
return ICE_OK;
}
