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_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;
}
