1 // SPDX-License-Identifier: GPL-2.0-only
2 
3 #include <linux/ethtool.h>
4 #include <linux/jiffies.h>
5 
6 #include "common.h"
7 #include "module_fw.h"
8 #include "cmis.h"
9 
10 /* For accessing the LPL field on page 9Fh, the allowable length extension is
11  * min(i, 15) byte octets where i specifies the allowable additional number of
12  * byte octets in a READ or a WRITE.
13  */
ethtool_cmis_get_max_lpl_size(u8 num_of_byte_octs)14 u32 ethtool_cmis_get_max_lpl_size(u8 num_of_byte_octs)
15 {
16 	return 8 * (1 + min_t(u8, num_of_byte_octs, 15));
17 }
18 
ethtool_cmis_cdb_compose_args(struct ethtool_cmis_cdb_cmd_args * args,enum ethtool_cmis_cdb_cmd_id cmd,u8 * lpl,u8 lpl_len,u8 * epl,u16 epl_len,u16 max_duration,u8 read_write_len_ext,u16 msleep_pre_rpl,u8 rpl_exp_len,u8 flags)19 void ethtool_cmis_cdb_compose_args(struct ethtool_cmis_cdb_cmd_args *args,
20 				   enum ethtool_cmis_cdb_cmd_id cmd, u8 *lpl,
21 				   u8 lpl_len, u8 *epl, u16 epl_len,
22 				   u16 max_duration, u8 read_write_len_ext,
23 				   u16 msleep_pre_rpl, u8 rpl_exp_len, u8 flags)
24 {
25 	args->req.id = cpu_to_be16(cmd);
26 	args->req.lpl_len = lpl_len;
27 	if (lpl)
28 		memcpy(args->req.payload, lpl, args->req.lpl_len);
29 	if (epl) {
30 		args->req.epl_len = cpu_to_be16(epl_len);
31 		args->req.epl = epl;
32 	}
33 
34 	args->max_duration = max_duration;
35 	args->read_write_len_ext =
36 		ethtool_cmis_get_max_lpl_size(read_write_len_ext);
37 	args->msleep_pre_rpl = msleep_pre_rpl;
38 	args->rpl_exp_len = rpl_exp_len;
39 	args->flags = flags;
40 	args->err_msg = NULL;
41 }
42 
ethtool_cmis_page_init(struct ethtool_module_eeprom * page_data,u8 page,u32 offset,u32 length)43 void ethtool_cmis_page_init(struct ethtool_module_eeprom *page_data,
44 			    u8 page, u32 offset, u32 length)
45 {
46 	page_data->page = page;
47 	page_data->offset = offset;
48 	page_data->length = length;
49 	page_data->i2c_address = ETHTOOL_CMIS_CDB_PAGE_I2C_ADDR;
50 }
51 
52 #define CMIS_REVISION_PAGE	0x00
53 #define CMIS_REVISION_OFFSET	0x01
54 
55 struct cmis_rev_rpl {
56 	u8 rev;
57 };
58 
cmis_rev_rpl_major(struct cmis_rev_rpl * rpl)59 static u8 cmis_rev_rpl_major(struct cmis_rev_rpl *rpl)
60 {
61 	return rpl->rev >> 4;
62 }
63 
cmis_rev_major_get(struct net_device * dev,u8 * rev_major)64 static int cmis_rev_major_get(struct net_device *dev, u8 *rev_major)
65 {
66 	const struct ethtool_ops *ops = dev->ethtool_ops;
67 	struct ethtool_module_eeprom page_data = {0};
68 	struct netlink_ext_ack extack = {};
69 	struct cmis_rev_rpl rpl = {};
70 	int err;
71 
72 	ethtool_cmis_page_init(&page_data, CMIS_REVISION_PAGE,
73 			       CMIS_REVISION_OFFSET, sizeof(rpl));
74 	page_data.data = (u8 *)&rpl;
75 
76 	err = ops->get_module_eeprom_by_page(dev, &page_data, &extack);
77 	if (err < 0) {
78 		if (extack._msg)
79 			netdev_err(dev, "%s\n", extack._msg);
80 		return err;
81 	}
82 
83 	*rev_major = cmis_rev_rpl_major(&rpl);
84 
85 	return 0;
86 }
87 
88 #define CMIS_CDB_ADVERTISEMENT_PAGE	0x01
89 #define CMIS_CDB_ADVERTISEMENT_OFFSET	0xA3
90 
91 /* Based on section 8.4.11 "CDB Messaging Support Advertisement" in CMIS
92  * standard revision 5.2.
93  */
94 struct cmis_cdb_advert_rpl {
95 	u8	inst_supported;
96 	u8	read_write_len_ext;
97 	u8	resv1;
98 	u8	resv2;
99 };
100 
cmis_cdb_advert_rpl_inst_supported(struct cmis_cdb_advert_rpl * rpl)101 static u8 cmis_cdb_advert_rpl_inst_supported(struct cmis_cdb_advert_rpl *rpl)
102 {
103 	return rpl->inst_supported >> 6;
104 }
105 
cmis_cdb_advertisement_get(struct ethtool_cmis_cdb * cdb,struct net_device * dev,struct ethnl_module_fw_flash_ntf_params * ntf_params)106 static int cmis_cdb_advertisement_get(struct ethtool_cmis_cdb *cdb,
107 				      struct net_device *dev,
108 				      struct ethnl_module_fw_flash_ntf_params *ntf_params)
109 {
110 	const struct ethtool_ops *ops = dev->ethtool_ops;
111 	struct ethtool_module_eeprom page_data = {};
112 	struct cmis_cdb_advert_rpl rpl = {};
113 	struct netlink_ext_ack extack = {};
114 	int err;
115 
116 	ethtool_cmis_page_init(&page_data, CMIS_CDB_ADVERTISEMENT_PAGE,
117 			       CMIS_CDB_ADVERTISEMENT_OFFSET, sizeof(rpl));
118 	page_data.data = (u8 *)&rpl;
119 
120 	err = ops->get_module_eeprom_by_page(dev, &page_data, &extack);
121 	if (err < 0) {
122 		if (extack._msg)
123 			netdev_err(dev, "%s\n", extack._msg);
124 		return err;
125 	}
126 
127 	if (!cmis_cdb_advert_rpl_inst_supported(&rpl)) {
128 		ethnl_module_fw_flash_ntf_err(dev, ntf_params,
129 					      "CDB functionality is not supported",
130 					      NULL);
131 		return -EOPNOTSUPP;
132 	}
133 
134 	cdb->read_write_len_ext = rpl.read_write_len_ext;
135 
136 	return 0;
137 }
138 
139 #define CMIS_PASSWORD_ENTRY_PAGE	0x00
140 #define CMIS_PASSWORD_ENTRY_OFFSET	0x7A
141 
142 struct cmis_password_entry_pl {
143 	__be32 password;
144 };
145 
146 /* See section 9.3.1 "CMD 0000h: Query Status" in CMIS standard revision 5.2.
147  * struct cmis_cdb_query_status_pl and struct cmis_cdb_query_status_rpl are
148  * structured layouts of the flat arrays,
149  * struct ethtool_cmis_cdb_request::payload and
150  * struct ethtool_cmis_cdb_rpl::payload respectively.
151  */
152 struct cmis_cdb_query_status_pl {
153 	u16 response_delay;
154 };
155 
156 struct cmis_cdb_query_status_rpl {
157 	u8 length;
158 	u8 status;
159 };
160 
161 static int
cmis_cdb_validate_password(struct ethtool_cmis_cdb * cdb,struct net_device * dev,const struct ethtool_module_fw_flash_params * params,struct ethnl_module_fw_flash_ntf_params * ntf_params)162 cmis_cdb_validate_password(struct ethtool_cmis_cdb *cdb,
163 			   struct net_device *dev,
164 			   const struct ethtool_module_fw_flash_params *params,
165 			   struct ethnl_module_fw_flash_ntf_params *ntf_params)
166 {
167 	const struct ethtool_ops *ops = dev->ethtool_ops;
168 	struct cmis_cdb_query_status_pl qs_pl = {0};
169 	struct ethtool_module_eeprom page_data = {};
170 	struct ethtool_cmis_cdb_cmd_args args = {};
171 	struct cmis_password_entry_pl pe_pl = {};
172 	struct cmis_cdb_query_status_rpl *rpl;
173 	struct netlink_ext_ack extack = {};
174 	int err;
175 
176 	ethtool_cmis_page_init(&page_data, CMIS_PASSWORD_ENTRY_PAGE,
177 			       CMIS_PASSWORD_ENTRY_OFFSET, sizeof(pe_pl));
178 	page_data.data = (u8 *)&pe_pl;
179 
180 	pe_pl = *((struct cmis_password_entry_pl *)page_data.data);
181 	pe_pl.password = params->password;
182 	err = ops->set_module_eeprom_by_page(dev, &page_data, &extack);
183 	if (err < 0) {
184 		if (extack._msg)
185 			netdev_err(dev, "%s\n", extack._msg);
186 		return err;
187 	}
188 
189 	ethtool_cmis_cdb_compose_args(&args, ETHTOOL_CMIS_CDB_CMD_QUERY_STATUS,
190 				      (u8 *)&qs_pl, sizeof(qs_pl), NULL, 0, 0,
191 				      cdb->read_write_len_ext, 1000,
192 				      sizeof(*rpl),
193 				      CDB_F_COMPLETION_VALID | CDB_F_STATUS_VALID);
194 
195 	err = ethtool_cmis_cdb_execute_cmd(dev, &args);
196 	if (err < 0) {
197 		ethnl_module_fw_flash_ntf_err(dev, ntf_params,
198 					      "Query Status command failed",
199 					      args.err_msg);
200 		return err;
201 	}
202 
203 	rpl = (struct cmis_cdb_query_status_rpl *)args.req.payload;
204 	if (!rpl->length || !rpl->status) {
205 		ethnl_module_fw_flash_ntf_err(dev, ntf_params,
206 					      "Password was not accepted",
207 					      NULL);
208 		return -EINVAL;
209 	}
210 
211 	return 0;
212 }
213 
214 /* Some CDB commands asserts the CDB completion flag only from CMIS
215  * revision 5. Therefore, check the relevant validity flag only when
216  * the revision supports it.
217  */
ethtool_cmis_cdb_check_completion_flag(u8 cmis_rev,u8 * flags)218 void ethtool_cmis_cdb_check_completion_flag(u8 cmis_rev, u8 *flags)
219 {
220 	*flags |= cmis_rev >= 5 ? CDB_F_COMPLETION_VALID : 0;
221 }
222 
223 #define CMIS_CDB_MODULE_FEATURES_RESV_DATA	34
224 
225 /* See section 9.4.1 "CMD 0040h: Module Features" in CMIS standard revision 5.2.
226  * struct cmis_cdb_module_features_rpl is structured layout of the flat
227  * array, ethtool_cmis_cdb_rpl::payload.
228  */
229 struct cmis_cdb_module_features_rpl {
230 	u8	resv1[CMIS_CDB_MODULE_FEATURES_RESV_DATA];
231 	__be16	max_completion_time;
232 };
233 
234 static u16
cmis_cdb_module_features_completion_time(struct cmis_cdb_module_features_rpl * rpl)235 cmis_cdb_module_features_completion_time(struct cmis_cdb_module_features_rpl *rpl)
236 {
237 	return be16_to_cpu(rpl->max_completion_time);
238 }
239 
cmis_cdb_module_features_get(struct ethtool_cmis_cdb * cdb,struct net_device * dev,struct ethnl_module_fw_flash_ntf_params * ntf_params)240 static int cmis_cdb_module_features_get(struct ethtool_cmis_cdb *cdb,
241 					struct net_device *dev,
242 					struct ethnl_module_fw_flash_ntf_params *ntf_params)
243 {
244 	struct ethtool_cmis_cdb_cmd_args args = {};
245 	struct cmis_cdb_module_features_rpl *rpl;
246 	u8 flags = CDB_F_STATUS_VALID;
247 	int err;
248 
249 	ethtool_cmis_cdb_check_completion_flag(cdb->cmis_rev, &flags);
250 	ethtool_cmis_cdb_compose_args(&args,
251 				      ETHTOOL_CMIS_CDB_CMD_MODULE_FEATURES,
252 				      NULL, 0, NULL, 0, 0,
253 				      cdb->read_write_len_ext, 1000,
254 				      sizeof(*rpl), flags);
255 
256 	err = ethtool_cmis_cdb_execute_cmd(dev, &args);
257 	if (err < 0) {
258 		ethnl_module_fw_flash_ntf_err(dev, ntf_params,
259 					      "Module Features command failed",
260 					      args.err_msg);
261 		return err;
262 	}
263 
264 	rpl = (struct cmis_cdb_module_features_rpl *)args.req.payload;
265 	cdb->max_completion_time =
266 		cmis_cdb_module_features_completion_time(rpl);
267 
268 	return 0;
269 }
270 
271 struct ethtool_cmis_cdb *
ethtool_cmis_cdb_init(struct net_device * dev,const struct ethtool_module_fw_flash_params * params,struct ethnl_module_fw_flash_ntf_params * ntf_params)272 ethtool_cmis_cdb_init(struct net_device *dev,
273 		      const struct ethtool_module_fw_flash_params *params,
274 		      struct ethnl_module_fw_flash_ntf_params *ntf_params)
275 {
276 	struct ethtool_cmis_cdb *cdb;
277 	int err;
278 
279 	cdb = kzalloc(sizeof(*cdb), GFP_KERNEL);
280 	if (!cdb)
281 		return ERR_PTR(-ENOMEM);
282 
283 	err = cmis_rev_major_get(dev, &cdb->cmis_rev);
284 	if (err < 0)
285 		goto err;
286 
287 	if (cdb->cmis_rev < 4) {
288 		ethnl_module_fw_flash_ntf_err(dev, ntf_params,
289 					      "CMIS revision doesn't support module firmware flashing",
290 					      NULL);
291 		err = -EOPNOTSUPP;
292 		goto err;
293 	}
294 
295 	err = cmis_cdb_advertisement_get(cdb, dev, ntf_params);
296 	if (err < 0)
297 		goto err;
298 
299 	if (params->password_valid) {
300 		err = cmis_cdb_validate_password(cdb, dev, params, ntf_params);
301 		if (err < 0)
302 			goto err;
303 	}
304 
305 	err = cmis_cdb_module_features_get(cdb, dev, ntf_params);
306 	if (err < 0)
307 		goto err;
308 
309 	return cdb;
310 
311 err:
312 	ethtool_cmis_cdb_fini(cdb);
313 	return ERR_PTR(err);
314 }
315 
ethtool_cmis_cdb_fini(struct ethtool_cmis_cdb * cdb)316 void ethtool_cmis_cdb_fini(struct ethtool_cmis_cdb *cdb)
317 {
318 	kfree(cdb);
319 }
320 
is_completed(u8 data)321 static bool is_completed(u8 data)
322 {
323 	return !!(data & 0x40);
324 }
325 
326 #define CMIS_CDB_STATUS_SUCCESS	0x01
327 
status_success(u8 data)328 static bool status_success(u8 data)
329 {
330 	return data == CMIS_CDB_STATUS_SUCCESS;
331 }
332 
333 #define CMIS_CDB_STATUS_FAIL	0x40
334 
status_fail(u8 data)335 static bool status_fail(u8 data)
336 {
337 	return data & CMIS_CDB_STATUS_FAIL;
338 }
339 
340 struct cmis_wait_for_cond_rpl {
341 	u8 state;
342 };
343 
344 static int
ethtool_cmis_module_poll(struct net_device * dev,struct cmis_wait_for_cond_rpl * rpl,u32 offset,bool (* cond_success)(u8),bool (* cond_fail)(u8))345 ethtool_cmis_module_poll(struct net_device *dev,
346 			 struct cmis_wait_for_cond_rpl *rpl, u32 offset,
347 			 bool (*cond_success)(u8), bool (*cond_fail)(u8))
348 {
349 	const struct ethtool_ops *ops = dev->ethtool_ops;
350 	struct ethtool_module_eeprom page_data = {0};
351 	struct netlink_ext_ack extack = {};
352 	int err;
353 
354 	ethtool_cmis_page_init(&page_data, 0, offset, sizeof(*rpl));
355 	page_data.data = (u8 *)rpl;
356 
357 	err = ops->get_module_eeprom_by_page(dev, &page_data, &extack);
358 	if (err < 0) {
359 		if (extack._msg)
360 			netdev_err_once(dev, "%s\n", extack._msg);
361 		return -EBUSY;
362 	}
363 
364 	if ((*cond_success)(rpl->state))
365 		return 0;
366 
367 	if (*cond_fail && (*cond_fail)(rpl->state))
368 		return -EIO;
369 
370 	return -EBUSY;
371 }
372 
ethtool_cmis_wait_for_cond(struct net_device * dev,u8 flags,u8 flag,u16 max_duration,u32 offset,bool (* cond_success)(u8),bool (* cond_fail)(u8),u8 * state)373 int ethtool_cmis_wait_for_cond(struct net_device *dev, u8 flags, u8 flag,
374 			       u16 max_duration, u32 offset,
375 			       bool (*cond_success)(u8), bool (*cond_fail)(u8),
376 			       u8 *state)
377 {
378 	struct cmis_wait_for_cond_rpl rpl = {};
379 	unsigned long end;
380 	int err;
381 
382 	if (!(flags & flag))
383 		return 0;
384 
385 	if (max_duration == 0)
386 		max_duration = U16_MAX;
387 
388 	end = jiffies + msecs_to_jiffies(max_duration);
389 	do {
390 		err = ethtool_cmis_module_poll(dev, &rpl, offset, cond_success,
391 					       cond_fail);
392 		if (err != -EBUSY)
393 			goto out;
394 
395 		msleep(20);
396 	} while (time_before(jiffies, end));
397 
398 	err = ethtool_cmis_module_poll(dev, &rpl, offset, cond_success,
399 				       cond_fail);
400 	if (err == -EBUSY)
401 		err = -ETIMEDOUT;
402 
403 out:
404 	*state = rpl.state;
405 	return err;
406 }
407 
408 #define CMIS_CDB_COMPLETION_FLAG_OFFSET	0x08
409 
cmis_cdb_wait_for_completion(struct net_device * dev,struct ethtool_cmis_cdb_cmd_args * args)410 static int cmis_cdb_wait_for_completion(struct net_device *dev,
411 					struct ethtool_cmis_cdb_cmd_args *args)
412 {
413 	u8 flag;
414 	int err;
415 
416 	/* Some vendors demand waiting time before checking completion flag
417 	 * in some CDB commands.
418 	 */
419 	msleep(args->msleep_pre_rpl);
420 
421 	err = ethtool_cmis_wait_for_cond(dev, args->flags,
422 					 CDB_F_COMPLETION_VALID,
423 					 args->max_duration,
424 					 CMIS_CDB_COMPLETION_FLAG_OFFSET,
425 					 is_completed, NULL, &flag);
426 	if (err < 0)
427 		args->err_msg = "Completion Flag did not set on time";
428 
429 	return err;
430 }
431 
432 #define CMIS_CDB_STATUS_OFFSET	0x25
433 
cmis_cdb_status_fail_msg_get(u8 status,char ** err_msg)434 static void cmis_cdb_status_fail_msg_get(u8 status, char **err_msg)
435 {
436 	switch (status) {
437 	case 0b10000001:
438 		*err_msg = "CDB Status is in progress: Busy capturing command";
439 		break;
440 	case 0b10000010:
441 		*err_msg =
442 			"CDB Status is in progress: Busy checking/validating command";
443 		break;
444 	case 0b10000011:
445 		*err_msg = "CDB Status is in progress: Busy executing";
446 		break;
447 	case 0b01000000:
448 		*err_msg = "CDB status failed: no specific failure";
449 		break;
450 	case 0b01000010:
451 		*err_msg =
452 			"CDB status failed: Parameter range error or parameter not supported";
453 		break;
454 	case 0b01000101:
455 		*err_msg = "CDB status failed: CdbChkCode error";
456 		break;
457 	case 0b01000110:
458 		*err_msg = "CDB status failed: Password error";
459 		break;
460 	default:
461 		*err_msg = "Unknown failure reason";
462 	}
463 };
464 
cmis_cdb_wait_for_status(struct net_device * dev,struct ethtool_cmis_cdb_cmd_args * args)465 static int cmis_cdb_wait_for_status(struct net_device *dev,
466 				    struct ethtool_cmis_cdb_cmd_args *args)
467 {
468 	u8 status;
469 	int err;
470 
471 	/* Some vendors demand waiting time before checking status in some
472 	 * CDB commands.
473 	 */
474 	msleep(args->msleep_pre_rpl);
475 
476 	err = ethtool_cmis_wait_for_cond(dev, args->flags, CDB_F_STATUS_VALID,
477 					 args->max_duration,
478 					 CMIS_CDB_STATUS_OFFSET,
479 					 status_success, status_fail, &status);
480 	if (err < 0 && !args->err_msg)
481 		cmis_cdb_status_fail_msg_get(status, &args->err_msg);
482 
483 	return err;
484 }
485 
486 #define CMIS_CDB_REPLY_OFFSET	0x86
487 
cmis_cdb_process_reply(struct net_device * dev,struct ethtool_module_eeprom * page_data,struct ethtool_cmis_cdb_cmd_args * args)488 static int cmis_cdb_process_reply(struct net_device *dev,
489 				  struct ethtool_module_eeprom *page_data,
490 				  struct ethtool_cmis_cdb_cmd_args *args)
491 {
492 	u8 rpl_hdr_len = sizeof(struct ethtool_cmis_cdb_rpl_hdr);
493 	u8 rpl_exp_len = args->rpl_exp_len + rpl_hdr_len;
494 	const struct ethtool_ops *ops = dev->ethtool_ops;
495 	struct netlink_ext_ack extack = {};
496 	struct ethtool_cmis_cdb_rpl *rpl;
497 	int err;
498 
499 	if (!args->rpl_exp_len)
500 		return 0;
501 
502 	ethtool_cmis_page_init(page_data, ETHTOOL_CMIS_CDB_CMD_PAGE,
503 			       CMIS_CDB_REPLY_OFFSET, rpl_exp_len);
504 	page_data->data = kmalloc(page_data->length, GFP_KERNEL);
505 	if (!page_data->data)
506 		return -ENOMEM;
507 
508 	err = ops->get_module_eeprom_by_page(dev, page_data, &extack);
509 	if (err < 0) {
510 		if (extack._msg)
511 			netdev_err(dev, "%s\n", extack._msg);
512 		goto out;
513 	}
514 
515 	rpl = (struct ethtool_cmis_cdb_rpl *)page_data->data;
516 	if ((args->rpl_exp_len > rpl->hdr.rpl_len + rpl_hdr_len) ||
517 	    !rpl->hdr.rpl_chk_code) {
518 		err = -EIO;
519 		goto out;
520 	}
521 
522 	args->req.lpl_len = rpl->hdr.rpl_len;
523 	memcpy(args->req.payload, rpl->payload, args->req.lpl_len);
524 
525 out:
526 	kfree(page_data->data);
527 	return err;
528 }
529 
530 static int
__ethtool_cmis_cdb_execute_cmd(struct net_device * dev,struct ethtool_module_eeprom * page_data,u8 page,u32 offset,u32 length,void * data)531 __ethtool_cmis_cdb_execute_cmd(struct net_device *dev,
532 			       struct ethtool_module_eeprom *page_data,
533 			       u8 page, u32 offset, u32 length, void *data)
534 {
535 	const struct ethtool_ops *ops = dev->ethtool_ops;
536 	struct netlink_ext_ack extack = {};
537 	int err;
538 
539 	ethtool_cmis_page_init(page_data, page, offset, length);
540 	page_data->data = kmemdup(data, page_data->length, GFP_KERNEL);
541 	if (!page_data->data)
542 		return -ENOMEM;
543 
544 	err = ops->set_module_eeprom_by_page(dev, page_data, &extack);
545 	if (err < 0) {
546 		if (extack._msg)
547 			netdev_err(dev, "%s\n", extack._msg);
548 	}
549 
550 	kfree(page_data->data);
551 	return err;
552 }
553 
554 #define CMIS_CDB_EPL_PAGE_START			0xA0
555 #define CMIS_CDB_EPL_PAGE_END			0xAF
556 #define CMIS_CDB_EPL_FW_BLOCK_OFFSET_START	128
557 #define CMIS_CDB_EPL_FW_BLOCK_OFFSET_END	255
558 
559 static int
ethtool_cmis_cdb_execute_epl_cmd(struct net_device * dev,struct ethtool_cmis_cdb_cmd_args * args,struct ethtool_module_eeprom * page_data)560 ethtool_cmis_cdb_execute_epl_cmd(struct net_device *dev,
561 				 struct ethtool_cmis_cdb_cmd_args *args,
562 				 struct ethtool_module_eeprom *page_data)
563 {
564 	u16 epl_len = be16_to_cpu(args->req.epl_len);
565 	u32 bytes_written = 0;
566 	u8 page;
567 	int err;
568 
569 	for (page = CMIS_CDB_EPL_PAGE_START;
570 	     page <= CMIS_CDB_EPL_PAGE_END && bytes_written < epl_len; page++) {
571 		u16 offset = CMIS_CDB_EPL_FW_BLOCK_OFFSET_START;
572 
573 		while (offset <= CMIS_CDB_EPL_FW_BLOCK_OFFSET_END &&
574 		       bytes_written < epl_len) {
575 			u32 bytes_left = epl_len - bytes_written;
576 			u16 space_left, bytes_to_write;
577 
578 			space_left = CMIS_CDB_EPL_FW_BLOCK_OFFSET_END - offset + 1;
579 			bytes_to_write = min_t(u16, bytes_left,
580 					       min_t(u16, space_left,
581 						     args->read_write_len_ext));
582 
583 			err = __ethtool_cmis_cdb_execute_cmd(dev, page_data,
584 							     page, offset,
585 							     bytes_to_write,
586 							     args->req.epl + bytes_written);
587 			if (err < 0)
588 				return err;
589 
590 			offset += bytes_to_write;
591 			bytes_written += bytes_to_write;
592 		}
593 	}
594 	return 0;
595 }
596 
cmis_cdb_calc_checksum(const void * data,size_t size)597 static u8 cmis_cdb_calc_checksum(const void *data, size_t size)
598 {
599 	const u8 *bytes = (const u8 *)data;
600 	u8 checksum = 0;
601 
602 	for (size_t i = 0; i < size; i++)
603 		checksum += bytes[i];
604 
605 	return ~checksum;
606 }
607 
608 #define CMIS_CDB_CMD_ID_OFFSET	0x80
609 
ethtool_cmis_cdb_execute_cmd(struct net_device * dev,struct ethtool_cmis_cdb_cmd_args * args)610 int ethtool_cmis_cdb_execute_cmd(struct net_device *dev,
611 				 struct ethtool_cmis_cdb_cmd_args *args)
612 {
613 	struct ethtool_module_eeprom page_data = {};
614 	u32 offset;
615 	int err;
616 
617 	args->req.chk_code =
618 		cmis_cdb_calc_checksum(&args->req,
619 				       offsetof(struct ethtool_cmis_cdb_request,
620 						epl));
621 
622 	if (args->req.lpl_len > args->read_write_len_ext) {
623 		args->err_msg = "LPL length is longer than CDB read write length extension allows";
624 		return -EINVAL;
625 	}
626 
627 	/* According to the CMIS standard, there are two options to trigger the
628 	 * CDB commands. The default option is triggering the command by writing
629 	 * the CMDID bytes. Therefore, the command will be split to 2 calls:
630 	 * First, with everything except the CMDID field and then the CMDID
631 	 * field.
632 	 */
633 	offset = CMIS_CDB_CMD_ID_OFFSET +
634 		offsetof(struct ethtool_cmis_cdb_request, body);
635 	err = __ethtool_cmis_cdb_execute_cmd(dev, &page_data,
636 					     ETHTOOL_CMIS_CDB_CMD_PAGE, offset,
637 					     sizeof(args->req.body),
638 					     &args->req.body);
639 	if (err < 0)
640 		return err;
641 
642 	if (args->req.epl_len) {
643 		err = ethtool_cmis_cdb_execute_epl_cmd(dev, args, &page_data);
644 		if (err < 0)
645 			return err;
646 	}
647 
648 	offset = CMIS_CDB_CMD_ID_OFFSET +
649 		offsetof(struct ethtool_cmis_cdb_request, id);
650 	err = __ethtool_cmis_cdb_execute_cmd(dev, &page_data,
651 					     ETHTOOL_CMIS_CDB_CMD_PAGE, offset,
652 					     sizeof(args->req.id),
653 					     &args->req.id);
654 	if (err < 0)
655 		return err;
656 
657 	err = cmis_cdb_wait_for_completion(dev, args);
658 	if (err < 0)
659 		return err;
660 
661 	err = cmis_cdb_wait_for_status(dev, args);
662 	if (err < 0)
663 		return err;
664 
665 	return cmis_cdb_process_reply(dev, &page_data, args);
666 }
667