1 files changed, 123 insertions, 37 deletions
diff --git a/drivers/uwb/drp-ie.c b/drivers/uwb/drp-ie.c
index 75491d47806b..2840d7bf9e67 100644
--- a/drivers/uwb/drp-ie.c
+++ b/drivers/uwb/drp-ie.c
@@ -22,6 +22,96 @@
 #include "uwb-internal.h"
+/*
+ * Return the reason code for a reservations's DRP IE.
+ */
+int uwb_rsv_reason_code(struct uwb_rsv *rsv)
+{
+        static const int reason_codes[] = {
+                [UWB_RSV_STATE_O_INITIATED]          = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_O_PENDING]            = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_O_MODIFIED]           = UWB_DRP_REASON_MODIFIED,
+                [UWB_RSV_STATE_O_ESTABLISHED]        = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_O_TO_BE_MOVED]        = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_O_MOVE_COMBINING]     = UWB_DRP_REASON_MODIFIED,
+                [UWB_RSV_STATE_O_MOVE_REDUCING]      = UWB_DRP_REASON_MODIFIED,
+                [UWB_RSV_STATE_O_MOVE_EXPANDING]     = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_T_ACCEPTED]           = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_T_CONFLICT]           = UWB_DRP_REASON_CONFLICT,
+                [UWB_RSV_STATE_T_PENDING]            = UWB_DRP_REASON_PENDING,
+                [UWB_RSV_STATE_T_DENIED]             = UWB_DRP_REASON_DENIED,
+                [UWB_RSV_STATE_T_RESIZED]            = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
+                [UWB_RSV_STATE_T_EXPANDING_PENDING]  = UWB_DRP_REASON_PENDING,
+                [UWB_RSV_STATE_T_EXPANDING_DENIED]   = UWB_DRP_REASON_DENIED,
+        };
+        return reason_codes[rsv->state];
+}
+/*
+ * Return the reason code for a reservations's companion DRP IE .
+ */
+int uwb_rsv_companion_reason_code(struct uwb_rsv *rsv)
+{
+        static const int companion_reason_codes[] = {
+                [UWB_RSV_STATE_O_MOVE_EXPANDING]     = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
+                [UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
+                [UWB_RSV_STATE_T_EXPANDING_PENDING]  = UWB_DRP_REASON_PENDING,
+                [UWB_RSV_STATE_T_EXPANDING_DENIED]   = UWB_DRP_REASON_DENIED,
+        };
+        return companion_reason_codes[rsv->state];
+}
+/*
+ * Return the status bit for a reservations's DRP IE.
+ */
+int uwb_rsv_status(struct uwb_rsv *rsv)
+{
+        static const int statuses[] = {
+                [UWB_RSV_STATE_O_INITIATED]          = 0,
+                [UWB_RSV_STATE_O_PENDING]            = 0,
+                [UWB_RSV_STATE_O_MODIFIED]           = 1,
+                [UWB_RSV_STATE_O_ESTABLISHED]        = 1,
+                [UWB_RSV_STATE_O_TO_BE_MOVED]        = 0,
+                [UWB_RSV_STATE_O_MOVE_COMBINING]     = 1,
+                [UWB_RSV_STATE_O_MOVE_REDUCING]      = 1,
+                [UWB_RSV_STATE_O_MOVE_EXPANDING]     = 1,
+                [UWB_RSV_STATE_T_ACCEPTED]           = 1,
+                [UWB_RSV_STATE_T_CONFLICT]           = 0,
+                [UWB_RSV_STATE_T_PENDING]            = 0,
+                [UWB_RSV_STATE_T_DENIED]             = 0,
+                [UWB_RSV_STATE_T_RESIZED]            = 1,
+                [UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
+                [UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 1,
+                [UWB_RSV_STATE_T_EXPANDING_PENDING]  = 1,
+                [UWB_RSV_STATE_T_EXPANDING_DENIED]   = 1,
+        };
+        return statuses[rsv->state];
+}
+/*
+ * Return the status bit for a reservations's companion DRP IE .
+ */
+int uwb_rsv_companion_status(struct uwb_rsv *rsv)
+{
+        static const int companion_statuses[] = {
+                [UWB_RSV_STATE_O_MOVE_EXPANDING]     = 0,
+                [UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
+                [UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 0,
+                [UWB_RSV_STATE_T_EXPANDING_PENDING]  = 0,
+                [UWB_RSV_STATE_T_EXPANDING_DENIED]   = 0,
+        };
+        return companion_statuses[rsv->state];
+}
 /*
 * Allocate a DRP IE.
 *
@@ -33,16 +123,12 @@
 static struct uwb_ie_drp *uwb_drp_ie_alloc(void)
 {
        struct uwb_ie_drp *drp_ie;
-        unsigned tiebreaker;
        drp_ie = kzalloc(sizeof(struct uwb_ie_drp) +
                        UWB_NUM_ZONES * sizeof(struct uwb_drp_alloc),
                        GFP_KERNEL);
        if (drp_ie) {
                drp_ie->hdr.element_id = UWB_IE_DRP;
-                get_random_bytes(&tiebreaker, sizeof(unsigned));
-                uwb_ie_drp_set_tiebreaker(drp_ie, tiebreaker & 1);
        }
        return drp_ie;
 }
@@ -103,43 +189,17 @@ static void uwb_drp_ie_from_bm(struct uwb_ie_drp *drp_ie,
 */
 int uwb_drp_ie_update(struct uwb_rsv *rsv)
 {
-        struct device *dev = &rsv->rc->uwb_dev.dev;
        struct uwb_ie_drp *drp_ie;
-        int reason_code, status;
+        struct uwb_rsv_move *mv;
+        int unsafe;
-        switch (rsv->state) {
+        if (rsv->state == UWB_RSV_STATE_NONE) {
-        case UWB_RSV_STATE_NONE:
                kfree(rsv->drp_ie);
                rsv->drp_ie = NULL;
                return 0;
-        case UWB_RSV_STATE_O_INITIATED:
-                reason_code = UWB_DRP_REASON_ACCEPTED;
-                status = 0;
-                break;
-        case UWB_RSV_STATE_O_PENDING:
-                reason_code = UWB_DRP_REASON_ACCEPTED;
-                status = 0;
-                break;
-        case UWB_RSV_STATE_O_MODIFIED:
-                reason_code = UWB_DRP_REASON_MODIFIED;
-                status = 1;
-                break;
-        case UWB_RSV_STATE_O_ESTABLISHED:
-                reason_code = UWB_DRP_REASON_ACCEPTED;
-                status = 1;
-                break;
-        case UWB_RSV_STATE_T_ACCEPTED:
-                reason_code = UWB_DRP_REASON_ACCEPTED;
-                status = 1;
-                break;
-        case UWB_RSV_STATE_T_DENIED:
-                reason_code = UWB_DRP_REASON_DENIED;
-                status = 0;
-                break;
-        default:
-                dev_dbg(dev, "rsv with unhandled state (%d)\n", rsv->state);
-                return -EINVAL;
        }
+        
+        unsafe = rsv->mas.unsafe ? 1 : 0;
        if (rsv->drp_ie == NULL) {
                rsv->drp_ie = uwb_drp_ie_alloc();
@@ -148,9 +208,11 @@ int uwb_drp_ie_update(struct uwb_rsv *rsv)
        }
        drp_ie = rsv->drp_ie;
+        uwb_ie_drp_set_unsafe(drp_ie,       unsafe);
+        uwb_ie_drp_set_tiebreaker(drp_ie,   rsv->tiebreaker);
        uwb_ie_drp_set_owner(drp_ie,        uwb_rsv_is_owner(rsv));
-        uwb_ie_drp_set_status(drp_ie,       status);
+        uwb_ie_drp_set_status(drp_ie,       uwb_rsv_status(rsv));
-        uwb_ie_drp_set_reason_code(drp_ie,  reason_code);
+        uwb_ie_drp_set_reason_code(drp_ie,  uwb_rsv_reason_code(rsv));
        uwb_ie_drp_set_stream_index(drp_ie, rsv->stream);
        uwb_ie_drp_set_type(drp_ie,         rsv->type);
@@ -168,6 +230,27 @@ int uwb_drp_ie_update(struct uwb_rsv *rsv)
        uwb_drp_ie_from_bm(drp_ie, &rsv->mas);
+        if (uwb_rsv_has_two_drp_ies(rsv)) {
+                mv = &rsv->mv; 
+                if (mv->companion_drp_ie == NULL) {
+                        mv->companion_drp_ie = uwb_drp_ie_alloc();
+                        if (mv->companion_drp_ie == NULL)
+                                return -ENOMEM;
+                }
+                drp_ie = mv->companion_drp_ie;
+                
+                /* keep all the same configuration of the main drp_ie */
+                memcpy(drp_ie, rsv->drp_ie, sizeof(struct uwb_ie_drp));
+                
+                /* FIXME: handle properly the unsafe bit */
+                uwb_ie_drp_set_unsafe(drp_ie,       1);
+                uwb_ie_drp_set_status(drp_ie,       uwb_rsv_companion_status(rsv));
+                uwb_ie_drp_set_reason_code(drp_ie,  uwb_rsv_companion_reason_code(rsv));
+        
+                uwb_drp_ie_from_bm(drp_ie, &mv->companion_mas);
+        }
        rsv->ie_valid = true;
        return 0;
 }
@@ -218,6 +301,8 @@ void uwb_drp_ie_to_bm(struct uwb_mas_bm *bm, const struct uwb_ie_drp *drp_ie)
        u8 zone;
        u16 zone_mask;
+        bitmap_zero(bm->bm, UWB_NUM_MAS);
        for (cnt = 0; cnt < numallocs; cnt++) {
                alloc = &drp_ie->allocs[cnt];
                zone_bm = le16_to_cpu(alloc->zone_bm);
@@ -229,3 +314,4 @@ void uwb_drp_ie_to_bm(struct uwb_mas_bm *bm, const struct uwb_ie_drp *drp_ie)
                }
        }
 }

diff --git a/drivers/uwb/drp-ie.c b/drivers/uwb/drp-ie.c index 75491d47806b..2840d7bf9e67 100644 --- a/drivers/uwb/drp-ie.c +++ b/drivers/uwb/drp-ie.c
@@ -22,6 +22,96 @@
22		22
23	#include "uwb-internal.h"	23	#include "uwb-internal.h"
24		24
		25
		26	/*
		27	* Return the reason code for a reservations's DRP IE.
		28	*/
		29	int uwb_rsv_reason_code(struct uwb_rsv *rsv)
		30	{
		31	static const int reason_codes[] = {
		32	[UWB_RSV_STATE_O_INITIATED] = UWB_DRP_REASON_ACCEPTED,
		33	[UWB_RSV_STATE_O_PENDING] = UWB_DRP_REASON_ACCEPTED,
		34	[UWB_RSV_STATE_O_MODIFIED] = UWB_DRP_REASON_MODIFIED,
		35	[UWB_RSV_STATE_O_ESTABLISHED] = UWB_DRP_REASON_ACCEPTED,
		36	[UWB_RSV_STATE_O_TO_BE_MOVED] = UWB_DRP_REASON_ACCEPTED,
		37	[UWB_RSV_STATE_O_MOVE_COMBINING] = UWB_DRP_REASON_MODIFIED,
		38	[UWB_RSV_STATE_O_MOVE_REDUCING] = UWB_DRP_REASON_MODIFIED,
		39	[UWB_RSV_STATE_O_MOVE_EXPANDING] = UWB_DRP_REASON_ACCEPTED,
		40	[UWB_RSV_STATE_T_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
		41	[UWB_RSV_STATE_T_CONFLICT] = UWB_DRP_REASON_CONFLICT,
		42	[UWB_RSV_STATE_T_PENDING] = UWB_DRP_REASON_PENDING,
		43	[UWB_RSV_STATE_T_DENIED] = UWB_DRP_REASON_DENIED,
		44	[UWB_RSV_STATE_T_RESIZED] = UWB_DRP_REASON_ACCEPTED,
		45	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
		46	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
		47	[UWB_RSV_STATE_T_EXPANDING_PENDING] = UWB_DRP_REASON_PENDING,
		48	[UWB_RSV_STATE_T_EXPANDING_DENIED] = UWB_DRP_REASON_DENIED,
		49	};
		50
		51	return reason_codes[rsv->state];
		52	}
		53
		54	/*
		55	* Return the reason code for a reservations's companion DRP IE .
		56	*/
		57	int uwb_rsv_companion_reason_code(struct uwb_rsv *rsv)
		58	{
		59	static const int companion_reason_codes[] = {
		60	[UWB_RSV_STATE_O_MOVE_EXPANDING] = UWB_DRP_REASON_ACCEPTED,
		61	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = UWB_DRP_REASON_ACCEPTED,
		62	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = UWB_DRP_REASON_CONFLICT,
		63	[UWB_RSV_STATE_T_EXPANDING_PENDING] = UWB_DRP_REASON_PENDING,
		64	[UWB_RSV_STATE_T_EXPANDING_DENIED] = UWB_DRP_REASON_DENIED,
		65	};
		66
		67	return companion_reason_codes[rsv->state];
		68	}
		69
		70	/*
		71	* Return the status bit for a reservations's DRP IE.
		72	*/
		73	int uwb_rsv_status(struct uwb_rsv *rsv)
		74	{
		75	static const int statuses[] = {
		76	[UWB_RSV_STATE_O_INITIATED] = 0,
		77	[UWB_RSV_STATE_O_PENDING] = 0,
		78	[UWB_RSV_STATE_O_MODIFIED] = 1,
		79	[UWB_RSV_STATE_O_ESTABLISHED] = 1,
		80	[UWB_RSV_STATE_O_TO_BE_MOVED] = 0,
		81	[UWB_RSV_STATE_O_MOVE_COMBINING] = 1,
		82	[UWB_RSV_STATE_O_MOVE_REDUCING] = 1,
		83	[UWB_RSV_STATE_O_MOVE_EXPANDING] = 1,
		84	[UWB_RSV_STATE_T_ACCEPTED] = 1,
		85	[UWB_RSV_STATE_T_CONFLICT] = 0,
		86	[UWB_RSV_STATE_T_PENDING] = 0,
		87	[UWB_RSV_STATE_T_DENIED] = 0,
		88	[UWB_RSV_STATE_T_RESIZED] = 1,
		89	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
		90	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 1,
		91	[UWB_RSV_STATE_T_EXPANDING_PENDING] = 1,
		92	[UWB_RSV_STATE_T_EXPANDING_DENIED] = 1,
		93
		94	};
		95
		96	return statuses[rsv->state];
		97	}
		98
		99	/*
		100	* Return the status bit for a reservations's companion DRP IE .
		101	*/
		102	int uwb_rsv_companion_status(struct uwb_rsv *rsv)
		103	{
		104	static const int companion_statuses[] = {
		105	[UWB_RSV_STATE_O_MOVE_EXPANDING] = 0,
		106	[UWB_RSV_STATE_T_EXPANDING_ACCEPTED] = 1,
		107	[UWB_RSV_STATE_T_EXPANDING_CONFLICT] = 0,
		108	[UWB_RSV_STATE_T_EXPANDING_PENDING] = 0,
		109	[UWB_RSV_STATE_T_EXPANDING_DENIED] = 0,
		110	};
		111
		112	return companion_statuses[rsv->state];
		113	}
		114
25	/*	115	/*
26	* Allocate a DRP IE.	116	* Allocate a DRP IE.
27	*	117	*
@@ -33,16 +123,12 @@
33	static struct uwb_ie_drp *uwb_drp_ie_alloc(void)	123	static struct uwb_ie_drp *uwb_drp_ie_alloc(void)
34	{	124	{
35	struct uwb_ie_drp *drp_ie;	125	struct uwb_ie_drp *drp_ie;
36	unsigned tiebreaker;
37		126
38	drp_ie = kzalloc(sizeof(struct uwb_ie_drp) +	127	drp_ie = kzalloc(sizeof(struct uwb_ie_drp) +
39	UWB_NUM_ZONES * sizeof(struct uwb_drp_alloc),	128	UWB_NUM_ZONES * sizeof(struct uwb_drp_alloc),
40	GFP_KERNEL);	129	GFP_KERNEL);
41	if (drp_ie) {	130	if (drp_ie) {
42	drp_ie->hdr.element_id = UWB_IE_DRP;	131	drp_ie->hdr.element_id = UWB_IE_DRP;
43
44	get_random_bytes(&tiebreaker, sizeof(unsigned));
45	uwb_ie_drp_set_tiebreaker(drp_ie, tiebreaker & 1);
46	}	132	}
47	return drp_ie;	133	return drp_ie;
48	}	134	}
@@ -103,43 +189,17 @@ static void uwb_drp_ie_from_bm(struct uwb_ie_drp *drp_ie,
103	*/	189	*/
104	int uwb_drp_ie_update(struct uwb_rsv *rsv)	190	int uwb_drp_ie_update(struct uwb_rsv *rsv)
105	{	191	{
106	struct device *dev = &rsv->rc->uwb_dev.dev;
107	struct uwb_ie_drp *drp_ie;	192	struct uwb_ie_drp *drp_ie;
108	int reason_code, status;	193	struct uwb_rsv_move *mv;
		194	int unsafe;
109		195
110	switch (rsv->state) {	196	if (rsv->state == UWB_RSV_STATE_NONE) {
111	case UWB_RSV_STATE_NONE:
112	kfree(rsv->drp_ie);	197	kfree(rsv->drp_ie);
113	rsv->drp_ie = NULL;	198	rsv->drp_ie = NULL;
114	return 0;	199	return 0;
115	case UWB_RSV_STATE_O_INITIATED:
116	reason_code = UWB_DRP_REASON_ACCEPTED;
117	status = 0;
118	break;
119	case UWB_RSV_STATE_O_PENDING:
120	reason_code = UWB_DRP_REASON_ACCEPTED;
121	status = 0;
122	break;
123	case UWB_RSV_STATE_O_MODIFIED:
124	reason_code = UWB_DRP_REASON_MODIFIED;
125	status = 1;
126	break;
127	case UWB_RSV_STATE_O_ESTABLISHED:
128	reason_code = UWB_DRP_REASON_ACCEPTED;
129	status = 1;
130	break;
131	case UWB_RSV_STATE_T_ACCEPTED:
132	reason_code = UWB_DRP_REASON_ACCEPTED;
133	status = 1;
134	break;
135	case UWB_RSV_STATE_T_DENIED:
136	reason_code = UWB_DRP_REASON_DENIED;
137	status = 0;
138	break;
139	default:
140	dev_dbg(dev, "rsv with unhandled state (%d)\n", rsv->state);
141	return -EINVAL;
142	}	200	}
		201
		202	unsafe = rsv->mas.unsafe ? 1 : 0;
143		203
144	if (rsv->drp_ie == NULL) {	204	if (rsv->drp_ie == NULL) {
145	rsv->drp_ie = uwb_drp_ie_alloc();	205	rsv->drp_ie = uwb_drp_ie_alloc();
@@ -148,9 +208,11 @@ int uwb_drp_ie_update(struct uwb_rsv *rsv)
148	}	208	}
149	drp_ie = rsv->drp_ie;	209	drp_ie = rsv->drp_ie;
150		210
		211	uwb_ie_drp_set_unsafe(drp_ie, unsafe);
		212	uwb_ie_drp_set_tiebreaker(drp_ie, rsv->tiebreaker);
151	uwb_ie_drp_set_owner(drp_ie, uwb_rsv_is_owner(rsv));	213	uwb_ie_drp_set_owner(drp_ie, uwb_rsv_is_owner(rsv));
152	uwb_ie_drp_set_status(drp_ie, status);	214	uwb_ie_drp_set_status(drp_ie, uwb_rsv_status(rsv));
153	uwb_ie_drp_set_reason_code(drp_ie, reason_code);	215	uwb_ie_drp_set_reason_code(drp_ie, uwb_rsv_reason_code(rsv));
154	uwb_ie_drp_set_stream_index(drp_ie, rsv->stream);	216	uwb_ie_drp_set_stream_index(drp_ie, rsv->stream);
155	uwb_ie_drp_set_type(drp_ie, rsv->type);	217	uwb_ie_drp_set_type(drp_ie, rsv->type);
156		218
@@ -168,6 +230,27 @@ int uwb_drp_ie_update(struct uwb_rsv *rsv)
168		230
169	uwb_drp_ie_from_bm(drp_ie, &rsv->mas);	231	uwb_drp_ie_from_bm(drp_ie, &rsv->mas);
170		232
		233	if (uwb_rsv_has_two_drp_ies(rsv)) {
		234	mv = &rsv->mv;
		235	if (mv->companion_drp_ie == NULL) {
		236	mv->companion_drp_ie = uwb_drp_ie_alloc();
		237	if (mv->companion_drp_ie == NULL)
		238	return -ENOMEM;
		239	}
		240	drp_ie = mv->companion_drp_ie;
		241
		242	/* keep all the same configuration of the main drp_ie */
		243	memcpy(drp_ie, rsv->drp_ie, sizeof(struct uwb_ie_drp));
		244
		245
		246	/* FIXME: handle properly the unsafe bit */
		247	uwb_ie_drp_set_unsafe(drp_ie, 1);
		248	uwb_ie_drp_set_status(drp_ie, uwb_rsv_companion_status(rsv));
		249	uwb_ie_drp_set_reason_code(drp_ie, uwb_rsv_companion_reason_code(rsv));
		250
		251	uwb_drp_ie_from_bm(drp_ie, &mv->companion_mas);
		252	}
		253
171	rsv->ie_valid = true;	254	rsv->ie_valid = true;
172	return 0;	255	return 0;
173	}	256	}
@@ -218,6 +301,8 @@ void uwb_drp_ie_to_bm(struct uwb_mas_bm bm, const struct uwb_ie_drp drp_ie)
218	u8 zone;	301	u8 zone;
219	u16 zone_mask;	302	u16 zone_mask;
220		303
		304	bitmap_zero(bm->bm, UWB_NUM_MAS);
		305
221	for (cnt = 0; cnt < numallocs; cnt++) {	306	for (cnt = 0; cnt < numallocs; cnt++) {
222	alloc = &drp_ie->allocs[cnt];	307	alloc = &drp_ie->allocs[cnt];
223	zone_bm = le16_to_cpu(alloc->zone_bm);	308	zone_bm = le16_to_cpu(alloc->zone_bm);
@@ -229,3 +314,4 @@ void uwb_drp_ie_to_bm(struct uwb_mas_bm bm, const struct uwb_ie_drp drp_ie)
229	}	314	}
230	}	315	}
231	}	316	}
		317