int __near_netlink_dep_link_down(uint32_t idx)
{
struct nl_msg *msg;
void *hdr;
int err;
DBG("");
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
NLM_F_REQUEST, NFC_CMD_DEP_LINK_DOWN, NFC_GENL_VERSION);
if (!hdr) {
err = -EINVAL;
goto nla_put_failure;
}
err = -EMSGSIZE;
NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
nla_put_failure:
nlmsg_free(msg);
return err;
}
int __near_netlink_dep_link_up(uint32_t idx, uint32_t target_idx,
uint8_t comm_mode, uint8_t rf_mode)
{
struct nl_msg *msg;
void *hdr;
int err;
DBG("");
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
NLM_F_REQUEST, NFC_CMD_DEP_LINK_UP, NFC_GENL_VERSION);
if (!hdr) {
err = -EINVAL;
goto nla_put_failure;
}
err = -EMSGSIZE;
NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx);
NLA_PUT_U8(msg, NFC_ATTR_COMM_MODE, comm_mode);
NLA_PUT_U8(msg, NFC_ATTR_RF_MODE, rf_mode);
err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
nla_put_failure:
nlmsg_free(msg);
return err;
}
int __near_netlink_activate_target(uint32_t idx, uint32_t target_idx,
uint32_t protocol)
{
struct nl_msg *msg;
void *hdr;
int err;
DBG("");
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
NLM_F_REQUEST, NFC_CMD_ACTIVATE_TARGET,
NFC_GENL_VERSION);
if (!hdr) {
err = -EINVAL;
goto nla_put_failure;
}
err = -EMSGSIZE;
NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
NLA_PUT_U32(msg, NFC_ATTR_TARGET_INDEX, target_idx);
NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, protocol);
err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
nla_put_failure:
nlmsg_free(msg);
return err;
}
int __near_netlink_get_adapters(void)
{
struct nl_msg *msg;
void *hdr;
int err;
DBG("");
if (!nfc_state || nfc_state->nfc_id < 0)
return -ENODEV;
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
NLM_F_DUMP, NFC_CMD_GET_DEVICE, NFC_GENL_VERSION);
if (!hdr) {
err = -EINVAL;
goto out;
}
err = nl_send_msg(nfc_state->cmd_sock, msg, get_devices_handler, NULL);
out:
nlmsg_free(msg);
return err;
}
int __near_netlink_start_poll(int idx,
uint32_t im_protocols, uint32_t tm_protocols)
{
struct nl_msg *msg;
void *hdr;
int err;
DBG("IM protos 0x%x TM protos 0x%x", im_protocols, tm_protocols);
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
NLM_F_REQUEST, NFC_CMD_START_POLL, NFC_GENL_VERSION);
if (!hdr) {
err = -EINVAL;
goto nla_put_failure;
}
err = -EMSGSIZE;
NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
if (im_protocols != 0) {
NLA_PUT_U32(msg, NFC_ATTR_IM_PROTOCOLS, im_protocols);
NLA_PUT_U32(msg, NFC_ATTR_PROTOCOLS, im_protocols);
}
if (tm_protocols != 0)
NLA_PUT_U32(msg, NFC_ATTR_TM_PROTOCOLS, tm_protocols);
err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
nla_put_failure:
nlmsg_free(msg);
return err;
}
int __near_netlink_adapter_enable(int idx, bool enable)
{
struct nl_msg *msg;
void *hdr;
int err;
uint8_t cmd;
DBG("");
msg = nlmsg_alloc();
if (!msg)
return -ENOMEM;
if (enable)
cmd = NFC_CMD_DEV_UP;
else
cmd = NFC_CMD_DEV_DOWN;
hdr = genlmsg_put(msg, NL_AUTO_PID, NL_AUTO_SEQ, nfc_state->nfc_id, 0,
NLM_F_REQUEST, cmd, NFC_GENL_VERSION);
if (!hdr) {
err = -EINVAL;
goto nla_put_failure;
}
err = -EMSGSIZE;
NLA_PUT_U32(msg, NFC_ATTR_DEVICE_INDEX, idx);
err = nl_send_msg(nfc_state->cmd_sock, msg, NULL, NULL);
nla_put_failure:
nlmsg_free(msg);
return err;
}
int nl_send(int pid, void *msg_buf, int msg_len, int flags) {
int ret;
void *part_buf;
u_char *msg_pt;
int pos;
u_int seq;
struct nl_stub80211_to_FINS_hdr *part_hdr;
//u_char *hdr_msg_len;
//u_char *hdr_part_len;
//u_char *hdr_pos;
u_char *msg_start;
int header_size;
int part_len;
//#################### Debug
#ifdef DEBUG
u_char *print_buf;
u_char *print_pt;
u_char *pt;
int i;
#endif
//####################
PRINT_DEBUG("Entered: pid=%d, msg_buf=%p, msg_len=%d, flags=0x%x", pid, msg_buf, msg_len, flags);
if (down_interruptible(&link_sem)) {
PRINT_ERROR("link_sem acquire fail");
}
//#################### Debug
#ifdef DEBUG
if (0) {
print_buf = (u_char *) kmalloc(3 * msg_len+1, GFP_KERNEL);
if (print_buf == NULL) {
PRINT_ERROR("print_buf allocation fail");
} else {
print_pt = print_buf;
pt = msg_buf;
for (i = 0; i < msg_len; i++) {
if (i == 0) {
sprintf(print_pt, "%02x", *(pt + i));
print_pt += 2;
} else if (i % 4 == 0) {
sprintf(print_pt, ":%02x", *(pt + i));
print_pt += 3;
} else {
sprintf(print_pt, " %02x", *(pt + i));
print_pt += 3;
}
}
*print_pt = '\0';
PRINT_DEBUG("msg_buf='%s'", print_buf);
kfree(print_buf);
}
}
#endif
//####################
part_buf = (u_char *) kmalloc(RECV_BUFFER_SIZE, GFP_KERNEL);
if (part_buf == NULL) {
PRINT_ERROR("part_buf allocation fail");
up(&link_sem);
return -1;
}
msg_pt = msg_buf;
pos = 0;
seq = 0;
part_hdr = (struct nl_stub80211_to_FINS_hdr *) part_buf;
//hdr_msg_len = part_buf;
//hdr_part_len = hdr_msg_len + sizeof(int);
//hdr_pos = hdr_part_len + sizeof(int);
//msg_start = hdr_pos + sizeof(int);
msg_start = part_buf + sizeof(struct nl_stub80211_to_FINS_hdr);
//header_size = msg_start - hdr_msg_len;
header_size = sizeof(struct nl_stub80211_to_FINS_hdr);
part_len = RECV_BUFFER_SIZE - header_size;
//*(int *) hdr_msg_len = msg_len;
//*(int *) hdr_part_len = part_len;
part_hdr->msg_len = msg_len;
part_hdr->part_len = part_len;
while (msg_len - pos > part_len) {
PRINT_DEBUG("pos=%d, seq=%d", pos, seq);
//*(int *) hdr_pos = pos;
part_hdr->pos = pos;
memcpy(msg_start, msg_pt, part_len);
ret = nl_send_msg(pid, seq, 0x0, part_buf, RECV_BUFFER_SIZE, flags/*| NLM_F_MULTI*/);
if (ret < 0) {
PRINT_ERROR("netlink error sending seq %d to user", seq);
up(&link_sem);
PRINT_DEBUG("Exited: pid=%d, msg_buf=%p, msg_len=%d, flags=0x%x, ret=%d", pid, msg_buf, msg_len, flags, -1);
return -1;
}
//msleep(1);
msg_pt += part_len;
pos += part_len;
seq++;
}
PRINT_DEBUG("pos=%d, seq=%d", pos, seq);
part_len = msg_len - pos;
//*(int *) hdr_part_len = part_len;
//*(int *) hdr_pos = pos;
part_hdr->part_len = part_len;
part_hdr->pos = pos;
memcpy(msg_start, msg_pt, part_len);
ret = nl_send_msg(pid, seq, NLMSG_DONE, part_buf, header_size + part_len, flags);
if (ret < 0) {
PRINT_ERROR("netlink error sending seq %d to user", seq);
up(&link_sem);
PRINT_DEBUG("Exited: pid=%d, msg_buf=%p, msg_len=%d, flags=0x%x, ret=%d", pid, msg_buf, msg_len, flags, -1);
return -1;
}
kfree(part_buf);
up(&link_sem);
PRINT_DEBUG("Exited: pid=%d, msg_buf=%p, msg_len=%d, flags=0x%x, ret=%d", pid, msg_buf, msg_len, flags, 0);
return 0;
} // end of function nl_send
