Пример #1
0
int
ng_l2cap_lp_con_cfm(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_hci_lp_con_cfm_ep	*ep = NULL;
	ng_l2cap_con_p		 con = NULL;
	int			 error = 0;

	/* Check message */
	if (msg->header.arglen != sizeof(*ep)) {
		NG_L2CAP_ALERT(
"%s: %s - invalid LP_ConnectCfm[Neg] message size\n",
			__func__, NG_NODE_NAME(l2cap->node));
		error = EMSGSIZE;
		goto out;
	}

	ep = (ng_hci_lp_con_cfm_ep *) (msg->data);

	/* Check if we have requested/accepted this connection */
	con = ng_l2cap_con_by_addr(l2cap, &ep->bdaddr);
	if (con == NULL) {
		NG_L2CAP_ERR(
"%s: %s - unexpected LP_ConnectCfm event. Connection does not exist\n",
			__func__, NG_NODE_NAME(l2cap->node));
		error = ENOENT;
		goto out;
	}

	/* Check connection state */
	if (con->state != NG_L2CAP_W4_LP_CON_CFM) {
		NG_L2CAP_ALERT(
"%s: %s - unexpected LP_ConnectCfm event. " \
"Invalid connection state, state=%d, con_handle=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), con->state, 
			con->con_handle);
		error = EINVAL;
		goto out;
	}

	/*
	 * Looks like it is our confirmation. It is safe now to cancel 
	 * connection timer and notify upper layer. If timeout already
	 * happened then ignore connection confirmation and let timeout
	 * handle that.
 	 */

	if ((error = ng_l2cap_lp_untimeout(con)) != 0)
		goto out;

	if (ep->status == 0) {
		con->state = NG_L2CAP_CON_OPEN;
		con->con_handle = ep->con_handle;
		ng_l2cap_lp_deliver(con);
	} else /* Negative confirmation - remove connection descriptor */
		ng_l2cap_con_fail(con, ep->status);
out:
	return (error);
} /* ng_l2cap_lp_con_cfm */
Пример #2
0
int
ng_l2cap_l2ca_write_req(ng_l2cap_p l2cap, struct mbuf *m)
{
	ng_l2cap_l2ca_hdr_t	*l2ca_hdr = NULL;
	ng_l2cap_chan_p		 ch = NULL;
	ng_l2cap_cmd_p		 cmd = NULL;
	int			 error = 0;
	u_int32_t		 token = 0;

	/* Make sure we can access L2CA data packet header */
	if (m->m_pkthdr.len < sizeof(*l2ca_hdr)) {
		NG_L2CAP_ERR(
"%s: %s - L2CA Data packet too small, len=%d\n",
			__func__,NG_NODE_NAME(l2cap->node),m->m_pkthdr.len);
		error = EMSGSIZE;
		goto drop;
	}

	/* Get L2CA data packet header */
	NG_L2CAP_M_PULLUP(m, sizeof(*l2ca_hdr));
	if (m == NULL)
		return (ENOBUFS);

	l2ca_hdr = mtod(m, ng_l2cap_l2ca_hdr_t *);
	token = l2ca_hdr->token;
	m_adj(m, sizeof(*l2ca_hdr));

	/* Verify payload size */
	if (l2ca_hdr->length != m->m_pkthdr.len) {
		NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. " \
"Payload length does not match, length=%d, len=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->length,
			m->m_pkthdr.len);
		error = EMSGSIZE;
		goto drop;
	}

	/* Check channel ID */
	if (l2ca_hdr->lcid < NG_L2CAP_FIRST_CID) {
		NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. Inavlid channel ID, cid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
		error = EINVAL;
		goto drop;
	}

	/* Verify that we have the channel and make sure it is open */
	ch = ng_l2cap_chan_by_scid(l2cap, l2ca_hdr->lcid);
	if (ch == NULL) {
		NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. Channel does not exist, cid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), l2ca_hdr->lcid);
		error = ENOENT;
		goto drop;
	}

	if (ch->state != NG_L2CAP_OPEN) {
		NG_L2CAP_ERR(
"%s: %s - invalid L2CA Data packet. Invalid channel state, scid=%d, state=%d\n",
			 __func__, NG_NODE_NAME(l2cap->node), ch->scid, 
			ch->state);
		error = EHOSTDOWN;
		goto drop; /* XXX not always - re-configure */
	}

	/* Create L2CAP command descriptor */
	cmd = ng_l2cap_new_cmd(ch->con, ch, 0, NGM_L2CAP_L2CA_WRITE, token);
	if (cmd == NULL) {
		error = ENOMEM;
		goto drop;
	}

	/* Attach data packet and link command to the queue */
	cmd->aux = m;
	ng_l2cap_link_cmd(ch->con, cmd);
	ng_l2cap_lp_deliver(ch->con);

	return (error);
drop:
	NG_FREE_M(m);

	return (error);
} /* ng_l2cap_l2ca_write_req */
Пример #3
0
int
ng_l2cap_l2ca_con_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_l2cap_l2ca_con_ip	*ip = NULL;
	ng_l2cap_con_p		 con = NULL;
	ng_l2cap_chan_p		 ch = NULL;
	ng_l2cap_cmd_p		 cmd = NULL;
	int			 error = 0;

	/* Check message */
	if (msg->header.arglen != sizeof(*ip)) {
		NG_L2CAP_ALERT(
"%s: %s - invalid L2CA_Connect request message size, size=%d\n",
			__func__, NG_NODE_NAME(l2cap->node),
			msg->header.arglen);
		error = EMSGSIZE;
		goto out;
	}

	ip = (ng_l2cap_l2ca_con_ip *)(msg->data);

	/* Check if we have connection to the remote unit */
	con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
	if (con == NULL) {
		/* Submit LP_ConnectReq to the lower layer */
		error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
		if (error != 0) {
			NG_L2CAP_ERR(
"%s: %s - unable to send LP_ConnectReq message, error=%d\n",
				__func__, NG_NODE_NAME(l2cap->node), error);
			goto out;
		}

		/* This should not fail */
		con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
		KASSERT((con != NULL),
("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
	}

	/*
	 * Create new empty channel descriptor. In case of any failure do 
	 * not touch connection descriptor.
	 */

	ch = ng_l2cap_new_chan(l2cap, con, ip->psm);
	if (ch == NULL) {
		error = ENOMEM;
		goto out;
	}

	/* Now create L2CAP_ConnectReq command */
	cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(con),
			NG_L2CAP_CON_REQ, msg->header.token);
	if (cmd == NULL) {
		ng_l2cap_free_chan(ch);
		error = ENOMEM;
		goto out;
	}

	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
		ng_l2cap_free_cmd(cmd);
		ng_l2cap_free_chan(ch);
		error = EIO;
		goto out;
	}

	/* Create L2CAP command packet */
	_ng_l2cap_con_req(cmd->aux, cmd->ident, ch->psm, ch->scid);
	if (cmd->aux == NULL) {
		ng_l2cap_free_cmd(cmd);
		ng_l2cap_free_chan(ch);
		error = ENOBUFS;
		goto out;
	}

	ch->state = NG_L2CAP_W4_L2CAP_CON_RSP;

	/* Link command to the queue */
	ng_l2cap_link_cmd(ch->con, cmd);
	ng_l2cap_lp_deliver(ch->con);
out:
	return (error);
} /* ng_l2cap_l2ca_con_req */
Пример #4
0
int
ng_l2cap_l2ca_cfg_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_l2cap_l2ca_cfg_rsp_ip	*ip = NULL;
	ng_l2cap_chan_p			 ch = NULL;
	ng_l2cap_cmd_p			 cmd = NULL;
	struct mbuf			*opt = NULL;
	u_int16_t			*mtu = NULL;
	ng_l2cap_flow_p			 flow = NULL;
	int				 error = 0;

	/* Check message */
	if (msg->header.arglen != sizeof(*ip)) {
		NG_L2CAP_ALERT(
"%s: %s - invalid L2CA_ConfigRsp request message size, size=%d\n",
			__func__, NG_NODE_NAME(l2cap->node),
			msg->header.arglen);
		error = EMSGSIZE;
		goto out;
	}

	ip = (ng_l2cap_l2ca_cfg_rsp_ip *)(msg->data);

	/* Check if we have this channel */
	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
	if (ch == NULL) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_ConfigRsp request message. " \
"Channel does not exist, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
		error = ENOENT;
		goto out;
	}

	/* Check channel state */
	if (ch->state != NG_L2CAP_CONFIG) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_ConfigRsp request message. " \
"Invalid channel state, state=%d, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ch->state,
			ch->scid);
		error = EINVAL;
		goto out;
	}

	/* Set channel settings */
	if (ip->omtu != ch->omtu) {
		ch->omtu = ip->omtu;
		mtu = &ch->omtu;
	}

	if (bcmp(&ip->iflow, &ch->iflow, sizeof(ch->iflow)) != 0) { 
		bcopy(&ip->iflow, &ch->iflow, sizeof(ch->iflow));
		flow = &ch->iflow;
	}

	if (mtu != NULL || flow != NULL) {
		_ng_l2cap_build_cfg_options(opt, mtu, NULL, flow);
		if (opt == NULL) {
			error = ENOBUFS;
			goto out;
		}
	}

	/* Create L2CAP command */
	cmd = ng_l2cap_new_cmd(ch->con, ch, ch->ident, NG_L2CAP_CFG_RSP,
			msg->header.token);
	if (cmd == NULL) {
		NG_FREE_M(opt);
		error = ENOMEM;
		goto out;
	}

	_ng_l2cap_cfg_rsp(cmd->aux,cmd->ident,ch->dcid,0,NG_L2CAP_SUCCESS,opt);
	if (cmd->aux == NULL) {
		ng_l2cap_free_cmd(cmd);
		error = ENOBUFS;
		goto out;
	}

	/* XXX FIXME - not here ??? */
	ch->cfg_state |= NG_L2CAP_CFG_OUT;
	if (ch->cfg_state == NG_L2CAP_CFG_BOTH)
		ch->state = NG_L2CAP_OPEN;

	/* Link command to the queue */
	ng_l2cap_link_cmd(ch->con, cmd);
	ng_l2cap_lp_deliver(ch->con);
out:
	return (error);
} /* ng_l2cap_l2ca_cfg_rsp_req */
Пример #5
0
int
ng_l2cap_l2ca_cfg_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_l2cap_l2ca_cfg_ip	*ip = NULL;
	ng_l2cap_chan_p		 ch = NULL;
	ng_l2cap_cmd_p		 cmd = NULL;
	struct mbuf		*opt = NULL;
        u_int16_t		*mtu = NULL, *flush_timo = NULL;
        ng_l2cap_flow_p		 flow = NULL;
	int			 error = 0;

	/* Check message */
	if (msg->header.arglen != sizeof(*ip)) {
		NG_L2CAP_ALERT(
"%s: %s - Invalid L2CA_Config request message size, size=%d\n",
			__func__, NG_NODE_NAME(l2cap->node),
			msg->header.arglen);
		error = EMSGSIZE;
		goto out;
	}

	ip = (ng_l2cap_l2ca_cfg_ip *)(msg->data);

	/* Check if we have this channel */
	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
	if (ch == NULL) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_Config request message. " \
"Channel does not exist, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
		error = ENOENT;
		goto out;
	}

	/* Check channel state */
	if (ch->state != NG_L2CAP_OPEN && ch->state != NG_L2CAP_CONFIG) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_Config request message. " \
"Invalid channel state, state=%d, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ch->state,
			ch->scid);
		error = EINVAL;
		goto out;
	}

	/* Set requested channel configuration options */
	ch->imtu = ip->imtu;
	bcopy(&ip->oflow, &ch->oflow, sizeof(ch->oflow));
	ch->flush_timo = ip->flush_timo;
	ch->link_timo = ip->link_timo;

	/* Compare channel settings with defaults */
	if (ch->imtu != NG_L2CAP_MTU_DEFAULT)
		mtu = &ch->imtu;
	if (ch->flush_timo != NG_L2CAP_FLUSH_TIMO_DEFAULT)
		flush_timo = &ch->flush_timo;
	if (bcmp(ng_l2cap_default_flow(), &ch->oflow, sizeof(ch->oflow)) != 0)
		flow = &ch->oflow;

	/* Create configuration options */
	_ng_l2cap_build_cfg_options(opt, mtu, flush_timo, flow);
	if (opt == NULL) {
                error = ENOBUFS;
		goto out;
	}

	/* Create L2CAP command descriptor */
	cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
			NG_L2CAP_CFG_REQ, msg->header.token);
	if (cmd == NULL) {
		NG_FREE_M(opt);
		error = ENOMEM;
		goto out;
	}

	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
		ng_l2cap_free_cmd(cmd);
		NG_FREE_M(opt);
		error = EIO;
		goto out;
	}

	/* Create L2CAP command packet */
	_ng_l2cap_cfg_req(cmd->aux, cmd->ident, ch->dcid, 0, opt);
	if (cmd->aux == NULL) {
		ng_l2cap_free_cmd(cmd);
		error =  ENOBUFS;
		goto out;
	}

	/* Adjust channel state for re-configuration */
	if (ch->state == NG_L2CAP_OPEN) {
		ch->state = NG_L2CAP_CONFIG;
		ch->cfg_state = 0;
	}

        /* Link command to the queue */
	ng_l2cap_link_cmd(ch->con, cmd);
	ng_l2cap_lp_deliver(ch->con);
out:
	return (error);
} /* ng_l2cap_l2ca_cfg_req */
Пример #6
0
int
ng_l2cap_l2ca_con_rsp_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_l2cap_l2ca_con_rsp_ip	*ip = NULL;
	ng_l2cap_con_p			 con = NULL;
	ng_l2cap_chan_p			 ch = NULL;
	ng_l2cap_cmd_p			 cmd = NULL;
	u_int16_t			 dcid;
	int				 error = 0;

	/* Check message */
	if (msg->header.arglen != sizeof(*ip)) {
		NG_L2CAP_ALERT(
"%s: %s - invalid L2CA_ConnectRsp request message size, size=%d\n",
			__func__, NG_NODE_NAME(l2cap->node),
			msg->header.arglen);
		error = EMSGSIZE;
		goto out;
	}

	ip = (ng_l2cap_l2ca_con_rsp_ip *)(msg->data);

	/* Check if we have this channel */
	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
	if (ch == NULL) {
		NG_L2CAP_ALERT(
"%s: %s - unexpected L2CA_ConnectRsp request message. " \
"Channel does not exist, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
		error = ENOENT;
		goto out;
	}

	/* Check channel state */
	if (ch->state != NG_L2CAP_W4_L2CA_CON_RSP) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_ConnectRsp request message. " \
"Invalid channel state, state=%d, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ch->state,
			ip->lcid);
		error = EINVAL;
		goto out;
	}

	dcid = ch->dcid;
	con = ch->con;

	/*
	 * Now we are pretty much sure it is our response. So create and send 
	 * L2CAP_ConnectRsp message to our peer.
	 */

	if (ch->ident != ip->ident)
		NG_L2CAP_WARN(
"%s: %s - channel ident and response ident do not match, scid=%d, ident=%d. " \
"Will use response ident=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ch->scid, 
			ch->ident, ip->ident);

	/* Check result */
	switch (ip->result) {
	case NG_L2CAP_SUCCESS:
		ch->state = NG_L2CAP_CONFIG;
		ch->cfg_state = 0;
		break;

	case NG_L2CAP_PENDING:
		break;

	default:
		ng_l2cap_free_chan(ch);
		ch = NULL;
		break;
	}

	/* Create L2CAP command */
	cmd = ng_l2cap_new_cmd(con, ch, ip->ident, NG_L2CAP_CON_RSP,
			msg->header.token);
	if (cmd == NULL) {
		if (ch != NULL)
			ng_l2cap_free_chan(ch);

		error = ENOMEM;
		goto out;
	}

	_ng_l2cap_con_rsp(cmd->aux, cmd->ident, ip->lcid, dcid, 
		ip->result, ip->status);
	if (cmd->aux == NULL) {
		if (ch != NULL)
			ng_l2cap_free_chan(ch);

		ng_l2cap_free_cmd(cmd);
		error = ENOBUFS;
		goto out;
	} 

	/* Link command to the queue */
	ng_l2cap_link_cmd(con, cmd);
	ng_l2cap_lp_deliver(con);
out:
	return (error);
} /* ng_l2cap_l2ca_con_rsp_req */
Пример #7
0
int
ng_l2cap_l2ca_get_info_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_l2cap_l2ca_get_info_ip	*ip = NULL;
	ng_l2cap_con_p			 con = NULL;
	ng_l2cap_cmd_p			 cmd = NULL;
	int				 error = 0;

	/* Verify message */
	if (msg->header.arglen != sizeof(*ip)) {
		NG_L2CAP_ALERT(
"%s: %s - invalid L2CA_GetInfo request message size, size=%d\n",
			__func__, NG_NODE_NAME(l2cap->node),
			msg->header.arglen);
		error = EMSGSIZE;
		goto out;
	}

	ip = (ng_l2cap_l2ca_get_info_ip *)(msg->data);

	/* Check if we have connection to the unit */
	con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
	if (con == NULL) {
		/* Submit LP_ConnectReq to the lower layer */
		error = ng_l2cap_lp_con_req(l2cap, &ip->bdaddr);
		if (error != 0) {
			NG_L2CAP_ERR(
"%s: %s - unable to send LP_ConnectReq message, error=%d\n",
				__func__, NG_NODE_NAME(l2cap->node), error);
			goto out;
		}

		/* This should not fail */
		con = ng_l2cap_con_by_addr(l2cap, &ip->bdaddr);
		KASSERT((con != NULL),
("%s: %s - could not find connection!\n", __func__, NG_NODE_NAME(l2cap->node)));
	}

	/* Create L2CAP command descriptor */
	cmd = ng_l2cap_new_cmd(con, NULL, ng_l2cap_get_ident(con),
			NG_L2CAP_INFO_REQ, msg->header.token);
	if (cmd == NULL) {
		error = ENOMEM;
		goto out;
	}

	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
		ng_l2cap_free_cmd(cmd);
		error = EIO;
		goto out;
	}

	/* Create L2CAP command packet */
	_ng_l2cap_info_req(cmd->aux, cmd->ident, ip->info_type);
	if (cmd->aux == NULL) {
		ng_l2cap_free_cmd(cmd);
		error = ENOBUFS;
		goto out;
	}

        /* Link command to the queue */
	ng_l2cap_link_cmd(con, cmd);
	ng_l2cap_lp_deliver(con);
out:
	return (error);
} /* ng_l2cap_l2ca_get_info_req */
Пример #8
0
int
ng_l2cap_l2ca_discon_req(ng_l2cap_p l2cap, struct ng_mesg *msg)
{
	ng_l2cap_l2ca_discon_ip	*ip = NULL;
	ng_l2cap_chan_p		 ch = NULL;
	ng_l2cap_cmd_p		 cmd = NULL;
	int			 error = 0;

	/* Check message */
	if (msg->header.arglen != sizeof(*ip)) {
		NG_L2CAP_ALERT(
"%s: %s - invalid L2CA_Disconnect request message size, size=%d\n",
			__func__, NG_NODE_NAME(l2cap->node),
			msg->header.arglen);
		error = EMSGSIZE;
		goto out;
	}

	ip = (ng_l2cap_l2ca_discon_ip *)(msg->data);

	/* Check if we have this channel */
	ch = ng_l2cap_chan_by_scid(l2cap, ip->lcid);
	if (ch == NULL) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_Disconnect request message. " \
"Channel does not exist, lcid=%d\n",
			__func__, NG_NODE_NAME(l2cap->node), ip->lcid);
		error = ENOENT;
		goto out;
	}

	/* Check channel state */
	if (ch->state != NG_L2CAP_CONFIG && ch->state != NG_L2CAP_OPEN && 
	    ch->state != NG_L2CAP_W4_L2CAP_DISCON_RSP) {
		NG_L2CAP_ERR(
"%s: %s - unexpected L2CA_Disconnect request message. " \
"Invalid channel state, state=%d, lcid=%d\n", 
			__func__, NG_NODE_NAME(l2cap->node), ch->state,
			ch->scid);
		error = EINVAL;
		goto out;
	}

	/* Create and send L2CAP_DisconReq message */
	cmd = ng_l2cap_new_cmd(ch->con, ch, ng_l2cap_get_ident(ch->con),
			NG_L2CAP_DISCON_REQ, msg->header.token);
	if (cmd == NULL) {
		ng_l2cap_free_chan(ch);
		error = ENOMEM;
		goto out;
	}

	if (cmd->ident == NG_L2CAP_NULL_IDENT) {
		ng_l2cap_free_chan(ch);
		ng_l2cap_free_cmd(cmd);
		error = EIO;
		goto out;
	}

	_ng_l2cap_discon_req(cmd->aux, cmd->ident, ch->dcid, ch->scid);
	if (cmd->aux == NULL) {
		ng_l2cap_free_chan(ch);
		ng_l2cap_free_cmd(cmd);
		error = ENOBUFS;
		goto out;
	}

	ch->state = NG_L2CAP_W4_L2CAP_DISCON_RSP;

	/* Link command to the queue */
	ng_l2cap_link_cmd(ch->con, cmd);
	ng_l2cap_lp_deliver(ch->con);
out:
	return (error);
} /* ng_l2cap_l2ca_discon_req */