959 } |
959 } |
960 |
960 |
961 // init SII write request |
961 // init SII write request |
962 INIT_LIST_HEAD(&request.list); |
962 INIT_LIST_HEAD(&request.list); |
963 request.slave = slave; |
963 request.slave = slave; |
964 request.data = data; |
964 request.words = (const uint16_t *) data; |
965 request.word_offset = 0; |
965 request.offset = 0; |
966 request.word_size = size / 2; |
966 request.nwords = size / 2; |
967 |
967 |
968 if (request.word_size < 0x0041) { |
968 if (request.nwords < 0x0041) { |
969 EC_ERR("SII data too short (%u words)! Mimimum is" |
969 EC_ERR("SII data too short (%u words)! Mimimum is" |
970 " 40 fixed words + 1 delimiter. Dropping.\n", |
970 " 40 fixed words + 1 delimiter. Dropping.\n", |
971 request.word_size); |
971 request.nwords); |
972 return -EINVAL; |
972 return -EINVAL; |
973 } |
973 } |
974 |
974 |
975 // calculate checksum |
975 // calculate checksum |
976 crc = ec_slave_sii_crc(data, 14); // CRC over words 0 to 6 |
976 crc = ec_slave_sii_crc(data, 14); // CRC over words 0 to 6 |
977 if (crc != data[14]) { |
977 if (crc != data[14]) { |
978 EC_WARN("SII CRC incorrect. Must be 0x%02x.\n", crc); |
978 EC_WARN("SII CRC incorrect. Must be 0x%02x.\n", crc); |
979 } |
979 } |
980 |
980 |
981 cat_header = (const uint16_t *) request.data |
981 cat_header = request.words + EC_FIRST_SII_CATEGORY_OFFSET; |
982 + EC_FIRST_SII_CATEGORY_OFFSET; |
|
983 cat_type = EC_READ_U16(cat_header); |
982 cat_type = EC_READ_U16(cat_header); |
984 while (cat_type != 0xFFFF) { // cycle through categories |
983 while (cat_type != 0xFFFF) { // cycle through categories |
985 if (cat_header + 1 > |
984 if (cat_header + 1 > request.words + request.nwords) { |
986 (const uint16_t *) request.data + request.word_size) { |
|
987 EC_ERR("SII data corrupted! Dropping.\n"); |
985 EC_ERR("SII data corrupted! Dropping.\n"); |
988 return -EINVAL; |
986 return -EINVAL; |
989 } |
987 } |
990 cat_size = EC_READ_U16(cat_header + 1); |
988 cat_size = EC_READ_U16(cat_header + 1); |
991 if (cat_header + cat_size + 2 > |
989 if (cat_header + cat_size + 2 > request.words + request.nwords) { |
992 (const uint16_t *) request.data + request.word_size) { |
|
993 EC_ERR("SII data corrupted! Dropping.\n"); |
990 EC_ERR("SII data corrupted! Dropping.\n"); |
994 return -EINVAL; |
991 return -EINVAL; |
995 } |
992 } |
996 cat_header += cat_size + 2; |
993 cat_header += cat_size + 2; |
997 cat_type = EC_READ_U16(cat_header); |
994 cat_type = EC_READ_U16(cat_header); |
1050 EC_WRITE_U16(sii_data + 14, crc); |
1047 EC_WRITE_U16(sii_data + 14, crc); |
1051 |
1048 |
1052 // init SII write request |
1049 // init SII write request |
1053 INIT_LIST_HEAD(&request.list); |
1050 INIT_LIST_HEAD(&request.list); |
1054 request.slave = slave; |
1051 request.slave = slave; |
1055 request.data = sii_data; |
1052 request.words = (const uint16_t *) sii_data; |
1056 request.word_offset = 0x0000; |
1053 request.offset = 0x0000; |
1057 request.word_size = 8; |
1054 request.nwords = 8; |
1058 |
1055 |
1059 if ((ret = ec_slave_schedule_sii_writing(&request))) |
1056 if ((ret = ec_slave_schedule_sii_writing(&request))) |
1060 return ret; // error code |
1057 return ret; // error code |
1061 |
1058 |
1062 slave->sii.alias = alias; // FIXME: do this in state machine |
1059 slave->sii.alias = alias; // FIXME: do this in state machine |