Exemplo n.º 1
0
Arquivo: hciemu.c Projeto: intgr/bluez
static void hci_status_param(uint16_t ocf, int plen, uint8_t *data)
{
	read_local_amp_info_rp ai;

	const uint16_t ogf = OGF_STATUS_PARAM;

	switch (ocf) {
	case OCF_READ_LOCAL_AMP_INFO:
		memset(&ai, 0, sizeof(ai));

		/* BT only */
		ai.amp_status = 0x01;
		ai.max_pdu_size = htobl(L2CAP_DEFAULT_MTU);
		ai.controller_type = HCI_AMP;
		ai.max_amp_assoc_length = htobl(HCI_MAX_ACL_SIZE);
		/* No flushing at all */
		ai.max_flush_timeout = 0xFFFFFFFF;
		ai.best_effort_flush_timeout = 0xFFFFFFFF;

		command_complete(ogf, ocf, sizeof(ai), &ai);
		break;

	default:
		command_status(ogf, ocf, 0x01);
		break;
	}
}
Exemplo n.º 2
0
Arquivo: banner.c Projeto: grp/u8it
u8archive* U8_packdir(u8archive * arc, U8Dir dir, u32 recursion)
{
	u32 i, oldnumnodes;
	U8File curfile;
	U8Dir curdir;
	u8node * curnode;
	
	oldnumnodes = arc->numnodes; //save the old number of nodes, for updating the directory node later
	
	arc->nodes = realloc(arc->nodes, (arc->numnodes + 1) * sizeof(u8node)); //expand buffer
	curnode = &arc->nodes[arc->numnodes]; //get pointer to node to work with
	
	curnode->type = 0x0001; // Is a folder.
	
	curnode->string_offset = htobl(arc->stringsz); //save string table offset
	arc->string_table = realloc(arc->string_table, arc->stringsz + strlen(curdir.name) + 1); //expand sting buffer
	memcpy(arc->string_table, curdir.name, strlen(curdir.name) + arc->stringsz + 1); //copy over string
	arc->stringsz += strlen(curdir.name) + 1;
	
	arc->numnodes++; //we added another node
	
	for(i = 0; i < dir.filecnt; i++) {	//Handle files.
		curfile = dir.files[i];
		
		arc->nodes = realloc(arc->nodes, (arc->numnodes + 1) * sizeof(u8node)); //expand buffer
		curnode = &arc->nodes[arc->numnodes]; //get pointer to node to work with
		
		curnode->type = 0; //file
		curnode->size = htobl(curfile.size); //save file size
		curnode->data_offset = htobl(arc->datasz); //store data offset (updated later)
		
		curnode->string_offset = htobl(arc->stringsz); //save string table offset
		arc->string_table = realloc(arc->string_table, arc->stringsz + strlen(curfile.dst) + 1); //expand sting buffer
		memcpy(arc->string_table + arc->stringsz, curfile.dst, strlen(curfile.dst) + 1); //copy over string
		arc->stringsz += strlen(curfile.dst) + 1;
		
		arc->data = realloc(arc->data, arc->datasz + inlinepad(curfile.size, 32)); //expand data buffer
		memset(arc->data + arc->datasz, 0, inlinepad(curfile.size, 32)); //clear buffer
		memcpy(arc->data + arc->datasz, curfile.data, curfile.size); //copy over data
		arc->datasz += inlinepad(curfile.size, 32);
		
		arc->numnodes++; //we added another node
	}
	
	for(i = 0; i < dir.dircnt; i++) {	//Handle folders.
		curdir = dir.dirs[i]; //get the dir to work with
		arc = U8_packdir(arc, curdir, ++recursion); //recursive call
	}
	
	arc->nodes[oldnumnodes].size = htobl(arc->numnodes) + 1; //add one for root node
	
	return arc; //for the calling function
}
Exemplo n.º 3
0
tBleStatus aci_updater_calc_crc(uint32_t address,
                         uint8_t num_sectors,
                         uint32_t *crc)
{
  struct hci_request rq;
  updater_calc_crc_cp cp;
  updater_calc_crc_rp resp;

  memset(&resp, 0, sizeof(resp));

  cp.address = htobl(address);
  cp.num_sectors = num_sectors;

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_VENDOR_CMD;
  rq.ocf = OCF_UPDATER_CALC_CRC;
  rq.cparam = &cp;
  rq.clen = UPDATER_CALC_CRC_CP_SIZE;
  rq.rparam = &resp;
  rq.rlen = UPDATER_CALC_CRC_RP_SIZE;

  if (hci_send_req(&rq) < 0)
    return -1;

  *crc = btohl(resp.crc);

  return resp.status;
}
Exemplo n.º 4
0
tBleStatus aci_updater_read_data_block(uint32_t address,
				uint16_t data_len,
				uint8_t *data)
{
  struct hci_request rq;
  updater_read_data_block_cp cp;
  uint8_t buffer[HCI_MAX_PACKET_SIZE];

  if((data_len+1) > HCI_MAX_PACKET_SIZE)
    return BLE_STATUS_INVALID_PARAMS;

  cp.address = htobl(address);
  cp.data_len = htobs(data_len);

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_VENDOR_CMD;
  rq.ocf = OCF_UPDATER_READ_DATA_BLOCK;
  rq.cparam = &cp;
  rq.clen = UPDATER_READ_DATA_BLOCK_CP_SIZE;
  rq.rparam = buffer;
  rq.rlen = data_len + 1;

  if (hci_send_req(&rq) < 0)
    return -1;

  // First byte is status
  memcpy(data, buffer+1, data_len);

  return buffer[0];
}
Exemplo n.º 5
0
Arquivo: banner.c Projeto: grp/u8it
IMD5* LaichMeta_IMD5(u8* file, u32 size)
{
	IMD5* header = malloc(sizeof(IMD5));
	memset(header, 0, sizeof(IMD5));
	header->imd5[0]		= 'I';
	header->imd5[1]		= 'M';
	header->imd5[2]		= 'D';
	header->imd5[3]		= '5';
	header->filesize	= htobl(size);
	//MD5(file, size, header->crypto);
	return header;
}
Exemplo n.º 6
0
Arquivo: banner.c Projeto: grp/u8it
IMET* LaichMeta_IMET(char** names, \
		     u32 iconsize, u32 bannersize, u32 soundsize)
{
	IMET* header		= malloc(sizeof(IMET));
	memset(header, 0, sizeof(IMET));
	header->imet[0]		= 'I';
	header->imet[1]		= 'M';
	header->imet[2]		= 'E';
	header->imet[3]		= 'T';
	header->unk		= htobll(0x0000060000000003);
	header->sizes[0]	= htobl(iconsize);
	header->sizes[1]	= htobl(bannersize);
	header->sizes[2]	= htobl(soundsize);
	header->flag1		= 0x00;
	
	int i;
	for(i = 0; i < 8; i++)
		memcpy(header->names[i], names[i], 2 * 42);
	
	//MD5(((u8*)header) + 0x40, 0x600, (u8*)header->crypto);
	return header;
}
Exemplo n.º 7
0
Arquivo: banner.c Projeto: grp/u8it
u8* LaichMeta_CompileU8(U8* input, u32* size)
{
	u8archive* arc;
	u32 offset = 0, data_offset, i;
	u8* outbuf;
	
	arc = calloc(1, sizeof(u8archive));
	arc->string_table = calloc(1, 1); //Initial NULL
	arc->stringsz = 1; //Initial NULL
	arc->data = NULL;
	arc->datasz = 0;
	arc->nodes = NULL;
	arc->numnodes = 0;
	
	arc = U8_packdir(arc, input->rootdir, 0);
	
	arc->rootnode.type			= htobs(0x0100);
	arc->rootnode.string_offset	= 0;
	arc->rootnode.data_offset	= 0;
	arc->rootnode.size			= htobl(arc->numnodes);
	
	arc->header.tag				= htobl(0x55AA382D);
	arc->header.root_offset		= htobl(0x20);
	arc->header.header_size		= htobl((sizeof(u8node) * arc->numnodes) + arc->stringsz);
	data_offset					= inlinepad(0x20 + (sizeof(u8node) * arc->numnodes) + arc->stringsz, 0x40);
	arc->header.data_offset		= htobl(data_offset);
	
	memset(arc->header.zeroes, 0, 16);
	
	for(i = 0; i < arc->numnodes; i++)	   // fix the data offset
		if (htobs(arc->nodes[i].type) == 0) // Checking if it is a file
			arc->nodes[i].data_offset = htobl(btohl(arc->nodes[i].data_offset) + data_offset);
	
	outbuf = calloc(1, sizeof(u8header) + (sizeof(u8node) * (arc->numnodes + 1)) + arc->stringsz + arc->datasz);
	
	memcpy(outbuf + offset, &arc->header, sizeof(u8header));
	offset += sizeof(u8header);
	memcpy(outbuf + offset, &arc->rootnode, sizeof(u8node));
	offset += sizeof(u8node);
	memcpy(outbuf + offset, arc->nodes, sizeof(u8node) * arc->numnodes);
	offset += sizeof(u8node) * arc->numnodes;
	memcpy(outbuf + offset, arc->string_table, arc->stringsz);
	offset += arc->stringsz;
	memcpy(outbuf + offset, arc->data, arc->datasz);
	offset += arc->datasz;
	
	*size = offset;
	
	free(arc);
	
	return outbuf;
}
Exemplo n.º 8
0
tBleStatus aci_updater_erase_sector(uint32_t address)
{
  struct hci_request rq;
  updater_erase_sector_cp cp;
  uint8_t status;

  cp.address = htobl(address);

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_VENDOR_CMD;
  rq.ocf = OCF_UPDATER_ERASE_SECTOR;
  rq.cparam = &cp;
  rq.clen = UPDATER_ERASE_SECTOR_CP_SIZE;
  rq.rparam = &status;
  rq.rlen = 1;

  if (hci_send_req(&rq) < 0)
    return -1;

  return status;
}
static void send_mode(char *svr)
{
	struct sco_options so;
	socklen_t len;
	uint32_t seq;
	int i, sk;

	if ((sk = do_connect(svr)) < 0) {
		syslog(LOG_ERR, "Can't connect to the server: %s (%d)",
							strerror(errno), errno);
		exit(1);
	}

	len = sizeof(so);
	if (getsockopt(sk, SOL_SCO, SCO_OPTIONS, &so, &len) < 0) {
		syslog(LOG_ERR, "Can't get SCO options: %s (%d)",
							strerror(errno), errno);
		exit(1);
	}

	syslog(LOG_INFO,"Sending ...");

	for (i = 6; i < so.mtu; i++)
		buf[i] = 0x7f;

	seq = 0;
	while (1) {
		*(uint32_t *) buf = htobl(seq);
		*(uint16_t *) (buf + 4) = htobs(data_size);
		seq++;

		if (send(sk, buf, so.mtu, 0) <= 0) {
			syslog(LOG_ERR, "Send failed: %s (%d)",
							strerror(errno), errno);
			exit(1);
		}

		usleep(1);
	}
}
Exemplo n.º 10
0
tBleStatus aci_gap_set_auth_requirement(uint8_t mitm_mode,
                                        uint8_t oob_enable,
                                        uint8_t oob_data[16],
                                        uint8_t min_encryption_key_size,
                                        uint8_t max_encryption_key_size,
                                        uint8_t use_fixed_pin,
                                        uint32_t fixed_pin,
                                        uint8_t bonding_mode)
{
  struct hci_request rq;
  gap_set_auth_requirement_cp cp;    
  uint8_t status;
    
  cp.mitm_mode = mitm_mode;
  cp.oob_enable = oob_enable;
  Osal_MemCpy(cp.oob_data, oob_data, 16);
  cp.min_encryption_key_size = min_encryption_key_size;
  cp.max_encryption_key_size = max_encryption_key_size;
  cp.use_fixed_pin = use_fixed_pin;
  cp.fixed_pin = htobl(fixed_pin);
  cp.bonding_mode = bonding_mode;

  Osal_MemSet(&rq, 0, sizeof(rq));
  rq.ogf = OGF_VENDOR_CMD;
  rq.ocf = OCF_GAP_SET_AUTH_REQUIREMENT;
  rq.cparam = &cp;
  rq.clen = sizeof(cp);
  rq.rparam = &status;
  rq.rlen = 1;

  if (hci_send_req(&rq, FALSE) < 0)
    return BLE_STATUS_TIMEOUT;

  if (status) {
    return status;
  }
    
  return 0;
}
Exemplo n.º 11
0
tBleStatus aci_gap_pass_key_response(uint16_t conn_handle, uint32_t passkey)
{
  struct hci_request rq;
  gap_passkey_response_cp cp;    
  uint8_t status;
    
  cp.conn_handle = htobs(conn_handle);
  cp.passkey = htobl(passkey);

  Osal_MemSet(&rq, 0, sizeof(rq));
  rq.ogf = OGF_VENDOR_CMD;
  rq.ocf = OCF_GAP_PASSKEY_RESPONSE;
  rq.cparam = &cp;
  rq.clen = sizeof(cp);
  rq.event = EVT_CMD_STATUS;
  rq.rparam = &status;
  rq.rlen = 1;
  
  if (hci_send_req(&rq, FALSE) < 0)
    return BLE_STATUS_TIMEOUT;
  
  return status;
}
Exemplo n.º 12
0
tBleStatus aci_updater_program_data_block(uint32_t address,
				   uint16_t len,
				   const uint8_t *data)
{
  struct hci_request rq;
  uint8_t status;
  uint8_t buffer[HCI_MAX_PACKET_SIZE];
  uint8_t indx = 0;

  if((len+6) > HCI_MAX_PACKET_SIZE)
    return BLE_STATUS_INVALID_PARAMS;

  address = htobl(address);
  memcpy(buffer + indx, &address, 4);
  indx += 4;

  len = htobs(len);
  memcpy(buffer + indx, &len, 2);
  indx += 2;

  memcpy(buffer + indx, data, len);
  indx +=  len;

  memset(&rq, 0, sizeof(rq));
  rq.ogf = OGF_VENDOR_CMD;
  rq.ocf = OCF_UPDATER_PROG_DATA_BLOCK;
  rq.cparam = (void *)buffer;
  rq.clen = indx;
  rq.rparam = &status;
  rq.rlen = 1;

  if (hci_send_req(&rq) < 0)
    return -1;

  return status;
}
Exemplo n.º 13
0
static int process_frames(int dev, int sock, int fd, unsigned long flags)
{
	struct cmsghdr *cmsg;
	struct msghdr msg;
	struct iovec  iv;
	struct hcidump_hdr *dh;
	struct btsnoop_pkt *dp;
	struct frame frm;
	struct pollfd fds[2];
	int nfds = 0;
	char *buf, *ctrl;
	int len, hdr_size = HCIDUMP_HDR_SIZE;

	if (sock < 0)
		return -1;

	if (snap_len < SNAP_LEN)
		snap_len = SNAP_LEN;

	if (flags & DUMP_BTSNOOP)
		hdr_size = BTSNOOP_PKT_SIZE;

	buf = malloc(snap_len + hdr_size);
	if (!buf) {
		perror("Can't allocate data buffer");
		return -1;
	}

	dh = (void *) buf;
	dp = (void *) buf;
	frm.data = buf + hdr_size;

	ctrl = malloc(100);
	if (!ctrl) {
		free(buf);
		perror("Can't allocate control buffer");
		return -1;
	}

	if (dev == HCI_DEV_NONE)
		printf("system: ");
	else
		printf("device: hci%d ", dev);

	printf("snap_len: %d filter: 0x%lx\n", snap_len, parser.filter);

	memset(&msg, 0, sizeof(msg));

	fds[nfds].fd = sock;
	fds[nfds].events = POLLIN;
	fds[nfds].revents = 0;
	nfds++;

	while (1) {
		int i, n = poll(fds, nfds, -1);
		if (n <= 0)
			continue;

		for (i = 0; i < nfds; i++) {
			if (fds[i].revents & (POLLHUP | POLLERR | POLLNVAL)) {
				if (fds[i].fd == sock)
					printf("device: disconnected\n");
				else
					printf("client: disconnect\n");
				return 0;
			}
		}

		iv.iov_base = frm.data;
		iv.iov_len  = snap_len;

		msg.msg_iov = &iv;
		msg.msg_iovlen = 1;
		msg.msg_control = ctrl;
		msg.msg_controllen = 100;

		len = recvmsg(sock, &msg, MSG_DONTWAIT);
		if (len < 0) {
			if (errno == EAGAIN || errno == EINTR)
				continue;
			perror("Receive failed");
			return -1;
		}

		/* Process control message */
		frm.data_len = len;
		frm.dev_id = dev;
		frm.in = 0;
		frm.pppdump_fd = parser.pppdump_fd;
		frm.audio_fd   = parser.audio_fd;

		cmsg = CMSG_FIRSTHDR(&msg);
		while (cmsg) {
			int dir;
			switch (cmsg->cmsg_type) {
			case HCI_CMSG_DIR:
				memcpy(&dir, CMSG_DATA(cmsg), sizeof(int));
				frm.in = (uint8_t) dir;
				break;
			case HCI_CMSG_TSTAMP:
				memcpy(&frm.ts, CMSG_DATA(cmsg),
						sizeof(struct timeval));
				break;
			}
			cmsg = CMSG_NXTHDR(&msg, cmsg);
		}

		frm.ptr = frm.data;
		frm.len = frm.data_len;

		switch (mode) {
		case WRITE:
			/* Save or send dump */
			if (flags & DUMP_BTSNOOP) {
				uint64_t ts;
				uint8_t pkt_type = ((uint8_t *) frm.data)[0];
				dp->size = htobe32(frm.data_len);
				dp->len  = dp->size;
				dp->flags = be32toh(frm.in & 0x01);
				dp->drops = 0;
				ts = (frm.ts.tv_sec - 946684800ll) * 1000000ll + frm.ts.tv_usec;
				dp->ts = htobe64(ts + 0x00E03AB44A676000ll);
				if (pkt_type == HCI_COMMAND_PKT ||
						pkt_type == HCI_EVENT_PKT)
					dp->flags |= be32toh(0x02);
			} else {
				dh->len = htobs(frm.data_len);
				dh->in  = frm.in;
				dh->ts_sec  = htobl(frm.ts.tv_sec);
				dh->ts_usec = htobl(frm.ts.tv_usec);
			}

			if (write_n(fd, buf, frm.data_len + hdr_size) < 0) {
				perror("Write error");
				return -1;
			}
			break;

		default:
			/* Parse and print */
			parse(&frm);
			break;
		}
	}

	return 0;
}
Exemplo n.º 14
0
static int do_connect(const char *svr)
{
	struct sockaddr_rc addr;
	struct rfcomm_conninfo conn;
	socklen_t optlen;
	int sk, opt;

	if (uuid != 0x0000)
		channel = get_channel(svr, uuid);

	if (channel == 0) {
		syslog(LOG_ERR, "Can't get channel number");
		return -1;
	}

	/* Create socket */
	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
	if (sk < 0) {
		syslog(LOG_ERR, "Can't create socket: %s (%d)",
							strerror(errno), errno);
		return -1;
	}

	/* Bind to local address */
	memset(&addr, 0, sizeof(addr));
	addr.rc_family = AF_BLUETOOTH;
	bacpy(&addr.rc_bdaddr, &bdaddr);

	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

#if 0
	/* Enable SO_TIMESTAMP */
	if (timestamp) {
		int t = 1;

		if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
			syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
							strerror(errno), errno);
			goto error;
		}
	}
#endif

	/* Enable SO_LINGER */
	if (linger) {
		struct linger l = { .l_onoff = 1, .l_linger = linger };

		if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
			syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
							strerror(errno), errno);
			goto error;
		}
	}

	/* Set link mode */
	opt = 0;
	if (master)
		opt |= RFCOMM_LM_MASTER;
	if (auth)
		opt |= RFCOMM_LM_AUTH;
	if (encrypt)
		opt |= RFCOMM_LM_ENCRYPT;
	if (secure)
		opt |= RFCOMM_LM_SECURE;

	if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
		syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	/* Connect to remote device */
	memset(&addr, 0, sizeof(addr));
	addr.rc_family = AF_BLUETOOTH;
	str2ba(svr, &addr.rc_bdaddr);
	addr.rc_channel = channel;

	if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		syslog(LOG_ERR, "Can't connect: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	/* Get connection information */
	memset(&conn, 0, sizeof(conn));
	optlen = sizeof(conn);

	if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
		syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
							strerror(errno), errno);
		//goto error;
	}

	syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x]",
		conn.hci_handle,
		conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);

	return sk;

error:
	close(sk);
	return -1;
}

static void do_listen(void (*handler)(int sk))
{
	struct sockaddr_rc addr;
	struct rfcomm_conninfo conn;
	socklen_t optlen;
	int sk, nsk, opt;
	char ba[18];

	/* Create socket */
	sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
	if (sk < 0) {
		syslog(LOG_ERR, "Can't create socket: %s (%d)",
							strerror(errno), errno);
		exit(1);
	}

	/* Bind to local address */
	memset(&addr, 0, sizeof(addr));
	addr.rc_family = AF_BLUETOOTH;
	bacpy(&addr.rc_bdaddr, &bdaddr);
	addr.rc_channel = channel;

	if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
		syslog(LOG_ERR, "Can't bind socket: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	/* Set link mode */
	opt = 0;
	if (master)
		opt |= RFCOMM_LM_MASTER;
	if (auth)
		opt |= RFCOMM_LM_AUTH;
	if (encrypt)
		opt |= RFCOMM_LM_ENCRYPT;
	if (secure)
		opt |= RFCOMM_LM_SECURE;

	if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
		syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	/* Enable deferred setup */
	opt = defer_setup;

	if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
						&opt, sizeof(opt)) < 0) {
		syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	/* Listen for connections */
	if (listen(sk, 10)) {
		syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	/* Check for socket address */
	memset(&addr, 0, sizeof(addr));
	optlen = sizeof(addr);

	if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
		syslog(LOG_ERR, "Can't get socket name: %s (%d)",
							strerror(errno), errno);
		goto error;
	}

	channel = addr.rc_channel;

	syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);

	while (1) {
		memset(&addr, 0, sizeof(addr));
		optlen = sizeof(addr);

		nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
		if (nsk < 0) {
			syslog(LOG_ERR,"Accept failed: %s (%d)",
							strerror(errno), errno);
			goto error;
		}
		if (fork()) {
			/* Parent */
			close(nsk);
			continue;
		}
		/* Child */
		close(sk);

		/* Get connection information */
		memset(&conn, 0, sizeof(conn));
		optlen = sizeof(conn);

		if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
			syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
							strerror(errno), errno);
			//close(nsk);
			//goto error;
		}

		ba2str(&addr.rc_bdaddr, ba);
		syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
			ba, conn.hci_handle,
			conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);

#if 0
		/* Enable SO_TIMESTAMP */
		if (timestamp) {
			int t = 1;

			if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
				syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
							strerror(errno), errno);
				goto error;
			}
		}
#endif

		/* Enable SO_LINGER */
		if (linger) {
			struct linger l = { .l_onoff = 1, .l_linger = linger };

			if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
				syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
							strerror(errno), errno);
				close(nsk);
				goto error;
			}
		}

		/* Handle deferred setup */
		if (defer_setup) {
			syslog(LOG_INFO, "Waiting for %d seconds",
							abs(defer_setup) - 1);
			sleep(abs(defer_setup) - 1);

			if (defer_setup < 0) {
				close(nsk);
				goto error;
			}
		}

		handler(nsk);

		syslog(LOG_INFO, "Disconnect: %m");
		exit(0);
	}

	return;

error:
	close(sk);
	exit(1);
}

static void dump_mode(int sk)
{
	int len;

	syslog(LOG_INFO, "Receiving ...");
	while ((len = read(sk, buf, data_size)) > 0)
		syslog(LOG_INFO, "Recevied %d bytes", len);
}

static void recv_mode(int sk)
{
	struct timeval tv_beg, tv_end, tv_diff;
	char ts[30];
	long total;
	uint32_t seq;

	syslog(LOG_INFO, "Receiving ...");

	memset(ts, 0, sizeof(ts));

	seq = 0;
	while (1) {
		gettimeofday(&tv_beg,NULL);
		total = 0;
		while (total < data_size) {
			//uint32_t sq;
			//uint16_t l;
			int r;

			if ((r = recv(sk, buf, data_size, 0)) < 0) {
				if (r < 0)
					syslog(LOG_ERR, "Read failed: %s (%d)",
							strerror(errno), errno);
				return;
			}

			if (timestamp) {
				struct timeval tv;

				if (ioctl(sk, SIOCGSTAMP, &tv) < 0) {
					timestamp = 0;
					memset(ts, 0, sizeof(ts));
				} else {
					sprintf(ts, "[%ld.%ld] ",
							tv.tv_sec, tv.tv_usec);
				}
			}

#if 0
			/* Check sequence */
			sq = btohl(*(uint32_t *) buf);
			if (seq != sq) {
				syslog(LOG_INFO, "seq missmatch: %d -> %d", seq, sq);
				seq = sq;
			}
			seq++;

			/* Check length */
			l = btohs(*(uint16_t *) (buf + 4));
			if (r != l) {
				syslog(LOG_INFO, "size missmatch: %d -> %d", r, l);
				continue;
			}

			/* Verify data */
			for (i = 6; i < r; i++) {
				if (buf[i] != 0x7f)
					syslog(LOG_INFO, "data missmatch: byte %d 0x%2.2x", i, buf[i]);
			}
#endif
			total += r;
		}
		gettimeofday(&tv_end,NULL);

		timersub(&tv_end,&tv_beg,&tv_diff);

		syslog(LOG_INFO,"%s%ld bytes in %.2f sec, %.2f kB/s", ts, total,
			tv2fl(tv_diff), (float)(total / tv2fl(tv_diff) ) / 1024.0);
	}
}

static void do_send(int sk)
{
	uint32_t seq;
	int i, fd, len;

	syslog(LOG_INFO,"Sending ...");

	if (filename) {
		fd = open(filename, O_RDONLY);
		if (fd < 0) {
			syslog(LOG_ERR, "Open failed: %s (%d)",
							strerror(errno), errno);
			exit(1);
		}
		len = read(fd, buf, data_size);
		send(sk, buf, len, 0);
		return;
	} else {
		for (i = 6; i < data_size; i++)
			buf[i] = 0x7f;
	}

	seq = 0;
	while ((num_frames == -1) || (num_frames-- > 0)) {
		*(uint32_t *) buf = htobl(seq);
		*(uint16_t *) (buf + 4) = htobs(data_size);
		seq++;

		if (send(sk, buf, data_size, 0) <= 0) {
			syslog(LOG_ERR, "Send failed: %s (%d)",
							strerror(errno), errno);
			exit(1);
		}

		if (num_frames && delay && count && !(seq % count))
			usleep(delay);
	}
}