static void do_icount(void)
{
	struct serial_icounter_struct icount;
	int fd;

	fd = device_open(opt_device1, O_RDONLY, 0);

	if (ioctl(fd, TIOCGICOUNT, &icount)) {
		pr_error("Failed to get icount: %s\n", strerror(errno));
		exit(-1);
	}

	printf("cts         = %d\n", icount.cts);
	printf("dsr         = %d\n", icount.dsr);
	printf("rng         = %d\n", icount.rng);
	printf("dcd         = %d\n", icount.dcd);
	printf("rx          = %d\n", icount.rx);
	printf("tx          = %d\n", icount.tx);
	printf("frame       = %d\n", icount.frame);
	printf("overrun     = %d\n", icount.overrun);
	printf("parity      = %d\n", icount.parity);
	printf("brk         = %d\n", icount.brk);
	printf("buf_overrun = %d\n", icount.buf_overrun);

	close(fd);
	exit(0);
}

void __assert_fail(/*const char* __assertion, const char* __file, 
    unsigned int __line, const char* __function*/)
{
    /*pr_error(LOG_MODULE_MAIN, "assert failed (%s) in func %s, in %s:%u", __assertion, __function, _file, _line);*/
    pr_error(LOG_MODULE_MAIN, "assert failed");
    panic(E_OS_ASSERT_FAIL);
}

extern void rc_error(char *s) {
	pr_error(s, -1);
	set(FALSE);
	redirq = NULL;
	cond = FALSE; /* no longer inside conditional */
	rc_raise(eError);
}

void tboot_config_keywords(void)
{
	int i;

	if (!tboot_config) {
		pr_error("tboot config wasn't init.\n");
		return;
	}

	fastboot_info("\n");
	fastboot_info("Keyword             Current value\n");
	fastboot_info("---------------------------------\n");
	for (i = 0; tc_keys[i]; i++) {
		char *buf;
		int len = 0;
		int left = 0;
		if (asprintf(&buf, "%-20s%s\n", tc_keys[i],
					tboot_config_get(tc_keys[i])) == -1)
			continue;
		fastboot_info(buf);
		free(buf);
	}
	fastboot_info("\n");
	fastboot_okay("");
}

static int usbtr_recv(transport_t tr_base, uint8_t *databuf, int max_len)
{
	struct olimex_transport *tr = (struct olimex_transport *)tr_base;
	int rlen;

#ifdef DEBUG_OLIMEX
	printc(__FILE__": %s : read max %d\n", __FUNCTION__, max_len);
#endif

	rlen = usb_bulk_read(tr->handle, tr->in_ep, (char *)databuf,
			     max_len, TIMEOUT);

#ifdef DEBUG_OLIMEX
	printc(__FILE__": %s : read %d\n", __FUNCTION__, rlen);
#endif

	if (rlen < 0) {
		pr_error(__FILE__": can't receive data");
		return -1;
	}

#ifdef DEBUG_OLIMEX
	debug_hexdump(__FILE__": USB transfer in", databuf, rlen);
#endif

	return rlen;
}

void default_msg_handler(struct cfw_message *msg, void *data)
{
	pr_error(LOG_MODULE_CFW, "Bug: %s should not be called data: %p",
		 __func__,
		 data);
	cfw_dump_message(msg);
}

static int hexout_write(FILE *out, int len, uint16_t addr,
			const uint8_t *payload)
{
	int i;
	int cksum = 0;

	if (fprintf(out, ":%02X%04X00", len, addr) < 0)
		goto fail;
	cksum += len;
	cksum += addr & 0xff;
	cksum += addr >> 8;

	for (i = 0; i < len; i++) {
		if (fprintf(out, "%02X", payload[i]) < 0)
			goto fail;
		cksum += payload[i];
	}

	if (fprintf(out, "%02X\n", ~(cksum - 1) & 0xff) < 0)
		goto fail;

	return 0;

fail:
	pr_error("hexout: can't write HEX data");
	return -1;
}

bool BLECentral::begin(void)
{
    bool retval = BLECentralRole::instance()->begin();
    if (!retval)
    {
        pr_error(LOG_MODULE_BLE,"%s: Intit failed", __FUNCTION__);
        return false;
    }
    
    // Start scan
    const bt_le_scan_param_t *scan_param = BLECentralRole::instance()->getScanParam();
    bt_le_scan_param_t zero_param;
    memset(&zero_param, 0x00, sizeof (zero_param));
    if (0 == memcmp(&zero_param, scan_param, sizeof (zero_param)))
    {
        // Not set the scan parameter.
        //  Use the default scan parameter to scan
        zero_param.type         = BT_HCI_LE_SCAN_ACTIVE;
        zero_param.filter_dup   = BT_HCI_LE_SCAN_FILTER_DUP_ENABLE;
        zero_param.interval     = BT_GAP_SCAN_FAST_INTERVAL;//BT_GAP_SCAN_SLOW_INTERVAL_1;//
        zero_param.window       = BT_GAP_SCAN_FAST_WINDOW; //BT_GAP_SCAN_SLOW_WINDOW_1;//
        retval = BLECentralRole::instance()->startScan(zero_param);
    }
    else
    {
        retval = BLECentralRole::instance()->startScan();
    }
    return retval;
}

static int configure_port(struct cp210x_transport *tr, int baud_rate)
{
	int ret;

	ret = usb_control_msg(tr->handle, CP210x_REQTYPE_HOST_TO_DEVICE,
			      CP210X_IFC_ENABLE, 0x1, 0, NULL, 0, 300);
#ifdef DEBUG_CP210X
	printc("%s: %s: Sending control message "
		"CP210x_REQTYPE_HOST_TO_DEVICE, ret = %d\n",
	       __FILE__, __FUNCTION__, ret);
#endif
	if (ret < 0) {
		pr_error(__FILE__": can't enable CP210x UART");
		return -1;
	}

	/* Set the baud rate to 500000 bps */
	ret = usb_control_msg(tr->handle, CP210x_REQTYPE_HOST_TO_DEVICE,
			      CP210X_SET_BAUDDIV, CP210X_CLOCK / baud_rate,
			      0, NULL, 0, 300);
#ifdef DEBUG_CP210X
	printc("%s: %s: Sending control message "
		"CP210X_SET_BAUDDIV, ret = %d\n",
	       __FILE__, __FUNCTION__, ret);
#endif
	if (ret < 0) {
		pr_error(__FILE__": can't set baud rate");
		return -1;
	}

	/* Set the modem control settings.
	 * Clear RTS, DTR and WRITE_DTR, WRITE_RTS
	 */
	ret = usb_control_msg(tr->handle, CP210x_REQTYPE_HOST_TO_DEVICE,
			      CP210X_SET_MHS, 0x303, 0, NULL, 0, 300);
#ifdef DEBUG_CP210X
	printc("%s: %s: Sending control message "
		"CP210X_SET_MHS, ret %d\n",
	       __FILE__, __FUNCTION__, ret);
#endif
	if (ret < 0) {
		pr_error(__FILE__": can't set modem control");
		return -1;
	}

	return 0;
}

static struct msg *msg_get(void)
{
	static unsigned int timeout = RX_TIMEOUT_INIT;
	struct timespec abstime;
	struct msg *msg;
	int error;

	pthread_mutex_lock(&list_mutex);

	msg = list_head;
	if (!msg) {
		pr_debug("No more messages, waiting...\n");

		if (clock_gettime(CLOCK_REALTIME, &abstime)) {
			pr_error("Failed to get clock: %s\n", strerror(errno));
			exit(-1);
		}
		abstime.tv_sec += timeout;

		do {
			error = pthread_cond_timedwait(&list_nonempty,
						       &list_mutex, &abstime);
			if (error == ETIMEDOUT) {
				pr_error("Timeout, aborting\n");
				print_stats();
				exit(-1);
			}

			msg = list_head;
		} while (!msg);
	}

	list_head = msg->next;
	msg->next = NULL;
	if (msg == list_tail)
		list_tail = NULL;
	list_size--;

	pthread_mutex_unlock(&list_mutex);

	pthread_cond_signal(&list_full);

	timeout = RX_TIMEOUT;

	return msg;
}

/**@brief Function to initialize Battery Service.
 *
 * @details Internal variable and structures are initialized, service and
 *          port handles allocated by the framework. Battery service registers
 *          to the framework.
 * @param[in]  batt_svc_queue Queue of messages to be exchanged.
 * @param[in]  service_id Battery service ID.
 */
static void bs_init(int service_id, T_QUEUE batt_svc_queue)
{
	battery_service.service_id = service_id;

	queue = batt_svc_queue;

	if (!charging_sm_init(batt_svc_queue, bs_evt_from_ch))
		pr_error(LOG_MODULE_BS, "Battery service initialization failed");
}

static int feed_section(struct elf32_info *info,
			FILE *in, const Elf32_Shdr *sh,
			binfile_imgcb_t cb, void *user_data)
{
	uint32_t offset = sh->sh_offset;
	uint32_t size = sh->sh_size;
	uint8_t buf[1024];
	uint32_t addr = file_to_phys(info, offset);
	const char *name = NULL;

	if (fseek(in, offset, SEEK_SET) < 0) {
		pr_error("elf32: can't seek to section");
		return -1;
	}

	if (info->string_tab &&
	    sh->sh_name < info->string_len)
		name = info->string_tab + sh->sh_name;

	while (size) {
		int ask = size > sizeof(buf) ? sizeof(buf) : size;
		int len = fread(buf, 1, ask, in);
		struct binfile_chunk ch = {0};

		if (len < 0) {
			pr_error("elf32: can't read section");
			return -1;
		}

		ch.name = name;
		ch.addr = addr;
		ch.data = buf;
		ch.len = len;

		if (cb(user_data, &ch) < 0)
			return -1;

		size -= len;
		offset += len;
		addr += len;
	}

	return 0;
}

int shw_i2c_io_init(void)
{
	if (i2c_init_bus(&i2c_io_bus) < 0) {
		pr_error("I2C I/O init failed for bus %s\n", i2c_io_bus.name);
		return -1;
	}

	pr_debug("I2C I/O init success for bus %s\n", i2c_io_bus.name);
	return 0;
}

struct cfw_message * cfw_clone_message(struct cfw_message * msg) {
    struct cfw_message * ret = (struct cfw_message *)
            message_alloc(CFW_MESSAGE_LEN(msg), NULL);
    if (ret == NULL) {
        pr_error(LOG_MODULE_CFW, "%s: Error allocating message", __func__);
    } else {
		memcpy(ret, msg, CFW_MESSAGE_LEN(msg));
    }
    return ret;
}

static void cfw_message_handler(struct cfw_message *msg, void *param)
{
	DRIVER_API_RC ret = DRV_RC_FAIL;
	static uint16_t adc_value;

	switch (CFW_MESSAGE_ID(msg)) {
	case MSG_ID_GPIO_SERVICE_CONFIGURE_RSP:
	case MSG_ID_GPIO_SERVICE_SET_STATE_RSP:
		break;
	case MSG_ID_ADC_SERVICE_SUBSCRIBE_RSP:
		pr_debug(LOG_MODULE_MAIN, "ADC subscribe Response");
		adc_svc_client =
			((adc_service_subscribe_rsp_msg_t *)msg)->
			adc_sub_conn_handle;
		ret = ((adc_service_subscribe_rsp_msg_t *)msg)->status;
		if (DRV_RC_OK != ret) {
			pr_error(LOG_MODULE_MAIN, "ADC subscribe fails %d", ret);
			panic(-1);
		}
		break;
	case MSG_ID_ADC_SERVICE_GET_VAL_EVT:
		ret = ((adc_service_get_evt_msg_t *)msg)->status;
		if (DRV_RC_OK == ret) {
			adc_value =
				((adc_service_get_evt_msg_t *)msg)->adc_value;
			pr_debug(LOG_MODULE_MAIN, "adc_value %d", adc_value);
			if (adc_value < LOWER_LIMIT)
				blink_freq = LED_BLINK_ONE_SEC;
			else if (adc_value > HIGHER_LIMIT)
				blink_freq = LED_BLINK_500_MS;
			else
				blink_freq = LED_BLINK_TWO_SEC;
		} else {
			pr_error(LOG_MODULE_MAIN, "ADC get value fails %d",
				 ret);
		}
		break;
	default:
		pr_info(LOG_MODULE_MAIN, "Discarded message: %x",
			CFW_MESSAGE_ID(msg));
	}
	cfw_msg_free(msg);
}

void startFileWriterTask( int priority )
{
        g_sampleRecordQueue = xQueueCreate(SAMPLE_RECORD_QUEUE_SIZE,
                                           sizeof( ChannelSample *));
        if (NULL == g_sampleRecordQueue) {
                pr_error("file: sampleRecordQueue err\r\n");
                return;
        }

        g_logfile = (FIL *) pvPortMalloc(sizeof(FIL));
        if (NULL == g_logfile) {
                pr_error("file: logfile sruct alloc err\r\n");
                return;
        }
        memset(g_logfile, 0, sizeof(FIL));

        xTaskCreate( fileWriterTask,( signed portCHAR * ) "fileWriter",
                     FILE_WRITER_STACK_SIZE, NULL, priority, NULL );
}

int libwr_cfg_read_verify_file(char *dotconfig, char *kconfig)
{
	FILE *f;
	int errors = 0;
	char s[256], name[256];
	struct cfg_item *c;

	struct kc {
		char *name;
		struct kc *next;
	} *all_configs = NULL, *kc, *next;

	if (libwr_cfg_read_file(dotconfig))
		return -1;

	/* Read Kconfig and store all config names */
	f = fopen(kconfig, "r");
	if (!f)
		return -1;
	while (fgets(s, sizeof(s), f)) {
		if (sscanf(s, "config %s", name) != 1)
			continue;
		kc = malloc(sizeof(*kc));
		if (!kc)
			return -1;
		kc->name = strdup(name);
		kc->next = all_configs;
		all_configs = kc;
	}
	fclose(f);

	/* Verify all configs, complain if missing */
	for (c = libwr_cfg; c; c = c->next) {
		for (kc = all_configs; kc; kc = kc->next)
			if (!strcmp(c->name, kc->name))
				break;
		if (!kc) {
			pr_error("Configuration \"%s\" not found\n",
				c->name);
			errors++;
		}
	}

	/* Free all kconfig allocs */
	for (kc = all_configs; kc; kc = next) {
		next = kc->next;
		free(kc->name);
		free(kc);
	}
	if (errors) {
		errno = EINVAL;
		return -1;
	}
	return 0;
}

uint8_t BLECharacteristicImp::discoverResponseProc(bt_conn_t *conn,
                                                   const bt_gatt_attr_t *attr,
                                                   bt_gatt_discover_params_t *params)
{
    const bt_addr_le_t* dst_addr = bt_conn_get_dst(conn);
    BLEDevice device(dst_addr);
    uint8_t retVal = BT_GATT_ITER_STOP;
    
    pr_debug(LOG_MODULE_BLE, "%s-%d: type-%d", __FUNCTION__, __LINE__, params->type);

    // Process the service
    switch (params->type)
    {
        case BT_GATT_DISCOVER_DESCRIPTOR:
        {
            if (NULL != attr)
            {
                retVal = BT_GATT_ITER_CONTINUE;
                const bt_uuid_t* desc_uuid = attr->uuid;
                uint16_t desc_handle = attr->handle;
    pr_debug(LOG_MODULE_BLE, "%s-%d:handle-%d:%d", __FUNCTION__, __LINE__,attr->handle, desc_handle);
                if (isClientCharacteristicConfigurationDescriptor(desc_uuid))
                {
                    setCCCDHandle(desc_handle);
                }
                else if (bt_uuid_cmp(BLEServiceImp::getPrimayUuid(), desc_uuid) == 0 ||
                         bt_uuid_cmp(getCharacteristicAttributeUuid(), desc_uuid) == 0 )
                {
                    retVal = BT_GATT_ITER_STOP;
                }
                else
                {
                    int retval = (int)addDescriptor(desc_uuid,
                                                    attr->perm,
                                                    desc_handle);
                    
                    if (BLE_STATUS_SUCCESS != retval)
                    {
                        pr_error(LOG_MODULE_BLE, "%s-%d: Error-%d", 
                                 __FUNCTION__, __LINE__, retval);
                        errno = ENOMEM;
                        retVal = BT_GATT_ITER_STOP;
                    }
                    
                }
            }
            break;
        }
        default:
        {
            break;
        }
    }
    return retVal;
}

static int open_device(struct bslhid_transport *tr, struct usb_device *dev)
{
	if (find_interface(tr, dev) < 0)
		return -1;

	printc_dbg("Opening interface %d (config %d)...\n",
		   tr->int_number, tr->cfg_number);

	if (find_endpoints(tr, dev) < 0)
		return -1;

	printc_dbg("Found endpoints: IN: 0x%02x, OUT: 0x%02x\n",
		   tr->in_ep, tr->out_ep);

	tr->handle = usb_open(dev);
	if (!tr->handle) {
		pr_error("bslhid: can't open device");
		return -1;
	}

#ifdef __Windows__
	if (usb_set_configuration(tr->handle, tr->cfg_number) < 0)
		pr_error("warning: bslhid: can't set configuration");
#endif

#ifdef __linux__
	if (usb_detach_kernel_driver_np(tr->handle, tr->int_number) < 0)
		pr_error("warning: bslhid: can't detach kernel driver");
#endif

	if (usb_claim_interface(tr->handle, tr->int_number) < 0) {
		pr_error("bslhid: can't claim interface");
		usb_close(tr->handle);
		return -1;
	}

	/* Save the bus path for a future suspend/resume */
	strncpy(tr->bus_name, dev->bus->dirname, sizeof(tr->bus_name));
	tr->bus_name[sizeof(tr->bus_name) - 1] = 0;

	return 0;
}

static void aml_pcmcia_work(struct work_struct *work)
{
	int cd1, cd2;
	struct aml_pcmcia *pc = container_of(work, struct aml_pcmcia, pcmcia_work);

	cd1 = pc->get_cd1();
	cd2 = pc->get_cd2();
	
	if(cd1 != cd2) {
		pr_error("CAM card not inerted correctly or CAM card not supported.\n");
	} else {
		if (!cd1) {
			pr_error("Adapter(%d) Slot(0): CAM Plugin\n", 0);
			pcmcia_plugin(pc);
		} else {
			pr_error("Adapter(%d) Slot(0): CAM Unplug\n", 0);
			pcmcia_unplug(pc);
		}
	}
}