rtl88x2CE_WiFi_linux add concurrent mode

hc

2024-09-20 cf4ce59b3b70238352c7f1729f0f7223214828ad

 kernel/drivers/net/dsa/mv88e6xxx/hwtstamp.c
..
..
@@ -1,3 +1,4 @@
1
+// SPDX-License-Identifier: GPL-2.0-or-later
1
2
 /*
2
3
  * Marvell 88E6xxx Switch hardware timestamping support
3
4
  *
..
..
@@ -7,11 +8,6 @@
7
8
  *      Erik Hons <erik.hons@ni.com>
8
9
  *      Brandon Streiff <brandon.streiff@ni.com>
9
10
  *      Dane Wagner <dane.wagner@ni.com>
10
- *
11
- * This program is free software; you can redistribute it and/or modify
12
- * it under the terms of the GNU General Public License as published by
13
- * the Free Software Foundation; either version 2 of the License, or
14
- * (at your option) any later version.
15
11
  */
16
12
 
17
13
 #include "chip.h"
..
..
@@ -151,7 +147,7 @@
151
147
 		return -ERANGE;
152
148
 	}
153
149
 
154
-	mutex_lock(&chip->reg_lock);
150
+	mv88e6xxx_reg_lock(chip);
155
151
 	if (tstamp_enable) {
156
152
 		chip->enable_count += 1;
157
153
 		if (chip->enable_count == 1 && ptp_ops->global_enable)
..
..
@@ -165,7 +161,7 @@
165
161
 		if (chip->enable_count == 0 && ptp_ops->global_disable)
166
162
 			ptp_ops->global_disable(chip);
167
163
 	}
168
-	mutex_unlock(&chip->reg_lock);
164
+	mv88e6xxx_reg_unlock(chip);
169
165
 
170
166
 	/* Once hardware has been configured, enable timestamp checks
171
167
 	 * in the RX/TX paths.
..
..
@@ -215,49 +211,20 @@
215
211
 		-EFAULT : 0;
216
212
 }
217
213
 
218
-/* Get the start of the PTP header in this skb */
219
-static u8 *parse_ptp_header(struct sk_buff *skb, unsigned int type)
220
-{
221
-	u8 *data = skb_mac_header(skb);
222
-	unsigned int offset = 0;
223
-
224
-	if (type & PTP_CLASS_VLAN)
225
-		offset += VLAN_HLEN;
226
-
227
-	switch (type & PTP_CLASS_PMASK) {
228
-	case PTP_CLASS_IPV4:
229
-		offset += ETH_HLEN + IPV4_HLEN(data + offset) + UDP_HLEN;
230
-		break;
231
-	case PTP_CLASS_IPV6:
232
-		offset += ETH_HLEN + IP6_HLEN + UDP_HLEN;
233
-		break;
234
-	case PTP_CLASS_L2:
235
-		offset += ETH_HLEN;
236
-		break;
237
-	default:
238
-		return NULL;
239
-	}
240
-
241
-	/* Ensure that the entire header is present in this packet. */
242
-	if (skb->len + ETH_HLEN < offset + 34)
243
-		return NULL;
244
-
245
-	return data + offset;
246
-}
247
-
248
214
 /* Returns a pointer to the PTP header if the caller should time stamp,
249
215
  * or NULL if the caller should not.
250
216
  */
251
-static u8 *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip, int port,
252
-				   struct sk_buff *skb, unsigned int type)
217
+static struct ptp_header *mv88e6xxx_should_tstamp(struct mv88e6xxx_chip *chip,
218
+						  int port, struct sk_buff *skb,
219
+						  unsigned int type)
253
220
 {
254
221
 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
255
-	u8 *hdr;
222
+	struct ptp_header *hdr;
256
223
 
257
224
 	if (!chip->info->ptp_support)
258
225
 		return NULL;
259
226
 
260
-	hdr = parse_ptp_header(skb, type);
227
+	hdr = ptp_parse_header(skb, type);
261
228
 	if (!hdr)
262
229
 		return NULL;
263
230
 
..
..
@@ -279,12 +246,11 @@
279
246
 static int seq_match(struct sk_buff *skb, u16 ts_seqid)
280
247
 {
281
248
 	unsigned int type = SKB_PTP_TYPE(skb);
282
-	u8 *hdr = parse_ptp_header(skb, type);
283
-	__be16 *seqid;
249
+	struct ptp_header *hdr;
284
250
 
285
-	seqid = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
251
+	hdr = ptp_parse_header(skb, type);
286
252
 
287
-	return ts_seqid == ntohs(*seqid);
253
+	return ts_seqid == ntohs(hdr->sequence_id);
288
254
 }
289
255
 
290
256
 static void mv88e6xxx_get_rxts(struct mv88e6xxx_chip *chip,
..
..
@@ -305,10 +271,10 @@
305
271
 	skb_queue_splice_tail_init(rxq, &received);
306
272
 	spin_unlock_irqrestore(&rxq->lock, flags);
307
273
 
308
-	mutex_lock(&chip->reg_lock);
274
+	mv88e6xxx_reg_lock(chip);
309
275
 	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
310
276
 				      reg, buf, ARRAY_SIZE(buf));
311
-	mutex_unlock(&chip->reg_lock);
277
+	mv88e6xxx_reg_unlock(chip);
312
278
 	if (err)
313
279
 		pr_err("failed to get the receive time stamp\n");
314
280
 
..
..
@@ -318,9 +284,9 @@
318
284
 	seq_id = buf[3];
319
285
 
320
286
 	if (status & MV88E6XXX_PTP_TS_VALID) {
321
-		mutex_lock(&chip->reg_lock);
287
+		mv88e6xxx_reg_lock(chip);
322
288
 		err = mv88e6xxx_port_ptp_write(chip, ps->port_id, reg, 0);
323
-		mutex_unlock(&chip->reg_lock);
289
+		mv88e6xxx_reg_unlock(chip);
324
290
 		if (err)
325
291
 			pr_err("failed to clear the receive status\n");
326
292
 	}
..
..
@@ -331,9 +297,9 @@
331
297
 		if (mv88e6xxx_ts_valid(status) && seq_match(skb, seq_id)) {
332
298
 			ns = timehi << 16 | timelo;
333
299
 
334
-			mutex_lock(&chip->reg_lock);
300
+			mv88e6xxx_reg_lock(chip);
335
301
 			ns = timecounter_cyc2time(&chip->tstamp_tc, ns);
336
-			mutex_unlock(&chip->reg_lock);
302
+			mv88e6xxx_reg_unlock(chip);
337
303
 			shwt = skb_hwtstamps(skb);
338
304
 			memset(shwt, 0, sizeof(*shwt));
339
305
 			shwt->hwtstamp = ns_to_ktime(ns);
..
..
@@ -361,9 +327,9 @@
361
327
 				   &ps->rx_queue2);
362
328
 }
363
329
 
364
-static int is_pdelay_resp(u8 *msgtype)
330
+static int is_pdelay_resp(const struct ptp_header *hdr)
365
331
 {
366
-	return (*msgtype & 0xf) == 3;
332
+	return (hdr->tsmt & 0xf) == 3;
367
333
 }
368
334
 
369
335
 bool mv88e6xxx_port_rxtstamp(struct dsa_switch *ds, int port,
..
..
@@ -371,7 +337,7 @@
371
337
 {
372
338
 	struct mv88e6xxx_port_hwtstamp *ps;
373
339
 	struct mv88e6xxx_chip *chip;
374
-	u8 *hdr;
340
+	struct ptp_header *hdr;
375
341
 
376
342
 	chip = ds->priv;
377
343
 	ps = &chip->port_hwtstamp[port];
..
..
@@ -409,12 +375,12 @@
409
375
 	if (!ps->tx_skb)
410
376
 		return 0;
411
377
 
412
-	mutex_lock(&chip->reg_lock);
378
+	mv88e6xxx_reg_lock(chip);
413
379
 	err = mv88e6xxx_port_ptp_read(chip, ps->port_id,
414
380
 				      ptp_ops->dep_sts_reg,
415
381
 				      departure_block,
416
382
 				      ARRAY_SIZE(departure_block));
417
-	mutex_unlock(&chip->reg_lock);
383
+	mv88e6xxx_reg_unlock(chip);
418
384
 
419
385
 	if (err)
420
386
 		goto free_and_clear_skb;
..
..
@@ -434,9 +400,9 @@
434
400
 	}
435
401
 
436
402
 	/* We have the timestamp; go ahead and clear valid now */
437
-	mutex_lock(&chip->reg_lock);
403
+	mv88e6xxx_reg_lock(chip);
438
404
 	mv88e6xxx_port_ptp_write(chip, ps->port_id, ptp_ops->dep_sts_reg, 0);
439
-	mutex_unlock(&chip->reg_lock);
405
+	mv88e6xxx_reg_unlock(chip);
440
406
 
441
407
 	status = departure_block[0] & MV88E6XXX_PTP_TS_STATUS_MASK;
442
408
 	if (status != MV88E6XXX_PTP_TS_STATUS_NORMAL) {
..
..
@@ -451,9 +417,9 @@
451
417
 
452
418
 	memset(&shhwtstamps, 0, sizeof(shhwtstamps));
453
419
 	time_raw = ((u32)departure_block[2] << 16) | departure_block[1];
454
-	mutex_lock(&chip->reg_lock);
420
+	mv88e6xxx_reg_lock(chip);
455
421
 	ns = timecounter_cyc2time(&chip->tstamp_tc, time_raw);
456
-	mutex_unlock(&chip->reg_lock);
422
+	mv88e6xxx_reg_unlock(chip);
457
423
 	shhwtstamps.hwtstamp = ns_to_ktime(ns);
458
424
 
459
425
 	dev_dbg(chip->dev,
..
..
@@ -507,8 +473,7 @@
507
473
 {
508
474
 	struct mv88e6xxx_chip *chip = ds->priv;
509
475
 	struct mv88e6xxx_port_hwtstamp *ps = &chip->port_hwtstamp[port];
510
-	__be16 *seq_ptr;
511
-	u8 *hdr;
476
+	struct ptp_header *hdr;
512
477
 
513
478
 	if (!(skb_shinfo(clone)->tx_flags & SKBTX_HW_TSTAMP))
514
479
 		return false;
..
..
@@ -517,15 +482,13 @@
517
482
 	if (!hdr)
518
483
 		return false;
519
484
 
520
-	seq_ptr = (__be16 *)(hdr + OFF_PTP_SEQUENCE_ID);
521
-
522
485
 	if (test_and_set_bit_lock(MV88E6XXX_HWTSTAMP_TX_IN_PROGRESS,
523
486
 				  &ps->state))
524
487
 		return false;
525
488
 
526
489
 	ps->tx_skb = clone;
527
490
 	ps->tx_tstamp_start = jiffies;
528
-	ps->tx_seq_id = be16_to_cpup(seq_ptr);
491
+	ps->tx_seq_id = be16_to_cpu(hdr->sequence_id);
529
492
 
530
493
 	ptp_schedule_worker(chip->ptp_clock, 0);
531
494
 	return true;