3 files changed, 184 insertions, 58 deletions

diff --git a/drivers/media/dvb/dvb-core/dmxdev.c b/drivers/media/dvb/dvb-core/dmxdev.c
index 6d6121eb5d59..3750ff48cba1 100644
--- a/drivers/media/dvb/dvb-core/dmxdev.c
+++ b/drivers/media/dvb/dvb-core/dmxdev.c
@@ -430,6 +430,8 @@ static int dvb_dmxdev_ts_callback(const u8 *buffer1, size_t buffer1_len,
 /* stop feed but only mark the specified filter as stopped (state set) */
 static int dvb_dmxdev_feed_stop(struct dmxdev_filter *dmxdevfilter)
 {
+        struct dmxdev_feed *feed;
+
         dvb_dmxdev_filter_state_set(dmxdevfilter, DMXDEV_STATE_SET);
 
         switch (dmxdevfilter->type) {
@@ -438,7 +440,8 @@ static int dvb_dmxdev_feed_stop(struct dmxdev_filter *dmxdevfilter)
                 dmxdevfilter->feed.sec->stop_filtering(dmxdevfilter->feed.sec);
                 break;
         case DMXDEV_TYPE_PES:
-                dmxdevfilter->feed.ts->stop_filtering(dmxdevfilter->feed.ts);
+                list_for_each_entry(feed, &dmxdevfilter->feed.ts, next)
+                        feed->ts->stop_filtering(feed->ts);
                 break;
         default:
                 return -EINVAL;
@@ -449,13 +452,23 @@ static int dvb_dmxdev_feed_stop(struct dmxdev_filter *dmxdevfilter)
 /* start feed associated with the specified filter */
 static int dvb_dmxdev_feed_start(struct dmxdev_filter *filter)
 {
+        struct dmxdev_feed *feed;
+        int ret;
+
         dvb_dmxdev_filter_state_set(filter, DMXDEV_STATE_GO);
 
         switch (filter->type) {
         case DMXDEV_TYPE_SEC:
                 return filter->feed.sec->start_filtering(filter->feed.sec);
         case DMXDEV_TYPE_PES:
-                return filter->feed.ts->start_filtering(filter->feed.ts);
+                list_for_each_entry(feed, &filter->feed.ts, next) {
+                        ret = feed->ts->start_filtering(feed->ts);
+                        if (ret < 0) {
+                                dvb_dmxdev_feed_stop(filter);
+                                return ret;
+                        }
+                }
+                break;
         default:
                 return -EINVAL;
         }
@@ -487,6 +500,9 @@ static int dvb_dmxdev_feed_restart(struct dmxdev_filter *filter)
 
 static int dvb_dmxdev_filter_stop(struct dmxdev_filter *dmxdevfilter)
 {
+        struct dmxdev_feed *feed;
+        struct dmx_demux *demux;
+
         if (dmxdevfilter->state < DMXDEV_STATE_GO)
                 return 0;
 
@@ -503,13 +519,12 @@ static int dvb_dmxdev_filter_stop(struct dmxdev_filter *dmxdevfilter)
                 dmxdevfilter->feed.sec = NULL;
                 break;
         case DMXDEV_TYPE_PES:
-                if (!dmxdevfilter->feed.ts)
-                        break;
                 dvb_dmxdev_feed_stop(dmxdevfilter);
-                dmxdevfilter->dev->demux->
-                    release_ts_feed(dmxdevfilter->dev->demux,
-                                    dmxdevfilter->feed.ts);
-                dmxdevfilter->feed.ts = NULL;
+                demux = dmxdevfilter->dev->demux;
+                list_for_each_entry(feed, &dmxdevfilter->feed.ts, next) {
+                        demux->release_ts_feed(demux, feed->ts);
+                        feed->ts = NULL;
+                }
                 break;
         default:
                 if (dmxdevfilter->state == DMXDEV_STATE_ALLOCATED)
@@ -521,19 +536,88 @@ static int dvb_dmxdev_filter_stop(struct dmxdev_filter *dmxdevfilter)
         return 0;
 }
 
+static void dvb_dmxdev_delete_pids(struct dmxdev_filter *dmxdevfilter)
+{
+        struct dmxdev_feed *feed, *tmp;
+
+        /* delete all PIDs */
+        list_for_each_entry_safe(feed, tmp, &dmxdevfilter->feed.ts, next) {
+                list_del(&feed->next);
+                kfree(feed);
+        }
+
+        BUG_ON(!list_empty(&dmxdevfilter->feed.ts));
+}
+
 static inline int dvb_dmxdev_filter_reset(struct dmxdev_filter *dmxdevfilter)
 {
         if (dmxdevfilter->state < DMXDEV_STATE_SET)
                 return 0;
 
+        if (dmxdevfilter->type == DMXDEV_TYPE_PES)
+                dvb_dmxdev_delete_pids(dmxdevfilter);
+
         dmxdevfilter->type = DMXDEV_TYPE_NONE;
         dvb_dmxdev_filter_state_set(dmxdevfilter, DMXDEV_STATE_ALLOCATED);
         return 0;
 }
 
+static int dvb_dmxdev_start_feed(struct dmxdev *dmxdev,
+                                 struct dmxdev_filter *filter,
+                                 struct dmxdev_feed *feed)
+{
+        struct timespec timeout = { 0 };
+        struct dmx_pes_filter_params *para = &filter->params.pes;
+        dmx_output_t otype;
+        int ret;
+        int ts_type;
+        enum dmx_ts_pes ts_pes;
+        struct dmx_ts_feed *tsfeed;
+
+        feed->ts = NULL;
+        otype = para->output;
+
+        ts_pes = (enum dmx_ts_pes)para->pes_type;
+
+        if (ts_pes < DMX_PES_OTHER)
+                ts_type = TS_DECODER;
+        else
+                ts_type = 0;
+
+        if (otype == DMX_OUT_TS_TAP)
+                ts_type |= TS_PACKET;
+        else if (otype == DMX_OUT_TSDEMUX_TAP)
+                ts_type |= TS_PACKET | TS_DEMUX;
+        else if (otype == DMX_OUT_TAP)
+                ts_type |= TS_PACKET | TS_DEMUX | TS_PAYLOAD_ONLY;
+
+        ret = dmxdev->demux->allocate_ts_feed(dmxdev->demux, &feed->ts,
+                                              dvb_dmxdev_ts_callback);
+        if (ret < 0)
+                return ret;
+
+        tsfeed = feed->ts;
+        tsfeed->priv = filter;
+
+        ret = tsfeed->set(tsfeed, feed->pid, ts_type, ts_pes, 32768, timeout);
+        if (ret < 0) {
+                dmxdev->demux->release_ts_feed(dmxdev->demux, tsfeed);
+                return ret;
+        }
+
+        ret = tsfeed->start_filtering(tsfeed);
+        if (ret < 0) {
+                dmxdev->demux->release_ts_feed(dmxdev->demux, tsfeed);
+                return ret;
+        }
+
+        return 0;
+}
+
 static int dvb_dmxdev_filter_start(struct dmxdev_filter *filter)
 {
         struct dmxdev *dmxdev = filter->dev;
+        struct dmxdev_feed *feed;
         void *mem;
         int ret, i;
 
@@ -631,56 +715,14 @@ static int dvb_dmxdev_filter_start(struct dmxdev_filter *filter)
                 break;
         }
         case DMXDEV_TYPE_PES:
-        {
-                struct timespec timeout = { 0 };
-                struct dmx_pes_filter_params *para = &filter->params.pes;
-                dmx_output_t otype;
-                int ts_type;
-                enum dmx_ts_pes ts_pes;
-                struct dmx_ts_feed **tsfeed = &filter->feed.ts;
-
-                filter->feed.ts = NULL;
-                otype = para->output;
-
-                ts_pes = (enum dmx_ts_pes)para->pes_type;
-
-                if (ts_pes < DMX_PES_OTHER)
-                        ts_type = TS_DECODER;
-                else
-                        ts_type = 0;
-
-                if (otype == DMX_OUT_TS_TAP)
-                        ts_type |= TS_PACKET;
-                else if (otype == DMX_OUT_TSDEMUX_TAP)
-                        ts_type |= TS_PACKET | TS_DEMUX;
-                else if (otype == DMX_OUT_TAP)
-                        ts_type |= TS_PACKET | TS_DEMUX | TS_PAYLOAD_ONLY;
-
-                ret = dmxdev->demux->allocate_ts_feed(dmxdev->demux,
-                                                      tsfeed,
-                                                      dvb_dmxdev_ts_callback);
-                if (ret < 0)
-                        return ret;
-
-                (*tsfeed)->priv = filter;
-
-                ret = (*tsfeed)->set(*tsfeed, para->pid, ts_type, ts_pes,
-                                     32768, timeout);
-                if (ret < 0) {
-                        dmxdev->demux->release_ts_feed(dmxdev->demux,
-                                                       *tsfeed);
-                        return ret;
-                }
-
-                ret = filter->feed.ts->start_filtering(filter->feed.ts);
-                if (ret < 0) {
-                        dmxdev->demux->release_ts_feed(dmxdev->demux,
-                                                       *tsfeed);
-                        return ret;
+                list_for_each_entry(feed, &filter->feed.ts, next) {
+                        ret = dvb_dmxdev_start_feed(dmxdev, filter, feed);
+                        if (ret < 0) {
+                                dvb_dmxdev_filter_stop(filter);
+                                return ret;
+                        }
                 }
-
                 break;
-        }
         default:
                 return -EINVAL;
         }
@@ -718,7 +760,7 @@ static int dvb_demux_open(struct inode *inode, struct file *file)
         dvb_ringbuffer_init(&dmxdevfilter->buffer, NULL, 8192);
         dmxdevfilter->type = DMXDEV_TYPE_NONE;
         dvb_dmxdev_filter_state_set(dmxdevfilter, DMXDEV_STATE_ALLOCATED);
-        dmxdevfilter->feed.ts = NULL;
+        INIT_LIST_HEAD(&dmxdevfilter->feed.ts);
         init_timer(&dmxdevfilter->timer);
 
         dvbdev->users++;
@@ -760,6 +802,55 @@ static inline void invert_mode(dmx_filter_t *filter)
                 filter->mode[i] ^= 0xff;
 }
 
+static int dvb_dmxdev_add_pid(struct dmxdev *dmxdev,
+                              struct dmxdev_filter *filter, u16 pid)
+{
+        struct dmxdev_feed *feed;
+
+        if ((filter->type != DMXDEV_TYPE_PES) ||
+            (filter->state < DMXDEV_STATE_SET))
+                return -EINVAL;
+
+        /* only TS packet filters may have multiple PIDs */
+        if ((filter->params.pes.output != DMX_OUT_TSDEMUX_TAP) &&
+            (!list_empty(&filter->feed.ts)))
+                return -EINVAL;
+
+        feed = kzalloc(sizeof(struct dmxdev_feed), GFP_KERNEL);
+        if (feed == NULL)
+                return -ENOMEM;
+
+        feed->pid = pid;
+        list_add(&feed->next, &filter->feed.ts);
+
+        if (filter->state >= DMXDEV_STATE_GO)
+                return dvb_dmxdev_start_feed(dmxdev, filter, feed);
+
+        return 0;
+}
+
+static int dvb_dmxdev_remove_pid(struct dmxdev *dmxdev,
+                                  struct dmxdev_filter *filter, u16 pid)
+{
+        struct dmxdev_feed *feed, *tmp;
+
+        if ((filter->type != DMXDEV_TYPE_PES) ||
+            (filter->state < DMXDEV_STATE_SET))
+                return -EINVAL;
+
+        list_for_each_entry_safe(feed, tmp, &filter->feed.ts, next) {
+                if ((feed->pid == pid) && (feed->ts != NULL)) {
+                        feed->ts->stop_filtering(feed->ts);
+                        filter->dev->demux->release_ts_feed(filter->dev->demux,
+                                                            feed->ts);
+                        list_del(&feed->next);
+                        kfree(feed);
+                }
+        }
+
+        return 0;
+}
+
 static int dvb_dmxdev_filter_set(struct dmxdev *dmxdev,
                                  struct dmxdev_filter *dmxdevfilter,
                                  struct dmx_sct_filter_params *params)
@@ -784,7 +875,10 @@ static int dvb_dmxdev_pes_filter_set(struct dmxdev *dmxdev,
                                      struct dmxdev_filter *dmxdevfilter,
                                      struct dmx_pes_filter_params *params)
 {
+        int ret;
+
         dvb_dmxdev_filter_stop(dmxdevfilter);
+        dvb_dmxdev_filter_reset(dmxdevfilter);
 
         if (params->pes_type > DMX_PES_OTHER || params->pes_type < 0)
                 return -EINVAL;
@@ -795,6 +889,11 @@ static int dvb_dmxdev_pes_filter_set(struct dmxdev *dmxdev,
 
         dvb_dmxdev_filter_state_set(dmxdevfilter, DMXDEV_STATE_SET);
 
+        ret = dvb_dmxdev_add_pid(dmxdev, dmxdevfilter,
+                                 dmxdevfilter->params.pes.pid);
+        if (ret < 0)
+                return ret;
+
         if (params->flags & DMX_IMMEDIATE_START)
                 return dvb_dmxdev_filter_start(dmxdevfilter);
 
@@ -958,6 +1057,24 @@ static int dvb_demux_do_ioctl(struct inode *inode, struct file *file,
                                              &((struct dmx_stc *)parg)->base);
                 break;
 
+        case DMX_ADD_PID:
+                if (mutex_lock_interruptible(&dmxdevfilter->mutex)) {
+                        ret = -ERESTARTSYS;
+                        break;
+                }
+                ret = dvb_dmxdev_add_pid(dmxdev, dmxdevfilter, *(u16 *)parg);
+                mutex_unlock(&dmxdevfilter->mutex);
+                break;
+
+        case DMX_REMOVE_PID:
+                if (mutex_lock_interruptible(&dmxdevfilter->mutex)) {
+                        ret = -ERESTARTSYS;
+                        break;
+                }
+                ret = dvb_dmxdev_remove_pid(dmxdev, dmxdevfilter, *(u16 *)parg);
+                mutex_unlock(&dmxdevfilter->mutex);
+                break;
+
         default:
                 ret = -EINVAL;
                 break;
diff --git a/drivers/media/dvb/dvb-core/dmxdev.h b/drivers/media/dvb/dvb-core/dmxdev.h
index 29746e70d325..c1379b56dfb4 100644
--- a/drivers/media/dvb/dvb-core/dmxdev.h
+++ b/drivers/media/dvb/dvb-core/dmxdev.h
@@ -53,13 +53,20 @@ enum dmxdev_state {
         DMXDEV_STATE_TIMEDOUT
 };
 
+struct dmxdev_feed {
+        u16 pid;
+        struct dmx_ts_feed *ts;
+        struct list_head next;
+};
+
 struct dmxdev_filter {
         union {
                 struct dmx_section_filter *sec;
         } filter;
 
         union {
-                struct dmx_ts_feed *ts;
+                /* list of TS and PES feeds (struct dmxdev_feed) */
+                struct list_head ts;
                 struct dmx_section_feed *sec;
         } feed;
 
diff --git a/include/linux/dvb/dmx.h b/include/linux/dvb/dmx.h
index fef943738a24..f078f3ac82d4 100644
--- a/include/linux/dvb/dmx.h
+++ b/include/linux/dvb/dmx.h
@@ -151,5 +151,7 @@ struct dmx_stc {
 #define DMX_GET_CAPS             _IOR('o', 48, dmx_caps_t)
 #define DMX_SET_SOURCE           _IOW('o', 49, dmx_source_t)
 #define DMX_GET_STC              _IOWR('o', 50, struct dmx_stc)
+#define DMX_ADD_PID              _IOW('o', 51, __u16)
+#define DMX_REMOVE_PID           _IOW('o', 52, __u16)
 
 #endif /*_DVBDMX_H_*/