void
test_llc_link_find_sap_by_uri(void)
{
struct llc_link *link;
int res;
link = llc_link_new();
cut_assert_not_null(link, cut_message("llc_link_new()"));
struct llc_service *service = llc_service_new_with_uri(NULL, void_service, "urn:nfc:xsn:foo", NULL);
res = llc_link_service_bind(link, service, -1);
cut_assert_not_equal_int(-1, res, cut_message("llc_link_service_bind()"));
uint8_t sap = llc_link_find_sap_by_uri(link, "urn:nfc:xsn:foo");
cut_assert_not_equal_int(0, sap, cut_message("llc_link_find_sap_by_uri()"));
cut_assert_equal_int(res, sap, cut_message("Wrong SAP"));
llc_link_service_unbind(link, service->sap);
sap = llc_link_find_sap_by_uri(link, "urn:nfc:xsn:foo");
cut_assert_equal_int(0, sap, cut_message("llc_link_find_sap_by_uri()"));
llc_service_free(service);
llc_link_free(link);
}
int
llc_connection_start (struct llc_connection *connection)
{
assert (connection);
struct mq_attr attr_up = {
.mq_msgsize = 3 + connection->local_miu,
.mq_maxmsg = 2,
};
if (asprintf (&connection->mq_up_name, "/libnfc-llcp-%d-%p-%s", getpid(), (void *) connection, "up") < 0) {
LLC_CONNECTION_LOG (LLC_PRIORITY_FATAL, "Cannot print to allocated string");
return -1;
}
connection->llc_up = mq_open (connection->mq_up_name, O_RDWR | O_CREAT, 0666, &attr_up);
if (connection->llc_up == (mqd_t) -1) {
LLC_CONNECTION_LOG (LLC_PRIORITY_FATAL, "Cannot open message queue '%s'", connection->mq_up_name);
llc_connection_free (connection);
return -1;
}
struct mq_attr attr_down = {
.mq_msgsize = 3 + connection->remote_miu,
.mq_maxmsg = 2,
};
if (asprintf (&connection->mq_down_name, "/libnfc-llcp-%d-%p-%s", getpid(), (void *) connection, "down") < 0) {
LLC_CONNECTION_LOG (LLC_PRIORITY_FATAL, "Cannot print to allocated string");
return -1;
}
connection->llc_down = mq_open (connection->mq_down_name, O_RDWR | O_CREAT | O_NONBLOCK, 0666, &attr_down);
if (connection->llc_down == (mqd_t) -1) {
LLC_CONNECTION_LOG (LLC_PRIORITY_FATAL, "Cannot open message queue '%s'", connection->mq_down_name);
llc_connection_free (connection);
return -1;
}
return 0;
}
struct llc_connection *
llc_data_link_connection_new (struct llc_link *link, const struct pdu *pdu, int *reason)
{
assert (link);
assert (pdu);
assert (reason);
struct llc_connection *res;
char sn[BUFSIZ];
int8_t service_sap = pdu->dsap;
uint16_t miux, miu = LLCP_DEFAULT_MIU;
uint8_t rw = 2;
*reason = -1;
size_t offset = 0;
while (offset < pdu->information_size) {
if (offset > pdu->information_size - 2) {
LLC_CONNECTION_MSG (LLC_PRIORITY_ERROR, "Incomplete TLV field in parameters list");
return NULL;
}
if (offset + 2 + pdu->information[offset+1] > pdu->information_size) {
LLC_CONNECTION_LOG (LLC_PRIORITY_ERROR, "Incomplete TLV value in parameters list (expected %d bytes but only %d left)", pdu->information[offset+1], pdu->information_size - (offset + 2));
return NULL;
}
switch (pdu->information[offset]) {
case LLCP_PARAMETER_MIUX:
if (parameter_decode_miux (pdu->information + offset, 2 + pdu->information[offset+1], &miux) < 0) {
LLC_CONNECTION_MSG (LLC_PRIORITY_ERROR, "Invalid MIUX parameter");
return NULL;
}
miu = 128 + miux;
break;
case LLCP_PARAMETER_RW:
if (parameter_decode_rw (pdu->information + offset, 2 + pdu->information[offset+1], &rw) < 0) {
LLC_CONNECTION_MSG (LLC_PRIORITY_ERROR, "Invalid RW parameter");
return NULL;
}
break;
case LLCP_PARAMETER_SN:
if (parameter_decode_sn (pdu->information + offset, 2 + pdu->information[offset+1], sn, sizeof (sn)) < 0) {
LLC_CONNECTION_MSG (LLC_PRIORITY_ERROR, "Invalid SN parameter");
return NULL;
}
if (pdu->dsap == 0x01) {
service_sap = llc_link_find_sap_by_uri (link, sn);
if (!service_sap) {
*reason = 0x02;
return NULL;
}
} else {
LLC_CONNECTION_LOG (LLC_PRIORITY_ERROR, "Ignoring SN parameter (DSAP is %d, not 1)", pdu->dsap);
}
break;
default:
LLC_CONNECTION_LOG (LLC_PRIORITY_INFO, "Unknown TLV Field 0x%02x (length: %d)",
pdu->information[offset], pdu->information[offset+1]);
}
offset += 2 + pdu->information[offset+1];
}
if (!link->available_services[service_sap]) {
*reason = 0x02;
return NULL;
}
int8_t connection_dsap = service_sap;
while (link->transmission_handlers[connection_dsap] && (connection_dsap <= MAX_LLC_LINK_SERVICE))
connection_dsap++;
if (connection_dsap > MAX_LLC_LINK_SERVICE) {
return NULL;
}
if ((res = llc_connection_new (link, connection_dsap, pdu->ssap))) {
assert (!link->transmission_handlers[connection_dsap]);
link->transmission_handlers[connection_dsap] = res;
res->service_sap = service_sap;
res->status = DLC_NEW;
res->rwr = rw;
res->remote_miu = miu;
res->local_miu = link->available_services[service_sap]->miu;
if (llc_connection_start (res) < 0) {
llc_connection_free (res);
return NULL;
}
}
return res;
}
struct llc_connection *
llc_outgoing_data_link_connection_new (struct llc_link *link, uint8_t local_sap, uint8_t remote_sap)
{
struct llc_connection *res;
if ((res = llc_connection_new (link, local_sap, remote_sap))) {
link->transmission_handlers[local_sap] = res;
res->service_sap = local_sap;
res->status = DLC_NEW;
//res->rwr = rw;
//res->remote_miu = miu;
res->local_miu = link->available_services[local_sap]->miu;
if (llc_connection_start (res) < 0) {
llc_connection_free (res);
return NULL;
}
}
return res;
}
struct llc_connection *
llc_outgoing_data_link_connection_new_by_uri (struct llc_link *link, uint8_t local_sap, const char *remote_uri)
{
struct llc_connection *res;
if ((res = llc_connection_new (link, local_sap, 1))) {
link->transmission_handlers[local_sap] = res;
res->service_sap = local_sap;
res->status = DLC_NEW;
//res->rwr = rw;
//res->remote_miu = miu;
res->local_miu = link->available_services[local_sap]->miu;
res->remote_uri = strdup (remote_uri);
if (llc_connection_start (res) < 0) {
llc_connection_free (res);
return NULL;
}
}
return res;
}
struct llc_connection *
llc_logical_data_link_new (struct llc_link *link, const struct pdu *pdu)
{
assert (link);
assert (pdu);
struct llc_connection *res;
uint8_t sap = 0;
if (!link->available_services[pdu->dsap]) {
return NULL;
}
while (link->datagram_handlers[sap] && (sap <= MAX_LOGICAL_DATA_LINK))
sap++;
if (sap > MAX_LOGICAL_DATA_LINK) {
LLC_CONNECTION_MSG (LLC_PRIORITY_CRIT, "No place left for new Logical Data Link");
return NULL;
}
if ((res = llc_connection_new (link, pdu->dsap, pdu->ssap))) {
link->datagram_handlers[sap] = res;
if (llc_connection_start (res) < 0) {
llc_connection_free (res);
return NULL;
}
}
return res;
}
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;
}
