C++ DCC_LOG2函数代码示例

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