uint8_t _temp_read(const temp_def *def)
{
  uint8_t val = 0;

  _temp_pin_out(def);

  for (uint8_t i = 0; i < 8; i++)
  {
    _temp_pin_down(def);

    sleep_us(2); // Pull low for minimum 1 us

    val >>= 1;

    _temp_pin_up(def);
    _temp_pin_in(def);

    sleep_us(2); // Sample within 15 us

    if (_temp_check(def))
    {
      val |= 0x80;
    }

    sleep_us(60); // Cycle lasts at least 60 us

    _temp_pin_out(def);
  }

  return val;
}

/**
 * \brief Forks a separate simple milisecond-sleep() -&- sync periodic timer
 *
 * Forks a very basic periodic timer process, that just ms-sleep()s for 
 * the specified interval and then calls the timer function.
 * The new "sync timer" process execution start immediately, the ms-sleep()
 * is called first (so the first call to the timer function will happen
 * \<interval\> seconds after the call to fork_basic_utimer)
 * @param child_id  @see fork_process()
 * @param desc      @see fork_process()
 * @param make_sock @see fork_process()
 * @param f         timer function/callback
 * @param param     parameter passed to the timer function
 * @param uinterval  interval in mili-seconds.
 * @return pid of the new process on success, -1 on error
 * (doesn't return anything in the child process)
 */
int fork_sync_utimer(int child_id, char* desc, int make_sock,
						utimer_function* f, void* param, int uinterval)
{
	int pid;
	ticks_t ts1 = 0;
	ticks_t ts2 = 0;

	pid=fork_process(child_id, desc, make_sock);
	if (pid<0) return -1;
	if (pid==0){
		/* child */
		ts2 = uinterval;
		if (cfg_child_init()) return -1;
		for(;;){
			if(ts2>0) sleep_us(uinterval);
			else sleep_us(1);
			ts1 = get_ticks_raw();
			cfg_update();
			f(TICKS_TO_MS(ts1), param); /* ticks in mili-seconds */
			ts2 = uinterval - get_ticks_raw() + ts1;
		}
	}
	/* parent */
	return pid;
}

void _temp_write(const temp_def *def, uint8_t data)
{
  for (uint8_t i = 0; i < 8; i++)
  {
    _temp_pin_down(def);

    sleep_us(2); // Pull low for minimum 1 us

    if (data & 0x01)
    {
      _temp_pin_up(def);
    }
    else
    {
      _temp_pin_down(def);
    }

    data >>= 1;

    sleep_us(60); // Cycle lasts at least 60 us

    _temp_pin_up(def);
  }

  _temp_pin_up(def);
}

static inline int reload_permanent_list(struct bl_rule *first,
					struct bl_rule *last,
					struct bl_head *head)
{
	struct bl_rule *p, *q;

	/* get list for write */
	lock_get( head->lock);
	while(head->count_write){
		lock_release( head->lock );
		sleep_us(5);
		lock_get( head->lock );
	}
	head->count_write = 1;
	while(head->count_read){
		lock_release( head->lock );
		sleep_us(5);
		lock_get( head->lock );
	}
	lock_release( head->lock );

	for(p = head->first ; p ; ){
		q = p;
		p = p->next;
		shm_free(q);
	}

	head->first = first;
	head->last = last;

	head->count_write = 0;

	return 0;
}

/**
 * \brief Forks a separate simple sleep() -&- sync periodic timer
 *
 * Forks a very basic periodic timer process, that just sleep()s for 
 * the specified interval and then calls the timer function.
 * The new "sync timer" process execution start immediately, the sleep()
 * is called first (so the first call to the timer function will happen
 * \<interval\> seconds after the call to fork_sync_timer)
 * @param child_id  @see fork_process()
 * @param desc      @see fork_process()
 * @param make_sock @see fork_process()
 * @param f         timer function/callback
 * @param param     parameter passed to the timer function
 * @param interval  interval in seconds.
 * @return pid of the new process on success, -1 on error
 * (doesn't return anything in the child process)
 */
int fork_sync_timer(int child_id, char* desc, int make_sock,
						timer_function* f, void* param, int interval)
{
	int pid;
	ticks_t ts1 = 0;
	ticks_t ts2 = 0;

	pid=fork_process(child_id, desc, make_sock);
	if (pid<0) return -1;
	if (pid==0){
		/* child */
		interval *= 1000;  /* miliseconds */
		ts2 = interval;
		if (cfg_child_init()) return -1;
		for(;;){
			if (ts2>interval)
				sleep_us(1000);    /* 1 milisecond sleep to catch up */
			else
				sleep_us(ts2*1000); /* microseconds sleep */
			ts1 = get_ticks_raw();
			cfg_update();
			f(TICKS_TO_S(ts1), param); /* ticks in sec for compatibility with old
									  timers */
			/* adjust the next sleep duration */
			ts2 = interval - TICKS_TO_MS(get_ticks_raw()) + TICKS_TO_MS(ts1);
		}
	}
	/* parent */
	return pid;
}

static inline void delete_expired(struct bl_head *elem, unsigned int ticks)
{
	struct bl_rule *p, *q;

	p = q = 0;

	/* get list for write */
	lock_get(elem->lock);
	while(elem->count_write){
		lock_release(elem->lock);
		sleep_us(5);
		lock_get(elem->lock);
	}
	elem->count_write = 1;
	while(elem->count_read){
		lock_release(elem->lock);
		sleep_us(5);
		lock_get(elem->lock);
	}
	lock_release(elem->lock);

	if(elem->first==NULL)
		goto done;

	for( q=0,p = elem->first ; p ; q=p,p=p->next) {
		if(p->expire_end > ticks)
			break;
	}

	if (q==NULL)
		/* nothing to remove */
		goto done;

	if (p==NULL) {
		/* remove everything */
		q = elem->first;
		elem->first = elem->last = NULL;
	} else {
		/* remove up to p */
		q->next = 0;
		q = elem->first;
		elem->first = p;
	}

done:
	elem->count_write = 0;

	for( ; q ; ){
		p = q;
		q = q->next;
		shm_free(p);
	}

	return;
}

void c_client::client_run_state()
{
	uchar message[UDP1_BUFFER_LONG];
	ushort *num_cmd = (ushort *)&message[2];
	int num_cmd_sent = 0, num_cmd_in = 0, num_cmd_err = 0;

	for (ushort i = 0; i < CMD_SENT; i++)
	{
		if (i % 8 == 0)
		{
			if ((i / 8) % 2 == 1)
				memcpy(message, message1_long, UDP1_BUFFER_LONG);
			else
				memcpy(message, message2_long, UDP1_BUFFER_LONG);
			*num_cmd = i;
			send_block((char *)message, UDP1_BUFFER_LONG);
			num_cmd_sent++;
			sleep_us(10000);
		}
		else
		{
			if (i % 2 == 1)
				memcpy(message, message1_short, 100);
			else
				memcpy(message, message2_short, 100);
			*num_cmd = i;
			send_block((char *)message, 100);
			num_cmd_sent++;
			sleep_us(2000);
		}
		cout_mtx.lock();
		cout << "client out cmd:" << (int) message[0] << " cmd_num=" << *num_cmd << endl;
		cout_mtx.unlock();

		int i1 = receive_block((char *)message);
		if (i1 > 0)
		{
			num_cmd_in++;
			cout_mtx.lock();
			cout << "client in cmd:" << (int)message[0] << " cmd_num=" << *num_cmd << endl;
			if ((i1 != 100) && (i1 != UDP1_BUFFER_LONG))
			{
				cout << "====== ERROR c_client received command length=" << i1 << endl;
				num_cmd_err++;
			}
			cout_mtx.unlock();
		}
	}
	cout_mtx.lock();
	cout << "===== END c_client::server_run_state() =====" << endl;
	cout << "Client sent commands=" << num_cmd_sent << " received=" << num_cmd_in << " wrong commands=" << num_cmd_err << endl;
	cout_mtx.unlock();
}

int async_task_run(int idx)
{
	async_task_t *ptask;
	int received;

	LM_DBG("async task worker %d ready\n", idx);

	for( ; ; ) {
		if(unlikely(_async_task_usleep)) sleep_us(_async_task_usleep);
		if ((received = recvfrom(_async_task_sockets[0],
							&ptask, sizeof(async_task_t*),
							0, NULL, 0)) < 0) {
			LM_ERR("failed to received task (%d: %s)\n", errno, strerror(errno));
			continue;
		}
		if(received != sizeof(async_task_t*)) {
			LM_ERR("invalid task size %d\n", received);
			continue;
		}
		if(ptask->exec!=NULL) {
			LM_DBG("task executed [%p] (%p/%p)\n", ptask,
					ptask->exec, ptask->param);
			ptask->exec(ptask->param);
		}
		shm_free(ptask);
	}

	return 0;
}

static inline struct mi_root* wait_async_reply(struct mi_handler *hdl)
{
	struct mi_root *mi_rpl;
	int i;
	int x;

	for( i=0 ; i<MAX_XMLRPC_WAIT ; i++ ) {
		if (hdl->param)
			break;
		sleep_us(1000*500);
	}

	if (i==MAX_XMLRPC_WAIT) {
		/* no more waiting ....*/
		lock_get(xr_lock);
		if (hdl->param==NULL) {
			hdl->param = XMLRPC_ASYNC_EXPIRED;
			x = 0;
		} else {
			x = 1;
		}
		lock_release(xr_lock);
		if (x==0) {
			LM_INFO("exiting before receiving reply\n");
			return NULL;
		}
	}

	mi_rpl = (struct mi_root *)hdl->param;
	if (mi_rpl==XMLRPC_ASYNC_FAILED)
		mi_rpl = NULL;

	free_async_handler(hdl);
	return mi_rpl;
}

/*
 * Initialize all loaded modules, the initialization
 * is done *AFTER* the configuration file is parsed
 */
int init_modules(void)
{
	struct sr_module* t;

	if(async_task_init()<0)
		return -1;

	for(t = modules; t; t = t->next) {
		if (t->exports.init_f) {
			if (t->exports.init_f() != 0) {
				LM_ERR("Error while initializing module %s\n", t->exports.name);
				return -1;
			}
			/* delay next module init, if configured */
			if(unlikely(modinit_delay>0))
				sleep_us(modinit_delay);
		}
		if (t->exports.response_f)
			mod_response_cbk_no++;
	}
	mod_response_cbks=pkg_malloc(mod_response_cbk_no *
									sizeof(response_function));
	if (mod_response_cbks==0){
		LM_ERR("memory allocation failure for %d response_f callbacks\n",
					mod_response_cbk_no);
		return -1;
	}
	for (t=modules, i=0; t && (i<mod_response_cbk_no); t=t->next) {
		if (t->exports.response_f) {
			mod_response_cbks[i]=t->exports.response_f;
			i++;
		}
	}
	return 0;
}

int write_to_fifo(const string& fifo, const char * buf, unsigned int len)
{
    int fd_fifo;
    int retry = SER_WRITE_TIMEOUT / SER_WRITE_INTERVAL;

    for(; retry>0; retry--) {

        if((fd_fifo = open(fifo.c_str(),
                           O_WRONLY | O_NONBLOCK)) == -1) {
            ERROR("while opening %s: %s\n",
                  fifo.c_str(),strerror(errno));

            if(retry)
                sleep_us(50000);
        }
        else {
            break;
        }
    }

    if(!retry)
        return -1;

    DBG("write_to_fifo: <%s>\n",buf);
    int l = write(fd_fifo,buf,len);
    close(fd_fifo);

    if(l==-1)
        ERROR("while writing: %s\n",strerror(errno));
    else
        DBG("Write to fifo: completed\n");

    return l;
}

static void calib(int max_cnt, s16 *avg_data) {
	int cnt = 0;
	int ax = 0, ay = 0, az = 0, gx = 0, gy = 0, gz = 0;
	while(1){
		sleep_us(10*1000);

		static s16 acc_x, acc_y, acc_z, gy_x, gy_y, gy_z;
		
		if(mouse_sensor_getdata_no_fifo(&acc_x, &acc_y, &acc_z, &gy_x, &gy_y, &gy_z)){
			ax += acc_x;
			ay += acc_y;
			az += acc_z;

			gx += gy_x;
			gy += gy_y;
			gz += gy_z;

			++cnt;
			if(cnt >= max_cnt){
				break;
			}
		}
	}
	avg_data[0] = gx / max_cnt;
	avg_data[1] = gy / max_cnt;
	avg_data[2] = gz / max_cnt;
	avg_data[3] = ax / max_cnt;
	avg_data[4] = ay / max_cnt;
	avg_data[5] = az / max_cnt;

}

void evrexec_process(evrexec_task_t *it, int idx)
{
	sip_msg_t *fmsg;
	sr_kemi_eng_t *keng = NULL;
	str sidx = STR_NULL;

	if(it!=NULL) {
		fmsg = faked_msg_next();
		set_route_type(LOCAL_ROUTE);
		if(it->wait>0) sleep_us(it->wait);
		keng = sr_kemi_eng_get();
		if(keng==NULL) {
			if(it->rtid>=0 && event_rt.rlist[it->rtid]!=NULL) {
				run_top_route(event_rt.rlist[it->rtid], fmsg, 0);
			} else {
				LM_WARN("empty event route block [%.*s]\n",
						it->ename.len, it->ename.s);
			}
		} else {
			sidx.s = int2str(idx, &sidx.len);
			if(sr_kemi_route(keng, fmsg, EVENT_ROUTE,
						&it->ename, &sidx)<0) {
				LM_ERR("error running event route kemi callback\n");
			}
		}
	}
	/* avoid exiting the process */
	while(1) { sleep(3600); }
}

void
sink_udp(int sockfd)	/* TODO: use recvfrom ?? */
{
	int		n, flags;

	if (pauseinit)
		sleep_us(pauseinit*1000);

	for ( ; ; ) {	/* read until peer closes connection; -n opt ignored */
			/* msgpeek = 0 or MSG_PEEK */
		flags = msgpeek;
	oncemore:
		if ( (n = recv(sockfd, rbuf, readlen, flags)) < 0) {
			err_sys("recv error");

		} else if (n == 0) {
			if (verbose)
				fprintf(stderr, "connection closed by peer\n");
			break;

#ifdef	notdef		/* following not possible with TCP */
		} else if (n != readlen)
			err_quit("read returned %d, expected %d", n, readlen);
#else
		}
#endif

		if (verbose) {
			fprintf(stderr, "received %d bytes%s\n", n,
					(flags == MSG_PEEK) ? " (MSG_PEEK)" : "");
			if (verbose > 1) {
	fprintf(stderr, "printing %d bytes\n", n);
				rbuf[n] = 0;	/* make certain it's null terminated */
				fprintf(stderr, "SDAP header: %lx\n", *((long *) rbuf));
				fprintf(stderr, "next long: %lx\n", *((long *) rbuf+4));
				fputs(&rbuf[8], stderr);
			}
		}

		if (pauserw)
			sleep_us(pauserw*1000);

		if (flags != 0) {
			flags = 0;		/* avoid infinite loop */
			goto oncemore;	/* read the message again */
		}
	}

static int m_usleep(struct sip_msg *msg, int *useconds)
{
	LM_DBG("sleep %d\n", *(unsigned int*)useconds);

	sleep_us(*(unsigned int*)useconds);

	return 1;
}

void lpc_eep_init(CORE* core)
{
    LPC_EEPROM->PWRDWN |= LPC_EEPROM_PWRDWN_Msk;
    //EEPROM operates on M3 clock
    LPC_EEPROM->CLKDIV = lpc_power_get_core_clock_inside() / EEP_CLK - 1;
    sleep_us(100);
    LPC_EEPROM->PWRDWN &= ~LPC_EEPROM_PWRDWN_Msk;
}

void _temp_init(const temp_def *def)
{
  // Send initialization sequence
  _temp_pin_out(def);
  _temp_pin_down(def);
  sleep_us(500); // Pull low for minimum 480 us
  _temp_pin_in(def);
  sleep_us(20); // Wait 15 to 60 us
  uint32_t presense_start = timer_now();
  while (_temp_check(def))
  {
    // Presense pulse 60 to 240 us
    if (timer_now() - presense_start > 3) break;
  }
  _temp_pin_out(def);
  _temp_pin_up(def);
  sleep_us(480); // Receive sequence is minimum 480 us
}

void rpc_mtree_match(rpc_t* rpc, void* ctx)
{
	str tname = STR_NULL;
	str tomatch = STR_NULL;
	int mode = -1;

	m_tree_t *tr;

	if(!mt_defined_trees())
	{
		rpc->fault(ctx, 500, "Empty tree list.");
		return;
	}

	if (rpc->scan(ctx, ".SSd", &tname, &tomatch, &mode) < 3) {
		rpc->fault(ctx, 500, "Invalid Parameters");
		return;
	}

	if (mode !=0 && mode != 2) {
		rpc->fault(ctx, 500, "Invalid parameter 'mode'");
		return;
	}

again:
	lock_get( mt_lock );
	if (mt_reload_flag) {
		lock_release( mt_lock );
		sleep_us(5);
		goto again;
	}
	mt_tree_refcnt++;
	lock_release( mt_lock );

	tr = mt_get_tree(&tname);
	if(tr==NULL)
	{
		/* no tree with such name*/
		rpc->fault(ctx, 404, "Not found tree");
		goto error;
	}

	if(mt_rpc_match_prefix(rpc, ctx, tr, &tomatch, mode)<0)
	{
		LM_DBG("no prefix found in [%.*s] for [%.*s]\n",
				tname.len, tname.s,
				tomatch.len, tomatch.s);
		rpc->fault(ctx, 404, "Not found");
	}

error:
	lock_get( mt_lock );
	mt_tree_refcnt--;
	lock_release( mt_lock );

}

static int m_usleep(struct sip_msg *msg, char *time, char *str2)
{
	int s;
	if(fixup_get_ivalue(msg, (gparam_t*)time, &s)!=0)
	{
		LM_ERR("cannot get time interval value\n");
		return -1;
	}
	sleep_us((unsigned int)s);
	return 1;
}

	uint16_t TTP229::ReadForEvent()
	{
		uint16_t buttonState = 0;
		
		for(uint8_t i = 0; i < keys; ++i) // sleep for some kind of slow data reading
		{
			digitalWrite(SCLport, SCLpin, LOW);
			
			sleep_us(1);
			
			digitalWrite(SCLport, SCLpin, HIGH);
			
			sleep_us(1);
			
			if(!digitalRead(SDOportPin, SDOpin))
				buttonState |= _BV(i);
		}
		
		return buttonState;
	}