--- zzzz-none-000/linux-3.10.107/drivers/uwb/drp.c 2017-06-27 09:49:32.000000000 +0000 +++ scorpion-7490-727/linux-3.10.107/drivers/uwb/drp.c 2021-02-04 17:41:59.000000000 +0000 @@ -29,19 +29,19 @@ enum uwb_drp_conflict_action { /* Reservation is maintained, no action needed */ UWB_DRP_CONFLICT_MANTAIN = 0, - + /* the device shall not transmit frames in conflicting MASs in * the following superframe. If the device is the reservation * target, it shall also set the Reason Code in its DRP IE to * Conflict in its beacon in the following superframe. */ UWB_DRP_CONFLICT_ACT1, - + /* the device shall not set the Reservation Status bit to ONE * and shall not transmit frames in conflicting MASs. If the * device is the reservation target, it shall also set the * Reason Code in its DRP IE to Conflict. - */ + */ UWB_DRP_CONFLICT_ACT2, /* the device shall not transmit frames in conflicting MASs in @@ -59,6 +59,7 @@ struct uwb_rceb *reply, ssize_t reply_size) { struct uwb_rc_evt_set_drp_ie *r = (struct uwb_rc_evt_set_drp_ie *)reply; + unsigned long flags; if (r != NULL) { if (r->bResultCode != UWB_RC_RES_SUCCESS) @@ -67,14 +68,14 @@ } else dev_err(&rc->uwb_dev.dev, "SET-DRP-IE: timeout\n"); - spin_lock_bh(&rc->rsvs_lock); + spin_lock_irqsave(&rc->rsvs_lock, flags); if (rc->set_drp_ie_pending > 1) { rc->set_drp_ie_pending = 0; - uwb_rsv_queue_update(rc); + uwb_rsv_queue_update(rc); } else { - rc->set_drp_ie_pending = 0; + rc->set_drp_ie_pending = 0; } - spin_unlock_bh(&rc->rsvs_lock); + spin_unlock_irqrestore(&rc->rsvs_lock, flags); } /** @@ -114,7 +115,8 @@ if (uwb_rsv_has_two_drp_ies(rsv) && (rsv->mv.companion_drp_ie != NULL)) { mv = &rsv->mv; - num_bytes += mv->companion_drp_ie->hdr.length + 2; + num_bytes += + mv->companion_drp_ie->hdr.length + 2; } } } @@ -138,21 +140,23 @@ memcpy(IEDataptr, rsv->drp_ie, rsv->drp_ie->hdr.length + 2); IEDataptr += rsv->drp_ie->hdr.length + 2; - + if (uwb_rsv_has_two_drp_ies(rsv) && (rsv->mv.companion_drp_ie != NULL)) { mv = &rsv->mv; memcpy(IEDataptr, mv->companion_drp_ie, mv->companion_drp_ie->hdr.length + 2); - IEDataptr += mv->companion_drp_ie->hdr.length + 2; + IEDataptr += + mv->companion_drp_ie->hdr.length + 2; } } } - result = uwb_rc_cmd_async(rc, "SET-DRP-IE", &cmd->rccb, sizeof(*cmd) + num_bytes, - UWB_RC_CET_GENERAL, UWB_RC_CMD_SET_DRP_IE, - uwb_rc_set_drp_cmd_done, NULL); - + result = uwb_rc_cmd_async(rc, "SET-DRP-IE", + &cmd->rccb, sizeof(*cmd) + num_bytes, + UWB_RC_CET_GENERAL, UWB_RC_CMD_SET_DRP_IE, + uwb_rc_set_drp_cmd_done, NULL); + rc->set_drp_ie_pending = 1; kfree(cmd); @@ -175,8 +179,8 @@ int ext_tie_breaker = uwb_ie_drp_tiebreaker(ext_drp_ie); int ext_status = uwb_ie_drp_status(ext_drp_ie); int ext_type = uwb_ie_drp_type(ext_drp_ie); - - + + /* [ECMA-368 2nd Edition] 17.4.6 */ if (ext_type == UWB_DRP_TYPE_PCA && our_type == UWB_DRP_TYPE_PCA) { return UWB_DRP_CONFLICT_MANTAIN; @@ -186,7 +190,7 @@ if (our_type == UWB_DRP_TYPE_ALIEN_BP) { return UWB_DRP_CONFLICT_MANTAIN; } - + /* [ECMA-368 2nd Edition] 17.4.6-2 */ if (ext_type == UWB_DRP_TYPE_ALIEN_BP) { /* here we know our_type != UWB_DRP_TYPE_ALIEN_BP */ @@ -214,7 +218,7 @@ our_beacon_slot > ext_beacon_slot) { return UWB_DRP_CONFLICT_MANTAIN; } - + if (our_status == 0) { if (our_tie_breaker == ext_tie_breaker) { /* [ECMA-368 2nd Edition] 17.4.6-6a */ @@ -243,9 +247,9 @@ return UWB_DRP_CONFLICT_MANTAIN; } -static void handle_conflict_normal(struct uwb_ie_drp *drp_ie, - int ext_beacon_slot, - struct uwb_rsv *rsv, +static void handle_conflict_normal(struct uwb_ie_drp *drp_ie, + int ext_beacon_slot, + struct uwb_rsv *rsv, struct uwb_mas_bm *conflicting_mas) { struct uwb_rc *rc = rsv->rc; @@ -262,7 +266,7 @@ uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_TO_BE_MOVED); if (bow->can_reserve_extra_mases == false) uwb_rsv_backoff_win_increment(rc); - + break; case UWB_DRP_CONFLICT_ACT3: uwb_rsv_backoff_win_increment(rc); @@ -277,13 +281,13 @@ switch(action) { case UWB_DRP_CONFLICT_ACT2: case UWB_DRP_CONFLICT_ACT3: - uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT); + uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT); default: break; } } - + } static void handle_conflict_expanding(struct uwb_ie_drp *drp_ie, int ext_beacon_slot, @@ -294,7 +298,7 @@ struct uwb_drp_backoff_win *bow = &rc->bow; struct uwb_rsv_move *mv = &rsv->mv; int action; - + if (companion_only) { /* status of companion is 0 at this point */ action = evaluate_conflict_action(drp_ie, ext_beacon_slot, rsv, 0); @@ -302,21 +306,24 @@ switch(action) { case UWB_DRP_CONFLICT_ACT2: case UWB_DRP_CONFLICT_ACT3: - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); + uwb_rsv_set_state(rsv, + UWB_RSV_STATE_O_ESTABLISHED); rsv->needs_release_companion_mas = false; if (bow->can_reserve_extra_mases == false) uwb_rsv_backoff_win_increment(rc); - uwb_drp_avail_release(rsv->rc, &rsv->mv.companion_mas); + uwb_drp_avail_release(rsv->rc, + &rsv->mv.companion_mas); } - } else { /* rsv is target */ + } else { /* rsv is target */ switch(action) { case UWB_DRP_CONFLICT_ACT2: case UWB_DRP_CONFLICT_ACT3: - uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_CONFLICT); + uwb_rsv_set_state(rsv, + UWB_RSV_STATE_T_EXPANDING_CONFLICT); /* send_drp_avail_ie = true; */ } } - } else { /* also base part of the reservation is conflicting */ + } else { /* also base part of the reservation is conflicting */ if (uwb_rsv_is_owner(rsv)) { uwb_rsv_backoff_win_increment(rc); /* remove companion part */ @@ -325,7 +332,8 @@ /* drop some mases with reason modified */ /* put in the companion the mases to be dropped */ - bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS); + bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, + conflicting_mas->bm, UWB_NUM_MAS); uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED); } else { /* it is a target rsv */ uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT); @@ -335,7 +343,7 @@ } static void uwb_drp_handle_conflict_rsv(struct uwb_rc *rc, struct uwb_rsv *rsv, - struct uwb_rc_evt_drp *drp_evt, + struct uwb_rc_evt_drp *drp_evt, struct uwb_ie_drp *drp_ie, struct uwb_mas_bm *conflicting_mas) { @@ -344,83 +352,107 @@ /* check if the conflicting reservation has two drp_ies */ if (uwb_rsv_has_two_drp_ies(rsv)) { mv = &rsv->mv; - if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) { - handle_conflict_expanding(drp_ie, drp_evt->beacon_slot_number, - rsv, false, conflicting_mas); + if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, + UWB_NUM_MAS)) { + handle_conflict_expanding(drp_ie, + drp_evt->beacon_slot_number, + rsv, false, conflicting_mas); } else { - if (bitmap_intersects(mv->companion_mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) { - handle_conflict_expanding(drp_ie, drp_evt->beacon_slot_number, - rsv, true, conflicting_mas); + if (bitmap_intersects(mv->companion_mas.bm, + conflicting_mas->bm, UWB_NUM_MAS)) { + handle_conflict_expanding( + drp_ie, drp_evt->beacon_slot_number, + rsv, true, conflicting_mas); } } - } else if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, UWB_NUM_MAS)) { - handle_conflict_normal(drp_ie, drp_evt->beacon_slot_number, rsv, conflicting_mas); + } else if (bitmap_intersects(rsv->mas.bm, conflicting_mas->bm, + UWB_NUM_MAS)) { + handle_conflict_normal(drp_ie, drp_evt->beacon_slot_number, + rsv, conflicting_mas); } } static void uwb_drp_handle_all_conflict_rsv(struct uwb_rc *rc, - struct uwb_rc_evt_drp *drp_evt, + struct uwb_rc_evt_drp *drp_evt, struct uwb_ie_drp *drp_ie, struct uwb_mas_bm *conflicting_mas) { struct uwb_rsv *rsv; - + list_for_each_entry(rsv, &rc->reservations, rc_node) { - uwb_drp_handle_conflict_rsv(rc, rsv, drp_evt, drp_ie, conflicting_mas); + uwb_drp_handle_conflict_rsv(rc, rsv, drp_evt, drp_ie, + conflicting_mas); + } +} + +static void uwb_drp_process_target_accepted(struct uwb_rc *rc, + struct uwb_rsv *rsv, struct uwb_rc_evt_drp *drp_evt, + struct uwb_ie_drp *drp_ie, struct uwb_mas_bm *mas) +{ + struct uwb_rsv_move *mv = &rsv->mv; + int status; + + status = uwb_ie_drp_status(drp_ie); + + if (rsv->state == UWB_RSV_STATE_T_CONFLICT) { + uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT); + return; + } + + if (rsv->state == UWB_RSV_STATE_T_EXPANDING_ACCEPTED) { + /* drp_ie is companion */ + if (!bitmap_equal(rsv->mas.bm, mas->bm, UWB_NUM_MAS)) { + /* stroke companion */ + uwb_rsv_set_state(rsv, + UWB_RSV_STATE_T_EXPANDING_ACCEPTED); + } + } else { + if (!bitmap_equal(rsv->mas.bm, mas->bm, UWB_NUM_MAS)) { + if (uwb_drp_avail_reserve_pending(rc, mas) == -EBUSY) { + /* FIXME: there is a conflict, find + * the conflicting reservations and + * take a sensible action. Consider + * that in drp_ie there is the + * "neighbour" */ + uwb_drp_handle_all_conflict_rsv(rc, drp_evt, + drp_ie, mas); + } else { + /* accept the extra reservation */ + bitmap_copy(mv->companion_mas.bm, mas->bm, + UWB_NUM_MAS); + uwb_rsv_set_state(rsv, + UWB_RSV_STATE_T_EXPANDING_ACCEPTED); + } + } else { + if (status) { + uwb_rsv_set_state(rsv, + UWB_RSV_STATE_T_ACCEPTED); + } + } + } } - + /* * Based on the DRP IE, transition a target reservation to a new * state. */ static void uwb_drp_process_target(struct uwb_rc *rc, struct uwb_rsv *rsv, - struct uwb_ie_drp *drp_ie, struct uwb_rc_evt_drp *drp_evt) + struct uwb_ie_drp *drp_ie, struct uwb_rc_evt_drp *drp_evt) { struct device *dev = &rc->uwb_dev.dev; struct uwb_rsv_move *mv = &rsv->mv; int status; enum uwb_drp_reason reason_code; struct uwb_mas_bm mas; - + status = uwb_ie_drp_status(drp_ie); reason_code = uwb_ie_drp_reason_code(drp_ie); uwb_drp_ie_to_bm(&mas, drp_ie); switch (reason_code) { case UWB_DRP_REASON_ACCEPTED: - - if (rsv->state == UWB_RSV_STATE_T_CONFLICT) { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_CONFLICT); - break; - } - - if (rsv->state == UWB_RSV_STATE_T_EXPANDING_ACCEPTED) { - /* drp_ie is companion */ - if (!bitmap_equal(rsv->mas.bm, mas.bm, UWB_NUM_MAS)) - /* stroke companion */ - uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_ACCEPTED); - } else { - if (!bitmap_equal(rsv->mas.bm, mas.bm, UWB_NUM_MAS)) { - if (uwb_drp_avail_reserve_pending(rc, &mas) == -EBUSY) { - /* FIXME: there is a conflict, find - * the conflicting reservations and - * take a sensible action. Consider - * that in drp_ie there is the - * "neighbour" */ - uwb_drp_handle_all_conflict_rsv(rc, drp_evt, drp_ie, &mas); - } else { - /* accept the extra reservation */ - bitmap_copy(mv->companion_mas.bm, mas.bm, UWB_NUM_MAS); - uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_EXPANDING_ACCEPTED); - } - } else { - if (status) { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_T_ACCEPTED); - } - } - - } + uwb_drp_process_target_accepted(rc, rsv, drp_evt, drp_ie, &mas); break; case UWB_DRP_REASON_MODIFIED: @@ -433,7 +465,8 @@ /* find if the owner wants to expand or reduce */ if (bitmap_subset(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) { /* owner is reducing */ - bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, mas.bm, UWB_NUM_MAS); + bitmap_andnot(mv->companion_mas.bm, rsv->mas.bm, mas.bm, + UWB_NUM_MAS); uwb_drp_avail_release(rsv->rc, &mv->companion_mas); } @@ -446,6 +479,48 @@ } } +static void uwb_drp_process_owner_accepted(struct uwb_rsv *rsv, + struct uwb_mas_bm *mas) +{ + struct uwb_rsv_move *mv = &rsv->mv; + + switch (rsv->state) { + case UWB_RSV_STATE_O_PENDING: + case UWB_RSV_STATE_O_INITIATED: + case UWB_RSV_STATE_O_ESTABLISHED: + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); + break; + case UWB_RSV_STATE_O_MODIFIED: + if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS)) + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); + else + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED); + break; + + case UWB_RSV_STATE_O_MOVE_REDUCING: /* shouldn' t be a problem */ + if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS)) + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); + else + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING); + break; + case UWB_RSV_STATE_O_MOVE_EXPANDING: + if (bitmap_equal(mas->bm, mv->companion_mas.bm, UWB_NUM_MAS)) { + /* Companion reservation accepted */ + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING); + } else { + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_EXPANDING); + } + break; + case UWB_RSV_STATE_O_MOVE_COMBINING: + if (bitmap_equal(mas->bm, rsv->mas.bm, UWB_NUM_MAS)) + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING); + else + uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING); + break; + default: + break; + } +} /* * Based on the DRP IE, transition an owner reservation to a new * state. @@ -455,7 +530,6 @@ struct uwb_rc_evt_drp *drp_evt) { struct device *dev = &rc->uwb_dev.dev; - struct uwb_rsv_move *mv = &rsv->mv; int status; enum uwb_drp_reason reason_code; struct uwb_mas_bm mas; @@ -467,44 +541,7 @@ if (status) { switch (reason_code) { case UWB_DRP_REASON_ACCEPTED: - switch (rsv->state) { - case UWB_RSV_STATE_O_PENDING: - case UWB_RSV_STATE_O_INITIATED: - case UWB_RSV_STATE_O_ESTABLISHED: - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); - break; - case UWB_RSV_STATE_O_MODIFIED: - if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); - } else { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MODIFIED); - } - break; - - case UWB_RSV_STATE_O_MOVE_REDUCING: /* shouldn' t be a problem */ - if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_ESTABLISHED); - } else { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING); - } - break; - case UWB_RSV_STATE_O_MOVE_EXPANDING: - if (bitmap_equal(mas.bm, mv->companion_mas.bm, UWB_NUM_MAS)) { - /* Companion reservation accepted */ - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING); - } else { - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_EXPANDING); - } - break; - case UWB_RSV_STATE_O_MOVE_COMBINING: - if (bitmap_equal(mas.bm, rsv->mas.bm, UWB_NUM_MAS)) - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_REDUCING); - else - uwb_rsv_set_state(rsv, UWB_RSV_STATE_O_MOVE_COMBINING); - break; - default: - break; - } + uwb_drp_process_owner_accepted(rsv, &mas); break; default: dev_warn(dev, "ignoring invalid DRP IE state (%d/%d)\n", @@ -544,9 +581,9 @@ cnflt_update_work); struct uwb_cnflt_alien *c; struct uwb_rc *rc = cnflt->rc; - + unsigned long delay_us = UWB_MAS_LENGTH_US * UWB_MAS_PER_ZONE; - + mutex_lock(&rc->rsvs_mutex); list_del(&cnflt->rc_node); @@ -555,10 +592,12 @@ bitmap_zero(rc->cnflt_alien_bitmap.bm, UWB_NUM_MAS); list_for_each_entry(c, &rc->cnflt_alien_list, rc_node) { - bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, c->mas.bm, UWB_NUM_MAS); + bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, + c->mas.bm, UWB_NUM_MAS); } - - queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, usecs_to_jiffies(delay_us)); + + queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, + usecs_to_jiffies(delay_us)); kfree(cnflt); mutex_unlock(&rc->rsvs_mutex); @@ -580,12 +619,10 @@ struct device *dev = &rc->uwb_dev.dev; struct uwb_mas_bm mas; struct uwb_cnflt_alien *cnflt; - char buf[72]; unsigned long delay_us = UWB_MAS_LENGTH_US * UWB_MAS_PER_ZONE; - + uwb_drp_ie_to_bm(&mas, drp_ie); - bitmap_scnprintf(buf, sizeof(buf), mas.bm, UWB_NUM_MAS); - + list_for_each_entry(cnflt, &rc->cnflt_alien_list, rc_node) { if (bitmap_equal(cnflt->mas.bm, mas.bm, UWB_NUM_MAS)) { /* Existing alien BP reservation conflicting @@ -599,16 +636,17 @@ /* alloc and initialize new uwb_cnflt_alien */ cnflt = kzalloc(sizeof(struct uwb_cnflt_alien), GFP_KERNEL); - if (!cnflt) + if (!cnflt) { dev_err(dev, "failed to alloc uwb_cnflt_alien struct\n"); + return; + } + INIT_LIST_HEAD(&cnflt->rc_node); - init_timer(&cnflt->timer); - cnflt->timer.function = uwb_cnflt_timer; - cnflt->timer.data = (unsigned long)cnflt; + setup_timer(&cnflt->timer, uwb_cnflt_timer, (unsigned long)cnflt); cnflt->rc = rc; INIT_WORK(&cnflt->cnflt_update_work, uwb_cnflt_update_work); - + bitmap_copy(cnflt->mas.bm, mas.bm, UWB_NUM_MAS); list_add_tail(&cnflt->rc_node, &rc->cnflt_alien_list); @@ -617,17 +655,17 @@ bitmap_or(rc->cnflt_alien_bitmap.bm, rc->cnflt_alien_bitmap.bm, mas.bm, UWB_NUM_MAS); queue_delayed_work(rc->rsv_workq, &rc->rsv_alien_bp_work, usecs_to_jiffies(delay_us)); - + /* start the timer */ uwb_cnflt_alien_stroke_timer(cnflt); } static void uwb_drp_process_not_involved(struct uwb_rc *rc, - struct uwb_rc_evt_drp *drp_evt, + struct uwb_rc_evt_drp *drp_evt, struct uwb_ie_drp *drp_ie) { struct uwb_mas_bm mas; - + uwb_drp_ie_to_bm(&mas, drp_ie); uwb_drp_handle_all_conflict_rsv(rc, drp_evt, drp_ie, &mas); } @@ -647,7 +685,7 @@ */ return; } - + /* * Do nothing with DRP IEs for reservations that have been * terminated. @@ -656,12 +694,12 @@ uwb_rsv_set_state(rsv, UWB_RSV_STATE_NONE); return; } - + if (uwb_ie_drp_owner(drp_ie)) uwb_drp_process_target(rc, rsv, drp_ie, drp_evt); else uwb_drp_process_owner(rc, rsv, src, drp_ie, drp_evt); - + }