static void *
com_android_snep_service(void *arg)
{
struct llc_connection *connection = (struct llc_connection *) arg;
uint8_t frame[] = {
0x10, 0x02,
0x00, 0x00, 0x00, 33,
0xd1, 0x02, 0x1c, 0x53, 0x70, 0x91, 0x01, 0x09, 0x54, 0x02,
0x65, 0x6e, 0x4c, 0x69, 0x62, 0x6e, 0x66, 0x63, 0x51, 0x01,
0x0b, 0x55, 0x03, 0x6c, 0x69, 0x62, 0x6e, 0x66, 0x63, 0x2e,
0x6f, 0x72, 0x67
};
uint8_t buf[1024];
int ret;
uint8_t ssap;
llc_connection_send(connection, frame, sizeof(frame));
ret = llc_connection_recv(connection, buf, sizeof(buf), &ssap);
if(ret>0){
printf("Send NDEF message done.\n");
}else if(ret == 0){
printf("Received no data\n");
}else{
printf("Error received data.");
}
llc_connection_stop(connection);
return NULL;
}
static void *
com_android_npp_service(void *arg)
{
struct llc_connection *connection = (struct llc_connection *) arg;
sleep(1);
uint8_t frame[] = { 0x01, // Protocol version
0x00, 0x00, 0x00, 0x01, // NDEF entries count
0x01, // Action code
0x00, 0x00, 0x00, 33, // NDEF length
// NDEF
0xd1, 0x02, 0x1c, 0x53, 0x70, 0x91, 0x01, 0x09, 0x54, 0x02,
0x65, 0x6e, 0x4c, 0x69, 0x62, 0x6e, 0x66, 0x63, 0x51, 0x01,
0x0b, 0x55, 0x03, 0x6c, 0x69, 0x62, 0x6e, 0x66, 0x63, 0x2e,
0x6f, 0x72, 0x67
};
llc_connection_send(connection, frame, sizeof(frame));
llc_connection_stop(connection);
return NULL;
}
void *
llc_service_llc_thread(void *arg)
{
struct llc_link *link = (struct llc_link *)arg;
mqd_t llc_up, llc_down;
int old_cancelstate;
#if defined(HAVE_DECL_PTHREAD_SET_NAME_NP) && HAVE_DECL_PTHREAD_SET_NAME_NP
char *thread_name;
#endif
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &old_cancelstate);
llc_up = mq_open(link->mq_up_name, O_RDONLY);
llc_down = mq_open(link->mq_down_name, O_WRONLY);
if (llc_up == (mqd_t) - 1)
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "mq_open(%s)", link->mq_up_name);
if (llc_down == (mqd_t) - 1)
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "mq_open(%s)", link->mq_down_name);
pthread_cleanup_push(llc_service_llc_thread_cleanup, arg);
pthread_setcancelstate(old_cancelstate, NULL);
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "Link activated");
for (;;) {
int res;
uint8_t buffer[1024];
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "mq_receive+");
pthread_testcancel();
res = mq_receive(llc_up, (char *) buffer, sizeof(buffer), NULL);
pthread_testcancel();
if (res < 0) {
pthread_testcancel();
}
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Received %d bytes", res);
if (res < 2) {
/* FIXME: Maybe we'd rather quit */
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_FATAL, "Too short for a PDU (expected 2 bytes, got %d)", res);
buffer[0] = buffer[1] = '\0';
res = 2;
}
struct pdu *pdu;
struct pdu **pdus, **p;
struct llc_connection *connection;
pthread_setcancelstate(PTHREAD_CANCEL_DISABLE, &old_cancelstate);
pdu = pdu_unpack((uint8_t *) buffer, res);
switch (pdu->ptype) {
case PDU_SYMM:
assert(!pdu->dsap);
assert(!pdu->ssap);
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Symmetry PDU");
break;
case PDU_PAX:
assert(!pdu->dsap);
assert(!pdu->ssap);
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Parameter Exchange PDU");
assert(0 == llc_link_configure(link, pdu->information, pdu->information_size));
break;
case PDU_AGF:
assert(!pdu->dsap);
assert(!pdu->ssap);
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Aggregated Frame PDU");
p = pdus = pdu_dispatch(pdu);
while (*p) {
uint8_t buffer[BUFSIZ];
ssize_t length = pdu_pack(*p, buffer, sizeof(buffer));
mq_send(link->llc_up, (char *) buffer, length, 1);
pdu_free(*p);
p++;
}
free(pdus);
break;
case PDU_SNL:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Service Name Lookup PDU");
if (!((link->version.major == 1) && (link->version.minor >= 1))) {
/*
* Even if we negociate LLCP 1.0, some LLCP implementation will
* use LLCP 1.1 SNL to discover available services so warn
* about this problem but perform th operation anyway.
*/
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ALERT, "SNL PDU (LLCP 1.1) received on LLCP %d.%d link", link->version.major, link->version.minor);
}
goto spawn_logical_data_link;
case PDU_UI:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Unnumbered Information PDU");
spawn_logical_data_link:
if (!link->available_services[pdu->dsap]) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_FATAL, "No service bound to SAP %d", pdu->dsap);
break;
}
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Spawning Logical Data Link [%d -> %d]", pdu->ssap, pdu->dsap);
if (!(connection = llc_logical_data_link_new(link, pdu))) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Cannot establish Logical Data Link [%d -> %d]", pdu->ssap, pdu->dsap);
break;
}
connection->user_data = link->available_services[pdu->dsap]->user_data;
if (pthread_create(&connection->thread, NULL, link->available_services[pdu->dsap]->thread_routine, connection) < 0) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Cannot launch Logical Data Link [%d -> %d] thread", connection->local_sap, connection->remote_sap);
break;
}
#if defined(HAVE_DECL_PTHREAD_SET_NAME_NP) && HAVE_DECL_PTHREAD_SET_NAME_NP
asprintf(&thread_name, "LDL on SAP %d", connection->service_sap);
pthread_set_name_np(connection->thread, thread_name);
free(thread_name);
#endif
if (mq_send(connection->llc_up, (char *) buffer, res, 0) < 0) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Cannot send data to Logical Data Link [%d -> %d]", connection->local_sap, connection->remote_sap);
break;
}
break;
case PDU_RR:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Receive Ready PDU");
assert(link->transmission_handlers[pdu->dsap]);
link->transmission_handlers[pdu->dsap]->state.sa = pdu->nr;
break;
case PDU_CONNECT:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Connect PDU");
if (!link->available_services[pdu->dsap]) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_FATAL, "No service bound to SAP %d", pdu->dsap);
struct pdu *reply;
int len;
uint8_t reason[] = { 0x02 }; // 0x02 ==> no service bound to the specified target SAP
reply = pdu_new_dm(pdu->ssap, pdu->dsap, reason);
len = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
if (mq_send(llc_down, (char *) buffer, len, 0) < 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Cannot reject connection");
}
break;
}
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Spawning Data Link Connection [%d -> %d] accept routine", pdu->ssap, pdu->dsap);
int error;
if (!(connection = llc_data_link_connection_new(link, pdu, &error))) {
struct pdu *reply;
int len;
uint8_t reason[] = { error };
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Cannot establish Data Link Connection [%d -> %d] (reason = %02x)", pdu->ssap, pdu->dsap, error);
reply = pdu_new_dm(pdu->ssap, pdu->dsap, reason);
len = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
if (mq_send(llc_down, (char *) buffer, len, 0) < 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Can't Reject connection");
}
break;
}
if (!link->available_services[connection->service_sap]->accept_routine) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Data Link Connection [%d -> %d] accepted (no accept routine provided)", connection->local_sap, connection->remote_sap);
connection->status = DLC_ACCEPTED;
} else if (pthread_create(&connection->thread, NULL, link->available_services[connection->service_sap]->accept_routine, connection) < 0) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Cannot launch Data Link Connection [%d -> %d] accept routine", connection->local_sap, connection->remote_sap);
break;
}
#if defined(HAVE_DECL_PTHREAD_SET_NAME_NP) && HAVE_DECL_PTHREAD_SET_NAME_NP
asprintf(&thread_name, "DLC Accept on SAP %d", connection->service_sap);
pthread_set_name_np(connection->thread, thread_name);
free(thread_name);
#endif
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Data Link Connection [%d -> %d] accept routine launched (service %d)", connection->local_sap, connection->remote_sap, connection->service_sap);
break;
case PDU_DISC:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Disconnect PDU");
if (!pdu->dsap && !pdu->ssap) {
link->status = LL_DEACTIVATED;
pthread_exit((void *) 2);
break;
} else {
struct pdu *reply;
llc_connection_stop(link->transmission_handlers[pdu->dsap]);
llc_connection_free(link->transmission_handlers[pdu->dsap]);
link->transmission_handlers[pdu->dsap] = NULL;
uint8_t reason[1] = { 0x00 };
reply = pdu_new_dm(pdu->ssap, pdu->dsap, reason);
int len = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
if (mq_send(llc_down, (char *) buffer, len, 0) < 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Can't send DM");
}
}
break;
case PDU_CC:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "Connection Complete PDU");
connection = link->transmission_handlers[pdu->dsap];
connection->remote_sap = pdu->ssap;
connection->status = DLC_RECEIVED_CC;
break;
case PDU_DM:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Disconnected Mode PDU");
llc_connection_stop(link->transmission_handlers[pdu->dsap]);
link->transmission_handlers[pdu->dsap]->status = DLC_REJECTED;
break;
case PDU_I:
assert(link->transmission_handlers[pdu->dsap]);
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Information PDU");
#if defined(HAVE_DEBUG)
struct mq_attr attr;
mq_getattr(link->transmission_handlers[pdu->dsap]->llc_up, &attr);
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_DEBUG, "MQ: %d / %d x %d bytes", attr.mq_curmsgs, attr.mq_maxmsg, attr.mq_msgsize);
#endif
if (pdu->ns != link->transmission_handlers[pdu->dsap]->state.r) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Invalid N(S)");
struct pdu *reply = pdu_new_frmr(pdu->ssap, pdu->dsap, pdu, link->transmission_handlers[pdu->dsap], FRMR_S);
int len = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
if (mq_send(llc_down, (char *) buffer, len, 0) < 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Can't send FRMR");
}
break;
}
if (pdu->information_size > link->transmission_handlers[pdu->dsap]->local_miu) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_FATAL, "Information PDU too long: %d (MIU: %d)", pdu->information_size, link->transmission_handlers[pdu->dsap]->local_miu);
struct pdu *reply = pdu_new_frmr(pdu->ssap, pdu->dsap, pdu, link->transmission_handlers[pdu->dsap], FRMR_I);
int len = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
if (mq_send(llc_down, (char *) buffer, len, 0) < 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Can't send FRMR");
}
break;
}
INC_MOD_16(link->transmission_handlers[pdu->dsap]->state.r);
link->transmission_handlers[pdu->dsap]->state.sa = pdu->nr;
if (mq_send(link->transmission_handlers[pdu->dsap]->llc_up, (char *) buffer, res, 0) < 0) {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Error sending %d bytes to service %d", res, pdu->dsap);
} else {
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_INFO, "Send %d bytes to service %d", res, pdu->dsap);
}
break;
case PDU_FRMR:
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "Frame Reject PDU");
assert(pdu->information_size == 4);
if (pdu->information[0] & 0x80) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "PDU was invalid or malformed");
} else {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "PDU was valid and wellformed");
}
if (pdu->information[0] & 0x40) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "PDU has incorect or unexpected information field");
} else {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "PDU has no incorect or unexpected information field");
}
if (pdu->information[0] & 0x20) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "PDU contains an invalid receive sequence number N(R)");
} else {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "PDU contains a valid receive sequence number N(R)");
}
if (pdu->information[0] & 0x10) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "PDU contains an invalid send sequence number N(S)");
} else {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "PDU contains a valid send sequence number N(S)");
}
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "Rejected frame informations:");
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, " PDU type: %d", pdu->information[0] & 0x0F);
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, " Sequence: %02x", pdu->information[1]);
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_ERROR, "Receiver status:");
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, " V(S): %02x", pdu->information[2] >> 4);
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, " V(R): %02x", pdu->information[2] & 0x0F);
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, " V(SA): %02x", pdu->information[3] >> 4);
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, " V(RA): %02x", pdu->information[3] & 0x0F);
break;
default:
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_WARN, "Unsupported LLC PDU: 0x%02x", pdu->ptype);
abort();
}
pdu_free(pdu);
pthread_setcancelstate(old_cancelstate, NULL);
/* ---------------- */
ssize_t length = 0;
for (int i = 0; i <= MAX_LOGICAL_DATA_LINK; i++) {
if (link->datagram_handlers[i]) {
pthread_t thread = link->datagram_handlers[i]->thread;
length = mq_receive(link->datagram_handlers[i]->llc_down, (char *) buffer, sizeof(buffer), NULL);
if (length > 0)
break;
switch (errno) {
case EAGAIN:
if (!thread) {
/*
* The service is not running anymore and it's down
* queue is empty. It can be garbage collected.
*/
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Garbage-collecting Logical Data Link [%d -> %d]", link->datagram_handlers[i]->local_sap, link->datagram_handlers[i]->remote_sap);
llc_connection_free(link->datagram_handlers[i]);
link->datagram_handlers[i] = NULL;
}
/* FALLTHROUGH */
case EINTR:
case ETIMEDOUT: /* XXX Should not happend */
/* NOOP */
break;
default:
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Can' read from service %d message queue", i);
break;
}
}
}
for (int i = 1; i <= MAX_LLC_LINK_SERVICE; i++) {
if (link->transmission_handlers[i]) {
pthread_t thread = link->transmission_handlers[i]->thread;
length = mq_receive(link->transmission_handlers[i]->llc_down, (char *) buffer, sizeof(buffer), NULL);
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Read %d bytes from service %d", length, i);
if (length > 0) {
#if defined(HAVE_DEBUG)
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_DEBUG, "%d %d %d %d",
link->transmission_handlers[i]->state.s,
link->transmission_handlers[i]->state.sa,
link->transmission_handlers[i]->state.r,
link->transmission_handlers[i]->state.ra
);
#endif
struct pdu *pdu = pdu_unpack(buffer, length);
if (pdu->ptype == PDU_I) {
if (link->transmission_handlers[i]->state.s == link->transmission_handlers[i]->state.sa + link->transmission_handlers[i]->rwr) {
/*
* We can't send data now
*/
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_WARN, "Data Link Connection [%d -> %d] send-window is full. Postponing message delivery", link->transmission_handlers[i]->local_sap, link->transmission_handlers[i]->remote_sap);
mq_send(link->transmission_handlers[i]->llc_down, (char *) buffer, length, 1);
length = -1;
continue;
}
}
struct pdu *reply = pdu_new_i(pdu->dsap, pdu->ssap, link->transmission_handlers[i], pdu->information, pdu->information_size);
length = pdu_pack(pdu, buffer, sizeof(buffer));
free(reply);
pdu_free(pdu);
INC_MOD_16(link->transmission_handlers[i]->state.s);
break;
}
switch (errno) {
case EAGAIN:
if (thread) {
/*
* If we have received some data not yet acknoledge, do it now.
*/
#if defined(HAVE_DEBUG)
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_DEBUG, "%d %d %d %d",
link->transmission_handlers[i]->state.s,
link->transmission_handlers[i]->state.sa,
link->transmission_handlers[i]->state.r,
link->transmission_handlers[i]->state.ra
);
#endif
if (link->transmission_handlers[i]->state.ra != link->transmission_handlers[i]->state.r) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_WARN, "Send acknoledgment for received data");
struct pdu *reply;
struct mq_attr attr;
mq_getattr(link->transmission_handlers[i]->llc_up, &attr);
if (attr.mq_curmsgs == attr.mq_maxmsg) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_INFO, "Message queue is full");
reply = pdu_new_rnr(link->transmission_handlers[i]);
} else {
reply = pdu_new_rr(link->transmission_handlers[i]);
}
length = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
link->transmission_handlers[i]->state.ra = link->transmission_handlers[i]->state.r;
break;
}
} else {
struct pdu *reply;
uint8_t reason[] = { 0x00 };
switch (link->transmission_handlers[i]->status) {
case DLC_NEW:
case DLC_CONNECTED:
/*
* The llc_connection thread is running.
* Do nothing.
*/
break;
case DLC_ACCEPTED:
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Data Link Connection [%d -> %d] accepted (service %d). Sending CC", connection->local_sap, connection->remote_sap, connection->service_sap);
reply = pdu_new_cc(connection);
length = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
/* FALLTHROUGH */
case DLC_RECEIVED_CC:
connection->user_data = link->available_services[connection->service_sap]->user_data;
if (pthread_create(&connection->thread, NULL, connection->link->available_services[connection->service_sap]->thread_routine, connection) < 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_FATAL, "Cannot start Data Link Connection thread");
link->transmission_handlers[i]->status = DLC_DISCONNECTED;
break;
}
#if defined(HAVE_DECL_PTHREAD_SET_NAME_NP) && HAVE_DECL_PTHREAD_SET_NAME_NP
asprintf(&thread_name, "DLC on SAP %d", connection->service_sap);
pthread_set_name_np(connection->thread, thread_name);
free(thread_name);
#endif
link->transmission_handlers[i]->status = DLC_CONNECTED;
break;
case DLC_REJECTED:
reason[0] = 0x03;
link->transmission_handlers[i]->status = DLC_DISCONNECTED;
/* FALLTHROUGH */
case DLC_DISCONNECTED:
reply = pdu_new_dm(connection->remote_sap, connection->local_sap, reason);
length = pdu_pack(reply, buffer, sizeof(buffer));
pdu_free(reply);
link->transmission_handlers[i]->status = DLC_TERMINATED;
/* FALLTHROUGH */
case DLC_TERMINATED:
/*
* The service is not running anymore and it's down
* queue is empty. It can be garbage collected.
*/
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Garbage-collecting Data Link Connection [%d -> %d]", link->transmission_handlers[i]->local_sap, link->transmission_handlers[i]->remote_sap);
llc_connection_free(link->transmission_handlers[i]);
link->transmission_handlers[i] = NULL;
break;
}
}
/* FALLTHROUGH */
case EINTR:
case ETIMEDOUT: /* XXX Should not happend */
/* NOOP */
break;
default:
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_ERROR, "Can't read from service %d message queue", i);
break;
}
}
}
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "mq_send+");
pthread_testcancel();
if (length <= 0) {
LLC_SERVICE_LLC_MSG(LLC_PRIORITY_TRACE, "Nothing to send");
continue;
}
res = mq_send(llc_down, (char *) buffer, length, 0);
pthread_testcancel();
if (res < 0) {
pthread_testcancel();
}
LLC_SERVICE_LLC_LOG(LLC_PRIORITY_TRACE, "Sent %d bytes", length);
}
pthread_cleanup_pop(1);
return NULL;
}
void *
llc_service_sdp_thread (void *arg)
{
struct llc_connection *connection = (struct llc_connection *) arg;
mqd_t llc_up, llc_down;
int old_cancelstate;
pthread_setcancelstate (PTHREAD_CANCEL_DISABLE, &old_cancelstate);
llc_up = mq_open (connection->mq_up_name, O_RDONLY);
llc_down = mq_open (connection->mq_down_name, O_WRONLY);
if (llc_up == (mqd_t)-1)
LLC_SDP_LOG (LLC_PRIORITY_ERROR, "mq_open(%s)", connection->mq_up_name);
if (llc_down == (mqd_t)-1)
LLC_SDP_LOG (LLC_PRIORITY_ERROR, "mq_open(%s)", connection->mq_down_name);
pthread_cleanup_push (llc_service_sdp_thread_cleanup, arg);
pthread_setcancelstate (old_cancelstate, NULL);
LLC_SDP_MSG (LLC_PRIORITY_INFO, "Service Discovery Protocol started");
int res;
uint8_t buffer[1024];
LLC_SDP_MSG (LLC_PRIORITY_TRACE, "mq_receive+");
pthread_testcancel ();
res = mq_receive (llc_up, (char *) buffer, sizeof (buffer), NULL);
pthread_testcancel ();
if (res < 0) {
pthread_testcancel ();
}
LLC_SDP_LOG (LLC_PRIORITY_TRACE, "Received %d bytes", res);
uint8_t tid;
char *uri;
switch (buffer[2]) {
case LLCP_PARAMETER_SDREQ:
if (parameter_decode_sdreq (buffer + 2, res - 2, &tid, &uri) < 0) {
LLC_SDP_MSG (LLC_PRIORITY_ERROR, "Ignoring PDU");
} else {
LLC_SDP_LOG (LLC_PRIORITY_TRACE, "Service Discovery Request #0x%02x for '%s'", tid, uri);
uint8_t sap = llc_link_find_sap_by_uri (connection->link, uri);
if (!sap) {
LLC_SDP_LOG (LLC_PRIORITY_ERROR, "No registered service provide '%s'", uri);
}
buffer[0] = 0x06;
buffer[1] = 0x41;
int n = parameter_encode_sdres (buffer + 2, sizeof (buffer) -2, tid, sap);
mq_send (llc_down, (char *) buffer, n + 2, 0);
LLC_SDP_LOG (LLC_PRIORITY_TRACE, "Sent %d bytes", n+2);
}
break;
default:
LLC_SDP_MSG (LLC_PRIORITY_ERROR, "Invalid parameter type");
}
pthread_cleanup_pop (1);
llc_connection_stop (connection);
return NULL;
}
