1 /* -----------------------------------------------------------------------------
2 * Copyright (c) 2011 Ozmo Inc
3 * Released under the GNU General Public License Version 2 (GPLv2).
4 * -----------------------------------------------------------------------------
6 #include <linux/init.h>
7 #include <linux/module.h>
8 #include <linux/timer.h>
9 #include <linux/sched.h>
10 #include <linux/netdevice.h>
11 #include <linux/errno.h>
13 #include "ozprotocol.h"
21 #include <asm/unaligned.h>
22 #include <linux/uaccess.h>
23 #include <net/psnap.h>
24 /*------------------------------------------------------------------------------
26 #define OZ_MAX_TX_POOL_SIZE 6
27 /* Maximum number of uncompleted isoc frames that can be pending.
29 #define OZ_MAX_SUBMITTED_ISOC 16
30 /*------------------------------------------------------------------------------
32 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd);
33 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f);
34 static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f);
35 static int oz_send_isoc_frame(struct oz_pd *pd);
36 static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f);
37 static void oz_isoc_stream_free(struct oz_isoc_stream *st);
38 static int oz_send_next_queued_frame(struct oz_pd *pd, int *more_data);
39 static void oz_isoc_destructor(struct sk_buff *skb);
40 static int oz_def_app_init(void);
41 static void oz_def_app_term(void);
42 static int oz_def_app_start(struct oz_pd *pd, int resume);
43 static void oz_def_app_stop(struct oz_pd *pd, int pause);
44 static void oz_def_app_rx(struct oz_pd *pd, struct oz_elt *elt);
45 /*------------------------------------------------------------------------------
46 * Counts the uncompleted isoc frames submitted to netcard.
48 static atomic_t g_submitted_isoc = ATOMIC_INIT(0);
49 /* Application handler functions.
51 static struct oz_app_if g_app_if[OZ_APPID_MAX] = {
88 /*------------------------------------------------------------------------------
91 static int oz_def_app_init(void)
95 /*------------------------------------------------------------------------------
98 static void oz_def_app_term(void)
101 /*------------------------------------------------------------------------------
104 static int oz_def_app_start(struct oz_pd *pd, int resume)
108 /*------------------------------------------------------------------------------
111 static void oz_def_app_stop(struct oz_pd *pd, int pause)
114 /*------------------------------------------------------------------------------
117 static void oz_def_app_rx(struct oz_pd *pd, struct oz_elt *elt)
120 /*------------------------------------------------------------------------------
121 * Context: softirq or process
123 void oz_pd_set_state(struct oz_pd *pd, unsigned state)
126 oz_event_log(OZ_EVT_PD_STATE, 0, 0, 0, state);
130 oz_trace("PD State: OZ_PD_S_IDLE\n");
132 case OZ_PD_S_CONNECTED:
133 oz_trace("PD State: OZ_PD_S_CONNECTED\n");
135 case OZ_PD_S_STOPPED:
136 oz_trace("PD State: OZ_PD_S_STOPPED\n");
139 oz_trace("PD State: OZ_PD_S_SLEEP\n");
142 #endif /* WANT_TRACE */
144 /*------------------------------------------------------------------------------
145 * Context: softirq or process
147 void oz_pd_get(struct oz_pd *pd)
149 atomic_inc(&pd->ref_count);
151 /*------------------------------------------------------------------------------
152 * Context: softirq or process
154 void oz_pd_put(struct oz_pd *pd)
156 if (atomic_dec_and_test(&pd->ref_count))
159 /*------------------------------------------------------------------------------
160 * Context: softirq-serialized
162 struct oz_pd *oz_pd_alloc(u8 *mac_addr)
164 struct oz_pd *pd = kzalloc(sizeof(struct oz_pd), GFP_ATOMIC);
167 atomic_set(&pd->ref_count, 2);
168 for (i = 0; i < OZ_APPID_MAX; i++)
169 spin_lock_init(&pd->app_lock[i]);
170 pd->last_rx_pkt_num = 0xffffffff;
171 oz_pd_set_state(pd, OZ_PD_S_IDLE);
172 pd->max_tx_size = OZ_MAX_TX_SIZE;
173 memcpy(pd->mac_addr, mac_addr, ETH_ALEN);
174 if (0 != oz_elt_buf_init(&pd->elt_buff)) {
178 spin_lock_init(&pd->tx_frame_lock);
179 INIT_LIST_HEAD(&pd->tx_queue);
180 INIT_LIST_HEAD(&pd->farewell_list);
181 pd->last_sent_frame = &pd->tx_queue;
182 spin_lock_init(&pd->stream_lock);
183 INIT_LIST_HEAD(&pd->stream_list);
187 /*------------------------------------------------------------------------------
188 * Context: softirq or process
190 void oz_pd_destroy(struct oz_pd *pd)
193 struct oz_tx_frame *f;
194 struct oz_isoc_stream *st;
195 struct oz_farewell *fwell;
196 oz_trace("Destroying PD\n");
197 /* Delete any streams.
199 e = pd->stream_list.next;
200 while (e != &pd->stream_list) {
201 st = container_of(e, struct oz_isoc_stream, link);
203 oz_isoc_stream_free(st);
205 /* Free any queued tx frames.
207 e = pd->tx_queue.next;
208 while (e != &pd->tx_queue) {
209 f = container_of(e, struct oz_tx_frame, link);
211 oz_retire_frame(pd, f);
213 oz_elt_buf_term(&pd->elt_buff);
214 /* Free any farewells.
216 e = pd->farewell_list.next;
217 while (e != &pd->farewell_list) {
218 fwell = container_of(e, struct oz_farewell, link);
222 /* Deallocate all frames in tx pool.
224 while (pd->tx_pool) {
226 pd->tx_pool = e->next;
227 kfree(container_of(e, struct oz_tx_frame, link));
230 dev_put(pd->net_dev);
233 /*------------------------------------------------------------------------------
234 * Context: softirq-serialized
236 int oz_services_start(struct oz_pd *pd, u16 apps, int resume)
238 struct oz_app_if *ai;
240 oz_trace("oz_services_start(0x%x) resume(%d)\n", apps, resume);
241 for (ai = g_app_if; ai < &g_app_if[OZ_APPID_MAX]; ai++) {
242 if (apps & (1<<ai->app_id)) {
243 if (ai->start(pd, resume)) {
245 oz_trace("Unabled to start service %d\n",
249 oz_polling_lock_bh();
250 pd->total_apps |= (1<<ai->app_id);
252 pd->paused_apps &= ~(1<<ai->app_id);
253 oz_polling_unlock_bh();
258 /*------------------------------------------------------------------------------
259 * Context: softirq or process
261 void oz_services_stop(struct oz_pd *pd, u16 apps, int pause)
263 struct oz_app_if *ai;
264 oz_trace("oz_stop_services(0x%x) pause(%d)\n", apps, pause);
265 for (ai = g_app_if; ai < &g_app_if[OZ_APPID_MAX]; ai++) {
266 if (apps & (1<<ai->app_id)) {
267 oz_polling_lock_bh();
269 pd->paused_apps |= (1<<ai->app_id);
271 pd->total_apps &= ~(1<<ai->app_id);
272 pd->paused_apps &= ~(1<<ai->app_id);
274 oz_polling_unlock_bh();
279 /*------------------------------------------------------------------------------
282 void oz_pd_heartbeat(struct oz_pd *pd, u16 apps)
284 struct oz_app_if *ai;
286 for (ai = g_app_if; ai < &g_app_if[OZ_APPID_MAX]; ai++) {
287 if (ai->heartbeat && (apps & (1<<ai->app_id))) {
288 if (ai->heartbeat(pd))
293 oz_pd_request_heartbeat(pd);
294 if (pd->mode & OZ_F_ISOC_ANYTIME) {
296 while (count-- && (oz_send_isoc_frame(pd) >= 0))
300 /*------------------------------------------------------------------------------
301 * Context: softirq or process
303 void oz_pd_stop(struct oz_pd *pd)
306 oz_trace("oz_pd_stop() State = 0x%x\n", pd->state);
307 oz_pd_indicate_farewells(pd);
308 oz_polling_lock_bh();
309 stop_apps = pd->total_apps;
312 oz_polling_unlock_bh();
313 oz_services_stop(pd, stop_apps, 0);
314 oz_polling_lock_bh();
315 oz_pd_set_state(pd, OZ_PD_S_STOPPED);
316 /* Remove from PD list.*/
318 oz_polling_unlock_bh();
319 oz_trace("pd ref count = %d\n", atomic_read(&pd->ref_count));
320 oz_timer_delete(pd, 0);
323 /*------------------------------------------------------------------------------
326 int oz_pd_sleep(struct oz_pd *pd)
330 oz_polling_lock_bh();
331 if (pd->state & (OZ_PD_S_SLEEP | OZ_PD_S_STOPPED)) {
332 oz_polling_unlock_bh();
335 if (pd->keep_alive_j && pd->session_id) {
336 oz_pd_set_state(pd, OZ_PD_S_SLEEP);
337 pd->pulse_time_j = jiffies + pd->keep_alive_j;
338 oz_trace("Sleep Now %lu until %lu\n",
339 jiffies, pd->pulse_time_j);
343 stop_apps = pd->total_apps;
344 oz_polling_unlock_bh();
348 oz_services_stop(pd, stop_apps, 1);
349 oz_timer_add(pd, OZ_TIMER_STOP, jiffies + pd->keep_alive_j, 1);
353 /*------------------------------------------------------------------------------
356 static struct oz_tx_frame *oz_tx_frame_alloc(struct oz_pd *pd)
358 struct oz_tx_frame *f = 0;
359 spin_lock_bh(&pd->tx_frame_lock);
361 f = container_of(pd->tx_pool, struct oz_tx_frame, link);
362 pd->tx_pool = pd->tx_pool->next;
365 spin_unlock_bh(&pd->tx_frame_lock);
367 f = kmalloc(sizeof(struct oz_tx_frame), GFP_ATOMIC);
369 f->total_size = sizeof(struct oz_hdr);
370 INIT_LIST_HEAD(&f->link);
371 INIT_LIST_HEAD(&f->elt_list);
375 /*------------------------------------------------------------------------------
376 * Context: softirq or process
378 static void oz_tx_frame_free(struct oz_pd *pd, struct oz_tx_frame *f)
380 spin_lock_bh(&pd->tx_frame_lock);
381 if (pd->tx_pool_count < OZ_MAX_TX_POOL_SIZE) {
382 f->link.next = pd->tx_pool;
383 pd->tx_pool = &f->link;
389 spin_unlock_bh(&pd->tx_frame_lock);
393 /*------------------------------------------------------------------------------
396 int oz_prepare_frame(struct oz_pd *pd, int empty)
398 struct oz_tx_frame *f;
399 if ((pd->mode & OZ_MODE_MASK) != OZ_MODE_TRIGGERED)
401 if (pd->nb_queued_frames >= OZ_MAX_QUEUED_FRAMES)
403 if (!empty && !oz_are_elts_available(&pd->elt_buff))
405 f = oz_tx_frame_alloc(pd);
409 (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ACK_REQUESTED;
410 ++pd->last_tx_pkt_num;
411 put_unaligned(cpu_to_le32(pd->last_tx_pkt_num), &f->hdr.pkt_num);
413 oz_select_elts_for_tx(&pd->elt_buff, 0, &f->total_size,
414 pd->max_tx_size, &f->elt_list);
416 spin_lock(&pd->tx_frame_lock);
417 list_add_tail(&f->link, &pd->tx_queue);
418 pd->nb_queued_frames++;
419 spin_unlock(&pd->tx_frame_lock);
422 /*------------------------------------------------------------------------------
423 * Context: softirq-serialized
425 static struct sk_buff *oz_build_frame(struct oz_pd *pd, struct oz_tx_frame *f)
427 struct sk_buff *skb = 0;
428 struct net_device *dev = pd->net_dev;
429 struct oz_hdr *oz_hdr;
432 /* Allocate skb with enough space for the lower layers as well
433 * as the space we need.
435 skb = alloc_skb(f->total_size + OZ_ALLOCATED_SPACE(dev), GFP_ATOMIC);
438 /* Reserve the head room for lower layers.
440 skb_reserve(skb, LL_RESERVED_SPACE(dev));
441 skb_reset_network_header(skb);
443 skb->protocol = htons(OZ_ETHERTYPE);
444 if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
445 dev->dev_addr, skb->len) < 0)
447 /* Push the tail to the end of the area we are going to copy to.
449 oz_hdr = (struct oz_hdr *)skb_put(skb, f->total_size);
450 f->hdr.last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
451 memcpy(oz_hdr, &f->hdr, sizeof(struct oz_hdr));
452 /* Copy the elements into the frame body.
454 elt = (struct oz_elt *)(oz_hdr+1);
455 for (e = f->elt_list.next; e != &f->elt_list; e = e->next) {
456 struct oz_elt_info *ei;
457 ei = container_of(e, struct oz_elt_info, link);
458 memcpy(elt, ei->data, ei->length);
459 elt = oz_next_elt(elt);
466 /*------------------------------------------------------------------------------
467 * Context: softirq or process
469 static void oz_retire_frame(struct oz_pd *pd, struct oz_tx_frame *f)
472 struct oz_elt_info *ei;
473 e = f->elt_list.next;
474 while (e != &f->elt_list) {
475 ei = container_of(e, struct oz_elt_info, link);
477 list_del_init(&ei->link);
479 ei->callback(pd, ei->context);
480 spin_lock_bh(&pd->elt_buff.lock);
481 oz_elt_info_free(&pd->elt_buff, ei);
482 spin_unlock_bh(&pd->elt_buff.lock);
484 oz_tx_frame_free(pd, f);
485 if (pd->elt_buff.free_elts > pd->elt_buff.max_free_elts)
486 oz_trim_elt_pool(&pd->elt_buff);
488 /*------------------------------------------------------------------------------
489 * Context: softirq-serialized
491 static int oz_send_next_queued_frame(struct oz_pd *pd, int *more_data)
494 struct oz_tx_frame *f;
497 spin_lock(&pd->tx_frame_lock);
498 e = pd->last_sent_frame->next;
499 if (e == &pd->tx_queue) {
500 spin_unlock(&pd->tx_frame_lock);
503 pd->last_sent_frame = e;
504 if (e->next != &pd->tx_queue)
506 f = container_of(e, struct oz_tx_frame, link);
507 skb = oz_build_frame(pd, f);
508 spin_unlock(&pd->tx_frame_lock);
509 oz_trace2(OZ_TRACE_TX_FRAMES, "TX frame PN=0x%x\n", f->hdr.pkt_num);
511 oz_event_log(OZ_EVT_TX_FRAME,
513 (((u16)f->hdr.control)<<8)|f->hdr.last_pkt_num,
515 if (dev_queue_xmit(skb) < 0)
520 /*------------------------------------------------------------------------------
521 * Context: softirq-serialized
523 void oz_send_queued_frames(struct oz_pd *pd, int backlog)
526 if (backlog < OZ_MAX_QUEUED_FRAMES) {
527 if (oz_send_next_queued_frame(pd, &more) >= 0) {
528 while (more && oz_send_next_queued_frame(pd, &more))
531 if (((pd->mode & OZ_F_ISOC_ANYTIME) == 0)
532 || (pd->isoc_sent == 0)) {
533 if (oz_prepare_frame(pd, 1) >= 0)
534 oz_send_next_queued_frame(pd, &more);
538 oz_send_next_queued_frame(pd, &more);
541 /*------------------------------------------------------------------------------
544 static int oz_send_isoc_frame(struct oz_pd *pd)
546 struct sk_buff *skb = 0;
547 struct net_device *dev = pd->net_dev;
548 struct oz_hdr *oz_hdr;
551 struct list_head list;
552 int total_size = sizeof(struct oz_hdr);
553 INIT_LIST_HEAD(&list);
555 oz_select_elts_for_tx(&pd->elt_buff, 1, &total_size,
556 pd->max_tx_size, &list);
557 if (list.next == &list)
559 skb = alloc_skb(total_size + OZ_ALLOCATED_SPACE(dev), GFP_ATOMIC);
561 oz_trace("Cannot alloc skb\n");
562 oz_elt_info_free_chain(&pd->elt_buff, &list);
565 skb_reserve(skb, LL_RESERVED_SPACE(dev));
566 skb_reset_network_header(skb);
568 skb->protocol = htons(OZ_ETHERTYPE);
569 if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
570 dev->dev_addr, skb->len) < 0) {
574 oz_hdr = (struct oz_hdr *)skb_put(skb, total_size);
575 oz_hdr->control = (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ISOC;
576 oz_hdr->last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
577 elt = (struct oz_elt *)(oz_hdr+1);
579 for (e = list.next; e != &list; e = e->next) {
580 struct oz_elt_info *ei;
581 ei = container_of(e, struct oz_elt_info, link);
582 memcpy(elt, ei->data, ei->length);
583 elt = oz_next_elt(elt);
585 oz_event_log(OZ_EVT_TX_ISOC, 0, 0, 0, 0);
587 oz_elt_info_free_chain(&pd->elt_buff, &list);
590 /*------------------------------------------------------------------------------
591 * Context: softirq-serialized
593 void oz_retire_tx_frames(struct oz_pd *pd, u8 lpn)
596 struct oz_tx_frame *f;
597 struct list_head *first = 0;
598 struct list_head *last = 0;
602 spin_lock(&pd->tx_frame_lock);
603 e = pd->tx_queue.next;
604 while (e != &pd->tx_queue) {
605 f = container_of(e, struct oz_tx_frame, link);
606 pkt_num = le32_to_cpu(get_unaligned(&f->hdr.pkt_num));
607 diff = (lpn - (pkt_num & OZ_LAST_PN_MASK)) & OZ_LAST_PN_MASK;
608 if (diff > OZ_LAST_PN_HALF_CYCLE)
614 pd->nb_queued_frames--;
617 last->next->prev = &pd->tx_queue;
618 pd->tx_queue.next = last->next;
621 pd->last_sent_frame = &pd->tx_queue;
622 spin_unlock(&pd->tx_frame_lock);
624 f = container_of(first, struct oz_tx_frame, link);
626 oz_retire_frame(pd, f);
629 /*------------------------------------------------------------------------------
630 * Precondition: stream_lock must be held.
633 static struct oz_isoc_stream *pd_stream_find(struct oz_pd *pd, u8 ep_num)
636 struct oz_isoc_stream *st;
637 list_for_each(e, &pd->stream_list) {
638 st = container_of(e, struct oz_isoc_stream, link);
639 if (st->ep_num == ep_num)
644 /*------------------------------------------------------------------------------
647 int oz_isoc_stream_create(struct oz_pd *pd, u8 ep_num)
649 struct oz_isoc_stream *st =
650 kzalloc(sizeof(struct oz_isoc_stream), GFP_ATOMIC);
654 spin_lock_bh(&pd->stream_lock);
655 if (!pd_stream_find(pd, ep_num)) {
656 list_add(&st->link, &pd->stream_list);
659 spin_unlock_bh(&pd->stream_lock);
664 /*------------------------------------------------------------------------------
665 * Context: softirq or process
667 static void oz_isoc_stream_free(struct oz_isoc_stream *st)
673 /*------------------------------------------------------------------------------
676 int oz_isoc_stream_delete(struct oz_pd *pd, u8 ep_num)
678 struct oz_isoc_stream *st;
679 spin_lock_bh(&pd->stream_lock);
680 st = pd_stream_find(pd, ep_num);
683 spin_unlock_bh(&pd->stream_lock);
685 oz_isoc_stream_free(st);
688 /*------------------------------------------------------------------------------
691 static void oz_isoc_destructor(struct sk_buff *skb)
693 atomic_dec(&g_submitted_isoc);
694 oz_event_log(OZ_EVT_TX_ISOC_DONE, atomic_read(&g_submitted_isoc),
697 /*------------------------------------------------------------------------------
700 int oz_send_isoc_unit(struct oz_pd *pd, u8 ep_num, u8 *data, int len)
702 struct net_device *dev = pd->net_dev;
703 struct oz_isoc_stream *st;
705 struct sk_buff *skb = 0;
706 struct oz_hdr *oz_hdr = 0;
708 spin_lock_bh(&pd->stream_lock);
709 st = pd_stream_find(pd, ep_num);
713 nb_units = st->nb_units;
718 spin_unlock_bh(&pd->stream_lock);
722 /* Allocate enough space for max size frame. */
723 skb = alloc_skb(pd->max_tx_size + OZ_ALLOCATED_SPACE(dev),
727 /* Reserve the head room for lower layers. */
728 skb_reserve(skb, LL_RESERVED_SPACE(dev));
729 skb_reset_network_header(skb);
731 skb->protocol = htons(OZ_ETHERTYPE);
732 size = sizeof(struct oz_hdr) + sizeof(struct oz_isoc_large);
733 oz_hdr = (struct oz_hdr *)skb_put(skb, size);
735 memcpy(skb_put(skb, len), data, len);
737 if (++nb_units < pd->ms_per_isoc) {
738 spin_lock_bh(&pd->stream_lock);
740 st->nb_units = nb_units;
743 spin_unlock_bh(&pd->stream_lock);
746 struct oz_isoc_large iso;
747 spin_lock_bh(&pd->stream_lock);
748 iso.frame_number = st->frame_num;
749 st->frame_num += nb_units;
750 spin_unlock_bh(&pd->stream_lock);
752 (OZ_PROTOCOL_VERSION<<OZ_VERSION_SHIFT) | OZ_F_ISOC;
753 oz.last_pkt_num = pd->trigger_pkt_num & OZ_LAST_PN_MASK;
755 iso.endpoint = ep_num;
756 iso.format = OZ_DATA_F_ISOC_LARGE;
757 iso.ms_data = nb_units;
758 memcpy(oz_hdr, &oz, sizeof(oz));
759 memcpy(oz_hdr+1, &iso, sizeof(iso));
760 if (dev_hard_header(skb, dev, OZ_ETHERTYPE, pd->mac_addr,
761 dev->dev_addr, skb->len) < 0) {
765 if (atomic_read(&g_submitted_isoc) < OZ_MAX_SUBMITTED_ISOC) {
766 skb->destructor = oz_isoc_destructor;
767 atomic_inc(&g_submitted_isoc);
768 oz_event_log(OZ_EVT_TX_ISOC, nb_units, iso.frame_number,
769 skb, atomic_read(&g_submitted_isoc));
770 if (dev_queue_xmit(skb) < 0)
773 oz_event_log(OZ_EVT_TX_ISOC_DROP, 0, 0, 0, 0);
779 /*------------------------------------------------------------------------------
782 void oz_apps_init(void)
785 for (i = 0; i < OZ_APPID_MAX; i++)
786 if (g_app_if[i].init)
789 /*------------------------------------------------------------------------------
792 void oz_apps_term(void)
795 /* Terminate all the apps. */
796 for (i = 0; i < OZ_APPID_MAX; i++)
797 if (g_app_if[i].term)
800 /*------------------------------------------------------------------------------
801 * Context: softirq-serialized
803 void oz_handle_app_elt(struct oz_pd *pd, u8 app_id, struct oz_elt *elt)
805 struct oz_app_if *ai;
806 if (app_id == 0 || app_id > OZ_APPID_MAX)
808 ai = &g_app_if[app_id-1];
811 /*------------------------------------------------------------------------------
812 * Context: softirq or process
814 void oz_pd_indicate_farewells(struct oz_pd *pd)
816 struct oz_farewell *f;
817 struct oz_app_if *ai = &g_app_if[OZ_APPID_USB-1];
819 oz_polling_lock_bh();
820 if (list_empty(&pd->farewell_list)) {
821 oz_polling_unlock_bh();
824 f = list_first_entry(&pd->farewell_list,
825 struct oz_farewell, link);
827 oz_polling_unlock_bh();
829 ai->farewell(pd, f->ep_num, f->report, f->len);