137 static struct sk_buff *llc_shdlc_alloc_skb(struct llc_shdlc *shdlc,  in llc_shdlc_alloc_skb()  argument
142 	skb = alloc_skb(shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM +  in llc_shdlc_alloc_skb()
143 			shdlc->tx_tailroom + payload_len, GFP_KERNEL);  in llc_shdlc_alloc_skb()
145 		skb_reserve(skb, shdlc->tx_headroom + SHDLC_LLC_HEAD_ROOM);  in llc_shdlc_alloc_skb()
151 static int llc_shdlc_send_s_frame(struct llc_shdlc *shdlc,  in llc_shdlc_send_s_frame()  argument
159 	skb = llc_shdlc_alloc_skb(shdlc, 0);  in llc_shdlc_send_s_frame()
165 	r = shdlc->xmit_to_drv(shdlc->hdev, skb);  in llc_shdlc_send_s_frame()
173 static int llc_shdlc_send_u_frame(struct llc_shdlc *shdlc,  in llc_shdlc_send_u_frame()  argument
183 	r = shdlc->xmit_to_drv(shdlc->hdev, skb);  in llc_shdlc_send_u_frame()
194 static void llc_shdlc_reset_t2(struct llc_shdlc *shdlc, int y_nr)  in llc_shdlc_reset_t2()  argument
197 	int dnr = shdlc->dnr;	/* MUST initially be < y_nr */  in llc_shdlc_reset_t2()
204 		skb = skb_dequeue(&shdlc->ack_pending_q);  in llc_shdlc_reset_t2()
210 	if (skb_queue_empty(&shdlc->ack_pending_q)) {  in llc_shdlc_reset_t2()
211 		if (shdlc->t2_active) {  in llc_shdlc_reset_t2()
212 			del_timer_sync(&shdlc->t2_timer);  in llc_shdlc_reset_t2()
213 			shdlc->t2_active = false;  in llc_shdlc_reset_t2()
219 		skb = skb_peek(&shdlc->ack_pending_q);  in llc_shdlc_reset_t2()
221 		mod_timer(&shdlc->t2_timer, *(unsigned long *)skb->cb +  in llc_shdlc_reset_t2()
223 		shdlc->t2_active = true;  in llc_shdlc_reset_t2()
234 static void llc_shdlc_rcv_i_frame(struct llc_shdlc *shdlc,  in llc_shdlc_rcv_i_frame()  argument
242 	if (shdlc->state != SHDLC_CONNECTED)  in llc_shdlc_rcv_i_frame()
245 	if (x_ns != shdlc->nr) {  in llc_shdlc_rcv_i_frame()
246 		llc_shdlc_send_s_frame(shdlc, S_FRAME_REJ, shdlc->nr);  in llc_shdlc_rcv_i_frame()
250 	if (shdlc->t1_active == false) {  in llc_shdlc_rcv_i_frame()
251 		shdlc->t1_active = true;  in llc_shdlc_rcv_i_frame()
252 		mod_timer(&shdlc->t1_timer, jiffies +  in llc_shdlc_rcv_i_frame()
253 			  msecs_to_jiffies(SHDLC_T1_VALUE_MS(shdlc->w)));  in llc_shdlc_rcv_i_frame()
258 		shdlc->rcv_to_hci(shdlc->hdev, skb);  in llc_shdlc_rcv_i_frame()
262 	shdlc->nr = (shdlc->nr + 1) % 8;  in llc_shdlc_rcv_i_frame()
264 	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {  in llc_shdlc_rcv_i_frame()
265 		llc_shdlc_reset_t2(shdlc, y_nr);  in llc_shdlc_rcv_i_frame()
267 		shdlc->dnr = y_nr;  in llc_shdlc_rcv_i_frame()
274 static void llc_shdlc_rcv_ack(struct llc_shdlc *shdlc, int y_nr)  in llc_shdlc_rcv_ack()  argument
278 	if (llc_shdlc_x_lt_y_lteq_z(shdlc->dnr, y_nr, shdlc->ns)) {  in llc_shdlc_rcv_ack()
279 		llc_shdlc_reset_t2(shdlc, y_nr);  in llc_shdlc_rcv_ack()
280 		shdlc->dnr = y_nr;  in llc_shdlc_rcv_ack()
284 static void llc_shdlc_requeue_ack_pending(struct llc_shdlc *shdlc)  in llc_shdlc_requeue_ack_pending()  argument
288 	pr_debug("ns reset to %d\n", shdlc->dnr);  in llc_shdlc_requeue_ack_pending()
290 	while ((skb = skb_dequeue_tail(&shdlc->ack_pending_q))) {  in llc_shdlc_requeue_ack_pending()
292 		skb_queue_head(&shdlc->send_q, skb);  in llc_shdlc_requeue_ack_pending()
294 	shdlc->ns = shdlc->dnr;  in llc_shdlc_requeue_ack_pending()
297 static void llc_shdlc_rcv_rej(struct llc_shdlc *shdlc, int y_nr)  in llc_shdlc_rcv_rej()  argument
303 	if (llc_shdlc_x_lteq_y_lt_z(shdlc->dnr, y_nr, shdlc->ns)) {  in llc_shdlc_rcv_rej()
304 		if (shdlc->t2_active) {  in llc_shdlc_rcv_rej()
305 			del_timer_sync(&shdlc->t2_timer);  in llc_shdlc_rcv_rej()
306 			shdlc->t2_active = false;  in llc_shdlc_rcv_rej()
310 		if (shdlc->dnr != y_nr) {  in llc_shdlc_rcv_rej()
311 			while ((shdlc->dnr = ((shdlc->dnr + 1) % 8)) != y_nr) {  in llc_shdlc_rcv_rej()
312 				skb = skb_dequeue(&shdlc->ack_pending_q);  in llc_shdlc_rcv_rej()
317 		llc_shdlc_requeue_ack_pending(shdlc);  in llc_shdlc_rcv_rej()
322 static void llc_shdlc_rcv_s_frame(struct llc_shdlc *shdlc,  in llc_shdlc_rcv_s_frame()  argument
327 	if (shdlc->state != SHDLC_CONNECTED)  in llc_shdlc_rcv_s_frame()
332 		llc_shdlc_rcv_ack(shdlc, nr);  in llc_shdlc_rcv_s_frame()
333 		if (shdlc->rnr == true) {	/* see SHDLC 10.7.7 */  in llc_shdlc_rcv_s_frame()
334 			shdlc->rnr = false;  in llc_shdlc_rcv_s_frame()
335 			if (shdlc->send_q.qlen == 0) {  in llc_shdlc_rcv_s_frame()
336 				skb = llc_shdlc_alloc_skb(shdlc, 0);  in llc_shdlc_rcv_s_frame()
338 					skb_queue_tail(&shdlc->send_q, skb);  in llc_shdlc_rcv_s_frame()
343 		llc_shdlc_rcv_rej(shdlc, nr);  in llc_shdlc_rcv_s_frame()
346 		llc_shdlc_rcv_ack(shdlc, nr);  in llc_shdlc_rcv_s_frame()
347 		shdlc->rnr = true;  in llc_shdlc_rcv_s_frame()
354 static void llc_shdlc_connect_complete(struct llc_shdlc *shdlc, int r)  in llc_shdlc_connect_complete()  argument
358 	del_timer_sync(&shdlc->connect_timer);  in llc_shdlc_connect_complete()
361 		shdlc->ns = 0;  in llc_shdlc_connect_complete()
362 		shdlc->nr = 0;  in llc_shdlc_connect_complete()
363 		shdlc->dnr = 0;  in llc_shdlc_connect_complete()
365 		shdlc->state = SHDLC_HALF_CONNECTED;  in llc_shdlc_connect_complete()
367 		shdlc->state = SHDLC_DISCONNECTED;  in llc_shdlc_connect_complete()
370 	shdlc->connect_result = r;  in llc_shdlc_connect_complete()
372 	wake_up(shdlc->connect_wq);  in llc_shdlc_connect_complete()
375 static int llc_shdlc_connect_initiate(struct llc_shdlc *shdlc)  in llc_shdlc_connect_initiate()  argument
381 	skb = llc_shdlc_alloc_skb(shdlc, 2);  in llc_shdlc_connect_initiate()
388 	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_RSET);  in llc_shdlc_connect_initiate()
391 static int llc_shdlc_connect_send_ua(struct llc_shdlc *shdlc)  in llc_shdlc_connect_send_ua()  argument
397 	skb = llc_shdlc_alloc_skb(shdlc, 0);  in llc_shdlc_connect_send_ua()
401 	return llc_shdlc_send_u_frame(shdlc, skb, U_FRAME_UA);  in llc_shdlc_connect_send_ua()
404 static void llc_shdlc_rcv_u_frame(struct llc_shdlc *shdlc,  in llc_shdlc_rcv_u_frame()  argument
416 		switch (shdlc->state) {  in llc_shdlc_rcv_u_frame()
432 				shdlc->w = w;  in llc_shdlc_rcv_u_frame()
433 				shdlc->srej_support = srej_support;  in llc_shdlc_rcv_u_frame()
434 				r = llc_shdlc_connect_send_ua(shdlc);  in llc_shdlc_rcv_u_frame()
435 				llc_shdlc_connect_complete(shdlc, r);  in llc_shdlc_rcv_u_frame()
449 			shdlc->hard_fault = -ECONNRESET;  in llc_shdlc_rcv_u_frame()
456 		if ((shdlc->state == SHDLC_CONNECTING &&  in llc_shdlc_rcv_u_frame()
457 		     shdlc->connect_tries > 0) ||  in llc_shdlc_rcv_u_frame()
458 		    (shdlc->state == SHDLC_NEGOTIATING)) {  in llc_shdlc_rcv_u_frame()
459 			llc_shdlc_connect_complete(shdlc, 0);  in llc_shdlc_rcv_u_frame()
460 			shdlc->state = SHDLC_CONNECTED;  in llc_shdlc_rcv_u_frame()
470 static void llc_shdlc_handle_rcv_queue(struct llc_shdlc *shdlc)  in llc_shdlc_handle_rcv_queue()  argument
479 	if (shdlc->rcv_q.qlen)  in llc_shdlc_handle_rcv_queue()
480 		pr_debug("rcvQlen=%d\n", shdlc->rcv_q.qlen);  in llc_shdlc_handle_rcv_queue()
482 	while ((skb = skb_dequeue(&shdlc->rcv_q)) != NULL) {  in llc_shdlc_handle_rcv_queue()
488 			if (shdlc->state == SHDLC_HALF_CONNECTED)  in llc_shdlc_handle_rcv_queue()
489 				shdlc->state = SHDLC_CONNECTED;  in llc_shdlc_handle_rcv_queue()
493 			llc_shdlc_rcv_i_frame(shdlc, skb, ns, nr);  in llc_shdlc_handle_rcv_queue()
496 			if (shdlc->state == SHDLC_HALF_CONNECTED)  in llc_shdlc_handle_rcv_queue()
497 				shdlc->state = SHDLC_CONNECTED;  in llc_shdlc_handle_rcv_queue()
501 			llc_shdlc_rcv_s_frame(shdlc, s_frame_type, nr);  in llc_shdlc_handle_rcv_queue()
506 			llc_shdlc_rcv_u_frame(shdlc, skb, u_frame_modifier);  in llc_shdlc_handle_rcv_queue()
529 static void llc_shdlc_handle_send_queue(struct llc_shdlc *shdlc)  in llc_shdlc_handle_send_queue()  argument
535 	if (shdlc->send_q.qlen)  in llc_shdlc_handle_send_queue()
538 		     shdlc->send_q.qlen, shdlc->ns, shdlc->dnr,  in llc_shdlc_handle_send_queue()
539 		     shdlc->rnr == false ? "false" : "true",  in llc_shdlc_handle_send_queue()
540 		     shdlc->w - llc_shdlc_w_used(shdlc->ns, shdlc->dnr),  in llc_shdlc_handle_send_queue()
541 		     shdlc->ack_pending_q.qlen);  in llc_shdlc_handle_send_queue()
543 	while (shdlc->send_q.qlen && shdlc->ack_pending_q.qlen < shdlc->w &&  in llc_shdlc_handle_send_queue()
544 	       (shdlc->rnr == false)) {  in llc_shdlc_handle_send_queue()
546 		if (shdlc->t1_active) {  in llc_shdlc_handle_send_queue()
547 			del_timer_sync(&shdlc->t1_timer);  in llc_shdlc_handle_send_queue()
548 			shdlc->t1_active = false;  in llc_shdlc_handle_send_queue()
552 		skb = skb_dequeue(&shdlc->send_q);  in llc_shdlc_handle_send_queue()
554 		*skb_push(skb, 1) = SHDLC_CONTROL_HEAD_I | (shdlc->ns << 3) |  in llc_shdlc_handle_send_queue()
555 				    shdlc->nr;  in llc_shdlc_handle_send_queue()
557 		pr_debug("Sending I-Frame %d, waiting to rcv %d\n", shdlc->ns,  in llc_shdlc_handle_send_queue()
558 			 shdlc->nr);  in llc_shdlc_handle_send_queue()
561 		r = shdlc->xmit_to_drv(shdlc->hdev, skb);  in llc_shdlc_handle_send_queue()
563 			shdlc->hard_fault = r;  in llc_shdlc_handle_send_queue()
567 		shdlc->ns = (shdlc->ns + 1) % 8;  in llc_shdlc_handle_send_queue()
572 		skb_queue_tail(&shdlc->ack_pending_q, skb);  in llc_shdlc_handle_send_queue()
574 		if (shdlc->t2_active == false) {  in llc_shdlc_handle_send_queue()
575 			shdlc->t2_active = true;  in llc_shdlc_handle_send_queue()
576 			mod_timer(&shdlc->t2_timer, time_sent +  in llc_shdlc_handle_send_queue()
585 	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;  in llc_shdlc_connect_timeout()  local
589 	schedule_work(&shdlc->sm_work);  in llc_shdlc_connect_timeout()
594 	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;  in llc_shdlc_t1_timeout()  local
598 	schedule_work(&shdlc->sm_work);  in llc_shdlc_t1_timeout()
603 	struct llc_shdlc *shdlc = (struct llc_shdlc *)data;  in llc_shdlc_t2_timeout()  local
607 	schedule_work(&shdlc->sm_work);  in llc_shdlc_t2_timeout()
612 	struct llc_shdlc *shdlc = container_of(work, struct llc_shdlc, sm_work);  in llc_shdlc_sm_work()  local
617 	mutex_lock(&shdlc->state_mutex);  in llc_shdlc_sm_work()
619 	switch (shdlc->state) {  in llc_shdlc_sm_work()
621 		skb_queue_purge(&shdlc->rcv_q);  in llc_shdlc_sm_work()
622 		skb_queue_purge(&shdlc->send_q);  in llc_shdlc_sm_work()
623 		skb_queue_purge(&shdlc->ack_pending_q);  in llc_shdlc_sm_work()
626 		if (shdlc->hard_fault) {  in llc_shdlc_sm_work()
627 			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);  in llc_shdlc_sm_work()
631 		if (shdlc->connect_tries++ < 5)  in llc_shdlc_sm_work()
632 			r = llc_shdlc_connect_initiate(shdlc);  in llc_shdlc_sm_work()
636 			llc_shdlc_connect_complete(shdlc, r);  in llc_shdlc_sm_work()
638 			mod_timer(&shdlc->connect_timer, jiffies +  in llc_shdlc_sm_work()
641 			shdlc->state = SHDLC_NEGOTIATING;  in llc_shdlc_sm_work()
645 		if (timer_pending(&shdlc->connect_timer) == 0) {  in llc_shdlc_sm_work()
646 			shdlc->state = SHDLC_CONNECTING;  in llc_shdlc_sm_work()
647 			schedule_work(&shdlc->sm_work);  in llc_shdlc_sm_work()
650 		llc_shdlc_handle_rcv_queue(shdlc);  in llc_shdlc_sm_work()
652 		if (shdlc->hard_fault) {  in llc_shdlc_sm_work()
653 			llc_shdlc_connect_complete(shdlc, shdlc->hard_fault);  in llc_shdlc_sm_work()
659 		llc_shdlc_handle_rcv_queue(shdlc);  in llc_shdlc_sm_work()
660 		llc_shdlc_handle_send_queue(shdlc);  in llc_shdlc_sm_work()
662 		if (shdlc->t1_active && timer_pending(&shdlc->t1_timer) == 0) {  in llc_shdlc_sm_work()
666 			shdlc->t1_active = false;  in llc_shdlc_sm_work()
667 			r = llc_shdlc_send_s_frame(shdlc, S_FRAME_RR,  in llc_shdlc_sm_work()
668 						   shdlc->nr);  in llc_shdlc_sm_work()
670 				shdlc->hard_fault = r;  in llc_shdlc_sm_work()
673 		if (shdlc->t2_active && timer_pending(&shdlc->t2_timer) == 0) {  in llc_shdlc_sm_work()
677 			shdlc->t2_active = false;  in llc_shdlc_sm_work()
679 			llc_shdlc_requeue_ack_pending(shdlc);  in llc_shdlc_sm_work()
680 			llc_shdlc_handle_send_queue(shdlc);  in llc_shdlc_sm_work()
683 		if (shdlc->hard_fault)  in llc_shdlc_sm_work()
684 			shdlc->llc_failure(shdlc->hdev, shdlc->hard_fault);  in llc_shdlc_sm_work()
689 	mutex_unlock(&shdlc->state_mutex);  in llc_shdlc_sm_work()
696 static int llc_shdlc_connect(struct llc_shdlc *shdlc)  in llc_shdlc_connect()  argument
702 	mutex_lock(&shdlc->state_mutex);  in llc_shdlc_connect()
704 	shdlc->state = SHDLC_CONNECTING;  in llc_shdlc_connect()
705 	shdlc->connect_wq = &connect_wq;  in llc_shdlc_connect()
706 	shdlc->connect_tries = 0;  in llc_shdlc_connect()
707 	shdlc->connect_result = 1;  in llc_shdlc_connect()
709 	mutex_unlock(&shdlc->state_mutex);  in llc_shdlc_connect()
711 	schedule_work(&shdlc->sm_work);  in llc_shdlc_connect()
713 	wait_event(connect_wq, shdlc->connect_result != 1);  in llc_shdlc_connect()
715 	return shdlc->connect_result;  in llc_shdlc_connect()
718 static void llc_shdlc_disconnect(struct llc_shdlc *shdlc)  in llc_shdlc_disconnect()  argument
722 	mutex_lock(&shdlc->state_mutex);  in llc_shdlc_disconnect()
724 	shdlc->state = SHDLC_DISCONNECTED;  in llc_shdlc_disconnect()
726 	mutex_unlock(&shdlc->state_mutex);  in llc_shdlc_disconnect()
728 	schedule_work(&shdlc->sm_work);  in llc_shdlc_disconnect()
736 static void llc_shdlc_recv_frame(struct llc_shdlc *shdlc, struct sk_buff *skb)  in llc_shdlc_recv_frame()  argument
740 		shdlc->hard_fault = -EREMOTEIO;  in llc_shdlc_recv_frame()
743 		skb_queue_tail(&shdlc->rcv_q, skb);  in llc_shdlc_recv_frame()
746 	schedule_work(&shdlc->sm_work);  in llc_shdlc_recv_frame()
754 	struct llc_shdlc *shdlc;  in llc_shdlc_init()  local
759 	shdlc = kzalloc(sizeof(struct llc_shdlc), GFP_KERNEL);  in llc_shdlc_init()
760 	if (shdlc == NULL)  in llc_shdlc_init()
763 	mutex_init(&shdlc->state_mutex);  in llc_shdlc_init()
764 	shdlc->state = SHDLC_DISCONNECTED;  in llc_shdlc_init()
766 	init_timer(&shdlc->connect_timer);  in llc_shdlc_init()
767 	shdlc->connect_timer.data = (unsigned long)shdlc;  in llc_shdlc_init()
768 	shdlc->connect_timer.function = llc_shdlc_connect_timeout;  in llc_shdlc_init()
770 	init_timer(&shdlc->t1_timer);  in llc_shdlc_init()
771 	shdlc->t1_timer.data = (unsigned long)shdlc;  in llc_shdlc_init()
772 	shdlc->t1_timer.function = llc_shdlc_t1_timeout;  in llc_shdlc_init()
774 	init_timer(&shdlc->t2_timer);  in llc_shdlc_init()
775 	shdlc->t2_timer.data = (unsigned long)shdlc;  in llc_shdlc_init()
776 	shdlc->t2_timer.function = llc_shdlc_t2_timeout;  in llc_shdlc_init()
778 	shdlc->w = SHDLC_MAX_WINDOW;  in llc_shdlc_init()
779 	shdlc->srej_support = SHDLC_SREJ_SUPPORT;  in llc_shdlc_init()
781 	skb_queue_head_init(&shdlc->rcv_q);  in llc_shdlc_init()
782 	skb_queue_head_init(&shdlc->send_q);  in llc_shdlc_init()
783 	skb_queue_head_init(&shdlc->ack_pending_q);  in llc_shdlc_init()
785 	INIT_WORK(&shdlc->sm_work, llc_shdlc_sm_work);  in llc_shdlc_init()
787 	shdlc->hdev = hdev;  in llc_shdlc_init()
788 	shdlc->xmit_to_drv = xmit_to_drv;  in llc_shdlc_init()
789 	shdlc->rcv_to_hci = rcv_to_hci;  in llc_shdlc_init()
790 	shdlc->tx_headroom = tx_headroom;  in llc_shdlc_init()
791 	shdlc->tx_tailroom = tx_tailroom;  in llc_shdlc_init()
792 	shdlc->llc_failure = llc_failure;  in llc_shdlc_init()
794 	return shdlc;  in llc_shdlc_init()
799 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_deinit()  local
801 	skb_queue_purge(&shdlc->rcv_q);  in llc_shdlc_deinit()
802 	skb_queue_purge(&shdlc->send_q);  in llc_shdlc_deinit()
803 	skb_queue_purge(&shdlc->ack_pending_q);  in llc_shdlc_deinit()
805 	kfree(shdlc);  in llc_shdlc_deinit()
810 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_start()  local
812 	return llc_shdlc_connect(shdlc);  in llc_shdlc_start()
817 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_stop()  local
819 	llc_shdlc_disconnect(shdlc);  in llc_shdlc_stop()
826 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_rcv_from_drv()  local
828 	llc_shdlc_recv_frame(shdlc, skb);  in llc_shdlc_rcv_from_drv()
833 	struct llc_shdlc *shdlc = nfc_llc_get_data(llc);  in llc_shdlc_xmit_from_hci()  local
835 	skb_queue_tail(&shdlc->send_q, skb);  in llc_shdlc_xmit_from_hci()
837 	schedule_work(&shdlc->sm_work);  in llc_shdlc_xmit_from_hci()