126 static struct sk_buff *llc_shdlc_alloc_skb(const struct llc_shdlc *shdlc,  in llc_shdlc_alloc_skb()  argument
131 	skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +  in llc_shdlc_alloc_skb()
132 			shdlc->tx_tailroom + payload_len, GFP_KERNEL);  in llc_shdlc_alloc_skb()
134 		skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);  in llc_shdlc_alloc_skb()
140 static int llc_shdlc_send_s_frame(const struct llc_shdlc *shdlc,  in llc_shdlc_send_s_frame()  argument
148 	skb = llc_shdlc_alloc_skb(shdlc, 0);  in llc_shdlc_send_s_frame()
154 	r = shdlc->xmit_to_drv(shdlc->hdev, skb);  in llc_shdlc_send_s_frame()
162 static int llc_shdlc_send_u_frame(const struct llc_shdlc *shdlc,  in llc_shdlc_send_u_frame()  argument
172 	r = shdlc->xmit_to_drv(shdlc->hdev, skb);  in llc_shdlc_send_u_frame()
183 static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)  in llc_shdlc_reset_t2()  argument
186 	int dnr = shdlc->dnr;	/* MUST initially be < y_nr */  in llc_shdlc_reset_t2()
193 		skb = skb_dequeue(&shdlc->ack_pending_q);  in llc_shdlc_reset_t2()
199 	if (skb_queue_empty(&shdlc->ack_pending_q)) {  in llc_shdlc_reset_t2()
200 		if (shdlc->t2_active) {  in llc_shdlc_reset_t2()
201 			del_timer_sync(&shdlc->t2_timer);  in llc_shdlc_reset_t2()
202 			shdlc->t2_active = false;  in llc_shdlc_reset_t2()
207 		skb = skb_peek(&shdlc->ack_pending_q);  in llc_shdlc_reset_t2()
209 		mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +  in llc_shdlc_reset_t2()
211 		shdlc->t2_active = true;  in llc_shdlc_reset_t2()
221 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,  in llc_shdlc_rcv_i_frame()  argument
229 	if (shdlc->state != SHDLC_CONNECTED)  in llc_shdlc_rcv_i_frame()
232 	if (x_ns != shdlc->nr) {  in llc_shdlc_rcv_i_frame()
233 		llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);  in llc_shdlc_rcv_i_frame()
237 	if (!shdlc->t1_active) {  in llc_shdlc_rcv_i_frame()
238 		shdlc->t1_active = true;  in llc_shdlc_rcv_i_frame()
239 		mod_timer(&shdlc->t1_timer, jiffies +  in llc_shdlc_rcv_i_frame()
240 			  msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));  in llc_shdlc_rcv_i_frame()
245 		shdlc->rcv_to_hci(shdlc->hdev, skb);  in llc_shdlc_rcv_i_frame()
249 	shdlc->nr = (shdlc->nr + 1) % 8;  in llc_shdlc_rcv_i_frame()
251 	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {  in llc_shdlc_rcv_i_frame()
252 		llc_shdlc_reset_t2(shdlc, y_nr);  in llc_shdlc_rcv_i_frame()
254 		shdlc->dnr = y_nr;  in llc_shdlc_rcv_i_frame()
261 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)  in llc_shdlc_rcv_ack()  argument
265 	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {  in llc_shdlc_rcv_ack()
266 		llc_shdlc_reset_t2(shdlc, y_nr);  in llc_shdlc_rcv_ack()
267 		shdlc->dnr = y_nr;  in llc_shdlc_rcv_ack()
271 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)  in llc_shdlc_requeue_ack_pending()  argument
275 	pr_debug("ns reset to %d\n", shdlc->dnr);  in llc_shdlc_requeue_ack_pending()
277 	while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {  in llc_shdlc_requeue_ack_pending()
279 		skb_queue_head(&shdlc->send_q, skb);  in llc_shdlc_requeue_ack_pending()
281 	shdlc->ns = shdlc->dnr;  in llc_shdlc_requeue_ack_pending()
284 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)  in llc_shdlc_rcv_rej()  argument
290 	if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {  in llc_shdlc_rcv_rej()
291 		if (shdlc->t2_active) {  in llc_shdlc_rcv_rej()
292 			del_timer_sync(&shdlc->t2_timer);  in llc_shdlc_rcv_rej()
293 			shdlc->t2_active = false;  in llc_shdlc_rcv_rej()
297 		if (shdlc->dnr != y_nr) {  in llc_shdlc_rcv_rej()
298 			while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {  in llc_shdlc_rcv_rej()
299 				skb = skb_dequeue(&shdlc->ack_pending_q);  in llc_shdlc_rcv_rej()
304 		llc_shdlc_requeue_ack_pending(shdlc);  in llc_shdlc_rcv_rej()
309 static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,  in llc_shdlc_rcv_s_frame()  argument
314 	if (shdlc->state != SHDLC_CONNECTED)  in llc_shdlc_rcv_s_frame()
319 		llc_shdlc_rcv_ack(shdlc, nr);  in llc_shdlc_rcv_s_frame()
320 		if (shdlc->rnr == true) {	/* see SHDLC 10.7.7 */  in llc_shdlc_rcv_s_frame()
321 			shdlc->rnr = false;  in llc_shdlc_rcv_s_frame()
322 			if (shdlc->send_q.qlen == 0) {  in llc_shdlc_rcv_s_frame()
323 				skb = llc_shdlc_alloc_skb(shdlc, 0);  in llc_shdlc_rcv_s_frame()
325 					skb_queue_tail(&shdlc->send_q, skb);  in llc_shdlc_rcv_s_frame()
330 		llc_shdlc_rcv_rej(shdlc, nr);  in llc_shdlc_rcv_s_frame()
333 		llc_shdlc_rcv_ack(shdlc, nr);  in llc_shdlc_rcv_s_frame()
334 		shdlc->rnr = true;  in llc_shdlc_rcv_s_frame()
341 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)  in llc_shdlc_connect_complete()  argument
345 	del_timer_sync(&shdlc->connect_timer);  in llc_shdlc_connect_complete()
348 		shdlc->ns = 0;  in llc_shdlc_connect_complete()
349 		shdlc->nr = 0;  in llc_shdlc_connect_complete()
350 		shdlc->dnr = 0;  in llc_shdlc_connect_complete()
352 		shdlc->state = SHDLC_HALF_CONNECTED;  in llc_shdlc_connect_complete()
354 		shdlc->state = SHDLC_DISCONNECTED;  in llc_shdlc_connect_complete()
357 	shdlc->connect_result = r;  in llc_shdlc_connect_complete()
359 	wake_up(shdlc->connect_wq);  in llc_shdlc_connect_complete()
362 static int llc_shdlc_connect_initiate(const struct llc_shdlc *shdlc)  in llc_shdlc_connect_initiate()  argument
366 	skb = llc_shdlc_alloc_skb(shdlc, 2);  in llc_shdlc_connect_initiate()
373 	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);  in llc_shdlc_connect_initiate()
376 static int llc_shdlc_connect_send_ua(const struct llc_shdlc *shdlc)  in llc_shdlc_connect_send_ua()  argument
380 	skb = llc_shdlc_alloc_skb(shdlc, 0);  in llc_shdlc_connect_send_ua()
384 	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);  in llc_shdlc_connect_send_ua()
387 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,  in llc_shdlc_rcv_u_frame()  argument
399 		switch (shdlc->state) {  in llc_shdlc_rcv_u_frame()
415 				shdlc->w = w;  in llc_shdlc_rcv_u_frame()
416 				shdlc->srej_support = srej_support;  in llc_shdlc_rcv_u_frame()
417 				r = llc_shdlc_connect_send_ua(shdlc);  in llc_shdlc_rcv_u_frame()
418 				llc_shdlc_connect_complete(shdlc, r);  in llc_shdlc_rcv_u_frame()
432 			shdlc->hard_fault = -ECONNRESET;  in llc_shdlc_rcv_u_frame()
439 		if ((shdlc->state == SHDLC_CONNECTING &&  in llc_shdlc_rcv_u_frame()
440 		     shdlc->connect_tries > 0) ||  in llc_shdlc_rcv_u_frame()
441 		    (shdlc->state == SHDLC_NEGOTIATING)) {  in llc_shdlc_rcv_u_frame()
442 			llc_shdlc_connect_complete(shdlc, 0);  in llc_shdlc_rcv_u_frame()
443 			shdlc->state = SHDLC_CONNECTED;  in llc_shdlc_rcv_u_frame()
453 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)  in llc_shdlc_handle_rcv_queue()  argument
462 	if (shdlc->rcv_q.qlen)  in llc_shdlc_handle_rcv_queue()
463 		pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);  in llc_shdlc_handle_rcv_queue()
465 	while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {  in llc_shdlc_handle_rcv_queue()
471 			if (shdlc->state == SHDLC_HALF_CONNECTED)  in llc_shdlc_handle_rcv_queue()
472 				shdlc->state = SHDLC_CONNECTED;  in llc_shdlc_handle_rcv_queue()
476 			llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);  in llc_shdlc_handle_rcv_queue()
479 			if (shdlc->state == SHDLC_HALF_CONNECTED)  in llc_shdlc_handle_rcv_queue()
480 				shdlc->state = SHDLC_CONNECTED;  in llc_shdlc_handle_rcv_queue()
484 			llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);  in llc_shdlc_handle_rcv_queue()
489 			llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);  in llc_shdlc_handle_rcv_queue()
512 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)  in llc_shdlc_handle_send_queue()  argument
518 	if (shdlc->send_q.qlen)  in llc_shdlc_handle_send_queue()
520 			 shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,  in llc_shdlc_handle_send_queue()
521 			 shdlc->rnr == false ? "false" : "true",  in llc_shdlc_handle_send_queue()
522 			 shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),  in llc_shdlc_handle_send_queue()
523 			 shdlc->ack_pending_q.qlen);  in llc_shdlc_handle_send_queue()
525 	while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&  in llc_shdlc_handle_send_queue()
526 	       (shdlc->rnr == false)) {  in llc_shdlc_handle_send_queue()
528 		if (shdlc->t1_active) {  in llc_shdlc_handle_send_queue()
529 			del_timer_sync(&shdlc->t1_timer);  in llc_shdlc_handle_send_queue()
530 			shdlc->t1_active = false;  in llc_shdlc_handle_send_queue()
534 		skb = skb_dequeue(&shdlc->send_q);  in llc_shdlc_handle_send_queue()
536 		*(u8 *)skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |  in llc_shdlc_handle_send_queue()
537 					shdlc->nr;  in llc_shdlc_handle_send_queue()
539 		pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,  in llc_shdlc_handle_send_queue()
540 			 shdlc->nr);  in llc_shdlc_handle_send_queue()
543 		r = shdlc->xmit_to_drv(shdlc->hdev, skb);  in llc_shdlc_handle_send_queue()
545 			shdlc->hard_fault = r;  in llc_shdlc_handle_send_queue()
549 		shdlc->ns = (shdlc->ns + 1) % 8;  in llc_shdlc_handle_send_queue()
554 		skb_queue_tail(&shdlc->ack_pending_q, skb);  in llc_shdlc_handle_send_queue()
556 		if (shdlc->t2_active == false) {  in llc_shdlc_handle_send_queue()
557 			shdlc->t2_active = true;  in llc_shdlc_handle_send_queue()
558 			mod_timer(&shdlc->t2_timer, time_sent +  in llc_shdlc_handle_send_queue()
567 	struct llc_shdlc *shdlc = from_timer(shdlc, t, connect_timer);  in llc_shdlc_connect_timeout()  local
569 	schedule_work(&shdlc->sm_work);  in llc_shdlc_connect_timeout()
574 	struct llc_shdlc *shdlc = from_timer(shdlc, t, t1_timer);  in llc_shdlc_t1_timeout()  local
578 	schedule_work(&shdlc->sm_work);  in llc_shdlc_t1_timeout()
583 	struct llc_shdlc *shdlc = from_timer(shdlc, t, t2_timer);  in llc_shdlc_t2_timeout()  local
587 	schedule_work(&shdlc->sm_work);  in llc_shdlc_t2_timeout()
592 	struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);  in llc_shdlc_sm_work()  local
595 	mutex_lock(&shdlc->state_mutex);  in llc_shdlc_sm_work()
597 	switch (shdlc->state) {  in llc_shdlc_sm_work()
599 		skb_queue_purge(&shdlc->rcv_q);  in llc_shdlc_sm_work()
600 		skb_queue_purge(&shdlc->send_q);  in llc_shdlc_sm_work()
601 		skb_queue_purge(&shdlc->ack_pending_q);  in llc_shdlc_sm_work()
604 		if (shdlc->hard_fault) {  in llc_shdlc_sm_work()
605 			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);  in llc_shdlc_sm_work()
609 		if (shdlc->connect_tries++ < 5)  in llc_shdlc_sm_work()
610 			r = llc_shdlc_connect_initiate(shdlc);  in llc_shdlc_sm_work()
614 			llc_shdlc_connect_complete(shdlc, r);  in llc_shdlc_sm_work()
616 			mod_timer(&shdlc->connect_timer, jiffies +  in llc_shdlc_sm_work()
619 			shdlc->state = SHDLC_NEGOTIATING;  in llc_shdlc_sm_work()
623 		if (timer_pending(&shdlc->connect_timer) == 0) {  in llc_shdlc_sm_work()
624 			shdlc->state = SHDLC_CONNECTING;  in llc_shdlc_sm_work()
625 			schedule_work(&shdlc->sm_work);  in llc_shdlc_sm_work()
628 		llc_shdlc_handle_rcv_queue(shdlc);  in llc_shdlc_sm_work()
630 		if (shdlc->hard_fault) {  in llc_shdlc_sm_work()
631 			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);  in llc_shdlc_sm_work()
637 		llc_shdlc_handle_rcv_queue(shdlc);  in llc_shdlc_sm_work()
638 		llc_shdlc_handle_send_queue(shdlc);  in llc_shdlc_sm_work()
640 		if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {  in llc_shdlc_sm_work()
643 			shdlc->t1_active = false;  in llc_shdlc_sm_work()
644 			r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,  in llc_shdlc_sm_work()
645 						   shdlc->nr);  in llc_shdlc_sm_work()
647 				shdlc->hard_fault = r;  in llc_shdlc_sm_work()
650 		if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {  in llc_shdlc_sm_work()
653 			shdlc->t2_active = false;  in llc_shdlc_sm_work()
655 			llc_shdlc_requeue_ack_pending(shdlc);  in llc_shdlc_sm_work()
656 			llc_shdlc_handle_send_queue(shdlc);  in llc_shdlc_sm_work()
659 		if (shdlc->hard_fault)  in llc_shdlc_sm_work()
660 			shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);  in llc_shdlc_sm_work()
665 	mutex_unlock(&shdlc->state_mutex);  in llc_shdlc_sm_work()
672 static int llc_shdlc_connect(struct llc_shdlc *shdlc)  in llc_shdlc_connect()  argument
676 	mutex_lock(&shdlc->state_mutex);  in llc_shdlc_connect()
678 	shdlc->state = SHDLC_CONNECTING;  in llc_shdlc_connect()
679 	shdlc->connect_wq = &connect_wq;  in llc_shdlc_connect()
680 	shdlc->connect_tries = 0;  in llc_shdlc_connect()
681 	shdlc->connect_result = 1;  in llc_shdlc_connect()
683 	mutex_unlock(&shdlc->state_mutex);  in llc_shdlc_connect()
685 	schedule_work(&shdlc->sm_work);  in llc_shdlc_connect()
687 	wait_event(connect_wq, shdlc->connect_result != 1);  in llc_shdlc_connect()
689 	return shdlc->connect_result;  in llc_shdlc_connect()
692 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)  in llc_shdlc_disconnect()  argument
694 	mutex_lock(&shdlc->state_mutex);  in llc_shdlc_disconnect()
696 	shdlc->state = SHDLC_DISCONNECTED;  in llc_shdlc_disconnect()
698 	mutex_unlock(&shdlc->state_mutex);  in llc_shdlc_disconnect()
700 	schedule_work(&shdlc->sm_work);  in llc_shdlc_disconnect()
708 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)  in llc_shdlc_recv_frame()  argument
712 		shdlc->hard_fault = -EREMOTEIO;  in llc_shdlc_recv_frame()
715 		skb_queue_tail(&shdlc->rcv_q, skb);  in llc_shdlc_recv_frame()
718 	schedule_work(&shdlc->sm_work);  in llc_shdlc_recv_frame()
726 	struct llc_shdlc *shdlc;  in llc_shdlc_init()  local
731 	shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);  in llc_shdlc_init()
732 	if (shdlc == NULL)  in llc_shdlc_init()
735 	mutex_init(&shdlc->state_mutex);  in llc_shdlc_init()
736 	shdlc->state = SHDLC_DISCONNECTED;  in llc_shdlc_init()
738 	timer_setup(&shdlc->connect_timer, llc_shdlc_connect_timeout, 0);  in llc_shdlc_init()
739 	timer_setup(&shdlc->t1_timer, llc_shdlc_t1_timeout, 0);  in llc_shdlc_init()
740 	timer_setup(&shdlc->t2_timer, llc_shdlc_t2_timeout, 0);  in llc_shdlc_init()
742 	shdlc->w = SHDLC_MAX_WINDOW;  in llc_shdlc_init()
743 	shdlc->srej_support = SHDLC_SREJ_SUPPORT;  in llc_shdlc_init()
745 	skb_queue_head_init(&shdlc->rcv_q);  in llc_shdlc_init()
746 	skb_queue_head_init(&shdlc->send_q);  in llc_shdlc_init()
747 	skb_queue_head_init(&shdlc->ack_pending_q);  in llc_shdlc_init()
749 	INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);  in llc_shdlc_init()
751 	shdlc->hdev = hdev;  in llc_shdlc_init()
752 	shdlc->xmit_to_drv = xmit_to_drv;  in llc_shdlc_init()
753 	shdlc->rcv_to_hci = rcv_to_hci;  in llc_shdlc_init()
754 	shdlc->tx_headroom = tx_headroom;  in llc_shdlc_init()
755 	shdlc->tx_tailroom = tx_tailroom;  in llc_shdlc_init()
756 	shdlc->llc_failure = llc_failure;  in llc_shdlc_init()
758 	return shdlc;  in llc_shdlc_init()
763 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_deinit()  local
765 	skb_queue_purge(&shdlc->rcv_q);  in llc_shdlc_deinit()
766 	skb_queue_purge(&shdlc->send_q);  in llc_shdlc_deinit()
767 	skb_queue_purge(&shdlc->ack_pending_q);  in llc_shdlc_deinit()
769 	kfree(shdlc);  in llc_shdlc_deinit()
774 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_start()  local
776 	return llc_shdlc_connect(shdlc);  in llc_shdlc_start()
781 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_stop()  local
783 	llc_shdlc_disconnect(shdlc);  in llc_shdlc_stop()
790 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_rcv_from_drv()  local
792 	llc_shdlc_recv_frame(shdlc, skb);  in llc_shdlc_rcv_from_drv()
797 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_xmit_from_hci()  local
799 	skb_queue_tail(&shdlc->send_q, skb);  in llc_shdlc_xmit_from_hci()
801 	schedule_work(&shdlc->sm_work);  in llc_shdlc_xmit_from_hci()