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