mxsimage.c 50 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347
  1. /*
  2. * Freescale i.MX23/i.MX28 SB image generator
  3. *
  4. * Copyright (C) 2012-2013 Marek Vasut <marex@denx.de>
  5. *
  6. * SPDX-License-Identifier: GPL-2.0+
  7. */
  8. #ifdef CONFIG_MXS
  9. #include <errno.h>
  10. #include <fcntl.h>
  11. #include <stdio.h>
  12. #include <string.h>
  13. #include <unistd.h>
  14. #include <limits.h>
  15. #include <openssl/evp.h>
  16. #include "mkimage.h"
  17. #include "mxsimage.h"
  18. #include <image.h>
  19. /*
  20. * DCD block
  21. * |-Write to address command block
  22. * | 0xf00 == 0xf33d
  23. * | 0xba2 == 0xb33f
  24. * |-ORR address with mask command block
  25. * | 0xf00 |= 0x1337
  26. * |-Write to address command block
  27. * | 0xba2 == 0xd00d
  28. * :
  29. */
  30. #define SB_HAB_DCD_WRITE 0xccUL
  31. #define SB_HAB_DCD_CHECK 0xcfUL
  32. #define SB_HAB_DCD_NOOP 0xc0UL
  33. #define SB_HAB_DCD_MASK_BIT (1 << 3)
  34. #define SB_HAB_DCD_SET_BIT (1 << 4)
  35. /* Addr.n = Value.n */
  36. #define SB_DCD_WRITE \
  37. (SB_HAB_DCD_WRITE << 24)
  38. /* Addr.n &= ~Value.n */
  39. #define SB_DCD_ANDC \
  40. ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT)
  41. /* Addr.n |= Value.n */
  42. #define SB_DCD_ORR \
  43. ((SB_HAB_DCD_WRITE << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  44. /* (Addr.n & Value.n) == 0 */
  45. #define SB_DCD_CHK_EQZ \
  46. (SB_HAB_DCD_CHECK << 24)
  47. /* (Addr.n & Value.n) == Value.n */
  48. #define SB_DCD_CHK_EQ \
  49. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT)
  50. /* (Addr.n & Value.n) != Value.n */
  51. #define SB_DCD_CHK_NEQ \
  52. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_MASK_BIT)
  53. /* (Addr.n & Value.n) != 0 */
  54. #define SB_DCD_CHK_NEZ \
  55. ((SB_HAB_DCD_CHECK << 24) | SB_HAB_DCD_SET_BIT | SB_HAB_DCD_MASK_BIT)
  56. /* NOP */
  57. #define SB_DCD_NOOP \
  58. (SB_HAB_DCD_NOOP << 24)
  59. struct sb_dcd_ctx {
  60. struct sb_dcd_ctx *dcd;
  61. uint32_t id;
  62. /* The DCD block. */
  63. uint32_t *payload;
  64. /* Size of the whole DCD block. */
  65. uint32_t size;
  66. /* Pointer to previous DCD command block. */
  67. uint32_t *prev_dcd_head;
  68. };
  69. /*
  70. * IMAGE
  71. * |-SECTION
  72. * | |-CMD
  73. * | |-CMD
  74. * | `-CMD
  75. * |-SECTION
  76. * | |-CMD
  77. * : :
  78. */
  79. struct sb_cmd_list {
  80. char *cmd;
  81. size_t len;
  82. unsigned int lineno;
  83. };
  84. struct sb_cmd_ctx {
  85. uint32_t size;
  86. struct sb_cmd_ctx *cmd;
  87. uint8_t *data;
  88. uint32_t length;
  89. struct sb_command payload;
  90. struct sb_command c_payload;
  91. };
  92. struct sb_section_ctx {
  93. uint32_t size;
  94. /* Section flags */
  95. unsigned int boot:1;
  96. struct sb_section_ctx *sect;
  97. struct sb_cmd_ctx *cmd_head;
  98. struct sb_cmd_ctx *cmd_tail;
  99. struct sb_sections_header payload;
  100. };
  101. struct sb_image_ctx {
  102. unsigned int in_section:1;
  103. unsigned int in_dcd:1;
  104. /* Image configuration */
  105. unsigned int verbose_boot:1;
  106. unsigned int silent_dump:1;
  107. char *input_filename;
  108. char *output_filename;
  109. char *cfg_filename;
  110. uint8_t image_key[16];
  111. /* Number of section in the image */
  112. unsigned int sect_count;
  113. /* Bootable section */
  114. unsigned int sect_boot;
  115. unsigned int sect_boot_found:1;
  116. struct sb_section_ctx *sect_head;
  117. struct sb_section_ctx *sect_tail;
  118. struct sb_dcd_ctx *dcd_head;
  119. struct sb_dcd_ctx *dcd_tail;
  120. EVP_CIPHER_CTX cipher_ctx;
  121. EVP_MD_CTX md_ctx;
  122. uint8_t digest[32];
  123. struct sb_key_dictionary_key sb_dict_key;
  124. struct sb_boot_image_header payload;
  125. };
  126. /*
  127. * Instruction semantics:
  128. * NOOP
  129. * TAG [LAST]
  130. * LOAD address file
  131. * LOAD IVT address IVT_entry_point
  132. * FILL address pattern length
  133. * JUMP [HAB] address [r0_arg]
  134. * CALL [HAB] address [r0_arg]
  135. * MODE mode
  136. * For i.MX23, mode = USB/I2C/SPI1_FLASH/SPI2_FLASH/NAND_BCH
  137. * JTAG/SPI3_EEPROM/SD_SSP0/SD_SSP1
  138. * For i.MX28, mode = USB/I2C/SPI2_FLASH/SPI3_FLASH/NAND_BCH
  139. * JTAG/SPI2_EEPROM/SD_SSP0/SD_SSP1
  140. */
  141. /*
  142. * AES libcrypto
  143. */
  144. static int sb_aes_init(struct sb_image_ctx *ictx, uint8_t *iv, int enc)
  145. {
  146. EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
  147. int ret;
  148. /* If there is no init vector, init vector is all zeroes. */
  149. if (!iv)
  150. iv = ictx->image_key;
  151. EVP_CIPHER_CTX_init(ctx);
  152. ret = EVP_CipherInit(ctx, EVP_aes_128_cbc(), ictx->image_key, iv, enc);
  153. if (ret == 1)
  154. EVP_CIPHER_CTX_set_padding(ctx, 0);
  155. return ret;
  156. }
  157. static int sb_aes_crypt(struct sb_image_ctx *ictx, uint8_t *in_data,
  158. uint8_t *out_data, int in_len)
  159. {
  160. EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
  161. int ret, outlen;
  162. uint8_t *outbuf;
  163. outbuf = malloc(in_len);
  164. if (!outbuf)
  165. return -ENOMEM;
  166. memset(outbuf, 0, sizeof(in_len));
  167. ret = EVP_CipherUpdate(ctx, outbuf, &outlen, in_data, in_len);
  168. if (!ret) {
  169. ret = -EINVAL;
  170. goto err;
  171. }
  172. if (out_data)
  173. memcpy(out_data, outbuf, outlen);
  174. err:
  175. free(outbuf);
  176. return ret;
  177. }
  178. static int sb_aes_deinit(EVP_CIPHER_CTX *ctx)
  179. {
  180. return EVP_CIPHER_CTX_cleanup(ctx);
  181. }
  182. static int sb_aes_reinit(struct sb_image_ctx *ictx, int enc)
  183. {
  184. int ret;
  185. EVP_CIPHER_CTX *ctx = &ictx->cipher_ctx;
  186. struct sb_boot_image_header *sb_header = &ictx->payload;
  187. uint8_t *iv = sb_header->iv;
  188. ret = sb_aes_deinit(ctx);
  189. if (!ret)
  190. return ret;
  191. return sb_aes_init(ictx, iv, enc);
  192. }
  193. /*
  194. * CRC32
  195. */
  196. static uint32_t crc32(uint8_t *data, uint32_t len)
  197. {
  198. const uint32_t poly = 0x04c11db7;
  199. uint32_t crc32 = 0xffffffff;
  200. unsigned int byte, bit;
  201. for (byte = 0; byte < len; byte++) {
  202. crc32 ^= data[byte] << 24;
  203. for (bit = 8; bit > 0; bit--) {
  204. if (crc32 & (1UL << 31))
  205. crc32 = (crc32 << 1) ^ poly;
  206. else
  207. crc32 = (crc32 << 1);
  208. }
  209. }
  210. return crc32;
  211. }
  212. /*
  213. * Debug
  214. */
  215. static void soprintf(struct sb_image_ctx *ictx, const char *fmt, ...)
  216. {
  217. va_list ap;
  218. if (ictx->silent_dump)
  219. return;
  220. va_start(ap, fmt);
  221. vfprintf(stdout, fmt, ap);
  222. va_end(ap);
  223. }
  224. /*
  225. * Code
  226. */
  227. static time_t sb_get_timestamp(void)
  228. {
  229. struct tm time_2000 = {
  230. .tm_yday = 1, /* Jan. 1st */
  231. .tm_year = 100, /* 2000 */
  232. };
  233. time_t seconds_to_2000 = mktime(&time_2000);
  234. time_t seconds_to_now = time(NULL);
  235. return seconds_to_now - seconds_to_2000;
  236. }
  237. static int sb_get_time(time_t time, struct tm *tm)
  238. {
  239. struct tm time_2000 = {
  240. .tm_yday = 1, /* Jan. 1st */
  241. .tm_year = 0, /* 1900 */
  242. };
  243. const time_t seconds_to_2000 = mktime(&time_2000);
  244. const time_t seconds_to_now = seconds_to_2000 + time;
  245. struct tm *ret;
  246. ret = gmtime_r(&seconds_to_now, tm);
  247. return ret ? 0 : -EINVAL;
  248. }
  249. static void sb_encrypt_sb_header(struct sb_image_ctx *ictx)
  250. {
  251. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  252. struct sb_boot_image_header *sb_header = &ictx->payload;
  253. uint8_t *sb_header_ptr = (uint8_t *)sb_header;
  254. /* Encrypt the header, compute the digest. */
  255. sb_aes_crypt(ictx, sb_header_ptr, NULL, sizeof(*sb_header));
  256. EVP_DigestUpdate(md_ctx, sb_header_ptr, sizeof(*sb_header));
  257. }
  258. static void sb_encrypt_sb_sections_header(struct sb_image_ctx *ictx)
  259. {
  260. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  261. struct sb_section_ctx *sctx = ictx->sect_head;
  262. struct sb_sections_header *shdr;
  263. uint8_t *sb_sections_header_ptr;
  264. const int size = sizeof(*shdr);
  265. while (sctx) {
  266. shdr = &sctx->payload;
  267. sb_sections_header_ptr = (uint8_t *)shdr;
  268. sb_aes_crypt(ictx, sb_sections_header_ptr,
  269. ictx->sb_dict_key.cbc_mac, size);
  270. EVP_DigestUpdate(md_ctx, sb_sections_header_ptr, size);
  271. sctx = sctx->sect;
  272. };
  273. }
  274. static void sb_encrypt_key_dictionary_key(struct sb_image_ctx *ictx)
  275. {
  276. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  277. sb_aes_crypt(ictx, ictx->image_key, ictx->sb_dict_key.key,
  278. sizeof(ictx->sb_dict_key.key));
  279. EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
  280. }
  281. static void sb_decrypt_key_dictionary_key(struct sb_image_ctx *ictx)
  282. {
  283. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  284. EVP_DigestUpdate(md_ctx, &ictx->sb_dict_key, sizeof(ictx->sb_dict_key));
  285. sb_aes_crypt(ictx, ictx->sb_dict_key.key, ictx->image_key,
  286. sizeof(ictx->sb_dict_key.key));
  287. }
  288. static void sb_encrypt_tag(struct sb_image_ctx *ictx,
  289. struct sb_cmd_ctx *cctx)
  290. {
  291. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  292. struct sb_command *cmd = &cctx->payload;
  293. sb_aes_crypt(ictx, (uint8_t *)cmd,
  294. (uint8_t *)&cctx->c_payload, sizeof(*cmd));
  295. EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
  296. }
  297. static int sb_encrypt_image(struct sb_image_ctx *ictx)
  298. {
  299. /* Start image-wide crypto. */
  300. EVP_MD_CTX_init(&ictx->md_ctx);
  301. EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
  302. /*
  303. * SB image header.
  304. */
  305. sb_aes_init(ictx, NULL, 1);
  306. sb_encrypt_sb_header(ictx);
  307. /*
  308. * SB sections header.
  309. */
  310. sb_encrypt_sb_sections_header(ictx);
  311. /*
  312. * Key dictionary.
  313. */
  314. sb_aes_reinit(ictx, 1);
  315. sb_encrypt_key_dictionary_key(ictx);
  316. /*
  317. * Section tags.
  318. */
  319. struct sb_cmd_ctx *cctx;
  320. struct sb_command *ccmd;
  321. struct sb_section_ctx *sctx = ictx->sect_head;
  322. while (sctx) {
  323. cctx = sctx->cmd_head;
  324. sb_aes_reinit(ictx, 1);
  325. while (cctx) {
  326. ccmd = &cctx->payload;
  327. sb_encrypt_tag(ictx, cctx);
  328. if (ccmd->header.tag == ROM_TAG_CMD) {
  329. sb_aes_reinit(ictx, 1);
  330. } else if (ccmd->header.tag == ROM_LOAD_CMD) {
  331. sb_aes_crypt(ictx, cctx->data, cctx->data,
  332. cctx->length);
  333. EVP_DigestUpdate(&ictx->md_ctx, cctx->data,
  334. cctx->length);
  335. }
  336. cctx = cctx->cmd;
  337. }
  338. sctx = sctx->sect;
  339. };
  340. /*
  341. * Dump the SHA1 of the whole image.
  342. */
  343. sb_aes_reinit(ictx, 1);
  344. EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
  345. sb_aes_crypt(ictx, ictx->digest, ictx->digest, sizeof(ictx->digest));
  346. /* Stop the encryption session. */
  347. sb_aes_deinit(&ictx->cipher_ctx);
  348. return 0;
  349. }
  350. static int sb_load_file(struct sb_cmd_ctx *cctx, char *filename)
  351. {
  352. long real_size, roundup_size;
  353. uint8_t *data;
  354. long ret;
  355. unsigned long size;
  356. FILE *fp;
  357. if (!filename) {
  358. fprintf(stderr, "ERR: Missing filename!\n");
  359. return -EINVAL;
  360. }
  361. fp = fopen(filename, "r");
  362. if (!fp)
  363. goto err_open;
  364. ret = fseek(fp, 0, SEEK_END);
  365. if (ret < 0)
  366. goto err_file;
  367. real_size = ftell(fp);
  368. if (real_size < 0)
  369. goto err_file;
  370. ret = fseek(fp, 0, SEEK_SET);
  371. if (ret < 0)
  372. goto err_file;
  373. roundup_size = roundup(real_size, SB_BLOCK_SIZE);
  374. data = calloc(1, roundup_size);
  375. if (!data)
  376. goto err_file;
  377. size = fread(data, 1, real_size, fp);
  378. if (size != (unsigned long)real_size)
  379. goto err_alloc;
  380. cctx->data = data;
  381. cctx->length = roundup_size;
  382. fclose(fp);
  383. return 0;
  384. err_alloc:
  385. free(data);
  386. err_file:
  387. fclose(fp);
  388. err_open:
  389. fprintf(stderr, "ERR: Failed to load file \"%s\"\n", filename);
  390. return -EINVAL;
  391. }
  392. static uint8_t sb_command_checksum(struct sb_command *inst)
  393. {
  394. uint8_t *inst_ptr = (uint8_t *)inst;
  395. uint8_t csum = 0;
  396. unsigned int i;
  397. for (i = 0; i < sizeof(struct sb_command); i++)
  398. csum += inst_ptr[i];
  399. return csum;
  400. }
  401. static int sb_token_to_long(char *tok, uint32_t *rid)
  402. {
  403. char *endptr;
  404. unsigned long id;
  405. if (tok[0] != '0' || tok[1] != 'x') {
  406. fprintf(stderr, "ERR: Invalid hexadecimal number!\n");
  407. return -EINVAL;
  408. }
  409. tok += 2;
  410. id = strtoul(tok, &endptr, 16);
  411. if ((errno == ERANGE && id == ULONG_MAX) || (errno != 0 && id == 0)) {
  412. fprintf(stderr, "ERR: Value can't be decoded!\n");
  413. return -EINVAL;
  414. }
  415. /* Check for 32-bit overflow. */
  416. if (id > 0xffffffff) {
  417. fprintf(stderr, "ERR: Value too big!\n");
  418. return -EINVAL;
  419. }
  420. if (endptr == tok) {
  421. fprintf(stderr, "ERR: Deformed value!\n");
  422. return -EINVAL;
  423. }
  424. *rid = (uint32_t)id;
  425. return 0;
  426. }
  427. static int sb_grow_dcd(struct sb_dcd_ctx *dctx, unsigned int inc_size)
  428. {
  429. uint32_t *tmp;
  430. if (!inc_size)
  431. return 0;
  432. dctx->size += inc_size;
  433. tmp = realloc(dctx->payload, dctx->size);
  434. if (!tmp)
  435. return -ENOMEM;
  436. dctx->payload = tmp;
  437. /* Assemble and update the HAB DCD header. */
  438. dctx->payload[0] = htonl((SB_HAB_DCD_TAG << 24) |
  439. (dctx->size << 8) |
  440. SB_HAB_VERSION);
  441. return 0;
  442. }
  443. static int sb_build_dcd(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  444. {
  445. struct sb_dcd_ctx *dctx;
  446. char *tok;
  447. uint32_t id;
  448. int ret;
  449. dctx = calloc(1, sizeof(*dctx));
  450. if (!dctx)
  451. return -ENOMEM;
  452. ret = sb_grow_dcd(dctx, 4);
  453. if (ret)
  454. goto err_dcd;
  455. /* Read DCD block number. */
  456. tok = strtok(cmd->cmd, " ");
  457. if (!tok) {
  458. fprintf(stderr, "#%i ERR: DCD block without number!\n",
  459. cmd->lineno);
  460. ret = -EINVAL;
  461. goto err_dcd;
  462. }
  463. /* Parse the DCD block number. */
  464. ret = sb_token_to_long(tok, &id);
  465. if (ret) {
  466. fprintf(stderr, "#%i ERR: Malformed DCD block number!\n",
  467. cmd->lineno);
  468. goto err_dcd;
  469. }
  470. dctx->id = id;
  471. /*
  472. * The DCD block is now constructed. Append it to the list.
  473. * WARNING: The DCD size is still not computed and will be
  474. * updated while parsing it's commands.
  475. */
  476. if (!ictx->dcd_head) {
  477. ictx->dcd_head = dctx;
  478. ictx->dcd_tail = dctx;
  479. } else {
  480. ictx->dcd_tail->dcd = dctx;
  481. ictx->dcd_tail = dctx;
  482. }
  483. return 0;
  484. err_dcd:
  485. free(dctx->payload);
  486. free(dctx);
  487. return ret;
  488. }
  489. static int sb_build_dcd_block(struct sb_image_ctx *ictx,
  490. struct sb_cmd_list *cmd,
  491. uint32_t type)
  492. {
  493. char *tok;
  494. uint32_t address, value, length;
  495. int ret;
  496. struct sb_dcd_ctx *dctx = ictx->dcd_tail;
  497. uint32_t *dcd;
  498. if (dctx->prev_dcd_head && (type != SB_DCD_NOOP) &&
  499. ((dctx->prev_dcd_head[0] & 0xff0000ff) == type)) {
  500. /* Same instruction as before, just append it. */
  501. ret = sb_grow_dcd(dctx, 8);
  502. if (ret)
  503. return ret;
  504. } else if (type == SB_DCD_NOOP) {
  505. ret = sb_grow_dcd(dctx, 4);
  506. if (ret)
  507. return ret;
  508. /* Update DCD command block pointer. */
  509. dctx->prev_dcd_head = dctx->payload +
  510. dctx->size / sizeof(*dctx->payload) - 1;
  511. /* NOOP has only 4 bytes and no payload. */
  512. goto noop;
  513. } else {
  514. /*
  515. * Either a different instruction block started now
  516. * or this is the first instruction block.
  517. */
  518. ret = sb_grow_dcd(dctx, 12);
  519. if (ret)
  520. return ret;
  521. /* Update DCD command block pointer. */
  522. dctx->prev_dcd_head = dctx->payload +
  523. dctx->size / sizeof(*dctx->payload) - 3;
  524. }
  525. dcd = dctx->payload + dctx->size / sizeof(*dctx->payload) - 2;
  526. /*
  527. * Prepare the command.
  528. */
  529. tok = strtok(cmd->cmd, " ");
  530. if (!tok) {
  531. fprintf(stderr, "#%i ERR: Missing DCD address!\n",
  532. cmd->lineno);
  533. ret = -EINVAL;
  534. goto err;
  535. }
  536. /* Read DCD destination address. */
  537. ret = sb_token_to_long(tok, &address);
  538. if (ret) {
  539. fprintf(stderr, "#%i ERR: Incorrect DCD address!\n",
  540. cmd->lineno);
  541. goto err;
  542. }
  543. tok = strtok(NULL, " ");
  544. if (!tok) {
  545. fprintf(stderr, "#%i ERR: Missing DCD value!\n",
  546. cmd->lineno);
  547. ret = -EINVAL;
  548. goto err;
  549. }
  550. /* Read DCD operation value. */
  551. ret = sb_token_to_long(tok, &value);
  552. if (ret) {
  553. fprintf(stderr, "#%i ERR: Incorrect DCD value!\n",
  554. cmd->lineno);
  555. goto err;
  556. }
  557. /* Fill in the new DCD entry. */
  558. dcd[0] = htonl(address);
  559. dcd[1] = htonl(value);
  560. noop:
  561. /* Update the DCD command block. */
  562. length = dctx->size -
  563. ((dctx->prev_dcd_head - dctx->payload) *
  564. sizeof(*dctx->payload));
  565. dctx->prev_dcd_head[0] = htonl(type | (length << 8));
  566. err:
  567. return ret;
  568. }
  569. static int sb_build_section(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  570. {
  571. struct sb_section_ctx *sctx;
  572. struct sb_sections_header *shdr;
  573. char *tok;
  574. uint32_t bootable = 0;
  575. uint32_t id;
  576. int ret;
  577. sctx = calloc(1, sizeof(*sctx));
  578. if (!sctx)
  579. return -ENOMEM;
  580. /* Read section number. */
  581. tok = strtok(cmd->cmd, " ");
  582. if (!tok) {
  583. fprintf(stderr, "#%i ERR: Section without number!\n",
  584. cmd->lineno);
  585. ret = -EINVAL;
  586. goto err_sect;
  587. }
  588. /* Parse the section number. */
  589. ret = sb_token_to_long(tok, &id);
  590. if (ret) {
  591. fprintf(stderr, "#%i ERR: Malformed section number!\n",
  592. cmd->lineno);
  593. goto err_sect;
  594. }
  595. /* Read section's BOOTABLE flag. */
  596. tok = strtok(NULL, " ");
  597. if (tok && (strlen(tok) == 8) && !strncmp(tok, "BOOTABLE", 8))
  598. bootable = SB_SECTION_FLAG_BOOTABLE;
  599. sctx->boot = bootable;
  600. shdr = &sctx->payload;
  601. shdr->section_number = id;
  602. shdr->section_flags = bootable;
  603. /*
  604. * The section is now constructed. Append it to the list.
  605. * WARNING: The section size is still not computed and will
  606. * be updated while parsing it's commands.
  607. */
  608. ictx->sect_count++;
  609. /* Mark that this section is bootable one. */
  610. if (bootable) {
  611. if (ictx->sect_boot_found) {
  612. fprintf(stderr,
  613. "#%i WARN: Multiple bootable section!\n",
  614. cmd->lineno);
  615. } else {
  616. ictx->sect_boot = id;
  617. ictx->sect_boot_found = 1;
  618. }
  619. }
  620. if (!ictx->sect_head) {
  621. ictx->sect_head = sctx;
  622. ictx->sect_tail = sctx;
  623. } else {
  624. ictx->sect_tail->sect = sctx;
  625. ictx->sect_tail = sctx;
  626. }
  627. return 0;
  628. err_sect:
  629. free(sctx);
  630. return ret;
  631. }
  632. static int sb_build_command_nop(struct sb_image_ctx *ictx)
  633. {
  634. struct sb_section_ctx *sctx = ictx->sect_tail;
  635. struct sb_cmd_ctx *cctx;
  636. struct sb_command *ccmd;
  637. cctx = calloc(1, sizeof(*cctx));
  638. if (!cctx)
  639. return -ENOMEM;
  640. ccmd = &cctx->payload;
  641. /*
  642. * Construct the command.
  643. */
  644. ccmd->header.checksum = 0x5a;
  645. ccmd->header.tag = ROM_NOP_CMD;
  646. cctx->size = sizeof(*ccmd);
  647. /*
  648. * Append the command to the last section.
  649. */
  650. if (!sctx->cmd_head) {
  651. sctx->cmd_head = cctx;
  652. sctx->cmd_tail = cctx;
  653. } else {
  654. sctx->cmd_tail->cmd = cctx;
  655. sctx->cmd_tail = cctx;
  656. }
  657. return 0;
  658. }
  659. static int sb_build_command_tag(struct sb_image_ctx *ictx,
  660. struct sb_cmd_list *cmd)
  661. {
  662. struct sb_section_ctx *sctx = ictx->sect_tail;
  663. struct sb_cmd_ctx *cctx;
  664. struct sb_command *ccmd;
  665. char *tok;
  666. cctx = calloc(1, sizeof(*cctx));
  667. if (!cctx)
  668. return -ENOMEM;
  669. ccmd = &cctx->payload;
  670. /*
  671. * Prepare the command.
  672. */
  673. /* Check for the LAST keyword. */
  674. tok = strtok(cmd->cmd, " ");
  675. if (tok && !strcmp(tok, "LAST"))
  676. ccmd->header.flags = ROM_TAG_CMD_FLAG_ROM_LAST_TAG;
  677. /*
  678. * Construct the command.
  679. */
  680. ccmd->header.checksum = 0x5a;
  681. ccmd->header.tag = ROM_TAG_CMD;
  682. cctx->size = sizeof(*ccmd);
  683. /*
  684. * Append the command to the last section.
  685. */
  686. if (!sctx->cmd_head) {
  687. sctx->cmd_head = cctx;
  688. sctx->cmd_tail = cctx;
  689. } else {
  690. sctx->cmd_tail->cmd = cctx;
  691. sctx->cmd_tail = cctx;
  692. }
  693. return 0;
  694. }
  695. static int sb_build_command_load(struct sb_image_ctx *ictx,
  696. struct sb_cmd_list *cmd)
  697. {
  698. struct sb_section_ctx *sctx = ictx->sect_tail;
  699. struct sb_cmd_ctx *cctx;
  700. struct sb_command *ccmd;
  701. char *tok;
  702. int ret, is_ivt = 0, is_dcd = 0;
  703. uint32_t dest, dcd = 0;
  704. cctx = calloc(1, sizeof(*cctx));
  705. if (!cctx)
  706. return -ENOMEM;
  707. ccmd = &cctx->payload;
  708. /*
  709. * Prepare the command.
  710. */
  711. tok = strtok(cmd->cmd, " ");
  712. if (!tok) {
  713. fprintf(stderr, "#%i ERR: Missing LOAD address or 'IVT'!\n",
  714. cmd->lineno);
  715. ret = -EINVAL;
  716. goto err;
  717. }
  718. /* Check for "IVT" flag. */
  719. if (!strcmp(tok, "IVT"))
  720. is_ivt = 1;
  721. if (!strcmp(tok, "DCD"))
  722. is_dcd = 1;
  723. if (is_ivt || is_dcd) {
  724. tok = strtok(NULL, " ");
  725. if (!tok) {
  726. fprintf(stderr, "#%i ERR: Missing LOAD address!\n",
  727. cmd->lineno);
  728. ret = -EINVAL;
  729. goto err;
  730. }
  731. }
  732. /* Read load destination address. */
  733. ret = sb_token_to_long(tok, &dest);
  734. if (ret) {
  735. fprintf(stderr, "#%i ERR: Incorrect LOAD address!\n",
  736. cmd->lineno);
  737. goto err;
  738. }
  739. /* Read filename or IVT entrypoint or DCD block ID. */
  740. tok = strtok(NULL, " ");
  741. if (!tok) {
  742. fprintf(stderr,
  743. "#%i ERR: Missing LOAD filename or IVT ep or DCD block ID!\n",
  744. cmd->lineno);
  745. ret = -EINVAL;
  746. goto err;
  747. }
  748. if (is_ivt) {
  749. /* Handle IVT. */
  750. struct sb_ivt_header *ivt;
  751. uint32_t ivtep;
  752. ret = sb_token_to_long(tok, &ivtep);
  753. if (ret) {
  754. fprintf(stderr,
  755. "#%i ERR: Incorrect IVT entry point!\n",
  756. cmd->lineno);
  757. goto err;
  758. }
  759. ivt = calloc(1, sizeof(*ivt));
  760. if (!ivt) {
  761. ret = -ENOMEM;
  762. goto err;
  763. }
  764. ivt->header = sb_hab_ivt_header();
  765. ivt->entry = ivtep;
  766. ivt->self = dest;
  767. cctx->data = (uint8_t *)ivt;
  768. cctx->length = sizeof(*ivt);
  769. } else if (is_dcd) {
  770. struct sb_dcd_ctx *dctx = ictx->dcd_head;
  771. uint32_t dcdid;
  772. uint8_t *payload;
  773. uint32_t asize;
  774. ret = sb_token_to_long(tok, &dcdid);
  775. if (ret) {
  776. fprintf(stderr,
  777. "#%i ERR: Incorrect DCD block ID!\n",
  778. cmd->lineno);
  779. goto err;
  780. }
  781. while (dctx) {
  782. if (dctx->id == dcdid)
  783. break;
  784. dctx = dctx->dcd;
  785. }
  786. if (!dctx) {
  787. fprintf(stderr, "#%i ERR: DCD block %08x not found!\n",
  788. cmd->lineno, dcdid);
  789. goto err;
  790. }
  791. asize = roundup(dctx->size, SB_BLOCK_SIZE);
  792. payload = calloc(1, asize);
  793. if (!payload) {
  794. ret = -ENOMEM;
  795. goto err;
  796. }
  797. memcpy(payload, dctx->payload, dctx->size);
  798. cctx->data = payload;
  799. cctx->length = asize;
  800. /* Set the Load DCD flag. */
  801. dcd = ROM_LOAD_CMD_FLAG_DCD_LOAD;
  802. } else {
  803. /* Regular LOAD of a file. */
  804. ret = sb_load_file(cctx, tok);
  805. if (ret) {
  806. fprintf(stderr, "#%i ERR: Cannot load '%s'!\n",
  807. cmd->lineno, tok);
  808. goto err;
  809. }
  810. }
  811. if (cctx->length & (SB_BLOCK_SIZE - 1)) {
  812. fprintf(stderr, "#%i ERR: Unaligned payload!\n",
  813. cmd->lineno);
  814. }
  815. /*
  816. * Construct the command.
  817. */
  818. ccmd->header.checksum = 0x5a;
  819. ccmd->header.tag = ROM_LOAD_CMD;
  820. ccmd->header.flags = dcd;
  821. ccmd->load.address = dest;
  822. ccmd->load.count = cctx->length;
  823. ccmd->load.crc32 = crc32(cctx->data, cctx->length);
  824. cctx->size = sizeof(*ccmd) + cctx->length;
  825. /*
  826. * Append the command to the last section.
  827. */
  828. if (!sctx->cmd_head) {
  829. sctx->cmd_head = cctx;
  830. sctx->cmd_tail = cctx;
  831. } else {
  832. sctx->cmd_tail->cmd = cctx;
  833. sctx->cmd_tail = cctx;
  834. }
  835. return 0;
  836. err:
  837. free(cctx);
  838. return ret;
  839. }
  840. static int sb_build_command_fill(struct sb_image_ctx *ictx,
  841. struct sb_cmd_list *cmd)
  842. {
  843. struct sb_section_ctx *sctx = ictx->sect_tail;
  844. struct sb_cmd_ctx *cctx;
  845. struct sb_command *ccmd;
  846. char *tok;
  847. uint32_t address, pattern, length;
  848. int ret;
  849. cctx = calloc(1, sizeof(*cctx));
  850. if (!cctx)
  851. return -ENOMEM;
  852. ccmd = &cctx->payload;
  853. /*
  854. * Prepare the command.
  855. */
  856. tok = strtok(cmd->cmd, " ");
  857. if (!tok) {
  858. fprintf(stderr, "#%i ERR: Missing FILL address!\n",
  859. cmd->lineno);
  860. ret = -EINVAL;
  861. goto err;
  862. }
  863. /* Read fill destination address. */
  864. ret = sb_token_to_long(tok, &address);
  865. if (ret) {
  866. fprintf(stderr, "#%i ERR: Incorrect FILL address!\n",
  867. cmd->lineno);
  868. goto err;
  869. }
  870. tok = strtok(NULL, " ");
  871. if (!tok) {
  872. fprintf(stderr, "#%i ERR: Missing FILL pattern!\n",
  873. cmd->lineno);
  874. ret = -EINVAL;
  875. goto err;
  876. }
  877. /* Read fill pattern address. */
  878. ret = sb_token_to_long(tok, &pattern);
  879. if (ret) {
  880. fprintf(stderr, "#%i ERR: Incorrect FILL pattern!\n",
  881. cmd->lineno);
  882. goto err;
  883. }
  884. tok = strtok(NULL, " ");
  885. if (!tok) {
  886. fprintf(stderr, "#%i ERR: Missing FILL length!\n",
  887. cmd->lineno);
  888. ret = -EINVAL;
  889. goto err;
  890. }
  891. /* Read fill pattern address. */
  892. ret = sb_token_to_long(tok, &length);
  893. if (ret) {
  894. fprintf(stderr, "#%i ERR: Incorrect FILL length!\n",
  895. cmd->lineno);
  896. goto err;
  897. }
  898. /*
  899. * Construct the command.
  900. */
  901. ccmd->header.checksum = 0x5a;
  902. ccmd->header.tag = ROM_FILL_CMD;
  903. ccmd->fill.address = address;
  904. ccmd->fill.count = length;
  905. ccmd->fill.pattern = pattern;
  906. cctx->size = sizeof(*ccmd);
  907. /*
  908. * Append the command to the last section.
  909. */
  910. if (!sctx->cmd_head) {
  911. sctx->cmd_head = cctx;
  912. sctx->cmd_tail = cctx;
  913. } else {
  914. sctx->cmd_tail->cmd = cctx;
  915. sctx->cmd_tail = cctx;
  916. }
  917. return 0;
  918. err:
  919. free(cctx);
  920. return ret;
  921. }
  922. static int sb_build_command_jump_call(struct sb_image_ctx *ictx,
  923. struct sb_cmd_list *cmd,
  924. unsigned int is_call)
  925. {
  926. struct sb_section_ctx *sctx = ictx->sect_tail;
  927. struct sb_cmd_ctx *cctx;
  928. struct sb_command *ccmd;
  929. char *tok;
  930. uint32_t dest, arg = 0x0;
  931. uint32_t hab = 0;
  932. int ret;
  933. const char *cmdname = is_call ? "CALL" : "JUMP";
  934. cctx = calloc(1, sizeof(*cctx));
  935. if (!cctx)
  936. return -ENOMEM;
  937. ccmd = &cctx->payload;
  938. /*
  939. * Prepare the command.
  940. */
  941. tok = strtok(cmd->cmd, " ");
  942. if (!tok) {
  943. fprintf(stderr,
  944. "#%i ERR: Missing %s address or 'HAB'!\n",
  945. cmd->lineno, cmdname);
  946. ret = -EINVAL;
  947. goto err;
  948. }
  949. /* Check for "HAB" flag. */
  950. if (!strcmp(tok, "HAB")) {
  951. hab = is_call ? ROM_CALL_CMD_FLAG_HAB : ROM_JUMP_CMD_FLAG_HAB;
  952. tok = strtok(NULL, " ");
  953. if (!tok) {
  954. fprintf(stderr, "#%i ERR: Missing %s address!\n",
  955. cmd->lineno, cmdname);
  956. ret = -EINVAL;
  957. goto err;
  958. }
  959. }
  960. /* Read load destination address. */
  961. ret = sb_token_to_long(tok, &dest);
  962. if (ret) {
  963. fprintf(stderr, "#%i ERR: Incorrect %s address!\n",
  964. cmd->lineno, cmdname);
  965. goto err;
  966. }
  967. tok = strtok(NULL, " ");
  968. if (tok) {
  969. ret = sb_token_to_long(tok, &arg);
  970. if (ret) {
  971. fprintf(stderr,
  972. "#%i ERR: Incorrect %s argument!\n",
  973. cmd->lineno, cmdname);
  974. goto err;
  975. }
  976. }
  977. /*
  978. * Construct the command.
  979. */
  980. ccmd->header.checksum = 0x5a;
  981. ccmd->header.tag = is_call ? ROM_CALL_CMD : ROM_JUMP_CMD;
  982. ccmd->header.flags = hab;
  983. ccmd->call.address = dest;
  984. ccmd->call.argument = arg;
  985. cctx->size = sizeof(*ccmd);
  986. /*
  987. * Append the command to the last section.
  988. */
  989. if (!sctx->cmd_head) {
  990. sctx->cmd_head = cctx;
  991. sctx->cmd_tail = cctx;
  992. } else {
  993. sctx->cmd_tail->cmd = cctx;
  994. sctx->cmd_tail = cctx;
  995. }
  996. return 0;
  997. err:
  998. free(cctx);
  999. return ret;
  1000. }
  1001. static int sb_build_command_jump(struct sb_image_ctx *ictx,
  1002. struct sb_cmd_list *cmd)
  1003. {
  1004. return sb_build_command_jump_call(ictx, cmd, 0);
  1005. }
  1006. static int sb_build_command_call(struct sb_image_ctx *ictx,
  1007. struct sb_cmd_list *cmd)
  1008. {
  1009. return sb_build_command_jump_call(ictx, cmd, 1);
  1010. }
  1011. static int sb_build_command_mode(struct sb_image_ctx *ictx,
  1012. struct sb_cmd_list *cmd)
  1013. {
  1014. struct sb_section_ctx *sctx = ictx->sect_tail;
  1015. struct sb_cmd_ctx *cctx;
  1016. struct sb_command *ccmd;
  1017. char *tok;
  1018. int ret;
  1019. unsigned int i;
  1020. uint32_t mode = 0xffffffff;
  1021. cctx = calloc(1, sizeof(*cctx));
  1022. if (!cctx)
  1023. return -ENOMEM;
  1024. ccmd = &cctx->payload;
  1025. /*
  1026. * Prepare the command.
  1027. */
  1028. tok = strtok(cmd->cmd, " ");
  1029. if (!tok) {
  1030. fprintf(stderr, "#%i ERR: Missing MODE boot mode argument!\n",
  1031. cmd->lineno);
  1032. ret = -EINVAL;
  1033. goto err;
  1034. }
  1035. for (i = 0; i < ARRAY_SIZE(modetable); i++) {
  1036. if (!strcmp(tok, modetable[i].name)) {
  1037. mode = modetable[i].mode;
  1038. break;
  1039. }
  1040. if (!modetable[i].altname)
  1041. continue;
  1042. if (!strcmp(tok, modetable[i].altname)) {
  1043. mode = modetable[i].mode;
  1044. break;
  1045. }
  1046. }
  1047. if (mode == 0xffffffff) {
  1048. fprintf(stderr, "#%i ERR: Invalid MODE boot mode argument!\n",
  1049. cmd->lineno);
  1050. ret = -EINVAL;
  1051. goto err;
  1052. }
  1053. /*
  1054. * Construct the command.
  1055. */
  1056. ccmd->header.checksum = 0x5a;
  1057. ccmd->header.tag = ROM_MODE_CMD;
  1058. ccmd->mode.mode = mode;
  1059. cctx->size = sizeof(*ccmd);
  1060. /*
  1061. * Append the command to the last section.
  1062. */
  1063. if (!sctx->cmd_head) {
  1064. sctx->cmd_head = cctx;
  1065. sctx->cmd_tail = cctx;
  1066. } else {
  1067. sctx->cmd_tail->cmd = cctx;
  1068. sctx->cmd_tail = cctx;
  1069. }
  1070. return 0;
  1071. err:
  1072. free(cctx);
  1073. return ret;
  1074. }
  1075. static int sb_prefill_image_header(struct sb_image_ctx *ictx)
  1076. {
  1077. struct sb_boot_image_header *hdr = &ictx->payload;
  1078. /* Fill signatures */
  1079. memcpy(hdr->signature1, "STMP", 4);
  1080. memcpy(hdr->signature2, "sgtl", 4);
  1081. /* SB Image version 1.1 */
  1082. hdr->major_version = SB_VERSION_MAJOR;
  1083. hdr->minor_version = SB_VERSION_MINOR;
  1084. /* Boot image major version */
  1085. hdr->product_version.major = htons(0x999);
  1086. hdr->product_version.minor = htons(0x999);
  1087. hdr->product_version.revision = htons(0x999);
  1088. /* Boot image major version */
  1089. hdr->component_version.major = htons(0x999);
  1090. hdr->component_version.minor = htons(0x999);
  1091. hdr->component_version.revision = htons(0x999);
  1092. /* Drive tag must be 0x0 for i.MX23 */
  1093. hdr->drive_tag = 0;
  1094. hdr->header_blocks =
  1095. sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
  1096. hdr->section_header_size =
  1097. sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
  1098. hdr->timestamp_us = sb_get_timestamp() * 1000000;
  1099. /* FIXME -- add proper config option */
  1100. hdr->flags = ictx->verbose_boot ? SB_IMAGE_FLAG_VERBOSE : 0,
  1101. /* FIXME -- We support only default key */
  1102. hdr->key_count = 1;
  1103. return 0;
  1104. }
  1105. static int sb_postfill_image_header(struct sb_image_ctx *ictx)
  1106. {
  1107. struct sb_boot_image_header *hdr = &ictx->payload;
  1108. struct sb_section_ctx *sctx = ictx->sect_head;
  1109. uint32_t kd_size, sections_blocks;
  1110. EVP_MD_CTX md_ctx;
  1111. /* The main SB header size in blocks. */
  1112. hdr->image_blocks = hdr->header_blocks;
  1113. /* Size of the key dictionary, which has single zero entry. */
  1114. kd_size = hdr->key_count * sizeof(struct sb_key_dictionary_key);
  1115. hdr->image_blocks += kd_size / SB_BLOCK_SIZE;
  1116. /* Now count the payloads. */
  1117. hdr->section_count = ictx->sect_count;
  1118. while (sctx) {
  1119. hdr->image_blocks += sctx->size / SB_BLOCK_SIZE;
  1120. sctx = sctx->sect;
  1121. }
  1122. if (!ictx->sect_boot_found) {
  1123. fprintf(stderr, "ERR: No bootable section selected!\n");
  1124. return -EINVAL;
  1125. }
  1126. hdr->first_boot_section_id = ictx->sect_boot;
  1127. /* The n * SB section size in blocks. */
  1128. sections_blocks = hdr->section_count * hdr->section_header_size;
  1129. hdr->image_blocks += sections_blocks;
  1130. /* Key dictionary offset. */
  1131. hdr->key_dictionary_block = hdr->header_blocks + sections_blocks;
  1132. /* Digest of the whole image. */
  1133. hdr->image_blocks += 2;
  1134. /* Pointer past the dictionary. */
  1135. hdr->first_boot_tag_block =
  1136. hdr->key_dictionary_block + kd_size / SB_BLOCK_SIZE;
  1137. /* Compute header digest. */
  1138. EVP_MD_CTX_init(&md_ctx);
  1139. EVP_DigestInit(&md_ctx, EVP_sha1());
  1140. EVP_DigestUpdate(&md_ctx, hdr->signature1,
  1141. sizeof(struct sb_boot_image_header) -
  1142. sizeof(hdr->digest));
  1143. EVP_DigestFinal(&md_ctx, hdr->digest, NULL);
  1144. return 0;
  1145. }
  1146. static int sb_fixup_sections_and_tags(struct sb_image_ctx *ictx)
  1147. {
  1148. /* Fixup the placement of sections. */
  1149. struct sb_boot_image_header *ihdr = &ictx->payload;
  1150. struct sb_section_ctx *sctx = ictx->sect_head;
  1151. struct sb_sections_header *shdr;
  1152. struct sb_cmd_ctx *cctx;
  1153. struct sb_command *ccmd;
  1154. uint32_t offset = ihdr->first_boot_tag_block;
  1155. while (sctx) {
  1156. shdr = &sctx->payload;
  1157. /* Fill in the section TAG offset. */
  1158. shdr->section_offset = offset + 1;
  1159. offset += shdr->section_size;
  1160. /* Section length is measured from the TAG block. */
  1161. shdr->section_size--;
  1162. /* Fixup the TAG command. */
  1163. cctx = sctx->cmd_head;
  1164. while (cctx) {
  1165. ccmd = &cctx->payload;
  1166. if (ccmd->header.tag == ROM_TAG_CMD) {
  1167. ccmd->tag.section_number = shdr->section_number;
  1168. ccmd->tag.section_length = shdr->section_size;
  1169. ccmd->tag.section_flags = shdr->section_flags;
  1170. }
  1171. /* Update the command checksum. */
  1172. ccmd->header.checksum = sb_command_checksum(ccmd);
  1173. cctx = cctx->cmd;
  1174. }
  1175. sctx = sctx->sect;
  1176. }
  1177. return 0;
  1178. }
  1179. static int sb_parse_line(struct sb_image_ctx *ictx, struct sb_cmd_list *cmd)
  1180. {
  1181. char *tok;
  1182. char *line = cmd->cmd;
  1183. char *rptr;
  1184. int ret;
  1185. /* Analyze the identifier on this line first. */
  1186. tok = strtok_r(line, " ", &rptr);
  1187. if (!tok || (strlen(tok) == 0)) {
  1188. fprintf(stderr, "#%i ERR: Invalid line!\n", cmd->lineno);
  1189. return -EINVAL;
  1190. }
  1191. cmd->cmd = rptr;
  1192. /* DCD */
  1193. if (!strcmp(tok, "DCD")) {
  1194. ictx->in_section = 0;
  1195. ictx->in_dcd = 1;
  1196. sb_build_dcd(ictx, cmd);
  1197. return 0;
  1198. }
  1199. /* Section */
  1200. if (!strcmp(tok, "SECTION")) {
  1201. ictx->in_section = 1;
  1202. ictx->in_dcd = 0;
  1203. sb_build_section(ictx, cmd);
  1204. return 0;
  1205. }
  1206. if (!ictx->in_section && !ictx->in_dcd) {
  1207. fprintf(stderr, "#%i ERR: Data outside of a section!\n",
  1208. cmd->lineno);
  1209. return -EINVAL;
  1210. }
  1211. if (ictx->in_section) {
  1212. /* Section commands */
  1213. if (!strcmp(tok, "NOP")) {
  1214. ret = sb_build_command_nop(ictx);
  1215. } else if (!strcmp(tok, "TAG")) {
  1216. ret = sb_build_command_tag(ictx, cmd);
  1217. } else if (!strcmp(tok, "LOAD")) {
  1218. ret = sb_build_command_load(ictx, cmd);
  1219. } else if (!strcmp(tok, "FILL")) {
  1220. ret = sb_build_command_fill(ictx, cmd);
  1221. } else if (!strcmp(tok, "JUMP")) {
  1222. ret = sb_build_command_jump(ictx, cmd);
  1223. } else if (!strcmp(tok, "CALL")) {
  1224. ret = sb_build_command_call(ictx, cmd);
  1225. } else if (!strcmp(tok, "MODE")) {
  1226. ret = sb_build_command_mode(ictx, cmd);
  1227. } else {
  1228. fprintf(stderr,
  1229. "#%i ERR: Unsupported instruction '%s'!\n",
  1230. cmd->lineno, tok);
  1231. return -ENOTSUP;
  1232. }
  1233. } else if (ictx->in_dcd) {
  1234. char *lptr;
  1235. uint32_t ilen = '1';
  1236. tok = strtok_r(tok, ".", &lptr);
  1237. if (!tok || (strlen(tok) == 0) || (lptr && strlen(lptr) != 1)) {
  1238. fprintf(stderr, "#%i ERR: Invalid line!\n",
  1239. cmd->lineno);
  1240. return -EINVAL;
  1241. }
  1242. if (lptr &&
  1243. (lptr[0] != '1' && lptr[0] != '2' && lptr[0] != '4')) {
  1244. fprintf(stderr, "#%i ERR: Invalid instruction width!\n",
  1245. cmd->lineno);
  1246. return -EINVAL;
  1247. }
  1248. if (lptr)
  1249. ilen = lptr[0] - '1';
  1250. /* DCD commands */
  1251. if (!strcmp(tok, "WRITE")) {
  1252. ret = sb_build_dcd_block(ictx, cmd,
  1253. SB_DCD_WRITE | ilen);
  1254. } else if (!strcmp(tok, "ANDC")) {
  1255. ret = sb_build_dcd_block(ictx, cmd,
  1256. SB_DCD_ANDC | ilen);
  1257. } else if (!strcmp(tok, "ORR")) {
  1258. ret = sb_build_dcd_block(ictx, cmd,
  1259. SB_DCD_ORR | ilen);
  1260. } else if (!strcmp(tok, "EQZ")) {
  1261. ret = sb_build_dcd_block(ictx, cmd,
  1262. SB_DCD_CHK_EQZ | ilen);
  1263. } else if (!strcmp(tok, "EQ")) {
  1264. ret = sb_build_dcd_block(ictx, cmd,
  1265. SB_DCD_CHK_EQ | ilen);
  1266. } else if (!strcmp(tok, "NEQ")) {
  1267. ret = sb_build_dcd_block(ictx, cmd,
  1268. SB_DCD_CHK_NEQ | ilen);
  1269. } else if (!strcmp(tok, "NEZ")) {
  1270. ret = sb_build_dcd_block(ictx, cmd,
  1271. SB_DCD_CHK_NEZ | ilen);
  1272. } else if (!strcmp(tok, "NOOP")) {
  1273. ret = sb_build_dcd_block(ictx, cmd, SB_DCD_NOOP);
  1274. } else {
  1275. fprintf(stderr,
  1276. "#%i ERR: Unsupported instruction '%s'!\n",
  1277. cmd->lineno, tok);
  1278. return -ENOTSUP;
  1279. }
  1280. } else {
  1281. fprintf(stderr, "#%i ERR: Unsupported instruction '%s'!\n",
  1282. cmd->lineno, tok);
  1283. return -ENOTSUP;
  1284. }
  1285. /*
  1286. * Here we have at least one section with one command, otherwise we
  1287. * would have failed already higher above.
  1288. *
  1289. * FIXME -- should the updating happen here ?
  1290. */
  1291. if (ictx->in_section && !ret) {
  1292. ictx->sect_tail->size += ictx->sect_tail->cmd_tail->size;
  1293. ictx->sect_tail->payload.section_size =
  1294. ictx->sect_tail->size / SB_BLOCK_SIZE;
  1295. }
  1296. return ret;
  1297. }
  1298. static int sb_load_cmdfile(struct sb_image_ctx *ictx)
  1299. {
  1300. struct sb_cmd_list cmd;
  1301. int lineno = 1;
  1302. FILE *fp;
  1303. char *line = NULL;
  1304. ssize_t rlen;
  1305. size_t len;
  1306. fp = fopen(ictx->cfg_filename, "r");
  1307. if (!fp)
  1308. goto err_file;
  1309. while ((rlen = getline(&line, &len, fp)) > 0) {
  1310. memset(&cmd, 0, sizeof(cmd));
  1311. /* Strip the trailing newline. */
  1312. line[rlen - 1] = '\0';
  1313. cmd.cmd = line;
  1314. cmd.len = rlen;
  1315. cmd.lineno = lineno++;
  1316. sb_parse_line(ictx, &cmd);
  1317. }
  1318. free(line);
  1319. fclose(fp);
  1320. return 0;
  1321. err_file:
  1322. fclose(fp);
  1323. fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
  1324. ictx->cfg_filename);
  1325. return -EINVAL;
  1326. }
  1327. static int sb_build_tree_from_cfg(struct sb_image_ctx *ictx)
  1328. {
  1329. int ret;
  1330. ret = sb_load_cmdfile(ictx);
  1331. if (ret)
  1332. return ret;
  1333. ret = sb_prefill_image_header(ictx);
  1334. if (ret)
  1335. return ret;
  1336. ret = sb_postfill_image_header(ictx);
  1337. if (ret)
  1338. return ret;
  1339. ret = sb_fixup_sections_and_tags(ictx);
  1340. if (ret)
  1341. return ret;
  1342. return 0;
  1343. }
  1344. static int sb_verify_image_header(struct sb_image_ctx *ictx,
  1345. FILE *fp, long fsize)
  1346. {
  1347. /* Verify static fields in the image header. */
  1348. struct sb_boot_image_header *hdr = &ictx->payload;
  1349. const char *stat[2] = { "[PASS]", "[FAIL]" };
  1350. struct tm tm;
  1351. int sz, ret = 0;
  1352. unsigned char digest[20];
  1353. EVP_MD_CTX md_ctx;
  1354. unsigned long size;
  1355. /* Start image-wide crypto. */
  1356. EVP_MD_CTX_init(&ictx->md_ctx);
  1357. EVP_DigestInit(&ictx->md_ctx, EVP_sha1());
  1358. soprintf(ictx, "---------- Verifying SB Image Header ----------\n");
  1359. size = fread(&ictx->payload, 1, sizeof(ictx->payload), fp);
  1360. if (size != sizeof(ictx->payload)) {
  1361. fprintf(stderr, "ERR: SB image header too short!\n");
  1362. return -EINVAL;
  1363. }
  1364. /* Compute header digest. */
  1365. EVP_MD_CTX_init(&md_ctx);
  1366. EVP_DigestInit(&md_ctx, EVP_sha1());
  1367. EVP_DigestUpdate(&md_ctx, hdr->signature1,
  1368. sizeof(struct sb_boot_image_header) -
  1369. sizeof(hdr->digest));
  1370. EVP_DigestFinal(&md_ctx, digest, NULL);
  1371. sb_aes_init(ictx, NULL, 1);
  1372. sb_encrypt_sb_header(ictx);
  1373. if (memcmp(digest, hdr->digest, 20))
  1374. ret = -EINVAL;
  1375. soprintf(ictx, "%s Image header checksum: %s\n", stat[!!ret],
  1376. ret ? "BAD" : "OK");
  1377. if (ret)
  1378. return ret;
  1379. if (memcmp(hdr->signature1, "STMP", 4) ||
  1380. memcmp(hdr->signature2, "sgtl", 4))
  1381. ret = -EINVAL;
  1382. soprintf(ictx, "%s Signatures: '%.4s' '%.4s'\n",
  1383. stat[!!ret], hdr->signature1, hdr->signature2);
  1384. if (ret)
  1385. return ret;
  1386. if ((hdr->major_version != SB_VERSION_MAJOR) ||
  1387. ((hdr->minor_version != 1) && (hdr->minor_version != 2)))
  1388. ret = -EINVAL;
  1389. soprintf(ictx, "%s Image version: v%i.%i\n", stat[!!ret],
  1390. hdr->major_version, hdr->minor_version);
  1391. if (ret)
  1392. return ret;
  1393. ret = sb_get_time(hdr->timestamp_us / 1000000, &tm);
  1394. soprintf(ictx,
  1395. "%s Creation time: %02i:%02i:%02i %02i/%02i/%04i\n",
  1396. stat[!!ret], tm.tm_hour, tm.tm_min, tm.tm_sec,
  1397. tm.tm_mday, tm.tm_mon, tm.tm_year + 2000);
  1398. if (ret)
  1399. return ret;
  1400. soprintf(ictx, "%s Product version: %x.%x.%x\n", stat[0],
  1401. ntohs(hdr->product_version.major),
  1402. ntohs(hdr->product_version.minor),
  1403. ntohs(hdr->product_version.revision));
  1404. soprintf(ictx, "%s Component version: %x.%x.%x\n", stat[0],
  1405. ntohs(hdr->component_version.major),
  1406. ntohs(hdr->component_version.minor),
  1407. ntohs(hdr->component_version.revision));
  1408. if (hdr->flags & ~SB_IMAGE_FLAG_VERBOSE)
  1409. ret = -EINVAL;
  1410. soprintf(ictx, "%s Image flags: %s\n", stat[!!ret],
  1411. hdr->flags & SB_IMAGE_FLAG_VERBOSE ? "Verbose_boot" : "");
  1412. if (ret)
  1413. return ret;
  1414. if (hdr->drive_tag != 0)
  1415. ret = -EINVAL;
  1416. soprintf(ictx, "%s Drive tag: %i\n", stat[!!ret],
  1417. hdr->drive_tag);
  1418. if (ret)
  1419. return ret;
  1420. sz = sizeof(struct sb_boot_image_header) / SB_BLOCK_SIZE;
  1421. if (hdr->header_blocks != sz)
  1422. ret = -EINVAL;
  1423. soprintf(ictx, "%s Image header size (blocks): %i\n", stat[!!ret],
  1424. hdr->header_blocks);
  1425. if (ret)
  1426. return ret;
  1427. sz = sizeof(struct sb_sections_header) / SB_BLOCK_SIZE;
  1428. if (hdr->section_header_size != sz)
  1429. ret = -EINVAL;
  1430. soprintf(ictx, "%s Section header size (blocks): %i\n", stat[!!ret],
  1431. hdr->section_header_size);
  1432. if (ret)
  1433. return ret;
  1434. soprintf(ictx, "%s Sections count: %i\n", stat[!!ret],
  1435. hdr->section_count);
  1436. soprintf(ictx, "%s First bootable section %i\n", stat[!!ret],
  1437. hdr->first_boot_section_id);
  1438. if (hdr->image_blocks != fsize / SB_BLOCK_SIZE)
  1439. ret = -EINVAL;
  1440. soprintf(ictx, "%s Image size (blocks): %i\n", stat[!!ret],
  1441. hdr->image_blocks);
  1442. if (ret)
  1443. return ret;
  1444. sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
  1445. if (hdr->key_dictionary_block != sz)
  1446. ret = -EINVAL;
  1447. soprintf(ictx, "%s Key dict offset (blocks): %i\n", stat[!!ret],
  1448. hdr->key_dictionary_block);
  1449. if (ret)
  1450. return ret;
  1451. if (hdr->key_count != 1)
  1452. ret = -EINVAL;
  1453. soprintf(ictx, "%s Number of encryption keys: %i\n", stat[!!ret],
  1454. hdr->key_count);
  1455. if (ret)
  1456. return ret;
  1457. sz = hdr->header_blocks + hdr->section_header_size * hdr->section_count;
  1458. sz += hdr->key_count *
  1459. sizeof(struct sb_key_dictionary_key) / SB_BLOCK_SIZE;
  1460. if (hdr->first_boot_tag_block != (unsigned)sz)
  1461. ret = -EINVAL;
  1462. soprintf(ictx, "%s First TAG block (blocks): %i\n", stat[!!ret],
  1463. hdr->first_boot_tag_block);
  1464. if (ret)
  1465. return ret;
  1466. return 0;
  1467. }
  1468. static void sb_decrypt_tag(struct sb_image_ctx *ictx,
  1469. struct sb_cmd_ctx *cctx)
  1470. {
  1471. EVP_MD_CTX *md_ctx = &ictx->md_ctx;
  1472. struct sb_command *cmd = &cctx->payload;
  1473. sb_aes_crypt(ictx, (uint8_t *)&cctx->c_payload,
  1474. (uint8_t *)&cctx->payload, sizeof(*cmd));
  1475. EVP_DigestUpdate(md_ctx, &cctx->c_payload, sizeof(*cmd));
  1476. }
  1477. static int sb_verify_command(struct sb_image_ctx *ictx,
  1478. struct sb_cmd_ctx *cctx, FILE *fp,
  1479. unsigned long *tsize)
  1480. {
  1481. struct sb_command *ccmd = &cctx->payload;
  1482. unsigned long size, asize;
  1483. char *csum, *flag = "";
  1484. int ret;
  1485. unsigned int i;
  1486. uint8_t csn, csc = ccmd->header.checksum;
  1487. ccmd->header.checksum = 0x5a;
  1488. csn = sb_command_checksum(ccmd);
  1489. ccmd->header.checksum = csc;
  1490. if (csc == csn)
  1491. ret = 0;
  1492. else
  1493. ret = -EINVAL;
  1494. csum = ret ? "checksum BAD" : "checksum OK";
  1495. switch (ccmd->header.tag) {
  1496. case ROM_NOP_CMD:
  1497. soprintf(ictx, " NOOP # %s\n", csum);
  1498. return ret;
  1499. case ROM_TAG_CMD:
  1500. if (ccmd->header.flags & ROM_TAG_CMD_FLAG_ROM_LAST_TAG)
  1501. flag = "LAST";
  1502. soprintf(ictx, " TAG %s # %s\n", flag, csum);
  1503. sb_aes_reinit(ictx, 0);
  1504. return ret;
  1505. case ROM_LOAD_CMD:
  1506. soprintf(ictx, " LOAD addr=0x%08x length=0x%08x # %s\n",
  1507. ccmd->load.address, ccmd->load.count, csum);
  1508. cctx->length = ccmd->load.count;
  1509. asize = roundup(cctx->length, SB_BLOCK_SIZE);
  1510. cctx->data = malloc(asize);
  1511. if (!cctx->data)
  1512. return -ENOMEM;
  1513. size = fread(cctx->data, 1, asize, fp);
  1514. if (size != asize) {
  1515. fprintf(stderr,
  1516. "ERR: SB LOAD command payload too short!\n");
  1517. return -EINVAL;
  1518. }
  1519. *tsize += size;
  1520. EVP_DigestUpdate(&ictx->md_ctx, cctx->data, asize);
  1521. sb_aes_crypt(ictx, cctx->data, cctx->data, asize);
  1522. if (ccmd->load.crc32 != crc32(cctx->data, asize)) {
  1523. fprintf(stderr,
  1524. "ERR: SB LOAD command payload CRC32 invalid!\n");
  1525. return -EINVAL;
  1526. }
  1527. return 0;
  1528. case ROM_FILL_CMD:
  1529. soprintf(ictx,
  1530. " FILL addr=0x%08x length=0x%08x pattern=0x%08x # %s\n",
  1531. ccmd->fill.address, ccmd->fill.count,
  1532. ccmd->fill.pattern, csum);
  1533. return 0;
  1534. case ROM_JUMP_CMD:
  1535. if (ccmd->header.flags & ROM_JUMP_CMD_FLAG_HAB)
  1536. flag = " HAB";
  1537. soprintf(ictx,
  1538. " JUMP%s addr=0x%08x r0_arg=0x%08x # %s\n",
  1539. flag, ccmd->fill.address, ccmd->jump.argument, csum);
  1540. return 0;
  1541. case ROM_CALL_CMD:
  1542. if (ccmd->header.flags & ROM_CALL_CMD_FLAG_HAB)
  1543. flag = " HAB";
  1544. soprintf(ictx,
  1545. " CALL%s addr=0x%08x r0_arg=0x%08x # %s\n",
  1546. flag, ccmd->fill.address, ccmd->jump.argument, csum);
  1547. return 0;
  1548. case ROM_MODE_CMD:
  1549. for (i = 0; i < ARRAY_SIZE(modetable); i++) {
  1550. if (ccmd->mode.mode == modetable[i].mode) {
  1551. soprintf(ictx, " MODE %s # %s\n",
  1552. modetable[i].name, csum);
  1553. break;
  1554. }
  1555. }
  1556. fprintf(stderr, " MODE !INVALID! # %s\n", csum);
  1557. return 0;
  1558. }
  1559. return ret;
  1560. }
  1561. static int sb_verify_commands(struct sb_image_ctx *ictx,
  1562. struct sb_section_ctx *sctx, FILE *fp)
  1563. {
  1564. unsigned long size, tsize = 0;
  1565. struct sb_cmd_ctx *cctx;
  1566. int ret;
  1567. sb_aes_reinit(ictx, 0);
  1568. while (tsize < sctx->size) {
  1569. cctx = calloc(1, sizeof(*cctx));
  1570. if (!cctx)
  1571. return -ENOMEM;
  1572. if (!sctx->cmd_head) {
  1573. sctx->cmd_head = cctx;
  1574. sctx->cmd_tail = cctx;
  1575. } else {
  1576. sctx->cmd_tail->cmd = cctx;
  1577. sctx->cmd_tail = cctx;
  1578. }
  1579. size = fread(&cctx->c_payload, 1, sizeof(cctx->c_payload), fp);
  1580. if (size != sizeof(cctx->c_payload)) {
  1581. fprintf(stderr, "ERR: SB command header too short!\n");
  1582. return -EINVAL;
  1583. }
  1584. tsize += size;
  1585. sb_decrypt_tag(ictx, cctx);
  1586. ret = sb_verify_command(ictx, cctx, fp, &tsize);
  1587. if (ret)
  1588. return -EINVAL;
  1589. }
  1590. return 0;
  1591. }
  1592. static int sb_verify_sections_cmds(struct sb_image_ctx *ictx, FILE *fp)
  1593. {
  1594. struct sb_boot_image_header *hdr = &ictx->payload;
  1595. struct sb_sections_header *shdr;
  1596. unsigned int i;
  1597. int ret;
  1598. struct sb_section_ctx *sctx;
  1599. unsigned long size;
  1600. char *bootable = "";
  1601. soprintf(ictx, "----- Verifying SB Sections and Commands -----\n");
  1602. for (i = 0; i < hdr->section_count; i++) {
  1603. sctx = calloc(1, sizeof(*sctx));
  1604. if (!sctx)
  1605. return -ENOMEM;
  1606. if (!ictx->sect_head) {
  1607. ictx->sect_head = sctx;
  1608. ictx->sect_tail = sctx;
  1609. } else {
  1610. ictx->sect_tail->sect = sctx;
  1611. ictx->sect_tail = sctx;
  1612. }
  1613. size = fread(&sctx->payload, 1, sizeof(sctx->payload), fp);
  1614. if (size != sizeof(sctx->payload)) {
  1615. fprintf(stderr, "ERR: SB section header too short!\n");
  1616. return -EINVAL;
  1617. }
  1618. }
  1619. size = fread(&ictx->sb_dict_key, 1, sizeof(ictx->sb_dict_key), fp);
  1620. if (size != sizeof(ictx->sb_dict_key)) {
  1621. fprintf(stderr, "ERR: SB key dictionary too short!\n");
  1622. return -EINVAL;
  1623. }
  1624. sb_encrypt_sb_sections_header(ictx);
  1625. sb_aes_reinit(ictx, 0);
  1626. sb_decrypt_key_dictionary_key(ictx);
  1627. sb_aes_reinit(ictx, 0);
  1628. sctx = ictx->sect_head;
  1629. while (sctx) {
  1630. shdr = &sctx->payload;
  1631. if (shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) {
  1632. sctx->boot = 1;
  1633. bootable = " BOOTABLE";
  1634. }
  1635. sctx->size = (shdr->section_size * SB_BLOCK_SIZE) +
  1636. sizeof(struct sb_command);
  1637. soprintf(ictx, "SECTION 0x%x%s # size = %i bytes\n",
  1638. shdr->section_number, bootable, sctx->size);
  1639. if (shdr->section_flags & ~SB_SECTION_FLAG_BOOTABLE)
  1640. fprintf(stderr, " WARN: Unknown section flag(s) %08x\n",
  1641. shdr->section_flags);
  1642. if ((shdr->section_flags & SB_SECTION_FLAG_BOOTABLE) &&
  1643. (hdr->first_boot_section_id != shdr->section_number)) {
  1644. fprintf(stderr,
  1645. " WARN: Bootable section does ID not match image header ID!\n");
  1646. }
  1647. ret = sb_verify_commands(ictx, sctx, fp);
  1648. if (ret)
  1649. return ret;
  1650. sctx = sctx->sect;
  1651. }
  1652. /*
  1653. * FIXME IDEA:
  1654. * check if the first TAG command is at sctx->section_offset
  1655. */
  1656. return 0;
  1657. }
  1658. static int sb_verify_image_end(struct sb_image_ctx *ictx,
  1659. FILE *fp, off_t filesz)
  1660. {
  1661. uint8_t digest[32];
  1662. unsigned long size;
  1663. off_t pos;
  1664. int ret;
  1665. soprintf(ictx, "------------- Verifying image end -------------\n");
  1666. size = fread(digest, 1, sizeof(digest), fp);
  1667. if (size != sizeof(digest)) {
  1668. fprintf(stderr, "ERR: SB key dictionary too short!\n");
  1669. return -EINVAL;
  1670. }
  1671. pos = ftell(fp);
  1672. if (pos != filesz) {
  1673. fprintf(stderr, "ERR: Trailing data past the image!\n");
  1674. return -EINVAL;
  1675. }
  1676. /* Check the image digest. */
  1677. EVP_DigestFinal(&ictx->md_ctx, ictx->digest, NULL);
  1678. /* Decrypt the image digest from the input image. */
  1679. sb_aes_reinit(ictx, 0);
  1680. sb_aes_crypt(ictx, digest, digest, sizeof(digest));
  1681. /* Check all of 20 bytes of the SHA1 hash. */
  1682. ret = memcmp(digest, ictx->digest, 20) ? -EINVAL : 0;
  1683. if (ret)
  1684. soprintf(ictx, "[FAIL] Full-image checksum: BAD\n");
  1685. else
  1686. soprintf(ictx, "[PASS] Full-image checksum: OK\n");
  1687. return ret;
  1688. }
  1689. static int sb_build_tree_from_img(struct sb_image_ctx *ictx)
  1690. {
  1691. long filesize;
  1692. int ret;
  1693. FILE *fp;
  1694. if (!ictx->input_filename) {
  1695. fprintf(stderr, "ERR: Missing filename!\n");
  1696. return -EINVAL;
  1697. }
  1698. fp = fopen(ictx->input_filename, "r");
  1699. if (!fp)
  1700. goto err_open;
  1701. ret = fseek(fp, 0, SEEK_END);
  1702. if (ret < 0)
  1703. goto err_file;
  1704. filesize = ftell(fp);
  1705. if (filesize < 0)
  1706. goto err_file;
  1707. ret = fseek(fp, 0, SEEK_SET);
  1708. if (ret < 0)
  1709. goto err_file;
  1710. if (filesize < (signed)sizeof(ictx->payload)) {
  1711. fprintf(stderr, "ERR: File too short!\n");
  1712. goto err_file;
  1713. }
  1714. if (filesize & (SB_BLOCK_SIZE - 1)) {
  1715. fprintf(stderr, "ERR: The file is not aligned!\n");
  1716. goto err_file;
  1717. }
  1718. /* Load and verify image header */
  1719. ret = sb_verify_image_header(ictx, fp, filesize);
  1720. if (ret)
  1721. goto err_verify;
  1722. /* Load and verify sections and commands */
  1723. ret = sb_verify_sections_cmds(ictx, fp);
  1724. if (ret)
  1725. goto err_verify;
  1726. ret = sb_verify_image_end(ictx, fp, filesize);
  1727. if (ret)
  1728. goto err_verify;
  1729. ret = 0;
  1730. err_verify:
  1731. soprintf(ictx, "-------------------- Result -------------------\n");
  1732. soprintf(ictx, "Verification %s\n", ret ? "FAILED" : "PASSED");
  1733. /* Stop the encryption session. */
  1734. sb_aes_deinit(&ictx->cipher_ctx);
  1735. fclose(fp);
  1736. return ret;
  1737. err_file:
  1738. fclose(fp);
  1739. err_open:
  1740. fprintf(stderr, "ERR: Failed to load file \"%s\"\n",
  1741. ictx->input_filename);
  1742. return -EINVAL;
  1743. }
  1744. static void sb_free_image(struct sb_image_ctx *ictx)
  1745. {
  1746. struct sb_section_ctx *sctx = ictx->sect_head, *s_head;
  1747. struct sb_dcd_ctx *dctx = ictx->dcd_head, *d_head;
  1748. struct sb_cmd_ctx *cctx, *c_head;
  1749. while (sctx) {
  1750. s_head = sctx;
  1751. c_head = sctx->cmd_head;
  1752. while (c_head) {
  1753. cctx = c_head;
  1754. c_head = c_head->cmd;
  1755. if (cctx->data)
  1756. free(cctx->data);
  1757. free(cctx);
  1758. }
  1759. sctx = sctx->sect;
  1760. free(s_head);
  1761. }
  1762. while (dctx) {
  1763. d_head = dctx;
  1764. dctx = dctx->dcd;
  1765. free(d_head->payload);
  1766. free(d_head);
  1767. }
  1768. }
  1769. /*
  1770. * MXSSB-MKIMAGE glue code.
  1771. */
  1772. static int mxsimage_check_image_types(uint8_t type)
  1773. {
  1774. if (type == IH_TYPE_MXSIMAGE)
  1775. return EXIT_SUCCESS;
  1776. else
  1777. return EXIT_FAILURE;
  1778. }
  1779. static void mxsimage_set_header(void *ptr, struct stat *sbuf, int ifd,
  1780. struct mkimage_params *params)
  1781. {
  1782. }
  1783. int mxsimage_check_params(struct mkimage_params *params)
  1784. {
  1785. if (!params)
  1786. return -1;
  1787. if (!strlen(params->imagename)) {
  1788. fprintf(stderr,
  1789. "Error: %s - Configuration file not specified, it is needed for mxsimage generation\n",
  1790. params->cmdname);
  1791. return -1;
  1792. }
  1793. /*
  1794. * Check parameters:
  1795. * XIP is not allowed and verify that incompatible
  1796. * parameters are not sent at the same time
  1797. * For example, if list is required a data image must not be provided
  1798. */
  1799. return (params->dflag && (params->fflag || params->lflag)) ||
  1800. (params->fflag && (params->dflag || params->lflag)) ||
  1801. (params->lflag && (params->dflag || params->fflag)) ||
  1802. (params->xflag) || !(strlen(params->imagename));
  1803. }
  1804. static int mxsimage_verify_print_header(char *file, int silent)
  1805. {
  1806. int ret;
  1807. struct sb_image_ctx ctx;
  1808. memset(&ctx, 0, sizeof(ctx));
  1809. ctx.input_filename = file;
  1810. ctx.silent_dump = silent;
  1811. ret = sb_build_tree_from_img(&ctx);
  1812. sb_free_image(&ctx);
  1813. return ret;
  1814. }
  1815. char *imagefile;
  1816. static int mxsimage_verify_header(unsigned char *ptr, int image_size,
  1817. struct mkimage_params *params)
  1818. {
  1819. struct sb_boot_image_header *hdr;
  1820. if (!ptr)
  1821. return -EINVAL;
  1822. hdr = (struct sb_boot_image_header *)ptr;
  1823. /*
  1824. * Check if the header contains the MXS image signatures,
  1825. * if so, do a full-image verification.
  1826. */
  1827. if (memcmp(hdr->signature1, "STMP", 4) ||
  1828. memcmp(hdr->signature2, "sgtl", 4))
  1829. return -EINVAL;
  1830. imagefile = params->imagefile;
  1831. return mxsimage_verify_print_header(params->imagefile, 1);
  1832. }
  1833. static void mxsimage_print_header(const void *hdr)
  1834. {
  1835. if (imagefile)
  1836. mxsimage_verify_print_header(imagefile, 0);
  1837. }
  1838. static int sb_build_image(struct sb_image_ctx *ictx,
  1839. struct image_type_params *tparams)
  1840. {
  1841. struct sb_boot_image_header *sb_header = &ictx->payload;
  1842. struct sb_section_ctx *sctx;
  1843. struct sb_cmd_ctx *cctx;
  1844. struct sb_command *ccmd;
  1845. struct sb_key_dictionary_key *sb_dict_key = &ictx->sb_dict_key;
  1846. uint8_t *image, *iptr;
  1847. /* Calculate image size. */
  1848. uint32_t size = sizeof(*sb_header) +
  1849. ictx->sect_count * sizeof(struct sb_sections_header) +
  1850. sizeof(*sb_dict_key) + sizeof(ictx->digest);
  1851. sctx = ictx->sect_head;
  1852. while (sctx) {
  1853. size += sctx->size;
  1854. sctx = sctx->sect;
  1855. };
  1856. image = malloc(size);
  1857. if (!image)
  1858. return -ENOMEM;
  1859. iptr = image;
  1860. memcpy(iptr, sb_header, sizeof(*sb_header));
  1861. iptr += sizeof(*sb_header);
  1862. sctx = ictx->sect_head;
  1863. while (sctx) {
  1864. memcpy(iptr, &sctx->payload, sizeof(struct sb_sections_header));
  1865. iptr += sizeof(struct sb_sections_header);
  1866. sctx = sctx->sect;
  1867. };
  1868. memcpy(iptr, sb_dict_key, sizeof(*sb_dict_key));
  1869. iptr += sizeof(*sb_dict_key);
  1870. sctx = ictx->sect_head;
  1871. while (sctx) {
  1872. cctx = sctx->cmd_head;
  1873. while (cctx) {
  1874. ccmd = &cctx->payload;
  1875. memcpy(iptr, &cctx->c_payload, sizeof(cctx->payload));
  1876. iptr += sizeof(cctx->payload);
  1877. if (ccmd->header.tag == ROM_LOAD_CMD) {
  1878. memcpy(iptr, cctx->data, cctx->length);
  1879. iptr += cctx->length;
  1880. }
  1881. cctx = cctx->cmd;
  1882. }
  1883. sctx = sctx->sect;
  1884. };
  1885. memcpy(iptr, ictx->digest, sizeof(ictx->digest));
  1886. iptr += sizeof(ictx->digest);
  1887. /* Configure the mkimage */
  1888. tparams->hdr = image;
  1889. tparams->header_size = size;
  1890. return 0;
  1891. }
  1892. static int mxsimage_generate(struct mkimage_params *params,
  1893. struct image_type_params *tparams)
  1894. {
  1895. int ret;
  1896. struct sb_image_ctx ctx;
  1897. /* Do not copy the U-Boot image! */
  1898. params->skipcpy = 1;
  1899. memset(&ctx, 0, sizeof(ctx));
  1900. ctx.cfg_filename = params->imagename;
  1901. ctx.output_filename = params->imagefile;
  1902. ctx.verbose_boot = 1;
  1903. ret = sb_build_tree_from_cfg(&ctx);
  1904. if (ret)
  1905. goto fail;
  1906. ret = sb_encrypt_image(&ctx);
  1907. if (!ret)
  1908. ret = sb_build_image(&ctx, tparams);
  1909. fail:
  1910. sb_free_image(&ctx);
  1911. return ret;
  1912. }
  1913. /*
  1914. * mxsimage parameters
  1915. */
  1916. static struct image_type_params mxsimage_params = {
  1917. .name = "Freescale MXS Boot Image support",
  1918. .header_size = 0,
  1919. .hdr = NULL,
  1920. .check_image_type = mxsimage_check_image_types,
  1921. .verify_header = mxsimage_verify_header,
  1922. .print_header = mxsimage_print_header,
  1923. .set_header = mxsimage_set_header,
  1924. .check_params = mxsimage_check_params,
  1925. .vrec_header = mxsimage_generate,
  1926. };
  1927. void init_mxs_image_type(void)
  1928. {
  1929. mkimage_register(&mxsimage_params);
  1930. }
  1931. #else
  1932. void init_mxs_image_type(void)
  1933. {
  1934. }
  1935. #endif