aboutsummaryrefslogtreecommitdiff
path: root/net/nfc/nci/hci.c
diff options
context:
space:
mode:
Diffstat (limited to 'net/nfc/nci/hci.c')
-rw-r--r--net/nfc/nci/hci.c90
1 files changed, 64 insertions, 26 deletions
diff --git a/net/nfc/nci/hci.c b/net/nfc/nci/hci.c
index b33fed6d1584..91ecbd1c2ec1 100644
--- a/net/nfc/nci/hci.c
+++ b/net/nfc/nci/hci.c
@@ -101,6 +101,20 @@ struct nci_hcp_packet {
#define NCI_HCP_MSG_GET_CMD(header) (header & 0x3f)
#define NCI_HCP_MSG_GET_PIPE(header) (header & 0x7f)
+static int nci_hci_result_to_errno(u8 result)
+{
+ switch (result) {
+ case NCI_HCI_ANY_OK:
+ return 0;
+ case NCI_HCI_ANY_E_REG_PAR_UNKNOWN:
+ return -EOPNOTSUPP;
+ case NCI_HCI_ANY_E_TIMEOUT:
+ return -ETIME;
+ default:
+ return -1;
+ }
+}
+
/* HCI core */
static void nci_hci_reset_pipes(struct nci_hci_dev *hdev)
{
@@ -146,18 +160,18 @@ static int nci_hci_send_data(struct nci_dev *ndev, u8 pipe,
if (!conn_info)
return -EPROTO;
- skb = nci_skb_alloc(ndev, 2 + conn_info->max_pkt_payload_len +
+ i = 0;
+ skb = nci_skb_alloc(ndev, conn_info->max_pkt_payload_len +
NCI_DATA_HDR_SIZE, GFP_KERNEL);
if (!skb)
return -ENOMEM;
- skb_reserve(skb, 2 + NCI_DATA_HDR_SIZE);
+ skb_reserve(skb, NCI_DATA_HDR_SIZE + 2);
*skb_push(skb, 1) = data_type;
- i = 0;
- len = conn_info->max_pkt_payload_len;
-
do {
+ len = conn_info->max_pkt_payload_len;
+
/* If last packet add NCI_HFP_NO_CHAINING */
if (i + conn_info->max_pkt_payload_len -
(skb->len + 1) >= data_len) {
@@ -177,9 +191,15 @@ static int nci_hci_send_data(struct nci_dev *ndev, u8 pipe,
return r;
i += len;
+
if (i < data_len) {
- skb_trim(skb, 0);
- skb_pull(skb, len);
+ skb = nci_skb_alloc(ndev,
+ conn_info->max_pkt_payload_len +
+ NCI_DATA_HDR_SIZE, GFP_KERNEL);
+ if (!skb)
+ return -ENOMEM;
+
+ skb_reserve(skb, NCI_DATA_HDR_SIZE + 1);
}
} while (i < data_len);
@@ -212,7 +232,8 @@ int nci_hci_send_cmd(struct nci_dev *ndev, u8 gate, u8 cmd,
const u8 *param, size_t param_len,
struct sk_buff **skb)
{
- struct nci_conn_info *conn_info;
+ struct nci_hcp_message *message;
+ struct nci_conn_info *conn_info;
struct nci_data data;
int r;
u8 pipe = ndev->hci_dev->gate2pipe[gate];
@@ -232,9 +253,15 @@ int nci_hci_send_cmd(struct nci_dev *ndev, u8 gate, u8 cmd,
r = nci_request(ndev, nci_hci_send_data_req, (unsigned long)&data,
msecs_to_jiffies(NCI_DATA_TIMEOUT));
-
- if (r == NCI_STATUS_OK && skb)
- *skb = conn_info->rx_skb;
+ if (r == NCI_STATUS_OK) {
+ message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+ r = nci_hci_result_to_errno(
+ NCI_HCP_MSG_GET_CMD(message->header));
+ skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+ if (!r && skb)
+ *skb = conn_info->rx_skb;
+ }
return r;
}
@@ -328,9 +355,6 @@ static void nci_hci_resp_received(struct nci_dev *ndev, u8 pipe,
struct nci_conn_info *conn_info;
u8 status = result;
- if (result != NCI_HCI_ANY_OK)
- goto exit;
-
conn_info = ndev->hci_dev->conn_info;
if (!conn_info) {
status = NCI_STATUS_REJECTED;
@@ -340,7 +364,7 @@ static void nci_hci_resp_received(struct nci_dev *ndev, u8 pipe,
conn_info->rx_skb = skb;
exit:
- nci_req_complete(ndev, status);
+ nci_req_complete(ndev, NCI_STATUS_OK);
}
/* Receive hcp message for pipe, with type and cmd.
@@ -378,7 +402,7 @@ static void nci_hci_msg_rx_work(struct work_struct *work)
u8 pipe, type, instruction;
while ((skb = skb_dequeue(&hdev->msg_rx_queue)) != NULL) {
- pipe = skb->data[0];
+ pipe = NCI_HCP_MSG_GET_PIPE(skb->data[0]);
skb_pull(skb, NCI_HCI_HCP_PACKET_HEADER_LEN);
message = (struct nci_hcp_message *)skb->data;
type = NCI_HCP_MSG_GET_TYPE(message->header);
@@ -395,7 +419,7 @@ void nci_hci_data_received_cb(void *context,
{
struct nci_dev *ndev = (struct nci_dev *)context;
struct nci_hcp_packet *packet;
- u8 pipe, type, instruction;
+ u8 pipe, type;
struct sk_buff *hcp_skb;
struct sk_buff *frag_skb;
int msg_len;
@@ -415,7 +439,7 @@ void nci_hci_data_received_cb(void *context,
/* it's the last fragment. Does it need re-aggregation? */
if (skb_queue_len(&ndev->hci_dev->rx_hcp_frags)) {
- pipe = packet->header & NCI_HCI_FRAGMENT;
+ pipe = NCI_HCP_MSG_GET_PIPE(packet->header);
skb_queue_tail(&ndev->hci_dev->rx_hcp_frags, skb);
msg_len = 0;
@@ -434,7 +458,7 @@ void nci_hci_data_received_cb(void *context,
*skb_put(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN) = pipe;
skb_queue_walk(&ndev->hci_dev->rx_hcp_frags, frag_skb) {
- msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN;
+ msg_len = frag_skb->len - NCI_HCI_HCP_PACKET_HEADER_LEN;
memcpy(skb_put(hcp_skb, msg_len), frag_skb->data +
NCI_HCI_HCP_PACKET_HEADER_LEN, msg_len);
}
@@ -452,11 +476,10 @@ void nci_hci_data_received_cb(void *context,
packet = (struct nci_hcp_packet *)hcp_skb->data;
type = NCI_HCP_MSG_GET_TYPE(packet->message.header);
if (type == NCI_HCI_HCP_RESPONSE) {
- pipe = packet->header;
- instruction = NCI_HCP_MSG_GET_CMD(packet->message.header);
- skb_pull(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN +
- NCI_HCI_HCP_MESSAGE_HEADER_LEN);
- nci_hci_hcp_message_rx(ndev, pipe, type, instruction, hcp_skb);
+ pipe = NCI_HCP_MSG_GET_PIPE(packet->header);
+ skb_pull(hcp_skb, NCI_HCI_HCP_PACKET_HEADER_LEN);
+ nci_hci_hcp_message_rx(ndev, pipe, type,
+ NCI_STATUS_OK, hcp_skb);
} else {
skb_queue_tail(&ndev->hci_dev->msg_rx_queue, hcp_skb);
schedule_work(&ndev->hci_dev->msg_rx_work);
@@ -488,6 +511,7 @@ EXPORT_SYMBOL(nci_hci_open_pipe);
int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx,
const u8 *param, size_t param_len)
{
+ struct nci_hcp_message *message;
struct nci_conn_info *conn_info;
struct nci_data data;
int r;
@@ -520,6 +544,12 @@ int nci_hci_set_param(struct nci_dev *ndev, u8 gate, u8 idx,
r = nci_request(ndev, nci_hci_send_data_req,
(unsigned long)&data,
msecs_to_jiffies(NCI_DATA_TIMEOUT));
+ if (r == NCI_STATUS_OK) {
+ message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+ r = nci_hci_result_to_errno(
+ NCI_HCP_MSG_GET_CMD(message->header));
+ skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+ }
kfree(tmp);
return r;
@@ -529,6 +559,7 @@ EXPORT_SYMBOL(nci_hci_set_param);
int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx,
struct sk_buff **skb)
{
+ struct nci_hcp_message *message;
struct nci_conn_info *conn_info;
struct nci_data data;
int r;
@@ -553,8 +584,15 @@ int nci_hci_get_param(struct nci_dev *ndev, u8 gate, u8 idx,
r = nci_request(ndev, nci_hci_send_data_req, (unsigned long)&data,
msecs_to_jiffies(NCI_DATA_TIMEOUT));
- if (r == NCI_STATUS_OK)
- *skb = conn_info->rx_skb;
+ if (r == NCI_STATUS_OK) {
+ message = (struct nci_hcp_message *)conn_info->rx_skb->data;
+ r = nci_hci_result_to_errno(
+ NCI_HCP_MSG_GET_CMD(message->header));
+ skb_pull(conn_info->rx_skb, NCI_HCI_HCP_MESSAGE_HEADER_LEN);
+
+ if (!r && skb)
+ *skb = conn_info->rx_skb;
+ }
return r;
}
generated by cgit v1.2.3 (git 2.25.1) at 2024-04-25 11:18:40 +0000